Method of unit quaternions

Let us first review the solution given by the unit quaternions (Horn, 1987) applicable when we use the distance image ${\bf d}_{kl}^{(2)}$, ${\bf d}_{kl}^{(3)}$, ${\bf d}_{kl}^{(5)}$, ${\bf d}_{kl}^{(6)}$, or ${\bf d}_{kl}^{(7)}$ in the sequential registration. The solution is similar in all the cases so let ${\bf r}_k^\circ$ denote either ${\bf r}_k$, ${\bf\hat r}_k$, or ${\bf\hat r}_k^\dag$ and let ${\bf r}_l^\circ$ be ${\bf\tilde r}_l$ or ${\bf r}_l^\ast$ so that $[h(({\bf r}_{kl}^\circ -{\bf r}_l^\circ )^2)]^{1/2}$ is one of the distance images. The images ${\bf r}_k^\circ$, $k=1,\ldots ,l-1$, are first transformed into a set of coordinate images ${\bf r}_{k1}^\circ$, $k=1,\ldots ,l-1$ in the reference frame ( ${\bf r}_{11}^\circ\equiv{\bf r}_1^\circ$). Then the weighted centers ${\bf\bar r}_1=\sum _{k=1}^{l-1}{\sum _{{\bf s}\in\Omega _{kl}}{(w_{kl}({\bf s}))^2{\bf r}_{k1}^\circ ({\bf s})/K_l}}$ and ${\bf\bar r}_l=\sum _{k=1}^{l-1}{\sum _{{\bf s}\in\Omega _{kl}}{(w_{kl}({\bf s}))^2{\bf r}_l^\circ ({\bf s})/K_l}}$are computed and the coordinate images transformed to ${\bf r}_{k1}'={\bf r}_{k1}^\circ -{\bf\bar r}_1$, $k=1,\ldots ,l-1$, and ${\bf r}_l'={\bf r}_l^\circ -{\bf\bar r}_l$. The data sets ${\bf r}_{k1}'$ and ${\bf r}_l'$ are related only by rotation which is estimated by minimizing f₂ with the distance image ${\bf d}_{kl}^{(8)}=[h(({\bf r}_{k1}'-{\bf r}_l')^2)]^{1/2}$. A correlation matrix is defined by

$$Q=\sum _{k=1}^{l-1}{\sum _{{\bf s}\in\Omega _{kl}}{(w_{kl}({\bf s}))^2{\bf r}_l'({\bf s})({\bf r}_{k1}'({\bf s}))^T}}.$$ (7)

Using the elements of Q a new matrix is computed given by

$$\widehat Q=\begin{pmatrix}Q_{11}+Q_{22}+Q_{33} &Q_{32}-Q_{23}... ..._{31}+Q_{13} &Q_{23}+Q_{32} &-Q_{11}-Q_{22}+Q_{33}\end{pmatrix}$$ (8)

