Efficient and Consistent Bundle Adjustment on Lidar Point Clouds

Zheng Liu , Xiyuan Liu , Member, IEEE, and Fu Zhang , Member, IEEE

Abstract—Simultaneous determination ofsensor poses and scene geometry is a fundamental problem for robot vision that is often achieved by Bundle Adjustment (BA). This article presents an efficient and consistent bundle adjustment method for light detection and ranging (lidar) sensors. The method employs edge and plane features to represent the scene geometry, and directly minimizes the natural Euclidean distance from each raw point to the respective geometry feature. A nice property of this formulation is that the geometry features can be analytically solved, drastically reducing the dimension ofthe numerical optimization. To represent and solve the resultant optimization problem more efficiently, this paper then adopts and formalizes the concept of point cluster, which encodes all raw points associated to the same feature by a compact set of parameters, thepoint cluster coordinates. We derive the closed-form derivatives, up to the second order, of the BA optimization based on the point cluster coordinates and show their theoretical properties such as the null spaces and sparsity. Based on these theoretical results, this paper develops an efficient second-order BA solver. Besides estimating the lidar poses, the solver also exploits the second order information to estimate the pose uncertainty caused by measurement noises, leading to consistent estimates of lidar poses. Moreover, thanks to the use of point cluster, the developed solver fundamentally avoids the enumeration of each raw point in all steps of the optimization: cost evaluation, derivatives evaluation and uncertainty evaluation. The implementation of our method is open sourced to benefit the robotics community.

Index Terms—Bundle adjustment (BA), light detection and ranging (lidar), simultaneous localization and mapping (SLAM).

I. INTRODUCTION

IGHT detection and ranging (lidar) has become an essenof autonomy [1], [2]. Enabled by the direct, dense, active and accurate (DDAA) depth measurements, lidar sensors have the ability to build a dense and accurate 3-D map of the environment in real-time and at a relatively low computational cost. These unique advantages have made lidar sensors essential to a variety of applications that require real-time, dense, and accurate 3-D mapping of the environment, such as autonomous driving [3], [4], unmanned aerial vehicle navigation [5], [6], [7], and realtime mobile mapping [8], [9], [10]. This trend becomes even more evident with recent developments in lidar technologies, which have enabled the commercialization and mass production of lightweight and high-performance solid-state lidars at a significantly lower cost [11], [12].

The central task of many lidar-based techniques, such as lidar-based odometry, simultaneous localization and mapping (SLAM), and multilidar calibration, is to register multiple point clouds, each measured by the lidar at different poses, into a consistent global point cloud map. However, the predominant point cloud registration methods, such as iterative closest point (ICP) [13] and its variants (e.g., generalized-ICP [14]), normal distribution transformation (NDT) [15], [16], and surfel registration [17], allow registration of two point clouds only. Such a pairwise registration leads to an incremental scan registration process for an odometry system (e.g., [17], [18], [19], [20]), which would rapidly accumulate drift, or a repeated pairwise registration process for 3-D mapping [21] or multilidar calibration [22], which would bring dramatic computation cost. All these necessitate an efficient concurrent multiple-scan registration technique.

Concurrent multiple lidar scan registration requires determining all lidar poses and the scene geometry simultaneously, a process referred to as bundle adjustment (BA) in computer vision. Compared to visual BA, which has been well-established in photogrammetry and played a fundamental role in various vital applications, including visual odometry (VO) [23], [24], [25], visual-inertial odometry (VIO) [26], [27], 3-D visual reconstruction [28], [29] and multicamera calibration [30], [31], lidar BA has a similarly fundamental role but is much less mature due to two major challenges. First, lidar has a long measuring range but low resolution between scanning lines. The measured point clouds are sparse (sometimes even not repeatedly [12]) distributed in a large 3-D space, making it difficult (almost impossible) to scan the same point feature in the space across different scans. This has fundamentally prevented the use of straightforward visual bundle adjustment formulation, which is largely based on point features benefiting from the high-resolution images accurately capturing individual point features. The second challenge lies in the large number of raw points (from tens of thousands to million points) collected by a practical lidar sensors. Processing all these points in the lidar BA is extremely computation intensive.

In this work, we propose an efficient and consistent BA framework specifically designed for lidar point clouds. The framework follows our previous work bundle adjustment for lidar mapping (BALM) [32], which formulates the lidar BA problem based on edge and plane features that are abundant in lidar scans. The BA formulation naturally minimizes the straightforward Euclidean distance of each point in a scan to the corresponding edge or plane, while the decision variables include the lidar poses and feature (edge and plane) parameters. Furthermore, it is shown that the geometry parameters (i.e., edge and plane) can be solved analytically, leading to an optimization that depends on the lidar poses only. Since the number of geometry features is often large, elimination of these geometry features from the optimization will drastically reduce the optimization dimension (hence time).

A key concept our proposed BA framework adopts and formalizes is the point cluster [33], [34], [35], which summarizes all points of a lidar scan associated with one feature by a compact set of parameters, point cluster coordinates. Based on the point cluster, we derive the closed-form derivatives (up to second order) of the BA optimization with respect to (w.r.t.) its decision variables (i.e., lidar poses). We prove that the formulated BA optimization and the closed-form derivatives can both be fully represented by the point cluster without enumerating the large number of individual points in a lidar scan. The removal of dependence on individual raw points drastically speeds up the evaluation of the cost function and derivatives, which further enables us to develop an efficient and consistent second-order solver, BALM2.0, which is also released on Github to benefit the community. Our experiment video is available on website.¹

We conducted extensive evaluations of the proposed BA method. Simulation study shows that the BA method produces consistent lidar pose estimate. Exhaustive benchmark comparison on 19 real-world open sequences shows that the BA method produces consistently higher performance (pose estimation accuracy, mapping accuracy, and computation efficiency) than their other counterparts. We finally integrate the BA method in three vital lidar applications: lidar-inertial odometry, multilidar calibration, and global mapping, and show how their accuracy and/or computation efficiency are improved by the proposed BA.

II. RELATED WORKS

A. Multiview Registration

The bundle adjustment problem is similar to the multiview registration problem that has been previously researched [36], [37], [38], [39], [40], [41]. These methods all adopt a twolayer framework: the first layer estimates the relative poses of a selected set of scan pairs using the pairwise registration methods (e.g., ICP [13]). From the relative poses, the second layer constructs and solves a pose graph to obtain a maximum a posteriori estimate ofall lidar poses. Such a two-layer framework decouples the raw point registration from the global pose estimation, so that each raw point registration only involves a small amount of local points contained in the two scans (instead of all scans sharing overlaps) and the pose graph optimization only involves a small amount of constraints arising from the relative poses (instead of raw points). The net effect is a significant saving of time, hence being largely used in online lidar SLAM systems [42], [43]. However, the advantage in computation efficiency comes with fundamental limitation in accuracy: the pairwise scan registration only considers the overlap among two scans at a time, while the overlap is really shared by all scans and should be registered concurrently. Moreover, the pose graph optimization only considers constraints from the relative poses, while the mapping consistency indicated by the raw points are completely ignored. Consequently, it is usually difficult to produce (or even be aware of) a globally consistent map that is necessary for high-accuracy localization and mapping tasks.

Some early works in computer vision and computer graphics have proposed multiview registration methods that directly optimize the mapping consistency from multiple range images, aiming for consistent surface modeling of 3-D objects. The work of Blais and Levine [44] is a direct extension of the ICP method, it minimizes the Euclidean distance between a preknown control point in one scan to all matched control points in the rest of the scans. Within this framework, [45] uses a quaternion representation in the optimization, and [46] extends the distance between control points to the distance between surfaces around the respective control points. More recently, Zhu et al. [47] propose a two-step registration method: the first step uses a K-means clustering to cluster points from all scans, and the second step estimates the scan poses by minimizing the Euclidean distance between each point in a cluster to the centroid. Since these methods rely on point features in the scan, they require a densely populated point cloud (e.g., by depth camera) for extracting such salient point features. While this is not a problem for small object reconstruction for which these methods are designed, it is not the case for scene reconstruction, where the LiDAR measurements are very sparse (sometimes even nonrepetitive), as explained above.

