CoBigICP: Robust and Precise Point Set Registration using Correntropy Metrics and Bidirectional Correspondence

Given two point clouds $\mathcal{A}$ and $\mathcal{B}$, point cloud registration problem aims at finding a rigid transformation that best aligns the source point cloud $\mathcal{B}=\{{\bf b}_{1:N_{s}}\}$ to the target point cloud $\mathcal{A}=\{{\bf a}_{1:N_{t}}\}$ [8], where $\bf a$ and $\bf b$ denote the 3D points. We define the association variable $\mathcal{I}\overset{\Delta}{=}\left\{m_{i},n_{i}\right\}_{i=1}^{N}\in\mathcal{I}_{g}$ where $m_{i}$, $n_{i}$ indicate ${\bf a}_{i}\overset{\Delta}{=}{\bf a}_{m_{i}}\in\mathcal{A}$ is a measurement of the same point as ${\bf b}_{i}\overset{\Delta}{=}{\bf b}_{n_{i}}\in\mathcal{B}$, and $\mathcal{I}_{g}$ is the set of all possible pair-wise correspondences. A general framework of ICP [10] is below:

$$\mathop{\min}_{\bf T}\sum\limits_{i=1}^{N}\left({\bf a}_{i}-\mathbf{T}{\bf b}_{i}\right)^{\top}\mathbf{\Omega}_{i}\left({\bf a}_{i}-\mathbf{T}{\bf b}_{i}\right),$$

(1)

where ${\bf{T}}\in{\rm SE(3)}$ denotes a rigid transformation that maps $\mathcal{B}$ to the $\mathcal{A}$, namely, ${\bf T}{\bf a}_{i}={\bf R}{\bf b}_{i}+{\bf t}$, $\mathbf{R}\in{\rm SO(3)}$, ${\bf{t}}\in\mathbb{R}^{3}$ are rotation matrix and translation vector. $\mathbf{\Omega}_{i}$ denotes the information matrix that captures the local geometrical shape of the point pair ${\bf a}_{i}$ and ${\bf b}_{i}$ and ${\bf e}_{i}$ denotes the pair-wise error ${\bf e}_{i}={\bf a}_{i}-\mathbf{T}{\bf b}_{i}$. The original version of ICP algorithm [10] defines $\mathbf{\Omega}_{i}=\bf{I}$. For the variant who applies point-to-plane metric, it is the case where $\mathbf{\Omega}_{i}={\bf n}_{i}{\bf n}_{i}^{\top}$, with ${\bf n}_{i}\in\mathbb{R}^{3}$ the surface normal of point ${\bf a}_{i}$.

The ICP algorithm can be summarized into two steps:

1. 1.

   Correspondence step: is to find a putative correspondence between two scans.

2. 2.

   Minimization step: is to compute a transformation via minimization of differences between corresponding points.

In practice, the exact correspondence in correspond step is usually unknown. But some approximations like nearest neighbor are proven to be effective under the premise of a good initial value. In the minimization step the difference between two point clouds are usually modeled by the sum of squared errors.

From a probabilistic perspective, the objective function of ICP can be reformulated to a well-defined parameter estimation problem and can be efficiently solved via maximum likelihood estimation (MLE). To be more specific, each pair-wise distance error is view as an independent measurement. A proper probabilistic distribution is utilized to generate all these measurements (noise modeling). Then minimizing the objective function (1) is equivalent to maximizing the log likelihood function.

We remark here that GICP, NICP and NDT [9] are within the aforementioned general ICP framework (1). With the noise modeling method of GICP and NDT being different.

