Fix/plane factor

[dellaert]

Adding unit test by Marco (@mcamurri) to debug issue #561

Note it does not seem to be the adding of a second factor that causes the problem. For example, commenting out both plane priors but adding two plane factors works. In fact and this is crazy just uncommenting the prior on P2 makes the solver fail. This is crazy as the prior is not even supposed to play a role when we eliminate P1.

I'm stumped. Could be a subtle memory bug in OrientedPlane3 or any of the derivatives, but I checked at least some of the prime suspects and did not see anything obviously wrong.

More investigation needed

[mcamurri]

Thank you very much @dellaert for considering the issue. I will try this PR very soon. All the credit for the discovery goes to @dwisth who found the issue and wrote the initial version of this test. I just played with it for a while.

[dellaert]

@mcamurri ?

[mcamurri]

Hi @dellaert, we had to put this issue aside for a while but we are back on it. We'll share some updates soon. @dwisth

[mcamurri]

See #680 for further developments

[dellaert]

I merged #680 and #362 in here

[dwisth]

Just an update, I've been looking into this and have had some luck with the "relative plane factor" as described in Kaess2015 (Equation 25). This is basically a ternary factor, with a local linearisation point for the plane.

It seems to work well for unit tests and some real datasets. I'm just polishing it off, doing some more testing, and will try to push soon.

[dellaert]

That makes a lot of sense, given our discussion. I'll assume you're adding a factor. And that we can also remove the one "error" function in favor of one with analytical derivatives :-)

[dwisth]

I just:

• Added the new plane factor, called LocalOrientedPlane3Factor.
• Removed the Unit3::error() function as you suggested, and switched all functions that were using this to Unit3::errorVector().

The only thing I'm stuck on now is the OrientedPlane3 lm_rotation_error unit test. I can't quite figure out why it doesn't optimize correctly (this is the same reason that I suggested switching to Unit3::localCoordinates instead of Unit3::errorVector). It might be something particular to the unit test but I'm not quite sure what it is. Any ideas?

[dellaert]

I think we should restrict this PR to the plane factors only, no changes are needed to Unit3 and other factors that derive from it. Regarding the unit test that doesn’t pass, if this is not important to your current use case, I would just comment it out. I looked at it as well and I could not figure out what was wrong either.

[dellaert]

Cool, thanks !
@ProfFan can you review and approve? (I created PR and can't approve)
One thing that still is not working is CI: still the failing test?

[ProfFan]

In general LGTM, but I think it's better if there are inline comments on why tests are disabled.

[dellaert]

Merged, thanks a lot all!!!!

[hunkyu]

Has this problem been solved? Which version is the final version? I found that gtsam 4.0.3 still has problems @dellaert