回転行列から単位四元数への変換

回転姿勢を表す回転行列を単位四元数に変換します。
回転行列を $\begin{pmatrix}m_{11} & m_{12} & m_{13}\\m_{21} & m_{22} & m_{23}\\m_{31} & m_{32} & m_{33}\\\end{pmatrix}$ とし、単位四元数を $q=(w,(x,y,z))\in{\mathbb H}$ とします。
このとき、次の式が成り立ちます。(四元数による回転の式 $(18)$ 参照)

$\begin{eqnarray} \begin{pmatrix}m_{11} & m_{12} & m_{13}\\m_{21} & m_{22} & m_{23}\\m_{31} & m_{32} & m_{33}\\\end{pmatrix} &=& \begin{pmatrix} w^2+x^2-y^2-z^2 & 2(xy-wz) & 2(xz+wy)\\ 2(xy+wz) & w^2-x^2+y^2-z^2 & 2(yz-wx)\\ 2(xy-wy) & 2(yz+wx) & w^2-x^2-y^2+z^2\\ \end{pmatrix}\tag{1} \end{eqnarray}$

また、 $q$ は単位四元数なので、以下が成り立ちます。

$\begin{eqnarray} w^2+x^2+y^2+z^2=1\tag{2} \end{eqnarray}$

$(1)$ の対角成分と $(2)$ より、4つの等式を書き出してみます。

