Jacobian tests are failing

[lisusdaniil]

Getting multiple failures when running ctest related to jacobian_test file.

The following tests FAILED:
3 - ConstAcc.PoseInterpolator (Failed)
6 - Singer.PoseInterpolator (Failed)
11 - IMU.PreintIMUCostTerm (Failed)
14 - P2P.P2PGlobalSuper (Failed)
15 - ConstVel.VelocityInterpolator (Failed)
Errors while running CTest

[keenan-burnett]

Try adjusting the abs / rel thresholds to make the unit test pass? As long as visual inspection confirms that the values are close.

[lisusdaniil]

Seems to be a mix of things. Only actual concerning one is the PreintIMUCostTerm term and maybe VelocityInterpolator (much less so though). I'll try to take a look at lgmath to see where the orientation component issue for onstAcc.PoseInterpolator & Singer.PoseInterpolator & P2P.P2PGlobalSuper come from in the next little bit, but those ones I'd appreciate if you could take a look at since you're more familiar with the math. I created a branch for this issue.

ConstAcc.PoseInterpolator & Singer.PoseInterpolator & P2P.P2PGlobalSuper

The orientation components of the numerical Jacobian for w1/w2/dw1/dw2 are missing/are zeros. This is almost certainly a tolerancing/conversion issue in lgmath. I reverted to older lgmath hashes, but it was still present in some form. Same thing for P2P.P2PGlobalSuper, but with bg Jacobians.

w2:
  -0.0156249  2.92652e-05 -2.93286e-05  1.26953e-07  2.92335e-05 -2.93604e-05
-2.93286e-05   -0.0156249  2.92652e-05 -2.93604e-05  1.26953e-07  2.92335e-05
 2.92652e-05 -2.93286e-05   -0.0156249  2.92335e-05 -2.93604e-05  1.26953e-07
           0            0            0   -0.0156249  2.92652e-05 -2.93286e-05
           0            0            0 -2.93286e-05   -0.0156249  2.92652e-05
           0            0            0  2.92652e-05 -2.93286e-05   -0.0156249
  -0.0156249  2.92652e-05 -2.93286e-05  1.26953e-07  2.92335e-05 -2.93604e-05
-2.93286e-05   -0.0156249  2.92652e-05 -2.93604e-05  1.26953e-07  2.92335e-05
 2.92652e-05 -2.93286e-05   -0.0156249  2.92335e-05 -2.93604e-05  1.26953e-07
           0            0            0            0            0            0
           0            0            0            0            0            0
           0            0            0            0            0            0

IMU.PreintIMUCostTerm

The covariance test fails quite badly

Preint_cov:
 6.24994e-05  9.71185e-12 -1.85321e-11  1.01237e-07 -8.70933e-07 -1.27549e-08  3.13946e-06 -2.70523e-05 -1.11369e-08
 9.71185e-12  6.24994e-05 -5.00082e-11  8.71968e-07  1.00778e-07 -8.39857e-08  2.70856e-05  3.12763e-06 -2.69222e-06
-1.85321e-11 -5.00082e-11  6.24999e-05  5.73812e-09   9.5459e-08 -1.89492e-10  -1.9782e-07  2.98546e-06 -1.18909e-08
 1.01237e-07  8.71968e-07  5.73812e-09  2.31253e-07  9.96996e-12 -2.22785e-09  3.73493e-06  2.45669e-10 -5.93505e-08
-8.70933e-07  1.00778e-07   9.5459e-08  9.96996e-12  2.31463e-07  1.09553e-10 -3.38751e-10  3.74058e-06 -3.49297e-09
-1.27549e-08 -8.39857e-08 -1.89492e-10 -2.22785e-09  1.09553e-10  2.08026e-07  -5.8377e-08  2.74225e-09  3.13069e-06
 3.13946e-06  2.70856e-05  -1.9782e-07  3.73493e-06 -3.38751e-10  -5.8377e-08  7.93846e-05  -1.2239e-08  -1.6504e-06
