AdjointMap jacobians

[gchenfc]

Feature

Lie Groups have AdjointMap implemented.

Just as (lowercase) Pose3.adjointMap has functions for

Matrix66 adjointMap() { ... }
Vector6 adjoint(Vector6 y) { ... }
Vector6 adjoint(Vector6 y, OptionalJacobian H) { ... }

It would be useful to have the equivalent one with jacobians for (uppercase) AdjointMap. Also, I propose to add another argument:

Vector Adjoint(Vector6 y, OptionalJacobian H, OptionalJacobian H_y) { ... }

to avoid having to calculate the AdjointMap again.

Unless there is a reason against this, I can begin working on this.

Motivation

Factors which need to convert tangent vectors to different frames will need the derivatives of AdjointMap. For example, in GTDynamics many factors such as TwistFactor, TwistAccelFactor, and WrenchEquivalenceFactor need this. Currently, GTDynamics is using an ugly way that would become much cleaner if this were implemented, at least for Pose3.

Notes

Currently only Pose3 has a function Adjoint(Vector6 y) { return AdjointMap() * y; }, and also Pose3's AdjointMap has a comment " /// FIXME Not tested - marked as incorrect".

[dellaert]

Sure. Make sure Jacobians for methods are ordered HSelf, Hargument. If this means the API changes, we need to think a bit how to handle, e.g., force the use of the two-argument jacobian version by not having defaults.

[ProfFan]

Ping @gchenfc