WTLS iterative algorithm of 3D similarity coordinate transformation based on Gibbs vectors

Abstract

The 3D similarity coordinate transformation is fundamental and frequently encountered in many areas of work such as geodesy, engineering surveying, LIDAR, terrestrial laser scanning, photogrammetry, machine vision, etc. The algorithms of 3D similarity transformation are divided into two categories. One is a closed-form algorithm that is straightforward and fast. However, it cannot provide the accuracy information for the transformation parameters. The other category of algorithm is iterative, and this can offer the accuracy information for the transformation parameters. However, the latter usually needs a good initial value of the unknown. Considering the accuracy information for transformation parameters is essential or indispensable from the viewpoint of uncertainty, this contribution proposes a weighted total least squares (WTLS) iterative algorithm of the 3D similarity coordinate transformation based on Gibbs vectors. It is fast in terms of fewer iterations, reliable and does not need good initial values of transformation parameters. Two cases including the registration of LIDAR points with big rotation angles and a geodetic datum transformation with small rotation angles are demonstrated to validate the new algorithm.

Introduction

The 3D similarity coordinate transformation aims to align the coordinate of points in different coordinate systems into a common coordinate system. It usually implements the transformation based on 3D seven-parameter similarity transformation utilizing a set of control points with the coordinates both in the source coordinate system and in the target coordinate system. This work is very popular in many fields, such as geodesy, engineering surveying, LIDAR, terrestrial laser scanning, photogrammetry, machine vision, etc. (Besl and McKay 1992; Crosilla and Beinat 2002; Horn 1987; Jaw and Chuang 2008; Kashani 2006; Krarup 1985; Marx 2017; Paffenholz and Bae 2012; Walker et al. 1991; Wang et al. 2014; Závoti and Kalmár 2016; Zeng 2014; Zeng et al. 2018).

As far as the algorithm of 3D similarity coordinate transformation is concerned, much related literature has been published. The algorithms proposed in that literature can be divided into two categories. One is the closed-form or analytical algorithm. This category of algorithms utilizes the technologies such as singular value decomposition (SVD) also known as the Procrustes algorithm, e.g., Umeyama (1991), Crosilla and Beinat (2002), Grafarend and Awange (2003), eigenvalue–eigenvector decomposition (a quaternion-based algorithm), e.g., Horn (1987), Shen et al. (2006), orthonormal matrix, e.g., Horn et al. (1988), Zeng (2015). Additionally, Leick and Van Gelder (1975), Zeng and Yi (2010) presented, respectively, a step-wise algorithm based on the physical meaning of the similarity transformation. Wang et al. (2014) proposed a closed-form pairwise registration algorithm of point clouds utilizing the dual quaternion. The closed-form algorithms are fast and efficient because the parameters are recovered directly by exact formulae without solution of nonlinear equations. However, this type of algorithm cannot deal with the general weight matrix of observations (i.e., the coordinates as pseudo-observations) as well as the accuracy estimation of transformation parameters. The other one is the iterative algorithm, which utilizes the numerical (iterative) computation technique to find the unknowns. Lots of iterative algorithms have been presented at present, e.g., Zeng and Tao (2003), Chen et al. (2004), El-Habiby et al. (2009), Zeng and Yi (2011), Zeng et al. (2016), Kurt (2018), Zeng et al. (2019). This type of algorithm usually requires a good initial value of unknown and iterative computation (e.g., Zeng and Tao 2003; Zeng and Yi 2011). But, in some situations, e.g., registration of LIDAR points due to the arbitrary size of rotation angles, it is difficult or even unlikely to obtain a good initial value of parameter. As a result, the algorithm needs many iterations or falls into a local minimum or diverges. Sometimes initial values are not required if global optimization algorithms are performed (e.g., Xu 2003); unfortunately it may lead to much more computation time and burden. The advantage of iterative algorithms over closed-form algorithms is the former can supply the accuracy information of unknowns, which is essential from the viewpoint of uncertainty.

In recent years, the errors-in-variables (EIV) model has evoked a lot of research interest. It considers the errors in all variables; for instance, in the 3D similarity transformation, the errors of target coordinates and source coordinates of control points are considered. Thus it is more reasonable than the traditional Gauss–Markov (GM) model which just considers the errors of target coordinates of control points. Golub and Van Loan (1980) presented the approach dealing with the EIV model and named it total least squares (TLS) technique. Afterward, a few types of TLS algorithms of 2D or 3D similarity coordinate transformation have been proposed. Teunissen (1988) derived a closed-form solution for 2D similarity transformation in the EIV model, i.e., symmetric 2D similarity transformation. Goryn and Hein (1995) presented a TLS solution for the 3D rigid transformation based on the Procrustes algorithm considering homogenous and uncorrelated errors. Schaffrin and Felus (2008) proposed TLS approaches to empirical coordinate transformation, which improved the accuracy in 2D affine transformations. Felus and Burtch (2009) proposed a closed-form weighted TLS (WTLS) solution to the 3D similarity transformation with pointwise weights and uncorrelated errors among points based on the Procrustes algorithm. Neitzel (2010) proposed a TLS solution to 2D similarity transformation within the nonlinear Gauss–Helmert model. Fang (2015) presented a WTLS algorithm with constraints and universal formula for geodetic transformation. Chang (2015) presented a rigorous solution without presupposed fixing of the scale parameter (which some authors assume to be 1). Mahboub (2016) proposed a WTLS solution to 3D symmetrical similarity transformation without linearization; however, more iterations may be needed than the linearized model method. Mercan et al. (2018) proposed a weighted similarity transformation based on quaternions. The algorithm has seven unknowns including the translation parameters and scaled quaternion, and the iterations may lead to divergence.

