C++ 旋转矩阵转四元素

将一个旋转矩阵转换为四元数的过程可以通过以下步骤实现：

1. 计算矩阵的迹(trace)：$t = R_{00} + R_{11} + R_{22}$

2. 计算四元数的 $w$ 分量：$w = \sqrt{1 + t} / 2$

3. 计算四元数的 $x, y, z$ 分量：

$x = (R_{21} - R_{12}) / (4w)$

$y = (R_{02} - R_{20}) / (4w)$

$z = (R_{10} - R_{01}) / (4w)$

注意，如果 $t$ 的值非常接近于0，则说明矩阵表示的旋转角度非常小，此时可能会出现数值稳定性问题，需要进行特殊处理。

以下是使用 C++ 实现的代码示例：

#include <cmath>
#include <Eigen/Dense>  // 需要安装 Eigen 库

Eigen::Quaterniond matrixToQuaternion(const Eigen::Matrix3d& mat)
{
    double t = mat.trace();
    double w, x, y, z;

    if (t > 0)
    {
        double s = sqrt(t + 1.0);
        w = s / 2.0;
        s = 0.5 / s;
        x = (mat(2, 1) - mat(1, 2)) * s;
        y = (mat(0, 2) - mat(2, 0)) * s;
        z = (mat(1, 0) - mat(0, 1)) * s;
    }
    else
    {
        int i = 0;
        if (mat(1, 1) > mat(0, 0)) i = 1;
        if (mat(2, 2) > mat(i, i)) i = 2;

        int j = (i + 1) % 3;
        int k = (j + 1) % 3;
        double s = sqrt(mat(i, i) - mat(j, j) - mat(k, k) + 1.0);
        double q[4] = { 0 };
        q[i + 1] = s / 2.0;
        s = 0.5 / s;
        q[3] = (mat(k, j) - mat(j, k)) * s;
        q[j + 1] = (mat(j, i) + mat(i, j)) * s;
        q[k + 1] = (mat(k, i) + mat(i, k)) * s;
        w = q[0];
        x = q[1];
        y = q[2];
        z = q[3];
    }

    return Eigen::Quaterniond(w, x, y, z);
}

其中，使用了 Eigen 库实现了矩阵和四元数的基本运算。