GICP attaches a probabilistic model to the minimization step. More explicitly, sets of underlying points are modeled by assigning Gaussian distributions to each of the points, ${\bf a}_{m}\sim\mathcal{N}\left(\mathbf{\hat{a}}_{m},\bm{\Sigma}_{m,A}\right)$, ${\bf b}_{n}\sim\mathcal{N}(\mathbf{\hat{b}}_{n},\bm{\Sigma}_{n,B})$. $\bm{\Sigma}_{m,A}$ and $\bm{\Sigma}_{n,B}$ stand for covariance matrices of each centered point set. With a correspondence $\mathcal{I}$ computed by nearest neighbor, the pair-wise error can be derived as $\mathbf{e}_{i}\sim\mathcal{N}\left(\mathbf{0},\bm{\Sigma}_{i,A}+\mathbf{R}\bm{\Sigma}_{i,B}\mathbf{R}^{\top}\right)$. In this context, any pair-wise error is drawn from the above multi-variate Gaussian distribution. Therefore, with MLE, the negative log-likelihood function is derived:

$$\mathbf{T}^{\ast}=\mathop{\arg\min}_{\mathbf{T}}\sum\limits_{i=1}^{N}\mathbf{e}_{i}^{\top}{\left(\mathbf{\Sigma}_{i,A}+\mathbf{R}\mathbf{\Sigma}_{i,B}\mathbf{R}^{\top}\right)^{-1}}\mathbf{e}_{i},$$

(2)

where the smallest eigenvalue of the two covariance matrices are replaced by a small constant $\epsilon$ to force points to lie on a local planar.

Like many ICP variants, GICP employs heuristic correspondence (nearest neighbor) which is not robust to outliers. Moreover, the information matrix in (2) is computationally expensive. Also the Gaussian assumption is brittle in complex scenarios where large portion of outliers pervasively exist.

As for NDT, the algorithm firstly subdivide the reference scan space into a grid of cells. For each cell, a PDF is computed to approximate the local surface. Then the goal of NDT is to find the best pose transform $\mathbf{T^{\ast}}$ that maximize the likelihood of the the current scan points lie on the reference surface.

Importantly, the PDF of the cells is considered to be a mixture of normal distribution and a uniform distribution (in [11] Chapter 6) to take the distribution of outliers into consideration:

$$\widetilde{p}({\bf x})=c_{1}\mathcal{N}(\bm{\mu},\Sigma)+c_{2}p_{0},$$

(3)

where $c_{1}$ and $c_{2}$ are constants and can be computed based on the user-specified outlier ratio.

NDT also neglects the importance of the correspondence step and adopts an implicit nearest neighbor. The spurious correspondence loses accuracy and the fixed portion of outlier makes NDT nonflexible.

We incorporate both point clouds local structure into the objective function in a two-way process, namely bidirectional correspondence. The bidirectional correspondence includes two parts: the bidirectional search and the bidirectional distance residual. Take ${\bf a}_{i}$ in the source point cloud and its corresponding point ${\bf b}_{i}$ in the target point cloud as an example. The bidirectional search is with the following mathematical formulation:

$\displaystyle\begin{split}\mathcal{I}_{bi}=\left\{m_{i},n_{i}|m_{i}=i,n_{i}=\mathcal{C}^{*}_{f}(i),\left\|{\bf a}_{\mathcal{C}^{*}_{b}(i)}-{\bf a}_{i}\right\|_{2}<\epsilon\right\}_{i=1}^{N},\end{split}$

(4)

where $\mathcal{I}_{bi}$ is an association variable denoting a set of putative correspondences, $m_{i},n_{i}$ are indices of the corresponding points and $\epsilon$ is an Euclidean distance bound. We adopt the following formulation to build the forward correspondence:

$$\mathcal{C}^{*}_{f}(i)=\mathop{\arg\min}_{\mathcal{C}_{f}(i)\in\left\{1,2,3,\cdots N_{s}\right\}}\left(\left\|{\bf a}_{i}-\mathbf{T}{\bf b}_{\mathcal{C}_{f}(i)}\right\|_{2}^{2}\right),$$

(5)

where $\left\|\cdot\right\|_{2}$ is the Euclidean norm. Conversely, we can compute the backward correspondence $\mathcal{C}^{*}_{b}(i)$ based on the acquisition of the forward correspondence as:

