Do not Omit Local Minimizer: a Complete Solution for Pose Estimation from 3D Correspondences

TL;DR

This paper presents a complete, efficient solution for pose estimation from 3D correspondences that accounts for local minima, ambiguous configurations, and is suitable for real-time applications.

Contribution

It introduces a novel algorithm that reveals local minimizers, solves complex rational equations, and handles ambiguous configurations, advancing pose estimation methods.

Findings

Outperforms previous algorithms with small correspondence sets.

Achieves same accuracy as global methods when the global minimum is found.

Suitable for real-time pose estimation applications.

Abstract

Estimating pose from given 3D correspondences, including point-to-point, point-to-line and point-to-plane correspondences, is a fundamental task in computer vision with many applications. We present a complete solution for this task, including a solution for the minimal problem and the least-squares problem of this task. Previous works mainly focused on finding the global minimizer to address the least-squares problem. However, existing works that show the ability to achieve global minimizer are still unsuitable for real-time applications. Furthermore, as one of contributions of this paper, we prove that there exist ambiguous configurations for any number of lines and planes. These configurations have several solutions in theory, which makes the correct solution may come from a local minimizer. Our algorithm is efficient and able to reveal local minimizers. We employ the Cayley-Gibbs-Rodriguez (CGR) parameterization of the rotation to derive a general rational cost for the three cases of 3D correspondences. The main contribution of this paper is to solve the resulting equation system of the minimal problem and the first-order optimality conditions of the least-squares problem, both of which are of complicated rational forms. The central idea of our algorithm is to introduce intermediate unknowns to simplify the problem. Extensive experimental results show that our algorithm significantly outperforms previous algorithms when the number of correspondences is small. Besides, when the global minimizer is the solution, our algorithm achieves the same accuracy as previous algorithms that have guaranteed global optimality, but our algorithm is applicable to real-time applications.

Tables

Table 1. Table 1: Minimal configurations.

name	#point	#line	#plane
Pt$0$L$0$Pl$6$	0	0	6
Pt$0$L$1$Pl$4$	0	1	4
Pt$1$L$0$Pl$3$	1	0	3
Pt$0$L$2$Pl$2$	0	2	2
Pt$1$L$1$Pl$1$	1	1	1
Pt$2$L$0$Pl$1$	2	0	1
Pt$0$L$3$Pl$0$	0	3	0

Table 2. Table 2: Experimental results on the 03, 04, and 07 sequences of the KITTI dataset [ 10 ] .

Method	KITTI 03			KITTI 04			KITTI 07
	$𝐭$ Err ($\%$)	$𝐑$ Err (∘)	Time (s)	$𝐭$ Err ($\%$)	$𝐑$ Err (∘)	Time (s)	$𝐭$ Err ($\%$)	$𝐑$ Err (∘)	Time (s)
Olsson [24]	2.03e-1	5.46e-1	2.22	5.75e-1	5.76e-1	1.88	3.60e-1	2.53e-1	2.08
Wientapper [41]	2.00e-1	5.72e-1	0.58	4.32e-1	3.49e-1	0.55	3.61e-1	2.58e-1	0.56
Briales [4]	1.96e-1	5.61e-1	2.16	4.31e-1	3.37e-1	1.88	3.60e-1	2.53e-1	2.03
Ours	1.95e-1	5.61e-1	0.33	4.31e-1	3.37e-1	0.30	3.60e-1	2.53e-1	0.30

Table 3. Table 3: Computational time. For the least-squares problem, we consider the time to solve (13), which is independent of the number of correspondences.

Algorithm	time ($ms$)
Our least-squares algorithm	2.95
DLSSol	20.0
Our minimal algorithm	0.296
DMinSol	0.599

Table 4. Table 4: Average numbers of point-to-point(P-to-P), point-to-pine(P-to-L), and point-to-plane(P-to-PL) correspondences per frame of 03, 04 and 07 KITTI sequences.

Sequences	Average Correspondences Per Frame
	#P-to-P	#P-to-L	#P-to-PL
KITTI 03	63	17	21117
KITTI 04	37	30	19994
KITTI 07	111	40	20126

Equations

$$r_{\pi }={\mathbf{n}}^T\left(\mathbf{R}{\mathbf{x}}_{\pi }+\mathbf{t}-{\mathbf{y}}_{\pi }\right)$$

$${\mathbf{r}}_l=\left({\mathbf{I}}_3-\mathbf{d}{\mathbf{d}}^T\right)\left(\mathbf{R}{\mathbf{x}}_l+\mathbf{t}-{\mathbf{y}}_l\right)$$

$${\mathbf{r}}_p=\mathbf{R}{\mathbf{x}}_p+\mathbf{t}-{\mathbf{y}}_p$$

$$\begin{array}[]{l}{C_{\pi lp}}\left({\bf{R}},{\bf{t}}\right)=\sum\limits_{i=1}^{{n_{\pi}}}{r_{{\pi_{i}}}^{2}}+\sum\limits_{i=1}^{{n_{l}}}{{\bf{r}}_{{l_{i}}}^{T}{{\bf{r}}_{{l_{i}}}}}+\sum\limits_{i=1}^{{n_{p}}}{{\bf{r}}_{{p_{i}}}^{T}{{\bf{r}}_{{p_{i}}}}},\\ s.t.\;{\bf{R}}{{\bf{R}}^{T}}={{\bf{I}}_{3}},\det\left({\bf{R}}\right)=1,\end{array}$$

$$\mathbf{R}=\frac{\bar{\mathbf{R}}}{1+{\mathbf{s}}^T\mathbf{s}},\bar{\mathbf{R}}=\left(\left(1-{\mathbf{s}}^T\mathbf{s}\right){\mathbf{I}}_3+2{\left[\mathbf{s}\right]}_{\times }+2\mathbf{s}{\mathbf{s}}^T\right)$$

$$r_g={\mathbf{a}}^T\mathbf{Rb}+{\mathbf{a}}^T\mathbf{t}+c$$

$$r_l^i={\mathbf{a}}_l^i\left(\mathbf{R}{\mathbf{x}}_l+\mathbf{t}-{\mathbf{y}}_l\right),i=1,2,3$$

$$r_p^i={\mathbf{e}}_p^i\left(\mathbf{R}{\mathbf{x}}_p+\mathbf{t}-{\mathbf{y}}_p\right),i=1,2,3$$

$$r_g=\frac{{\mathbf{k}}^T\mathbf{v}}{1+{\mathbf{s}}^T\mathbf{s}}$$

$$r_g^2=\frac{q\left(\mathbf{s},\mathbf{t}\right)}{{\left(1+{\mathbf{s}}^T\mathbf{s}\right)}^2}$$

$${\mathbf{r}}_{l_i}^T{\mathbf{r}}_{l_i}={\left(r_{l_i}^1\right)}^2+{\left(r_{l_i}^2\right)}^2+{\left(r_{l_i}^3\right)}^2$$

$$C_{\pi lp}\left(\mathbf{s},\mathbf{t}\right)=\frac{{\bar{C}}_{\pi lp}\left(\mathbf{s},\mathbf{t}\right)}{{\left(1+{\mathbf{s}}^T\mathbf{s}\right)}^2}$$

