4
$\begingroup$

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}$.

$\endgroup$
1
  • $\begingroup$ Its almost like a least squares problem, but for rotations. $\endgroup$ Oct 28, 2022 at 12:08

3 Answers 3

3
$\begingroup$

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) $

$\endgroup$
2
$\begingroup$

I might have found my answer.

First, convert ${\rm R}$ into a quaternion with a vector and a scalar part $\boldsymbol{q} = \pmatrix{ \boldsymbol{v} \\ s }$. Then decompose ${\rm R}$ using the quaternion rotation $${\rm R} = 1 + 2s [\boldsymbol{v}\times] + 2 [\boldsymbol{v}\times] [\boldsymbol{v}\times]$$ where $[\boldsymbol{v}\times]$ is the 3×3 skew-symmetric cross product operator matrix of vector $\boldsymbol{v}$.

Similarly, write the (inverse) Rodrigues rotation matrix using the unknown angle $\theta$

$$ {\rm rot}(\boldsymbol{\hat{k}},\, -\theta) = 1 - \sin \theta [\boldsymbol{\hat{k}}\times] + (1 - \cos \theta) [\boldsymbol{\hat{k}}\times]\,[\boldsymbol{\hat{k}}\times]$$

Rewrite the minimization problem as

$$ {\rm find}( \theta) \overset{\rm maximize}{ \rightarrow} u={\rm tr}(U) $$

where

$$ {\rm U} = {\rm rot}( \boldsymbol{\hat{k}},\, -\theta)\;{\rm R} $$

The trace value $u$ is expanded in terms of the three components of the axis $\boldsymbol{\hat{k}} = (\boldsymbol{\hat{k}}_1, \boldsymbol{\hat{k}}_2, \boldsymbol{\hat{k}}_3)$ and the three components of $\boldsymbol{v} = (\boldsymbol{v}_1,\boldsymbol{v}_2,\boldsymbol{v}_3)$

$$ u = 4s \sin \theta (\boldsymbol{\hat{k}} \cdot \boldsymbol{v}) - 2 \cos \theta \left( \lambda \right) + \ldots $$

where

$$ \small \lambda = (\boldsymbol{2 \hat{k}}_1^2 + \boldsymbol{\hat{k}}_2^2 + \boldsymbol{\hat{k}}_3^2) \boldsymbol{v}_1^2 + (\boldsymbol{\hat{k}}_1^2 +2 \boldsymbol{\hat{k}}_2^2 + \boldsymbol{\hat{k}}_3^2) \boldsymbol{v}_2^2 + (\boldsymbol{\hat{k}}_1^2 + \boldsymbol{\hat{k}}_2^2 + 2 \boldsymbol{\hat{k}}_3^2) \boldsymbol{v}_3^2 + 2 \boldsymbol{\hat{k}}_3 \boldsymbol{v}_3 ( \boldsymbol{\hat{k}}_2 \boldsymbol{v}_2 + \boldsymbol{\hat{k}}_1 \boldsymbol{v}_1) + 2 \boldsymbol{\hat{k}}_1 \boldsymbol{\hat{k}}_2 \boldsymbol{v}_1 \boldsymbol{v}_2 $$

and the $\ldots$ terms are not dependent on $\theta$.

The maximal value of $u$ is found with ${\rm d}u / {\rm d} \theta = 0$ which occurs when

$$ \theta = \tan^{-1} \left( \frac{- 2 s\,(\boldsymbol{\hat{k}} \cdot \boldsymbol{v})}{\lambda} \right) $$

or the complementary angle

$$ \theta = \pi - \tan^{-1} \left( \frac{- 2 s\,(\boldsymbol{\hat{k}} \cdot \boldsymbol{v})}{\lambda} \right) $$

So I don't know if the above is correct or not. As a test case if ${\rm R}$ is a single rotation about $\boldsymbol{\hat{k}}$ then the above recovers the correct angle. But I am not sure how to test for more general cases, and how to prove algebraically that the above is correct.

$\endgroup$
0
$\begingroup$

The answer by @HosamHajjir is the correct answer.

I code it up in C# using the classes from System.Numeric.Vectors for reference.

public static float GetAngleAboutAxis(this Quaternion orientation, Vector3 axis, bool complementary = false)
{
    // U = rot(k,θ) Rᵀ
    //   = (1+ sin(θ) k× + (1-cos(θ)) k×k×) Rᵀ
    //   = Rᵀ + sin(θ) k×Rᵀ + (1-cos(θ)) k×k×Rᵀ
    //   = [Rᵀ + k×k×Rᵀ] + sin(θ) [k×Rᵀ] - cos(θ) [k×k×Rᵀ]
    //
    // solve max(tr(U))
    //
    //  tr(U)  = α +  β sin(θ) - γ cos(θ)
    //     α = tr([Rᵀ + k×k×Rᵀ])
    //     β = tr([k×Rᵀ])
    //     γ = tr([k×k×Rᵀ])
    //
    //  max(tr(U)) => θ = -atan(β/γ), θ = π-atan(β/γ)

    // Note: Built-in quaternion->matrix returns the
    // inverse rotation for some reason. So no need
    // to take the transpose in this case. phew.
    var R = Matrix4x4.CreateFromQuaternion(orientation);
    var B = Cross(axis) * R;
    var C = Cross(axis) * B;
    float β = Trace(B);
    float γ = Trace(C);
    float θ = -(float)Math.Atan(β / γ);

    return complementary ? PI + θ : θ;
}

It uses the trace of the three components of the Rodrigue's rotation formula to express the trace of ${\rm U} = {\rm rot}( \boldsymbol{\hat{k}},\, \theta)\;{\rm R}^\intercal$ in terms of a constant term, a term of $\sin \theta$ and a term of $\cos \theta$.

The expression $u = {\rm tr}({\rm U}) = \alpha + \beta \sin \theta - \gamma \cos \theta$ is easily maximimized by taking the derivative and setting it to zero

$$ \theta = - \tan^{-1} \left( \frac{\beta}{\gamma} \right) $$

$\endgroup$

You must log in to answer this question.

Not the answer you're looking for? Browse other questions tagged .