Running yet another dataset, after trying to set the camera’s focus (was not stabilized) and doing some configuration of the IMU.
I tried running dynamic_reconfigure but it did not recognize the %s_agc_enable argument with %s in {camera, cam0, logitech_camera, logitech}:
rosrun dynamic_reconfigure dynparam set /logitech_camera/image_mono/compressed "{'logitech_camera_agc_enable':0, 'logitech_camera_aec_enable':0,'logitech_camera_coarse_shutter_width':300}"
Then, I ran rosrun kalibr kalibr_camera_focus --topic /logitech_camera/image_mono and showed a Siemens star.
I’ve set the camera’s frequency to 20 Hz, and the IMU’s frequency to 200 Hz (by setting its period to 576 - see the documentation of mtdevice.py on the package’s page).
Now I am running on this newly generated dataset on my laptop (taking long). Meanwhile, I am upgrading the Robolab’s quadcore from Ubuntu 13.10 to 14.04, so I can install ROS Indigo with Kalibr.
Upgrading the pc, installing ROS Indigo and compiling and running Kalibr was faster than running Kalibr on my laptop. Output of the run:
$ rosrun kalibr kalibr_calibrate_imu_camera --time-calibration --bag bags/2014-09-15_calibration_MTi_Logitech.bag --cam repos/kalibr/config/camera/logitech.yaml --imu repos/kalibr/config/imu/MTx.yaml --target repos/kalibr/config/calibration_target/aprilgrid.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-15_calibration_MTi_Logitech.bag
Topic: /MTi/imu/data
Number of messages: 15378
Reading IMU data (/MTi/imu/data)
Read 15378 imu readings over 78.3 seconds
Initializing calibration target:
Type: aprilgrid
Tags:
Rows: 6
Cols: 6
Size: 0.024 [m]
Spacing 0.00700008 [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-15_calibration_MTi_Logitech.bag
Topic: /logitech_camera/image_mono
Number of images: 3012
Extracting calibration target corners
Extracted corners for 2838 images (of 3012 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 7824 knots (100.000000 knots per second over 78.239979 seconds)
Time shift camera to imu (t_imu = t_cam + shift):
-0.015267256558
Estimating imu-camera rotation prior
Initializing a pose spline with 7824 knots (100.000000 knots per second over 78.239979 seconds)
Orientation prior camera-imu found as: (T_i_c)
[[ 0.27988532 -0.7056156 -0.65097682]
[-0.07671698 0.65947008 -0.74780594]
[ 0.95696328 0.25924089 0.1304433 ]]
Gyro bias prior found as: (b_gyro)
[ 0.00095385 0.02348369 0.02932684]
Initializing a pose spline with 7832 knots (100.000000 knots per second over 78.319979 seconds)
Initializing the bias splines with 3916 knots
Adding camera error terms (/logitech_camera/image_mono)
Added 2838 camera error terms
Adding accelerometer error terms (/MTi/imu/data)
Added 15370 of 15378 accelerometer error terms (skipped 8 out-of-bounds measurements)
Adding gyroscope error terms (/MTi/imu/data)
Added 15370 of 15378 gyroscope error terms (skipped 8 out-of-bounds measurements)
Before Optimization
===================
Reprojection error squarred (cam0): mean 8.63857819034e+17, median 66.3410361425, std: 3.46196215528e+20
Gyro error squarred (imu0): mean 3.66015716939, median 0.0721432190082, std: 27.3440582321
Accelerometer error squarred (imu0): mean 27003999428.2, median 33483.180051, std: 8.52679713829e+11
Transformation T_cam0_imu0 (imu0 to cam0, T_ci): [m]
[[ 0.27988532 -0.7056156 -0.65097682 0. ]
[-0.07671698 0.65947008 -0.74780594 0. ]
[ 0.95696328 0.25924089 0.1304433 0. ]
[ 0. 0. 0. 1. ]]
cam0 to imu0 time: [s] (t_imu = t_cam + shift)
-0.015267256558
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 15683 design variables and 379148 error terms
The Jacobian matrix is 914348 x 70557
[0.0]: J: 2.94209e+23
CHOLMOD warning: matrix not positive definite
Cholmod warning: matrix not positive definite.Solution failed
[WARNING] System solution failed
CHOLMOD warning: matrix not positive definite
Cholmod warning: matrix not positive definite.Solution failed
[WARNING] System solution failed
CHOLMOD warning: matrix not positive definite
Cholmod warning: matrix not positive definite.Solution failed
[WARNING] System solution failed
CHOLMOD warning: matrix not positive definite
Cholmod warning: matrix not positive definite.Solution failed
[WARNING] System solution failed
CHOLMOD warning: matrix not positive definite
Cholmod warning: matrix not positive definite.Solution failed
[WARNING] System solution failed
Last step was a regression. Reverting
[1]: J: 4.40723e+23, dJ: -1.46514e+23, deltaX: 0.0973612, LM - lambda:1.04858e+08 mu:64
Last step was a regression. Reverting
[2]: J: 4.36755e+23, dJ: -1.42546e+23, deltaX: 0.015062, LM - lambda:1.34218e+10 mu:128
[3]: J: 4.94291e+22, dJ: 2.4478e+23, deltaX: 0.00271222, LM - lambda:3.43597e+12 mu:256
[4]: J: 8.8522e+21, dJ: 4.05769e+22, deltaX: 0.0043299, LM - lambda:1.14532e+12 mu:2
[5]: J: 1.44253e+21, dJ: 7.40967e+21, deltaX: 0.00234147, LM - lambda:8.35996e+11 mu:2
[6]: J: 2.31735e+20, dJ: 1.21079e+21, deltaX: 0.0011416, LM - lambda:5.72131e+11 mu:2
[7]: J: 3.70234e+19, dJ: 1.94711e+20, deltaX: 0.000491001, LM - lambda:3.86017e+11 mu:2
[8]: J: 5.9343e+18, dJ: 3.10891e+19, deltaX: 0.000189282, LM - lambda:2.58443e+11 mu:2
[9]: J: 9.43995e+17, dJ: 4.99031e+18, deltaX: 0.000206831, LM - lambda:1.72999e+11 mu:2
[10]: J: 1.49467e+17, dJ: 7.94528e+17, deltaX: 0.000235243, LM - lambda:1.15059e+11 mu:2
[11]: J: 2.38517e+16, dJ: 1.25615e+17, deltaX: 0.000269024, LM - lambda:7.6063e+10 mu:2
[12]: J: 4.12274e+15, dJ: 1.9729e+16, deltaX: 0.000304198, LM - lambda:4.97539e+10 mu:2
[13]: J: 1.03911e+15, dJ: 3.08362e+15, deltaX: 0.000334853, LM - lambda:3.16589e+10 mu:2
[14]: J: 5.48393e+14, dJ: 4.90719e+14, deltaX: 0.000352678, LM - lambda:1.8643e+10 mu:2
[15]: J: 4.64139e+14, dJ: 8.42537e+13, deltaX: 0.000368281, LM - lambda:8.79233e+09 mu:2
[16]: J: 4.48445e+14, dJ: 1.5694e+13, deltaX: 0.000397707, LM - lambda:3.19289e+09 mu:2
[17]: J: 4.44862e+14, dJ: 3.58315e+12, deltaX: 0.000427907, LM - lambda:1.27632e+09 mu:2
[18]: J: 4.36974e+14, dJ: 7.88759e+12, deltaX: 0.00275195, LM - lambda:4.2544e+08 mu:2
[19]: J: 4.23612e+14, dJ: 1.33626e+13, deltaX: 0.00845069, LM - lambda:1.41813e+08 mu:2
[20]: J: 4.17669e+14, dJ: 5.94313e+12, deltaX: 0.013509, LM - lambda:4.72711e+07 mu:2
[21]: J: 4.15858e+14, dJ: 1.81097e+12, deltaX: 0.0213645, LM - lambda:1.5757e+07 mu:2
[22]: J: 4.15099e+14, dJ: 7.58899e+11, deltaX: 0.0405975, LM - lambda:5.25235e+06 mu:2
[23]: J: 4.13545e+14, dJ: 1.55427e+12, deltaX: 0.209032, LM - lambda:1.75078e+06 mu:2
[24]: J: 4.0401e+14, dJ: 9.53422e+12, deltaX: 1.54635, LM - lambda:583594 mu:2
[25]: J: 3.59982e+14, dJ: 4.40283e+13, deltaX: 8.10981, LM - lambda:194531 mu:2
[26]: J: 2.35341e+14, dJ: 1.24641e+14, deltaX: 30.2471, LM - lambda:64843.8 mu:2
Exception in thread block: [aslam::Exception] /home/robolab/ros/repos/kalibr/src/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp:447: toTransformationMatrixImplementation() assert(_bufferTmin <= _time.toScalar() < _bufferTmax) failed [1.41078e+09 <= 1.41078e+09 < 1.41078e+09]: Spline Coefficient Buffer Exceeded. Set larger buffer margins!
Exception in thread block: [aslam::Exception] /home/robolab/ros/repos/kalibr/src/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp:447: toTransformationMatrixImplementation() assert(_bufferTmin <= _time.toScalar() < _bufferTmax) failed [1.41078e+09 <= 1.41078e+09 < 1.41078e+09]: Spline Coefficient Buffer Exceeded. Set larger buffer margins!
Exception in thread block: [aslam::Exception] /home/robolab/ros/repos/kalibr/src/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp:447: toTransformationMatrixImplementation() assert(_bufferTmin <= _time.toScalar() < _bufferTmax) failed [1.41078e+09 <= 1.41078e+09 < 1.41078e+09]: Spline Coefficient Buffer Exceeded. Set larger buffer margins!
Exception in thread block: [aslam::Exception] /home/robolab/ros/repos/kalibr/src/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp:447: toTransformationMatrixImplementation() assert(_bufferTmin <= _time.toScalar() < _bufferTmax) failed [1.41078e+09 <= 1.41078e+09 < 1.41078e+09]: Spline Coefficient Buffer Exceeded. Set larger buffer margins!
Exception in thread block: [aslam::Exception] /home/robolab/ros/repos/kalibr/src/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp:447: toTransformationMatrixImplementation() assert(_bufferTmin <= _time.toScalar() < _bufferTmax) failed [1.41078e+09 <= 1.41078e+09 < 1.41078e+09]: Spline Coefficient Buffer Exceeded. Set larger buffer margins!
Exception in thread block: [aslam::Exception] /home/robolab/ros/repos/kalibr/src/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp:447: toTransformationMatrixImplementation() assert(_bufferTmin <= _time.toScalar() < _bufferTmax) failed [1.41078e+09 <= 1.41078e+09 < 1.41078e+09]: Spline Coefficient Buffer Exceeded. Set larger buffer margins!
Exception in thread block: [aslam::Exception] /home/robolab/ros/repos/kalibr/src/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp:447: toTransformationMatrixImplementation() assert(_bufferTmin <= _time.toScalar() < _bufferTmax) failed [1.41078e+09 <= 1.41078e+09 < 1.41078e+09]: Spline Coefficient Buffer Exceeded. Set larger buffer margins!
[ERROR] [1410794477.280918]: Optimization failed!
Traceback (most recent call last):
File "/home/robolab/ros/repos/kalibr/src/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera", line 206, in <module>
main()
File "/home/robolab/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/robolab/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!