$$\begin{array}[]{l}{g_{{s_{i}}}}=\frac{{\partial{C_{\pi lp}}}}{{\partial{s_{i}}}}=\frac{1}{{{{\left({1+{{\bf{s}}^{T}}{\bf{s}}}\right)}^{3}}}}\left({\left({1+{{\bf{s}}^{T}}{\bf{s}}}\right)\frac{{\partial{{\bar{C}}_{\pi lp}}}}{{\partial{s_{i}}}}-4{s_{i}}{{\bar{C}}_{\pi lp}}}\right)=0\\ {g_{{t_{i}}}}=\frac{{\partial{C_{\pi lp}}}}{{\partial{t_{i}}}}=\frac{1}{{{{\left({1+{{\bf{s}}^{T}}{\bf{s}}}\right)}^{2}}}}\frac{{\partial{{\bar{C}}_{\pi lp}}}}{{\partial{t_{i}}}}=0,\;i=1,2,3\end{array}$$

$$\begin{array}[]{l}{{\bar{g}}_{{s_{i}}}}=\left({1+{{\bf{s}}^{T}}{\bf{s}}}\right)\frac{{\partial{{\bar{C}}_{\pi lp}}}}{{\partial{s_{i}}}}-4{s_{i}}{{\bar{C}}_{\pi lp}}=0\\ {{\bar{g}}_{{t_{i}}}}=\frac{{\partial{{\bar{C}}_{\pi lp}}}}{{\partial{t_{i}}}}=0,\;i=1,2,3\end{array}$$

$${\mathbf{x}}_k={\mathbf{x}}_{k-1}-J_{\bar{G}}^{-1}\left({\mathbf{x}}_{k-1}\right)\bar{G}\left({\mathbf{x}}_{k-1}\right)$$

$$\rho =\frac{1}{\sqrt{1+s_1^2+s_2^2+s_3^2}}.$$

$$\alpha =\rho s_1,\beta =\rho s_2,\gamma =\rho s_3.$$

$$r_g={\mathbf{a}}_g^T\mathbf{u}+{{\mathbf{b}}_g}^T\mathbf{t}$$

$${\mathbf{e}}_g=\mathbf{Au}+\mathbf{Bt}$$

$$C_{\pi lp}\left(\rho ,\alpha ,\beta ,\gamma ,\mathbf{t}\right)={\mathbf{e}}_g^T{\mathbf{e}}_g$$

$$\mathbf{t}=-{\left({\mathbf{B}}^T\mathbf{B}\right)}^{-1}{\mathbf{B}}^T\mathbf{Au}$$

$$C_{\pi lp}\left(\rho ,\alpha ,\beta ,\gamma \right)={\mathbf{u}}^T{\mathbf{C}}^T\mathbf{Cu}={\mathbf{u}}^T\mathbf{Qu}$$

$$\begin{array}{r}{g}_{\rho }=\frac{\partial C_{\pi lp}}{\partial \rho }=0,g_{\alpha }=\frac{\partial C_{\pi lp}}{\partial \alpha }=0,\\ g_{\beta }=\frac{\partial C_{\pi lp}}{\partial \beta }=0,g_{\gamma }=\frac{\partial C_{\pi lp}}{\partial \gamma }=0.\end{array}$$

$$y_i=\left(1+{\mathbf{s}}^T\mathbf{s}\right)t_i,i=1,2,3.$$

$$r_g=\frac{{\mathbf{c}}^T\mathbf{x}+{\mathbf{a}}^T\mathbf{y}}{1+{\mathbf{s}}^T\mathbf{s}}=0,$$

$${\bar{r}}_g={\mathbf{c}}^T\mathbf{x}+{\mathbf{a}}^T\mathbf{y}=0.$$

$${\mathbf{C}}_1\mathbf{x}+{\mathbf{A}}_1\mathbf{y}=0_{3\times 1},{\mathbf{C}}_2\mathbf{x}+{\mathbf{A}}_2\mathbf{y}=0_{3\times 1},$$

$$\mathbf{y}=-{\mathbf{A}}_1^{-1}{\mathbf{C}}_1\mathbf{x}.$$

$$\begin{array}{rl}{f}_i=& k_{i1}{s}_1^2+k_{i2}{s}_2^2+k_{i3}{s}_3^2+k_{i4}{s}_1{s}_2+k_{i5}{s}_1{s}_3+\\ & k_{i6}{s}_2{s}_3+k_{i7}{s}_1+k_{i8}{s}_2+k_{i9}{s}_3+k_{i10}=0\end{array}$$

$$\begin{array}{rl}{f}_i=& k_{i1}{s}_1^2+k_{i2}{s}_2^2+k_{i4}{s}_1{s}_2+p_{i1}(s_3)s_1+\\ & p_{i2}(s_3)s_2+p_{i3}(s_3),i=1,2,3,\end{array}$$

Full text

1 Introduction

Estimating the pose from 3D correspondences, i.e. point-to-point, point-to-line and point-to-plane correspondences, is known as the 3D registration problem in the literature [25, 24, 26, 40, 4, 41]. It is one of the fundamental problems in computer vision with a wide range of applications, such as simultaneous localization and mapping (SLAM) [45, 44, 23, 30, 37], extrinsic calibration [47, 38, 22, 11, 49] and iterative closes point (ICP) framework [3]. Besides, some camera pose estimation problems, such as the perspective-n-point (PnP) problem [12, 16] and the perspective-n-line (PnL) problem [42, 29], can be transformed to a 3D registration problem [41]. However, the research on this problem is not as thorough as other pose estimation problems.

Previous works mainly focus on solving the least-squares problem. Although large progress has been made, achieving the optimal solution and real-time performance is still a challenge. Some algorithms [26, 4] are capable of finding the global minimum, however, their running time makes them not suitable for real-time applications. Recently, Wientapper [41] provided an efficient algorithm, but their algorithm can not achieve the global optimality in theory, and has the risk of no solution. Furthermore, previous works generally assume that the correct solution is the global minimizer. However, we find that the correct solution may come from a local minimizer in certain configurations.

Minimal solution is important to eliminate outliers in the RANSAC framework [9]. As the pose has 6 degrees of freedom (DoF), any combinations of 3D correspondences providing 6 independent constraints form a minimal configuration. Previous algorithms are proposed to solve these minimal configurations case by case [6, 32, 31]. The contributions of this paper are as follows:

First, we prove that there exist ambiguous configurations for any number of plane and line correspondences, which leads to multiple solutions. When a configuration approximates ambiguity, the correct solution of a problem may come from a local minimum. Therefore, previous works [25, 26, 4] that only compute the global minimizer will fail in this case. Revealing local minimizers is essential for an algorithm to handle all the configurations.

Second, we present an efficient and accurate solution for the least-squares 3D registration problem. We use the CGR parametrization to represent the rotation, which generates a rational cost function. We derive its first-order optimality conditions. They form a high order polynomial system, and are hard to solve. Four intermediate unknowns are introduced to relax the original problem, which results in much simpler first-order optimality conditions. Gröbner basis method [8] is applied to solve this equation system. Then we refine the solution by the Newton-Raphson method.

