Add SGal(3)

[artivis]

Add the Galilean Group SGal(3) as per,

• "Absolute humanoid localization and mapping based on IMU Lie group and fiducial markers" Mederic Fourmy, Dinesh Atchuthan, Nicolas Mansard, Joan Sola, Thomas Flayols, avilable at HAL
• "All About the Galilean Group SGal(3)" J. Kelly - arxiv.

Todo:

• [x] left Jacobian
• [x] 'act' Jacobians

[artivis]

@joansola FYI I've added SGal(3) to the autodiff test suite.

[joansola]

Regarding the left jac. Is the right jacobian ready, then? I think J.Kelly provides the left, and then we'll have to implement also the right via the trick Jr(a)=Jl(-a)?

[artivis]

Regarding the left jac. Is the right jacobian ready, then? I think J.Kelly provides the left, and then we'll have to implement also the right via the trick Jr(a)=Jl(-a)?

The rjac is implemented with respect to the left using the adj (see here).
This can easily be replaced with return (-*this).ljac();.

You'll notice in the function some comments, I've implemented the 'easy blocks' of the right jac and validated those against auto diff.

[joansola]

This can easily be replaced with return (-*this).ljac();.

This would be a time saver over the Adj solution. And it works for all groups.

[artivis]

This would be a time saver over the Adj solution. And it works for all groups.

I'd have to check but I believe that all the other groups implement a closed-form anyway.

[joansola]

This can easily be replaced with return (-*this).ljac();.

We should definitely do this in sgal3

[joansola]

I think I found a bug here (L266, bugged is commented):

    cD = (Scalar(4) + theta_sq * (Scalar(1) + cos_t) - Scalar(4) * (theta * sin_t + cos_t) ) / (Scalar(2) * theta_cu);
    // cD = (Scalar(4) + theta_sq * (Scalar(1) + cos_t) - Scalar(4) * (theta * sin_t - cos_t) ) / (Scalar(2) * theta_cu);

it's a sign before cos(t). Compare with eq (36) in kelly's paper

still, the corrected code does not pass the Jacobian test either.

[joansola]

Also, this statement has no else clause. What happens then if the if returns false?

L262:

  if (theta_sq > Constants<Scalar>::eps) {
    cA = (Scalar(2) - theta * sin_t - Scalar(2) * cos_t) / theta_cu;
    cB = (theta_cu + Scalar(6) * theta + Scalar(6) * theta * cos_t - Scalar(12) * sin_t) / (Scalar(6) * theta_cu);
    cC = (Scalar(12) * sin_t - theta_cu - Scalar(3) * theta_sq * sin_t - Scalar(12) * theta * cos_t) / (Scalar(6) * theta_cu);
    cD = (Scalar(4) + theta_sq * (Scalar(1) + cos_t) - Scalar(4) * (theta * sin_t + cos_t) ) / (Scalar(2) * theta_cu);
    // cD = (Scalar(4) + theta_sq * (Scalar(1) + cos_t) - Scalar(4) * (theta * sin_t - cos_t) ) / (Scalar(2) * theta_cu);
    cE = (theta_sq + Scalar(2) * (cos_t - Scalar(1))) / (Scalar(2) * theta_cu);
    cF = (theta_cu + Scalar(6) * (sin_t - theta)) / (Scalar(6) * theta_cu);
  }

[joansola]

Implemented the else clause:

  if (theta_sq > Constants<Scalar>::eps) {
    cA = (Scalar(2) - theta * sin_t - Scalar(2) * cos_t) / theta_cu;
    cB = (theta_cu + Scalar(6) * theta + Scalar(6) * theta * cos_t - Scalar(12) * sin_t) / (Scalar(6) * theta_cu);
    cC = (Scalar(12) * sin_t - theta_cu - Scalar(3) * theta_sq * sin_t - Scalar(12) * theta * cos_t) / (Scalar(6) * theta_cu);
    cD = (Scalar(4) + theta_sq * (Scalar(1) + cos_t) - Scalar(4) * (theta * sin_t + cos_t) ) / (Scalar(2) * theta_cu);
    // cD = (Scalar(4) + theta_sq * (Scalar(1) + cos_t) - Scalar(4) * (theta * sin_t - cos_t) ) / (Scalar(2) * theta_cu);
    cE = (theta_sq + Scalar(2) * (cos_t - Scalar(1))) / (Scalar(2) * theta_cu);
    cF = (theta_cu + Scalar(6) * (sin_t - theta)) / (Scalar(6) * theta_cu);
  }else{
    cA = theta / Scalar(12);
    cB = theta_sq / Scalar(24);
    cC = theta_sq / Scalar(10);
    cD = theta_cu / Scalar(240);
    cE = theta / Scalar(24);
    cF = theta_sq / Scalar(120);
  }

