search

ethz-asl / kalibr

The Kalibr visual-inertial calibration toolbox
Other
4.21k stars 1.38k forks source link

Optimization failed! #623

Closed XinZhanhua closed 1 year ago

XinZhanhua commented 1 year ago

Hello everyone! I have a question about the camera imu calibration. I recorded a camera-imu ROS package for calibration, but it fails every time when it comes to optimization. After reviewing the output logs, I found that the IMU residuals are extremely large. I have uploaded my bag files and output result files to Google Drive. If anyone could help me with this issue, I would greatly appreciate it. Thank you!

Here is the rosbag

rosbag

Here is the output:

huarsuma@ubuntu:~/kalibr_workspace$ rosrun kalibr kalibr_calibrate_imu_camera --bag /media/huarsuma/2FEA-9429/dataset/calib_imu_camera4_changed/cam_imu.bag --target src/Kalibr/config/april_6x6_80x80cm.yaml --imu src/Kalibr/config/imu.yaml --cam src/Kalibr/config/cam_april.yaml
importing libraries
the rosdep view is empty: call 'sudo rosdep init' and 'rosdep update'
Initializing IMUs:
  Update rate: 200.0
  Accelerometer:
    Noise density: 0.019562876341422137 
    Noise density (discrete): 0.2766608504106694 
    Random walk: 0.0007794986922470586
  Gyroscope:
    Noise density: 0.00152
    Noise density (discrete): 0.021496046148071046 
    Random walk: 0.00026645
Initializing imu rosbag dataset reader:
    Dataset:          /media/huarsuma/2FEA-9429/dataset/calib_imu_camera4_changed/cam_imu.bag
    Topic:            /imu0
    Number of messages: 16107
Reading IMU data (/imu0)
  Read 16107 imu readings over 81.4 seconds                          
Initializing calibration target:
  Type: aprilgrid
  Tags: 
    Rows: 6
    Cols: 6
    Size: 0.0303846 [m]
    Spacing 0.00911538 [m]
Initializing camera chain:
Camera chain - cam0:
  Camera model: pinhole
  Focal length: [431.6633, 432.8507]
  Principal point: [296.8663, 229.4059]
  Distortion model: radtan
  Distortion coefficients: [0.0737, -0.1457, 0.0017, 0.002]
  baseline: no data available
Initializing camera rosbag dataset reader:
    Dataset:          /media/huarsuma/2FEA-9429/dataset/calib_imu_camera4_changed/cam_imu.bag
    Topic:            /cam0/image_raw
    Number of images: 1598
Extracting calibration target corners
  Extracted corners for 1598 images (of 1598 images)                              

Building the problem
    Spline order: 6
    Pose knots per second: 100
    Do pose motion regularization: False
        xddot translation variance: 1000000.000000
        xddot rotation variance: 100000.000000
    Bias knots per second: 50
    Do bias motion regularization: True
    Blake-Zisserman on reprojection errors -1
    Acceleration Huber width (sigma): -1.000000
    Gyroscope Huber width (sigma): -1.000000
    Do time calibration: True
    Max iterations: 30
    Time offset padding: 0.030000
Estimating time shift camera to imu:

Initializing a pose spline with 7985 knots (100.000000 knots per second over 79.853872 seconds)
  Time shift camera to imu (t_imu = t_cam + shift):
46.10070873209812

Estimating imu-camera rotation prior

Initializing a pose spline with 7985 knots (100.000000 knots per second over 79.853872 seconds)
Gravity was intialized to [ 4.98111461 -1.77637872  8.25841381] [m/s^2]
  Orientation prior camera-imu found as: (T_i_c)
[[-0.61905352  0.20881201 -0.75708011]
 [-0.35882449 -0.93270459  0.03615422]
 [-0.69858266  0.29404028  0.65232092]]
  Gyro bias prior found as: (b_gyro)
[ -8.29103539  10.58256541 -18.82383482]

Initializing a pose spline with 7997 knots (100.000000 knots per second over 79.973872 seconds)

Initializing the bias splines with 3999 knots

Adding camera error terms (/cam0/image_raw)
  Added 1598 camera error terms                                  

Adding accelerometer error terms (/imu0)
  Added 6584 of 16107 accelerometer error terms (skipped 9523 out-of-bounds measurements)

