Итеративное умножение относительных преобразований приводит к быстрой нестабильности - PullRequest
Новая
       25

Итеративное умножение относительных преобразований приводит к быстрой нестабильности

0 голосов
/ 03 июня 2019

Я пишу программу, которая получает преобразования Eigen и сохраняет их в контейнере после применения некоторого шума.В частности, в момент времени k я получаю преобразование T k .Я получаю из контейнера преобразование T k-1 , создаю дельту = T k-1 -1 · T k ,применить некоторый шум к дельте и сохранить 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;

Я удивлен, как быстро происходит расхождение.Поскольку ожидается, что моя настоящая программа будет повторяться более сотни раз, есть ли способ создать более стабильные относительные преобразования?

1 Ответ

2 голосов
/ 04 июня 2019

Да, используйте 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;

Если бы вы изучили разницу, которую вы распечатали, вращающаяся часть матрицы получает огромную ошибку, в то время как переводная частьтерпимый.Обратное на матрице вращения будет быстро влиять на проблемы со стабильностью, поэтому использовать ее напрямую не рекомендуется.

...