Given the $3\times3$ rotation matrices $A$ and $B$, find the rotation matrix $X$ that satisfies $AX=XB.$
Solution to $AX=XB$ for $3\times3$ rotation matrices.
5 Answers
This equation commonly arises in the hand-eye calibration problem (robotics), where one want to find a transformation matrix between a camera mounted on the end-effector and the end-effector itself.
- In case no noise is presented in A and B, a minimum of two equations $A_i X=X B_i$ is required to determine a unique solution. Shiu and Ahmad 1989
- Unfortunately this is not true in practice. Instead, a more common approach is to find a "best-fit" solution from a set of equalities $A_i X=X B_i$. There are many solutions has been proposed for this including [F. Park 1994-Robot Sensor Calibration: Solving AX = XB on the Euclidean Group], [K. Daniilidis-The dual quaternion approach to hand-eye calibration], [K. H. Strobl and G. Hirzinger. Optimal Hand-Eye Calibration], etc.
Well, this is equivalent to $A = XBX^{-1}$, so it's only possible if $A$ and $B$ are equivalent. If they are, you can solve this by diagonalizing in $\mathbb{C}$ and finding both base changes $T$, $S$. Then: $A = T^{-1}DT = T^{-1}SBS^{-1}T$ if $B = S^{-1}DS$. And $X = T^{-1}S$.
Edit: This only gives a unitary $X$. Maybe this can be fixed.
-
0I think it can be fixed. The phrase 3D rotation matrix surely implies real arithmetic. $A$3D rotation has an axis fixed by the rotation, and this is unique up to scaling/orientation. So treat $A$ and $B$ as orthogonally similar with $X$ mapping the axis of one to the other. – 2012-10-04
If $A,B$ are 3x3 real matrices representing rotations ("3D rotation matrices"), the following are equivalent:
(1) $tr(A) = tr(B)$
(2) There exists real orthogonal $X$ such that $A = XBX^{-1}$.
(3) There exists rotation matrix $X$ such that $A = XBX^{-1}$.
Since rotation matrices are those orthogonal matrices with determinant 1, it's evident that (3) implies (2), and by replacing $X$ by $-X$ if necessary, also (2) implies (3).
Since similar matrices have equal traces, (2) implies (1). It remains only to show (1) implies (2), which we will do in a constructive fashion.
Setting aside the trivial case of zero rotation (identity matrix), every rotation in three dimensions is characterized by an axis of points fixed by the rotation and an angle by which points rotate around that axis. In the case of a special orthogonal linear transformation (rotation represented by a 3x3 matrix), the axis passes through the origin and may be identified by a unit vector $u$ that generates it. If we adopt a convention that positive angles of rotation are counterclockwise about the axis pointed in direction $u$, then changing the sign of the angle is equivalent to changing instead the vector $u$ into $-u$ (and keeping the same angle as before).
As the Wikipedia article points out, if rotation matrix $A$ has angle of rotation $\theta$ about an axis, then the trace of $A$ is $1 + 2 \cos \theta$. For example, by our convention the matrix which represents counterclockwise rotation by $\theta$ about the positive x-axis is:
$R_x(\theta) = \begin{pmatrix} 1 & 0 & 0 \\ 0 & \cos \theta & -\sin \theta \\ 0 & \sin \theta & \cos \theta \end{pmatrix} $
and the trace is the sum of diagonal entries.
If $A$ is a rotation matrix with $tr(A) = 1 + 2 \cos \theta$, let $u$ be a unit vector that generates the axis of rotation, i.e. $Au = u$. Extend that to an orthonormal basis $\{u,v,w\}$ for $\mathbb{R}^3$, and obtain an orthogonal matrix $P$ using these basis vectors as columns. Then similar rotation matrix $P^{-1} A P$ is either $R_x(\theta)$ or $R_x(-\theta)$ in the notation above. If the latter, replace $w$ by $-w$, so that:
$R_x(\theta) = P^{-1} A P$
Do the same with rotation matrix $B$ also having $tr(B) = 1 + 2 \cos \theta$:
$R_x(\theta) = Q^{-1} B Q$
Now $X = P Q^{-1}$ satisfies (2), so proving that (1) implies (2) as desired.
Genericaly, there is an infinity of solutions in $SO_3$. Let $A=Rot(\Delta,\alpha)$ with $\alpha\notin \pi\mathbb{Z}$; since $A=XBX^{-1}$, $A,B$ are othogonally similar, that is equivalent to say that $B$ can be written in the form $B=Rot(\Delta_1,\alpha)$. Let $P\in SO_3$ be fixed s.t. $B=P^TAP$.
Then $A(XP^T)=(XP^T)A$ and $XP^T\in SO_3$ commute with $A$, that is equivalent to $XP^T\in U=\{Rot(\Delta,\beta);\beta\in [0,2\pi)\}$. Finally, the set of solutions is $X\in UP$.
For $P$, it suffices to choose any rotation that sends $\Delta_1$ on $\Delta$; for example, the one with axis $u\wedge u_1$ where $u,u_1$ are unitary vectors on $\Delta,\Delta_1$.
This problem can be solved more easily when representing the rotations with unit quaternions, such that the equation becomes
$ q_a\,q = q\,q_b \tag{1} $
where the unit quaternions $q_a$, $q_b$ and $q$ are related to the rotation matrices $A$, $B$ and $X$ respectively. For this problem unit quaternions have the advantage of having less degrees of freedom (four instead of nine) and the unit length condition, which is a requirement for it to represents a rotation, is much easier to ensure then $X\in SO(3)$.
It can be shown that the product of two quaternions 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_a = q\,q_b\,q^{-1} $
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_{b,1} \\ q_{b,2} \\ q_{b,3} \\ q_{b,4} \end{bmatrix} = \begin{bmatrix} q_{a,1} \\ q_{a,2} \\ q_{a,3} \\ q_{a,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 $q$ converted to its equivalent rotation matrix representation, 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}$, due to the one on the first diagonal position. 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_b$, $\vec{u}_b$, to the rotation axis of $q_a$, $\vec{u}_a$,
$ \vec{u}_a = R(q)\,\vec{u}_b $
which is a more simplified equation then the original equation with rotation matrices. However there are infinitely many rotations which would satisfy this mapping.
Such rotation mapping 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}_b \times \vec{u}_a}{\|\vec{u}_b \times \vec{u}_a\|} \tag{4} $
where $\vec{u}_a$ and $\vec{u}_b$ are both assumed to be of unit length.
The rotation mapping 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\}$, the sum of the axes of rotation, which in this case do not necessary have to be of unit length but they do need to be of equal length.
It can also be noted that equations $(4)$ and $(5)$ do not require the knowledge of the complete unit quaternions $q_a$ and $q_b$. Namely only the axis of rotation is sufficient which can be calculated from rotation matrices quite easily.