Possible bug in SE3Base::internalMultiplyByGenerator

[NikolausDemmel]

I think there might be the translation missing in:

Sophus/sophus/se3.hpp

Line 124 in 3f84e42

res.template tail<3>() = so3().unit_quaternion() * internal_gen_t;

Should this maybe be the following, to implement the full SE product of "this * generator_i"?

res.template tail<3>() = translation() + so3().unit_quaternion() * internal_gen_t;

@strasdat, can you suggest a good reference for SE(3) / SO(3) / SU(2) derivatives as needed for manifold optimization?

[jlblancoc]

Should this maybe be the following,...

👍 the formula makes sense, but it's hard to believe such an error is overlooked by unit tests (!?).

can you suggest a good reference for SE(3) / SO(3) / SU(2) derivatives as needed for manifold optimization?

Sections 7 to 10 of these notes I collected over the years might hopefully be a good starting point... (feedback is welcome!)

[NikolausDemmel]

Should this maybe be the following,...

👍 the formula makes sense, but it's hard to believe such an error is overlooked by unit tests (!?).

Ah, good point about the unit tests. There is in fact a unit test, and it passes as is, and fails with my suggestion. Thinking about it again, my comment is wrong, since we are multiplying by a generator, and not by another SE3 element. The generators (as 4x4 matrices) all have 0 in their last row, which means that the translation of this is in fact irrelevant and should not be added, which should translate over to the quaternion representation in the same way, I guess.

[NikolausDemmel]

can you suggest a good reference for SE(3) / SO(3) / SU(2) derivatives as needed for manifold optimization?

Sections 7 to 10 of these notes I collected over the years might hopefully be a good starting point... (feedback is welcome!)

@jlblancoc: I'm aware of your tech report, and it has been very useful to me many times, thank your very much!

However, for me it leaves some questions open, in particular when trying to understand the implementation here.

Your report discusses the optimization related Jacobians with respect to a matrix representation, not the quaternion one. Also, you follow the convention for current state estimate x and update in local parameterization u of x boxplus u = exp(u) * x, whereas Sophus implements x boxplus u = x * exp(u). Finally, it seems that Sophus uses a more general rule for computing the Jacobian of the boxplus update, something like d (x * exp(u)) / d u_i = vec(x * generator_i), where generator_i is the i'th generator of the lie algebra. I'm not sure of that holds in general? Do you know a reference for the latter rule?

P.S.: I have some minor comments on this part of your report. In what form would you prefer the feedback? Via email?

[NikolausDemmel]

P.S.: regarding this issue here, looking at your report, equation (10.19), it makes sense that the translation does not appear in the Jacobian of Sohpus' code either.

[jlblancoc]

Hi @NikolausDemmel ,

P.S.: I have some minor comments on this part of your report. In what form would you prefer the feedback? Via email?

Until now people contacted via email, but I have just published the whole thing on a GH repo so it's easier to discuss and propose changes to anyone. Feel free of opening issue tickets there to post your comments on the report.

you follow the convention for current state estimate x and update in local parameterization u of x boxplus u = exp(u) * x, whereas Sophus implements x boxplus u = x * exp(u).

AFAIK, both are valid choices. I can't remember from the top of my head if one has more advantages than the other in computational terms.

Your report discusses the optimization related Jacobians with respect to a matrix representation, not the quaternion one.

I tend to like the matrix version since it led to really "clean" (and linear) Jacobians. Probably we can obtain the Jacobians wrt quaternions by applying the chain rule. That conversion Jacobian would be a nice addition to the report, since it will allow any of the other formulas to work for quaternions too.

something like d (x * exp(u)) / d u_i = vec(x * generator_i), where generator_i is the i'th generator of the lie algebra. I'm not sure of that holds in general? Do you know a reference for the latter rule?

I was not familiar with that rule, but writing it in matrix form, looks valid in general to me.

[strasdat]

can you suggest a good reference for SE(3) / SO(3) / SU(2) derivatives as needed for manifold optimization?

While SE(3) / SO(3) etc. are pretty much covered by standard textbooks, I'm not aware of a good reference when it comes to the concept I call "internal generators". This concept arises by the fact that we represent SO(3) internally as SU(2).

I do have some vague plans to

• add all relevant Jacobians of Sophus Lie groups to the c++ library (including matrix representations as well as quaternion ones)
• derive all Jacobians and other interesting functions (e.g exp/log) using sympy and include such symbolic proofs inside of Sophus.

When I find some time...

[NikolausDemmel]

you follow the convention for current state estimate x and update in local parameterization u of x boxplus u = exp(u) * x, whereas Sophus implements x boxplus u = x * exp(u).

AFAIK, both are valid choices. I can't remember from the top of my head if one has more advantages than the other in computational terms.

This is also my understanding. I have came across both variants many times and there does not seem to be a clear advantage of either approach in general.

I tend to like the matrix version since it led to really "clean" (and linear) Jacobians. Probably we can obtain the Jacobians wrt quaternions by applying the chain rule. That conversion Jacobian would be a nice addition to the report, since it will allow any of the other formulas to work for quaternions too.

Yes that would be useful. In general there is of course also the problem that there are different conventions for quaternions (see Joan Sola's techreport for a detailed discussion).

While SE(3) / SO(3) etc. are pretty much covered by standard textbooks, I'm not aware of a good reference when it comes to the concept I call "internal generators". This concept arises by the fact that we represent SO(3) internally as SU(2).d

Is there really something fundamental going on here? It is simply a matter of representing rotations in different and equivalent ways. And its fine to mix and match, as long as you are consistent. E.g. for manifold optimization, what Jacobians you need depends on whether you differentiate down to the quaternion representation, or stop at the direction cosine matrix, and at the same time whether you apply the update directly to the quaternion, or whether you update an intermediate direction cosine matrix and then convert the result to a quaternion.

I do have some vague plans to

Would be nice to have :-D

[strasdat]

Is there really something fundamental going on here?

@NikolausDemmel, you are right. There is not. I merged some changes which make this hopefully much cleaner: SO(2), SO(3), SE(2) and SE(3) have now a number of Jacobians implemented:

Dx_exp_x with is Dx exp(x).
Dx_exp_x_at_0 with is Dx exp(x) at x=0,
Dx_exp_x_matrix_at_0 with is Dx exp(x).matrix() at x=0, and
Dx_this_mul_exp_x_at_0 with is Dx (this * exp(x)) at x=0.

Here, Dx_exp_x_matrix_at_0 is equivalent to the infinitesimal generator of the matrix Lie group.
And Dx_exp_x_at_0 is what I called "internal generator" before, but I don't believe this term is canonical hence I will avoid it from now on. Finally Dx_this_mul_exp_x_at_0 is the Jacobian which is needed by Ceres and hence replaces internalMultiplyByGenerator.

All such derivatives are derived from sympy, see py/ directory - which is covered by unit tests.

In addition all derivatives have c++ unit test coverage by comparing the Jacobians with numerical Jacobians using finite differences.

[strasdat]

Hence, I believe PR #133 addresses this issue. Please reopen if this is not the case.

[NikolausDemmel]

Thanks @strasdat