As mentioned above, the closed-form solution to 3D similarity coordinate transformation is straightforward and fast, but cannot provide the accuracy of recovered transformation parameters. Iterative solutions to 3D similarity coordinate transformation can deal with general weight and provide the accuracy of recovered transformation parameters; however, it usually needs a good initial value of unknowns. Therefore, this contribution intends to present a new WTLS iterative solution to 3D similarity coordinate transformation based on Gibbs vectors, which is fast and does not need a relatively good initial value of the unknowns; in other words, is not sensitive to the initial value of unknowns.

In the next section, firstly 3D similarity transformation is introduced and the model of similarity transformation based on Gibbs vectors is established. Secondly in order to simplify the transformation model for improvement of computation performance, the translation parameters are derived in the WTLS sense and a WTLS iterative algorithm of 3D similarity transformation based on Gibbs vectors is proposed with detailed derivation. Two cases including the registration of LIDAR points and geodetic datum transformation are studied; the results show the new algorithms are fast and reliable. Finally, conclusions are drawn in the last section.

Formulation and WTLS iterative algorithm of 3D similarity transformation

In this section, the basic EIV model of 3D similarity transformation is introduced, and in order to avoid transcendental function and functional constraints, the model of similarity transformation based on Gibbs vectors is established. Lastly, a WTLS iterative algorithm of 3D similarity transformation based on Gibbs vectors is put forward.

3D similarity transformation in EIV model without functional constraints

Assume that a set of control points are given with their coordinates in both source and target coordinate systems. Then the 3D similarity transformation in EIV model can be expressed as

$${\mathbf{p}}_{i}^{t} - {\mathbf{e}}_{i}^{t} = \lambda {\mathbf{R}}\left( {{\mathbf{p}}_{i}^{o} - {\mathbf{e}}_{i}^{o} } \right) + {\mathbf{t}}$$

(1)

subject to

$${\mathbf{R}}^{\text{T}} {\mathbf{R}} = {\mathbf{I}}_{3} \;{\text{and}}\;\det ({\mathbf{R}}) = + 1 ,$$

(2)

where ${\mathbf{p}}_{i}^{o} = \left[ {\begin{array}{*{20}c} {x_{i}^{o} } & {y_{i}^{o} } & {z_{i}^{o} } \\ \end{array} } \right]^{\text{T}}$ and ${\mathbf{p}}_{i}^{t} = \left[ {\begin{array}{*{20}c} {x_{i}^{t} } & {y_{i}^{t} } & {z_{i}^{t} } \\ \end{array} } \right]^{\text{T}}$ are the 3D coordinate vectors of control point $i$ ($i = 1,2, \ldots ,n$) in the source coordinate system and the target coordinate system (denoted with superscript $o$ and $t$), respectively. ${\mathbf{e}}_{i}^{o}$ and ${\mathbf{e}}_{i}^{t}$ are the error vectors of ${\mathbf{p}}_{i}^{o}$ and ${\mathbf{p}}_{i}^{t}$, respectively. And superscript ${\text{T}}$ is transpose of matrix, $\det$ represents determinant computation of matrix, ${\mathbf{I}}_{3}$ is a 3 × 3 identity matrix. $\lambda$ denotes the scale factor, ${\mathbf{t}} = \left[ {\begin{array}{*{20}c} {t_{x} } & {t_{y} } & {t_{z} } \\ \end{array} } \right]^{\text{T}}$ is the vector of three translation parameters and ${\mathbf{R}}$ represents the rotation matrix of size 3 × 3, which is traditionally expressed by three rotation angles. Assume ${\mathbf{R}}$ is produced by counterclockwise coordinate-frame rotations $\theta_{x}$, $\theta_{y}$, $\theta_{z}$ ($\theta_{x}$ applied first, $\theta_{z}$ applied last). Then ${\mathbf{R}}$ can be represented by rotation angles as

$${\mathbf{R}} = \left[ {\begin{array}{*{20}c} {\cos \theta_{z} \cos \theta_{y} } & {\sin \theta_{z} \cos \theta_{x} + \cos \theta_{z} \sin \theta_{y} \sin \theta_{x} } & {\sin \theta_{z} \sin \theta_{x} - \cos \theta_{z} \sin \theta_{y} \cos \theta_{x} } \\ { - \sin \theta_{z} \cos \theta_{y} } & {\cos \theta_{z} \cos \theta_{x} - \sin \theta_{z} \sin \theta_{y} \sin \theta_{x} } & {\cos \theta_{z} \sin \theta_{x} + \sin \theta_{z} \sin \theta_{y} \cos \theta_{x} } \\ {\sin \theta_{y} } & { - \cos \theta_{y} \sin \theta_{x} } & {\cos \theta_{y} \cos \theta_{x} } \\ \end{array} } \right].$$

(3)

If R is known, and the 3 main diagonal elements are positive, $\theta_{x}$, $\theta_{y}$, $\theta_{z}$ can be calculated quickly by (3) as

$$\theta_{x} = - \tan^{ - 1} \frac{{{\mathbf{R}}_{32} }}{{{\mathbf{R}}_{33} }},\;\theta_{y} = \sin^{ - 1} ({\mathbf{R}}_{31} ),\;\theta_{z} = - \tan^{ - 1} \frac{{{\mathbf{R}}_{21} }}{{{\mathbf{R}}_{11} }},$$

(4)

where ${\mathbf{R}}_{ij}$ is the element of ${\mathbf{R}}$ in the row i and column j.

For $n$ control points, the 3D similarity transformation in EIV model is constructed easily from Eq. (1).

$${\mathbf{p}}_{{}}^{t} \varvec{ - }{\mathbf{e}}_{{}}^{t} = {\mathbf{{\rm B}p}}_{{}}^{o} - {\mathbf{Be}}_{{}}^{o} + {\mathbf{Ct}},$$