$$\mathcal{C}_{b}^{*}(i)=\mathop{\arg\min}_{\mathcal{C}_{b}(i)\in\left\{1,2,3,\cdots N_{t}\right\}}\left(\left\|{\bf a}_{\mathcal{C}_{b}(i)}-\mathbf{T}{\bf b}_{\mathcal{C}^{*}_{f}(i)}\right\|_{2}^{2}\right).$$

(6)

By taking this measure, quantity of involving points is reduced while correspondence is also ameliorated.

For the minimization step, to incorporate local information into the objective function, we propose the bidirectional distance residual. Firstly, we adopt the same approximation as NICP [3]: neglect the covariance matrix of the source point cloud. This leads to a fixed information matrix during the whole registration process. We denote $\mathcal{I}_{bi}$ as the result of the bidirectional search and the uni-directional distance objective function can be formulated as:

$$\mathcal{J}_{uni}(i)=\left({\bf a}_{i}-\mathbf{T}{\bf b}_{i}\right)^{\top}\mathbf{\Omega}_{i,A}\left({\bf a}_{i}-\mathbf{T}{\bf b}_{i}\right).$$

(7)

Inversely, to incorporate the structure of the target point cloud, the following bidirectional distance objective function is derived:

	$\displaystyle\mathcal{J}_{bi}(i)$	$\displaystyle=\left({\bf a}_{i}-\mathbf{T}{\bf b}_{i}\right)^{\top}\mathbf{\Omega}_{i,A}\left({\bf a}_{i}-\mathbf{T}{\bf b}_{i}\right)$		(8)
		$\displaystyle+\left({\bf b}_{i}-\mathbf{T}^{-1}{\bf a}_{i}\right)^{\top}\mathbf{\Omega}_{i,B}\left({\bf b}_{i}-\mathbf{T}^{-1}{\bf a}_{i}\right).$		(9)

With ${\bf b}_{i}-\mathbf{T}^{-1}{\bf a}_{i}=\mathbf{T}^{-1}\left(\mathbf{T}{\bf b}_{i}-{\bf a}_{i}\right)$, the above equation can be simplified as:

$$\mathcal{J}_{bi}(i)\!=\!\left({\bf a}_{i}\!-\!\mathbf{T}{\bf b}_{i}\right)^{\top}\left(\mathbf{\Omega}_{i,A}\!+\!{\bf R}\mathbf{\Omega}_{i,B}{\bf R}^{\top}\right)\left({\bf a}_{i}\!-\!\mathbf{T}{\bf b}_{i}\right),$$

(10)

with

$$\mathbf{\Omega}_{i}=\mathbf{\Omega}_{i,A}+{\bf{R}}\mathbf{\Omega}_{i,B}{\bf{R}}^{\top}$$

(11)

being the bidirectional information matrix. Recall the information matrix of GICP is:

$$\mathbf{\Omega}_{i}^{G}=\left(\mathbf{\Omega}_{i,A}^{-1}+{\bf R}\mathbf{\Omega}_{i,B}^{-1}{\bf R}^{\top}\right)^{-1}.$$

(12)

More explicitly, the bidirectional residual is achieved as follows: by fixing the target point cloud and applying the transformation to the source point cloud, we get the forward residual error and by fixing the source point cloud and applying the inverse transformation to the target point cloud, we get the backward residual error. Compare (12) with (11), the latter can be view as an approximation of the original GICP one. We avoid to compute one time of matrix inversion with the aforementioned approximation and ease the computation of gradient.

The intuition of the above formation comes from a neutrality thinking. Given a correspondence between a point-pair, it is not reasonable to emphasize any of them. The source point cloud contains equivalent amount of information with respect to the target point cloud. Both of the two point clouds should be treated equally and have an equal contribution to the final objective function. Consequently, information of both point clouds have been considered and a bidirectional correspondence have been established.

