2
$\begingroup$

Is there a closed-form solution of $\textbf{R}\textbf{R}_1=\textbf{R}_2\textbf{R}$ with respect to $\textbf{R}\in SO(3)$?

$\textbf{R}_1$, $\textbf{R}_2 \in SO(3)$ are given.

Added:

I tried holmeski's solution but it fails because of rank deficiency in A matrix.(why?) The following code simulates holmeski's solution in matlab. (please correct me if the code is incorrect)

clear all
cTl=rotx(rand*100)*roty(rand*100)*rotz(rand*100)
lTc = inv(cTl)

for k = 1: 9
    l1Tl2{k}=rotx(rand*100)*roty(rand*100)*rotz(rand*100)
    c1Tc2{k}= cTl*l1Tl2{k}*lTc;
end
R = calib_RR1_R2R_closedform(l1Tl2,c1Tc2)  


function R = calib_RR1_R2R_closedform(l1Tl2,c1Tc2_klt)    

Astack=[]
for k = 1:length(l1Tl2)
    R1 = l1Tl2{k}(1:3,1:3);
    R2 = c1Tc2_klt{k}(1:3,1:3);
    A = [ R1(1,1) + R1(1,2) + R1(1,3) - R2(1,1) - R2(1,2) - R2(1,3), R1(2,1) + R1(2,2) + R1(2,3) - R2(2,1) - R2(2,2) - R2(2,3), R1(3,1) + R1(3,2) + R1(3,3) - R2(3,1) - R2(3,2) - R2(3,3), R1(1,1) + R1(1,2) + R1(1,3) - R2(1,1) - R2(1,2) - R2(1,3), R1(2,1) + R1(2,2) + R1(2,3) - R2(2,1) - R2(2,2) - R2(2,3), R1(3,1) + R1(3,2) + R1(3,3) - R2(3,1) - R2(3,2) - R2(3,3), R1(1,1) + R1(1,2) + R1(1,3) - R2(1,1) - R2(1,2) - R2(1,3), R1(2,1) + R1(2,2) + R1(2,3) - R2(2,1) - R2(2,2) - R2(2,3), R1(3,1) + R1(3,2) + R1(3,3) - R2(3,1) - R2(3,2) - R2(3,3)]
    Astack = [Astack; A];
end

det(Astack)

[U S V] = svd(Astack);
x=V(:,end);
R=reshape(x,3,3);
$\endgroup$
  • $\begingroup$ A faster way of constructing A would to use the Kronecker product, namely A = kron(I, R2) - kron(R1', I); and after this you can also get the solutions by looking at the null space of A: null(A). But from my testing it seems that you will always get three solutions, so any linear combination of those is still a valid solution. So you would still need to find which linear combination yields a rotation matrix. $\endgroup$ – fibonatic Apr 5 '18 at 14:15
  • $\begingroup$ I also did some numerical testing and it seems that there is never an unique solution, there is always a circular line of solutions. $\endgroup$ – fibonatic Apr 5 '18 at 15:17
  • $\begingroup$ Thanks for the advice. That will be very useful for my later research! You are right about the non-unique solution. According to the paper in the link below in my answer, one pair of the equation is not enough to determine the unique solution. I will edit my answer soon. $\endgroup$ – C.O Park Apr 5 '18 at 22:36
  • $\begingroup$ Then at least this method does limit the search space from 9 dimensions to only 3. $\endgroup$ – fibonatic Apr 5 '18 at 23:00
2
$\begingroup$

I believe you can solve this using a least squares approach since all the math in equation is linear. Rearrange the equation so \begin{equation} \bf RR_1 -R_2R = 0 \end{equation}

Set up the relation \begin{equation} \bf Ax = b \end{equation}

where the vector $\bf x$ contains all values of $\bf R$, $\bf B$ is a $[9,1]$ zero vector, and $\bf A$ is a $[9,9]$ matrix which contains the $\bf R_1$ and $R_2$ terms.

Then solve for $\bf x$ \begin{equation} \bf x = A^{-1} b \end{equation}

You'll then have to restructure the $\bf x$ vector back into $\bf R$.

I know this may not be exactly what you're looking for but it is a closed form solution.

| improve this answer | |
$\endgroup$
  • $\begingroup$ Thanks for the answer! Auctaully, I already have a non-linear model for the optimization as $e = log(e^{[r]_x}\textbf{R}\textbf{R}_1)-log(\textbf{R}_2e^{[r]_x}\textbf{R})$ where $\textbf{r}\in so(3)$. It works well but just wondering if there is a closed form solution:) Thanks anyway! $\endgroup$ – C.O Park Apr 3 '18 at 22:36
  • 1
    $\begingroup$ Is the nonlinear approach an iterative solver? Is what I proposed not closed form? $\endgroup$ – holmeski Apr 3 '18 at 22:41
  • $\begingroup$ You are right! I just realized that your answer is what I was looking for. Perfect! Thanks! $\endgroup$ – C.O Park Apr 3 '18 at 23:03
  • $\begingroup$ But this would always give $R=0$, since $b$ is a vector of zeros. But clearly $0\notin SO(3)$. $\endgroup$ – fibonatic Apr 4 '18 at 0:40
  • 1
    $\begingroup$ You could check if the $A$ matrix has an eigenvalue of zero, in which case its corresponding eigenvector would be a solution. $\endgroup$ – fibonatic Apr 4 '18 at 0:49
