Catadioptric camera-imu system
cjxie opened this issue · 1 comments
cjxie commented
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
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.