search

gogojjh / M-LOAM

Robust Odometry and Mapping for Multi-LiDAR Systems with Online Extrinsic Calibration
http://gofile.me/4jm56/zU2yvg3bH
473 stars 88 forks source link

lidar_pure_odom_factor #8

Open narutojxl opened 3 years ago

narutojxl commented 3 years ago

Hi doctor jiao, Thanks for your hard work firsly! I'm curious about the jacobians in lidar_pure_odom_factor.hpp and lidar_online_calib_factor.hpp, so i carefully calculate them, according to right pertubation of SO(3) and the jacobian's type is numerator layout. Jacobians in lidar_online_calib_factor.hpp are all consistent with my result, but there are there mismatchs in lidar_pure_odom_factor.hpp. I'm not sure whether there are still some errors in my derivation, although i carefully checked the result twice. Thanks for your time and help a lot !

Jacobians of point to plane with point to line, the only difference is : In point-to-plane there is a w.transpose() in the front. In point-to-line w.transpose()is replaced by eta * Utility::skewSymmetric(ba - bb) in the code, according to link rules of derivate.

We can see jacobians of point to line residual wrt R_p, line 249, Rp.trans() is in the skewSymmetric.

gogojjh commented 3 years ago

I think the major difference is the choice of left/right perturbation on SO(3). Both choices are allowed since they are the approximations. But using the left perturbation can may the Jacobians simpler.

If you are not sure about the derived Jacobian, you can use check() for the verification. See estimator.cpp, search if (CHECK_JACOBIAN) for the usage.

Thanks.

narutojxl commented 3 years ago

Hi doctor jiao, If we use left perturbation to derive jacobian, the PoseLocalParameterization::Plus() , pose_local_parameterization.cpp#L42 , maybe changed to q_plus = (dq *q).normalized();, as it is a globally pertubation in hamilton. When we parameterization double raw pointer in ceres param blocks. I guess when you deriving jacobian according to right perturbation, consistent with my derivation process. Thanks for pointing me out the check() function, i will read it, thanks again!

narutojxl commented 3 years ago

Hi author, I find a interesting thing. I did two experiments, the test conditions are:

The first experiment is the raw code's output when frame_cnt_== 100 is:

optimization with pure odometry [LidarPureOdomEdgeFactor] check begins analytical: 0.053 -0.100 0.216 -0.971 -0.823 1.795 0.673 0.000 0.100 -0.216 0.971 0.786 -1.709 -0.637 0.000 0.004 -0.348 0.938 0.786 -1.709 -0.637 0.000 perturbation: 0.053 -0.100 0.216 -0.971 -0.823 1.795 0.673 0.100 -0.216 0.971 0.786 -1.709 -0.637 0.004 -0.348 0.938 0.786 -1.709 -0.637 The second experiment is my experiment, i change the point to line residual r wrt ΔR, lidar_pure_odom_factor.hpp#L274, to jaco_ex.rightCols<3>() = - eta * Utility::skewSymmetric(ba - bb) * Rp.transpose() * Ri * (Rext * Utility::skewSymmetric(point_)), namely my derivation jacobian, the output when frame_cnt_== 100 is:

optimization with pure odometry [LidarPureOdomEdgeFactor] check begins analytical: 0.081 0.914 -0.211 -0.348 1.360 -0.203 3.399 0.000 -0.914 0.211 0.348 -1.409 0.286 -3.432 0.000 -0.869 0.314 0.383 -1.409 0.286 -3.432 0.000 perturbation: 0.081 0.914 -0.211 -0.348 1.360 -0.203 3.399 -0.914 0.211 0.348 -1.409 0.286 -3.432 -0.869 0.314 0.383 -1.409 0.286 -3.432

It seems that we can not check the derived jacobian is correct or wrong by check() function. This is my derivation process. Thanks for your help and time.

gogojjh commented 3 years ago

Have you checked that the points (data for calculating the residuals) are the same in two cases?

narutojxl commented 3 years ago

No, the two case's source points are different. The analytical jacobians are the same with perturbation in both case, no matter what the jacobians are.

gogojjh commented 3 years ago

The Jacobians from analytical and perturbation are the same, meaning that the derived Jacobians are approximate. You can try to change the formulas of Jacobians and check later. They will be different.

narutojxl commented 3 years ago

2021-01-15 10-47-16屏幕截图

Because our jacobian in the point to line residual wrt ΔR exists a small difference, The difference is whether plus Utility::skewSymmetric(t_ext), i guess the influnence is small. But our jacobian in the point to plane residual wrt R_p exists a big difference, The difference is whether Rp.trans() in the skewSymmetric., So i did another two experiments for surf points jacobian.

Test conditions are:

raw code 3 experiments: In the 3 experiments, the analytical wrt R_p is different from perturbation wrt R_p.