Unfortunately, still no luck, and tests fail.

[joansola]

please tell me if you want me to commit @artivis

[joansola]

This is also weird: double assignation of matrix S:

  Ref33 S = Jl.template block<3, 3>(3, 3);        // first assign
  S = t() / Scalar(6) * V                         // second assign
    + (cA * W + cB * W * W) * (t() * V)
    // + cC * t() * (W * V * W)
    + cC * (W * V * (t() * W))
    // + cD * t() * (W * W * V * W)
    + cD * (W * W * V * (t() * W))
    + t() * V * (cE * W + cF * W * W)
    ;

  Jl.template block<3, 3>(0, 6) -= S;           // update

This seems wrong to me. Where is N1 in the (0,6) block of Jl?

In my opinion we should do:

Jl.template block<3,3>(0,6) = N1;     // <-- where is N1? Is it block (3,3)?
Ref 33 S = // bla bla all the formulas. S is N2 in Kelly's paper
Jl.template block<3,3>(0,6) -= S;    // update with N2

[joansola]

A little recap.

The tangent space is [theta ; v ; rho ; t]? please @artivis respond below:

• [ ] true
• [x] false

Then, the error vector in the log() jacobian test is like so:

diff:
 9.8819340106803821e-05
-0.00037940868386976412
-5.7989384822931811e-05
-3.2089439994109625e-09
 -1.950976047382369e-09
-1.2978911279049044e-09
 3.0092006664261817e-10
  -1.27925553483621e-09
 4.7633497146648551e-10
                      0

that is, it is highest in the angular part (theta). Does that mean that the error lies in the angular matrix blocks of the Jacobian? Because then it is not in N2... how do you reason to indicate that the error is probably in N2?

[artivis]

Quick answers, I'll come back to it later.

• Good catch for S, it is indeed a mistake. Altho fixing it doesn't fix the tests.
• N1 is computed and assigned here
• The tangent space is as in the paper, that is [rho, nu, theta, iota] ([translational part, vel part, ang part, time part]).
• If you look at the ceres-based test, path/to/build/test/ceres/gtest_sgal3_ceres, you'll see the whole Jacobian matrix compared against it's auto diff counter part. The diff is then computed element-wise on the matrices. That's my main indication that the blocks I pointed out earlier are the problem. Then given that N1 is essentially Q in SE3 and we already have this tested, I assume that for this block N2 is to be scrutinized first.

[joansola]

OK it all makes sense. But I checked N1 many times already and still cannot find the bug.

[joansola]

This trick of using 3x3 blocks of Jl as temporary placeholders is driving me crazy!

[artivis]

This trick of using 3x3 blocks of Jl as temporary placeholders is driving me crazy!

Jajaja, just replace them. We can optimize the code later ...

[joansola]

OK here's what I did:

• remove all temporary placeholders in Jl, replace with Matrix<Scalar,3,3>
• Name all 3x3 blocks D, E, L, M, N, N1 and N2 as in the paper
• Put computations in alfabetical order: D, E, L, M, N1, N2, N
• Add a bunch of comments

Still does not work. Will commit and continue from here.

EDIT : pushed all changes, ljac() is still WIP

[artivis]

Hi @joansola, Maybe another lead for debugging: I added a couple more tests that currently fail for sgal3.
Interestingly, one of them is delta.exp().inverse().adj() * delta.ljac() == (-delta).ljac().

[joansola]

Hi @joansola, Maybe another lead for debugging: I added a couple more tests that currently fail for sgal3. Interestingly, one of them is delta.exp().inverse().adj() * delta.ljac() == (-delta).ljac().

possibly this equivalent one is a little bit less cumbersome (removing the inverse()):

delta.ljac() == delta.exp().adj() * (-delta).ljac()

EDIT: modified and pushed

[joansola]

I suspect this is wrong:

