The question sounds similar to Angle of rotation around arbitrary axis from matrix but it is not.
I don't want an angle-axis extraction, from a 3×3 rotation matrix ${\rm R}$. I know how to do that by converting to a quaternion.
But if I want to specify the axis, like given ${\rm R}$ what is the rotation (angle) about the x -axis for example that gets closest to ${\rm R}$. And instead of one of the elementary axis, I want to specify a vector $\boldsymbol{\hat{k}}$ about which this unknown angle $\theta$.
My difficulty is how to define when two rotation matrices are the closest. Here is how to formalize this problem
- Given a general 3×3 orientation matrix ${\rm R}$
- Given a specific direction $\boldsymbol{\hat{k}}$
- Find the angle $\theta$ such that ${\rm R} \overset{\rm nearest}{\rightarrow} {\rm rot}(\boldsymbol{\hat{k}}, \theta)$
or how to find the angle $\theta$ such that the matrix ${\rm U}$ is closest to identity in
$$ {\rm R} ={\rm rot}(\boldsymbol{\hat{k}},\, \theta) \,{\rm U} $$
In this context, the minimal rotation within ${\rm U}$ would be the angle $\theta$ that results in the maximum of $${\rm tr}( {\rm U}) = 1 + 2 \cos \varphi$$
where $\varphi$ represents the angle of the remaining transformation $U$ to get from the rotation about $\boldsymbol{\hat{k}}$ to ${\rm R}$.
Best Answer
The Rodrigues' rotation matrix formula gives
$R(\mathbf{k}, \theta) = \mathbf{kk}^T + (I - \mathbf{kk}^T) \cos \theta + S_{\mathbf{k}} \sin \theta $
where $S_{\mathbf{k}} = \begin{bmatrix} 0 && - k_z && k_y \\ k_z && 0 && - k_x \\ -k_y && k_x && 0 \end{bmatrix} $
And you want $R(\mathbf{k}, \theta) $ to be as close as possible to a given rotation matrix $R_0$. For that, as you suggested, you want to maximize the trace of $R_0^T R(\mathbf{k}, \theta)$, and this evaluates to,
$ \text{trace}(R_0^T R(\mathbf{k}, \theta)) = c_1 + c_2 \cos \theta + c_3 \sin \theta $
Let
$c_1 = \text{trace}(R_0^T \mathbf{kk}^T) $
$c_2 = \text{trace}(R_0^T (I - \mathbf{kk}^T)) $
$ c_3 = \text{trace}(R_0^T S_{\mathbf{k}}) $
Clearly, this sinusoid has a single maximum at
$ \theta^* = \text{atan2}(c_2, c_3) $