Third, we present an unified solution for the potential minimal configurations. Previous algorithms are proposed to solve the minimal problems case by case [6, 32, 31]. We also use the CGR parametrization to represent the rotation. which generates a third order equation system for the minimal configurations. Three intermediate unknowns are introduced to simplify the equation system. We introduce a novel hidden variable method [8] to solve the resulting second order equation system for the rotation matrix. Then the translation can be computed from a linear system.

We evaluate our algorithm with synthetic and real data. Extensive experimental results show that recovering the local minimal is essential, especially when the number of correspondences $N$ is small. For a small $N$, our algorithm significantly outperforms previous works [25, 26, 4, 41]. Besides, experimental results verify that our algorithm can converge to the global minimizer with the same accuracy as the previous work with guaranteed globally optimality [26, 4], however, our algorithm is much faster.

2 Related Works

This paper focuses on the pose estimation from point-to-point, point-to-line and point-to-plane correspondences correspondences. Pose estimation from point-to-point correspondences has been solved in early works [1, 13, 14]. There exist closed-form solutions for this problem. However, estimating pose from point-to-line and point-to-plane correspondences is more complicated. These 3D correspondences actually yield similar distance function, which is employed by previous works to construct a general cost function [25, 24, 26, 40, 4, 41]. The difference of various cost functions lies in the parameterization of the rotation matrix. The raw rotation matrix is adopted in [24, 4], and the quaternion [13] is used in [25, 26, 40, 41]. Previous works mainly focus on finding the global minimizer of the resulting cost function. In [26], they proposed a provably optimal algorithm. They employed convex underestimators with branch-and-bound methods to iteratively compute the global minimum of the cost function. Although this algorithm can guarantee global optimality, it is very time-consuming. Olsson et al. [24] reduced the computational complexity by applying the Lagrangian dual relaxation to approximate the cost function. Their experimental results showed that a single convex semidefinite program can well approximate the original problem. Recently, an improved result is obtained by a strengthened Lagrangian dual relaxation [4]. Although without theoretical guarantees, their experimental results show that this algorithm can achieve the global minimum. Although these algorithms [24, 4] have made progress in efforts to reduce the computational time, they are still not suitable to demanding real-time applications. Recently, Wientapper [41] provided an efficient algorithm based on Gröbner basis polynomial solver. They introduced the first order derivatives of the norm-one constraint of the quaternion into the first order optimality conditions of the cost function as [16], rather than apply the Lagrangian formulation for the norm-one constraint of the quaternion. This results in an efficient solution, however, their result is not optimal in theory. Furthermore, as the number of equations is greater than the number of unknowns, the equation system may have no solution. They introduced 4 pre-rotations to solve the problem 4 times to handle this problem. The pre-rotation is to make the no-solution problem happen less likely, however, it can not fully solve this problem in theory.

Minimal solution is essential in the RANSAC framework [9] for robust pose estimation. Any combination of the 3D correspondences resulting in 6 independent constraints forms a minimal configuration. In the literature, specific algorithms have been proposed to solve certain configurations. In [7], a minimal solution for three line-to-plane correspondences were proposed. In [39, 48], they studied the minimal solution of a more specific line-to-plane configuration, i.e., lines are all on a plane. As one line-to-plane correspondence can be treated as two point-to-plane correspondences, these problems can be formulated as a point-to-plane registration problem. Naroditsky et al. [22] presented a solution to 6 point-to-plane correspondences. Ramalingam et al. [31] solved all the point-to-plane minimal configurations. They designed specific intermediate coordinate systems to simplify each minimal configuration. This method is not convenient in the application, as it needs to implement a bunch of algorithms.

3D registration without correspondence is an related but more complicated problem. Several works [20, 27, 43, 50] presented globally optimal solution for 3D point registration without correspondence. The ICP framework [3] gives a general way to find correspondences and pose at the same time. Although this framework was originally designed for points, other 3D models can also be introduced into this framework [35, 5, 36]. The ICP framework iteratively finds the nearest elements as correspondences then calculates the pose until it converges. Efficiently and accurately estimating the pose from current nearest 3D correspondences is critical for the ICP framework.

3 Problem Formulation

In this paper, we use italic, boldfaced lowercase and boldfaced uppercase letters to represent scalars, vectors and matrices, respectively. The paper focuses on the problem of pose estimation from point-to-plane, point-to-line and point-to-point correspondences. We first consider the point-to-plane correspondence. Suppose we have a point ${{\mathbf{x}}_{l}}$ in one coordinate system and the corresponding plane in another coordinate system represented by the plane’s norm-one normal $\mathbf{n}$ and a point ${{\mathbf{y}}^{\pi}}$ lying on it. Then the scalar residual between a point and a plane can be written as

[TABLE]

For the point-to-line correspondence, we can calculate the 3-dimensional residual vector between a point ${{\mathbf{x}}_{l}}$ and the corresponding line represented by the norm-one direction $\mathbf{d}$ and a point ${{\mathbf{y}}_{l}}$ on it as

[TABLE]

where ${{\mathbf{I}}_{3}}$ is the identify matrix. Lastly, a point-to-point correspondence yields a $3$-dimensional residual vector

[TABLE]

$\mathbf{R}$ and $\mathbf{t}$ have 6 DoF. Our problem is to calculate $\mathbf{R}$ and $\mathbf{t}$ from any configuration of ${{n}_{\pi}}$ point-to-plane, ${{n}_{l}}$ point-to-line, and ${{n}_{p}}$ point-to-point correspondences that has at least 6 effective constraints. Let use define $N$ as the number of effective correspondences. When $N>6$, this is a least squares problem described in section 4. When $N=6$, this forms a minimal problem solved in section 5.

4 Least Squares Solution

Using the notation in (1), (2) and (3), we define our cost function as follows

[TABLE]

where $\det\left(\mathbf{R}\right)$ represents the determinant of $\mathbf{R}$.

4.1 Ambiguous Configurations

In previous works, the global minimizer is treated as the optimal solution. It is well known that there is an unique solution for at least three point-to-point correspondences, except for the degenerate configuration, i.e. all the points are collinear. However, for plane and line, there exist configurations which have several solutions. We call them as ambiguous configurations. The ambiguous configuration differs from the degenerate configuration. The ambiguous configuration has several solutions, but the degenerate configuration has infinite solutions. Formally, we have the following lemma for point-to-line correspondences. Proof is in the supplementary material.

Lemma 1**.**

For any $n_{l}$ points and any $\left\{{{{\bf{R}}_{1}},{{\bf{t}}_{1}}}\right\}$ and $\left\{{{{\bf{R}}_{2}},{{\bf{t}}_{2}}}\right\}$, there exist $n_{l}$ lines to make $\left\{{{{\bf{R}}_{1}},{{\bf{t}}_{1}}}\right\}$ and $\left\{{{{\bf{R}}_{2}},{{\bf{t}}_{2}}}\right\}$ are exact solutions for the $n_{l}$ point-to-line correspondences.

Similarly, we have lemma 2 for point-to-plane correspondences.

Lemma 2**.**

