Question about PreintegrationBase::computeErrorAndJacobians()

[[e-mail]]

Hi all,

Thank you for providing this toolbox. When I was using the function PreintegrationBase::computeErrorAndJacobians() to compute the jacobian matrices with respect to Navstate_i and Navstate_j. The jacobian matrices returned by this function seems different to the paper:

"Supplementary Material to: IMU Preintegration on Manifold for Ecient Visual-Inertial Maximum-a-Posteriori Estimation "

The jacobians I got from gtsam: J_imu_Rt_i:

 0.999583  -0.00370357    0.0297364            0            0            0
  0.00440801     0.999819   -0.0271464            0            0            0
  -0.0297405    0.0272337     0.999382            0            0            0
 -5.0354e-05  -0.00420948 -0.000147319     0.999123   -0.0133408    0.0396955
  0.00375861  0.000612347     0.011244    0.0156952     0.998099   -0.0596028
 0.000223624   -0.0110903  0.000677763   -0.0388249    0.0601735     0.997433
 -0.00215178    -0.171387  -0.00343979            0            0            0
    0.153193    0.0249156     0.457572            0            0            0
  0.00655498    -0.451272    0.0274797            0            0            0
           0            0            0            0            0            0
           0            0            0            0            0            0
           0            0            0            0            0            0
           0            0            0            0            0            0
           0            0            0            0            0            0
           0            0            0            0            0            0

J_imu_v_i:

          0           0           0
          0           0           0
          0           0           0
 -0.0155027  -0.0104749   0.0463674
 -0.0433632   0.0230977 -0.00928024
 -0.0194754  -0.0430902   -0.016246
  -0.310054   -0.209498     0.92735
  -0.867265    0.461954   -0.185605
  -0.389509   -0.861806   -0.324921
          0           0           0
          0           0           0
          0           0           0
          0           0           0
          0           0           0
          0           0           0

J_imu_Rt_j:

  -0.999936  -0.0102197  0.00937156           0           0           0
  0.0100118   -0.999605  -0.0329148           0           0           0
-0.00959333   0.0328508    -0.99961           0           0           0
          0    0.068479   0.0163512          -1          -0          -0
  -0.068479           0   0.0653147          -0          -1          -0
 -0.0163512  -0.0653147           0          -0          -0          -1
          0   -0.034401  -0.0930645           0           0           0
   0.034401           0   0.0709897           0           0           0
  0.0930645  -0.0709897           0           0           0           0
          0           0           0           0           0           0
          0           0           0           0           0           0
          0           0           0           0           0           0
          0           0           0           0           0           0
          0           0           0           0           0           0
          0           0           0           0           0           0

J_imu_v_j:

        0         0         0
        0         0         0
        0         0         0
        0         0         0
        0         0         0
        0         0         0
 0.310054  0.209498  -0.92735
 0.867265 -0.461954  0.185605
 0.389509  0.861806  0.324921
        0         0         0
        0         0         0
        0         0         0
        0         0         0
        0         0         0
        0         0         0

Navstate_i:

R:
|-0.308271, -0.884918, -0.349126|
|-0.168604, 0.412013, -0.895443|
|0.936238, -0.217175, -0.276212|
p:[0.385935, 0.326425, 1.69786]'
v:[0.394045, 0.334516, -0.0816263]'

Navstate_j:

R:
|-0.310054, -0.867265, -0.389509|
|-0.209498, 0.461954, -0.861806|
|0.92735, -0.185605, -0.324921|
p:[0.370738, 0.26437, 1.73446]'
v:[0.21931, 0.451185, -0.0402843]'

Could you tell me if I used this function in the wrong way? Thank you!

Sicong

[Frank Dellaert]

It's a bit hard without telling us where to look for differences :-) But, before you do, take a look at https://bitbucket.org/gtborg/gtsam/pull-requests/249/enable-both-tangent-and-manifold-imu-pre/diff

You can define a constant that should get the math from the paper. See ImuFactor.h:

#ifdef GTSAM_TANGENT_PREINTEGRATION
typedef TangentPreintegration PreintegrationType;
#else
typedef ManifoldPreintegration PreintegrationType;
#endif

Does it agree when you switch?

[Sasha Lambert]

@Sicongt were you able to match the Jacobians?

[Frank Dellaert]

No response from reporter