2
$\begingroup$

This problem can be made easier when formulating an equivalent problem using unit quaternions

$$ q\,q_a = q_b\,q \tag{1} $$

where each quaternions can be expressed as $w + x\,i + y\,j + z\,k$ while satisfying that $w^2 + x^2 + y^2 + z^2 = 1$, where $i^2 = j^2 = k^2 = i\,j\,k = -1$ (it can be noted that $i$, $j$ and $k$ do not commute, for example $i\,j \neq j\,i$).

For quaternions multiplication it can be shown that it can be written as a matrix vector product, where the matrix is a function of one of the two quaternions. Namely when considering the quaternions $q = q_1 + q_2\,i + q_3\,j + q_4\,k$ and $r = r_1 + r_2\,i + r_3\,j + r_4\,k$, then their product can be written as

$$ \begin{align} q\,r &= \begin{bmatrix} 1 & i & j & k \end{bmatrix} \begin{bmatrix} q_1 & -q_2 & -q_3 & -q_4 \\ q_2 & q_1 & -q_4 & q_3 \\ q_3 & q_4 & q_1 & -q_2 \\ q_4 & -q_3 & q_2 & q_1 \end{bmatrix} \begin{bmatrix} r_1 \\ r_2 \\ r_3 \\ r_4 \end{bmatrix} \\ &= \begin{bmatrix} 1 & i & j & k \end{bmatrix} \begin{bmatrix} r_1 & -r_2 & -r_3 & -r_4 \\ r_2 & r_1 & r_4 & -r_3 \\ r_3 & -r_4 & r_1 & r_2 \\ r_4 & r_3 & -r_2 & r_1 \end{bmatrix} \begin{bmatrix} q_1 \\ q_2 \\ q_3 \\ q_4 \end{bmatrix} \end{align} $$

Dividing by a unit quaternions is the same as multiplication with its conjugate, which is defined as negating the complex parts: $w - x\,i - y\,j - z\,k$. This allows us to write equation $(1)$ as

$$ q\,q_a\,q^{-1} = q_b $$

which is equivalent to

$$ \begin{bmatrix} q_1 & -q_2 & -q_3 & -q_4 \\ q_2 & q_1 & -q_4 & q_3 \\ q_3 & q_4 & q_1 & -q_2 \\ q_4 & -q_3 & q_2 & q_1 \end{bmatrix} \begin{bmatrix} q_1 & q_2 & q_3 & q_4 \\ -q_2 & q_1 & -q_4 & q_3 \\ -q_3 & q_4 & q_1 & -q_2 \\ -q_4 & -q_3 & q_2 & q_1 \end{bmatrix} \begin{bmatrix} q_{a,1} \\ q_{a,2} \\ q_{a,3} \\ q_{a,4} \end{bmatrix} = \begin{bmatrix} q_{b,1} \\ q_{b,2} \\ q_{b,3} \\ q_{b,4} \end{bmatrix}. \tag{2} $$

It can be shown that, while using the fact that $q$ is a unit quaternion, the two matrices in the above equation can be combined into the following expression

$$ \begin{bmatrix} q_1 & -q_2 & -q_3 & -q_4 \\ q_2 & q_1 & -q_4 & q_3 \\ q_3 & q_4 & q_1 & -q_2 \\ q_4 & -q_3 & q_2 & q_1 \end{bmatrix} \begin{bmatrix} q_1 & q_2 & q_3 & q_4 \\ -q_2 & q_1 & -q_4 & q_3 \\ -q_3 & q_4 & q_1 & -q_2 \\ -q_4 & -q_3 & q_2 & q_1 \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1-2(q_3^2+q_4^2) & 2(q_2q_3-q_1q_4) & 2(q_2q_4+q_1q_3) \\ 0 & 2(q_2q_3+q_1q_4) & 1-2(q_2^2+q_4^2) & 2(q_3q_4-q_1q_2) \\ 0 & 2(q_2q_4-q_1q_3) & 2(q_3q_4+q_1q_2) & 1-2(q_2^2+q_3^2) \end{bmatrix} \tag{3} $$

