Closed-form solution to point- and plane-based co-registration of terrestrial LiDAR point clouds

Abstract

Co-registration is required when the alignment of two or more point clouds obtained for mapping natural and built environments is needed. While closed-form solutions are suitable for co-registration, most of the existing approaches rely on unit quaternion solutions for the estimation of transformation parameters from point or plane correspondences. This paper presents a novel co-registration of terrestrial light detection and ranging point clouds solution to create globally consistent 3-D environments. Our method exploits the advantages of the dual quaternion solution combining both points and plane correspondences. The role of our relaxation labeling technique in 3-D matching (3PRL) is investigated, and its efficiency to find the best plane correspondences is shown. The paper also presents a method to treat degenerate plane configurations with corresponding virtual points. Experimental results reveal that our 3PRL technique can update and improve the 3-D matching probabilities using binary relations. At the same time, the proposed dual quaternions point- and plane-based optimization indicated that the mathematical optimization might represent a valid model for co-registration of point clouds. A closer inspection of co-registration accuracy revealed that the translation and rotation error mean decreased drastically, with margins between 0.10 m and 0.17 m and 0.01° and 0.33°, respectively. Experiments have shown that our method generally achieves better results than existing methods.

Appendix

For the estimation of the translation parameters, we proposed the minimization of the point to plane distance as follows:

$$\mathrm{E}={\sum }_{\mathrm{i}=1}^{\mathrm{n}}{\left(-{\mathrm{d}}_{\mathrm{i}}^{\mathrm{p}}+{\overrightarrow{\mathrm{n}}}_{\mathrm{i}}^{\mathrm{r}} \cdot \overrightarrow{\mathrm{t}}+{\mathrm{d}}_{\mathrm{i}}^{\mathrm{r}} \right)}^{2}=\mathrm{min}$$

(A1)

The Eq. A1 can be rewritten as follows:

$$\begin{array}{c}\mathrm{E}={\sum }_{\mathrm{i}=1}^{\mathrm{n}}{\left({\overrightarrow{\mathrm{n}}}_{\mathrm{i}}^{\mathrm{r}} \cdot \overrightarrow{\mathrm{t}}+{\mathrm{d}}_{\mathrm{i}}^{\mathrm{r}}-{\mathrm{d}}_{\mathrm{i}}^{\mathrm{p}} \right)}^{2}\\ ={\sum }_{\mathrm{i}=1}^{\mathrm{n}}\left({\left({\overrightarrow{\mathrm{n}}}_{\mathrm{i}}^{\mathrm{r}} \cdot \overrightarrow{\mathrm{t}}\right)}^{2}\right.\\ \begin{array}{c}+2\left({\overrightarrow{\mathrm{n}}}_{\mathrm{i}}^{\mathrm{r}} \cdot \overrightarrow{\mathrm{t}}\right)\left({\mathrm{d}}_{\mathrm{i}}^{\mathrm{r}}-{\mathrm{d}}_{\mathrm{i}}^{\mathrm{p}}\right)\\ \left.+{\left({\mathrm{d}}_{\mathrm{i}}^{\mathrm{r}}-{\mathrm{d}}_{\mathrm{i}}^{\mathrm{p}}\right)}^{2}\right)=\\ \begin{array}{c}={\sum }_{\mathrm{i}=1}^{\mathrm{n}}{\left({\overrightarrow{\mathrm{n}}}_{\mathrm{i}}^{\mathrm{r}} \cdot \overrightarrow{\mathrm{t}}\right)}^{2}+2{\sum }_{\mathrm{i}=1}^{\mathrm{n}}\left({\overrightarrow{\mathrm{n}}}_{\mathrm{i}}^{\mathrm{r}} \cdot \overrightarrow{\mathrm{t}}\right)\left({\mathrm{d}}_{\mathrm{i}}^{\mathrm{r}}-{\mathrm{d}}_{\mathrm{i}}^{\mathrm{p}}\right)\\ +{\sum }_{\mathrm{i}=1}^{\mathrm{n}}{\left({\mathrm{d}}_{\mathrm{i}}^{\mathrm{r}}-{\mathrm{d}}_{\mathrm{i}}^{\mathrm{p}}\right)}^{2}\end{array}\end{array}\end{array}$$