with the eigenvalues $\kappa _1\geq\ldots\geq\kappa _4$ and unit eigenvectors ${\bf q}_1,\ldots ,{\bf q}_4$, respectively. The eigenvector ${\bf q}_1$ for the largest eigenvalue $\kappa _1$ gives the unit quaternion that minimizes f₂. The corresponding rotation matrix R_1l can be computed directly from the elements of ${\bf q}_1$ as shown, e.g., in (Kanatani, 1994a). The representation by three angles ${\bf a'}_{1l}=(\theta ,\varphi ,\omega )^T$ can be further derived from the elements of R_1l. Using the estimated rotation, the translation is given by ${\bf t}_{1l}={\bf\bar r}_1-R_{1l}^{-1}{\bf\bar r}_l$.

We will utilize Lemma 2 to estimate $\hbox{Cov}({\bf q}_1)$ and we thus need to find out how the errors in the data, calibration parameters, and previously estimated registration parameters are propagated to $\widehat Q$ for the different distance images. According to (12) and (11), $\widehat Q$ depends on the elements of Q which, in turn, depend on ${\bf r}_{k1}'$ and ${\bf r}_l'$. Differentiation of each element of Q with respect to a measurement $j_k({\bf s})$ for some $k=1,\ldots ,l-1$ and ${\bf s}\in\Omega _{kl}$ yields
 $$\begin{align}\frac{\partial Q}{\partial j_k({\bf s})}=&(w_{kl}({\bf s}))^2\bigg ... ... ({\bf s})}{\partial j_k({\bf s})}({\bf r}_{k1}'({\bf s}))^T\bigg ]. \end{align}$$
Similar formulas hold for the other derivatives needed. It can be seen from (9) how ${\bf r}_{k1}^\circ$ depends on j_k, $\hat j_k$, ${\bf b}_k$, and ${\bf a}_{1k}$, and how ${\bf r}_l^\circ$ depends on j_k, $\hat j_k$, j_l, ${\bf b}_k$, ${\bf b}_l$, and ${\bf a}_{1k}$ for all the other distance images except ${\bf d}_{kl}^{(7)}$. For that one we notice that ${\bf\hat r}_{k1}^\dag$ depends only on the plane parameters given by the location vector ${\bf m}_1$ and normal ${\bf n}_1$, and on ${\bf r}_l^\ast (j_l,{\bf b}_l)$. By evaluating the derivatives of Q, we can apply Lemma 1 to ${\bf u}=\widehat Q{\bf q}_1$ where ${\bf q}_1$ is fixed, and obtain $\hbox{Cov}({\bf u})$. For ${\bf d}_{kl}^{(2)}$ and ${\bf d}_{kl}^{(3)}$, $\hbox{Cov}({\bf u})$ can be computed mostly by summing up the individual contributions, since the measurements $j_k({\bf s})$ are assumed independent and the cross covariance between the registration parameters ${\bf a}_{1k}$ and a single measurement $j_k({\bf s})$ is small when compared to $\hbox{Cov}({\bf a}_{1k})$ and can thus be neglected. The cross covariance $\hbox{E}(\delta{\bf a}_{1k}\delta{\bf b}^T)$ is, however, evaluated using Lemma 1. As we discussed in the previous section, the modeled values ${\hat j}_k({\bf s})$ are correlated so that for ${\bf d}_{kl}^{(5)}$, ${\bf d}_{kl}^{(6)}$, and ${\bf d}_{kl}^{(7)}$, we compute $\hbox{Cov}({\bf u})$ using (10) with $\Delta{\bf a}_{1l}$ replaced by ${\bf u}$. Formula (13) is utilized when the derivatives of ${\bf u}$ needed in (10) are evaluated.

Once $\hbox{Cov}({\bf u})$ has been estimated, Lemma 2 gives for $\hbox{Cov}({\bf q}_1)=E(\delta{\bf q}_1\delta{\bf q}_1^T)$

$$\hbox{Cov}({\bf q}_1)=\bigg [\sum _{i=2}^4{\frac{{\bf q}_i{\b... ...2}^4{\frac{{\bf q}_i{\bf q}_i^T}{\kappa _1-\kappa _i}}\bigg ].$$ (9)

We further evaluate the Jacobian $\frac{\partial{\bf a'}_{1l}}{\partial{\bf q}_1}$ and use Lemma 1 to obtain $\hbox{Cov}({\bf a'}_{1l})$. The covariances of the centers ${\bf\bar r}_1$ and ${\bf\bar r}_l$ are obtained using the above described dependencies of ${\bf r}_{k1}^\circ$ and ${\bf r}_l^\circ$ on the data and the calibration, registration, and plane parameters. For the translation, we then have $\hbox{Cov}({\bf t}_{1l})=\hbox{Cov}({\bf\bar r}_1)+R_{1l}^{-1}\hbox{Cov}({\bf\b... ...\hbox{Cov}({\bf a'}_{1l})(\frac{\partial{\bf t}_{1l}}{\partial{\bf a'}_{1l}})^T$. The cross covariance is given by $\hbox{E}(\delta{\bf t}_{1l}(\delta{\bf a'}_{1l})^T)=\frac{\partial{\bf t}_{1l}}{\partial{\bf a'}_{1l}}\hbox{Cov}({\bf a'}_{1l})$.