In most variants of ICP, mean squared error (MSE) is used to measure the the residual error. These methods cannot efficiently resist the presence of heavy outlier. Fig. 2(a) illustrates an MSE-kind error function in the joint space. MSE is a quadratic function, it takes small values in the bottom of the valley and equals 0 on the $x=y$ line. But with the difference between $x$ and $y$ getting bigger, the MSE function rises quadratically and have an amplification effect on the residual error. This makes MSE capable of dealing with distributions like Gaussian but not proper for heavy-tail distributions. The latter is exactly the case when outliers present in points clouds[12].

An illustration of correntropy is shown in Fig. 2(b) in the joint space. As a matter of fact, correntropy emphasizes the error close to the $x=y$ line, and drops steadily while being far away from it, which means correntropy can restrict the residual error from been excessively amplified and is more suitable for being a similarity measure for non-Gaussian variables.

As stated by Hasanbelliu et al. in [12], with a finite number of points $\left\{{\bf x}_{i},{\bf y}_{i}\right\}_{i=1:N}$, a sample estimator of correntropy can be drawn as:

$$\hat{v}\left(X,Y\right)=\frac{1}{N}\sum\limits_{i=1}^{N}G_{\sigma}\left({\left\|{\bf x}_{i}-{\bf y}_{i}\right\|_{2}^{2}}\right),$$

(13)

where $G_{\sigma}\left(r^{2}\right)=\frac{1}{\sqrt{2\pi}\sigma}\exp{\left(-\frac{r^{2}}{2\sigma^{2}}\right)}$ is the density function of the Gaussian kernel with the bandwidth parameter $\sigma$. For more information about the derivation, please refer to [12].

Correntropy is known to be robust to non-Gaussian noise and it has been introduced to the ICP algorithm [13]. Though they are widely used in robotics, the concept of entropy and parameter tuning are often confusing and can be challenging in real world applications. However, we discover that correntropy is equivalent to noise modeling in NDT. Therefore, it is possible to apply knowledge relating to NDT to correntropy metrics directly.

If we consider a pair of points $\left({\bf a}_{i},{\bf b}_{i}\right)$ is associated by bidirectional search $\mathcal{I}_{bi}$. The relationship between the two points can be drawn as ${\bf a}_{i}=\mathbf{T^{\ast}}{\bf b}_{i}+{\bm{\epsilon}}_{i}$, where ${\bm{\epsilon}}_{i}$ is the sensor error. It is well-handled by the noise modeling of GICP. While the point-pair belongs to inlier, the Gaussian assumption of the pair-wise residual error is valid. However, when one of them belongs to outlier, the pair-wise residual error ${\bf e}_{i}={\bf a}_{i}-\mathbf{T^{\ast}}{\bf b}_{i}-{\bf\epsilon}_{i}$ cannot be properly modeled by a single Gaussian distribution, but a mixture of a Gaussian and a uniform distribution:

$${\bf e}_{i}\sim\mathcal{N}\left({\bf 0},\bf{\Omega}_{i}\right)+\mathcal{U}_{i},$$

(14)

or equivalently as:

$$p_{mix}\left({\bf e}_{i}\right)=c_{1}\exp{\left(-\frac{{\bf e}_{i}^{\top}\bf{\Omega}_{i}{\bf e}_{i}}{2}\right)}+c_{2}p_{0},$$

(15)

where $p_{0}$ is the ratio of outlier. Using MLE, the objective function can be derived as:

$$\mathbf{T}^{\ast}\!=\!\mathop{\arg\max}_{\mathbf{T}}\prod\limits_{i=1}^{N}p_{mix}\left({\bf e}_{i}\right)\!=\!\mathop{\arg\min}_{\bf T}\sum\limits_{i=1}^{N}-\log{p_{mix}\left({\bf e}_{i}\right)}$$

(16)

The PDF in (15) do not have a good mathematical form in the optimization perspective as the formed objective function has no simple first- and second-derivatives therefore we use the approximation provided by [9] to the negative log-likelihood in (16):