(5)

where ${\mathbf{p}}^{o} = \left[ {\begin{array}{*{20}c} {{\mathbf{p}}_{1}^{{o{\text{T}}}} } & {{\mathbf{p}}_{2}^{{o{\text{T}}}} } & \ldots & {{\mathbf{p}}_{n}^{{o{\text{T}}}} } \\ \end{array} } \right]^{\text{T}}$, ${\mathbf{p}}_{{}}^{t} = \left[ {\begin{array}{*{20}c} {{\mathbf{p}}_{1}^{{t{\text{T}}}} } & {{\mathbf{p}}_{2}^{{t{\text{T}}}} } & \ldots & {{\mathbf{p}}_{n}^{{t{\text{T}}}} } \\ \end{array} } \right]^{\text{T}}$, ${\mathbf{B}} = {\mathbf{I}}_{n} \otimes \lambda {\mathbf{R}}$, ${\mathbf{C}} = {\mathbf{1}}_{n} \otimes {\mathbf{I}}_{3}$, ${\mathbf{e}}_{{}}^{o} = \left[ {\begin{array}{*{20}c} {{\mathbf{e}}_{1}^{{o{\text{T}}}} } & {{\mathbf{e}}_{2}^{{o{\text{T}}}} } & \cdots & {{\mathbf{e}}_{n}^{{o{\text{T}}}} } \\ \end{array} } \right]^{\text{T}}$, ${\mathbf{e}}_{{}}^{t} = \left[ {\begin{array}{*{20}c} {{\mathbf{e}}_{1}^{{t{\text{T}}}} } & {{\mathbf{e}}_{2}^{{t{\text{T}}}} } & \ldots & {{\mathbf{e}}_{n}^{{t{\text{T}}}} } \\ \end{array} } \right]^{\text{T}}$, and ${\mathbf{1}}_{n} = \underbrace {{\left[ {\begin{array}{*{20}c} 1& 1& \ldots & 1\\ \end{array} } \right]}^{\text{T}}}_{n}$. ${\mathbf{I}}_{n}$ is the identity matrix of size $n \times n$, the symbol $\otimes$ means the Kronecker product.

The objective of 3D similarity transformation in EIV model is to recover the seven transformation parameters in the principle of total least squares, i.e.,

$${\mathbf{e}}_{{}}^{{t{\text{T}}}} {\mathbf{We}}_{{}}^{t} + {\mathbf{e}}_{{}}^{{o{\text{T}}}} {\mathbf{We}}_{{}}^{o} = \hbox{min} ,$$

(6)

where ${\mathbf{W}}$ of size $3n \times 3n$ is a weight matrix of observations, adopting the point (i.e., row or column) weight in Felus and Burtch (2009) as follows, since the point weight is more reasonable than the identity matrix weight usually employed in practice.

$${\mathbf{W}} = \left[ {\begin{array}{*{20}c} {w_{1} {\mathbf{I}}_{3} } & {} & {} & {} \\ {} & {w_{2} {\mathbf{I}}_{3} } & {} & {} \\ {} & {} & \ddots & {} \\ {} & {} & {} & {w_{n} {\mathbf{I}}_{3} } \\ \end{array} } \right],$$

(7)

where $w_{i}$ is the weight of point $i$ ($i = 1,2, \ldots ,n$). Note that the components of ${\mathbf{R}}$ in Eq. (3) are trigonometric functions of rotation angles, which causes a high computation burden in the solution of parameters. In some particular situations, for instance in geodesy, the small rotation angles are small usually at the level of seconds; the elements of rotation matrix can be reduced to rotation angles (in radians) or constants (one or zero) and the 3D similarity transformation model is simplified to a linear one. Evidently this reduced treatment causes model error and then is not applicable to big rotation angle cases. In this contribution, the nonlinear 3D similarity transformation model is adopted to maintain validity for any rotation angles.

Apart from using rotations, there are other representation of ${\mathbf{R}}$, such as unit quaternion (e.g., Horn 1987; Shen et al. 2006; Zeng and Yi 2011; Závoti and Kalmár 2016), dual quaternion (e.g., Walker et al. 1991; Wang et al. 2014; Zeng et al. 2018, 2019), direction cosine matrix (e.g., Chen et al. 2004; Wang et al. 2018), and the Rodrigues matrix or Gibbs vector (e.g., Zeng and Yi 2010; Závoti and Kalmár 2016; Zeng et al. 2016; Kurt 2018). The first three approaches to the representation of ${\mathbf{R}}$ introduce functional constraints, for instance the norm of quaternion is unity as the unit quaternion representation is concerned. Additional functional constraints increase the complexity of formulation and computation. However, the 4th approach is free of transcendental function and functional constraints, and then improves the speed of computation. For this sake, this paper adopts the Rodrigues matrix or Gibbs vector to represent the rotation matrix ${\mathbf{R}}$. Due to the orthogonality of rotation matrix, it can be represented by a Rodrigues matrix as

$${\mathbf{R}} = ({\mathbf{I}}_{3} + {\mathbf{S}})({\mathbf{I}}_{3} - {\mathbf{S}})^{ - 1} ,$$

(8)

where

$${\mathbf{S}} = \left[ {\begin{array}{*{20}c} 0 & { - c} & b \\ c & 0 & { - a} \\ { - b} & a & 0 \\ \end{array} } \right]$$

(9)

is a skew-symmetric matrix, and ${\mathbf{v}} = \left( {\begin{array}{*{20}c} a & b & c \\ \end{array} } \right)^{\text{T}}$ is a Gibbs vector. If Eq. (8) is expanded, it is rewritten as

