7 Unit tests fail in Quaternion mode

[Frank Dellaert]

The following tests FAILED:

 77 - testBetweenFactor (Failed)
 79 - testEssentialMatrixFactor (Failed)
 87 - testRotateFactor (Failed)
 89 - testCombinedImuFactor (Failed)
 91 - testImuFactor (Failed)
156 - testPoseBetweenFactor (Failed)
157 - testPosePriorFactor (Failed)

All of them point to an issue with retract/localCoordinates, as they are mostly inaccurate derivatives. It is not ok to just reduce the test tolerance. Luca should look?

[Chris Beall]

Issue #83 was marked as a duplicate of this issue.

[Chris Beall]

Some of these also failed in rotation matrix mode with full Expmap because of hard-coded unit test values that were only appropriate with the Cayley transform:

     89 - testRotateFactor (Failed)
     91 - testEssentialMatrixFactor (Failed)
    158 - testPoseBetweenFactor (Failed)
    167 - testPosePriorFactor (Failed)

However, for Rot3, rotation matrix with full Expmap results agree with Quaternion mode results very well. So, for factors on rotations something like this does the trick:

#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
  Vector expectedE = (Vector(3) << -0.0248752, 0.202981, -0.0890529);
#else
  Vector expectedE = (Vector(3) << -0.0246305, 0.20197, -0.08867);
#endif

Some of the numerical derivative tolerances had to be lowered to 1E-5, but that's still acceptable, I think?

For Pose3 there are more possible combinations of modes, making things a bit messier. These tests still fail in quaternion mode as of commit 842554a:

     93 - testCombinedImuFactor (Failed)
     94 - testImuFactor (Failed)
    161 - testPoseBetweenFactor (Failed)
    168 - testSmartProjectionHessianPoseFactor (Failed)
    169 - testPosePriorFactor (Failed)

[Chris Beall]

Fixed with issue #137