[Paper Review] BALM: Bundle Adjustment for Lidar Mapping

Zheng Liu, Fu Zhang|arXiv (Cornell University)|Oct 16, 2020

Robotics and Sensor-Based Localization41 references4 citations

TL;DR

This paper proposes BALM, a novel bundle adjustment method for LiDAR SLAM that formulates optimization by minimizing the distance from feature points to their matched edges or planes, analytically solving for feature parameters to reduce optimization dimensionality. The method enables real-time, high-accuracy map refinement in LOAM-based systems by leveraging closed-form derivatives and adaptive voxelization, significantly reducing drift even in challenging environments with small FoV lidars.

ABSTRACT

A local Bundle Adjustment (BA) on a sliding window of keyframes has been widely used in visual SLAM and proved to be very effective in lowering the drift. But in lidar SLAM, BA method is hardly used because the sparse feature points (e.g., edge and plane) make the exact point matching impossible. In this paper, we formulate the lidar BA as minimizing the distance from a feature point to its matched edge or plane. Unlike the visual SLAM (and prior plane adjustment method in lidar SLAM) where the feature has to be co-determined along with the pose, we show that the feature can be analytically solved and removed from the BA, the resultant BA is only dependent on the scan poses. This greatly reduces the optimization scale and allows large-scale dense plane and edge features to be used. To speedup the optimization, we derive the analytical derivatives of the cost function, up to second order, in closed form. Moreover, we propose a novel adaptive voxelization method to search feature correspondence efficiently. The proposed formulations are incorporated into a LOAM back-end for map refinement. Results show that, although as a back-end, the local BA can be solved very efficiently, even in real-time at 10Hz when optimizing 20 scans of point-cloud. The local BA also considerably lowers the LOAM drift. Our implementation of the BA optimization and LOAM are open-sourced to benefit the community.

Motivation & Objective

• To address the lack of effective bundle adjustment in LiDAR SLAM due to sparse, non-repetitive point clouds and the infeasibility of exact point matching.
• To formulate a LiDAR BA that minimizes the distance from feature points to their matched edges or planes, avoiding the need to co-optimize feature locations.
• To reduce optimization complexity by analytically solving for edge and plane parameters, leaving only scan poses as variables in the optimization.
• To enable real-time performance on large-scale dense feature maps by deriving closed-form gradients and Hessian matrices.
• To improve mapping consistency and reduce drift in LOAM-based systems, especially in feature-poor or small FoV environments.

Proposed method

• Formulates LiDAR BA as minimizing the orthogonal distance from each feature point to its matched edge or plane, rather than enforcing exact point correspondence.
• Analytically solves for edge and plane parameters in closed form, decoupling them from the optimization and reducing the variable space to only scan poses.
• Derives analytical first- and second-order derivatives (gradient and Hessian) of the cost function with respect to scan poses for efficient Gauss-Newton optimization.
• Introduces an adaptive voxelization method to accelerate feature correspondence search by dynamically adjusting voxel size based on local point density.
• Integrates the proposed BA as a back-end into the LOAM framework to refine the map using a sliding window of 20 scans.
• Employs a pose graph optimization strategy where only the poses of recent scans are refined, maintaining computational efficiency.

Experimental results

Research questions

• RQ1 Can bundle adjustment be effectively applied to LiDAR SLAM despite the sparsity and non-repetitiveness of point clouds?
• RQ2 Can feature parameters (edges and planes) in LiDAR BA be analytically solved, thereby reducing the optimization dimensionality?
• RQ3 Can closed-form derivatives of the cost function enable fast, real-time optimization in LiDAR BA?
• RQ4 How effective is the proposed adaptive voxelization in accelerating feature correspondence search without sacrificing accuracy?
• RQ5 Can the integration of BA as a back-end significantly reduce drift in LOAM-based systems, especially under challenging conditions like small FoV or featureless environments?

Key findings

• The proposed BALM method achieves real-time performance at 10Hz when optimizing a sliding window of 20 LiDAR scans, demonstrating feasibility for online deployment.
• By analytically solving for edge and plane parameters, the optimization scale is drastically reduced, enabling the use of dense edge and plane features.
• The method significantly reduces drift in LOAM-based mapping, as validated on both spinning LiDARs and small FoV LiDARs such as the Livox Horizon.
• The adaptive voxelization method improves correspondence search efficiency, contributing to the overall real-time performance.
• The closed-form derivatives of the cost function enable fast and stable convergence in Gauss-Newton optimization, enhancing robustness.
• The open-sourced implementation of BALM and LOAM provides a valuable resource for the community to extend and improve LiDAR SLAM systems.