smart factors with extrinsics calibration

[lucacarlone]

This PR creates a new smart projection factor that is also able to optimize the extrinsic calibration of a camera (the pose between the camera and the body). In particular, the PR provides the SmartStereoProjectionFactorPP factor class. The suffix "PP" follows the same convention as ProjectionFactorPPP (but we do not have the last "P" since this is a smart factor that does not require providing the 3D Point).

[lucacarlone]

@dellaert : I'm starting on this now (and will probably take a few weeks to complete). Let me know if you see something off in the plan!

[lucacarlone]

btw: the extrinsics might be kept outside the smart factors (ie as a Pose3 variable in the factor graph), which would not require the implementation of a new factor. In that case, rather than keeping a body pose and a body-to-camera pose (as I suggest in the PR), we would keep both poses wrt the global frame. That does not sound very elegant and would cost some sparsity but is straightforward to implement.

[dellaert]

@lucacarlone Interesting!
We might want to connect, as there are already some factors in unstable (look for ProjectionFactor string there) that allow calibration to be changed. Unfortunately the naming convention is weird, and we might want to take this opportunity to move to gtsam proper and have a better, uniform naming.

[dellaert]

@lucacarlone let me know when this is ready for review

[lucacarlone]

@dellaert : this is getting in pretty good shape. I plan to send it out for review by this weekend.

[lucacarlone]

@dellaert : this is ready for review (all tests pass on my side, but I'm also waiting for the CI results)

[lucacarlone]

@dellaert : most CI are passing, but some (3-4) give an "g++-5: internal compiler error: Killed (program cc1plus)" error. have you seen this before?

On some CI machines, the following lines cause an indeterminant linear system, which I find odd since the hessian is not singular:

// This passes on my machine but gets and indeterminant linear system exception in CI.
// This is a very redundant test, so it's not a problem to omit.
// GaussianFactorGraph::shared_ptr GFG = graph.linearize(result);
// Matrix H = GFG->hessian().first;
// double det = H.determinant();
// // std::cout << "det " << det << std::endl; // det = 2.27581e+80 (so it's not underconstrained)
// VectorValues delta = GFG->optimize();
// VectorValues expected = VectorValues::Zero(delta);
// EXPECT(assert_equal(expected, delta, 1e-4));

I commented it out for now, but would love to get it back since it is supposed to work.

[dellaert]

@dellaert : most CI are passing, but some (3-4) give an "g++-5: internal compiler error: Killed (program cc1plus)" error. have you seen this before?

On some CI machines, the following lines cause an indeterminant linear system, which I find odd since the hessian is not singular:

// This passes on my machine but gets and indeterminant linear system exception in CI.
// This is a very redundant test, so it's not a problem to omit.
// GaussianFactorGraph::shared_ptr GFG = graph.linearize(result);
// Matrix H = GFG->hessian().first;
// double det = H.determinant();
// // std::cout << "det " << det << std::endl; // det = 2.27581e+80 (so it's not underconstrained)
// VectorValues delta = GFG->optimize();
// VectorValues expected = VectorValues::Zero(delta);
// EXPECT(assert_equal(expected, delta, 1e-4));

I commented it out for now, but would love to get it back since it is supposed to work.

Yeah, internal compiler error seems to be short for "I ran out of memory". I'm hoping I'm right about that :-) It's been failing on almost all our PRs.

As far as indeterminant linear system, no idea. It would be nice if someone could get to the bottom of this :-) Weird that it's only on some systems.

Is this CI then ready for review?

[lucacarlone]

@dellaert : most CI are passing, but some (3-4) give an "g++-5: internal compiler error: Killed (program cc1plus)" error. have you seen this before?
On some CI machines, the following lines cause an indeterminant linear system, which I find odd since the hessian is not singular:
// This passes on my machine but gets and indeterminant linear system exception in CI.
// This is a very redundant test, so it's not a problem to omit.
// GaussianFactorGraph::shared_ptr GFG = graph.linearize(result);
// Matrix H = GFG->hessian().first;
// double det = H.determinant();
// // std::cout << "det " << det << std::endl; // det = 2.27581e+80 (so it's not underconstrained)
// VectorValues delta = GFG->optimize();
// VectorValues expected = VectorValues::Zero(delta);
// EXPECT(assert_equal(expected, delta, 1e-4));
I commented it out for now, but would love to get it back since it is supposed to work.

Yeah, internal compiler error seems to be short for "I ran out of memory". I'm hoping I'm right about that :-) It's been failing on almost all our PRs.

As far as indeterminant linear system, no idea. It would be nice if someone could get to the bottom of this :-) Weird that it's only on some systems.

Is this CI then ready for review?

yes, this is ready to go!

[mikesheffler]

@dellaert : most CI are passing, but some (3-4) give an "g++-5: internal compiler error: Killed (program cc1plus)" error. have you seen this before?

Yeah, internal compiler error seems to be short for "I ran out of memory". I'm hoping I'm right about that :-) It's been failing on almost all our PRs.

That's always been my experience.

I've seen it a few times while compiling GTSAM (specifically, the wrap, but that's anecdotal evidence), but it's been fine when I've just tried to compile again after failure. CI is probably already a bottleneck, but reducing the -j argument will probably do the trick, if you can.

[varunagrawal]

@mikesheffler in my experience, re-running the job after failure (just the single set, not all of them) usually does the trick. My guess is that all of the requested CI jobs are being run on a single physical machine and the partitioning of jobs across servers is not optimized, leading to the intermittent memory issues.

In any case, we should fix it on our end (which is actually an open issue on the wrapper side).

[varunagrawal]

@dellaert @lucacarlone we disabled the crashing CI until we can resolve the optimizations with the wrapper (in case you're wondering what fixed the CI suddenly).

[lucacarlone]

@dellaert @lucacarlone we disabled the crashing CI until we can resolve the optimizations with the wrapper (in case you're wondering what fixed the CI suddenly).

perfect - thanks @varunagrawal !

[lucacarlone]

@dellaert : ping

[dellaert]

Awesomesauce! Will merge. very thoroughly tested.