B. Bundle or Plane Adjustment

In recent years, researchers in the robotics community have shown increasing interests to address the bundle adjustment problem on (lidar) point clouds more formally. Kaess [48] exploits the plane features in the bundle adjustment and minimizes the difference between the plane measured in a scan and the plane predicted from the optimization variables: scan poses and plane parameters. This formulation was later integrated into a key-frame-based online SLAM system [49]. Since the method minimizes the plane-to-plane distance, it requires segmenting each scan and estimating the contained local planes in advance. Such plane segmentation and estimation usually require dense point clouds measured by RGB-D cameras on which the work was demonstrated.

A more formal bundle adjustment method on lidar point cloud, termed as the plannar (bundle) adjustment, was later proposed in [34], which minimizes the natural Euclidean distance between each point in a scan to the plane predicted from the scan poses and plane parameters (the optimization variables). Compared with plane-to-plane distance in [48], the point-to-plane metric is faster, more accurate, and more suitable for lidar sensors, where local plane segmentation or estimation are less reliable due to sparse point clouds. Moreover, the direct use of raw points in the point-to-plane metric could also lead to a more consistent estimate of the optimization variables by considering the measurement noises in the raw points. Then, the formulated nonlinear least square problem is solved by a Levenberg–Marquardt (LM) algorithm. To lower the computation load caused by the large number of points measurements associated with the same plane feature, [34] proposes a reduction technique to eliminate the enumeration of individual points in the evaluation of the residual and Jacobian. Furthermore, due to the very similar structure of the visual bundle adjustment, the proposed bundle adjustment is also compatible with the Schur complement trick [50], which eliminates the plane parameters in each iteration of the LM algorithm. This plane adjustment method is largely used in many online lidar SLAM systems developed subsequently [51], [52] or before [53], [54].

On the other hand, Ferrer [33] exploits plane features similar to [34] and minimizes any deviation of each raw point from the plane equation. The resultant optimization cost then reduces to the minimum eigenvalue of a covariance matrix and is thus termed as the Eigen-factor (EF). The author further derived the closed-form gradient of the cost function w.r.t. to both the scan poses and plane parameters and employed a gradient-based method to solve the optimization iteratively. Due to the secondorder nature of the eigenvalue (as confirmed in [32], see below), the gradient method converges very slowly (requiring a few hundreds of iterations) [33], [52].

Our previous work, BALM [32], takes another step towards more efficient bundle adjustment. Similar to [34], BALM minimizes the natural Euclidean distance between each point in a scan to the plane (i.e., point-to-plane metric). Based on this cost metric, BALM proved that all plane parameters can be analytically solved with closed-form solutions in advance, hence the large number of plane parameters can be completely removed from the resultant optimization. Such an elimination of plane parameters is analogous to the well-studied separable least-squares problem [55], [56], [57], [58] in general, but is specifically designed for the LiDAR BA problem. A prominent advantage of the feature elimination is the significant reduction of the optimization dimension, which poses a fundamental difference from all the previous plannar adjustment methods [34], [51], [52], [53] and visual bundle adjustment methods [50]. The feature elimination also removed the various issues caused by plane representation in the optimization, such as the normal constraints in the Hesse normal representation $(\mathbf{n},d)[34],$ singularity issue in the closest-point (CP) representation nd [51], [52], [53] and over-parameterization issue in the quaternion representation [48]. With the feature elimination, BALM further proved that the point-to-plane (or edge) distance is essentially the eigenvalues of the covariance matrix used in [33], thus unifying the two metrics in [33] and [34]. While both BALM and [33] eliminate the feature from the BA optimization, [33] uses a gradient method to solve this optimization, which leads to very slow convergence as reviewed above. In contrast, BALM [32] derived the second-order derivatives of the cost function and developed an LM-like second-order solver. The developed solver requires significantly less iterations to converge, achieving real-time sliding window optimization when integrated to lidar odometry and mapping (LOAM) [18]. A further advantage of BALM against previous method [33], [34], [53] is that the whole framework is naturally extendable to edge features besides plane features.

A major drawback of the BALM [32] is that the evaluation of the second-order derivatives including Jacobian and Hessian requires to enumerate each individual lidar point, leading to a computational complexity of $O(N^2)$ where N is the number of points [52]. Consequently, the method is hard to be used in large-scale problems where the lidar points are huge in number. This problem is partially addressed in [35], which aggregates all points associated with the same plane feature in a scan in the scan local frame. However, to ensure convergence, [35] modifies the cost function by including an extra heuristic penalty term, which is not a true representation of the map consistency. Moreover, the cost function in [35] still involves the plane feature similar to [33], [34], [51], [52] and [53]. To lower the computation load caused by optimizing the large number offeature parameters, the method further fixes the feature parameters in the optimization, which could slow down the optimization speed.

Our BA formulation in this article is based on BALM [32], hence inheriting the fundamental feature elimination advantage when compared to [34], [51], [52], and [53], and the fast convergence advantage when compared to [33]. To address the computational complexity of $O(N^2)$ in BALM, we adopt and formalize the concept of point cluster, which fundamentally eliminates the enumeration of each individual point in the evaluation of the cost function, Jacobian and Hessian matrix. Consequently, the computational complexity is irrelevant to both the feature dimension (similar to BALM [32]) and the point number (similar to [34] and [51]). The point cluster in our method is similar to the point aggregation used in [35] (and also used in [33] and [34]), but the overall BA formulation is fundamentally different: 1) it minimizes the true map consistency (the point-to-plane distance) without trading off with any other heuristic penalty; and 2) it performs exact feature elimination with rigorous proof instead of empirical fixation. Based on these nice theoretical results, we develop an efficient second-order solver, termed as BALM2.0. Besides solving the nominal lidar poses, the solver also estimates the uncertainty of the estimated lidar pose by leveraging the second-order derivative information, which is another new contribution compared with existing works.

III. BUNDLE ADJUSTMENT FORMULATION AND OPTIMIZATION

In this chapter, we derive our BA formulation and optimization. First, following [32], we formulate the BA as minimizing the the point-to-plane (or point-to-edge) distance (Section III-A) and show that the feature parameters can be eliminated from the formulated optimization (Section III-B). Then, we introduce the point cluster in Section III-C, based on which the first- and second-order derivatives are derived in Section III-D. Based on these theoretical results, we present our second-order solver in Section III-E. Finally, in Section III-F, we show how to estimate the uncertainty of the BA solution. Throughout this article, we use notations summarized in Table I or otherwise specified in the context.

TABLE I NOMENCLATURES

Fig. 1. Factor graph representation of the bundle adjustment formulation.

A. BA Formulation

Shown in Fig. 1, assume there are $M_f$ features, each denoted by parameter ${\pi }_i(i=1,...,M_f)$ , observed by $M_p$ lidar poses, each denoted by ${\mathbf{T}}_j=({\mathbf{R}}_j,{\mathbf{t}}_j)(j=1,...,M_p)$ , the bundle adjustment refers to simultaneously determining all the lidar poses (denoted by $\mathbf{T}=({\mathbf{T}}_1,\dots ,{\mathbf{T}}_{M_p}))$ and feature parameters (denoted by $\pi =({\pi }_1,\dots ,{\pi }_{M_f}))$ , such that reconstructed map agrees with the lidar measurements to the best extent. Denote $c({\pi }_i,\mathbf{T})$ the map consistency due to the ith feature, a straightforward BA formulation is

In our BA formulation, we make use ofplane and edge features that are often abundant in lidar point cloud and minimize the natural Euclidean distance between each measured raw lidar point and its corresponding plane or edge feature. Specifically, assume a total number of $N_{ij}$ lidar points are measured on the ith feature at the jth lidar pose, each denoted by ${\mathbf{p}}_{f_{ijk}}(k=$ $1,\dots ,N_{ij})$ . Its predicted location in the global frame is