$$-\log{\left(p_{mix}\left({\bf e}_{i}\right)\right)}\approx d_{1}\exp\left(-d_{2}\frac{{\bf e}_{i}^{\top}\bf{\Omega}_{i}{\bf e}_{i}}{2}\right)+d_{3},$$

(17)

where $d_{1}$, $d_{2}$ and $d_{3}$ are computed by requiring the approximation function behaves similarly to $-\log{p_{mix}\left({\bf e}_{i}\right)}$ at extreme values. Fig. 3(b) shows shows the validity of the aforementioned approximation (the green line).

The parameter $d_{3}$ can be omitted as it is just an offset and has no affect on solving the minimum of a summation objective function in (16).

Comparing (17) to (13), they have a same kind of mathematical formulation with a scaled square of the Euclidean distance inside of the exp term. The bandwidth parameter $\sigma$ in (13) plays the same role as $d_{2}$ in (17). For NDT, the outlier ratio is fixed, which is equivalent to a fixed $\sigma$ in correntropy. So we draw the conclusion that correntropy is equivalent to noise modeling in NDT. Thanks to the insight in optimization in correntropy based applications, the $\sigma$ should be gradually changed during the optimization, which is equivalent to adaptively tuning the outlier ratio for NDT. Therefore, the proposed CoBigICP has the potential to be more robust and accurate than NDT.

To recap briefly, the pair-wise error can be view as Gaussian if they are inliers and cannot if they are outliers. Since we can never have the perfect correspondence, the error distribution may not be Gaussian and MSE is not the optimal. We use a more delicate noise model that is a mixture of a Gaussian distribution and a uniform distribution similar to NDT. As proved before, this is actually a correntropy-based noise modeling technique. So, with the bidirectional search $\mathcal{I}_{bi}$ the objective function of the proposed method is formulated as:

$\displaystyle\begin{split}\mathbf{T}^{\ast}&=\mathop{\arg\max}_{\mathbf{T}}\sum\limits_{i=1}^{N}G_{\sigma}(r_{i}),\\ r_{i}^{2}&={\left({\bf a}_{i}-\mathbf{T}{\bf b}_{i}\right)^{\top}\bf{\Omega}_{i}\left({\bf a}_{i}-\mathbf{T}{\bf b}_{i}\right)}.\end{split}$

(18)

Note that the sign of the regularization term is changed due to the different sign of MSE and correntropy. The bandwidth parameter $\sigma$ can be tuned easily to model the portion of outlier. As shown in Hasanbelliu’s work [12], one feasible approach would be to change the kernel size with a decay rate during optimization. To this end, our approach is more flexible than a fixed portion of outlier.

The point set registration problem is finally converted to optimize a smooth function on a differentiable manifold. To solve the optimization problem in (18), traditional approaches are based on gradient-descent and these methods usually have slow rate of convergence. We propose a lie-algebra-based solver that can convert the problem into an iteratively reweighted least squares (IRLS) and the solution can be found in analytical form. The proposed solving process is based the standard workflow of Riemannian based optimization method in [14]. The solution $\mathbf{T}$ is actually on a Riemannian manifold: special Euclidean group $SE(3)$ and the rotation matrix $\bf{R}$ belongs to special orthogonal group SO(3), which can be defined as follows: $SO(3)\overset{\text{def}}{=}\left\{{\bf R}\in\mathbb{R}^{3\times 3}:{\bf R}^{\top}{\bf R}={\bf{I}},det({\bf R})=1\right\}$, $SE(3)\overset{\text{def}}{=}\left\{\mathbf{T}\in\begin{pmatrix}{\bf R}&{\bf t}\\ 0&1\end{pmatrix}:{\bf R}\in SO(3),{\bf t}\in\mathbb{R}^{3}\right\}$. The group $SO(3)$ forms a smooth manifold. The tangent space of the manifold is denoted as $\bf{so}(3)\in\mathbb{R}^{3}$, which is also called the Lie Algebra ($\bf{se}(3)\in\mathbb{R}^{6}$ as well). $\bf{so}(3)$ also coincides with $3\times 3$ skew symmetric matrices, denoted by the hat operator:

