xtsxisaxns / multicam_calibration

multicam_calibration - extrinsic and intrinsic calbration of cameras

Installation

Download the package:

mkdir catkin_ws
cd catkin_ws
mkdir src
cd src
git clone https://github.com/KumarRobotics/multicam_calibration.git

You will need the following packages in your ROS workspace:

git clone https://github.com/catkin/catkin_simple
git clone https://github.com/versatran01/apriltag.git

And probably libceres:

sudo apt install libceres-dev

What else you are missing you'll find out now:

cd catkin_ws
catkin config -DCMAKE_BUILD_TYPE=Release
catkin build

How to use:

First, produce the best starting guess you can come up with, and edit it into calib/example/example_camera-initial.yaml:

cam0:
  camera_model: pinhole
  intrinsics: [605.054, 604.66, 641.791, 508.728]
  distortion_model: equidistant
  distortion_coeffs: [-0.0146915, 0.000370396, -0.00425216, 0.0015107]
  resolution: [1280, 1024]
  rostopic: /rig/left/image_mono
cam1:
  T_cn_cnm1:
  - [ 0.99999965648, -0.00013331925,  0.00081808159, -0.19946344647]
  - [ 0.00013388601,  0.99999975107, -0.00069277676, -0.00005674605]
  - [-0.00081798903,  0.00069288605,  0.99999942540,  0.00010022941]
  - [ 0.00000000000,  0.00000000000,  0.00000000000,  1.00000000000]
  camera_model: pinhole
  intrinsics: [605.097, 604.321, 698.772, 573.558]
  distortion_model: equidistant
  distortion_coeffs: [-0.0146155, -0.00291494, -0.000681981, 0.000221855]
  resolution: [1280, 1024]
  rostopic: /rig/right/image_mono

Adjust the topics to match your camera sources.

You must use an aprilgrid target for calibration, layout follows Kalibr conventions and is specified in config/aprilgrid.yaml.

Then launch the camera calibration:

roslaunch multicam_calibration calibration.launch

You can see the camera images and the detected tags overlaid with any of the ros image visualization tools. There is a sample perspective file in the config directory:

rqt --perspective-file=config/example.perspective

Then play your calibration bag (or do live calibration):

rosbag play falcam_rig_2018-01-09-14-28-56.bag

You should see the tags detected, and output like this on the terminal:

type is multicam_calibration/CalibrationNodelet
[ INFO] [1515674455.127216052]: added camera: cam0
[ INFO] [1515674455.130332740]: added camera: cam1
[ INFO] [1515674455.131238617]: not using approximate sync
[ INFO] [1515674455.132790610]: writing extracted corners to file corners.csv
[ INFO] [1515674458.646217104]: frame number:   10, total number of tags found:  349 336
[ INFO] [1515674459.958243084]: frame number:   20, total number of tags found:  698 686
[ INFO] [1515674461.349852261]: frame number:   30, total number of tags found:  1048 1036
... more lines here ....
[ WARN] [1515674512.667679323]: no detections found, skipping frame!
[ INFO] [1515674512.757430315]: frame number:  410, total number of tags found:  11896 13300

When you think you have enough frames collected, you can start the calibration:

rosservice call /multicam_calibration/calibration

This should give you output like this:

Num params: 2476
Num residuals: 201928
iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
0  4.478809e+03    0.00e+00    5.32e+06   0.00e+00   0.00e+00  1.00e+04        0    2.45e-01    3.10e-01
1  1.291247e+03    3.19e+03    2.03e+05   1.46e+00   1.55e+00  3.00e+04        1    5.11e-01    8.21e-01
2  1.288842e+03    2.40e+00    6.22e+03   2.38e-01   1.04e+00  9.00e+04        1    4.56e-01    1.28e+00
3  1.288794e+03    4.79e-02    3.19e+02   3.57e-02   1.02e+00  2.70e+05        1    4.37e-01    1.71e+00
4  1.288792e+03    2.27e-03    3.73e+01   7.64e-03   1.01e+00  8.10e+05        1    4.38e-01    2.15e+00
5  1.288792e+03    2.61e-05    5.09e+00   7.20e-04   1.01e+00  2.43e+06        1    4.38e-01    2.59e+00
6  1.288792e+03    6.92e-08    5.35e-01   3.46e-05   1.03e+00  7.29e+06        1    4.37e-01    3.03e+00

Solver Summary (v 1.12.0-eigen-(3.2.92)-lapack-suitesparse-(4.4.6)-cxsparse-(3.1.4)-openmp)

Original                  Reduced
Parameter blocks                          410                      410
Parameters                               2476                     2476
Residual blocks                           409                      409
Residual                               201928                   201928

Minimizer                        TRUST_REGION

Sparse linear algebra library    SUITE_SPARSE
Trust region strategy     LEVENBERG_MARQUARDT