Adding gyroscope error terms (/imu0)
  Added 6584 of 16107 gyroscope error terms (skipped 9523 out-of-bounds measurements)

Before Optimization
===================
Normalized Residuals
----------------------------
Reprojection error (cam0):     mean 0.6005738697711501, median 0.5037858922053948, std: 0.4213423959744383
Gyroscope error (imu0):        mean 2487.975141912056, median 1064.8388645937684, std: 9089.445166965334
Accelerometer error (imu0):    mean 33.884162433007205, median 10.992373811295707, std: 91.83414658304814

Residuals
----------------------------
Reprojection error (cam0) [px]:     mean 0.6005738697711501, median 0.5037858922053948, std: 0.4213423959744383
Gyroscope error (imu0) [rad/s]:     mean 53.48162846579517, median 22.88982537356722, std: 195.38713276944816
Accelerometer error (imu0) [m/s^2]: mean 9.374421194169031, median 3.041159486665041, std: 25.406913090404167

Optimizing...
Using the block_cholesky linear system solver
Using the levenberg_marquardt trust region policy
Using the block_cholesky linear system solver
Using the levenberg_marquardt trust region policy
Initializing
Optimization problem initialized with 16014 design variables and 234600 error terms
The Jacobian matrix is 482366 x 72045
[0.0]: J: 5.84775e+11
Exception in thread block: [aslam::Exception] /home/huarsuma/kalibr_workspace/src/Kalibr/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp:447: toTransformationMatrixImplementation() assert(_bufferTmin <= _time.toScalar() < _bufferTmax) failed [1.68725e+09 <= 1.68725e+09 < 1.68725e+10]: Spline Coefficient Buffer Exceeded. Set larger buffer margins!
Exception in thread block: [aslam::Exception] /home/huarsuma/kalibr_workspace/src/Kalibr/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp:447: toTransformationMatrixImplementation() assert(_bufferTmin <= _time.toScalar() < _bufferTmax) failed [1.68725e+09 <= 1.68725e+09 < 1.68725e+10]: Spline Coefficient Buffer Exceeded. Set larger buffer margins!
Exception in thread block: [aslam::Exception] /home/huarsuma/kalibr_workspace/src/Kalibr/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp:447: toTransformationMatrixImplementation() assert(_bufferTmin <= _time.toScalar() < _bufferTmax) failed [1.68725e+09 <= 1.68725e+09 < 1.68725e+10]: Spline Coefficient Buffer Exceeded. Set larger buffer margins!
Exception in thread block: [aslam::Exception] /home/huarsuma/kalibr_workspace/src/Kalibr/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp:447: toTransformationMatrixImplementation() assert(_bufferTmin <= _time.toScalar() < _bufferTmax) failed [1.68725e+09 <= 1.68725e+09 < 1.68725e+10]: Spline Coefficient Buffer Exceeded. Set larger buffer margins!Exception in thread block: 
[aslam::Exception] /home/huarsuma/kalibr_workspace/src/Kalibr/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp:447: toTransformationMatrixImplementation() assert(_bufferTmin <= _time.toScalar() < _bufferTmax) failed [1.68725e+09 <= 1.68725e+09 < 1.68725e+10]: Spline Coefficient Buffer Exceeded. Set larger buffer margins!
[ERROR] [1687319144.720857]: std::exception
[ERROR] [1687319144.734533]: Optimization failed!
Traceback (most recent call last):
  File "/home/huarsuma/kalibr_workspace/devel/lib/kalibr/kalibr_calibrate_imu_camera", line 15, in <module>
    exec(compile(fh.read(), python_script, 'exec'), context)
  File "/home/huarsuma/kalibr_workspace/src/Kalibr/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera", line 247, in <module>
    main()
  File "/home/huarsuma/kalibr_workspace/src/Kalibr/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera", line 209, in main
    iCal.optimize(maxIterations=parsed.max_iter, recoverCov=parsed.recover_cov)
  File "/home/huarsuma/kalibr_workspace/src/Kalibr/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccCalibrator.py", line 180, in optimize
    raise RuntimeError("Optimization failed!")
RuntimeError: Optimization failed!
XinZhanhua commented 1 year ago