-2.70523e-05  3.12763e-06  2.98546e-06  2.45669e-10  3.74058e-06  2.74225e-09  -1.2239e-08  7.95428e-05 -1.22703e-07
-1.11369e-08 -2.69222e-06 -1.18909e-08 -5.93505e-08 -3.49297e-09  3.13069e-06  -1.6504e-06 -1.22703e-07  6.26635e-05

Sigma:
 6.22034e-05 -1.35491e-06 -1.28769e-06  8.56451e-08 -8.42661e-07 -1.73997e-08  2.73352e-06 -2.67481e-05   2.1326e-07
-1.35491e-06  6.28188e-05 -6.37365e-07  8.14682e-07  1.46391e-07 -6.15646e-08  2.69523e-05  3.89634e-06  -2.1313e-06
-1.28769e-06 -6.37365e-07  6.15448e-05 -3.89968e-08  1.85527e-07 -2.05873e-08 -4.28648e-07  4.30766e-06 -1.64394e-07
 8.56451e-08  8.14682e-07 -3.89968e-08  2.30398e-07  9.62866e-10 -2.30408e-09  3.72334e-06  1.08695e-08 -4.88989e-08
-8.42661e-07  1.46391e-07  1.85527e-07  9.62866e-10  2.33693e-07 -2.29645e-09  1.02136e-08   3.7675e-06 -3.11577e-08
-1.73997e-08 -6.15646e-08 -2.05873e-08 -2.30408e-09 -2.29645e-09  2.02123e-07 -8.82546e-08 -1.61386e-08   3.0115e-06
 2.73352e-06  2.69523e-05 -4.28648e-07  3.72334e-06  1.02136e-08 -8.82546e-08  7.95089e-05 -1.61448e-09  -1.9119e-06
-2.67481e-05  3.89634e-06  4.30766e-06  1.08695e-08   3.7675e-06 -1.61386e-08 -1.61448e-09  7.95829e-05 -3.16517e-07
  2.1326e-07  -2.1313e-06 -1.64394e-07 -4.88989e-08 -3.11577e-08   3.0115e-06  -1.9119e-06 -3.16517e-07  6.06551e-05
/home/dl/Documents/phd/dev/steam/tests/jacobian_test.cpp:2817: Failure
Expected: (fabs((Sigma(i, j) - preint_cov(i, j)) / preint_cov(i, j))) < (0.01), actual: 139512 vs 0.01
/home/dl/Documents/phd/dev/steam/tests/jacobian_test.cpp:2817: Failure
Expected: (fabs((Sigma(i, j) - preint_cov(i, j)) / preint_cov(i, j))) < (0.01), actual: 69483.1 vs 0.01
/home/dl/Documents/phd/dev/steam/tests/jacobian_test.cpp:2817: Failure
Expected: (fabs((Sigma(i, j) - preint_cov(i, j)) / preint_cov(i, j))) < (0.01), actual: 0.154017 vs 0.01
/home/dl/Documents/phd/dev/steam/tests/jacobian_test.cpp:2817: Failure
Expected: (fabs((Sigma(i, j) - preint_cov(i, j)) / preint_cov(i, j))) < (0.01), actual: 0.0324608 vs 0.01

P2P.P2PGlobal

This guy is just fully broken