########### raw code: surf point ########### size of finding laser_cloud: 17964 21258 lidarTracker: 6.489906ms [NON_LINEAR] build map: 9.367192ms optimization with online calibration [LidarPureOdomPlaneNormFactor] check begins raw point is(1.817, -10.328, -2.810) analytical: 0.040 0.450 -0.873 -0.188 -0.337 -0.351 0.823 0.000 -0.450 0.873 0.188 0.397 0.330 -0.956 0.000 -0.258 0.941 0.218 0.397 0.330 -0.956 0.000 perturbation: 0.040 0.450 -0.873 -0.188 -0.393 -0.301 0.818 -0.450 0.873 0.188 0.397 0.330 -0.956 -0.258 0.941 0.218 0.397 0.330 -0.956 Start Calibration ! 0 D factor: 0.000: 1.000 0.000 0.000 0.000 0.000 0.000 1 D factor: 139.652: -0.098 0.085 -0.979 -0.141 0.070 0.023 2 D factor: 121.216: -0.117 0.131 -0.972 -0.131 0.084 0.028 0: calib eig is 0.000000 1: calib eig is 28.953894 100.000 100.000 100.000 0.000 48.555 evaluate residual: 4.412340ms Ceres Solver Report: Iterations: 5, Initial cost: 2.654929e+02, Final cost: 2.652173e+02, Termination: NO_CONVERGENCE ceres solver costs: 14.114399ms whole marginalization costs: 9.780671ms laser_1, eligible calib size: 0 [ WARN] [1610677361.653727769, 1583835688.268045121]: cir_bufcnt = 4, Qs_.size()=5 frame: 100, odom process time: 52.694ms

###########    raw code:   surf point      ###########
size of finding laser_cloud: 17964 21258 
lidarTracker: 9.893504ms
[NON_LINEAR]
build map: 14.813927ms
optimization with online calibration
[LidarPureOdomPlaneNormFactor] check begins
raw point is(-27.480, -37.218, -12.396)
analytical:
-0.000
-0.160  -0.255   0.954 -43.329  19.963  -1.941   0.000
0.160   0.255  -0.954  38.459 -28.468   0.217   0.000
0.183   0.240  -0.954  38.459 -28.468   0.217   0.000
perturbation:
-0.000
-0.160  -0.255   0.954 -38.551  28.159  -0.800
0.160   0.255  -0.954  38.459 -28.468   0.217
0.183   0.240  -0.954  38.459 -28.468   0.217
Start Calibration !
0 D factor: 0.000: 1.000 0.000 0.000 0.000 0.000 0.000
1 D factor: 165.909: -0.106  0.136 -0.984 -0.043  0.024  0.016
2 D factor: 122.130: -0.116  0.128 -0.972 -0.131  0.085  0.028
0: calib eig is 0.000000
1: calib eig is 28.979010
100.000 100.000 100.000   0.000  48.716
evaluate residual: 4.773207ms
Ceres Solver Report: Iterations: 5, Initial cost: 2.624571e+02, Final cost: 2.622181e+02, Termination: NO_CONVERGENCE
ceres solver costs: 14.490101ms
whole marginalization costs: 8.687258ms
laser_1, eligible calib size: 0
[ WARN] [1610678978.611517602, 1583835688.280602612]: cir_buf_cnt_ = 4, Qs_.size()=5
frame: 100, odom process time: 66.411ms

###########    raw code:   surf point     ###########
size of finding laser_cloud: 17964 21258 
lidarTracker: 8.304935ms
[NON_LINEAR]
build map: 11.480876ms
optimization with online calibration
[LidarPureOdomPlaneNormFactor] check begins
raw point is(-27.480, -37.218, -12.396)
analytical:
-0.000
-0.159  -0.255   0.954 -43.323  19.984  -1.882   0.000
0.159   0.255  -0.954  38.458 -28.468   0.217   0.000
0.183   0.240  -0.953  38.458 -28.468   0.217   0.000
perturbation:
-0.000
-0.159  -0.255   0.954 -38.545  28.165  -0.881
0.159   0.255  -0.954  38.458 -28.468   0.217
0.183   0.240  -0.953  38.458 -28.468   0.217
Start Calibration !
0 D factor: 0.000: 1.000 0.000 0.000 0.000 0.000 0.000
1 D factor: 157.609: -0.116  0.159 -0.979 -0.047  0.020  0.017
2 D factor: 119.486:  0.125 -0.126  0.971  0.134 -0.086 -0.028
0: calib eig is 0.000000
1: calib eig is 29.628106
100.000 100.000 100.000   0.000  48.716
evaluate residual: 6.931444ms
Ceres Solver Report: Iterations: 5, Initial cost: 2.657323e+02, Final cost: 2.654116e+02, Termination: NO_CONVERGENCE
ceres solver costs: 12.067319ms
whole marginalization costs: 8.665480ms
laser_1, eligible calib size: 0
[ WARN] [1610679281.500481854, 1583835688.271937530]: cir_buf_cnt_ = 4, Qs_.size()=5
frame: 100, odom process time: 57.094ms

change point to plane jacobian with mine, 3 experiments: In the 3 experiments, the analytical wrt R_p is the same with perturbation wrt R_p.