Fig. 2. Plane and edge features used in the lidar BA. (a) Plane formulation. q_i is a point in the plane and n_i is the plane normal. (b) Line formulation. q_i is a point on the edge and n_i is the edge direction. (a) The i-th plane feature ${\pi }_i=({\mathbf{n}}_i,{\mathbf{q}}_i)$ . (b) The i-th edge feature ${\mathbf{\pi }}_i=({\mathbf{n}}_i,{\mathbf{q}}_i)$

For a plane feature, it is parameterized by ${\pi }_i=({\mathbf{n}}_i,{\mathbf{q}}_i)$ with ${\mathbf{n}}_i$ the plane normal vector and ${\mathbf{q}}_i$ an arbitrary point on the plane, both in the global frame [see Fig. 2(a)]. Then, the Euclidean distance between a measured point ${\mathbf{p}}_{f_{ijk}}$ to the plane is $\Vert {\mathbf{n}}_i^T({\mathbf{p}}_{ijk}-{\mathbf{q}}_i){\Vert }_2$ . Aggregating the distance for all points observed in all poses leads to the total map consistency corresponding to this plane feature

where $\begin{array}{c}\hfill N_i={\sum }_{j=1}^{M_p}{N}_{ij}\end{array}$ is the total number of lidar points ob-served on the plane feature by all poses.

For an edge feature, it is parameterized by ${\pi }_i=({\mathbf{n}}_i,{\mathbf{q}}_i)$ with ${\mathbf{n}}_i$ the edge direction vector and ${\mathbf{q}}_i$ an arbitrary point on the edge, both in the global frame [see Fig. 2(b)]. Then, the Euclidean distance between a measured point ${\mathbf{p}}_{f_{ijk}}$ to the edge is $\Vert (\mathbf{I}-{\mathbf{n}}_i{\mathbf{n}}_i^T)({\mathbf{p}}_{ijk}-{\mathbf{q}}_i){\Vert }_2$ . Aggregating the distance for all points observed in all poses leads to the total map consistency corresponding to this edge feature

where $\begin{array}{c}\hfill N_i={\sum }_{j=1}^{M_p}{N}_{ij}\end{array}$ is the total number of lidar points ob-served on the edge feature by all poses.

B. Elimination of Feature Parameters

In this section, we show that in the BA optimization (1), the feature parameter π can really be solved with a closed-form solution. The key observation is that one cost item $c({\pi }_i,\mathbf{T})$ depends solely on one feature parameter, so that the feature parameter can be optimized independently. Concretely

In case of a plane feature, we substitute (3) into $c({\pi }_i,\mathbf{T})$

where ${\lambda }_l({\mathbf{A}}_i)$ denotes the lth largest eigenvalue of matrix ${\mathbf{A}}_i,$ ${\mathbf{u}}_l({\mathbf{A}}_i)$ denotes the corresponding eigenvector, the matrix ${\mathbf{A}}_i,$ and vector ${\overline{\mathbf{p}}}_i$ are defined as

The proof will be given in Supplementary III-A [59]. Note that the optimal solution ${\mathbf{q}}_i^{\star }$ in (6) is not unique, any deviation from ${\mathbf{q}}_i^{\star }$ along a direction perpendicular to ${\mathbf{n}}_i^{\star }$ will equally serve the optimal solution. However, these equivalent optimal solution will not change the plane nor the optimal cost (hence the results that follow next). Indeed, the point ${\mathbf{q}}_i^{\star }$ could be an arbitrary point on the plane as it is defined to be.

In case of an edge feature, we substitute (4) into $c({\pi }_i,\mathbf{T})$

Again, the optimal solution ${\mathbf{q}}_i^{\star }$ in (8) is not unique, any deviation from ${\mathbf{q}}_i^{\star }$ along the direction ${\mathbf{n}}_i^{\star }$ will equally serve the optimal solution. However, these equivalent optimal solution will not change the edge nor the optimal cost (hence the results that follow next).

As can be seen from (6) and (8), the parameter ${\pi }_i$ for each feature, either it is a plane or edge, can be analytically solved and hence removed from the BA optimization process. Consequently, the original BA optimization in (1) reduces to

where $l\in \{2,3\}$ and we omitted the exact number of eigenvalues in the cost function for brevity.

Note that the matrix ${\mathbf{A}}_i$ in (9) depends on the lidar pose T since each involved point ${\mathbf{p}}_{ijk}$ depends on the pose [see (7) and (2)]. Hence the decision variables of the resultant optimization in (9) involve the lidar pose T only, which dramatically reduces the optimization dimension (hence computation time).

C. Point Cluster

With the feature parameters eliminated, another difficulty remaining in the BA optimization (9) is that the evaluation of matrix A_i (and its Jacobian or Hessian necessary for developing a numerical solver) requires to enumerate every point observed at each lidar pose. Such an enumeration is extremely computationally expensive due to the large number of points in a lidar scan. In this section, we show such point enumeration can be avoided by point cluster, which is detailed as follows.

A point cluster is a finite point set denoted by set $\mathcal{C}=\{{\mathbf{p}}_k\in$ ${\mathbb{R}}^3|k=1,\dots ,n\}$ , the corresponding point cluster coordinate, denoted as ( ), is defined as

where ${\mathbb{S}}^{4\times 4}$ denotes the set of $4\times 4$ symmetric matrix.

A point cluster can be thought as a generalized point, for which a rigid transform could be applied. Similarly, we can define rigid transformation on a point cluster as follows.

Definition 1: (Rigid transform): Given a point cluster with point collection $\mathcal{C}=\{{\mathbf{p}}_k\in {\mathbb{R}}^3|k=1,\dots ,n\}$ and a pose $\mathbf{T}=$ $\begin{array}{cc}\hfill \backslash lceil\mathbf{R}& \mathbf{t}{\backslash rceil}_{{ϵ}^{}\mathit{S}\mathit{E}(3)}\hfill \end{array}$ . The rigid transformation of the point cluster ${}^{c,}$ denoted by $\mathbf{T}\circ c,$ is defined as

Besides rigid transformation, we also define cluster merging operation, as follows.

Definition 2: (Cluster merging): Given two point clusters with point collections ${\mathcal{C}}_1=\{{\mathbf{p}}_k^1\in {\mathbb{R}}^3|k=1,\dots ,n_1\}$ and ${\mathcal{C}}_2=\{{\mathbf{p}}_k^{\overline{2}}\in {\mathbb{R}}^3|k=1,\dots ,n_2\}$ in the same reference frame, respectively. The merged cluster, denoted by $c_1\oplus c_2$ , is defined as

Next we will show that the two operations defined above can be fully represented by their point cluster coordinates.

Theorem 1: Given a point cluster and a pose $\mathbf{T}=\left[\begin{array}{cc}\mathbf{R}\hfill & \mathbf{t}\hfill \\ 0\hfill & 1\hfill \end{array}\right]\in$ SE(3). The rigid transformation of the point cluster satisfies

Proof: See Supplementary III-B [59].

Theorem 2: Given two point clusters $c_1$ and $c_2$ in the same reference frame. The merged cluster satisfies

Proof: See Supplementary III-C [59].

As can be seen, rigid transformation and cluster merging operations on point clusters can be represented by usual matrix multiplication and addition on the point cluster coordinates. A visual illustration of the two operations and their coordinate representations are shown in Fig. 3. These results are crucially important: Theorem 1 indicates that the point cluster can be constructed in one frame (e.g., local lidar frame) and transformed to another (e.g., the global frame) without enumerating each individual points; Theorem 2 indicates that two (and by induction more) point clusters can be further merged to form a new point cluster. A particular case of Theorem 2 is when the second point cluster contains a single point, indicating that the point cluster can be constructed incrementally as lidar points arrives sequentially.