[ RUN      ] P2P.P2PGlobal
jacobian_test: /usr/include/eigen3/Eigen/src/Core/Product.h:95: Eigen::Product<Lhs, Rhs, Option>::Product(const Lhs&, const Rhs&) [with _Lhs = Eigen::Matrix<double, -1, -1>; _Rhs = Eigen::Matrix<double, 1, 6>; int Option = 0; Eigen::Product<Lhs, Rhs, Option>::Lhs = Eigen::Matrix<double, -1, -1>; Eigen::Product<Lhs, Rhs, Option>::Rhs = Eigen::Matrix<double, 1, 6>]: Assertion `lhs.cols() == rhs.rows() && "invalid matrix product" && "if you wanted a coeff-wise or a dot product use the respective explicit functions"' failed.
Aborted (core dumped)

ConstVel.VelocityInterpolator

This guy is preetttty close, but feels a bit high. Likely some small error would be my guess? Maybe a group Jacobian somewhere or something... so probably fine to ignore? All of the Jacobians here are a bit outside the 0.02 bound.

T1:
 -14.9625  0.730248 -0.767721 0.0748906  0.709509   -0.7844
-0.767721  -14.9625  0.730248   -0.7844 0.0748906  0.709509
 0.730248 -0.767721  -14.9625  0.709509   -0.7844 0.0748906
        0         0         0  -14.9625  0.730248 -0.767721
        0         0         0 -0.767721  -14.9625  0.730248
        0         0         0  0.730248 -0.767721  -14.9625
 -14.9667  0.732638 -0.765956 0.0666042  0.714589 -0.781194
-0.765956  -14.9667  0.732638 -0.781194 0.0666042  0.714589
 0.732638 -0.765956  -14.9667  0.714589 -0.781194 0.0666042
        0         0         0  -14.9667  0.732638 -0.765956
        0         0         0 -0.765956  -14.9667  0.732638
        0         0         0  0.732638 -0.765956  -14.9667
/home/dl/Documents/phd/dev/steam/tests/jacobian_test.cpp:2304: Failure
Expected: (fabs((njac(i, j) - ajac(i, j)) / ajac(i, j))) < (0.02), actual: 0.110647 vs 0.02
/home/dl/Documents/phd/dev/steam/tests/jacobian_test.cpp:2304: Failure
Expected: (fabs((njac(i, j) - ajac(i, j)) / ajac(i, j))) < (0.02), actual: 0.110647 vs 0.02
/home/dl/Documents/phd/dev/steam/tests/jacobian_test.cpp:2304: Failure
Expected: (fabs((njac(i, j) - ajac(i, j)) / ajac(i, j))) < (0.02), actual: 0.110647 vs 0.02

[keenan-burnett]

The ConstVel.VelocityInterpolator Jacobians use a first-order approximation of the SE(3) Jacobian, so it's not surprising that this is slightly off.

[keenan-burnett]

My docker image got nuked from obelisk, so I can't run anything.

[keenan-burnett]

Do you have an image I can run with?

[lisusdaniil]

Do you have an image I can run with?

I'm just building steam locally and running ctest's

[keenan-burnett]

I have a commit hash for steam and lgmath that I used within the steam_icp repo when I last looked at this:
https://github.com/utiasASRL/steam_icp

[keenan-burnett]

Just for some context,
P2PGlobal was created for doing IMU preintegration with C_iv and r_vi_in_i instead of the usual T_vi that we normally do in steam.

[keenan-burnett]

IMU.PreintIMUCostTerm:
If I remember correctly, I copied the math from the Forster et al. paper correctly, their approach just fails the unit test :)

[lisusdaniil]

I have a commit hash for steam and lgmath that I used within the steam_icp repo when I last looked at this: https://github.com/utiasASRL/steam_icp

Sorry I've been doing all development locally, so I dont have an image ready to go. Any of the doppler_radar ones on Obelisk should probably be fine

[keenan-burnett]

Ok, I revived my docker image steam_icp:latest

As expected, with my lgmath and steam commit hashes, the only two that fail are: IMU.PreintIMUCostTerm and ConstVel.VelocityInterpolator

[  FAILED  ] ConstVel.VelocityInterpolator (1 ms)
[----------] 1 test from ConstVel (1 ms total)

[----------] Global test environment tear-down
[==========] 13 tests from 5 test suites ran. (217 ms total)
[  PASSED  ] 11 tests.
[  FAILED  ] 2 tests, listed below:
[  FAILED  ] IMU.PreintIMUCostTerm
[  FAILED  ] ConstVel.VelocityInterpolator

[keenan-burnett]

so, some change since those commit hashes resulted in a regression.

[keenan-burnett]

I'm using

lgmath: https://github.com/utiasASRL/lgmath/tree/c97d13a883bca8915d94093e0c5f80236a889274
steam: https://github.com/utiasASRL/steam/tree/8fdbf0ad6f2c099e874a6c6eea0b8d6861628efe

[lisusdaniil]

Cool, I can look into the orientation component-related failures