Question

我正在编写一个程序，该程序接收本征变换并将其应用一些噪声后存储在容器中。特别是在时间k，我收到了变换T _k。我从容器中获得变换T _k-1，创建增量= T _k-1 ^-1·T _{k < / sub>，将一些噪声应用于增量并将T _k-1·增量存储为容器的新元素。}

我注意到，在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;

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

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

1 个答案: