I've got $N$ position vectors $\mathbf{a}_i = \begin{pmatrix} a_{i,x} \\ a_{i,y} \\ a_{i,z} \end{pmatrix}$ in one coordinate system and $N$ corresponding position vectors $\mathbf{b}_i = \begin{pmatrix} b_{i,x} \\ b_{i,y} \\ b_{i,z} \end{pmatrix}$ in a rotated coordinate system. With $\mathbf{A} = \begin{pmatrix} \mathbf{a}_1 ... \mathbf{a}_M\end{pmatrix}$ and $\mathbf{B} = \begin{pmatrix} \mathbf{b}_1 ... \mathbf{b}_M\end{pmatrix}$ and $\mathbf{M}(\phi,\theta,\psi) = \mathbf{M}_{\mathrm{yaw}}(\phi)\mathbf{M}_{\mathrm{pitch}}(\theta)\mathbf{M}_{\mathrm{roll}}(\psi)$, $\mathbf{M} \in \mathbb{R}^{3 \times 3}$ corresponding to the euler rotation matrix yaw, pitch and roll, the following holds $\mathbf{A} \approx \mathbf{M}(\phi,\theta,\psi)\mathbf{B}$ which leads to the minimisation problem $$\min_{\phi,\theta,\psi} \|\mathbf{A}-\mathbf{M}(\phi,\theta,\psi)\mathbf{B}\|_2^2$$ using the Newton-method $\mathbf{F}(\phi,\theta,\psi) = \mathbf{A}-\mathbf{M}(\phi,\theta,\psi)\mathbf{B}$ with $\mathbf{F} : \mathbb{R}^3 \rightarrow \mathbb{R}^{3 \times N}$

The Newton-Method requires a Taylor series to the first order and therefore $\mathbf{F}'$. But how do you build a Jacobi-matrix of a $\mathbb{R}^{3 \times N}$-matrix?


It can be solved by rewriting the matrix to a vector: $$\mathbf{c} = \begin{pmatrix} a_{1,x} \\ a_{1,y} \\ a_{1,z} \\a_{2,x} \\ a_{2,y} \\ a_{2,z} \\ ..\end{pmatrix}, \mathbf{d}(\phi,\theta,\psi) = \begin{pmatrix} M_{1,x}b_{1,x}+M_{2,x}b_{1,y}+M_{3,x}b_{1,z} \\ M_{1,y}b_{1,x}+M_{2,y}b_{1,y}+M_{3,y}b_{1,z} \\ M_{1,z}b_{1,x}+M_{2,z}b_{1,y}+M_{3,z}b_{1,z} \\ M_{1,x}b_{2,x}+M_{2,x}b_{2,y}+M_{3,x}b_{2,z} \\ M_{1,y}b_{2,x}+M_{2,y}b_{2,y}+M_{3,y}b_{2,z} \\ M_{1,z}b_{2,x}+M_{2,z}b_{2,y}+M_{3,z}b_{2,z} \\ .. \end{pmatrix}$$ i.e. $\mathbf{A} \in \mathbb{R}^{3 \times 3} \rightarrow \mathbf{c} \in \mathbb{R}^9$ and $\mathbf{M}(\phi,\theta,\psi)\mathbf{B} \in \mathbb{R}^{3 \times 3} \rightarrow \mathbf{d}(\phi,\theta,\psi) \in \mathbb{R}^9$ $$\Rightarrow \min_{\phi,\theta,\psi} \|\mathbf{A}-\mathbf{M}(\phi,\theta,\psi)\mathbf{B}\|_2^2 \Leftrightarrow \min_{\phi,\theta,\psi} \|\mathbf{c}-\mathbf{d}(\phi,\theta,\psi)\|_2^2$$ The Taylor series to the first order of $\mathbf{d}(\phi,\theta,\psi)$ can be derived by $\mathbf{d}(\phi,\theta,\psi) = \mathbf{d}(\phi_k,\theta_k,\psi_k) + \underbrace{\mathbf{J}(\mathbf{d}(\phi_k,\theta_k,\psi_k))}_{=\mathbf{G}} \underbrace{\begin{pmatrix} \phi - \phi_k \\ \theta - \theta_k \\ \psi - \psi_k \end{pmatrix}}_{=\Delta\mathbf{\varphi}_k}$ where $\mathbf{J}(\mathbf{d})$ is the Jacobi-matrix of $\mathbf{d}$, $\mathbf{G} \in \mathbb{R}^{3N \times 3}$ and $\Delta\mathbf{\varphi}_k \in \mathbb{R}^3$

$\Delta\mathbf{\varphi}_k$ can then be calculated by $$\Delta\mathbf{\varphi}_k = (\mathbf{G}^T\mathbf{G})^{-1}\mathbf{G}^T(\mathbf{c}-\mathbf{d}(\phi_k,\theta_k,\psi_k))$$ the next iterative step is $$\mathbf{\varphi}_{k+1} = \mathbf{\varphi}_{k} + \Delta\mathbf{\varphi}_k$$ The stop criterion can be $\|\Delta\mathbf{\varphi}_k\| < \epsilon$ and start value could be $\mathbf{\varphi}_0 = \begin{pmatrix} 0\\0\\0 \end{pmatrix}$

This solves the above problem, but I'm not sure why the transformation can be done.


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.