Add SGal(3) - Githubissues

artivis commented 5 months ago

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 commented 5 months ago

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

joansola commented 5 months ago

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 commented 5 months ago

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 commented 5 months ago

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 commented 5 months ago

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 commented 5 months ago

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

We should definitely do this in sgal3

joansola commented 5 months ago

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 commented 5 months ago

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 commented 5 months ago

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 commented 5 months ago

please tell me if you want me to commit @artivis

joansola commented 5 months ago

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 commented 5 months ago

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 commented 5 months ago

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 commented 5 months ago

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

joansola commented 5 months ago

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

artivis commented 5 months ago

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 commented 5 months ago

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 commented 5 months ago

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 commented 5 months ago

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 commented 5 months ago

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 commented 5 months ago

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 commented 5 months ago

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 commented 5 months ago

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 commented 5 months ago

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] commented 5 months ago

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 commented 5 months ago

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

joansola commented 5 months ago

@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 commented 5 months ago

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 commented 5 months ago

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 commented 5 months ago

Comments addressed :+1: .

artivis commented 5 months ago

Thanks @joansola for your help on this PR!

joansola commented 5 months ago

Visca!! Thanks to you @artivis

artivis / manif

Add SGal(3) #289

Codecov Report