Converting the images only to grayscale did not do the trick. The Numpy array had shape (n, m, 1)
, so it was a 3D array. Reshaping the array to (n, m)
resolved the problem.
And a fourth pull request for Kalibr is created.
I still have not managed to calibrate the sensors:
rosrun kalibr kalibr_calibrate_imu_camera --time-calibration --bag bags/2014-09-03_calibration_MTi_Logitech_short.bag --cam repos/kalibr/config/camera/logitech.yaml --imu repos/kalibr/config/imu/MTx.yaml --target repos/kalibr/config/calibration_target/checkerboard.yaml
importing libraries
Initializing IMUs:
Update rate: 100
Accelerometer:
Noise density: 0.004
Noise density (discrete): 0.04
Random walk: 0.02
Gyroscope:
Noise density: 0.1
Noise density (discrete): 1.0
Random walk: 1.0
Initializing imu rosbag dataset reader:
Dataset: bags/2014-09-03_calibration_MTi_Logitech_short.bag
Topic: /MTi/imu/data
Number of messages: 3706
Reading IMU data (/MTi/imu/data)
Read 3706 imu readings over 37.0 seconds
Initializing calibration target:
Type: checkerboard
Rows
Count: 9
Distance: 0.0255 [m]
Cols
Count: 6
Distance: 0.0255 [m]
Initializing camera chain:
Camera chain - cam0:
Camera model: pinhole
Focal length: [552.357544, 319.346027]
Principal point: [553.012817, 242.821647]
Distortion model: radtan
Distortion coefficients: [-0.010014, -0.046626, 0.001439, -0.010307]
baseline: no data available
Initializing camera rosbag dataset reader:
Dataset: bags/2014-09-03_calibration_MTi_Logitech_short.bag
Topic: /logitech_camera/image_mono
Number of images: 362
Extracting calibration target corners
Extracted corners for 332 images (of 362 images)
Building the problem
Spline order: 6
Pose knots per second: 70
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 (m/s^2): -1.000000
Gyroscope Huber width (rad/s): -1.000000
Do time calibration: True
Max iterations: 30
Time offset padding: 0.020000
Estimating time shift camera to imu:
Initializing a pose spline with 3628 knots (100.000000 knots per second over 36.279248 seconds)
Time shift camera to imu (t_imu = t_cam + shift):
0.0699825957397
Estimating imu-camera rotation prior
Initializing a pose spline with 3628 knots (100.000000 knots per second over 36.279248 seconds)
Orientation prior camera-imu found as: (T_i_c)
[[ 0.02033651 -0.95293512 -0.30249147]
[ 0.08176357 0.30312578 -0.9494364 ]
[ 0.99644425 -0.00542456 0.0840799 ]]
Gyro bias prior found as: (b_gyro)
[ 0.00032935 0.01364942 0.00667651]
Initializing a pose spline with 3636 knots (100.000000 knots per second over 36.359248 seconds)
Initializing the bias splines with 1818 knots
Adding camera error terms (/logitech_camera/image_mono)
Added 332 camera error terms
Adding accelerometer error terms (/MTi/imu/data)
Added 3637 of 3706 accelerometer error terms (skipped 69 out-of-bounds measurements)
Adding gyroscope error terms (/MTi/imu/data)
Added 3637 of 3706 gyroscope error terms (skipped 69 out-of-bounds measurements)
Before Optimization
===================
Reprojection error squarred (cam0): mean 87.0119793457, median 20.0493732631, std: 187.52177023
Gyro error squarred (imu0): mean 0.0955577154315, median 0.00996923223342, std: 0.301478956037
Accelerometer error squarred (imu0): mean 11945.9977171, median 9706.04181133, std: 7834.11524383
Transformation T_cam0_imu0 (imu0 to cam0, T_ci): [m]
[[ 0.02033651 -0.95293512 -0.30249147 0. ]
[ 0.08176357 0.30312578 -0.9494364 0. ]
[ 0.99644425 -0.00542456 0.0840799 0. ]
[ 0. 0. 0. 1. ]]
cam0 to imu0 time: [s] (t_imu = t_cam + shift)
0.0699825957397
Optimizing...
No linear system solver set in the options. Defaulting to the sparse_cholesky solver
Using the sparse_cholesky linear system solver
Using the levenberg_marquardt trust region policy
No linear system solver set in the options. Defaulting to the sparse_cholesky solver
Using the sparse_cholesky linear system solver
Using the levenberg_marquardt trust region policy
Initializing
Optimization problem initialized with 7291 design variables and 28005 error terms
The Jacobian matrix is 121460 x 32793
[0.0]: J: 4.49354e+07
[1]: J: 3.14689e+06, dJ: 4.17885e+07, deltaX: 0.36895, LM - lambda:100 mu:2
Exception in thread block: [aslam::Exception] /home/pkok/ros/repos/kalibr/src/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp:447: toTransformationMatrixImplementation() assert(_bufferTmin <= _time.toScalar() < _bufferTmax) failed [1.40976e+09 <= 1.40976e+09 < 1.40976e+09]: Spline Coefficient Buffer Exceeded. Set larger buffer margins!
Exception in thread block: [aslam::Exception] /home/pkok/ros/repos/kalibr/src/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp:447: toTransformationMatrixImplementation() assert(_bufferTmin <= _time.toScalar() < _bufferTmax) failed [1.40976e+09 <= 1.40976e+09 < 1.40976e+09]: Spline Coefficient Buffer Exceeded. Set larger buffer margins!
[ERROR] [1410360054.521921]: Optimization failed!
Traceback (most recent call last):
File "/home/pkok/ros/repos/kalibr/src/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera", line 206, in <module>
main()
File "/home/pkok/ros/repos/kalibr/src/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera", line 182, in main
iCal.optimize(maxIterations=parsed.max_iter, recoverCov=parsed.recover_cov)
File "/home/pkok/ros/repos/kalibr/src/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccCalibrator.py", line 181, in optimize
raise RuntimeError("Optimization failed!")
RuntimeError: Optimization failed!
I’ve set up my quadcore PC to run Kalibr and ROS as well, so that might speed things up a bit, maybe. (Laptop has a 2-core AMD processor)