For any $n_{\pi}$ points and any $\left\{{{{\bf{R}}_{1}},{{\bf{t}}_{1}}}\right\}$, $\left\{{{{\bf{R}}_{2}},{{\bf{t}}_{2}}}\right\}$ and $\left\{{{{\bf{R}}_{3}},{{\bf{t}}_{3}}}\right\}$, there exist $n_{\pi}$ planes to make $\left\{{{{\bf{R}}_{1}},{{\bf{t}}_{1}}}\right\}$, $\left\{{{{\bf{R}}_{2}},{{\bf{t}}_{2}}}\right\}$ and $\left\{{{{\bf{R}}_{3}},{{\bf{t}}_{3}}}\right\}$ are exact solutions for the $n_{\pi}$ point-to-plane correspondences.

Finally, we have the following theorem for $n_{l}$ point-to-line and $n_{\pi}$ point-to-plane correspondences.

Theorem 1**.**

For any $n_{l}$ points on lines and $n_{\pi}$ points on planes and any $\left\{{{{\bf{R}}_{1}},{{\bf{t}}_{1}}}\right\}$ and $\left\{{{{\bf{R}}_{2}},{{\bf{t}}_{2}}}\right\}$, there exist $n_{l}$ lines and $n_{\pi}$ planes to make $\left\{{{{\bf{R}}_{1}},{{\bf{t}}_{1}}}\right\}$ and $\left\{{{{\bf{R}}_{2}},{{\bf{t}}_{2}}}\right\}$ are exact solutions for the $n_{l}$ point-to-line and $n_{\pi}$ point-to-plane correspondences.

When measurements approximate an ambiguous configuration, a prior is required to identify the correct solution. The prior is generally available in real applications. For instance, we generally have a rough estimation of the pose between two sensors in the extrinsic calibration problem [47, 38, 22, 11, 49]. For the pose estimation in SLAM [45, 44, 23, 30, 37], current pose should be consistent with the previous motion trajectory.

Previous works [25, 24, 26, 4] using convex approximation can only find the global minimizer. However, the global minimizer may not be the correct solution of a problem if measurements approximate an ambiguous configuration. Therefore, we can not simply omit local minimizers.

4.2 Rotation Parameterization

Solving for the rotation matrix is the crux for pose estimation. Previous works [25, 26, 4, 41] adopt non-minimal representations for $\bf{R}$, which results in additional quadratic constraints in the minimization problem. This paper adopts the CGR parametrization [12, 21] which gives a minimal representation for the rotation matrix, removing the quadratic constraints in (4) . The CGR parametrization expresses a rotation matrix as

[TABLE]

where $\mathbf{s}=\left[{{s}_{1}};{{s}_{2}};{{s}_{3}}\right]$ is a 3-dimensional vector, and ${\left[{\bf{s}}\right]_{\times}}$ is the skew-symmetric matrix of $\bf{s}$.

4.3 Rational Formulation of Residual

We will show that the residuals from point-to-point, point-to-line and point-to-plane correspondences have a general form

[TABLE]

where $\mathbf{a}$ and $\mathbf{b}$ are two $3$-dimensional vectors and $c$ is a scalar. We first consider the point-to-plane residual ${{r}_{\pi}}$ in (1). It is obvious that $\mathbf{a}=\mathbf{n},\ \mathbf{b}={{\mathbf{y}}_{\pi}}$ and $c=-{{\mathbf{n}}^{T}}{{\mathbf{y}}_{\pi}}$. Let us then consider the point-to-line residual. We define ${{\mathbf{r}}_{l}}=\left[r_{l}^{1},r_{l}^{2},r_{l}^{3}\right]^{T}$ and ${{\mathbf{A}}_{l}}=\left({{\mathbf{I}}_{3}}-\mathbf{d}{{\mathbf{d}}^{T}}\right)=\left[\mathbf{a}_{l}^{1};\mathbf{a}_{l}^{2};\mathbf{a}_{l}^{3}\right]$, where $\mathbf{a}_{l}^{1},\mathbf{a}_{l}^{2}$ and $\mathbf{a}_{l}^{3}$ are the three rows of ${{\mathbf{A}}_{l}}$. Using this notation, equation (2) can be written as

[TABLE]

It is easy to find that $\mathbf{a}={{\left(\mathbf{a}_{l}^{i}\right)}^{T}},\ \mathbf{b}={{\mathbf{x}}_{l}}$ and $c=-\mathbf{a}_{l}^{i}{{\mathbf{y}}_{\pi}}$. Lastly, we consider the point-to-point correspondence. Similar to the point-to-line correspondence, we define ${{\mathbf{r}}_{p}}={{\left[r_{p}^{1},r_{p}^{2},r_{p}^{3}\right]}^{T}}$ and ${{\mathbf{I}}_{3}}=\left[\mathbf{e}_{l}^{1};\mathbf{e}_{l}^{2};\mathbf{e}_{l}^{3}\right]$. Substituting this notation into (3), it is easy to find

[TABLE]

The similarity between (8) and (7) is obvious. Therefore, $r_{p}^{i}$ also has the general form (6).

Let $\mathbf{t}=\left[{{t}_{1}};{{t}_{2}};{{t}_{3}}\right]$. Substituting (5) into the general residual ${{r}_{g}}$ in (6) and adding the tree terms together yields

[TABLE]

where ${{\mathbf{k}}^{T}}\mathbf{v}$ is a third order polynomial with terms ${\bf{v}}=[s_{1}^{2}{t_{1}},s_{1}^{2}{t_{2}},s_{1}^{2}{t_{3}},s_{1}^{2},{s_{1}}{s_{2}},{s_{1}}{s_{3}},{s_{1}},s_{2}^{2}{t_{1}},s_{2}^{2}{t_{2}},s_{2}^{2}{t_{3}},s_{2}^{2},{s_{2}}{s_{3}},\\ {s_{2}},s_{3}^{2}{t_{1}},s_{3}^{2}{t_{2}},s_{3}^{2}{t_{3}},s_{3}^{2},{s_{3}},{t_{1}},{t_{2}},{t_{3}},1{]^{T}}$.

4.4 First-Order Optimality Conditions

Now we consider the least-squares problem (4). Using (9), the squared residual can be represented as

[TABLE]

where $q\left(\mathbf{s},\mathbf{t}\right)={{\mathbf{v}}^{T}}\mathbf{k}{{\mathbf{k}}^{T}}\mathbf{v}$ is a 6th order polynomial in $\mathbf{s}$ and $\mathbf{t}$. For the point-to-line distance $\mathbf{r}_{l_{i}}^{T}{{\mathbf{r}}_{l_{i}}}$ in (4), we have

[TABLE]

As mentioned above, $r_{l_{i}}^{1},$ $r_{l_{i}}^{2}$ and $r_{l_{i}}^{3}$ all have the same form as ${{r}_{g}}$. Therefore, the point-to-line distance $\mathbf{r}_{l_{i}}^{T}{{\mathbf{r}}_{l_{i}}}$ will have the same form as (10). Similarly, $\mathbf{r}_{p}^{T}{{\mathbf{r}}_{p}}$ will also yield the same form as (10). After the summation of squared residuals in (4), we know that ${{C}_{\pi lp}}$ would have the same rational form as (10). We write it as

[TABLE]

where ${{\bar{C}}_{\pi lp}}$ is a 6th order polynomial function in $\mathbf{s}$ and $\mathbf{t}$. To find the critical points of (12), we first calculate its first order optimality conditions as follows:

[TABLE]