the lower right $3\times3$ matrix of the the right hand side of $(3)$ is just the rotation matrix representation of $q$, denoted as $R(q)$. Substituting this result back into equation $(2)$ then we can conclude that there can only be a solution for $q$ given $q_a$ and $q_b$ if and only if $q_{a,1} = q_{b,1}$. This essentially comes down to that both $q_a$ and $q_b$ represent a rotation of the same angle, which would be identical to the requirement that the rotation matrices $R_1$ and $R_2$ have the same eigenvalues/are similar. Another way of writing a quaternion is the axis-angle representation $\cos\left(\theta\over2\right) + \sin\left(\theta\over2\right)(u_x\,i+u_y\,j+u_z\,k)$, where $\theta$ is the angle and $\begin{bmatrix}u_x & u_y & u_z\end{bmatrix}^\top$ the (unit-)axis of rotation. So therefore we can conclude from equation $(2)$ and $(3)$ that $R(q)$ has to map the rotation axis of $q_a$, $\vec{u}_a$, to the rotation axis of $q_b$, $\vec{u}_b$, so $\vec{u}_b = R(q)\,\vec{u}_a$. There are infinitely many rotations which would satisfy this mapping.

The rotations with the smallest angle can be found by using a rotation axis perpendicular to both given axes, which can be obtained using the cross product of the two axes

$$ q_{\min} = \sqrt{\frac{1 + \vec{u}_a \cdot \vec{u}_b}{2}} + \sqrt{\frac{1 - \vec{u}_a \cdot \vec{u}_b}{2}} \begin{bmatrix} i & j & k \end{bmatrix} \frac{\vec{u}_a \times \vec{u}_b}{\|\vec{u}_a \times \vec{u}_b\|} \tag{4} $$

where $\vec{u}_a$ and $\vec{u}_b$ are both assumed to be of unit length.

The rotations with the largest angle can be found by rotating 180° around the average of the two axis

$$ q_{\max} = \begin{bmatrix} i & j & k \end{bmatrix} \frac{\vec{u}_a + \vec{u}_b}{\|\vec{u}_a + \vec{u}_b\|}. \tag{5} $$

And if it is desired these could be converted to rotation matrices. For example the rotation matrix corresponding to equation $(5)$ can simplified down to the following expression

$$ R_{\max} = \frac{1}{s_1^2 + s_2^2 + s_3^2} \begin{bmatrix} s_1^2-s_2^2-s_3^2 & 2\,s_1\,s_2 & 2\,s_1\,s_3 \\ 2\,s_2\,s_1 & s_2^2-s_1^2-s_3^2 & 2\,s_2\,s_3 \\ 2\,s_3\,s_1 & 2\,s_3\,s_2 & s_3^2-s_1^2-s_2^2 \end{bmatrix} \tag{6} $$

with $s_n = q_{a,n+1} + q_{b,n+1}\ \forall\,n\in\{1,2,3\}$.

| improve this answer | |
$\endgroup$
1
$\begingroup$

After long hours of search, I realized that it is not a simple problem at all. There are many related papers exists published back in the 1980s 1, 2. The short summary is as follows

\begin{equation} ^L\textbf{R} \textbf{R}=\textbf{R} ^C\textbf{R} \end{equation}

$^L\textbf{R}_{i}=\boldsymbol{e}^{[^L\textbf{r}_i]_\times} , ^C\textbf{R}_i=\boldsymbol{e}^{[^C\textbf{r}_i]_\times}$

$\textbf{M} = \sum_{i=1}^{I}{^L\textbf{r}_i {^C\textbf{r}_i^\top}}$

$\textbf{R} = (\textbf{M}^\top\textbf{M})^{-1/2}\textbf{M}^\top$

Single pair of rotation is not enough to decide the unique solution.

Also, the following post is actually asking the same question. here

If scale translation should be considered as well, Eq 8.27 of this book has the answer.

Here is matlab implementation of this problem.here

| improve this answer | |
$\endgroup$
  • $\begingroup$ This seems to be a continuation of your question, rather than a proper answer. If that's the case, you can append the contents of this answer in the question itself as an "edit", then remove the answer. $\endgroup$ – Shahbaz Apr 4 '18 at 20:04
  • $\begingroup$ You are right I have edited it. As above contains the answer to the question as well, I just moved the question part to top. $\endgroup$ – C.O Park Apr 4 '18 at 20:13
  • $\begingroup$ Links might break, so could you give a summary of your solution in your answer. $\endgroup$ – fibonatic Apr 5 '18 at 0:16

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.