(A2)

Assuming that all normal vectors ${\mathrm{n}}_{\mathrm{i}}^{\mathrm{r}}$ have unit norm for $1 \le \mathrm{ i }\le \mathrm{ n}$, we have:

$$\mathrm{E}={\sum }_{\mathrm{i}=1}^{\mathrm{n}}{\left({\overrightarrow{\mathrm{n}}}_{\mathrm{i}}^{\mathrm{r}} \cdot \overrightarrow{\mathrm{t}}\right)}^{2}+2{\sum }_{\mathrm{i}=1}^{\mathrm{n}}\left({\overrightarrow{\mathrm{n}}}_{\mathrm{i}}^{\mathrm{r}} \cdot \overrightarrow{\mathrm{t}}\right)\left({\mathrm{d}}_{\mathrm{i}}^{\mathrm{r}}-{\mathrm{d}}_{\mathrm{i}}^{\mathrm{p}}\right)+{\sum }_{\mathrm{i}=1}^{\mathrm{n}}{\left({\mathrm{d}}_{\mathrm{i}}^{\mathrm{r}}-{\mathrm{d}}_{\mathrm{i}}^{\mathrm{p}}\right)}^{2}=\mathrm{min}$$

(A3)

Considering that ${\sum }_{\mathrm{i}=1}^{\mathrm{n}}{\left({\mathrm{d}}_{\mathrm{i}}^{\mathrm{r}}-{\mathrm{d}}_{\mathrm{i}}^{\mathrm{p}}\right)}^{2}=\mathrm{c}$:

$$\mathrm{E}=\mathrm{c}+{\sum }_{\mathrm{i}=1}^{\mathrm{n}}{\left({\overrightarrow{\mathrm{n}}}_{\mathrm{i}}^{\mathrm{r}} \cdot \overrightarrow{\mathrm{t}}\right)}^{2}+2{\sum }_{\mathrm{i}=1}^{\mathrm{n}}\left({\overrightarrow{\mathrm{n}}}_{\mathrm{i}}^{\mathrm{r}} \cdot \overrightarrow{\mathrm{t}}\right)\left({\mathrm{d}}_{\mathrm{i}}^{\mathrm{r}}-{\mathrm{d}}_{\mathrm{i}}^{\mathrm{p}}\right)$$

(A4)

By developing the Eq. (A4), we find the Eq. (22). Introducing the quaternions concept and rewritten the left product ${\overrightarrow{\mathrm{n}}}_{\mathrm{i}}^{\mathrm{r}}\mathrm{q}$, we have:

$$\left[{\mathrm{L}}_{{\overrightarrow{\mathrm{n}}}_{\mathrm{i}}^{\mathrm{r}}}\right]\mathrm{Q}=\left[\begin{array}{cc}\begin{array}{c}\begin{array}{cc}0& -{\mathrm{n}}_{{\mathrm{x}}_{\mathrm{i}}}\\ {\mathrm{n}}_{{\mathrm{x}}_{\mathrm{i}}}& 0\end{array}\\ \begin{array}{cc}{\mathrm{n}}_{{\mathrm{y}}_{\mathrm{i}}}& {\mathrm{n}}_{{\mathrm{z}}_{\mathrm{i}}}\\ {\mathrm{n}}_{{\mathrm{z}}_{\mathrm{i}}}& {-\mathrm{n}}_{{\mathrm{y}}_{\mathrm{i}}}\end{array}\end{array}& \begin{array}{cc}\begin{array}{c}\begin{array}{c}-{\mathrm{n}}_{{\mathrm{y}}_{\mathrm{i}}}\\ {-\mathrm{n}}_{{\mathrm{z}}_{\mathrm{i}}}\end{array}\\ \begin{array}{c}0\\ {\mathrm{n}}_{{\mathrm{x}}_{\mathrm{i}}}\end{array}\end{array}& \begin{array}{c}\begin{array}{c}-{\mathrm{n}}_{{\mathrm{z}}_{\mathrm{i}}}\\ {\mathrm{n}}_{{\mathrm{y}}_{\mathrm{i}}}\end{array}\\ \begin{array}{c}{-\mathrm{n}}_{{\mathrm{x}}_{\mathrm{i}}}\\ 0\end{array}\end{array}\end{array}\end{array}\right]\left[\begin{array}{c}\begin{array}{c}{\mathrm{q}}_{0}\\ {\mathrm{q}}_{1}\end{array}\\ \begin{array}{c}{\mathrm{q}}_{2}\\ {\mathrm{q}}_{3}\end{array}\end{array}\right]$$