$${\mathbf{R}} = \frac{1}{{1{ + }a^{2} + b^{2} + c^{2} }}\left[ {\begin{array}{*{20}c} {1 + a^{2} - b^{2} - c^{2} } & {2(ab - c)} & {2(ac + b)} \\ {2(ab + c)} & {1 - a^{2} + b^{2} - c^{2} } & {2(bc - a)} \\ {2(ac - b)} & {2(bc + a)} & {1 - a^{2} - b^{2} + c^{2} } \\ \end{array} } \right].$$

(10)

It is easy to see that the rotation matrix ${\mathbf{R}}$ is expressed in terms of polynomials without trigonometric functions and functional constraints.

Deduction of translation parameters and simplified 3D similarity transformation model

The similarity transformation parameters in preceding EIV model based on the Rodrigues matrix or Gibbs vector theoretically can be recovered by the TLS derivation; however, it is found in computation practice that the normal equation is singular because the numerical magnitude of translation parameter ${\mathbf{t}}$ and those of the Gibbs vector ${\mathbf{v}}$ as well as scale factor $\lambda$ probably differ greatly from each other, which prevents a solution to the equation. For this reason, we seek to eliminate the translation parameters ${\mathbf{t}}$ from the original model.

Lagrangian extremum principle is a reliable way to solve the 3D similarity transformation problem in the sense of TLS. In order to obtain the explicit solution of ${\mathbf{t}}$ regarding $\lambda$ and ${\mathbf{R}}$ as known parameters, the Lagrangian extremum problem which just considers ${\mathbf{t}}$ as unknown is established as

$$\mathop {\hbox{min} }\limits_{{{\mathbf{e}}_{{}}^{o} \varvec{,}{\mathbf{e}}_{{}}^{t} ,{\mathbf{t}},{\mathbf{k}}}} \left\{ {L({\mathbf{e}}_{{}}^{o} \varvec{,}{\mathbf{e}}_{{}}^{t} ,{\mathbf{t}},{\mathbf{k}}) = {\mathbf{e}}_{{}}^{{o{\text{T}}}} {\mathbf{We}}_{{}}^{o} + {\mathbf{e}}_{{}}^{{t{\text{T}}}} {\mathbf{We}}_{{}}^{t} + 2{\mathbf{k}}^{\text{T}} ({\mathbf{p}}_{{}}^{t} \varvec{ - }{\mathbf{e}}_{{}}^{t} - {\mathbf{Bp}}_{{}}^{o} + {\mathbf{Be}}_{{}}^{o} - {\mathbf{Ct}})} \right\},$$

(11)

where ${\mathbf{k}}$ is a vector of the Lagrangian multiplier. In order to obtain the solution, partial derivations with respect to all variables should be set to zero. From the partial derivations with respect to ${\mathbf{e}}_{{}}^{o}$, one gets

$${\mathbf{e}}_{{}}^{o} = - {\mathbf{W}}^{ - 1} {\mathbf{B}}^{\text{T}} {\mathbf{k}}.$$

(12)

From the partial derivations with respect to ${\mathbf{e}}_{{}}^{t}$, one gets

$${\mathbf{e}}_{{}}^{t} = {\mathbf{W}}^{ - 1} {\mathbf{k}}.$$

(13)

Inserting Eqs. (12) and (13) into the partial derivations with respect to ${\mathbf{k}}$ which is identical to Eq. (5) and arranging the terms, one obtains

$${\mathbf{k}} = \left( {{\mathbf{W}}^{ - 1} + {\mathbf{BW}}^{ - 1} {\mathbf{B}}^{\text{T}} } \right)^{ - 1} \left( {{\mathbf{p}}_{{}}^{t} - {\mathbf{Bp}}_{{}}^{o} - {\mathbf{Ct}}} \right).$$

(14)

Denoting

$${\mathbf{W}}_{1} = \left( {{\mathbf{W}}^{ - 1} + {\mathbf{BW}}^{ - 1} {\mathbf{B}}^{\text{T}} } \right)^{ - 1}$$

(15)

substituting ${\mathbf{B}} = {\mathbf{I}}_{n} \otimes \lambda {\mathbf{R}}$ into Eq. (15), the following formula can be easily derived:

$${\mathbf{W}}_{1} = \left( {1 + \lambda^{2} } \right)^{ - 1} {\mathbf{W}}.$$

(16)

Combining ${\mathbf{C}}^{\text{T}} {\mathbf{k}} = {\mathbf{C}}^{\text{T}} {\mathbf{W}}_{1} \left( {{\mathbf{p}}_{{}}^{t} - {\mathbf{Bp}}_{{}}^{o} - {\mathbf{Ct}}} \right)$ from Eq. (14) with ${\mathbf{k}}^{\text{T}} {\mathbf{C}} = 0$ from the partial derivative of t, one obtains the solution of ${\mathbf{t}}$ as

$${\mathbf{t}} = \left( {{\mathbf{C}}^{\text{T}} {\mathbf{W}}_{1} {\mathbf{C}}} \right)^{ - 1} {\mathbf{C}}^{\text{T}} {\mathbf{W}}_{1} \left( {{\mathbf{p}}_{{}}^{t} - {\mathbf{Bp}}_{{}}^{o} } \right).$$

(17)

Inserting Eq. (16) into Eq. (17), one obtains the following formula and its proof is given in Additional file 1: Appendix S1:

$${\mathbf{t}} = \overline{{{\mathbf{p}}_{{}}^{t} }} - \lambda {\mathbf{R}}\overline{{{\mathbf{p}}_{{}}^{o} }} ,$$

(18)

where