I made a very foolish mistake, I got the angular velocity unit of IMU wrong. The modified bag file has been uploaded to Google drivecorrected rosbag But there are still strange errors in the results. The residual is still very large, and the time offset is very large, reaching 46 seconds. What's the matter?


huarsuma@ubuntu:~/kalibr_workspace$ rosrun kalibr kalibr_calibrate_imu_camera --bag /media/huarsuma/2FEA-9429/dataset/calib_imu_camera4_changed/cam_imu.bag --target src/Kalibr/config/april_6x6_80x80cm.yaml --imu src/Kalibr/config/imu.yaml --imu-models calibrated --cam src/Kalibr/config/cam_april.yaml

importing libraries
the rosdep view is empty: call 'sudo rosdep init' and 'rosdep update'
Initializing IMUs:
  Update rate: 200.0
  Accelerometer:
    Noise density: 0.019562876341422137 
    Noise density (discrete): 0.2766608504106694 
    Random walk: 0.0007794986922470586
  Gyroscope:
    Noise density: 0.00152
    Noise density (discrete): 0.021496046148071046 
    Random walk: 0.00026645
Initializing imu rosbag dataset reader:
    Dataset:          /media/huarsuma/2FEA-9429/dataset/calib_imu_camera4_changed/cam_imu.bag
    Topic:            /imu0
    Number of messages: 16106
Reading IMU data (/imu0)
  Read 16106 imu readings over 81.4 seconds                          
Initializing calibration target:
  Type: aprilgrid
  Tags: 
    Rows: 6
    Cols: 6
    Size: 0.0303846 [m]
    Spacing 0.00911538 [m]
Initializing camera chain:
Camera chain - cam0:
  Camera model: pinhole
  Focal length: [431.6633, 432.8507]
  Principal point: [296.8663, 229.4059]
  Distortion model: radtan
  Distortion coefficients: [0.0737, -0.1457, 0.0017, 0.002]
  baseline: no data available
Initializing camera rosbag dataset reader:
    Dataset:          /media/huarsuma/2FEA-9429/dataset/calib_imu_camera4_changed/cam_imu.bag
    Topic:            /cam0/image_raw
    Number of images: 1598
Extracting calibration target corners
  Extracted corners for 1598 images (of 1598 images)                              

Building the problem
    Spline order: 6
    Pose knots per second: 100
    Do pose motion regularization: False
        xddot translation variance: 1000000.000000
        xddot rotation variance: 100000.000000
    Bias knots per second: 50
    Do bias motion regularization: True
    Blake-Zisserman on reprojection errors -1
    Acceleration Huber width (sigma): -1.000000
    Gyroscope Huber width (sigma): -1.000000
    Do time calibration: True
    Max iterations: 30
    Time offset padding: 0.030000
Estimating time shift camera to imu:

Initializing a pose spline with 7985 knots (100.000000 knots per second over 79.853872 seconds)
  Time shift camera to imu (t_imu = t_cam + shift):
46.100755998326626

Estimating imu-camera rotation prior

Initializing a pose spline with 7985 knots (100.000000 knots per second over 79.853872 seconds)
Gravity was intialized to [ 4.97140371 -1.80382192  8.25831669] [m/s^2]
  Orientation prior camera-imu found as: (T_i_c)
[[-0.61791916  0.21273705 -0.75691404]
 [-0.36089683 -0.9320334   0.03266821]
 [-0.69851943  0.29335419  0.65269742]]
  Gyro bias prior found as: (b_gyro)
[-0.1514186   0.1915444  -0.32247083]

Initializing a pose spline with 7997 knots (100.000000 knots per second over 79.973872 seconds)

Initializing the bias splines with 3999 knots

Adding camera error terms (/cam0/image_raw)
  Added 1598 camera error terms                                  

Adding accelerometer error terms (/imu0)
  Added 6584 of 16106 accelerometer error terms (skipped 9522 out-of-bounds measurements)

Adding gyroscope error terms (/imu0)
  Added 6584 of 16106 gyroscope error terms (skipped 9522 out-of-bounds measurements)

