Real-time SLAM algorithm based on RGB-D data

A real-time SLAM (simultaneous localization and mapping) algorithm based on RGB-D (RGB-depth) data is proposed, which can get 6D poses of the robot and build the 3D map of the environment. Firstly, key features of the RGB images are extracted, whose 3D coordinates and colors, along with their corresponding covariance matrices, are then calculated using the Gaussian mixture model. Then the transformation matrix T_t between features in the current frame and feature set of the environment model can be gotten by using ICP (iterative closest point) algorithm, and sensor pose matrix P_t for optimal observation is obtained by sampling around T_t. Based on P_t, the dense point clouds in the current frame are then transformed into global coordinate to build the 3D map. Finally, the feature set of the environment model is updated using Kalman filter. To decrease the error of ICP, especially when the features are poor, particle sampling is conducted to improve the localization accuracy. Moreover, the color information is used to increase the success rate of data association between the features in the current frame and those in the environment model. It costs about 31 ms for each frame. In the mean time, the minimum localization error for FR1 benchmark is 1.7 cm and the average error is 11.9 cm. Therefore, the algorithm is accurate and fast enough for real-time SLAM.