0

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 Tk. I get from the container the transform Tk-1, create the delta = Tk-1-1 · Tk, apply some noise to delta and store Tk-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?

Bob__
  • 12,361
  • 3
  • 28
  • 42
mcamurri
  • 153
  • 11
  • Small hint: You might be luckier in finding an answer at [SciComp Stackexchange](https://scicomp.stackexchange.com/). – andreee Jun 03 '19 at 15:16

1 Answers1

2

Yes, use a Quaterniond for the rotations:

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;

If you would have examined the difference you printed out, the rotation part of the matrix gets a huge error, while the translation part is tolerable. The inverse on the rotation matrix will hit stability issues quickly, so using it directly is usually not recommended.

Avi Ginsburg
  • 10,323
  • 3
  • 29
  • 56
  • Why isn't the class `Isometry3d` not using the `Quaterniond` internally? Isn't there a way to tell it to use quaternions internally and output the rototation matrix on-the-fly when I call `.matrix()`? – mcamurri Jun 04 '19 at 09:42
  • See [this](https://stackoverflow.com/questions/46176533/how-to-ensure-eigen-isometry-stays-isometric). – Avi Ginsburg Jun 04 '19 at 10:21
  • @MarcoCamurri Using Quaternions instead of matrices is inferior when applying the transformation to (multiple) vectors. Quaternions are beneficial when multiplying rotations and are much easier to (re-) normalize. If you want both, you will always have to compromise. – chtz Jun 04 '19 at 12:03
  • @MarcoCamurri, by “inferior” chtz means “less efficient,” not “less accurate.” – Avi Ginsburg Jun 04 '19 at 13:10