Есть ли способ избежать ошибки точности кватернионов? ⇐ C++

[Anonymous]

Быстрый вопрос: я пытаюсь использовать кватернионы для трехмерного вращения.
Я заметил, что иногда у меня возникают небольшие ошибки здесь и там. В качестве математической библиотеки я использую 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
static void testQuat() {
Pose parentGlobalLocation = Pose::identity();

Pose childGlobalLocation = Pose::identity();
Pose childLocalLocation = Pose::identity();

parentGlobalLocation.rotation = Eigen::AngleAxis(45.0 * (EIGEN_PI / 180.0), Eigen::Vector3(1, 0, 0));
childGlobalLocation = parentGlobalLocation;
Pose shift = Pose(Eigen::Quaternion(1, 0, 0, 0), Eigen::Vector3(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(0, 3) = Eigen::Vector3d(0, 0, 10000000);

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

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

int main()
{
testQuat();
testMatrix();
return 0;
}

Выходные данные при использовании кватерниона:
[0.000, -0.754, 10000000.000]
Вместо этого, когда я использую матрицу, результат будет:
[0.000, 0.000, 10000000.000]
Ожидается: [0.000, 0.000, 10000000.000]

Подробнее здесь: https://stackoverflow.com/questions/792 ... sion-error