Understanding initialization of SE2Tangent in se2_sam.cpp

[Shubodh]

FINAL EDIT: Summary of our discussion: The primary issue was my incorrect understanding of u = (vx*dt, 0, w*dt) in se2/3_sam.cpp (line 51). Joan has clarified it here. Also, if you're using a representation like g2o format, you might want to take a look here.

(Added the above at the time of closing the issue to summarize our discussion. See next para for "ORIGINAL POST")

ORIGINAL POST: Issue: manif's SE2Tangent must be initialized as an element in tangent space (or more precisely, its isomorphic space ) which is referred to as in the paper (let us call this "Tau vector") and this is clearly NOT the same as (Let me call this "Delta vector"). But it is being treated as the "Delta vector" in se2_sam.cpp (see Line 51 or Line 263). I feel this is incorrect theoretically, please point out if I'm wrong. Please read on:

I have elaborated the issue in great detail on this Notion page. Over there, I have given a quick revision of theory and given theoretical justification. I also mentioned how I verified it by matching results from factor graph library GTSAM.

I get better results if I initialize it as "Tau vector" as opposed to "Delta vector" like it is suggested in the example code. A point of caution here is that even if you initialize it as "Delta vector" (which is theoretically wrong) for small pose graphs, you don't get extremely bad results, so a user might initialize it incorrectly without even realizing it (This error will not be noticeable for small pose graphs but it will make a big difference for bigger ones). This is because, for small angles (which is the case in robotics for odometry factors), both the vectors don't differ by a significant amount. You can verify by going through the math described in that notion page.

[joansola]

Dear Shubodh,

Maybe I understood you incorrectly but I do not see where the example is wrong. We do consider the vector u of controls to be a tau vector in se3. Then, we use it as such in L377, where be aware that the symbol + has been overloaded with the function rplus, thus doing

X_new = X + u = X.rplus(u) = X * u.exp()    //  = X * Exp(u) in the paper , eq. (25)

as it must be. Does this solve the issue?

[Shubodh]

Thank you for your quick reply Joan. I understand that once u is initialized correctly to be a tau vector, then there are no issues with what comes later such as overloading +. But I feel in the example se2_sam.cpp, the way it should be initialized itself is written in a misleading way (Please point out if I am wrong):

When we say u = (vx*dt, 0, w*dt) in line 51, we are referring to the "Delta vector" (as per my above terminology) and NOT "Tau vector", am I correct in this understanding? That is, (vx*dt, 0, w*dt) gives us (dx, dy, dtheta) which is NOT the same as tau vector?

So, if I have my measurements in "Delta vector" (which is the case commonly such as g2o files) representation, I will have to explicitly convert it using the math I described in that Notion page TO "Tau vector", am I right to say this?

[joansola]

But we are not referring u as a delta vector, which would be in SE2, but as a tangent vector, which is in se2. Where does this confusion come from? From anywhere in the paper / in manif / in the se2sam file? I-d be happy to clarify any misleading passage.

If g2o provides deltas, then convert them to tangents with tangent = log(delta). Since the delta is in the form [x,y,theta] and not [x,y,R] (they are equivalent), the log is simply log([x;y;theta]) = [V(theta).inv * [x;y] ; theta]

[joansola]

see this in L47 stating u is in se2

 *  The control signal u is a twist in se(2) comprising longitudinal
 *  velocity vx and angular velocity wz, with no other velocity
 *  components, integrated over the sampling time dt.
 *
 *      u = (vx*dt, 0, w*dt)

[joansola]

That is, (vx*dt, 0, w*dt) gives us (dx, dy, dtheta) which is NOT the same as tau vector?

this (vx*dt, 0, w*dt) IS a tau vector

[artivis]

I think maybe @Shubodh 's issue comes from the confusion between SE2/se2 and Tn x SO2/ tn x so2. Please have a closer look at Example 7 in the paper that should help clarify.

[artivis]

If g2o provides deltas, then convert them to tangents with tangent = log(delta)