(A5)

The inner product $\langle \widehat{\mathrm{q}}, \left({\overrightarrow{\mathrm{n}}}_{\mathrm{i}}^{\mathrm{r}}\mathrm{q}\right)\rangle$ can be written as the product of matrices, as follows:

$${\widehat{\mathrm{Q}}}^{\mathrm{T}}\left[{\mathrm{L}}_{{\overrightarrow{\mathrm{n}}}_{\mathrm{i}}^{\mathrm{r}}}\right]\mathrm{Q}={\mathrm{Q}}^{\mathrm{T}}{\left[{\mathrm{L}}_{{\overrightarrow{\mathrm{n}}}_{\mathrm{i}}^{\mathrm{r}}}\right]}^{\mathrm{T}}\widehat{\mathrm{Q}}$$

(A6)

Rewritten $\left({\mathrm{d}}_{\mathrm{i}}^{\mathrm{r}}-{\mathrm{d}}_{\mathrm{i}}^{\mathrm{p}}\right)\langle \widehat{\mathrm{q}}, \left({\overrightarrow{\mathrm{n}}}_{\mathrm{i}}^{\mathrm{r}}\mathrm{q}\right)\rangle$ as product of matrices, we have:

$$\left({\mathrm{d}}_{\mathrm{i}}^{\mathrm{r}}-{\mathrm{d}}_{\mathrm{i}}^{\mathrm{p}}\right){\widehat{\mathrm{Q}}}^{\mathrm{T}}\left(\left[{\mathrm{L}}_{{\overrightarrow{\mathrm{n}}}_{\mathrm{i}}^{\mathrm{r}}}\right]\mathrm{Q}\right)={\widehat{\mathrm{Q}}}^{\mathrm{T}}\left(\left({\mathrm{d}}_{\mathrm{i}}^{\mathrm{r}}-{\mathrm{d}}_{\mathrm{i}}^{\mathrm{p}}\right)\left[{\mathrm{L}}_{{\overrightarrow{\mathrm{n}}}_{\mathrm{i}}^{\mathrm{r}}}\right]\right)\mathrm{Q}$$

(A7)

Thus, the sum is equal to the product of matrices:

