lundi 3 juin 2019

Iterative multiplication of relative transforms leads to fast instability

I'm writing a program that receives Eigen transforms and stores them in a container after applying some noise. In particular, at time k, I receive transform T_k. I get from the container the transform T_{k-1}, create the delta = T_{k-1}^-1 * T_k, apply some noise to delta and store T_{k-1} * delta as a new element of the container.

I've noticed that after 50 iterations the values are completely wrong and at every iteration I see that the last element of the container, when pre-multiplied by its inverse, is not even equal to the identity.

I've already checked that the container follows the rules of allocation specified by Eigen. I think the problem is related to the instability of the operations I'm doing.

The following simple code produce the nonzero values when max = 35 and goes to infinity when max is bigger than 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;

I'm surprised how fast the divergence happens. Since my real program is expected to iterate more than hundreds of times, is there a way to create relative transforms that are more stable?

Aucun commentaire:

Enregistrer un commentaire