From jderobot
Jump to: navigation, search

Lidar Odometry[edit]

I have developed a ROS node which publishes estimated odometry information as computed using the pointclouds (published by the ROSbag file of the dataset). The published odometry data are then visualized in the Rviz along with the Groundtruth data and the Pointclouds. For this particular case I have used the LCAS pointcloud dataset . For working with the Pointclouds inside the algorithm I have used the PCL Library. Here is the graph of the topics and the nodes involved:-

The Velodyne_pointcloud CloudNodelet converts the 'velodyne packets' to 'velodyne points' which are processed by the Odometry Node. For Lidar Odometry I have used ICP based Pointcloud pair registration technique thus obtaining the transform between subsequent cloud pairs and concatenating them to produce the final odometry path. Explanation of Pointcloud registration using ICP in brief:

This method is very much prone to odometry drift error as unlike a full-fledged SLAM algorithm , it does not correct its pose through pose-graph optimization or loop closure techniques.

   The resulting video:-
   RED = GroundTruth pose 
   GREEN = Estimated pose
   Github Code

Finally I tested Lidar odometry estimation using the state-of-the-art Lidar odometry and mapping algorithm using the same dataset. And here is the result:-

REFERENCES:- [1] An Iterative Closest Points Algorithm for Registration of 3D Laser Scanner Point Clouds with Geometric Features

[2] LOAM: Lidar Odometry and Mapping in Real-time

2D VISUAL ODOMETRY WITH RGBD (new exercise) with solution[edit]


I have updated the exercise infrastructure with the Readme file containing detailed instructions about how to run the exercise and where to insert the user's code and also added config file where users have to mention the ROSbag filename of the RGBD dataset.

I have also created a RGBD based solution algorithm. Here is the video of the working solution

The solution code is available here :

An important advantage of the RGBD based odometry over Monocular Odometry is that it removes the prior manual intervention about the Global scale factor.

  The pipeline of the solution algorithm is as follows :

1) Detect features from the first available RGB image using FAST algorithm.

2) Track the detected features in the next available RGB image using Lucas-Kanade Optical Flow Algorithm.

3)Create the 3D pointcloud (of the tracked/detected feature points) of the latest two available RGB image with the help of their depth image .

4) Create a Consistency matrix for each of the two consecutive poinclouds.Consistency matrix is a NxN matrix (where N is the number of features points in the pointcloud.) such that :-

  M{i,j} = 1 if the eucledian distance between point pair i and j is below a certain very small threshold value. 

         = 0 otherwise.

This step plays an important role in extraction of inlier points and removal of noisy datapoints from the generated poinclouds.

5)Estimation of motion from the set of inlier points. Since the surrounding environment of the robot is immobile , we can use Least-Squares Rigid body motion using SVD to compute the best-fitting rigid transformation that aligns and outputs Rotation and Translation matrix between two sets of corresponding points of their pointclouds.

7) Concatenate the Rotation and Translational matrix to obtain the predicted path.

8) Smoothening of the generated predicted path using B-spline smoothing function (scipy.interpolate.splprep)

I tried to implement the algorithm as described in this paper for RGBD odometry.


[2] Visual Odometry and Mapping for Autonomous Flight Using an RGB-D Camera

IMU and Monocular Visual odometry Fusion with Unscented Kalman Filter[edit]

Apart from creating a solution based on Depth enhanced RGB image , I have also tried to create a solution based on fusion of IMU pose estimation and Monocular Visual odometry estimation using Unscented Kalman Filter. I preferred Unscented KF over Extended KF mainly because Unscented KF performs Unscented transform to a number of sample points to predict the prior state variables whereas EKF linearizes the non-linear process model at a single point, so in many practical cases Unscented KF outperforms Extended KF .Also For the process model I have used the IMU readings for the propagation/prediction of the state variables i.e. (x and y coordinates) using 'dead-reckoning'. Then the state variables are updated using the estimation from the Monocular Visual Odometry algorithm. In this way two different sensors can be fused for better odometry estimation.