Given                     Used
Linear solver          SPARSE_NORMAL_CHOLESKY   SPARSE_NORMAL_CHOLESKY
Threads                                     4                        4
Linear solver threads                       1                        1
Linear solver ordering              AUTOMATIC                      410

Cost:
Initial                          4.478809e+03
Final                            1.288792e+03
Change                           3.190017e+03

Minimizer iterations                        7
Successful steps                            7
Unsuccessful steps                          0

Time (in seconds):
Preprocessor                           0.0653

Residual evaluation                  0.0680
Jacobian evaluation                  1.4113
Linear solver                        1.5961
Minimizer                              3.2011

Postprocessor                          0.0000
Total                                  3.2663

Termination:                      CONVERGENCE (Function tolerance reached. |cost_change|/cost: 1.930077e-13 <= 1.000000e-12)

[ INFO] [1515674589.074056064]: writing calibration to /home/pfrommer/Documents/foo/src/multicam_calibration/calib/example/example_camera-2018-01-11-07-43-09.yaml
cam0:
camera_model: pinhole
intrinsics: [604.355, 604.153, 642.488, 508.135]
distortion_model: equidistant
distortion_coeffs: [-0.014811, -0.00110814, -0.00137418, 0.000474477]
resolution: [1280, 1024]
rostopic: /rig/left/image_mono
cam1:
T_cn_cnm1:
- [ 0.99999720028,  0.00030730438,  0.00234627487, -0.19936845450]
- [-0.00030303357,  0.99999829718, -0.00182038902,  0.00004464487]
- [-0.00234683029,  0.00181967292,  0.99999559058,  0.00029671670]
- [ 0.00000000000,  0.00000000000,  0.00000000000,  1.00000000000]
camera_model: pinhole
intrinsics: [604.364, 603.62, 698.645, 573.02]
distortion_model: equidistant
distortion_coeffs: [-0.0125438, -0.00503567, 0.00031359, 0.000546495]
resolution: [1280, 1024]
rostopic: /rig/right/image_mono
[ INFO] [1515674589.251025662]: ----------------- reprojection errors: ---------------
[ INFO] [1515674589.251045482]: total error:     0.283519 px
[ INFO] [1515674589.251053450]: avg error cam 0: 0.28266 px
[ INFO] [1515674589.251059520]: avg error cam 1: 0.284286 px
[ INFO] [1515674589.251070091]: max error: 8.84058 px at frame: 110 for cam: 1
[ INFO] [1515674589.251410620]: -------------- simple homography test ---------
[ INFO] [1515674589.331235450]: camera: 0 points: 47700 reproj err: 0.440283
[ INFO] [1515674589.331257726]: camera: 1 points: 53252 reproj err: 0.761365

In the calib/example directory you can now find the output of the calibration:

ls -1
example_camera-2018-01-11-08-24-22.yaml
example_camera-initial.yaml
example_camera-latest.yaml

Managed calibrations

Sometimes a calibration consists of a sequence of steps, for example: first the intrinsics of each sensor, then the extrinsics of the sensors with respect to each other. This is particularly useful when image data between sensors is not synchronized.

To help with this, you can write a little python program that does that. In fact, you just have to modify the section below in src/example_calib_manager.py, and voila, when you trigger your calibration manager, it will in turn run multiple calibrations via service calls into the calibration node, each time retaining the previous calibration's output as initial value. Here is an example section, adjust as needed:

    # first do intrinsics of cam0
    set_p(FIX_INTRINSICS, "cam0", False)
    set_p(FIX_EXTRINSICS, "cam0", True)
    set_p(SET_ACTIVE,     "cam0", True)
    set_p(FIX_INTRINSICS, "cam1", True)
    set_p(FIX_EXTRINSICS, "cam1", True)
    set_p(SET_ACTIVE,     "cam1", False)
    run_cal()
    # then do intrinsics of cam1
    set_p(FIX_INTRINSICS, "cam0", True)
    set_p(FIX_EXTRINSICS, "cam0", True)
    set_p(SET_ACTIVE,     "cam0", False)
    set_p(FIX_INTRINSICS, "cam1", False)
    set_p(FIX_EXTRINSICS, "cam1", True)
    set_p(SET_ACTIVE,     "cam1", True)
    run_cal()
    # now extrinsics between the two
    set_p(FIX_INTRINSICS, "cam0", True)
    set_p(FIX_EXTRINSICS, "cam0", True)
    set_p(SET_ACTIVE,     "cam0", True)
    set_p(FIX_INTRINSICS, "cam1", True)
    set_p(FIX_EXTRINSICS, "cam1", False)
    set_p(SET_ACTIVE,     "cam1", True)
    run_cal()

Unit tests

For unit testing of the calibration code, refer to this page.