Fig. 3. Two operations on point cluster. (a) Rigid transform. (b) Cluster merging.

Remark 1: The concept of point cluster and its two operations are not new and have been used in previous works such as [33], [34], and [35]. In this article, we formalized this concept by: 1) introducing the point cluster coordinate composing of P, v, and n as in (10); 2) formalizing the two operations—rigid transform and cluster merging; 3) explicitly showing the relation between point cluster operations and their coordinates.

Remark 2: A point set and its coordinate is not a one-to-one mapping. While it is obvious that the coordinate is uniquely determined from the point set as shown in (10), the reverse way does not hold: the point set cannot be recovered from its coordinate uniquely. Since different point sets may lead to the same coordinate, the raw points must be saved if a reclustering is needed.

Now, we apply the point cluster to the BA problem concerned in this article. To start with, we group all points on the same feature as a point cluster. For example, the point cluster for the ith feature is ${\mathcal{C}}_i\triangleq \{{\mathbf{p}}_{ijk}|j=1,\dots ,M_p,k=1,\dots ,N_{ij}\}$ . Denote ${\mathbf{C}}_i$ the coordinate of the point cluster, following (10), we obtain:

A key result we show now is that this point cluster coordinate ${\mathbf{C}}_i$ is completely sufficient to represent the matrix A_i required

Fig. 4. For the ith feature (either plane or edge), all points observed at the jth pose are clustered as a point cluster and is represented by ${\mathbf{C}}_{f_{ij}}$ in its local frame. (a) Plane feature. (b) Line feature.

in the BA optimization (9). According to (7), we have

where we denote A_i as a function of ${\mathbf{C}}_i$ since the ${\mathbf{A}}_i$ is fully represented (and uniquely determined) by ${\mathbf{C}}_i$ . We slightly abuse the notation here by denoting the function as $\mathbf{A}(·)$

On the other hand, since the point set $c_i$ is defined on points in the global frame, the coordinate ${\mathbf{C}}_i$ is dependent on the lidar pose, which remains to be optimized. To explicitly parameterize the lidar pose, we note that

where ${\mathbf{p}}_{f_{ijk}}$ is a point represented in the lidar local frame [see (2)], $c_{ij}$ is the point set that is composed of all points on the ith feature (either plane or edge) observed at the jth lidar pose, and $c_{f_{ij}}$ is the same point set as $c_{ij}$ , but represented in the lidar local frame (see Fig. 4).

The relation between the point cluster $c_i$ and ${\mathcal{C}}_{f_{ij}}$ shown in (20) will lead to a relation between their coordinates ${\mathbf{C}}_i$ and ${\mathbf{C}}_{f_{ij}}$ as follows:

As a result, the BA optimization in (9) further reduces to

where the function $\mathbf{A}(·)$ is defined in (16). Note that the cost function in (22) only requires the knowledge of point cluster coordinate ${\mathbf{C}}_{f_{i.j}}$ without enumerating each individual points. The coordinate ${\overline{\mathbf{C}}}_{f_{ij}}$ is computed as [following (10)]:

which can be constructed during the feature association stage before the optimization. In particular, if the jth pose observes no point on the ith feature, ${\mathbf{C}}_{f_{ij}}={\mathbf{0}}_{4\times 4}$ , which will naturally remove the dependence on the jth pose for the ith cost item as shown in (22).

Theorem 3: Given a matrix function $\mathbf{A}(\mathbf{C})\triangleq \frac{1}{N}\mathbf{P}-$ $\frac{1}{N^2}\mathbf{v}{\mathbf{v}}^T$ with $\mathbf { C } = \lceil \mathbf { P } \mathbf { \Psi } \mathbf { \Psi } \mathbf { \Psi } \mathbf { \Psi } \mathbf { v } \rceil \in \mathbb { S } ^ { 4 \times 4 } , \lambda _ { l } ( \mathbf { A } )$denotesthelthlargesteigenvalueofA,then$\lambda _ { l } ( \mathbf { A } \mathbf { \bar { ( C ) } } )$isinvarianttoanyrigidtransformation$\mathbf { T } _ { 0 } \in S E ( 3 )$ . That is

Proof: See Supplementary III-D [59].

Theorem 3 implies that left multiplying all poses ${\mathbf{T}}_j\forall j$ , by the same transform ${\mathbf{T}}_0$ does not change the optimization at all. That is, the BA optimization is invariant to the change of the global reference frame, which is the well-known gauge freedom in a bundle adjustment problem.

D. First- and Second-Order Derivatives

As shown in the previous section, the BA optimization problem in (22) is completely equivalent to the original formulation (1), where each cost item standards for the squared Euclidean distance from a point to a plane (or edge). This squared distance is essentially a quadratic optimization, which requires the knowledge of the second-order information of the cost function for efficient solving. In this section, we derive such first- and second-order derivatives. Without loss of generality, we only discuss the ith feature, which contributes a cost item in the form of

with ${\mathbf{C}}_{f_{ij}}\in {\mathbb{R}}^{4\times 4}$ being a precomputed matrix [see (23)].

To derive the derivative of the cost item (25) w.r.t. the pose ${\mathbf{T}}_j$ , which is an element of the Special Euclidean group $SE(3)$ we parameterize its perturbation by a special addition, called boxplus (-operation). For the pose vector $\mathbf{T}=(\dots ,{\mathbf{T}}_j,\dots )$ we define the the  operation as follows:

where $\delta { \bf T } \triangleq ( \operatorname { \mathrm { . ~ . ~ . ~ } } , \delta { \bf T } _ { j } , \operatorname { \mathrm { . ~ . ~ . ~ } } ) \in \mathbb { R } ^ { 6 M _ { \tau } }$with$\delta \mathbf { T } _ { j } \triangleq ( \delta \phi _ { j } , \delta \mathbf { t } _ { j } ) \in$ $\mathbb { R } ^ { 6 } \quad \forall j \in { 1 , \dots , M _ { p } } ,$ , is the perturbation on the pose vector.

For a scalar function $f(\mathbf{T}):SE(3)\times \dots \times SE(3)\mapsto$ R, denote $\left(\frac{\partial f(\mathbf{T})}{\partial \mathbf{T}}\right)\left({\mathbf{T}}_0\right)$ its first-order derivative and $\left(\frac{\partial f^2(\mathbf{T})}{\partial {\mathbf{T}}^2}\right)({\mathbf{T}}_0)$ its second-order derivative, both at a chosen point of the input ${\mathbf{T}}_0.$ . The  operation enables us to parameterize the input of function f(·), T, by its perturbation δT from a given point ${\mathbf{T}}_0\mathbf{:}$ $\mathbf{T}={\mathbf{T}}_0$  δT. Since the map between T and $\delta \mathbf{T}$ is bijective if $\Vert \delta {\varphi }_j\Vert <\pi \forall j$ , the scalar function $f(\mathbf{T})$ in terms of $\mathbf{T}$ can be equivalently written as a function $f({\mathbf{T}}_0⊞\delta \mathbf{T})$ in terms of $\delta \mathbf{T}$ As a consequence, the derivatives of $f(\mathbf{T})$ w.r.t. T at the point ${\mathbf{T}}_0$ can be defined as the derivatives of $f({\mathbf{T}}_0⊞\delta \mathbf{T})$ w.r.t. δT at zero, where the latter is a normal derivative w.r.t. Euclidean vectors

In the following discussion, we use T as the reference point to replace ${\mathbf{T}}_0$ in the derivatives and omit it for the sake of notation simplification.

Based on the derivatives defined in (28) and (29), we have the following results for the first and second-order derivatives of the cost item (25).

Theorem 4: Given

1) Matrices $\mathbf { C } _ { j } = [ \mathbf { P } _ { j } \quad \mathbf { v } _ { j } ] _ { \ u { \mathbf { v } } _ { j } } \mathbf { \Psi } _ { j } = , \ldots , M _ { p } .$

2) Poses ${\mathbf{T}}_j\in S\overline{E(3)},j=\overline{1},\dots ,M_p$