$\begin{eqnarray} \left\{ \begin{array}{l} w^2+x^2-y^2-z^2=m_{11}\\ w^2-x^2+y^2-z^2=m_{22}\\ w^2-x^2-y^2+z^2=m_{33}\\ w^2+x^2+y^2+z^2=1\\ \end{array} \right.\tag{3} \end{eqnarray}$

$(3)$ を行列を使って表します。

$\begin{eqnarray} \begin{pmatrix} 1 & 1 & -1 & -1\\ 1 & -1 & 1 & -1\\ 1 & -1 & -1 & 1\\ 1 & 1 & 1 & 1\\ \end{pmatrix} \begin{pmatrix}w^2\\x^2\\y^2\\z^2\end{pmatrix} = \begin{pmatrix}m_{11}\\m_{22}\\m_{33}\\1\end{pmatrix}\tag{4} \end{eqnarray}$

$(4)$ を $w^2,x^2,y^2,z^2$ について解きます。

$\begin{eqnarray} \begin{pmatrix}w^2\\x^2\\y^2\\z^2\end{pmatrix} = \frac{1}{4} \begin{pmatrix} 1 & 1 & 1 & 1\\ 1 & -1 & -1 & 1\\ -1 & 1 & -1 & 1\\ -1 & -1 & 1 & 1\\ \end{pmatrix} \begin{pmatrix}m_{11}\\m_{22}\\m_{33}\\1\end{pmatrix}\tag{5} \end{eqnarray}$

$(5)$ を行列使わずに書くと、以下のようになります。

$\begin{eqnarray} \left\{ \begin{array}{l} 4w^2=m_{11}+m_{22}+m_{33}+1\\ 4x^2=m_{11}-m_{22}-m_{33}+1\\ 4y^2=-m_{11}+m_{22}-m_{33}+1\\ 4z^2=-m_{11}-m_{22}+m_{33}+1 \end{array} \right.\tag{6} \end{eqnarray}$

$(2)$ より、 $w^2,x^2,y^2,z^2$ のうち少なくとも1つは0より大きいものが存在します。
計算機で求めることを考慮すると、 $w^2,x^2,y^2,z^2$ のうち、 $\gg 0$ であるものがどれかによって、場合分けします。
なぜなら、いずれかの値を定めた場合、他の値はその値で割ることで求まるためです。

$({\rm I})$ $|w|>\dfrac{1}{2}$ の場合
$|w|>\dfrac{1}{2}\Leftrightarrow|w|^2=\dfrac{1}{4}(m_{11}+m_{22}+m_{33}+1)>\dfrac{1}{4}\Leftrightarrow m_{11}+m_{22}+m_{33}>0$ が成り立ちます。
よって、 $|w|>\dfrac{1}{2}$ かどうかは、 $m_{11}+m_{22}+m_{33}>0$ で判断できます。
このとき、 $|w|$ は、 $|w|,|x|,|y|,|z|$ の中で最大とは限りませんが、 $|w|>\dfrac{1}{2}$ なので、 $w$ で除算をして $x,y,z$ を求めても安全であろうと考えるわけです。
$w$ の符号を正として、 $w$ を求めます。

$\begin{eqnarray} w=\frac{1}{2}\sqrt{m_{11}+m_{22}+m_{33}+1} \end{eqnarray}$

$w$ が求まると、 $x,y,z$ を求めることができます。

$\begin{eqnarray} x=\frac{m_{32}-m_{23}}{4w},\ y=\frac{m_{13}-m_{31}}{4w},\ z=\frac{m_{21}-m_{12}}{4w} \end{eqnarray}$

$({\rm II})$ $|w|\leq\dfrac{1}{2}$ の場合
このとき、 $x^2,y^2,z^2$ のうち最大であるものを最初に定め、それを基準として他の値を決めていきます。
以降で、 $x^2,y^2,z^2$ の大小比較を $m_{11},m_{22},m_{33}$ で行いますが、それは、 $m_{11}+1=2(w^2+x^2),m_{22}+1=2(w^2+y^2),m_{33}+1=2(w^2+z^2)$ が成り立つためです。

$({\rm II-i})$ $x^2$ が $x^2,y^2,z^2$ の中で最大の場合 $(x^2> y^2,x^2> z^2)$
$x^2$ が $x^2,y^2,z^2$ の中で最大かどうかは、 $m_{11} > m_{22},m_{11}> m_{33}$ で判断できます。
$x^2$ が $x^2,y^2,z^2$ の中で最大であるとき、 $3x^2=x^2+y^2+z^2=1-w^2\geq1-\dfrac{1}{4}=\dfrac{3}{4}\Leftrightarrow x^2>\dfrac{1}{4}\Leftrightarrow|x|>\dfrac{1}{2}$ が成り立ちます。
$x$ の符号を正として、 $x$ を求めます。

$\begin{eqnarray} x=\frac{1}{2}\sqrt{m_{11}-m_{22}-m_{33}+1} \end{eqnarray}$

$x$ が求まると、 $w,y,z$ を求めることができます。

$\begin{eqnarray} w=\frac{m_{32}-m_{23}}{4x},\ y=\frac{m_{21}+m_{12}}{4x},\ z=\frac{m_{13}+m_{31}}{4x} \end{eqnarray}$

$({\rm II-ii})$ $y^2$ が $x^2,y^2,z^2$ の中で最大の場合 $(y^2> x^2,y^2> z^2)$
$y^2$ が $x^2,y^2,z^2$ の中で最大かどうかは、 $({\rm II-i})$ で無い且つ $m_{22}> m_{33}$ で判断できます。
$y^2$ が $x^2,y^2,z^2$ の中で最大であるとき、 $3y^2=x^2+y^2+z^2=1-w^2\geq1-\dfrac{1}{4}=\dfrac{3}{4}\Leftrightarrow y^2>\dfrac{1}{4}\Leftrightarrow|y|>\dfrac{1}{2}$ が成り立ちます。
$y$ の符号を正として、 $y$ を求めます。

$\begin{eqnarray} y=\frac{1}{2}\sqrt{-m_{11}+m_{22}-m_{33}+1} \end{eqnarray}$

$y$ が求まると、 $w,x,z$ を求めることができます。

$\begin{eqnarray} w=\frac{m_{13}-m_{31}}{4y},\ x=\frac{m_{21}+m_{12}}{4y},\ z=\frac{m_{32}+m_{23}}{4y} \end{eqnarray}$

$({\rm II-iii})$ $z^2$ が $x^2,y^2,z^2$ の中で最大の場合 $(z^2> x^2,z^2> y^2)$
$z^2$ が $x^2,y^2,z^2$ の中で最大かどうかは、 $({\rm II-i}),({\rm II-ii})$ で無いことで判断できます。
$z^2$ が $x^2,y^2,z^2$ の中で最大であるとき、 $3z^2+x^2+y^2+z^2=1-w^2\geq1-\dfrac{1}{4}=\dfrac{3}{4}\Leftrightarrow z^2>\dfrac{1}{4}\Leftrightarrow|z|>\dfrac{1}{2}$ が成り立ちます。
$z$ の符号を正として、 $z$ を求めます。

$\begin{eqnarray} z=\frac{1}{2}\sqrt{-m_{11}-m_{22}+m_{33}+1} \end{eqnarray}$

$z$ が求まると、 $w,x,y$ を求めることができます。

$\begin{eqnarray} w=\frac{m_{21}-m_{12}}{4z},\ x=\frac{m_{13}+m_{31}}{4z},\ y=\frac{m_{32}+m_{23}}{4z} \end{eqnarray}$

最後に

$({\rm I})$ で $w$ の符号を正としている理由は、符号はどうでもよいからです。
なぜなら $q$ は回転を表す単位四元数であり、 $aq\ (a\in{\mathbb R})$ も同じ回転を表すためです。
よって、 $({\rm I})$ では $w$ の符号を正にして、残りの $x,y,z$ の値を決めています。

参考プログラム

glMatrix.jsのquat.fromMat3メソッドのソースを貼り付けておきます。
非常にうまい実装だと思います。

/**
 * Creates a quaternion from the given 3x3 rotation matrix.
 *
 * NOTE: The resultant quaternion is not normalized, so you should be sure
 * to renormalize the quaternion yourself where necessary.
 *
 * @param {quat} out the receiving quaternion
 * @param {ReadonlyMat3} m rotation matrix
 * @returns {quat} out
 * @function
 */
export function fromMat3(out, m) {
  // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
  // article "Quaternion Calculus and Fast Animation".
  let fTrace = m[0] + m[4] + m[8];
  let fRoot;
  if (fTrace > 0.0) {
    // |w| > 1/2, may as well choose w > 1/2
    fRoot = Math.sqrt(fTrace + 1.0); // 2w
    out[3] = 0.5 * fRoot;
    fRoot = 0.5 / fRoot; // 1/(4w)
    out[0] = (m[5] - m[7]) * fRoot;
    out[1] = (m[6] - m[2]) * fRoot;
    out[2] = (m[1] - m[3]) * fRoot;
  } else {
    // |w| <= 1/2
    let i = 0;
    if (m[4] > m[0]) i = 1;
    if (m[8] > m[i * 3 + i]) i = 2;
    let j = (i + 1) % 3;
    let k = (i + 2) % 3;
    fRoot = Math.sqrt(m[i * 3 + i] - m[j * 3 + j] - m[k * 3 + k] + 1.0);
    out[i] = 0.5 * fRoot;
    fRoot = 0.5 / fRoot;
    out[3] = (m[j * 3 + k] - m[k * 3 + j]) * fRoot;
    out[j] = (m[j * 3 + i] + m[i * 3 + j]) * fRoot;
    out[k] = (m[k * 3 + i] + m[i * 3 + k]) * fRoot;
  }
  return out;
}

また、他に実装している方のリンクを張っておきます。
回転行列からクォータニオン（四元数）に戻す

参考リンク

四元数による回転
 回転行列からクォータニオン（四元数）に戻す