########### our surf point ########### size of finding laser_cloud: 17964 21258 lidarTracker: 7.970606ms [NON_LINEAR] build map: 10.618513ms optimization with online calibration [LidarPureOdomPlaneNormFactor] check begins raw point is(1.817, -10.328, -2.810) analytical: 0.044 0.457 -0.869 -0.191 -0.406 -0.334 0.927 0.000 -0.457 0.869 0.191 0.402 0.361 -1.066 0.000 -0.268 0.939 0.216 0.402 0.361 -1.066 0.000 perturbation: 0.044 0.457 -0.869 -0.191 -0.406 -0.334 0.927 -0.457 0.869 0.191 0.402 0.361 -1.066 -0.268 0.939 0.216 0.402 0.361 -1.066 Start Calibration ! 0 D factor: 0.000: 1.000 0.000 0.000 0.000 0.000 0.000 1 D factor: 127.805: 0.111 -0.101 0.975 0.146 -0.074 -0.027 2 D factor: 117.959: -0.120 0.139 -0.969 -0.135 0.088 0.029 0: calib eig is 0.000000 1: calib eig is 28.724523 100.000 100.000 100.000 0.000 48.632 evaluate residual: 4.500181ms Ceres Solver Report: Iterations: 5, Initial cost: 2.618356e+02, Final cost: 2.616036e+02, Termination: NO_CONVERGENCE ceres solver costs: 13.804082ms whole marginalization costs: 8.637802ms laser_1, eligible calib size: 0 [ WARN] [1610676678.010147387, 1583835688.265840037]: cir_bufcnt = 4, Qs_.size()=5 frame: 100, odom process time: 55.005ms

###########    our  surf point   ###########
size of finding laser_cloud: 17964 21258 
lidarTracker: 10.016887ms
[NON_LINEAR]
build map: 14.010215ms
optimization with online calibration
[LidarPureOdomPlaneNormFactor] check begins
raw point is(1.817, -10.328, -2.810)
analytical:
0.042
0.438 -0.882 -0.175 -0.598 -0.309  0.701  0.000
-0.438  0.882  0.175  0.604  0.335 -0.842  0.000
-0.248  0.948  0.199  0.604  0.335 -0.842  0.000
perturbation:
0.042
0.438 -0.882 -0.175 -0.598 -0.309  0.701
-0.438  0.882  0.175  0.604  0.335 -0.842
-0.248  0.948  0.199  0.604  0.335 -0.842
Start Calibration !
0 D factor: 0.000: 1.000 0.000 0.000 0.000 0.000 0.000
1 D factor: 131.249: -0.102  0.106 -0.975 -0.145  0.073  0.024
2 D factor: 114.680:  0.127 -0.126  0.970  0.135 -0.087 -0.029
0: calib eig is 0.000000
1: calib eig is 29.147198
100.000 100.000 100.000   0.000  48.632
evaluate residual: 6.314779ms
Ceres Solver Report: Iterations: 5, Initial cost: 2.632974e+02, Final cost: 2.630406e+02, Termination: NO_CONVERGENCE
ceres solver costs: 13.676148ms
whole marginalization costs: 8.688113ms
laser_1, eligible calib size: 0
[ WARN] [1610681053.395534104, 1583835688.283631865]: cir_buf_cnt_ = 4, Qs_.size()=5
frame: 100, odom process time: 67.648ms

###########    our   surf point  ###########
size of finding laser_cloud: 17964 21258 
lidarTracker: 9.815224ms
[NON_LINEAR]
build map: 14.035856ms
optimization with online calibration
[LidarPureOdomPlaneNormFactor] check begins
raw point is(1.817, -10.328, -2.810)
analytical:
0.048
0.449 -0.877 -0.171 -0.629 -0.344  0.807  0.000
-0.449  0.877  0.171  0.628  0.368 -0.946  0.000
-0.258  0.946  0.197  0.628  0.368 -0.946  0.000
perturbation:
0.048
0.449 -0.877 -0.171 -0.629 -0.344  0.807
-0.449  0.877  0.171  0.628  0.368 -0.946
-0.258  0.946  0.197  0.628  0.368 -0.946
Start Calibration !
0 D factor: 0.000: 1.000 0.000 0.000 0.000 0.000 0.000
1 D factor: 137.559: -0.112  0.107 -0.976 -0.135  0.068  0.023
2 D factor: 121.488: -0.122  0.131 -0.971 -0.133  0.084  0.027
0: calib eig is 0.000000
1: calib eig is 29.169096
100.000 100.000 100.000   0.000  48.770
evaluate residual: 4.457903ms
Ceres Solver Report: Iterations: 5, Initial cost: 2.660389e+02, Final cost: 2.658291e+02, Termination: NO_CONVERGENCE
ceres solver costs: 14.873410ms
whole marginalization costs: 8.657054ms
laser_1, eligible calib size: 0
[ WARN] [1610681208.987375236, 1583835688.280467081]: cir_buf_cnt_ = 4, Qs_.size()=5
frame: 100, odom process time: 65.797ms
gogojjh commented 3 years ago

Thanks for your experiments, I will check later.