It may be handy to provide TnSO2Tangent in manif and have it operate seamlessly with SE2, e.g. in pseudo-code:

SE2 = SE2 + TnSO2Tangent

It is not as proper as implementing both the group and tangent but since only the tangent parameterization differs, it is likely more practical.

[Shubodh]

Grateful for your time @artivis and @joansola. Please bear with me, let me clarify my source of confusion:

Before we get to our problem, I feel there is a typo in eqns 157 and 173. On the RHS of the equation, there is "p", instead of which there should be "t" which is translation from equation 152. It should be instead of , isn't it?

Please point out if I'm wrong, but for continuing forward, I will assume I'm right and call it "t" below instead of "p".

As per my understanding, (vx*dt, 0, w*dt) is referring to the increment in x, y and theta. As per this, in the RHS of eqn 157: , I thought translation "t" correspond to the first two elements (vx*dt, 0) and theta or "Log(R)" in the eqn is (wdt). So in other words, as per this understanding of "increment in x, y, theta", `(vxdt, 0, wdt)indeed corresponds to the "Delta vector" on whichlog(delta)has to be applied to convert to "tau vector" as you explained above (log([x;y;theta]) = [V(theta).inv [x;y] ; theta]`.

If g2o provides deltas, then convert them to tangents with tangent = log(delta). Since the delta is in the form [x,y,theta] and not [x,y,R] (they are equivalent), the log is simply log([x;y;theta]) = [V(theta).inv * [x;y] ; theta]

---

this (vx*dt, 0, w*dt) IS a tau vector

I don't understand how (vx*dt, 0, w*dt) is itself "Tau vector", because as per my understanding, it is an increment vector, which is (dx, dy, dtheta) = (t, Log(R)) from eqn 157.

[artivis]

Hi @Shubodh, Again, I have the feeling that you are looking for something that is not there. Have you carefully read Section IV and especially Example 7? If you do so you will realize that SE2Tangent(dx, dy, dt) != T2SO2Tangent(dx, dy, dt) or in your wording 'tau vector != delta vector'. They both are in tangent spaces (increments) but different spaces. The later is the familiar 'move then rotate' but the former is 'move and rotate at the same time' (a.k.a screw). For very small increments they are nearly interchangeable, the closer to zero the more this is true. But for larger increments they are very different as you realized in your experiment. Therefore you need to know in which tangent space your data lies in order to integrate it properly. If G2o gives you increments in T2SO2Tangent then you cannot directly use them as manif::SE2Tangent, simply because the maths don't match, again as you realized in your experiment. For now, by lack of a better solution, you can use the 'trick' mentioned by jsola se2_increment = SE2(dx, dy, dt).log();.

[joansola]

The reason (dx, dy, dtheta) is tau vector is because of this:

• v = (vx, vy, w) is a vector of velocities, therefore tangent to the group.

• v*dt is vector*scalar, therefore in the same space as v, and parallel to it.

• Therefore, v*dt is tangent to the group, ie v*dt is a tau vector. If you do v*dt:=(dx, dy, dtheta) then (dx, dy, dtheta) is a tau vector

The fact that you can construct a delta as element of the group and name its elements dx,dy,dtheta does not mean that you cannot construct a tau vector and name its elements also dx,dy,dtheta. In such case, the dx,dy,dtheta of the delta are not equal to the dx,dy,dtheta of the tau, although you have given them the same name.

[joansola]

@artivis I think in this case the issue is not exactly about R2xSO2, although it is quite similar.

In any case, @Shubodh, try to see where the confusion is. If my comment above is just what you need, then we can call it done. If not, we can go on with the discussion. Thanks!

[joansola]

Effectively, in 157 should be t and not p. Thanks for pointing this, I-ll fix it right now

[Shubodh]

Thanks a lot for your time Jeremie and Joan. Joan's reply clarifies my doubt. I am looking at Example 7 as well & will revert if I face any issues understanding it but for now, my main doubt is cleared. Closing the issue.

Also, edited the name of the issue and my first post to summarize our discussion.