$$\overline{{{\mathbf{p}}_{{}}^{t} }} = \frac{{\sum\nolimits_{i = 1}^{n} {w_{i} } {\mathbf{p}}_{i}^{t} }}{{\sum\nolimits_{i = 1}^{n} {w_{i} } }}$$

(19)

and

$$\overline{{{\mathbf{p}}_{{}}^{o} }} = \frac{{\sum\nolimits_{i = 1}^{n} {{\text{w}}_{i} } {\mathbf{p}}_{i}^{o} }}{{\sum\nolimits_{i = 1}^{n} {{\text{w}}_{i} } }}$$

(20)

are the weighted coordinates of the barycenter in the target coordinate system and the source coordinate system, respectively.

Inserting Eq. (17) into Eq. (5) and carrying out arrangements, one gets

$$\left( {{\mathbf{I}}_{3n} - {\mathbf{C}}\left( {{\mathbf{C}}^{\text{T}} {\mathbf{W}}_{1} {\mathbf{C}}} \right)^{ - 1} {\mathbf{C}}^{\text{T}} {\mathbf{W}}_{1} } \right){\mathbf{p}}_{{}}^{t} { - }{\mathbf{e}}_{{}}^{t} = \left( {{\mathbf{I}}_{3n} - {\mathbf{C}}\left( {{\mathbf{C}}^{\text{T}} {\mathbf{W}}_{1} {\mathbf{C}}} \right)^{ - 1} {\mathbf{C}}^{\text{T}} {\mathbf{W}}_{1} } \right){\mathbf{Bp}}_{{}}^{o} - {\mathbf{Be}}_{{}}^{o} .$$

(21)

Denoting

$${\mathbf{D}} = {\mathbf{I}}_{3n} - {\mathbf{C}}\left( {{\mathbf{C}}^{\text{T}} {\mathbf{W}}_{1} {\mathbf{C}}} \right)^{ - 1} {\mathbf{C}}^{\text{T}} {\mathbf{W}}_{1} ,$$

(22)

thus Eq. (21) is rewritten as

$${\mathbf{Dp}}_{{}}^{t} { - }{\mathbf{e}}_{{}}^{t} = {\mathbf{DBp}}_{{}}^{o} - {\mathbf{Be}}_{{}}^{o} .$$

(23)

It is easily proved that

$${\mathbf{DB}} = {\mathbf{BD}},$$

(24)

and the proof is given in Additional file 2: Appendix S2. Then Eq. (23) is re-expressed as

$$\Delta {\mathbf{p}}_{{}}^{t} { - }{\mathbf{e}}_{{}}^{t} = {\mathbf{B}}\Delta {\mathbf{p}}_{{}}^{o} - {\mathbf{Be}}_{{}}^{o} ,$$

(25)

where

$$\Delta {\mathbf{p}}_{{}}^{t} = {\mathbf{Dp}}_{{}}^{t}$$

(26)

and

$$\Delta {\mathbf{p}}_{{}}^{o} = {\mathbf{Dp}}_{{}}^{o}$$

(27)

are centralized coordinate vectors, i.e., the difference of original coordinates and the weighted coordinates of the barycenter, in the target coordinate system and source coordinate system, respectively. ${\mathbf{D}}$ is a centering matrix. It is obvious to see that the translation parameter ${\mathbf{t}}$ is eliminated.

WTLS iterative algorithm of 3D similarity transformation in errors-in-variables model

Linearizing Eq. (25) by Taylor’s formula, one obtains

$$\begin{aligned} \Delta {\mathbf{p}}_{{}}^{t} - {\mathbf{e}}_{{}}^{t} = {\mathbf{B}}_{{}}^{j} \Delta {\mathbf{p}}_{{}}^{o} - {\mathbf{B}}_{{}}^{j} {\mathbf{e}}_{{}}^{o,j} + \frac{{\partial {\mathbf{B}}}}{\partial \lambda }\left( {\Delta {\mathbf{p}}_{{}}^{o} - {\mathbf{e}}_{{}}^{o,j} } \right){\text{d}}\lambda + \frac{{\partial {\mathbf{B}}}}{\partial a}\left( {\Delta {\mathbf{p}}_{{}}^{o} - {\mathbf{e}}_{{}}^{o,j} } \right){\text{d}}a \\ + \frac{{\partial {\mathbf{B}}}}{\partial b}\left( {\Delta {\mathbf{p}}_{{}}^{o} - {\mathbf{e}}_{{}}^{o,j} } \right){\text{d}}b + \frac{{\partial {\mathbf{B}}}}{\partial c}\left( {\Delta {\mathbf{p}}_{{}}^{o} - {\mathbf{e}}_{{}}^{o,j} } \right){\text{d}}c - {\mathbf{B}}_{{}}^{j} {\text{d}}{\mathbf{e}}_{{}}^{o,j} , \\ \end{aligned}$$

(28)

where superscript $j$ denotes the approximation from iteration $j$ of the corresponding variable, ${\text{d}}{\mathbf{x}} = \left[ {\begin{array}{*{20}c} {{\text{d}}\lambda } & {{\text{d}}a} & {{\text{d}}b} & {{\text{d}}c} \\ \end{array} } \right]^{\text{T}}$ is the correction vector of unknowns ${\mathbf{x}} = \left[ {\begin{array}{*{20}c} \lambda & a & b & c \\ \end{array} } \right]^{\text{T}}$, and

$$\frac{{\partial {\mathbf{B}}}}{\partial \lambda } = {\mathbf{I}}_{n} \otimes {\mathbf{R}},$$

(29)

$$\frac{{\partial {\mathbf{B}}}}{\partial a} = {\mathbf{I}}_{n} \otimes \lambda \frac{{\partial {\mathbf{R}}}}{\partial a},$$

(30)

