Feature: support for composite groups

[joansola]

In several developments it is handy to consider composite manifolds, such as T3xSO3 instead of SE3, or the IMU implementation using T3xT3xSO3.

In the paper, a notation is introduced so as to be able to define, for composite manifolds, the full family of operators: composition, inverse, exp, log, plus and minus, mainly. It is also possible to produce the Jacobians as a block-concatenation of the jacobians of each individual group.

It'd be very interesting if we could create a (probably variadic templated) way of producing composites of any number of groups, and then have the API ready to manipulate them as a single group.

This is probably not trivial to do.

[artivis]

I actually considered that not so long ago and it is not trivial, especially if we want it variadic.
Let us leave this open for later reference.

ps- We would have to add a Tn group too.

[joansola]

The T(n) group is trivial

The composite is nice, it would save a lot of coding in the ProcessorMotion classes in wolf

[artivis]

I know T(n) is trivial but if we want a generic/variadic composite group, (as to compose e.g. T3xT3xSO3) then we will need to be able to call some manif API on the T(n) blocks.

[joansola]

I said variadic just like this. I have no idea actually. Maybe there are better ways, traits, or whatsoever...

[joansola]

but yes, Tn is necessary if we want the composite, clearly

[joansola]

I think this feature is a must if we at wolf are to use manif.

In particular, in wolf we are interested in having position, orientation and velocity separated in independent state blocks, so that they can be fixed / estimated individually.

Then, since most factor residuals have automatic differentiation, this is performed in a block basis, which is equivalent to performing the Lie operations in a composite fashion.

We therefore need the motion processors to provide covariances matrices also defined in the composite tangent space, otherwise it would collide with what happens in the factors: the covariances matrices would be not compatible.

The current alternative is to make the composite verbose in the processors. But this is not beneficial in terms of clarity and elegance, and the interest of manif is rather small.

So let us discuss the possibilities of this composite feature...

• one option is to code generic composite API
• another option is to add the T(n)xSO(n) groups, with appropriate connections with SE(n) -- this is quite easy I guess.
• a third option is to extend the API of SE(n) to transform to/from TxSO(n), with the Jacobians of this transformation. This way, we could integrate covariances in SE(n), and at the last moment transform it to TxSO(n) --- I am a bit unsure that this is proper, but I am investigating it.

[joansola]

My comment above is inaccurate, or wrong. In fact, there are 3 different views of the translation+rotation thing. They are exposed in Example 7 in the paper.
As a summary, here are their group and tangent elements, and the main operations of exp(), log() and compose():

• SEn

Lie Group [R, t | 0, 1]
Tangent: Lie algebra [v, th]
exp(v,th) = [exp(th), V(th)*v | 0, 1]
log(T) = [V(th).inv*t | log(R)]
T1 * T2 = [R1*R2, t1 + R1*t2 | 0, 1]

• TnxSOn

Lie Group [R, t | 0, 1]                // same as SEn
Tangent: [t, th]                       // diff than SEn
exp(t,th) = [exp(th), t | 0, 1]        // diff than SEn
log(T) = [t | log(R)]                  // diff than SEn
T1 * T2 = [R1*R2, t1 + R1*t2 | 0, 1]   // same as SEn

• composite <Tn,SOn>

Lie Group <R, t >               // diff than SEn and TxSOn
Tangent: <t, th>                // diff than SEn and TxSOn
exp(t,th) = <exp(th), t>        // "same" as TxSOn, not the same format but same algebra
log(T) = <t , log(R)>           // "same" as TxSOn, not the same format but same algebra
T1 * T2 = <R1*R2, t1 + t2>      // diff than SEn and TxSOn

The main problem, implementation wise, is that sometimes we want SEn, sometimes TnxSOn, and sometimes <Tn,SOn>, all in the same project. That is:

• We use exp() in SE3 to convert from velocity commands to the group,

delta = [delta R, delta t] = exp(v*dt, w*dt)