3) A matrix $\mathbf { C } = \left| \begin{array} { l l } { \mathbf { P } } & { \mathbf { v } } \ { \mathbf { v } ^ { T } } & { N } \end{array} \right| \triangleq \sum _ { j = 1 } ^ { M _ { p } } \mathbf { T } _ { j } \mathbf { C } _ { j } \mathbf { T } _ { j } ^ { T } \in \mathbb { S } ^ { 4 \times 4 } ,$,whichistheaggregationof$\mathbf { C } _ { j } .$,andamatrixfunction$\mathbf { A } ( \mathbf { C } ) \triangleq$ $\begin{array} { r } { \frac { 1 } { N } \mathbf { P } - \frac { 1 } { N ^ { 2 } } \mathbf { v } \mathbf { v } ^ { T } \in \mathbb { S } ^ { 3 \times 3 } } \end{array}$

4) A function ${\lambda }_l(\mathbf{A}(\mathbf{C})),{\lambda }_l(\mathbf{A})$ denotes the lth $(l=1,2,3)$ largest eigenvalue of A with corresponding eigenvector u_l.

The Jacobian matrix ${\mathbf{J}}_l$ and Hessian matrix ${\mathbf{H}}_l$ of the function ${\lambda }_l(\mathbf{A}(\mathbf{C}))$ ) with respect to the poses T are

where ${\mathbf{g}}_{ll}$ is ${\mathbf{g}}_{kl}$ with $k=l.{\mathbf{g}}_{kl}$ and ${\mathbf{W}}_l$ are matrices partitioned as

with block elements $\mathbf { g } _ { k l } ^ { j } \in \mathbb { R } ^ { 1 \times 6 } , \mathbf { W } _ { l } ^ { i j } \in \mathbb { R } ^ { 6 \times 6 } , \forall i , j \in \lbrace 1 , \dots ,$ $M _ { p } }$ defined as

Proof: See Supplementary III-E [59].

Corollary 4.1: The Jacobian matrix J_l and Hessian matrix ${\mathbf{H}}_l$ in Theorem 4 satisfy that, for any l = 1, 2, 3

where ${\mathbf{J}}_l^j$ is the jth column block of ${\mathbf{J}}_l$ and ${\mathbf{H}}_l^{ij}$ is the ith row, jth column block of H_l.

Proof: See Supplementary III-F [59].

Remark 3: (39) implies that the Jacobian and Hessian matrices have null space containing the space spanned by . This essentially means that the cost function (25) in the BA optimization does not change along the direction where all the poses are perturbed by the same quantity w, which agrees with gauge freedom stated in Theorem 3.

Remark 4: The results in (40) and (41) imply that the blocks in Jacobian and Hessian matrices are zeros and hence their computation can be saved if any of the related poses does not observe the current feature $(\mathrm{i}.\mathrm{e}.,{\mathbf{C}}_i=\mathbf{0}\mathrm{o}\mathrm{r}{\mathbf{C}}_j=\mathbf{0})$ . This sparse structure could save much computation time if a feature is observed only by a sparse set of poses.

Remark 5: The derivatives in Theorem 4 are obtained based on the pose perturbation defined in (26), which multiplies the perturbation δT on the left ofthe current pose (i.e., a perturbation in the global frame). If other perturbation (denoted by δT^˘ ) is preferred (e.g., a perturbation in the local frame to integrate with other measurements such as IMU pre-integration), where $\delta \mathbf{T}=\mathbf{L}\delta \breve{\mathbf{T}}$ with L the Jacobian between the two perturbation parameterization, its first- and second-order derivatives can be computed as $\breve{\mathbf{J}}=\mathbf{J}$ · L and $\breve{\mathbf{H}}={\mathbf{L}}^T$ · H · L, respectively. It can _{be seen that J}˘ _{and H}˘ _{preserves a nullspace of L · with} defined in (39).

E. Second-Order Solver

The Jacobin and Hessian matrix from Theorem 4 are computed for one cost item (25) that corresponds to one feature in the space. Denote ${\mathbf{J}}_i,{\mathbf{H}}_i$ the Jacobian and Hessian matrix for the ith feature (or cost item), to determine the incremental update $\Delta \mathbf{T}$ , we make use of the second order approximation of the total cost function c(T) in (22)

where $\begin{array} { r } { \mathbf { J } = \sum _ { i = 1 } ^ { M _ { f } } \mathbf { J } _ { i } , \mathbf { H } = \sum _ { i = 1 } ^ { M _ { f } } \mathbf { H } _ { i } } \end{array}$.Foranyd$\in w$,since$\mathbf { J } _ { i } \mathbf { d } = 0 , \mathbf { d } ^ { T } \mathbf { H } _ { i } \mathbf { d } = 0 \quad \forall i\mathbf { J } \mathbf { d } = 0$and${ \bf d } ^ { T } { \bf H } { \bf d } =$ $\begin{array} { r l } { 0 } & { { } \forall i , } \end{array}$,whichmeansthatanyadditionalupdatealongd$\in { \boldsymbol { w } }$ does not change the approximation at all. One way to resolve the gauge freedom is fixing the first pose at its initial value throughout the optimization. That is, setting $\Delta {\mathbf{T}}_1=\mathbf{0}\mathrm{i}\mathrm{n}(42)$ Then, setting the differentiation of the cost approximation in (42) w.r.t. ΔT (excluding $\Delta {\mathbf{T}}_1)$ to zero leads to the optimal update $\Delta {\mathbf{T}}^{\star }$

where we used a Levenberg–Marquardt (LM) algorithm-like method to reweight the gradient and Newton’s direction by the damping parameter $\mu .$ . The complete algorithm is summarized in Supplementary (Algorithm 1) with time analysis detailed in Supplementary (Section IV). Overall, the solver has a complexity of $O(M_f{M}_p+M_f{M}_p^2+M_p^3)$ , which is linear to the number of feature $M_f,$ , irrelevant to the number of points $N_{:}$ , and cubic to the number of pose $M_p$ . The term $M_f{M}_p$ and $M_f{M}_p^2$ are due to the calculation of Jacobian and Hessian, respectively, and the term $M_p^3$ is due to (43).

F. Covariance Estimation

Assume the solver converges to an optimal pose ${\mathbf{T}}^{\star }$ , it is often useful to estimate the confidence level of the estimated pose. Let T^gt be the ground-true pose, which is unknown, and $\delta {\mathbf{T}}^{\star }$ be the difference between the optimal estimate ${\mathbf{T}}^{\star }$ and the ground-true T^gt, where ${\mathbf{T}}^{\mathrm{g}\mathrm{t}}={\mathbf{T}}^{\star }⊞\delta {\mathbf{T}}^{\star }$ . The aim is to estimate the covariance of the error $\delta {\mathbf{T}}^{\star }$ , denoted by ${\mathbf{\Sigma }}_{\delta {\mathbf{T}}^{\star }}$

Ultimately, the estimation error $\delta {\mathbf{T}}^{\star }$ is caused by the measurement noise in each raw point. Denote ${\mathbf{p}}_{f_{ijk}}^{\mathrm{g}\mathrm{t}}$ the ground-true location of the kth point observed on the ith feature at the jth lidar pose, with measurement noise $\delta {\mathbf{p}}_{f_{ijk}}\in \mathcal{N}(\mathbf{0},{\mathit{\Sigma }}_{{\mathbf{p}}_{f_{ijk}}})$ , the measured point location, denoted by ${\mathbf{p}}_{f_{ijk}}$ , is

Aggregating the ground-true points and the measured ones lead to the ground-true point cluster, denoted by ${\mathbf{C}}_{f_{ijk}}^{\mathrm{g}\mathrm{t}}$ , and the

measured point cluster, denoted by ${\mathbf{C}}_{f_{ijk}}$ , respectively, [see (23)]:

where

which can be constructed in advance along with the point cluster ${\mathbf{C}}_{f_{ij}}$ during the feature associations stage.