$$\frac{{\partial {\mathbf{B}}}}{\partial b} = {\mathbf{I}}_{n} \otimes \lambda \frac{{\partial {\mathbf{R}}}}{\partial b},$$

(31)

$$\frac{{\partial {\mathbf{B}}}}{\partial c} = {\mathbf{I}}_{n} \otimes \lambda \frac{{\partial {\mathbf{R}}}}{\partial c},$$

(32)

$$\begin{array}{ll} \frac{{\partial {\mathbf{R}}}}{{\partial v_{x} }} &= \frac{{\partial {\mathbf{S}}}}{{\partial v_{x} }}({\mathbf{I}}_{3} - {\mathbf{S}})^{ - 1} + ({\mathbf{I}}_{3} + {\mathbf{S}})({\mathbf{I}}_{3} - {\mathbf{S}})^{ - 1} \frac{{\partial {\mathbf{S}}}}{{\partial v_{x} }}({\mathbf{I}}_{3} - {\mathbf{S}})^{ - 1} \\ &= \frac{{\partial {\mathbf{S}}}}{{\partial v_{x} }}({\mathbf{I}}_{3} - {\mathbf{S}})^{ - 1} + {\mathbf{R}}\frac{{\partial {\mathbf{S}}}}{{\partial v_{x} }}({\mathbf{I}}_{3} - {\mathbf{S}})^{ - 1} \\ &= ({\mathbf{I}}_{3} + {\mathbf{R}})\frac{{\partial {\mathbf{S}}}}{{\partial v_{x} }}({\mathbf{I}}_{3} - {\mathbf{S}})^{ - 1} ,\quad v_{x} = a,b,c, \\ \end{array}$$

(33)

$$\frac{{\partial {\mathbf{S}}}}{\partial a} = \left[ {\begin{array}{*{20}c} 0 & 0 & 0 \\ 0 & 0 & { - 1} \\ 0 & 1 & 0 \\ \end{array} } \right],$$

(34)

$$\frac{{\partial {\mathbf{S}}}}{\partial b} = \left[ {\begin{array}{*{20}c} 0 & 0 & 1 \\ 0 & 0 & 0 \\ { - 1} & 0 & 0 \\ \end{array} } \right],$$

(35)

$$\frac{{\partial {\mathbf{S}}}}{\partial c} = \left[ {\begin{array}{*{20}c} 0 & { - 1} & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 0 \\ \end{array} } \right].$$

(36)

Denoting

$${\mathbf{A}} = \left[ {\begin{array}{*{20}c} {\frac{{\partial {\mathbf{B}}}}{\partial \lambda }\left( {\Delta {\mathbf{p}}_{{}}^{o} - {\mathbf{e}}_{{}}^{o,j} } \right)} & {\frac{{\partial {\mathbf{B}}}}{\partial a}\left( {\Delta {\mathbf{p}}_{{}}^{o} - {\mathbf{e}}_{{}}^{o,j} } \right)} & {\frac{{\partial {\mathbf{B}}}}{\partial b}\left( {\Delta {\mathbf{p}}_{{}}^{o} - {\mathbf{e}}_{{}}^{o,j} } \right)} & {\frac{{\partial {\mathbf{B}}}}{\partial c}\left( {\Delta {\mathbf{p}}_{{}}^{o} - {\mathbf{e}}_{{}}^{o,j} } \right)} \\ \end{array} } \right]$$

(37)

and considering

$${\mathbf{e}}_{{}}^{o} = {\mathbf{e}}_{{}}^{o,j} + d{\mathbf{e}}_{{}}^{o,j} ,$$

(38)

Eq. (28) is rewritten as

$$\Delta {\mathbf{p}}_{{}}^{t} - {\mathbf{e}}_{{}}^{t} = {\mathbf{B}}_{{}}^{j} \Delta {\mathbf{p}}_{{}}^{o} - {\mathbf{B}}_{{}}^{j} {\mathbf{e}}_{{}}^{o} + {\mathbf{A}}_{{}}^{j} {\text{d}}{\mathbf{x}}.$$

(39)

The WTLS solution process of ${\text{d}}{\mathbf{x}}$ in Eq. (39) is similar to that of ${\mathbf{t}}$ in Eq. (5) by the Lagrangian extremum principle shown in the preceding subsection. Replacing ${\mathbf{p}}_{{}}^{t}$, ${\mathbf{p}}_{{}}^{o}$, ${\mathbf{C}}$, ${\mathbf{t}}$ with $\Delta {\mathbf{p}}_{{}}^{t}$, $\Delta {\mathbf{p}}_{{}}^{o}$, ${\mathbf{A}}_{{}}^{j}$, ${\text{d}}{\mathbf{x}}$ in the solution process of ${\mathbf{t}}$, respectively, one can easily get the solution as

$${\text{d}}{\mathbf{x}} = \left( {{\mathbf{A}}_{{}}^{{j{\text{T}}}} {\mathbf{W}}_{1} {\mathbf{A}}_{{}}^{j} } \right)^{ - 1} {\mathbf{A}}_{{}}^{{j{\text{T}}}} {\mathbf{W}}_{1} \left( {\Delta {\mathbf{p}}_{{}}^{t} - {\mathbf{B}}_{{}}^{j} \Delta {\mathbf{p}}_{{}}^{o} } \right),$$

(40)

$${\mathbf{e}}_{{}}^{o} = - {\mathbf{W}}_{{}}^{ - 1} {\mathbf{B}}^{\text{T}} {\mathbf{k}}',$$

(41)

$${\mathbf{e}}_{{}}^{t} = {\mathbf{W}}_{{}}^{ - 1} {\mathbf{k}}',$$

(42)

where

