Normalize rotations after composition

[varunagrawal]

This PR fixes #372

I added normalize function to orthogonalize the rotation after composition for the matrix representation and I used the normalized method for the quaternion representation.

I added a test case based on #372 and while it initially failed, it now succeeds, validating this approach.

The idea is from here.

[varunagrawal]

@Alexma3312 FYI 🙂

[ProfFan]

Amazing find :) Two questions:

• this works when the error is small? If so it should be documented
• should this be enabled by default?
  • This operation is a bit expensive (1 det = 9 mul, then 3 block ops)
  • if ROT3_QUAT is enabled then one quat->mat and one mat->quat
  • not necessary? since quat is normalized by itself?

[varunagrawal]

1. If the error is less than 1e-12 it just returns the current rotation.
2. Since we check if the determinant is very close to 1, we don't always perform the normalization. The determinant isn't too expensive for 3x3 matrices and I am sure Eigen optimizes that at compile time. Since we only perform the normalization occasionally (when the drift is large), the block operations cost should be amortized.
3. I did propose using the quat formulation but @dellaert found it inelegant. Also, I believe the matrix to quaternion conversion and vice-versa requires more FLOPS than the determinant and block operations combined.

[dellaert]

Very cool! But please search for normalize through the GTSAM code base as I think we might already have some of this in place. I can be mistaken.

And: we really cannot add this in the low-level operations. We can discuss in person.

[dellaert]

That's unacceptable I'm afraid: people will have to do this explicitly themselves, it can't be part of the low-level geometry.

[varunagrawal]

Updated.

[varunagrawal]

I did a search and the only normalize functions/methods I could find were in Point3 and Rot2. Nothing for Rot3.

There is a ClosestTo method in SO3.cpp which also does a normalization, but uses a full SVD which is far more expensive than what this PR is proposing.

[dellaert]

OK, please add comments in both places to compare and contrast them. Maybe move declaration and definitions of the new method to be adjacent To ClosestTo.