Before Optimization
===================
Normalized Residuals
----------------------------
Reprojection error (cam0):     mean 0.6005728211832596, median 0.5037892572920444, std: 0.42134149672752996
Gyroscope error (imu0):        mean 47.382121815380835, median 22.016492078730955, std: 157.62288074705134
Accelerometer error (imu0):    mean 33.88387138440802, median 10.990480734307566, std: 91.83464262110566

Residuals
----------------------------
Reprojection error (cam0) [px]:     mean 0.6005728211832596, median 0.5037892572920444, std: 0.42134149672752996
Gyroscope error (imu0) [rad/s]:     mean 1.0185282771369502, median 0.4732675297430412, std: 3.3882687185305147
Accelerometer error (imu0) [m/s^2]: mean 9.374340672416068, median 3.0406357463756093, std: 25.407050324714994

Optimizing...
Using the block_cholesky linear system solver
Using the levenberg_marquardt trust region policy
Using the block_cholesky linear system solver
Using the levenberg_marquardt trust region policy
Initializing
Optimization problem initialized with 16014 design variables and 234600 error terms
The Jacobian matrix is 482366 x 72045
[0.0]: J: 2.41566e+08
Exception in thread block: [aslam::Exception] /home/huarsuma/kalibr_workspace/src/Kalibr/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp:447: toTransformationMatrixImplementation() assert(_bufferTmin <= _time.toScalar() < _bufferTmax) failed [1.68725e+09 <= 1.68725e+09 < 1.68725e+10]: Spline Coefficient Buffer Exceeded. Set larger buffer margins!
Exception in thread block: [aslam::Exception] /home/huarsuma/kalibr_workspace/src/Kalibr/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp:447: toTransformationMatrixImplementation() assert(_bufferTmin <= _time.toScalar() < _bufferTmax) failed [1.68725e+09 <= 1.68725e+09 < 1.68725e+10]: Spline Coefficient Buffer Exceeded. Set larger buffer margins!
Exception in thread block: [aslam::Exception] /home/huarsuma/kalibr_workspace/src/Kalibr/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp:447: toTransformationMatrixImplementation() assert(_bufferTmin <= _time.toScalar() < _bufferTmax) failed [1.68725e+09 <= 1.68725e+09 < 1.68725e+10]: Spline Coefficient Buffer Exceeded. Set larger buffer margins!
Exception in thread block: [aslam::Exception] /home/huarsuma/kalibr_workspace/src/Kalibr/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp:447: toTransformationMatrixImplementation() assert(_bufferTmin <= _time.toScalar() < _bufferTmax) failed [1.68725e+09 <= 1.68725e+09 < 1.68725e+10]: Spline Coefficient Buffer Exceeded. Set larger buffer margins!
Exception in thread block: [aslam::Exception] /home/huarsuma/kalibr_workspace/src/Kalibr/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp:447: toTransformationMatrixImplementation() assert(_bufferTmin <= _time.toScalar() < _bufferTmax) failed [1.68725e+09 <= 1.68725e+09 < 1.68725e+10]: Spline Coefficient Buffer Exceeded. Set larger buffer margins!
[ERROR] [1687344725.525347]: std::exception
[ERROR] [1687344725.538920]: Optimization failed!
Traceback (most recent call last):
  File "/home/huarsuma/kalibr_workspace/devel/lib/kalibr/kalibr_calibrate_imu_camera", line 15, in <module>
    exec(compile(fh.read(), python_script, 'exec'), context)
  File "/home/huarsuma/kalibr_workspace/src/Kalibr/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera", line 247, in <module>
    main()
  File "/home/huarsuma/kalibr_workspace/src/Kalibr/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera", line 209, in main
    iCal.optimize(maxIterations=parsed.max_iter, recoverCov=parsed.recover_cov)
  File "/home/huarsuma/kalibr_workspace/src/Kalibr/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccCalibrator.py", line 180, in optimize
    raise RuntimeError("Optimization failed!")
RuntimeError: Optimization failed!
XinZhanhua commented 1 year ago

Any help would be greatly appreciated

XinZhanhua commented 1 year ago

maybe it's the fault of my IMU. I got a relatively normal result after I use a new IMU. 

