如何避免Eigen中四元数的精度误差？附复现代码

使用Eigen库通过四元数处理3D旋转时出现精度误差，矩阵实现同逻辑结果正确。以下是复现代码：

struct Pose
{
    Eigen::Quaterniond rotation;
    Eigen::Vector3d position;

    explicit Pose(Eigen::Vector3d position);

    Pose(Eigen::Quaterniond rotation, Eigen::Vector3d position);

    Pose operator*(Pose p) const;

    static Pose identity();

    Pose inverse() const;
};

Pose::Pose(Eigen::Vector3d position) : rotation(Eigen::Quaterniond()),
                                       position(position) 
{
}

Pose::Pose(Eigen::Quaterniond rotation, Eigen::Vector3d position) : rotation(rotation),
                                                                    position(position)
{
}

Pose Pose::identity() 
{
    return Pose(Eigen::Quaterniond(), //
                Eigen::Vector3d(0.0f, 0.0f, 0.0f));
}

Pose Pose::operator*(Pose p) const 
{
    return Pose(rotation * p.rotation, // rotation
                (rotation * p.position) + position); // position
}

Pose Pose::inverse() const 
{
    const Eigen::Quaterniond qInv = (rotation.inverse());
    return Pose(qInv, qInv * (-position));
}


template<typename T>
static void testQuat() {
    Pose<T> parentGlobalLocation = Pose<T>::identity();

    Pose<T> childGlobalLocation = Pose<T>::identity();
    Pose<T> childLocalLocation = Pose<T>::identity();

    parentGlobalLocation.rotation = Eigen::AngleAxis<T>(45.0 * (EIGEN_PI / 180.0), Eigen::Vector3<T>(1, 0, 0));
    childGlobalLocation = parentGlobalLocation;
    Pose<T> shift = Pose<T>(Eigen::Quaternion<T>(1, 0, 0, 0), Eigen::Vector3<T>(0, 0, 10));

    int i = 0;
    while (true) {
        childGlobalLocation = childGlobalLocation * shift;
        childLocalLocation = parentGlobalLocation.inverse() * childGlobalLocation;

        ++i;
        if (i % 10 == 0 || i == 1) {
            printf("iter %d [%.15lf, %.15lf, %.15lf]\n", i, childLocalLocation.position.x(),
                   childLocalLocation.position.y(), childLocalLocation.
                   position.z());
        }
    }
}

static void testMatrix() 
{
    Eigen::Matrix4d parentGlobalLocation;
    parentGlobalLocation.setIdentity();

    Eigen::Matrix4d childGlobalLocation;
    Eigen::Matrix4d childLocalLocation;
    childGlobalLocation.setIdentity();
    childLocalLocation.setIdentity();

    parentGlobalLocation.topLeftCorner(3, 3) = Eigen::AngleAxisd(45.0 * (EIGEN_PI / 180), Eigen::Vector3d(1, 0, 0)).
            toRotationMatrix();
    childGlobalLocation = parentGlobalLocation;

    Eigen::Matrix4d shift;
    shift.setIdentity();
    shift.block<3, 1>(0, 3) = Eigen::Vector3d(0, 0, 10000000);

    childGlobalLocation = childGlobalLocation * shift;
    childLocalLocation = parentGlobalLocation.inverse() * childGlobalLocation;

    Eigen::Vector3d pos = childLocalLocation.block<3, 1>(0, 3);
    printf("[%.3f, %.3f, %.3f]\n", pos.x(), pos.y(), pos.z());
}

int main() 
{
    testQuat<float>();
    testMatrix();
    return 0;
}

四元数实现输出：

[0.000, -0.754, 10000000.000]

矩阵实现输出（预期结果）：

[0.000, 0.000, 10000000.000]

• 归一化四元数：四元数必须保持单位长度才能准确表示旋转，每次旋转运算（乘法、逆运算）后对四元数执行归一化。例如修改operator*和inverse方法：

  Pose Pose::operator*(Pose p) const 
  {
      Eigen::Quaterniond rot = rotation * p.rotation;
      rot.normalize();
      return Pose(rot, (rotation * p.position) + position);
  }
  
  Pose Pose::inverse() const 
  {
      Eigen::Quaterniond qInv = rotation.conjugate(); // 单位四元数的逆等于共轭
      qInv.normalize();
      return Pose(qInv, qInv * (-position));
  }
  

• 切换到双精度浮点数：当前测试用的float单精度类型精度有限，大位移值会放大运算误差。将testQuat<float>()改为testQuat<double>()，用双精度类型提升计算精度。
• 优化运算逻辑减少累积误差：避免循环中反复叠加位姿，直接计算总位移后再做逆变换。比如总位移是shift重复N次，可直接计算parentGlobalLocation * (shift^N)再求逆，减少中间步骤的误差累积。
• 定期修正位姿：在循环迭代过程中，定期对四元数做归一化，对位置坐标做误差修正，防止误差随迭代次数无限增大。

内容的提问来源于stack exchange，提问作者Premoli