----------------------------------

The difficulty of solving (\[equ:14\]) lies in the denominator of CGR parameterization (\[equ:5\]). To solve this problem, we introduce the following intermediate variable $$\label{equ:16}
\rho  = \frac{1}{{\sqrt {1 + s_1^2 + s_2^2 + s_3^2} }}.$$ Substituting (\[equ:16\]) into (\[equ:9\]) will transform (\[equ:9\]) from a rational function to a polynomial function. Furthermore, we define $$\label{equ:17}
\alpha =\rho {{s}_{1}},\ \beta =\rho {{s}_{2}},\ \gamma =\rho {{s}_{3}}.$$ Using (\[equ:16\]) and (\[equ:17\]), the residual (\[equ:9\]) can be expressed as $$\label{equ:18}
{{r}_{g}}=\mathbf{a}_{g}^{T}\mathbf{u}+{{\mathbf{b}_{g}}^{T}}\mathbf{t}$$ 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 (\[equ:9\]) into a polynomial function (\[equ:18\]) that is easier to handle. Furthermore, $\mathbf{t}$ in (\[equ:18\]) is decoupled from $\mathbf{R}$, and (\[equ:18\]) is linear in $\mathbf{t}$.

Now we consider the least-squares problem (\[equ: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 $$\label{equ:19}
{{\bf{e}}_g} = {\bf{Au}} + {\bf{Bt}}$$ 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 (\[equ:4\]) can be written as $$\label{equ:20}
{C_{\pi lp}}\left(\rho,\alpha,\beta,\gamma,{\bf{t}}\right) = {\bf{e}}_g^T{{\bf{e}}_g}$$ As (\[equ:19\]) is linear for $\mathbf{t}$, then $\bf{t}$ 