Better SE(3) and SE2(3) Jacobians

[dellaert]

Implements ExpmapTranslation with SO3::applyLeftJacobian and simplifies the Expmap jacobians in a way that also provides insight in how they relate to the applyLeftJacobian Jacobians. The key piece of code is this:

  Matrix3 H;
  Vector t = local.applyLeftJacobian(v, H); // = J_l * v !

  // We return Jacobians for use in Expmap, so we multiply with X, that
  // translates from left to right for our right expmap convention:
  Matrix3 X = local.rightJacobian() * local.leftJacobianInverse();
  Q = X * H;

This does three things:

• replaces very complicated expression $-0.5 * V + C * (W * V + V * W - WVW) + F * (WW * V + V * WW - 3 * WVW) - 0.5 * E * (WVW * W + W * WVW)$ from before
• explains why the bottom-right block of the Expmap Jacobian was $J_r$, not $J_l$
• functor in Pose3 is now no longer needed.

Smaller things:

• added applyLeftJacobianInverse and tested
• ExpmapTranslation no longer needs R
• Changed sign of D and E "constants" and renamed them to E and F.
• New D "constant" for use in inverses

[varunagrawal]

Tentative approval but that unit test being killed is bothering me.

[ProfFan]

I think it's better that we stop and think about the nearZero throughly. As I mentioned in #1233, we have to be very careful in these approximations.

for example, the expression for D here has a 1000x error when theta is small.

[ProfFan]

Lie Derivatives.pdf

[ProfFan]

Particularly D and E are problematic.

[dellaert]

@ProfFan I agree we have to look into it - and that issue is amazing - but I'd rather have a separate PR to deal with the nonZero thresholds. I will add the more thorough tests in this PR, but would like to merge after that.

I will add a citation to Ethan Eade's document as I am using his A,B,C constants now.

[ProfFan]

Cool cool

[dellaert]

PS, why did you say "The cutoff should be 1 × 10−3 to use the Taylor expansion" for A? And, I don't understand what you are plotting, actually...