Canceling the denominator of (13) yields

[TABLE]

$\frac{{\partial{{\bar{C}}_{\pi lp}}}}{{\partial{s_{i}}}}$ and $\frac{{\partial{{\bar{C}}_{\pi lp}}}}{{\partial{t_{i}}}}$ are of degree 5. Therefore, ${{\bar{g}}_{{{s}_{i}}}}$ and ${{\bar{g}}_{{{t}_{i}}}}$ are of degree 7 and 5, respectively. Although the Gröbner basis method gives a general way to solve the polynomial system, it is computationally demanding and numerically unstable to apply it to a high order polynomial system, as the experimental results in Fig. 1. The Newton-Raphson method provides an alternative way to find the roots of (14). Denote an equation system as $\bar{G}\left({\bf{x}}\right)$. For the $k$th iteration, the Newton-Raphson method updates the solution as

[TABLE]

This iterative method requires an initial solution ${{\mathbf{s}}_{0}}$ and ${{\mathbf{t}}_{0}}$. In the next section, we will describe how to calculate an accurate ${{\mathbf{s}}_{0}}$ and ${{\mathbf{t}}_{0}}$.

4.5 Initial Estimation from Relaxation

The difficulty of solving (14) lies in the denominator of CGR parameterization (5). To solve this problem, we introduce the following intermediate variable

[TABLE]

Substituting (16) into (9) will transform (9) from a rational function to a polynomial function. Furthermore, we define

[TABLE]

Using (16) and (17), the residual (9) can be expressed as

[TABLE]

where $\mathbf{u}={{\left[{{\alpha}^{2}},\alpha\beta,\alpha\gamma,\alpha\rho,{{\beta}^{2}},\beta\gamma,\beta\rho,{{\gamma}^{2}},\gamma\rho,{{\rho}^{2}},1\right]}^{T}}$. This formulation turns the rational function (9) into a polynomial function (18) that is easier to handle. Furthermore, $\mathbf{t}$ in (18) is decoupled from $\mathbf{R}$, and (18) is linear in $\mathbf{t}$.

Now we consider the least-squares problem (4) for the new unknowns. We stack the residuals from ${{n}_{\pi}}$ point-to-plane, ${{n}_{l}}$ point-to-line, and ${{n}_{p}}$ point-to-point correspondences to get

[TABLE]

where $\bf{A}$ is a $\left({{n}_{\pi}}+3{{n}_{l}}+3{{n}_{p}}\right)\times 11$ matrix and $\bf{B}$ is a $\left({{n}_{\pi}}+3{{n}_{l}}+3{{n}_{p}}\right)\times 3$ matrix. Then (4) can be written as

[TABLE]

As (19) is linear for $\mathbf{t}$, then $\bf{t}$ has a closed-form solution as

[TABLE]

Substituting (21) into (20), we derive a cost function only involving $\rho,\alpha,\beta,\gamma$ as

[TABLE]

where ${\bf{C}}={{\bf{A}}-{\bf{B}}{{\left({{{\bf{B}}^{T}}{\bf{B}}}\right)}^{-1}}{{\bf{B}}^{T}}{\bf{A}}}$. The elements in the vector $\mathbf{u}$ are second order monomials except for the constant term. Thus (22) is a fourth order polynomial function. We compute the first order optimality conditions to get all the stationary points. This first-order condition contains four third order polynomials for $\rho,\alpha,\beta$ and $\gamma$ as

[TABLE]

According to Bézout’s theorem [8], there exist at most ${{3}^{4}}=81$ solutions. We find that the polynomial system in (23) only contains third and first degree monomials. This equation system stratifies the 2-fold symmetry [18, 2]. That is to say if any nontrivial $\mathbf{\xi}={{\left[\rho,\alpha,\beta,\gamma\right]}^{T}}$ is a solution, $-\mathbf{\xi}$ is also a solution. Thus there are at most 40 independent solutions. We adopt the algorithm introduced in [19] to generate the polynomial solver which can utilize this property to yield an efficient solution.

After solving (23), we are then able to compute $\mathbf{t}$ from (21). The CGR parameters ${{s}_{1}},{{s}_{2}}$ and ${{s}_{3}}$ can be recovered from the definitions in (16) and (17). The above formulation treats $\rho,\alpha,\beta,\gamma$ as independent unknowns. However, they are related as they are functions of ${{s}_{1}},{{s}_{2}}$ and ${{s}_{3}}$. To recover the minimizer of (12), we refine the solution using the Newton-Raphson iteration method in (15). We summarize our least-squares solution in Algorithm 1.

5 Minimal Solution

Unlike most pose estimation problems having an unique minimal configuration, our problem has multiple minimal configurations. Specifically, any combination of 3D correspondences with 6 effective constraints forms a minimal configuration. Table 1 lists the minimal configurations. We use the pattern Pt$i$L$j$Pl$k$ to represent $i$ point-to-point, $j$ point-to-line and $k$ point-to-plane correspondences. As one point-to-point, point-to-line and point-to-plane correspondence provide 3, 2 and 1 constraints, respectively, it is easy to verify that the configurations in Table 1 are minimal configurations, except for the case Pt$2$L[math]Pl$1$ (i.e. 2 point-to-point and 1 point-to-plane correspondences). It seems that Pt$2$L[math]Pl$1$ has 7 constraints, as 2 point-to-point correspondences seemingly provide 6 constraints and 1 point-to-plane correspondence provides 1 constraint. However, the collinear configuration is degenerate for point-to-point correspondences [1]. Two point-to-point correspondences form a collinear configuration. One additional constraint is required to recover $\bf{R}$ and $\bf{t}$. We propose an unified solution for these minimal cases.

Let us first consider the rational equation from (9). Its numerator, i.e. ${\bf{k}}^{T}{\bf{v}}$, is a third order polynomial. Assume that we have 6 equations in the form of (9). This will lead to an equation system with 6 third order equations, which is hard to solve. We can not use the intermediate unknowns defined in (16) and (17) to the minimal problem, since this will result in 7 unknowns, but there are only 6 constraints. Here we introduce 3 new intermediate unknowns as

[TABLE]

Define ${{\bf{y}}=\left[y_{1},y_{2},y_{3}\right]^{T}}$. Then (9) can be rewritten as

[TABLE]

where ${\bf{x}}=\left[s_{1}^{2},s_{2}^{2},s_{2}^{2},s_{1}s_{2},s_{1}s_{3},s_{2}s_{3},s_{1},s_{2},s_{3},1\right]^{T}$. Define ${\bar{r}_{g}}$ the numerator of (25). Eliminating the denominator of (25), we have

[TABLE]

This is a quadratic equation in $s_{1},s_{2},s_{3}$ and a linear equation in the new unknowns $y_{1},y_{2},y_{3}$. Compared to the original equation from the numerator of (9), the new one is much simpler. For the minimal problem, we have 6 effective equations as (26). Let us divide the 6 equations into two groups. Each group has 3 equations as

[TABLE]

We can easily calculate $\bf{y}$ using the first three equations as

[TABLE]

Substituting (28) into the second part of (27), we have

[TABLE]

where $k_{ij}$ ($i\in\left[1,3\right],j\in\left[1,10\right]$) is an element of ${\bf{K}}={\bf{C}}_{2}-{\bf{A}}_{2}{\bf{A}}_{1}^{-1}{\bf{C}}_{1}$.