$\displaystyle{\bf\phi}^{\wedge}=\begin{bmatrix}\phi_{1}\\ \phi_{2}\\ \phi_{3}\end{bmatrix}^{\wedge}=\begin{bmatrix}0&-\phi_{3}&\phi_{2}\\ \phi_{3}&0&-\phi_{1}\\ -\phi_{2}&\phi_{1}&0\end{bmatrix}\in so(3)$

(19)

A standard approach for optimization problem on manifold [14] consists of determining a retraction $\mathcal{R}_{\mathbf{T}}$. The above function is reparametrized as:

$$\mathop{\max}_{\mathbf{T}\in SE(3)}f({\bf T})\Rightarrow\mathop{\max}_{\delta{\bf x}\in\mathbb{R}^{n}}f(\mathcal{R}_{\mathbf{T}}(\delta{\bf x})).$$

(20)

As stated by Forster in [15], the use of the retraction allows framing the optimization problem over an Euclidean space of suitable dimension, e.g. $\delta{\bf x}\in\mathbb{R}^{3}$ when we work in $SO(3)$ and $\delta{\bf x}\in\mathbb{R}^{6}$ when we work in $SE(3)$. Retraction is actually a bijective map between a perturbation $\delta{\bf x}$ of the tangent space at $\bf x$ and a neighborhood of $\bf x$ on the manifold. And after all, the right hand side of (20) can be formulated as an IRLS. By solving the IRLS, we get a vector $\delta{\bf x}^{\ast}$ in the tangent space. Then the current guess on the manifold, which is $\bf x$, can be updated.

The exponential map associates the lie algebra to a rotation has a first-order approximation: $exp({\bm{\phi}}^{\wedge})\approx\bf{I}+{\bm{\phi}}^{\wedge}$. Exponential map can be a retraction. It is not the computational optimal according to [15]. In this paper, we use the following retraction for $SE(3)$:

$$\mathcal{R}_{\mathbf{T}}(\delta{\bf x})=\left(exp({\bf\xi}^{\wedge}){\bf R},exp({\bf\xi}^{\wedge}){\bf t}+\Delta{\bf t}\right),$$

(21)

where $\delta{\bf x}=\begin{bmatrix}{\bf\xi}\\ \Delta{\bf t}\end{bmatrix}\in\mathbb{R}^{6}$. $\mathcal{R}_{\bf T}(\delta{\bm{\xi}},\delta{\bf t})$ means we use retraction at ${\bf T}\doteq({\bf R},{\bf t})$.

By applying the first-order approximation: $exp({\bf\phi}^{\wedge})\approx\bf{I}+{\bf\phi}^{\wedge}$ to (21), we get the perturbed version of $\bf R$ and $\bf t$:

$$\mathcal{R}_{\bf T}(\delta{\bf x})=\left(\left({\bf{I}}+{\bf\xi}^{\wedge}\right){\bf R},\left({\bf{I}}+{\bf\xi}^{\wedge}\right){\bf t}+\Delta{\bf t}\right).$$

(22)

We can now reparametrize the pair-wise error in (18):

$\displaystyle\begin{split}{\bf a}_{i}-{\bf R}{\bf b}_{i}-{\bf t}&={\bf a}_{i}-\left({\bf{I}}+{\bf\xi}^{\wedge}\right){\bf R}{\bf b}_{i}-\left({\bf{I}}+{\bf\xi}^{\wedge}\right){\bf t}-\Delta{\bf t}\\ &={\bf v}_{i}+{\bf H}_{i}\delta{\bf x},\end{split}$

(23)

with $\bf{{\bf v}}_{i}={\bf a}_{i}-{\bf Rb}_{i}-{\bf t},{\bf H}_{i}=\begin{bmatrix}({\bf Rb}_{i}+{\bf t})^{\wedge}-{\bf{I}}\end{bmatrix}$. In the derivation of (23), the property of cross product is used: ${\bf a}^{\wedge}{\bf b}=-{\bf b}^{\wedge}{\bf a}$. Now that (18) has the following form:

