相对变换的迭代乘法导致快速不稳定

Question

我正在编写一个程序，它接收 Eigen 变换并在应用一些噪声后将它们存储在容器中。特别是，在时间 k，我接收到变换 T_k。我从容器中获取转换 T_k-1，创建 delta = T_k-1^-1 · T_k，对delta应用一些噪声，将T_k-1 · delta作为容器的新元素存储。

我注意到，在 50 次迭代之后，值完全错误，并且在每次迭代中，我都看到容器的最后一个元素在预乘以它的倒数时，甚至不等于标识。

我已经检查过容器是否遵循 Eigen 指定的分配规则。我觉得这个问题和我做的操作不稳定有关。

下面的简单代码在 max = 35 时产生非零值，并在 max 大于 60 时产生无穷大。

Eigen::Isometry3d my_pose = Eigen::Isometry3d::Identity();
my_pose.translate(Eigen::Vector3d::Random());
my_pose.rotate(Eigen::Quaterniond::UnitRandom());
Eigen::Isometry3d my_other_pose = my_pose;
int max = 35;
for(int i=0; i < max; i++)
{
    my_pose = my_pose * my_pose.inverse() * my_pose;
}
std::cerr << my_pose.matrix() - my_other_pose.matrix() << std::endl;

我很惊讶分歧发生的速度。由于我的真实程序预计会迭代数百次以上，有没有办法创建更稳定的相对转换？

Answer 1

是的，使用 Quaterniond 进行旋转：

Eigen::Isometry3d my_pose = Eigen::Isometry3d::Identity();
my_pose.translate(Eigen::Vector3d::Random());
my_pose.rotate(Eigen::Quaterniond::UnitRandom());
Eigen::Isometry3d my_other_pose = my_pose;
Eigen::Quaterniond q(my_pose.rotation());
int max = 35;
for (int i = 0; i < max; i++) {
    std::cerr << q.matrix() << "\n\n";
    std::cerr << my_pose.matrix() << "\n\n";
    q = q * q.inverse() * q;
    my_pose = my_pose * my_pose.inverse() * my_pose;
}
std::cerr << q.matrix() - Eigen::Quaterniond(my_other_pose.rotation()).matrix() << "\n";
std::cerr << my_pose.matrix() - my_other_pose.matrix() << std::endl;

如果您检查打印出的差异，矩阵的旋转部分会出现巨大误差，而平移部分是可以容忍的。旋转矩阵的逆会很快遇到稳定性问题，所以通常不建议直接使用它。

相对变换的迭代乘法导致快速不稳定

Iterative multiplication of relative transforms leads to fast instability

c++11

eigen3