5.1 Hidden Variable Polynomial Solver

Here we present a hidden variable method [8] to solve (29). We treat one unknown as a constant. This unknown is called the hidden variable. Without loss of generality, we treat $s_{1}$ and $s_{2}$ as unknowns, and $s_{3}$ as a constant. Thus (29) can be rewritten as an equation system in $s_{1}$ and $s_{2}$ as

[TABLE]

where ${p_{i1}}({s_{3}})={k_{i5}}{s_{3}}+{k_{i7}}$, ${p_{i2}}({s_{3}})={k_{i6}}{s_{3}}+{k_{i8}}$, ${p_{i3}}({s_{3}})={k_{i3}}{s_{3}}^{2}+{k_{i9}}{s_{3}}+{k_{i10}}$. Then we introduce an auxiliary variable ${s_{0}}$ to convert ${f_{i}}$ to a homogeneous equation ${F_{i}}$, i.e., every monomial in $F_{i}$ is of degree 2. This leads to the following homogeneous equation system:

[TABLE]

If we treat $s_{0}^{2}$, $s_{1}$ and $s_{2}$ as variables, we can rewrite (31) as a linear homogeneous system in $s_{0}^{2}$, $s_{1}$ and $s_{2}$ as follows

[TABLE]

where $P_{i1}=p_{i3}(s_{3})$, $P_{i2}=k_{i1}{s_{1}}+k_{i4}{s_{2}}+p_{i1}(s_{3})s_{0}$, $P_{i3}=k_{i2}s_{2}+p_{i2}(s_{3})$, $i=1,2,3$. Let us denote the coefficient matrix of (32) as ${\bf{P}}=\left(P_{ij}\right)_{3\times 3}$. If the polynomial system (31) has a nontrivial solution, there will exist a nontrivial solution for the homogeneous linear system (32). According to the algebraic theory, (32) has a nontrivial solution if and only if the determinant of $\bf{P}$ is zero, i.e., $\det\left({\bf{P}}\right)=0$. Define $F_{4}=\det\left({\bf{P}}\right)$. $F_{4}$ has the same form as $F_{i}$ in (31). Similarly, we can construct two homogeneous linear systems for $\left[s_{0},s_{1}^{2},s_{2}\right]^{T}$ and $\left[s_{0},s_{1},s_{2}^{2}\right]^{T}$. This will yield two homogeneous equations $F_{5}=0$ and $F_{6}=0$, respectively. Stacking $F_{i}$ ($i=1,\cdots,6$) together, we have a homogeneous linear system as

[TABLE]

where ${\bf{C}}\left(s_{3}\right)$ is a $6\times 6$ matrix with polynomials in $s_{3}$ as elements, and ${\bf{h}}=[s_{0}^{2},s_{1}^{2},s_{2}^{2},s_{0}s_{1},s_{0}s_{2},s_{1}s_{2}]^{T}$. If (33) has a nontrivial solution, if and only if $\det\left({\bf{C}}\left(s_{3}\right)\right)=0$. This is an eighth order polynomial equation in $s_{3}$. We can get $s_{3}$ by solving this equation. Comparing (30) and (31), we can find that if $[s_{1},s_{2}]$ is a solution of (30), $[s_{1},s_{2},1]$ is a solution of (31), and vice versa. After we get the solution of $s_{3}$, we can back-substitute it into (33) and set $s_{0}=1$, then solve the resulting linear system for $s_{1}$ and $s_{2}$. After we get $s_{1},s_{2},s_{3}$, we can recover $\bf{R}$ by (5) and calculate $\bf{y}$ using (28). Then $\bf{t}$ can be recovered from (24). We summarize our minimal solution in Algorithm 2.

6 Experiments

In this section, we compare our algorithm with the state-of-the-art algorithms, including Briales’s algorithm [4], BnB [26], Olsson’s algorithm [24] and Wientapper’s algorithm [41]. We also consider the point-to-point case alone, as it has closed-form solution. We adopt Arun’s algorithm [1] implemented in OpenGV [15]. The algorithms are evaluated in terms of accuracy and computational time using synthetic and real data.

6.1 Experiments with Synthetic Data

Our synthetic data is generated as [4]. Specifically, each geometric element is determined by randomly sampling a point within a sphere of radius $10m$. For lines and planes, a random unit direction and normal are generated. We uniformly sample the Euler angles $\alpha,\beta,\gamma$ of the rotation matrix ( $\alpha,\gamma\in\left[{0^{\circ},360^{\circ}}\right]$ and $\beta\in\left[{0^{\circ},180^{\circ}}\right]$). The translation elements are uniformly distributed within $\left[-10m,10m\right]$. We use ${\mathbf{\hat{R}}}$ and ${\mathbf{\hat{t}}}$ to represent the estimated rotation and translation and use ${{\mathbf{R}}_{gt}}$ and ${{\mathbf{t}}_{gt}}$ to represent the ground truth. The rotation error is evaluated by the angle of the axis-angle representation of ${\mathbf{R}}_{gt}^{-1}{\mathbf{R}}$, and the translation error by $\text{ }{{{\left\|{{\mathbf{t}}_{gt}}-\mathbf{\hat{t}}\right\|}_{2}}}/{\left\|{{\mathbf{t}}_{gt}}\right\|}\;$. We consider the effective number of correspondences as [4]. Specifically, the effective number of correspondences for ${{n}_{\pi}}$ point-to-plane, ${{n}_{l}}$ point-to-line, and ${{n}_{p}}$ point-to-point correspondences is calculated as $N={{n}_{\pi}}+2{{n}_{l}}+3{{n}_{p}}$. Given an $N$, we randomly generate a combination of ${n}_{\pi}$, ${{n}_{l}}$ and ${{n}_{p}}$ whose effective number of correspondences is $N$.

Effect of intermediate unknowns  We introduce intermediate unknowns to simplify the least-squares and the minimal problems. To verify their benefit, we evaluate the performance of the direct least-squares solution (DLSSol) from solving (13), and the direct minimal solution (DMinSol) from solving the equation system from the numerator of (9) for the minimal problem. We employed the algorithm in [19] to generate the solvers. For the least-squares problem, we run 2000 trails for each $N\in[7,15]$. Fig. 1 shows the results. Directly solving (13) can recover the global minimizer of (4) in theory. However, the large mean errors of DLSSol verify that this polynomial solver is very unstable. For the minimal problem, we run 20000 trails for the 6 point-to-plane configuration. Fig. 2 shows the results. The DMinSol has a much longer tail than our algorithm.

Table 3 lists the average computational time of different algorithms. For the least-squares problem, we compare the computational time of solving the first order optimality conditions (13). This time is independent of the number of correspondences. As shown in Table 3, our least-squares solution is about 7 times faster than DLSSol, and our minimal solution is about 2 times faster than DMinSol.

The above results verify that our intermediate unknowns can increase the numeric stability as well as reduce the computational time.

Least-squares solution  We conduct experiments to evaluate the performances of different algorithms under varying number of correspondences, increasing level of noise and computational time. The results of all experiments are from 100 independent trials as [4].

