PaperlayerView exact revision on arXiv

Source-linked AI summary

BALM: Bundle Adjustment for Lidar Mapping

Zheng Liu, Fu Zhang

arXiv:2010.08215v2cs.RO

TL;DR

Lidar SLAM needs bundle adjustment to reduce drift, but sparse, non-repetitive point clouds make exact point matching infeasible. The paper formulates pose-only BA using distances to edge and plane features, with analytical derivatives and adaptive voxelization for efficient correspondence search. Integrated into LOAM, the method reduces drift and runs nearly in real time while optimizing 20 scans.

  • Problem

    Sparse and non-repetitive lidar point clouds make exact point matching infeasible, limiting the use of bundle adjustment for lidar SLAM drift reduction.

  • Method

    The method minimizes distances from feature points to matched edges or planes, analytically eliminates feature parameters, derives second-order derivatives, and uses adaptive voxelization for correspondence search.

  • Results

    The LOAM back-end reduces drift and runs nearly in real time at 10Hz when optimizing a sliding window of 20 scans.

  • Takeaways & Limitations

    Lidar BA can refine LOAM maps using dense edge and plane features while retaining efficient pose-only optimization.

Abstract

from arXiv · show

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.

I. INTRODUCTION

The paper addresses lidar SLAM drift by formulating bundle adjustment around sparse edge and plane features rather than exact point matches. Its pose-only optimization, analytical derivatives, and adaptive voxelization enable efficient LOAM map refinement, with experiments showing reduced drift.

  • Lidar BA is difficult because sparse, non-repetitive point clouds make exact point matching infeasible.
  • The proposed BA minimizes feature-point distance to matched edges or planes and analytically eliminates feature parameters, leaving scan poses as optimization variables.This reduces optimization dimension and permits large-scale dense edge and plane features.
  • Analytical first- and second-order derivatives of the cost function support efficient Gauss-Newton optimization.The formulation is designed to speed convergence compared with inefficient gradient-descent optimization.
  • The local BA is incorporated into LOAM as a back-end for refining incrementally built maps and lowering accumulated drift.The motivation is especially relevant to featureless environments, small-FoV lidars, and incremental registration pipelines.
  • Adaptive voxelization efficiently searches correspondences for both plane and edge features without requiring segmentation.The method adapts to environments containing large planes and small planar patches.

III. BA FORMULATION AND DERIVATIVES

The lidar BA jointly optimizes scan poses using sparse points assigned to common edge or plane features. Because feature parameters can be solved analytically, the resulting optimization depends only on poses and eigenvalues of a point covariance matrix.

  • A. Direct BA formulation: Each feature group contains sparse points from multiple scans, whose global coordinates are determined by the corresponding scan poses.The points are transformed using each scan’s rotation and translation.
  • A. Direct BA formulation: Lidar BA jointly determines scan poses and the global map, representing one feature by a point q and unit vector n.For planes, n is the plane normal; for edges, n is the edge direction.
  • A. Direct BA formulation: The plane objective minimizes the summed squared distances from transformed feature points to a plane.The plane is parameterized by q and n.
  • A. Direct BA formulation: The optimal feature parameters can be solved in closed form before bundle adjustment, leaving an optimization that depends only on scan poses.The optimal q need not be unique because it can move within a plane or along an edge without changing the cost.
  • A. Direct BA formulation: The edge objective likewise minimizes summed squared distances from feature points to their common edge.Both edge and plane constraints are expressed through distances to the corresponding geometric feature.
  • A. Direct BA formulation: After eliminating the feature parameters, pose optimization reduces to minimizing selected eigenvalues of the point covariance matrix A.For plane and edge features, the relevant costs are expressed through eigenvalues of A and its trace.
  • A. Direct BA formulation: Closed-form derivatives through second order are derived with respect to the pose by first differentiating the point vector.This supports efficient optimization of the pose-only cost.

B. The Derivatives

The derivative section provides first- and second-order derivatives for eigenvalue-based costs of the covariance matrix formed from feature points.

  • B. The Derivatives: The derivative analysis assumes a group of points and the covariance matrix A defined for those points.The mean point is used in defining the covariance matrix.
  • B. The Derivatives: The second-order derivative result assumes distinct eigenvalues, with λ_i ≠ λ_k whenever i ≠ k.This condition is required for the stated Hessian formulation.

C. Second order approximation

The pose-dependent BA cost is approximated to second order and minimized iteratively with Levenberg–Marquardt using derivatives propagated from transformed feature points.

  • C. Second order approximation: The cost function is approximated by a second-order expansion using a Jacobian J(p) and Hessian H(p).J(p) contains the first derivatives, while H(p) contains the second derivatives with respect to point-vector elements.
  • C. Second order approximation: Perturbing scan pose T_j changes only feature points originating from scan j, while other point derivatives are zero.This structure follows the scan-of-origin mapping for each feature point.
  • C. Second order approximation: The point-level derivatives are substituted into the pose-dependent approximation through the chain rule.The substitution connects derivatives with respect to transformed points to derivatives with respect to scan poses.
  • C. Second order approximation: Levenberg–Marquardt repeatedly minimizes the eigenvalue cost by solving a damped second-order system at each iteration.The damping parameter μ is selected by the LM method.

IV. ADAPTIVE VOXELIZATION

