Pose3::Adjoint(xi) Jacobians

[gchenfc]

Closes #433

This PR implements the Jacobians for Pose3::Adjoint(x) := Pose3::AdjointMap() * x.

It uses Rot3.rotate and cross(Point3, Point3) to calculate intermediate derivatives.

This should help in GTDynamics to eliminate the AdjointMapJacobianQ function and allow it to be generalized to other joint types.

[varunagrawal]

Some comments.

[varunagrawal]

I merged the latest develop since it has some CI fixes.

[varunagrawal]

Fixed various stuff so LGTM.

[varunagrawal]

I am wondering why you made the tolerances tighter. I made them looser because the CI wasn't passing, and this is because the scale of the values in the matrix were quite different, so it's more of a numerical stability issue than anything else.

[gchenfc]

Ya idk why I made the tolerances higher - that was stupid - late night fogginess

[dellaert]

If cross does not have derivatives, it probably should have, and they should be used below, and wherever we jump through hoops to compute them in place.

I’d like to sit down for 30 minutes after you have taken into account my comments. See how far you get with the PDF rewrite I’m asking.

[gchenfc]

@dellaert In general I think a lot of these comments are directed at my (maybe misguided) attempts at reducing copy operations, since so there's so many matrices that are the same (e.g. derivatives for all the rotate operations w.r.t. point being rotated are all the same). Also, I tried to keep the chain-rule stuff for readability, which explains why I have so many aliases (const Matrix &).

Would you like to:

1. address them one-by-one,
2. ditch all these "optimizations" opting instead for readability (i.e. extra copies, recompute things that have already been computed, etc), or
3. ditch the chain-rule stuff and just use the algebraic expressions (readable code but unclear where the math came from unless reader reads math.pdf)

?

[dellaert]

@dellaert In general I think a lot of these comments are directed at my (maybe misguided) attempts at reducing copy operations, since so there's so many matrices that are the same (e.g. derivatives for all the rotate operations w.r.t. point being rotated are all the same). Also, I tried to keep the chain-rule stuff for readability, which explains why I have so many aliases (const Matrix &).

Would you like to:

1. address them one-by-one,
2. ditch all these "optimizations" opting instead for readability (i.e. extra copies, recompute things that have already been computed, etc), or
3. ditch the chain-rule stuff and just use the algebraic expressions (readable code but unclear where the math came from unless reader reads math.pdf)

?

Try to avoid calculating R.matrix many times, and don't use chain rule if it threatens do re-compute R.matrix(). But I would use cross derivatives (I checked and they exist). And then make the math.pdf match the code exactly.

It really is just [R*w; t x R*w + R*v] so:

R = R.matrix()
Rw = R * w
txRw = cross(t, Rw, ...)
if (H_pose) { ... }
return [Rw; t x Rw + R*v]

If you will pass a matrix in a derivative slot, make sure it has right size and is not a reference.

[varunagrawal]

Hey Gerry, before I review, can we figure out the CI failures? The issue seems to be in the way you're passing in the matrices for OptionalJacobians. Please be sure to run tests locally since the CI does take a hot minute to complete.

[dellaert]

Hey Gerry, before I review, can we figure out the CI failures? The issue seems to be in the way you're passing in the matrices for OptionalJacobians. Please be sure to run tests locally since the CI does take a hot minute to complete.

Varun hold off on a review. I'd like to see another iteration from gerry before you review, as well. The CI is failing which I think might be because of an uninitialized memory of Rw? I commented on that in my review.

[varunagrawal]

It's because of a const and the fact that we're not passing in a reference to the matrix.

[gchenfc]

@dellaert It took a lot of reading but figured out that the derivative D_1 Ad_T(\xi_b) is just Ad_T * ad_{\xi_b}. In retrospect, of course this should have been obvious.

AdjointTranspose() case is a little harder and I haven't figured out a clean way to derive it without just crunching through the algebra (despite many, many hours trying), but in any case the code is also reasonably clean.

Note: AdjointMap() * adjointMap(xi_b) includes a fair amount of 0-block multiplies that we could avoid by explicitly constructing the product, but I didn't for now. Let me know if you prefer I benchmark / do that, but for now I think it's much easier to understand this way.

[gchenfc]

ping @dellaert , can you review this?

[dellaert]

Incredible!

[dellaert]

seems very similar to AdT

[gchenfc]

Yes, similar but not quite exactly the same

[what, vhat
 vhat, 0   ];

vs adT:

[what, vhat,
 0,    what];

vs AdT:

[R', -R*that,
 0,     R'  ];

I tried for a while to figure out a cleaner representation and to figure out why it was so close to but not quite adT or AdT, but finally I gave up. Although maybe there is some simplification, I couldn't find it nor could I figure out a way to derive this without just working through the algebra. I tried reading up on lie coalgebras but it was just way too dense for me to parse at my current knowledge level.