ethz-asl / kalibr

The Kalibr visual-inertial calibration toolbox
Other
4.18k stars 1.37k forks source link

Catadioptric camera-imu system #668

Closed cjxie closed 4 months ago

cjxie commented 5 months ago

Hi, it's not really an issue but seeking for some helps on my project. I have a robot system with a calibrated catadioptric camera system which follows the omni-radtan pattern and an imu. The problem is I would like to perform the imu-to-cam calibration, but it seems that 360 degree FOV camera is not supported by kalibr.

Log is here:

Initializing IMUs:
    Update rate: 100.0
    Accelerometer:
      Noise density: 0.0010994037631587815 
      Noise density (discrete): 0.010994037631587815 
      Random walk: 6.732403211512887e-05
    Gyroscope:
      Noise density: 0.0001070003276476196
      Noise density (discrete): 0.001070003276476196 
      Random walk: 2.0591776034736455e-06
  Initializing imu rosbag dataset reader:
      Dataset:          checkerboard_omni_imu.bag
      Topic:            /imu_raw
      Number of messages: 28041
  Reading IMU data (/imu_raw)
    Read 28041 imu readings over 280.4 seconds                         
  Initializing calibration target:
    Type: checkerboard
    Rows
      Count: 7
      Distance: 0.08 [m]
    Cols
      Count: 5
      Distance: 0.08 [m]
  Initializing camera chain:
  Camera chain - cam0:
    Camera model: omni
    Focal length: [694.7608918466028, 690.6802935490878]
    Principal point: [974.9069903035256, 580.6590391633499]
    Omni xi: 1.5225025077908436
    Distortion model: radtan
    Distortion coefficients: [-0.34410410700670163, 0.4695895881645599, -0.0004645861699824783, -0.001727048103209565]
    baseline: no data available
  Initializing camera rosbag dataset reader:
      Dataset:          checkerboard_omni_imu.bag
      Topic:            /camera/panoramic/image/compressed
  [ WARN] [1707173180.092826]: BagImageDatasetReader: truncated 2003 / 2260 images (frequency)
      Number of images: 2260
  Extracting calibration target corners
    Extracted corners for 173 images (of 257 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 27907 knots (100.000000 knots per second over 279.068799 seconds)
    Time shift camera to imu (t_imu = t_cam + shift):
  1.1600128956085944

  Estimating imu-camera rotation prior

  Initializing a pose spline with 27907 knots (100.000000 knots per second over 279.068799 seconds)
  Gravity was intialized to [8.42279946 5.00031478 0.47087604] [m/s^2]
    Orientation prior camera-imu found as: (T_i_c)
  [[ 0.93598863  0.03570475 -0.35021487]
   [ 0.00391869  0.99372476  0.11178434]
   [ 0.35200842 -0.10600125  0.92997516]]
    Gyro bias prior found as: (b_gyro)
  [-0.01063567 -0.00250998 -0.27605552]

  Initializing a pose spline with 27919 knots (100.000000 knots per second over 279.188799 seconds)

  Initializing the bias splines with 13959 knots

  Adding camera error terms (/camera/panoramic/image/compressed)
    Added 173 camera error terms                               

  Adding accelerometer error terms (/imu_raw)
    Added 27823 of 28041 accelerometer error terms (skipped 218 out-of-bounds measurements)

  Adding gyroscope error terms (/imu_raw)
    Added 27823 of 28041 gyroscope error terms (skipped 218 out-of-bounds measurements)

  Before Optimization
  ===================
  Normalized Residuals
  ----------------------------
  Reprojection error (cam0):     mean 765.5281384980374, median 852.6314692244414, std: 266.13352968949937
  Gyroscope error (imu0):        mean 691.4988464463628, median 439.41129200673237, std: 707.377705378033
  Accelerometer error (imu0):    mean 75371006293.73973, median 94646.02718153346, std: 421026312066.50055

  Residuals
  ----------------------------
  Reprojection error (cam0) [px]:     mean 765.5281384980374, median 852.6314692244414, std: 266.13352968949937
  Gyroscope error (imu0) [rad/s]:     mean 0.7399060313771182, median 0.47017152216784214, std: 0.7568964624607086
  Accelerometer error (imu0) [m/s^2]: mean 828631679.5240164, median 1040.5419845140618, std: 4628779118.747741

  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 55856 design variables and 61591 error terms
  The Jacobian matrix is 178826 x 251337
  [0.0]: J: 5.09005e+27
  CHOLMOD warning: matrix not positive definite. file: ../Supernodal/t_cholmod_super_numeric.c line: 911
  [WARNING] System solution failed
  CHOLMOD warning: matrix not positive definite. file: ../Supernodal/t_cholmod_super_numeric.c line: 911
  [WARNING] System solution failed
  CHOLMOD warning: matrix not positive definite. file: ../Supernodal/t_cholmod_super_numeric.c line: 911
  [WARNING] System solution failed
  CHOLMOD warning: matrix not positive definite. file: ../Supernodal/t_cholmod_super_numeric.c line: 911
  [WARNING] System solution failed
  [1]: J: 5.08717e+27, dJ: 2.87931e+24, deltaX: 1.18329e+06, LM - lambda:163840 mu:32
  Exception in thread block: [aslam::Exception] /catkin_ws/src/kalibr/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp:447: toTransformationMatrixImplementation() assert(_bufferTmin <= _time.toScalar() < _bufferTmax) failed [1.70716e+09 <= 1.70716e+09 < 1.70716e+09]: Spline Coefficient Buffer Exceeded. Set larger buffer margins!
  Exception in thread block: [aslam::Exception] /catkin_ws/src/kalibr/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp:447: toTransformationMatrixImplementation() assert(_bufferTmin <= _time.toScalar() < _bufferTmax) failed [1.70716e+09 <= 1.70716e+09 < 1.70716e+09]: Spline Coefficient Buffer Exceeded. Set larger buffer margins!
  [ERROR] [1707174277.562826]: std::exception
  [ERROR] [1707174277.565545]: Optimization failed!
  Traceback (most recent call last):
    File "/catkin_ws/devel/lib/kalibr/kalibr_calibrate_imu_camera", line 15, in <module>
      exec(compile(fh.read(), python_script, 'exec'), context)
    File "/catkin_ws/src/kalibr/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera", line 247, in <module>
      main()
    File "/catkin_ws/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 "/catkin_ws/src/kalibr/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccCalibrator.py", line 180, in optimize
      raise RuntimeError("Optimization failed!")
  RuntimeError: Optimization failed!

Corner extraction was fine but optimization failed. Is there any idea of how can I make it work? Any suggestion will be of great help.

cjxie commented 4 months ago

The issue is caused by unstable results of solvePnP while estimating the extrinsic of camera. Flipping the rotation matrix and translation vector temporarily fixes the issue.