$${\mathbf{k}}' = {\mathbf{W}}_{1} \left( {\Delta {\mathbf{p}}_{{}}^{t} - {\mathbf{B}}_{{}}^{j} \Delta {\mathbf{p}}_{{}}^{o} - {\mathbf{A}}_{{}}^{j} {\text{d}}{\mathbf{x}}} \right).$$

(43)

Next, we can utilize an iterative algorithm to compute the final solution of ${\mathbf{x}}$. Set the initial value ${\mathbf{x}}^{0} = \left[ {\begin{array}{*{20}c} 1& 0& 0& 0\\ \end{array} } \right]^{\text{T}}$, the initial value of the unknown ${\mathbf{e}}_{{}}^{o}$ as $\underbrace {{\left[ {\begin{array}{*{20}c} 0& 0& \cdots & 0\\ \end{array} } \right]}^{\text{T}}}_{ 3n}$ and compute the ${\text{d}}{\mathbf{x}}$, if the absolute values of all elements of ${\text{d}}{\mathbf{x}}$ are less than a small tolerance value $\tau$ (e.g., 1.0 × 10⁻¹⁰), stop the iteration, otherwise update the ${\mathbf{x}}$ with previous iterative value of ${\mathbf{x}}$ plus ${\text{d}}{\mathbf{x}}$ and repeat the iterative computation until the stop criterion is satisfied. After iteration, the variance factor of unit weight is estimated as

$$\sigma = \pm \sqrt {\frac{{{\mathbf{e}}_{{}}^{{o{\text{T}}}} {\mathbf{We}}_{{}}^{o} + {\mathbf{e}}_{{}}^{{t{\text{T}}}} {\mathbf{We}}_{{}}^{t} }}{3n - 7}} ,$$

(44)

or if ${\mathbf{A}}_{{}}^{j} {\text{d}}{\mathbf{x}}$ is a sufficiently small term, the approximate formula as follows is adopted:

$$\sigma \approx \pm \sqrt {\frac{{\left( {\Delta {\mathbf{p}}_{{}}^{t} - {\mathbf{B}}_{{}}^{j} \Delta {\mathbf{p}}_{{}}^{o} } \right)^{\text{T}} {\mathbf{W}}_{1} \left( {\Delta {\mathbf{p}}_{{}}^{t} - {\mathbf{B}}_{{}}^{j} \Delta {\mathbf{p}}_{{}}^{o} } \right)}}{3n - 7}} .$$

(45)

By Eq. (40) the estimated covariance matrix of transformation parameters ${\mathbf{x}}$ is derived as

$${\mathbf{D}}_{x} = \sigma^{2} \left( {{\mathbf{A}}_{{}}^{{j{\text{T}}}} {\mathbf{W}}_{1} {\mathbf{A}}_{{}}^{j} } \right)^{ - 1} .$$

(46)

Similarly by Eq. (17) the estimated covariance matrix of transformation parameters ${\mathbf{t}}$ is derived as

$${\mathbf{D}}_{{\mathbf{t}}} = \sigma^{2} \left( {{\mathbf{C}}^{\text{T}} {\mathbf{W}}_{1} {\mathbf{C}}} \right)^{ - 1} .$$

(47)

To sum up, the above presented WTLS iterative algorithm is listed in Table 1.

Table 1 WTLS iterative algorithm of 3D similarity transformation in EIV model

Numerical analysis and discussion

Two cases are studied. One is the registration of LIDAR points involving big rotation angles and the weight matrix is not considered, i.e., an identity matrix is adopted. The other one is 3D coordinate transformation in geodesy, which encounters small rotation angles and point-weighting is adopted.

Case 1 (registration of LIDAR points)

This data are adopted from Wang et al. (2014). The point features extracted from two neighboring LIDAR point clouds are listed in Table 2. In order to verify the new algorithm, the 18 points are divided into control points and check points. The first 10 points in Table 2 are arbitrarily chosen as control points and the last 8 points are check points. The distribution of all points is depicted in Fig. 1. In Fig. 1 plus signs denote control points and cross signs denote check points. The weight matrix adopted is an identity matrix in this case; in other words, the weights of all points are the same, and the correlation among the three coordinates of each point is not considered.

Table 2 Point features extracted from two neighboring LIDAR point clouds

Fig. 1

3D space distribution of point features in the source coordinate system. Plus signs denote control points and cross signs denote check points

The transformation parameters in the WTLS sense are computed by the new algorithm and Algorithm 4 presented in Felus and Burtch (2009), henceforth described as the Felus–Burtch Algorithm. All results are listed in Table 3. It is seen from Table 3, that the new algorithm needs only 6 iterations to converge. And for two algorithms, the results are exactly the same. Thus the new algorithm is fast and reliable. Additionally, the Felus–Burtch Algorithm does not provide the accuracies of recovered transformation parameters; however, the new algorithm does. It gives the accuracy information which is shown in Table 3. For more details, refer to the covariance matrix of recovered transformation parameters offered by the new algorithm, which is listed in Table 4. The predicted errors of coordinates of control points by the new algorithm are listed in Table 5. It is worth mentioning that the formula to compute $\widetilde{{\mathbf{E}}}_{Y}$ in Algorithm 4 presented from Felus and Burtch (2009) is wrong. The first term namely the inverse of weight matrix should be removed from the formula. If the LS transformation parameters and predicted error of coordinates of control points are computed with the algorithm in Wang et al. (2014), one finds that LS variance factor of unit weight is 0.0234 m, which is bigger than that of WTLS. Therefore, the WTLS solution is better than the LS solution from the viewpoint of variance factor of unit weight.

Table 3 Recovered parameters for Case 1 using the new algorithm and the Felus–Burtch algorithm, respectively

Table 4 Covariance matrixes of recovered transformation parameters obtained by the new algorithm

