gtsam::retract vs Sophus::exp

[Brice Rebsamen]

Hello

I wrote this unit test to compare the expmap between sophus and gtsam:

```
TEST(GtsamSophusTest, RetractComparisonWithSophus) {
const gtsam::Vector3 dr{0.1, 0.2, 0.3};
const gtsam::Vector3 dt{1.0, 2.0, 3.0};

// Gtsam tangent has rotation first, whereas in sophus it's the opposite!
const gtsam::Vector6 dx_gtam = (gtsam::Vector6{} << dr, dt).finished();
const gtsam::Vector6 dx_sophus = (gtsam::Vector6{} << dt, dr).finished();

const gtsam::Pose3 A_gtsam;
const gtsam::Pose3 B_gtsam = A_gtsam.retract(dx_gtam);

const Sophus::SE3d A_sophus;
const Sophus::SE3d B_sophus = A_sophus * Sophus::SE3d::exp(dx_sophus);

const Eigen::Quaterniond Q_gtsam = B_gtsam.rotation().toQuaternion();
const Eigen::Quaterniond Q_sophus = B_sophus.unit_quaternion();
const double rot_err_deg = Q_sophus.angularDistance(Q_gtsam) * 57.3;
EXPECT_TRUE(Q_gtsam.isApprox(Q_sophus))
<< Q_gtsam.coeffs().transpose() << " vs " << Q_sophus.coeffs().transpose()
<< ": err = " << rot_err_deg << " degrees";
EXPECT_TRUE(B_gtsam.translation().isApprox(B_sophus.translation()))
<< B_gtsam.translation().transpose() << " vs "
<< B_sophus.translation().transpose();
}
```

I was surprised to find that the rotations disagree by about 0.25
degrees, which is a lot more than just numerical differences!

Sophus represents rotations by a Quaternion. Gtsam represents
rotations as a 3x3 matrix, but it has an option to use quaternions
instead. After turning on this option, gtsam’s retraction operation
agrees exactly with Sophus’ one.

I also don’t know why that would make such a large difference, I’d
expect some numerical differences and maybe some different runtime,
but not such a large error…

I am trying to decide whether to turn on the gtsam option so that it
uses quaternion for rotations, but I don’t know why it’s not enabled
by default, and whether there would be a good reason not to…

Could someone explain why there' such a difference between the
quaternion retract operation and the matrix's one? And any reason not
to represent rotations as quaternions?

Regards
Brice

[Brice Rebsamen]

Note that I'm on 4.0.0 where GTSAM_ROT3_EXPMAP / GTSAM_POSE3_EXPMAP are not defined by default as it is now in 4.1.0. If I define those 2 variables then the test passes. Without those the Cayley map is used instead of the Expmap. Is that for speed only?

[Frank Dellaert]

Exactly, that explains the difference. It was for speed but now seems more trouble than it's worth.