回転行列の補間 - 機械学習基礎理論独習

回転行列の補間とは

回転行列 ${\bf M}_a\in{\mathbb R}^{3\times3}$ を回転行列 ${\bf M}_b\in{\mathbb R}^{3\times3}$ までパラメータ $t\in[0,1]\subset{\mathbb R}$ を使って補間することとします。
線形補間は意味ないことが分かるかと思います。

四元数の球面線形補間

${\bf M}_a,{\bf M}_b$ に対する四元数を $p_a,p_b\in{\mathbb H}$ とします。
この時、補間に対応する変換を示す四元数を $q(t)\in{\mathbb H}$ とします。

$\begin{eqnarray} q(t)=\frac{\sin(1-t)\theta}{\sin\theta}p_a+\frac{\sin t\theta}{\sin\theta}p_b\tag{1} \end{eqnarray}$

$\theta$ は $q_a,q_b$ のなす角とします。
この $q(t)$ を使って 3次元ベクトルを補完すると、角速度が一定になります。(証明したわけではなく、プログラムでそうなることを確認しました。)

回転行列の差分の回転行列の回転角の線形補間

考える補間は、 $t=0$ のときは、 ${\bf M}_a$ で、 $t=1$ のときは、 ${\bf M}_b$ なわけです。
そこで、補間を ${\bf M}(t){\bf M}_a$ で表すことを考えます。
$t=1$ のとき、以下が成り立ちます。

$\begin{eqnarray} &&{\bf M}(t=1){\bf M}_a={\bf M}_b\\ &&\rightarrow{\bf M}(t=1)={\bf M}_b{\bf M}_a^{-1}\tag{2} \end{eqnarray}$

$(2)$ で求めた ${\bf M}(t=1)$ から回転軸 ${\bf a}\in{\mathbb R}^{3\times3}$ と回転角 $\theta\in{\mathbb R}$ を求めます。
$\theta$ は回転角なので、絶対値が小さくなるように、 $\theta > \pi$ のとき、 $\theta\leftarrow\theta-2\pi$ とします。
回転軸 ${\bf a}$ と回転角 $\theta t$ から作成した回転行列が ${\bf M}(t)$ となります。
そして、この ${\bf M}(t)$ は回転行列なので、当然角速度が一定です。
また、この ${\bf M}(t)$ は $(1)$ で求めた $q(t)$ と同じ回転を表します。(証明はありません。計算機で確認しました。)

おまけ - オイラー角を線形補間

確認はしていませんが、オイラー角でも同様の補間は可能かと思います。
${\bf M}_a,{\bf M}_b$ に対するオイラー角を求めて、それを線形補間すれば、 ${\bf M}(t),q(t)$ と同等な補間ができるんじゃないかなって思っています。
オイラー角が $\pi$ より大きい場合は、 $2\pi$ 引いたほうが良いと思います。

確認に使用したプログラム

getSlertMatrixが「四元数の球面線形補間」に対応する関数で、getAxisAngleMatrixが回転行列の差分の回転行列の回転角の線形補間に対応する関数です。

function getSlertMatrix(t, startRotMat3, endRotMat3) {
    // mat3 -> quat
    const startRotQuat = q1.fromMat3(startRotMat3);
    const endRotQuat = q1.fromMat3(endRotMat3);
    // slerp
    const rotQuat = q1.slerp(startRotQuat, endRotQuat, t);
    // quat -> mat3
    const rotMat3 = m3.fromQuat(rotQuat);
    return rotMat3;
}

function getAxisAngleMatrix(t, startRotMat3, endRotMat3) {
    const startRotMat3Inv = m3.invert(startRotMat3);
    const toRotMat3 = m3.multiply(endRotMat3, startRotMat3Inv);
    // mat3 -> quat
    const toRotQuat = q1.fromMat3(toRotMat3);
    // quat -> axis angle
    const axis = v3.create();
    let angle = quat.getAxisAngle(axis, toRotQuat); // [0, 2π]
    angle = angle > Math.PI ? angle - 2 * Math.PI : angle;
    // calculate delta angle
    const deltaAngle = angle * t;
    // axis angle -> quat
    const deltaRotQuat = q1.setAxisAngle(axis, deltaAngle); 
    // quat -> m3
    const deltaRotMat3 = m3.fromQuat(deltaRotQuat);
    const rotMat3 = m3.multiply(deltaRotMat3, startRotMat3);
    return rotMat3;
}

// 確認用プログラム
for(let i = 0, len = 100; i < len; i += 1) {
    const t = i  / len;
    const mat = getAxisAngleMatrix(t, startRotMat3, endRotMat3);
    const mat2 = getSlertMatrix(t, startRotMat3, endRotMat3);
    const pos = v3.transformMat3([1, 0, 0], mat);
    const pos2 = v3.transformMat3([1, 0, 0], mat2);
    console.log(equalsVector3(pos, pos2));
}

function equalsVector3(v0, v1, epsilon = 1e-5) {
    const adx = Math.abs(v0[0] - v1[0]);
    const ady = Math.abs(v0[1] - v1[1]);
    const adz = Math.abs(v0[2] - v1[2]);
    if(adx < epsilon && ady < epsilon && adz < epsilon) {
        return true;
    } else {
        return false;
    }
}