In the following discussion, to simplify the notation, we denote $\mathbf { C } _ { f } ^ { \mathrm { g t } } = { \mathbf { C } _ { f _ { i , i } } ^ { \mathrm { g t } } } , \mathbf { C } _ { f } = { \mathbf { C } _ { f _ { i j } } } , \delta \hat { \mathbf { C } } _ { f } \overset { , } { = } { \delta \mathbf { C } _ { f _ { i j } } }$ the groundtruth, measurements, and noises of all point clusters observed on any features at any lidar poses.

Although the ground-true pose ${\mathbf{T}}^{\mathrm{g}\mathrm{t}}$ and point cluster ${\mathbf{C}}_f^{\mathrm{g}\mathrm{t}}$ are unknown, they are genuinely the optimal solution of $(22)$ and hence the Jacobian evaluated there should be zero, i.e.,

where we wrote the Jacobian as an explicit function of the pose and point clusters. Now, we approximate the left hand side of (48) by its first order approximation

Noticing that $\begin{array} { r } { \frac { \mathbf { J } ^ { T } ( \mathbf { T } ^ { \star } \boxplus \delta \mathbf { T } , \mathbf { C } _ { f } ) } { \partial \delta \mathbf { T } } = \mathbf { H } ( \mathbf { T } ^ { \star } , \mathbf { C } _ { f } ) } \end{array}$,theHessianmatrixof(22)evaluatedat$\bar { \mathbf { T } ^ { \star } }$ [also see (42)], we have

which implies

Since ${\mathbf{T}}^{\star }$ is the converged solution using the measured cluster ${\mathbf{C}}_f$ , they should lead to zero update, i.e., ${\mathbf{H}}^{-1}{\mathbf{J}}^T({\mathbf{T}}^{\star },{\mathbf{C}}_f)=0$ [see (43) with zero $\mu$ at convergence]. Therefore

We defer the exact derivation and results of $\frac { \mathbf { J } ^ { T } ( \mathbf { T } ^ { \star } , \mathbf { C } _ { f } ) } { \partial \mathbf { C } _ { f } } \delta \mathbf { C } _ { f } ,$ $\mathbf { \Sigma } _ { \pmb { \Sigma } _ { \delta \mathbf { C } _ { j } } }$and$\pmb { \Sigma } _ { \delta \mathbf { T } ^ { \star } }$toSupplementaryIII-G[59].Notethattheevaluationof$\pmb { \Sigma } _ { \delta \mathbf { T } } .$-onlyrequiresthecovariance$\delta \mathbf { C } _ { f _ { i j } }$ , which has been constructed in advance according to (47), avoiding the enumeration of each raw point during the optimization.

IV. IMPLEMENTATIONS

We implemented our proposed method in C++ and tested it in Unbuntu 20.04 running on a desktop equipped with Intel i7- 10750H CPU and 16 Gb RAM. Since the reduced optimization problem (22) is not in a standard least square problem, which existing solvers (e.g., Google Ceres [60]) applies to, we implemented the optimization algorithm with steps and parameters described in Supplementary (Algorithm 1). When solving the linear equation on Line 9 at each iteration, we use the LDLT Cholesky decomposition decomposition method implemented in Eigen library 3.3.7. The termination conditions on Line 19 are iteration number below $50(\mathrm{i}.\mathrm{e}.,j_{\mathrm{m}\mathrm{a}\mathrm{x}}=50)$ , rotation update below $10^{-6}$ rad, and translation update below $10^{-6}$ m.

V. CONSISTENCY EVALUATION

This study aims to verify the consistency of the proposed BA method. That is, whether the estimated covariance $\mathbf{\Delta }{\Sigma }_{\delta \mathbf{T}},$ from (53) agrees with the ground-true covariance of the pose estimation error $\delta {\mathbf{T}}^{\star }$ . As the ground-true covariance is unknown, we refer to a standard measure of consistency, the normalized estimation error squared (NEES) [61], [62], which is defined as follows:

where $\delta {\mathbf{T}}_j^{\star }$ is the estimation error of the pose defined according to (27)

where the superscript ${}^{66}{\mathrm{g}\mathrm{t}}^7$ denotes the ground-true poses and $({\mathbf{R}}_j^{\star },{\mathbf{t}}_j^{\star })$ denotes the estimated pose for the jth scan. Assume the pose estimate $({\mathbf{R}}_j^{\star },{\mathbf{t}}_j^{\star })$ is unbiased $(\mathrm{i}.\mathrm{e}.,E(\delta {\mathbf{T}}_i^{\star })=\mathbf{0})$ , if the computed covariance ${\mathbf{\Sigma }}_{\delta {\mathbf{T}}^{\star }}$ is the ground-truth, we can obtain the expectation

(54)

That is, if the solver is consistent, the expectation of NEES should be equal to the dimension of the optimization variable. If the expectation of NEES is far higher than the dimension, the estimator is over-confident (i.e., the computed covariance is less than the ground-truth). Conversely, it is conservative.

In practice, the expectation of NEES is evaluated by Monte Carlo method, where the NEES is computed for many runs and then averaged to produce the empirical expectation

where ${\eta }^{(i)}$ is the NEES computed at the ith Monte Carlo run.

To conduct the Monte Carlo evaluation, we simulate a 16-channel lidar along a rectangular trajectory in a cuboid semiclosed space shown in Fig. 5. The size ofthe space is 30 m ×

(a)

Fig. 5. Simulation setup: A 16-channel lidar moves along a rectangular trajectory in a cuboid semiclosed space. The white line is the trajectory and the red lines are the laser points.

Fig. 6. Error (red) of rotation (deg) and position (m) with 3σ bounds (blue) for one simulation run.

20 $\mathrm{m}\times 8$ m and the length of trajectory is about 92 m. 100 scans are equally sampled on the trajectory and the number of points in each scan is 28,800. To simulate realistic measurements, each point is corrupted with an independent isotropic Gaussian noise with multiple standard deviations ${\sigma }_p\in \{0.05,0.10,\dots ,1.00\}$ m and for each value of the standard deviation ${\sigma }_p,$ we performed 100 Monte Carlo experiments, leading to a total number of 2000 experiments. In each run, we compute the optimal pose estimate from the Supplementary (Algorithm 1) with the same parameters specified in Section IV and the covariance matrix from (53). The initial trajectory required by the algorithm is obtained by perturbing the ground-true trajectory with a Gaussian noise with standard deviation $\delta \varphi =2$ deg and $\delta t=0.1$ m on each pose. To avoid unnecessary errors, we use the ground-true plane association across different scans and ignore the in-frame motion distortion in the simulation.

Fig. 6 shows orientation and position errors with the corresponding 3σ bounds in one Monte Carlo experiment with ${\sigma }_p=0.05$ m. As can be seen, the pose estimation errors are very small and they all remain within the 3σ bounds very well, which suggests that our new method is consistent.

Furthermore, we test the consistency of our BA method under different levels of point noise, where the standard deviation of a point noise ranges from ${\sigma }_p=0.05$ to 1 m. The results are shown in Fig. 7(a) for the NEES averaged over 100 runs for each noise level and in Fig. 7(b) for the average pose error. For better visualization, the average NEES is normalized by the pose dimension (i.e., 600 for 100 poses on the trajectory) in Fig. 7(a). As can be seen, the normalized average NEES is very close to one, which suggests that our method is consistent, when the point noise is up to 0.3 m. Beyond this noise level, the first order approximation in (50) no longer holds, which undermines the accuracy of the computed covariance. We should note that this noise level rarely occurs in actual lidar sensors, which are well below 0.1 m. Moreover, from Fig. 7(b), we can see that our method produces accurate pose estimation even when the point noise are unrealistically large [up to 1 m, see Fig. 7(c) for the point cloud map at this point noise level].