template <typename _Derived>
void SGal3TangentBase<_Derived>::fillE(
  Eigen::Ref<Eigen::Matrix<Scalar, 3, 3>> E,
  const Eigen::Map<const SO3Tangent<Scalar>>& so3
) {
  using I = typename Eigen::DiagonalMatrix<Scalar, 3>;

  const Scalar theta_sq = so3.coeffs().squaredNorm();

  E.noalias() = I(Scalar(0.5), Scalar(0.5), Scalar(0.5)).toDenseMatrix();

  // small angle approx.
  if (theta_sq < Constants<Scalar>::eps) {
    return;
  }

  const Scalar theta = sqrt(theta_sq); // rotation angle

  const Scalar A = (theta - sin(theta)) / theta_sq;
  const Scalar B = (theta_sq + Scalar(2) * cos(theta) - Scalar(2)) / (Scalar(2) * theta_sq);

  const typename SO3Tangent<Scalar>::LieAlg W = so3.hat();

  E.noalias() += A * W + B * W * W;
}

since W is not normalized in the code, and it is in the paper (it is u^)

[joansola]

since W is not normalized in the code, and it is in the paper (it is u^)

This actually happens also in matrices L, M and N2

[joansola]

OK BUGS solved!!! It was indeed the non-normalized W matrices in the code with respect to the normalized u^ matrices in the paper.

Had to change the following:

• fillE() in SGal3TangentBase.h, used in exp() and block E of ljac()
• a verbose implementation of fillE() in SGal3Base.h, inside log()
• Computations of L and N2 in SGal3TangentBase.h

Missing -- and thus not passing tests:

• act()
• Jacobians of act()

[joansola]

Regarding act().

I see in Kelly's paper he speaks of the action on events, an event being a tuple [p,t] in R3 x R.

He also speaks of plenty of other possible actions: rotation, boost, and who knows what else.

Given that there are multiple possible actions, and we need to pick one, let us consider this:

• In SE(3), act is just transforming points with T + R*p
• In SE_2(3), act is doing the same as in SE(3).

So, in SGal(3) we could also keep doing the same as in SE(3). What do you think?

The thing is, if we want to combine positions, velocities, etc, considering time, we can always compose two different SGal(3) objects.

An action in the likes of SE(3) would mean a compisition with a SGal object with time 0 :

 [R V T ; 0 1 t ; 0 0 1] * [dR dV dT ; 0 1 0 ; 0 0 1] = [R*dR V+R*dV T+R*dT ; 0 1 t ; 0 0 1]

and if the second object has velocity dV=0 and trivial orientation dR=I,

 [R V T ; 0 1 t ; 0 0 1] * [I 0 dT ; 0 1 0 ; 0 0 1] = [R V T+R*dT ; 0 1 t ; 0 0 1]

so, the simple action T+R*dT is just a composition with a SGal with only dT, that is with dR=I, dV=0, dt=0

[artivis]

So, in SGal(3) we could also keep doing the same as in SE(3). What do you think?

I guess that's reasonable and coherent. Let me implement that and update this PR.

[codecov[bot]]

Codecov Report

Merging #289 (0642c38) into devel (974a241) will decrease coverage by 0.23%. The diff coverage is 98.92%.

Additional details and impacted files

```diff @@ Coverage Diff @@ ## devel #289 +/- ## ========================================== - Coverage 98.15% 97.93% -0.23% ========================================== Files 56 62 +6 Lines 1843 2223 +380 ========================================== + Hits 1809 2177 +368 - Misses 34 46 +12 ```

[artivis]

@joansola all the tests are passing successfully :+1: .
I now have to fix the Python bindings ...

[joansola]

@joansola all the tests are passing successfully 👍 . I now have to fix the Python bindings ...

Does this include the Jacobians of act()?

EDIT: I see. Cool! Thanks !

[artivis]

I've got everything working fine :+1: . I'll optimize a bit the Jac (e.g. use again those temporary blocks) and clean the history before doing a force-push.

[joansola]

e.g. use again those temporary blocks

yeah, you like freak coding :-) But please use block <3,3>(6,0) which is not used, otherwise it looks scary!

Also perhaps one can define all powers of theta up to theta^5, they are used multiple times. theta^6 only once.

Do you think these optims matter so much?

[artivis]

Comments addressed :+1: .

[artivis]

Thanks @joansola for your help on this PR!

[joansola]

Visca!! Thanks to you @artivis