改进位姿估计环节的ORB-SLAM稠密建图算法

为了提高ORB-SLAM2系统的位姿估计精度并解决仅能生成稀疏地图的问题，提出了一种融合ICP算法与曼哈顿世界假说的位姿估计策略并在ORB-SLAM2系统中加入稠密建图线程来实现稠密建图。首先通过ORB特征点法、LSD算法和AHC方法进行点、线、面特征的提取，其中点、线特征跟上一帧匹配，面特征在全局地图中匹配。然后采用基于surfel的稠密建图策略将图像划分为非平面与平面区域，非平面采用ICP算法计算位姿，平面则通过面与面的正交关系确定曼哈顿世界从而使用不同的位姿估计策略，其中曼哈顿世界场景通过位姿解耦实现基于曼哈顿帧观测的无漂移旋转估计，而曼哈顿世界场景下的平移以及非曼哈顿世界场景位姿采用追踪的点、线、面特征进行估计和优化；最后根据关键帧和相应位姿实现稠密建图。采用TUM数据集对所提建图方法进行验证，实验结果与ORB-SLAM2算法比较，最终均方根误差RMSE平均减少0.24cm，平均定位精度提高7.17%，验证了所提方法进行稠密建图的可行性和有效性。

Improved Pose Estimation for Dense Mapping based on ORB-SLAM

In order to improve pose-estimation accuracy of ORB-SLAM2 system and solve problem that only sparse-map can be generated, a pose-estimation strategy that integrates ICP algorithm and Manhattan world hypothesis is proposed and a dense-mapping thread is added to ORB-SLAM2 system to build dense-map. Firstly, point, line and surface features are extracted by ORB algorithm, LSD algorithm and AHC method, where point and line features are matched with previous frame and surface features are matched in global map. Then, dense-mapping strategy based on surfel is used to divide the image into non-planar and planar regions, and the ICP algorithm is used to calculate the poses in non-planar, while the planar is used to determine the Manhattan world through the face-to-face orthogonal relationship so as to use different pose-estimation strategies, where the Manhattan world scene achieves drift-free rotation estimation based on Manhattan frame observation by pose-decoupling, while the translation in Manhattan world scene and the non-Manhattan world scene poses are estimated and optimized using the tracked point, line and surface features; finally, the dense map is built based on the key frames and the corresponding poses. The proposed method is validated using the TUM dataset, and the final root mean square error RMSE is reduced by 0.24cm on average and the average localization accuracy is improved by 7.17% when compared with the ORB-SLAM2 algorithm, which verifies the feasibility and effectiveness of the proposed method for dense mapping.