• We use TnxSOn for composition to integrate the delta above into a larger Delta,

Delta+ = Delta * delta // `*` is composition

• We want the composite definition of the derivatives of the above, so that Ceres can then use autodiff on state blocks and still have the covariance defined in the same tangent space of the composite. In particular, the derivatives of the composition of SEn or TnxSOn (the compositions are the same) are computed using the definition of the derivativee of the composite, that is, derivating in a block-by-block basis. This is explained as follows:

Consider D+=(R+,T+), D=(R,T), d=(r,t), and the composition D+=D*d defined by SEn or TnxSOn,

D*d = [R*r , T+R*t | 0, 1]

Then, using the derivative of the composite, we can separate the Jacobian in 4 blocks,

J_D+_D = [J_T+_T , J_T+_R
          J_R+_T , J_R+_R]

where the blocks are defined by the Jacobians of either SOn or Tn:

J_T+_T = I        // Tn jacobian of 'T + R.act(t)' wrt 'T'
J_T+_R = -R*[t]_x // SO3 Jacobian of 'R.act(t)' wrt 'R'
J_R+_T = 0        // no dependency 
J_R+_R = R.tr     // SO3 Jacobian of composition 'R*r' wrt 'R'

• We often want the composite definition of minus to define errors

e = [e_T, e_th] = D2 (-) D1 = [T2 - T1 , log(R1.tr*R2)]

What all this implies, sorry for the length of this, is that it is still unclear what we need from manif in terms of implementation. I will raise this topic for discussion in wolf and come back here later.

[joansola]

OK I gave another thought to all this.

What is needed is that, just for Jacobian computation, the groups SE2 and SE3 be treated as composites. That is, that the Jacobians be treated in a per-block basis. I will write an example for the Jacobian of composition.

// Regular SE3 Jacobians:
//==================
SE3 T1, T2, T;
Matrix6s J_T_T1, J_T_T2;
T = T1.compose(T2, J_T_T1, J_T_T2); 

// Block SE3 Jacobians
//================

// 1. Proposal API for Manif
T = T1.compose<BLOCK_JAC>(T2, J_T_T1, J_T_T2); // for example a template but who cares now about implementation

// 2. Equivalent implementation
// We compute the composition by blocks T, R, with T a vector and R in SO3:
// We have obviously T = T1*T2 = [R1*R1 , P1+R1*P2 | 0 0 0 1]
// and we notice that R1*P2 = R1.act(P2)
Vector3s P1, P2, P; // positions
SO3 R1, R2, R;      // orientations
Matrix3s J_P_P1, J_P_R1, J_R_P1, J_R_R1; // 3x3 block Jacobians
Matrix3s J_P_P2, J_P_R2, J_R_P2, J_R_R2; // 3x3 block Jacobians
P      = P1 + R1.act(P2, J_P_R1, J_P_P2);
J_P_P1 = Identity(3,3)
R      = R1.compose(R2, J_R_R1, J_R_R2)
J_R_P1 = Zero(3,3)
J_R_P2 = Zero(3,3)

// 3. Explanation: The idea with the block-wise Jacobian of SE3 is that:
J_T_T1 = [ J_P_P1 , J_P_R1 
           J_R_P1 , J_R_R1 ]
J_T_T2 = [ J_P_P2 , J_P_R2 
           J_R_P2 , J_R_R2 ]

// 4. Note: the composition, itself, is exactly the same in both cases. 
// Only the Jacobians differ. 
// This stemps from using the composite form of (+) and (-) in the Jacobian definition:
            f(X(+)w) (-) f(X)
J_Y_X = lim -----------------
                   w

Is this doable -- interesting? For us it would solve the problem with elegance, since all this mess would be executed with just T = T1.compose<BLOCK_JAC>(T2, J_T_T1, J_T_T2);

[artivis]