(c)
Fig. 7. (a) Normalized NEES averaged over 100 Monte Carlo runs at different point noise levels. The NEES is normalized by the pose dimension (i.e., 600) for better visualization. (b) Rotation (red) and translation (blue) errors at different point noise levels. (c) Point cloud map with ground-true poses at noise levels $\begin{array}{c}\hfill {\overline{\sigma }}_p=0.05,0.5,\end{array}$ and 1 m, respectively.

VI. BENCHMARK EVALUATION

In this section, we compare our method with other multiview registration methods for lidar point clouds. The experiment will be divided into two parts: Section VI-A evaluates all methods with known feature association on synthetic point clouds, and Section VI-B evaluates the overall BA pipeline including both optimization solver and feature association on various realworld open datasets.

To verify the effectiveness of our method, we compare it with four state-of-the-art methods that focus on the lidar bundle adjustment (or similar) problem: EF [33], BALM [32], plane adjustment (PA) [51], and bundle adjustment for multiview registration (BAREG) [35]. Among them, EF,² BALM,³ BAREG⁴ are open sourced, so we use the available implementation on Github. PA is not available anywhere, so we re-implemented it in C++. To reduce the time cost of PA, we used the reduced Jacobian and residual technique in [34] (we derived it based on the cost function in (10) of [51]), which avoids the enumeration of each individual point. The re-implemented PA is solved by the Ceres solver with “DENSE_SCHUR” [60], which leverages the Schur complement trick to reduce the linear equation dimension at each optimization iteration. To better exploit the separable structure reducing the solving time, we also compare with PA with inner iterations enabled in Ceres [denoted as “PA (inner)”].

For the solver parameters, our method and the reimplemented PA [and its variant PA (inner)] use the parameters specified in Section IV and [34], respectively, while EF, BALM, and BAREG use their default parameters as available on their open source implementation. All methods use the same termination condition shown in Section IV (i.e., maximal iteration number below 200, rotation update below $10^{-6}$ rad, and translation update below $10^{-6}\mathrm{m})$ , except for EF, which we found it converges too slowly and hence set the maximal iteration steps to 2000. In addition to the open source version of BALM (denoted by BALM), which samples only three points from each plane to lower the computation load, we also evaluated another vision [denoted by BALM (full)] which keeps all the points on a plane and use the same default parameters as its open sourced version. All solvers use the same initial pose trajectories detailed later.

A. Synthetic Point Cloud

To verify the effectiveness of the optimization solvers and their scalability to the number of pose $M_p$ , number of feature $M_f$ , and number ofpoints N per feature, we design a point-cloud generator which generates $M_f$ random planes and $M_p$ lidar scans at random poses. Each pose corresponds to one group of point-cloud whose number of points on each plane is $N.$ Hence, there are totally $N{M}_f$ points at each scan. We use the ground-true plane association provided by the simulator. To mimic the real lidar point noises, we also corrupt the points sampled on each plane by an isotropic Gaussian noise with standard deviation ${\sigma }_p=0.05\mathrm{m}$ , the typical noise level for existing lidar sensors. The initial poses are perturbed from the ground-true poses with errors randomly sampled from a Gaussian distribution. The base standard deviation of the Gaussian distribution is $\Vert \delta \varphi \Vert =0.1$ deg for rotation and $\Vert \delta \mathbf{t}\Vert =1$ cm for translation. In the nominal settings, $M_f=M_p=N=100$ and the initial pose error standard deviation is 10× the base value. From the nominal settings, we enumerate each of the $M_f,M_p$ and N at values {10, 30, 100, 300, 1000, 3000} and the initial pose error standard deviation at values 1×, 5×, 10×, 15×, 20×, 25× of the base value to investigate the performance of each solver at different scales. This makes a total number of 21 scenes. In each scene, the experiment is repeated for ten times with separately sampled poses, planes, and point noises, leading to a total 210 experiments.

1) Convergence: First we investigate the convergence performance of all methods. Fig. 8(a) and (b), respectively, shows the convergence of cost and point-to-plane distance in one repeat experiment with the nominal settings $(\mathrm{i}.\mathrm{e}.,M_f=M_p=N=$ 100 and initial pose error 10×). Since different method uses different cost function, to compare them in one figure, the cost value of each method is normalized by its initial cost and then it is rebased such that the converged cost value of all methods are aligned at the same value. Similarly, we normalize the pointto-plane distance by its initial value as well, which is valid to do because all methods have the same initial pose leading to the same initial point-to-plane distance. As can be seen, EF converes rather slowly and requires the most number of iterations. This is because EF optimizes a cost function similar to ours in (9), which is essentially a quadratic function, but uses only the gradient information for optimization. Indeed, slow convergence of the gradient descent method on a quadratic cost function is a very typical phenomenon [63]. PA, PA (inner), and BAREG converge fast at the beginning but slowly when approaching the final convergence value. This is because PA optimizes both the plane parameters and scan poses, leading to a very large number of optimization variables that significantly slow down the speed at convergence. PA (inner) converges faster than the original PA due to the inner iteration, but still slower than our method. For BAREG, the empirical fixation of plane parameters also causes the optimization to slow down. In contrast, BALM, BALM (full), and our method eliminates the plane parameters exactly and the resultant optimization problem is only in dimension of the pose number. Further leveraging the exact Hessian information in their optimization update, BALM, BALM (full), and our method converge in a few iterations, which often represent the fastest convergence.

(b)

EFBALM→BALM (full)→PAPA (inner)BAREG→Our
Fig. 8. Convergence of different methods for BA optimization. (a) Cost value versus iterations in one repeat experiment with the nominal settings $M_f=100.$ $M_p=100,N=100$ and initial pose error 10×. (b) Point-to-plane distance versus optimization time in one repeat experiment. (c) Iteration steps experienced by each method in all repeat experiments (i.e., 10) of all scenes (i.e., 21). The y-axis value represents how many experiments out of the 210 total experiments has experienced the iteration number indicated by the x-axis. (d) Breakdown of time spent on each iteration of all BA methods. The time is averaged among all experiments that all methods have participated.

Fig. 9. Benchmark results on synthetic point cloud.

Fig. 8(c) shows the iteration experienced by each BA optimization method, where for each data point, the y-axis value represents how many out of the total experiments experienced the iteration number indicated by the x-axis. As can be seen, the overall trend agrees with the results in Fig. 8(a) very well: our proposed method and BALM (full) require only four or five iterations, while BAREG, PA, and PA (inner) require up to 20, 40, and 50 iterations, respectively. EF requires even more iterations beyond 100.

Fig. 8(d) shows the computation time in each iteration. As can be seen, EF consumes the least time for each iteration due to the lack of Hessian computation and linear equation solving. BALM (full) consumes the most time since the computation of Jacobian, Hessian, and residuals require to enumerate each individual point, leading to a complexity of $O(N^2)$ . The other methods, PA, PA (inner), BAREG, and ours, consume similar time for each iteration.

2) Accuracy: Fig. $9(\mathrm{a}),(\mathrm{b}),(\mathrm{d}),(\mathrm{e}),(\mathrm{g}),(\mathrm{h}),(\mathrm{j})$ , and (k) shows the statistic values of the pose estimation accuracy in terms of rmse. In each subplot, we fix three parameters of $M_f,M_p,N$ and initial pose error at the nominal values and change the fourth parameter to investigate its effect on the pose accuracy. Since the error of EF is much larger than the others, we used a broken y-axis to better display all the RMSE. As can be seen, overall the accuracy increases with the points per plane N [in (d) and (e)] or number of plane features $M_f$ [in (g) and (h)] since both increases the number of pose constraints. In contrast, no such monotonic accuracy improvement is found for the number of poses [in (a) and (b)] as the pose number increases because increasing the pose number itself does not gives more pose constraints. Likewise, the accuracy also remains similar for different initial poses error for all methods except EF, which did not converge at the maximum iteration number. Relatively speaking, our proposed method and BALM (full) achieves the same highest accuracy, since they essentially optimizes the same cost using the same exact Hessian information. The next best methods are BAREG, PA, and PA (inner). While optimizing the same point to plane distance with our method [and BALM (full)], PA has significantly more optimization variables, which cause a much slower convergence where the solution is still slightly premature at the preset iteration number. Although PA (inner) has used inner iterations to alleviate this problem, its iterations are still larger than our methods and BAREG. The next accurate method is BALM, which samples only three instead of all points [as in our method, BALM (full), BAREG, PA, and PA (inner)] and hence has higher rmse. Finally, EF has the highest rmse due to the very slow convergence, the solution is much premature even at the preset iteration number.

