有没有办法避免四元数精度误差？

Question

快速提问，我正在尝试使用四元数来处理 3D 旋转。 我注意到有时我会出现一些小错误。我使用 Eigen 作为数学库。

这是显示问题的最短代码。我还使用矩阵而不是四元数创建了相同的代码逻辑，并且使用矩阵的结果是正确的。

你有什么提示吗？

这是代码示例：

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]

预期为 [0.000, 0.000, 10000000.000]

Answer 1

1. 您所描述的精度损失是预期的。使用“Eigen”库时始终标准化四元数。

2. 要在精度方面优化“t1.inverse() * t2”形式的变换乘法，您可以实现自定义函数 t1.inverseMul(t2)。

qr = q1.inverse()q2; pr = qr(p2-p1);