This approach has the benefit of being simple but only answers the specific case of <T2/3,SO2/3>, it does not generalize to exotic composite. For the specific case of composites, I think we better go a generalized path, even if it is trickier to implement.
Moreover the proposed compose<BLOCK_JACK> treats an object as something it is not, therefore introduces confusion.
For your particular use case, remember that objects in manif can be used in conjunction with Eigen::Map, thus you could simply use the appropriate map in the right place: Map<SE3d>, Map<T3xSO3> or Map< <T3,SO3> > (assuming T3xSO3 and a generic composite groups are implemented).

[joansola]

The thing is: there are two different needs:

1. Implementing generalized composites, where all operations are done block-wise: true composite
2. Implementing block-wise Jacobians to groups that are not composite. These include SE2/3 and the future IMU group

In wolf, we do not need 1 for anything.

What I need right now is 2. And since the choice is project-dependent, it could depend on a configuration flag in the library. Like manif::setJacobianStyle(BLOCK); or something. I've seen similar option flags in libs like GTSAM, which are even set at compilation time! But for us setting them at runtime would be I think better.

[joansola]

Do we have any new thought on this?

[artivis]

Hi, nothing new at the moment. I still don't like the idea of implementing block-wise Jacobians to groups that are not composite. Let us discuss it over Slack some time.

[markusgft]

Dear all (@artivis)

In several developments it is handy to consider composite manifolds, such as T3xSO3 instead of SE3, or the IMU implementation using T3xT3xSO3.

I would like to back up the request for such a feature! manif is a great tool, but I'm already hitting its limits due to lack of composite manifold support (like introduced in the paper).

Are there any plans for releasing such a feature?

Best, and keep up the great work!

[joansola]

Hi Markus.

As you can read in this issue, a general implementation is not trivial. That is, something that you can pass any number of lie groups and treat it with the same API as a single Lie group.

We'll "unsleep" the topic as the feature is really powerful for coding algorithms generically.

[markusgft]

Maybe, as a sidenote ... a good starting point and probably already sufficient to many users might be a "reduced" API which does not support all methods that can be called on the individual (atomic) Lie-groups, but only a subset of them!

[artivis]

@markusgft This feature seems to be especially expected and as a matter of fact we do need it internally too. However it does requires some effort to get it done and done right. Unfortunately I won't have a sufficient bandwidth to work on this before February.

[markusgft]

The thing is: there are two different needs:

1. Implementing generalized composites, where all operations are done block-wise: true composite

2. Implementing block-wise Jacobians to groups that are not composite. These include SE2/3 and the future IMU group

Dear @artivis, thanks for checking on this! I understand that it might take some time to implement properly. I just want to add on top of this, that for me and the people in my group, above cited feature 1 would already be great step forward, and would cover many of our use-cases. And, I guess it is bit simpler to realize than 2. So, maybe, it would be a compromise to first provide 1. and then later expand it to 2.? Thanks a lot in any case!

[artivis]

@markusgft So at the moment I am only considering to implement the point 1. which is clear from almost every aspect but also the most needed. However it is also the most tricky to implement given that we want something very generic - think e.g. Composite<R3, R3, SO3> Imu for the IMU group.

[artivis]

Hi @minrui-hust, thanks for your comment.
manif does provide a Eigen::Map access, see an example here.
This feature is not thoroughly tested but I have used it in a couple projects and it seems to work fine. Please give it a try and let us know if you encounter any issues. Feel free also to open tickets.

[markusgft]

Dear all,
sorry for bugging everyone again, but I am still very interested in the composite group feature.
Would it be possible to comment on the development timeline and give an outlook on when this feature might appear?
Thanks, and keep up the great work!

[artivis]

Hi @markusgft,
Sorry for coming back to you so late. I cannot give you any ETA for this feature as I do not currently have the time necessary to work on it. We have noticed a separate effort from @minrui-hust on his own fork to implement the feature (see an example here). This is an impressive work and hopefully it would be a great basis to get the feature in manif.

[joansola]

Any news regarding composites? -- this is just a PING --