3) Computation Time: Finally, we show the total computation time of different solvers at different feature number $M_f,$ pose number $M_p$ , point number N and initial pose error. The results are shown in Fig. 9(c),(f),(i), and (l). As can be seen, the time consumption of all methods increases with the number of poses $M_p$ [see (c)) and plane features $M_f$ (see (i)], which is reasonable since more poses or planes lead to a higher optimization dimension or more number of cost items, respectively. On the other hand, as the point number N increases [see (f)], the method BALM (full) increases rapidly since its time complexity involves $O(N^2)$ while the rest methods (including ours) do not increase notably since they do not need to evaluate every raw point. For the effect of initial pose errors in [see (1)], they do not affect the solving time significantly.

Relatively speaking, our method achieves the lowest total computation time in all cases due to the small number of iteration numbers [see Fig. 8(c)] and low time-complexity per iteration [see Fig. 8(d)]. The next efficient method is BAREG, which has very low time-complexity per iteration due to the empirical feature parameter fixation but significantly more iteration numbers due to the same reason. Compared with our method, PA [and PA (inner)] has similar time complexity per iteration as discussed in Supplementary (Section IV), but requires more iterations to converge. Hence their time costs are a little higher than ours and BAREG. BALM requires more iterations than our method and more time in each iteration due to the enumeration ofthe sampled points. Collectively, it leads to a computation time higher than our method and also BAREG and PA [and PA (inner)]. The slow convergence problem is more severe in EF, leading to an even higher computation time. Finally, the most time-consuming method is BALM (full), which, although has very small iteration numbers, consumes large time in each iteration.

B. Real-World Datasets

In this experiment, we conduct benchmark comparison on three real-world datasets. The first dataset is “Hilti” [64] which is a handheld SLAM dataset including indoor and outdoor environments. We use the lidar data collected by Ouster OS0-64 in the dataset. The ground-true lidar pose trajectory is captured by a total station or motion capture system. The second dataset “VIRAL” [65] is collected on an unmanned aerial vehicle (UAV) equipped with two 16-channel OS1 lidars. One lidar is horizontal and the other is vertical. We will use the horizontal one in this experiment. The ground-true positions are provided by a Leica Nova MS60 MultiStation tracking a crystal prism on the UAV. The last dataset “UrbanLoco” [66] is collected by a car driving on urban streets. The lidar is a Velodyne HDL 32E and the ground truth is given by the Novatel SPAN-CPT, a navigation system incorporating Real Time Kinematic (RTK) and precisional IMU measurements.

Two preprocessing are performed for all sequences: motion compensation and scan downsample. To compensate the points distortion caused by continuous lidar movements within a scan, we run a tightly coupled lidar-inertial odometry, FAST-LIO2 [19], which estimates the IMU bias (and other state variables) and compensates the point motion distortion in real-time. We kept all points in a scan whose distortion has been compensated by FAST-LIO2 and discard the odometry output. The processed data are then downsampled from the original 10 to 2 Hz for all sequences. This is because the BA methods need to process all scans at once, a 10 Hz scan rate causes prohibitively high computation load for all BA methods. The downsampling is also similar to the keyframe selection in common SLAM frameworks.

We compare our method with EF, BALM, PA, PA (inner), and BAREG. Noticing that the computation time of BALM (full) is prohibitively high due to the extremely large number of lidar points, we hence remove it from the benchmark comparison. For the rest methods, their solver parameters are kept the same for all sequences with values detailed in previous sections.

For feature association, we use the adaptive voxelization proposed in BALM [32], which registers all points in the world frame (using an initial trajectory) and recursively cuts the space into smaller subvoxels until the subvoxel contains only one feature (either plane or edge) that associates points from different scans. EF did not address the feature association problem and PA did not open relevant codes, so we use this method for them too. BAREG used a similar adaptive voxelization method but has its own implementation, so we retain its own implementation. All feature associations have the same set of parameters: the root voxel size L = 1 m for “Hilti” and L = 2 m for ${}^{66}VIRA{L}^{33}$ and “UrbanLoco,” the maximum voxelization layer $l_{\mathrm{m}\mathrm{a}\mathrm{x}}=3.$ the minimum number of points $n_{\mathrm{m}\mathrm{i}\mathrm{n}}=20$ for a feature test, and the feature test thresholds $\gamma =\frac{1}{25}$

The above feature association method is able to extract and associate both plane and edge features. Since the other BA methods, including EF, PA [and PA (inner)], and BAREG, are only designed for plane features, we use only plane features for them. For our method, it is applicable to both plane and edge features, so we test two variants: the one with only plane features, denoted as “Ours,” for comparison with other BA methods, and the one with both plane and edge features, denoted as “Ours (edge).” Moreover, besides the default implementation of our method with double-precision numbers, we test the stability of our method with single-precision floating number implementation, denoted as “Ours(float).” Note that all other BA methods were implemented with double-precision.

In addition to the multiview registration methods, we also compare with classic pairwise registration methods, including ICP, generalized-ICP (GICP), and NDT offered in PCL library. We run the pairwise registration methods in an incremental manner, where each new scan is registered and merged to previous scans incrementally. To constrain the computation time, in each new scan registration, only the last 20 scans are used. The pose estimation from the ICP is then used as the initial trajectory for feature association and optimization of the BA methods, including EF, BALM, PA, PA (inner), BAREG, and ours.

1) Accuracy: Table II shows the ATE results. As can be seen, our method consistently achieves the best results in all 19 sequences even with single-precision. The next accurate method is PA (inner), PA and BAREG, followed by BALM and EF. This trend is in great agreement with the results on synthetic point cloud shown in Section VI-A-2. In particular, our method achieves an accuracy within a few centimeters in all sequences of “Hilti” and “VIRAL,” with only one exception (i.e., UzhArea2), which will be analyzed later. The centimeter level accuracy achieved by our method is at the same level of lidar point noises. Moreover, using only lidar measurements, our method achieved an average accuracy of 4.2 cm on all VIRAL dataset sequences, which outperforms the accuracy 4.7 cm reported in VIRAL-SLAM [65] that fuses all data from stereo camera, IMU, lidar, and UWB. The accuracy on “UrbanLoco” is lower (analyzed later) than other datasets, but still outperforms the other BA methods. Finally, we can notice that the BA methods [i.e., EF, BALM, PA, PA (inner), BAREG, and ours] generally outperforms the pairwise registration methods (i.e., ICP, GICP, and NDT) due to the full consideration of multiview constraints.

When comparing among different variants of our method, the single-precision implementation has a lower accuracy than double-precision as expected, but it offers significant time savings as discussed later. The incorporation of edge features leads to no noticeable accuracy improvement. The accuracy difference with and without edge features are as small as 6 mm. This is because in real-world point clouds, edge features extracted based on local smoothness (e.g., [18]) are very noisy because the laser pulse emitted by lidars can barely hit an edge exactly due to the limited angular resolution. The situation is further exacerbated when the edge is located at far or when the lidar has increased laser beam divergence, which creates many bleeding points behind an edge and degrades the edge points extraction more [67]. On the other hand, in real-world environments, edge features are often created by depth discontinuity at the edge of a foreground object, which meanwhile makes a good plane feature, so adding the edge feature does contribute many new effective constraints.

TABLE II
ABSOLUTE TRAJECTORY ERROR (RMSE, METERS) FOR DIFFERENT METHODS