Adaptive voxelization groups corresponding sparse edge and plane points efficiently using recursive geometric subdivision, while supporting compact map storage and derivative-safe optimization.

  • IV. ADAPTIVE VOXELIZATION: Adaptive voxelization recursively subdivides space from a default voxel size and retains voxels whose points fit a common plane or edge.Geometric consistency is examined using eigenvalues of the point covariance matrix.
  • IV. ADAPTIVE VOXELIZATION: The method is compatible with octrees and can terminate early, making correspondence search more efficient than constructing a full feature-point Kd-tree.The advantage is especially relevant for environments containing large planes or long edges.
  • IV. ADAPTIVE VOXELIZATION: Separate voxel maps are built for edge and planar features, using octrees indexed by a Hash table.Different octrees can have different depths according to the geometry of each default-size cube.
  • IV. ADAPTIVE VOXELIZATION: Each leaf voxel stores feature points corresponding to the same plane or edge.This stored correspondence supports later BA cost construction.
  • IV. ADAPTIVE VOXELIZATION: When a voxel contains too many points, points from the same scan can be averaged to reduce Hessian size without degrading mapping consistency.The averaged points remain on the same plane determined by the raw feature points.
  • IV. ADAPTIVE VOXELIZATION: Voxels with repeated covariance eigenvalues are skipped because the second-order Hessian requires distinct eigenvalues.Recursive subdivision also stops at a maximum tree depth or a minimum point count.
  • IV. ADAPTIVE VOXELIZATION: The voxelization naturally extends to non-planar features by using finer voxels and allowing larger variance in the planarity test.This extension includes curved surfaces.

V. LOAM WITH LOCAL BA

The LOAM back-end combines adaptive voxelization with local BA to refine poses and maps using a sliding window. Experiments on handheld and indoor lidar data report reduced drift and effective map refinement.

  • System overview: The system uses parallel feature-extraction, odometry, and map-refinement threads within LOAM.Feature extraction provides edge and plane features; odometry registers scans to the map, while map refinement performs local BA.
  • System overview: Adaptive voxel correspondence replaces nearest-point matching with plane or edge voxels during scan-to-map alignment.Voxel centers and normals or directions are computed for matching new feature points.
  • Map refinement: Local BA refines lidar poses in a sliding window using cost items from voxels containing window points.Updated poses are then used to update the centers and normals of involved voxels.
  • Map refinement: Older scans are merged into compact map summaries once the sliding window is full, avoiding storage of their raw points.A recursive covariance formulation summarizes points outside the window for continued odometry and refinement.
  • Experiments: The 817m Livox Horizon walk returned to its start with BALM, while LOAM showed significant drift; elevation error was 0.27m versus 3.98m.The experiment lasted 20 minutes and used a 25°×82° field-of-view lidar.
  • Experiments: Indoor stairway experiments and outdoor mapping comparisons further validated the proposed local BA.The supplied results describe indoor validation and compare BALM with LOAM in the outdoor walking dataset.

C. Velodyne VLP-16

The Velodyne VLP-16 evaluation compares BALM with LOAM and LeGO-LOAM while examining correspondence-search and local-BA runtime. The reported implementation is designed for efficient refinement, with most 20-scan BA and voxel-map updates completing within 100ms.

  • Mapping results: The VLP-16 experiment compares paths from LOAM, LeGO-LOAM, and BALM on data with the same starting and ending point.Drift when returning to the start is reported in Table II.
  • Running time: BALM matches each feature point to its closest plane or edge voxel, whereas LOAM searches for five closest points.The comparison includes kd-tree construction, correspondence search, and LM optimization time.
  • Running time: A fixed two-step LM optimization is used for both methods to make the runtime comparison fair.
  • Running time: In most cases, local BA and voxel-map update complete in 100ms for a sliding window of 20 scans.This is reported as nearly real-time operation at the odometry rate of 10Hz.
  • Overall evaluation: The framework was evaluated across various lidars and environments, and the authors report effective map refinement.The supplied conclusion summarizes the experimental validation without providing a VLP-16-specific drift value.
  • Scope and limitations: The current implementation uses temporally adjacent scans, which can introduce redundant information when neighboring scans overlap substantially.Future work proposes keyframes for the local sliding window.

A. Proof of theorem 1

The supplied proof passages describe differentiating orthogonality relations and assembling derivatives, but they do not provide enough intact mathematical context to state the theorem’s proof steps precisely.

  • Derivative construction: The proof differentiates the orthogonality condition U^T U = I with respect to a pose component.
  • Derivative construction: It notes that C_p is skew-symmetric with zero diagonal elements when deriving diagonal terms.
  • Derivative construction: The derivation stacks partial derivatives of λ_k with respect to the pose parameters.

B. Proof of theorem 2

The proof derives matrix-element expressions and partial derivatives for transformed point quantities, under the assumption that the eigenvectors u_m and u_n are constant.

  • Point components: The derivation applies to q ∈ {x_j, y_j, z_j}, treating q as an element of point p_j.The point is written as p_i = [x_i y_i z_i]^T and p_j = [x_j y_j z_j]^T.
  • Matrix structure: For m ≠ n, the m-th row and n-th column element of C_q is specified through the matrix elements C^{m,n}_q.The diagonal elements of C_q are zero.
  • Derivative construction: Using e_k, the partial derivatives of u_k with respect to all elements of p_j are stacked to form the required derivative expressions.Here e_k is a 3 × 1 vector with one at position k and zeros elsewhere.
  • Derivative construction: The component derivatives C^{m,n}_{x_j}, C^{m,n}_{y_j}, and C^{m,n}_{z_j} belong to R^{1×3} for m,n ∈ {1,2,3}.These component expressions are then stacked as part of the derivation.
  • Assumption: The derivation treats the vectors u_m and u_n as constant when stacking the component terms.This is the stated assumption used in the construction.
  • Proof completion: The specific form of F^{m,n}_{p_j} is obtained by analogous derivations, followed by rewriting equation (24) as equation (7).The proof concludes after the derivative relation is substituted into the target expression.
Loading 2010.08215v2…