Table 5 Predicted errors at 10 control points in the source system and target system by the new algorithm

Next, the new algorithm in this contribution and the Felus–Burtch Algorithm are used to calculate the target coordinate of check points once the transformation parameters are recovered by the 3D similarity transformation model, i.e., Eq. (1). The errors of computed coordinates of check points are obtained by subtracting the computed coordinates of check points from the known coordinates of check points. The errors are listed in Table 6.

Table 6 Errors of computed target coordinates of check points by the two algorithms

Lastly, in order to test the dependence of the new algorithm on the initial values of parameters, six sets of initial values of parameters are adopted which are listed in Table 7. From Table 7, it is seen that the maximum biased angle from the correct solution is 2.5°, 18.9°, 29.4°, 44.5°, 59.4°, 74.9° for Set 1, Set 2, Set 3, Set 4, Set 5, Set 6, respectively. Set 3 gives the default initial values of parameters. The corresponding initial values of the Gibbs vector elements are computed from the initial values of rotation angles in Table 7. Then the new algorithm is performed with six sets of initial values of parameters, respectively. Results show that the algorithm successively finds the correct solutions with six sets of initial values of parameters. Number of iterations is 5, 5, 6, 6, 8, and 8 for Set 1, Set 2, Set 3, Set 4, Set 5, Set 6, respectively. It is evidently seen that with the increase of maximum biased angle from Set 1 to Set 6, the number of iterations increases slowly from 5 to 8. Thus the new algorithm is not sensitive to the initial values of parameters. Further, the iterative process is drawn in Fig. 2. From this figure, it is seen that the convergence rate is fast since that the logarithm (base 10) of objective function, i.e., ${ \log }_{10} \left( {{\mathbf{e}}_{{}}^{{o{\text{T}}}} {\mathbf{We}}_{{}}^{o} + {\mathbf{e}}_{{}}^{{t{\text{T}}}} {\mathbf{We}}_{{}}^{t} } \right)$ linearly decreases for all six sets of initial values. So the new algorithm is very fast in terms of fewer iterations. It is worth noting that for this big rotation angle case, no good initial values of parameters are required while usually good initial values of parameters are needed, as shown in Zeng and Yi (2011).

Table 7 Six sets of initial values of transformation parameters

Fig. 2

Iterative process of the new algorithm with different initial values of parameters

Case 2 (geodetic datum transformation)

The data are from Grafarend and Awange (2003). The 3D coordinates of 7 points in the source coordinate system and the target coordinate system, respectively, are given in Table 8. The 3D space distribution of 7 points in the source coordinate system is shown in Fig. 3. In order to validate the new algorithm, the 7 points are divided into control points and check points. Four points are selected as control points from Fig. 3 based on the consideration that usually, in order to obtain good transformation parameters, control points should be distributed evenly and enclose as much of the surveying area as possible. The blue asterisk denotes the control points, and the red circle denotes the check points in Fig. 3. The selected four control points are shown with bold font in Table 8. The weight matrix adopted is a pointwise one, i.e., for each point, it has isotropic weight and its different coordinate components are not correlated, and for all points, they are independent of each other. The pointwise weights are listed in Table 9.

Table 8 Coordinates of 7 points in the source system and target system

Fig. 3

3D space distribution of 7 points in the source coordinate system. The blue asterisk denotes the control points, and the red circle denotes the check points

Table 9 Pointwise weighting

The recovered transformation parameters in the WTLS sense by the new algorithm and the Felus–Burtch Algorithm are listed in Table 10. It is seen from Table 10 that the new algorithm needs only 2 iterations to complete computation. The results are identical for the two algorithms if the decimal rounding is ignored. Thus the new algorithm is fast and reliable for this case. As mentioned in the preceding case, the new algorithm can provide the accuracies of recovered transformation parameters while the Felus–Burtch Algorithm cannot. The accuracy information is shown in Table 10. For more details, refer to the covariance matrix of recovered transformation parameters offered by the new algorithm, which are listed in Table 11. The predicted errors of coordinates of control points by the new algorithm are listed in Table 12. Table 12 shows as far as absolute values are concerned, the predicted errors of coordinates of the new algorithm control points in the source coordinate system are approximately identical to those in the target coordinate system. It is because the point weights are the same for the two systems, the scale factor is very close to 1 and the rotation angles are very small resulting in almost an identity rotation matrix.

Table 10 Recovered parameters for Case 2 using the new algorithm and the Felus–Burtch Algorithm, respectively

Table 11 Covariance matrix of transformation parameters obtained by the new algorithm

Table 12 Predicted error coordinates of control points in the source system and target system by the new algorithm

Next, the new algorithm and the Felus–Burtch Algorithm are employed to calculate the target coordinates of check points. Then errors of computed coordinates of the check points are computed. The errors are the same for the two algorithms and are listed in Table 13.

Table 13 Errors of computed target coordinates of check points by the two algorithms

Conclusions

The paper presents a WTLS iterative algorithm of the 3D similarity coordinate transformation based on the Gibbs vector considering that transformation model is compact and without redundant constraints of parameter, such as the norm of the unit quaternion is 1. The cases studies show that whether the rotation angles are big or small, the new algorithm is fast in terms of fewer iterations, reliable and does not require good initial value of unknowns or not sensitive to the initial value. The new algorithm can offer accuracy of the unknowns, i.e., transformation parameters while the analytical TIS algorithm, e.g., the Felus–Burtch Algorithm cannot. This is meaningful and indispensable from the viewpoint of the accuracy assessment. Additionally, it is shown that the TLS solution is advantageous over the LS solution from the viewpoint of variance factor of unit weight. In the near future, we intend to present new algorithms that deal with more general weighting of observations.