$$\begin{array}{c}\mathrm{E}=\mathrm{c}+4{\sum }_{\mathrm{i}=1}^{\mathrm{n}}{\left({\widehat{\mathrm{Q}}}^{\mathrm{T}}\left[{\mathrm{L}}_{{\overrightarrow{\mathrm{n}}}_{\mathrm{i}}^{\mathrm{r}}}\right]\mathrm{Q}\right)}^{2}\\ +4{\sum }_{\mathrm{i}=1}^{\mathrm{n}}\left({\widehat{\mathrm{Q}}}^{\mathrm{T}}\left(\left({\mathrm{d}}_{\mathrm{i}}^{\mathrm{r}}-{\mathrm{d}}_{\mathrm{i}}^{\mathrm{p}}\right)\left[{\mathrm{L}}_{{\overrightarrow{\mathrm{n}}}_{\mathrm{i}}^{\mathrm{r}}}\right]\right)\mathrm{Q }\right)=\\ \begin{array}{c}=\mathrm{c}+4{\widehat{\mathrm{Q}}}^{\mathrm{T}}\left({\sum }_{\mathrm{i}=1}^{\mathrm{n}}\left(\left[{\mathrm{L}}_{{\overrightarrow{\mathrm{n}}}_{\mathrm{i}}^{\mathrm{r}}}\right]{\mathrm{QQ}}^{\mathrm{T}}{\left[{\mathrm{L}}_{{\overrightarrow{\mathrm{n}}}_{\mathrm{i}}^{\mathrm{r}}}\right]}^{\mathrm{T}}\right)\right)\widehat{\mathrm{Q}}\\ +4{\widehat{\mathrm{Q}}}^{\mathrm{T}}\left({\sum }_{\mathrm{i}=1}^{\mathrm{n}}\left(\left({\mathrm{d}}_{\mathrm{i}}^{\mathrm{r}}-{\mathrm{d}}_{\mathrm{i}}^{\mathrm{p}}\right)\left[{\mathrm{L}}_{{\overrightarrow{\mathrm{n}}}_{\mathrm{i}}^{\mathrm{r}}}\right]\right)\right)\mathrm{Q}=\\ =\mathrm{c}+4{\widehat{\mathrm{Q}}}^{\mathrm{T}}{\mathrm{W}}_{4}\widehat{\mathrm{Q}}+4{\widehat{\mathrm{Q}}}^{\mathrm{T}}{\mathrm{W}}_{5}\mathrm{Q}\end{array}\end{array}$$

(A8)

Then, in order to find $\widehat{\mathrm{Q}}$ the Eq. A8 should be minimized with the following constraint:

$$4{\widehat{\mathrm{Q}}}^{\mathrm{T}}\mathrm{Q}=0$$

(A9)

which can be solved using the Lagrange multipliers, as follows:

$$\widetilde{\mathrm{E}}=\mathrm{c}+2{\widehat{\mathrm{Q}}}^{\mathrm{T}}{\mathrm{W}}_{4}\widehat{\mathrm{Q}}+4{\widehat{\mathrm{Q}}}^{\mathrm{T}}{\mathrm{W}}_{5}\mathrm{Q}+4\uplambda {\widehat{\mathrm{Q}}}^{\mathrm{T}}\mathrm{Q}$$

(A10)

where $\uplambda$ denotes the Lagrange multipliers. The partial derivative are:

$$\frac{\partial \widetilde{\mathrm{E}}}{\partial \widehat{\mathrm{Q}}}=4\left({\mathrm{W}}_{4}+{\mathrm{W}}_{4}^{\mathrm{T}}\right)\widehat{\mathrm{Q}}+4{\mathrm{W}}_{5}\mathrm{Q}+4\mathrm{\lambda Q}=0$$

(A11)

The two beforementioned equations are equal to the following linear system:

$$\left\{\begin{array}{c}4\left({\mathrm{W}}_{4}+{\mathrm{W}}_{4}^{\mathrm{T}}\right)\widehat{\mathrm{Q}}+4{\mathrm{W}}_{5}Q+4\lambda Q=0\\ 4{\widehat{\mathrm{Q}}}^{\mathrm{T}}Q=0\end{array}\right.$$

(A12)

As ${\mathrm{W}}_{4}$ is a symmetric matrix, we have:

$$\left\{\begin{array}{c}\left({2\mathrm{W}}_{4}\right)\widehat{\mathrm{Q}}+Q\lambda =-{\mathrm{W}}_{5}Q\\ {\mathrm{Q}}^{\mathrm{T}}\widehat{\mathrm{Q}}=0\end{array}\right.$$

(A13)