The first experiment considers a fixed noise level and an increasing number of correspondences. Let us denote the standard deviation of a zero mean Gaussian noise distribution as $\delta$. We set $\delta=0.05m$. $N$ varies from $7$ to $15$. Fig. 3 shows the result. For the pose-to-point case, most of the algorithms can recover the optimal solution except for [24] when the number of points is small. For the mixed correspondences, the results of BnB [26] and Briales’s algorithm [4] overlap. This is consistent to the results in [4]. Our algorithm significantly outperforms previous works when $N$ is small as the ambiguous case more likely happens for a small $N$. When $N$ is large, our algorithm achieves the same accuracy as [26] that has guaranteed global optimality. Wientapper’s algorithm [41] can not find the optimal solution even $N$ is large.

In the second experiment, $\delta$ is from $0.01m$ to $0.11m$, stepping by 0.02m. The results are illustrated in Fig. 4. For the point-to-point case, most of the algorithms provide the optimal solution as [1], except for [41] when $\delta=0.1m$. For the mixed case, BnB, Briales’s algorithm and our algorithm present better results than other algorithms. Our algorithm gives a slightly better result than BnB and Briales’s algorithm when $\delta=0.11m$.

In the last experiment, we compare the computational time of different algorithms. We vary the effective number of correspondences $N$ from 10 to 2000. For every $N$, we run each algorithm 100 times to calculate the average running time. Fig. 5 provides the result. We did not run the BnB algorithm [26], because it is too slow for a large $N$. It is clear that our algorithm is the fastest one among the compared algorithms. Wientapper’s algorithm [41] is efficient. But their algorithm needs to compute a problem 4 times, thus the gap between the computational time of two algorithms increases as $N$ enlarges. Actually, $N$ could be 10 times larger in real applications than our simulation, as shown in Table 4.

Minimal solution  The equation system (29) is important for the estimation of $\bf{R}$. Actually, Ramalingam et al. [31] formulated some point-to-plane configurations as a quadratic equation system as (29). We first consider the numeric stability of the polynomial solver for (29). We compare our hidden variable method, E3Q3 [17], and the Gröbner basis polynomial solver generated by [19]. We randomly generate a real solution and the coefficients of (29). Then we substitute this real solution into (29) to calculate the constant terms of (29). We run each algorithm 20000 trails and compute the estimation error of this real solution. Fig. 6 (a) shows the estimation error histograms of the compared algorithms. It is clear that our hidden variable method is more stable than other algorithms. The histograms of E3Q3 and the Gröbner basis method have long tails. This is because E3Q3 and the Gröbner basis method require to compute the inverse of a matrix. When a matrix approximates a singular matrix, the performance of these algorithms will degrade. Fig. 6 (b) demonstrates the performance of E3Q3 under a degenerate case. We generate nearly singular matrix for E3Q3, whose least singular value is in $(0,10^{-6})$ . In this situation, E3Q3 performs very bad.

We also evaluate the numeric stability of our unified minimal solution. We run 2000 independent trails for each minimal configuration. As demonstrated in Fig. 7, our algorithm provides accurate solutions for all minimal configurations. This avoids solving each configuration case by case.

6.2 Experiments with Real Data

We generated our real-world dataset from the KITTI dataset [10]. Our dataset contains point-to-point, point-to-line and point-to-plane correspondences described in Table 4. There are more than 20000 correspondences in each frame, with the majority of them being point-to-plane correspondences. We evaluate the accuracy and running time of different algorithms on this dataset.

For each frame, 2D feature points are detected by the ORB feature detector [34]. We project LiDAR points into the image plane, and select the LiDAR points around an ORB feature. Then, we fit a local plane to these LiDAR points. Finally, we calculate the 3D coordinates of an ORB feature by calculating the intersection of the back-projection ray of the ORB feature and the local plane. We use the ORB descriptors to match 2D feature points, and obtain the 3D point-to-point correspondences.

Next, we generate point-to-line correspondences. For each frame, 2D lines are detected by the Line Segment Detector (LSD) [33] and described by the Line Band Descriptor (LBD) [46]. A 2D line is represented by two 2D endpoints. The 3D endpoints of a line is generated as the 2D feature points described above. We then generate 3D line correspondences by matching their LBD features. Given a line-to-line correspondence, two point-to-line correspondences are generated for each of its endpoints.

Finally, we calculated point-to-plane correspondences. For each frame, planes are extracted from LiDAR points by the region growing algorithm [28]. To match a plane point to a previous plane, we find the nearest LiDAR point in previous planes.

We evaluate the performance of different algorithms on sequences 03, 04, and 07 of the KITTI dataset. We did not test BnB [26], as it is extremely slow on this large dataset. Our algorithm achieves the same or slightly better result as the state-of-the-art algorithm [4] while being around 7 times faster, as shown in Table  2.

7 Conclusions

In this paper, we present an efficient and accurate least-squares solution and an unified minimal solution for the 3D registration problem. We proof that there exist ambiguous configurations for any number of point-to-plane and point-to-line correspondences. This requires an algorithm has the ability to reveal local minimizers. We use the CGR parameterization to represent the rotation, removing the quadratic constraints on the rotation. However, this results in a rational form residual which is hard to solve. We introduce several intermediate variables to simplify the first-order optimality conditions of the least-squares problem, and the equation system of the minimal configuration. We evaluate our algorithm through synthetic and real data. The experimental results show that computing local minimizers is essential, especially when $N$ is small. Besides, our algorithm is as accurate as previous globally optimal solutions when $N$ is large, but is much faster.

Appendix A Proof for lemma 1, lemma 2 and theorem 1

As we mentioned in the paper, point-to-line and point-to-plane correspondences have ambiguous configurations. Here we prove lemma 1, lemma 2 and theorem 1 in our paper.

Lemma 1. For any $n_{l}$ points and any $\left\{{{{\bf{R}}_{1}},{{\bf{t}}_{1}}}\right\}$ and $\left\{{{{\bf{R}}_{2}},{{\bf{t}}_{2}}}\right\}$, there exist $n_{l}$ lines to make $\left\{{{{\bf{R}}_{1}},{{\bf{t}}_{1}}}\right\}$ and $\left\{{{{\bf{R}}_{2}},{{\bf{t}}_{2}}}\right\}$ are exact solutions for the $n_{l}$ point-to-line correspondences.

Proof.