The video of the solution :

I think better results can be produced by providing proper process covariance matrix and measurement noise covariance matrix. I could not figure out a robust method to get the values of the covariance matrices.

New exercise infrastructure in JdeRobot Academy (Visual/IMU/Laser odometry/SLAM)[edit]

This new exercise infrastructure will be able to test the performance and accuracy of users' SLAM/odometry algorithm on the TUM RGBD dataset. The performance(speed) of the users' algorithm is tested on the basis of the Real-time factor and the accuracy of the algorithm is computed on the basis of Absolute Trajectory Error. Both these parameters are shown dynamically on the GUI . Also the predicted and actual(groundtruth) positions are visualized simultaneously on a graph widget for comparing them visually. The exercise infrastructure has simple yet flexible API's to access datas of various sensors according to users' will from the ROSbag file of the dataset . Here is the look of the GUI of the exercise infrastructure:-

I have also tested the exercise infrastructure for bugs by writing a monocular visual odometry algorithm from scratch. Here is the video of that visual odometry solution :- (watch in 2x speed)

P.S.- The error measurements on the GUI are only updated when the pause button is clicked.

  Solution using data from the IMU sensors:

Next I will use the predicted positions of both these solutions to compute more accurate path using Kalman Filter.

Monocular Visual Odometry[edit]

This week my task was to create an algorithm for monocular visual odometry and using the TUM dataset for testing purpose [1] .

   The ground-truth of the whole dataset :

As you can see that the whole path is quite complex , so I extracted only a part of the whole path for testing and validating my algorithm.

The first step of Visual Odometry is to detect features in a frame and track them through the sequence of frames as available in the dataset. So for detection of features I used FAST detection algorithm from OpenCV then the features are then tracked by Lucas-Kanade Optical Flow algorithm. The following video shows detection and tracking of features using Lucas-Kanade Optical Flow algorithm.

As the optical flow algorithm tracks the features through the sequence of images , whenever the number of feature reduces below certain threshold the feature detector again triggers to produce new features and then track them.

Next the essential matrix is computed from the detected/tracked features between subsequent frames using the OpenCV function 'findEssentialMat'. Then Rotational and translational matrix for predicting the egomotion of the camera is computed using the 'recoverPose' function. The estimation of scale of the translation matrix is one of the critical step in Monocular Visual Odometry. For this the detected features are triangulated to a 3D pointcloud and the scale is then estimated using this formula.

   Here is the current prediction of my algorithm .
   Video of the working algorithm:-

Currently the algorithm not robust and the scale is quite ambiguous .I will try my best to increase the performance of this algorithm and then integrate the accelerometer data using EKF for better prediction of the path.

Follow Line exercise[edit]

This week I prepared a solution for the Follow Line exercise. To detect the direction of the lines from the camera image data I used following OpenCV operations like blurring , masking , eroding ,edge-detection and detecting straight lines using Hough Lines. The result can be seen here:-

For controlling the robot accurately ,I implemented a PID (Proportional Integral Derivative ) based control system. The parameters for the PID algorithm were hand-tuned for perfection and smoothness of the motion of the robot.

   Video of the solution:-

Link to the solution code :

Vacuum Cleaner exercise solution[edit]

I started my collaboration with JdeRobot Robotics Club by first creating a solution for the Vacuum Cleaner exercise. So I had to decide which algorithm to use for covering the maximum part of an unknown world within limited time. After going through and trying various other coverage algorithms I finally settled on using the BACKTRACKING SPIRAL ALGORITHM (BSA) [1]. I found it was the ideal algorithm for autonomously navigating the robot by only taking information about its environment from its laser sensor. So without much complication it only had to determine if there is any obstacle in its front , right and left and then decide its heading according to the following spiral algorithm:-

And the algorithm's implementation on a grid-like map looks like this:

Here are some snapshots of the working solution:

And the final working video of the solution:

Link to the solution code :