$\displaystyle\begin{split}\delta{\bf x}^{\ast}&=\mathop{\arg\max}_{{\bf x}}\sum\limits_{i=1}^{N}w_{G}{\left({\bf v}_{i}+{\bf H}_{i}\bf{x}\right)^{\mathrm{T}}\bf{\Omega}_{i}\left({\bf v}_{i}+{\bf H}_{i}\bf{x}\right)}\\ &=\mathop{\arg\max}_{{\bf x}}\left({\bf x}^{\mathrm{T}}\mathbf{A}{\bf x}+2{\bf b}^{\mathrm{T}}{\bf x}+const\right)\\ &=-\mathbf{A}^{\dagger}{\bf b},\end{split}$

(24)

where $\mathbf{A}=\sum\limits_{i=1}^{N}w_{G}{\bf H}_{i}^{\mathrm{T}}\bf{\Omega}_{i}{\bf H}_{i}$, ${\bf b}=\sum\limits_{i=1}^{N}w_{G}{\bf H}_{i}^{\mathrm{T}}\bf{\Omega}_{i}{\bf v}_{i}$ and $w_{G}=G_{\sigma}\left(r_{i}\right)$ is the weight function (13).

Importantly, $\mathbf{A}^{\dagger}$ denotes the pseudoinverse matrix of $\mathbf{A}$. This aims at dealing with the situation where $\mathbf{A}$ being rank-deficient.

The derivation of the above function is based on the assumption that in each iteration, the rotation matrix in the information matrix in (18) can be approximate by the result of the last iteration. With this assumption, the objective function of the Lie Algebra based optimization problem remains quadratic. Consequently, the result is in an elegant closed-form and there’s no need to use gradient based methods. After we get $\delta{\bf x}^{\ast}$, the current estimation on $SE(3)$ can be updated by using the retraction (21) again.

In this section we conduct experiments of all methods on a series of consecutive pairs of scans in two data sets to evaluate their performances under both structured and semi-structured environment:

• -

  $\bm{ETHHauptgebaude}$ collects lidar frames along a corridor with repetitive pillars and arches.

• -

  $\bm{GazeboSummer}$ is a semi-structured environment, with a mixture of man-made structures, vegetation and some dynamic objects like the walking people.

We compared CoBigICP with GICP, NDT and MiNoM [6] by computing the RPE for each consecutive pair. Main results are shown in table I and II. Note that the method CoGICP aims at ablation study which discards the bidirectional correspondence and involves only the correntropy metric. Compared to other methods, CoBigICP is the most competitive with the best accuracy and favorable time consumption. In the ETH Hauptgebaude data set, CoBigICP far more exceeds the other algorithms and this phenomenon supports the importance of correspondence step while also validating our bidirectional correspondence that we propose in this paper. The result of Gazebo Summer shows CoBigICP is also capable for semi-structured environment. The ablation study between CoGICP and CoBigICP shows effectiveness of bidirectional correspondence in both accuracy and speed. An illustrative reconstruction result is shown in Fig. 1. We compute the pose estimation using CoBigICP and we put the transformed point cloud together. The reconstruction result (colored points) is consistent with the ground truth (white points).

In this section, 6720 scan pairs from ETH Hauptgebaude (not necessarily consecutive) with different initial pose perturbations provided by Pomerleau et al. [1] are used to evaluate the robustness or convergence basin of algorithms. The different hardness of perturbations are indicated by easyPose, midiumPose and hardPose. These perturbations makes the registration challenging and meaningful. To lead a fair comparison, results of d2dndt, p2dndt and trimmed-pt2pl [7] are provided by the authors and are downloaded directly from the website. Results in Fig. 4 show that CoBigICP has the widest translation convergence basin while being also competitive with rotation perturbation. The phenomenon is consistent with our analysis in Section III B.