Hybrid metric-feature mapping based on camera and Lidar sensor fusion

Mapping the surroundings is a challenging task required for mobile robot. Fusion of heterogeneous sensors measurement has gradually been the main focus of robot perception. In this work, the Lidar and camera mea-surement is fused to obtain robust motion estimation and construct hybrid metric-feature map. By coupling visual information, the powerful tracking ability of visual features is heuristically applied to support the scan matching. With the estimation of Lidar odometry, the camera motion in complicated scene can be guaranteed. Meanwhile, the hybrid map is divided into submaps to improve the mapping efficiency, and each submap is composed of a grid-based metric submap and a group of visual features. Then a multi-stage loop closure strategy is applied to determine the loop candidate. Besides, multi-layer optimization including local bundle adjustment and pose graph optimization is performed to obtain global consistent map. The effectiveness of the proposed method is verified in different scenarios.