Define ${{\bf{x}}_{i}}$ the $i$th point of an arbitrary point set with ${n_{l}}$ points. For any $\left\{{{{\bf{R}}_{1}},{{\bf{t}}_{1}}}\right\}$ and $\left\{{{{\bf{R}}_{2}},{{\bf{t}}_{2}}}\right\}$, we can have ${\bf{y}}_{i}^{1}={{\bf{R}}_{1}}{{\bf{x}}_{i}}+{{\bf{t}}_{1}}$ and ${\bf{y}}_{i}^{2}={{\bf{R}}_{2}}{{\bf{x}}_{i}}+{{\bf{t}}_{2}}$. Then we can use ${\bf{y}}_{i}^{1}$ and ${\bf{y}}_{i}^{2}$ to define a line ${{\bf{l}}_{i}}=\left[{{{\bf{d}}_{i}};{{\bf{y}}_{i}}}\right]$ with direct ${{\bf{d}}_{i}}=\frac{{{\bf{y}}_{i}^{1}-{\bf{y}}_{i}^{2}}}{{{{\left\|{{\bf{y}}_{i}^{1}-{\bf{y}}_{i}^{2}}\right\|}_{2}}}}$ and a point ${{\bf{y}}_{i}}={\bf{y}}_{i}^{1}$ on it, as demonstrated in Fig. 8 (a). According to how we construct ${\bf{l}}_{i}$, we know that ${\bf{l}}_{i}$ passes through ${\bf{y}}_{i}^{1}$ and ${\bf{y}}_{i}^{2}$. Therefore $\left\{{{{\bf{R}}_{1}},{{\bf{t}}_{1}}}\right\}$ and $\left\{{{{\bf{R}}_{2}},{{\bf{t}}_{2}}}\right\}$ are two solutions for the ${n_{l}}$ point-to-line $\left\{{{{\bf{x}}_{i}}\leftrightarrow{{\bf{l}}_{i}}}\right\}_{i=1}^{{n_{l}}}$. ∎

Lemma 2. For any $n_{\pi}$ points and any $\left\{{{{\bf{R}}_{1}},{{\bf{t}}_{1}}}\right\}$, $\left\{{{{\bf{R}}_{2}},{{\bf{t}}_{2}}}\right\}$ and $\left\{{{{\bf{R}}_{3}},{{\bf{t}}_{3}}}\right\}$, there exist $n_{\pi}$ planes to make $\left\{{{{\bf{R}}_{1}},{{\bf{t}}_{1}}}\right\}$, $\left\{{{{\bf{R}}_{2}},{{\bf{t}}_{2}}}\right\}$ and $\left\{{{{\bf{R}}_{3}},{{\bf{t}}_{3}}}\right\}$ are exact solutions for the $n_{\pi}$ point-to-plane correspondences.

Proof.

Define ${{\bf{x}}_{i}}$ the $i$th point of an arbitrary point set with $n_{\pi}$ points. For any $\left\{{{{\bf{R}}_{1}},{{\bf{t}}_{1}}}\right\}$, $\left\{{{{\bf{R}}_{2}},{{\bf{t}}_{2}}}\right\}$ and $\left\{{{{\bf{R}}_{3}},{{\bf{t}}_{3}}}\right\}$, we can have ${\bf{y}}_{i}^{1}={{\bf{R}}_{1}}{{\bf{x}}_{i}}+{{\bf{t}}_{1}}$, ${\bf{y}}_{i}^{2}={{\bf{R}}_{2}}{{\bf{x}}_{i}}+{{\bf{t}}_{2}}$ and ${\bf{y}}_{i}^{3}={{\bf{R}}_{3}}{{\bf{x}}_{i}}+{{\bf{t}}_{3}}$. Then we can find a plane ${\bf{\pi}}_{i}$ passing through ${\bf{y}}_{i}^{1}$, ${\bf{y}}_{i}^{2}$ and ${\bf{y}}_{i}^{3}$, as demonstrated in Fig. 8 (b). According to how we construct ${\pi_{i}}$, we know that $\left\{{{{\bf{R}}_{1}},{{\bf{t}}_{1}}}\right\}$, $\left\{{{{\bf{R}}_{2}},{{\bf{t}}_{2}}}\right\}$ and $\left\{{{{\bf{R}}_{3}},{{\bf{t}}_{3}}}\right\}$ are three solutions for the ${n_{\pi}}$ point-to-plane correspondences $\left\{{{{\bf{x}}_{i}}\leftrightarrow{{\bf{\pi}}_{i}}}\right\}_{i=1}^{{n_{\pi}}}$.

∎

Theorem 1. For any $n_{l}$ points on lines, $n_{\pi}$ points on planes and any $\left\{{{{\bf{R}}_{1}},{{\bf{t}}_{1}}}\right\}$ and $\left\{{{{\bf{R}}_{2}},{{\bf{t}}_{2}}}\right\}$, there exist $n_{l}$ lines and $n_{\pi}$ planes to make $\left\{{{{\bf{R}}_{1}},{{\bf{t}}_{1}}}\right\}$ and $\left\{{{{\bf{R}}_{2}},{{\bf{t}}_{2}}}\right\}$ are exact solutions for the $n_{l}$ point-to-line and $n_{\pi}$ point-to-plane correspondences.

Proof.

We first consider the $n_{l}$ points on lines. Define ${\bf{x}}_{i}^{l}$ is the $i$th point. According to Lemma 1, we can find $n_{l}$ lines to make $\left\{{{{\bf{R}}_{1}},{{\bf{t}}_{1}}}\right\}$ and $\left\{{{{\bf{R}}_{2}},{{\bf{t}}_{2}}}\right\}$ are exact solutions for the $n_{l}$ point-to-line correspondences $\left\{{{{\bf{x}}_{i}^{l}}\leftrightarrow{{\bf{l}}_{i}}}\right\}_{i=1}^{{n_{l}}}$.

Then we consider the $n_{\pi}$ points on planes. For the $j$th point ${{\bf{x}}_{j}^{\pi}}$ within them, we can define ${}^{1}{\bf{y}}_{j}^{\pi}={{\bf{R}}_{1}}{{\bf{x}}_{j}^{\pi}}+{{\bf{t}}_{1}}$ and ${}^{2}{\bf{y}}_{j}^{\pi}={{\bf{R}}_{2}}{{\bf{x}}_{j}^{\pi}}+{{\bf{t}}_{2}}$. Let us denote ${\bf{\pi}}_{j}$ as a plane passing through the line defined by ${{}^{1}{\bf{y}}}_{j}^{\pi}$ and ${}^{2}{\bf{y}}_{j}^{\pi}$. According to how we construct ${\pi_{j}}$, we know that $\left\{{{{\bf{R}}_{1}},{{\bf{t}}_{1}}}\right\}$ and $\left\{{{{\bf{R}}_{2}},{{\bf{t}}_{2}}}\right\}$ are two solutions for the ${n_{\pi}}$ point-to-plane correspondences $\left\{{{{\bf{x}}_{j}^{\pi}}\leftrightarrow{{\bf{\pi}}_{j}}}\right\}_{j=1}^{{n_{\pi}}}$.

Therefore, $\left\{{{{\bf{R}}_{1}},{{\bf{t}}_{1}}}\right\}$ and $\left\{{{{\bf{R}}_{2}},{{\bf{t}}_{2}}}\right\}$ are the two solutions for the $n_{l}$ point-to-line correspondences $\left\{{{{\bf{x}}_{i}^{l}}\leftrightarrow{{\bf{l}}_{i}}}\right\}_{i=1}^{{n_{l}}}$ and ${n_{\pi}}$ point-to-plane correspondences $\left\{{{{\bf{x}}_{i}^{\pi}}\leftrightarrow{{\bf{\pi}}_{i}}}\right\}_{i=1}^{{n_{\pi}}}$. ∎

According to Lemma 1 and Theorem 1, our algorithm keeps the smallest 2 minimuma, if we use point-to-line correspondences or point-to-plane and point-to-line correspondences for pose estimation. If we only have point-to-plane correspondences, our algorithm keeps the smallest 3 minimuma , according to Lemma 2.