Pose estimation and map reconstruction are basic requirements for robotic autonomous behavior. In this paper, we propose a point–plane-based method to simultaneously estimate the robot’s poses and reconstruct the current environment’s map using RGB-D cameras. First, we detect and track the point and plane features from color and depth images, and reliable constraints are obtained, even for low-texture scenes. Then, we construct cost functions from these features, and we utilize the plane’s minimal representation to minimize these functions for pose estimation and local map optimization. Furthermore, we extract the Manhattan World (MW) axes on the basis of the plane normals and vanishing directions of parallel lines for the MW scenes, and we add the MW constraint to the point–plane-based cost functions for more accurate pose estimation. The results of experiments on public RGB-D datasets demonstrate the robustness and accuracy of the proposed algorithm for pose estimation and map reconstruction, and we show its advantages compared with alternative methods.

Orientation estimation is a crucial part of robotics tasks such as motion control, autonomous navigation, and 3D mapping. In this paper, we propose a robust visual-based method to estimate robots’ drift-free orientation with RGB-D cameras. First, we detect and track hybrid features (i.e., plane, line, and point) from color and depth images, which provides reliable constraints even in uncharacteristic environments with low texture or no consistent lines. Then, we construct a cost function based on these features and, by minimizing this function, we obtain the accurate rotation matrix of each captured frame with respect to its reference keyframe. Furthermore, we present a vanishing direction-estimation method to extract the Manhattan World (MW) axes; by aligning the current MW axes with the global MW axes, we refine the aforementioned rotation matrix of each keyframe and achieve drift-free orientation. Experiments on public RGB-D datasets demonstrate the robustness and accuracy of the proposed algorithm for orientation estimation. In addition, we have applied our proposed visual compass to pose estimation, and the evaluation on public sequences shows improved accuracy.

Conventional initialization algorithms for feature-based monocular visual Simultaneous Localization and Mapping(V-SLAM)/Visual Odometry(VO) systems used point-based matrix decomposition. However, line segments are common in urban scenes and provide valuable information about scene structure. In this work, we propose incorporating line segments for initializing, which allows to obtain accurate initial camera poses and generate reliable 3D line segment landmarks as well as point landmarks. The proposed algorithm, denoted as PLSfSM (Point and Line Structure from Small Motion), employs both point and Plücker line reprojection constraints to recover structure from small motion. In particular, we propose a closed-form solution to estimate rotation and optimize rotation with line segment correspondences, which provides good initial guesses for rotation and line directions. Mathematically proven criteria are introduced to detect degeneracy of line reconstruction. The algorithm is evaluated on synthetic indoor dataset and simulation data. Qualitative experiments on both indoor and outdoor scenes are conducted to verify the robustness. Experimental results validate that the proposed algorithm which combines both points and line segments for initialization outperforms the state-of-the-art methods which only use points for initialization

Line segments are common in urban scenes and contain rich structural information. However, different from point-based reconstruction, reconstructed 3D lines may have large displacement from the ground truth in spite of a very small sum of reprojection error. In this work, we present a method to analyze the uncertainty of line reconstruction and provide a quantitative evaluation of accuracy. A new minimal four-vector line representation based on Plücker line is introduced, which is tailed for uncertainty analysis of line reconstruction. Each component of the compact representation has a certain physical meaning about the 3D line’s orientation or position. The reliability of the reconstructed lines can be measured by the confidence interval of each component in the proposed representation. Based on the uncertainty analysis of two-view line triangulation, the uncertainty of multi-view line reconstruction is also derived. Combining the proposed uncertainty analysis with reprojection error, a more reliable 3D line map can be obtained. Experiments on simulation data, synthetic and real-world image sequences validate the feasibility of the proposed method.