Calibration results
===================
Normalized Residuals
----------------------------
Reprojection error (cam0):     mean 0.19880245104856573, median 0.1703944043753489, std: 0.1490796066918056
Gyroscope error (imu0):        mean 0.6713623841075546, median 0.5730686981746765, std: 0.43804105545163835
Accelerometer error (imu0):    mean 0.2760301669874792, median 0.23789703939517942, std: 0.17220236201352984

Residuals
----------------------------
Reprojection error (cam0) [px]:     mean 0.19880245104856573, median 0.1703944043753489, std: 0.1490796066918056
Gyroscope error (imu0) [rad/s]:     mean 0.014431636790854992, median 0.012318711181977844, std: 0.009416150742738166
Accelerometer error (imu0) [m/s^2]: mean 0.07636674073775505, median 0.06581679722925086, std: 0.04764165191738911

Transformation (cam0):
-----------------------
T_ci:  (imu0 to cam0): 
[[ 0.99461901 -0.10359842  0.00062699 -0.04054825]
 [ 0.00208902  0.01400454 -0.99989975  0.02967977]
 [ 0.10357925  0.99452061  0.0141456  -0.13446845]
 [ 0.          0.          0.          1.        ]]

T_ic:  (cam0 to imu0): 
[[ 0.99461901  0.00208902  0.10357925  0.0541962 ]
 [-0.10359842  0.01400454  0.99452061  0.12911526]
 [ 0.00062699 -0.99989975  0.0141456   0.03160435]
 [ 0.          0.          0.          1.        ]]

timeshift cam0 to imu0: [s] (t_imu = t_cam + shift)
-0.015262041243016567

Gravity vector in target coords: [m/s^2]
[ 0.25747142 -9.79557915 -0.3856947 ]

Calibration configuration
=========================

cam0
-----
  Camera model: pinhole
  Focal length: [431.6633, 432.8507]
  Principal point: [296.8663, 229.4059]
  Distortion model: radtan
  Distortion coefficients: [0.0737, -0.1457, 0.0017, 0.002]
  Type: aprilgrid
  Tags: 
    Rows: 6
    Cols: 6
    Size: 0.0303846 [m]
    Spacing 0.00911538 [m]

IMU configuration
=================

IMU0:
 ----------------------------
  Model: calibrated
  Update rate: 200.0
  Accelerometer:
    Noise density: 0.019562876341422137 
    Noise density (discrete): 0.2766608504106694 
    Random walk: 0.0007794986922470586
  Gyroscope:
    Noise density: 0.00152
    Noise density (discrete): 0.021496046148071046 
    Random walk: 0.00026645
  T_ib (imu0 to imu0)
    [[1. 0. 0. 0.]
     [0. 1. 0. 0.]
     [0. 0. 1. 0.]
     [0. 0. 0. 1.]]
  time offset with respect to IMU0: 0.0 [s]
goldbattle commented 1 year ago

What was specifically wrong with the old IMU? Timesteps? Units?

XinZhanhua commented 1 year ago

What was specifically wrong with the old IMU? Timesteps? Units?

Thank you very much for your response. Both problems you mentioned have errors. but the main error is the timestamp error. After I changed the units, the residual is still very large. Then I changed the IMU and it finally looks good. By the way, I'm still not sure about the quality of my calibration result. The residual of the camera is about 0.15 px, and the residual of IMU is about 1e-2 of magnitude. How about the quality of calibration? The reason I ask this question is when I used the calibration result in ORBSLAM3, there is always a matching error in the mono-intertial mode, which will be OK in the mono SLAM mode. I'm debugging these days, but still confused. I am questioning whether the calibration results are accurate. THANK YOU for your help!

MaZhaoYong commented 2 months ago

What was specifically wrong with the old IMU? Timesteps? Units?

Thank you very much for your response. Both problems you mentioned have errors. but the main error is the timestamp error. After I changed the units, the residual is still very large. Then I changed the IMU and it finally looks good. By the way, I'm still not sure about the quality of my calibration result. The residual of the camera is about 0.15 px, and the residual of IMU is about 1e-2 of magnitude. How about the quality of calibration? The reason I ask this question is when I used the calibration result in ORBSLAM3, there is always a matching error in the mono-intertial mode, which will be OK in the mono SLAM mode. I'm debugging these days, but still confused. I am questioning whether the calibration results are accurate. THANK YOU for your help!

所以请问大哥你是换了个IMU就可以了？