Numerous unit test failures with GTSAM_ROT3_EXPMAP=OFF

[chrisbeall]

Description

Numerous unit test failures with GTSAM_ROT3_EXPMAP=OFF. This configuration apparently is not be exercised by CI.

The following tests FAILED:
	 25 - testEssentialMatrix (Failed)
	100 - testAdaptAutoDiff (Failed)
	189 - testSimilarity3 (Failed)
	219 - testPoseBetweenFactor (Failed)
	233 - testPosePriorFactor (Failed)

Steps to reproduce

1. Check out latest develop
2. Set GTSAM_ROT3_EXPMAP=OFF, and also GTSAM_BUILD_WITH_MARCH_NATIVE=OFF (separate issue caused by march=native reported in testSmartProjectionFactor unit test fails with march=native #590)
3. make check

Expected behavior

Tests pass, and CI should probably exercise this configuration.

Environment

Ubuntu 16.04, gcc 5.4.0
Mac OS, Xcode 12.1, Apple clang 12.0.0.12000032

Additional information

Detailed failures on Ubuntu:

 25/243 Test  #25: testEssentialMatrix ....................***Failed    0.01 sec
not equal:
expected[0.1; 0.2; 0.3; 0; 0];
actual[0.101183235; 0.202366469; 0.303549704; 0; 0];
/home/cbeall/git/gtsam/gtsam/geometry/tests/testEssentialMatrix.cpp:83: Failure: "assert_equal(d.head(5), actual2, 1e-8)" 
There were 1 failures

100/243 Test #100: testAdaptAutoDiff ......................***Failed    0.01 sec
/home/cbeall/git/gtsam/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp:189: Failure: "equal_with_abs_tol(expectedP, P)" 
Not equal:
expected:
[
	1.31247;
	1.20573
]
actual:
[
	1.31841;
	1.20368
]
/home/cbeall/git/gtsam/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp:202: Failure: "assert_equal(expectedMeasurement, actual, 1e-6)" 
Not equal:
expected:
[
	1.31247;
	1.20573
]
actual:
[
	1.31841;
	1.20368
]
/home/cbeall/git/gtsam/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp:228: Failure: "assert_equal(expectedMeasurement, actual, 1e-6)" 
Not equal:
expected:
[
	1.31247;
	1.20573
]
actual:
[
	1.31841;
	1.20368
]
/home/cbeall/git/gtsam/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp:234: Failure: "assert_equal(expectedMeasurement, actual2, 1e-6)" 
There were 4 failures

189/243 Test #189: testSimilarity3 ........................***Failed    0.02 sec
/home/cbeall/git/gtsam/gtsam_unstable/geometry/tests/testSimilarity3.cpp:348: Failure: "Exception: 
Indeterminant linear system detected while working near variable
0 (Symbol: 0).

Thrown when a linear system is ill-posed.  The most common cause for this
error is having underconstrained variables.  Mathematically, the system is
underdetermined.  See the GTSAM Doxygen documentation at
http://borg.cc.gatech.edu/ on gtsam::IndeterminantLinearSystemException for
more information." 
There were 1 failures

219/243 Test #219: testPoseBetweenFactor ..................***Failed    0.01 sec
not equal:
expected[-0.0298395126; 0.0131455995; 0.0969712917; -0.13918755; -0.142346243; -0.0390885321];
actual[-0.0298135268; 0.0131341516; 0.096886844; -0.145701634; -0.134898526; -0.0421026389];
/home/cbeall/git/gtsam/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp:136: Failure: "assert_equal(expectedError, actualError, 1e-9)" 
not equal:
expected[0.0173371937; 0.0222228304; -0.012504191; 0.0264132886; 0.0052376953; -7.16127036e-05];
actual[0.0173358202; 0.0222210698; -0.0125032004; 0.0263800787; 0.00540285006; 0.000175859556];
/home/cbeall/git/gtsam/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp:181: Failure: "assert_equal(expectedError, actualError, 1e-9)" 
There were 2 failures

233/243 Test #233: testPosePriorFactor ....................***Failed    0.01 sec
not equal:
expected[-0.184137862; 0.139419284; -0.158399297; 0.740211734; -1.19821028; 1.00815609];
actual[-0.182948258; 0.13851858; -0.157375975; 0.766913166; -1.22976117; 0.949345561];
/home/cbeall/git/gtsam/gtsam_unstable/slam/tests/testPosePriorFactor.cpp:131: Failure: "assert_equal(expectedError, actualError, 1e-8)" 
not equal:
expected[-0.0227128853; 0.19376511; 0.276418421; 1.49751986; -0.549375791; 0.452761203];
actual[-0.0224998729; 0.191947887; 0.273826035; 1.36483392; -0.754590051; 0.585710674];
/home/cbeall/git/gtsam/gtsam_unstable/slam/tests/testPosePriorFactor.cpp:173: Failure: "assert_equal(expectedError, actualError, 1e-8)" 
There were 2 failures

[varunagrawal]

I believe this is primarily a ROT3_EXPMAP issue since the values are differing by an order of 1e-2. Nevertheless, I can add the CI stage for this while also proposing a fix.

[varunagrawal]

I found some related info!

Issue #248 goes into a lot of detail about why the Cayley parameterization is bad (which is what is used when we set ROT3_EXPMAP=OFF).

@ProfFan's post also explains why the tests fail. We're basically seeing poorer convergence due to the approximation. I believe this is the reason we don't have a CI path for ROT3_EXPMAP=ON.

@dellaert you mentioned removing the Cayley parameterization completely. Is this still on the cards?

[dellaert]

ROT3_EXPMAP=ON turns off Cayley and turns on full exponential map. We changed the defaults to have ROT3_EXPMAP=ON, AFAIK, based on Fan's work. Other than that, let's just fix the failing unit tests by branching on the flag?

[varunagrawal]

I did some investigating and the fix for this might be easier than expected. It seems like the majority of tests are failing due to setting GTSAM_ROT3_EXPMAP=OFF but the corresponding GTSAM_POSE3_EXPMAP is still ON. This is because the macro GTSAM_POSE3_EXPMAP is used everywhere in the tests, but the internal behavior of Pose3 has been updated.

I made a simple change to the cmake files and it has done the trick, only one Similiarity3 test is now failing. 🙂

I'll also add in a CI path for GTSAM_ROT3_EXPMAP=OFF and GTSAM_POSE3_EXPMAP=OFF.

[chrisbeall]

It seems like the majority of tests are failing due to setting GTSAM_ROT3_EXPMAP=OFF but the corresponding GTSAM_POSE3_EXPMAP is still ON

I think this was by design, to let users control the behavior of Pose3 and Rot3 independently. Perhaps this no longer makes sense, and a single flag is better?

I failed to mention in my report that all tests (except for testSimilarity3) pass when I set both flags to OFF. Both flags OFF used to be the default up until July 22, when #417 landed.

[dellaert]

Yeah, seems strange that you would have GTSAM_POSE3_EXPMAP=ON and GTSAM_ROT3_EXPMAP=OFF. I propose that combination is disallowed. Let's see how many unit tests fail after that :-)

[varunagrawal]

@dellaert only the AlingPose3 test in testSimilarity3.cpp fails after disallowing that combination.

I looked into the issue (which took forever because of lack of documentation on the Local function for Cayley maps), and the problem is that it doesn't handle the case where (R+I) is singular (R is the rotation matrix, I is identity).

I have already fixed that, though that should be a separate PR. I also recommend using the following snippet instead of the hand written current function implementation. This would allow Eigen to optimize the operations.

Matrix3 A = R.matrix();
return SO3::Vee((A + I_3x3).inverse() * (A - I_3x3)) * 2;

The current implementation is

const double a = A(0, 0), b = A(0, 1), c = A(0, 2);
const double d = A(1, 0), e = A(1, 1), f = A(1, 2);
const double g = A(2, 0), h = A(2, 1), i = A(2, 2);
const double di = d * i, ce = c * e, cd = c * d, fg = f * g;
const double M = 1 + e - f * h + i + e * i;
const double K = -4.0 / (cd * h + M + a * M - g * (c + ce) - b * (d + di - fg));
const double x = a * f - cd + f;
const double y = b * f - ce - c;
const double z = fg - di - d;
return K * Vector3(x, y, z);

which requires re-tracing all the steps in the matrix computations.

[varunagrawal]

Note that in the snippet above, K will end up being inf if R is singular.

[varunagrawal]

Hmm this test case is weird because the prior is a 180 degree rotation around X and Z individually, which the Cayley map cannot accommodate. We should #ifdef that test for Expmap only.