CameraImages: support reading multicamera image and calibration files

[matlabbe]

CameraImages and CameraRGBDImages are now able to run a folder of RGB / Depth / camera calibration files for multi-camera setup. The format supported is the same that is exported by DatabaseViewer->Extract images... and rtabmap-export --images when multi-camera data is in the database. Example of supported format for a 4 cameras rig data:

rgb/1.jpg    # e.g., 2560x400 (4x 640x400) RGB or Grayscale 
rgb/2.jpg    # e.g., 2560x400 (4x 640x400) RGB or Grayscale (4x 640x400)
...

depth/1.png    # e.g., 2560x400 (4x 640x400) 16UC1 (mm) or 32FC1 (meters) (4x 640x400)
depth/2.png    # e.g., 2560x400 (4x 640x400) 16UC1 (mm) or 32FC1 (meters) (4x 640x400)
...

calib/1_0.yaml   # calibration for camera 0 of frame 1 (should include base_link->camera0_optical_frame)
calib/1_1.yaml   # calibration for camera 1 of frame 1 (should include base_link->camera1_optical_frame)
calib/1_2.yaml   # calibration for camera 2 of frame 1 (should include base_link->camera2_optical_frame)
calib/1_3.yaml   # calibration for camera 3 of frame 1 (should include base_link->camera3_optical_frame)
calib/2_0.yaml   # calibration for camera 0 of frame 2 (should include base_link->camera0_optical_frame)
calib/2_1.yaml   # calibration for camera 1 of frame 2 (should include base_link->camera1_optical_frame)
calib/2_2.yaml   # calibration for camera 2 of frame 2 (should include base_link->camera2_optical_frame)
calib/2_3.yaml   # calibration for camera 3 of frame 2 (should include base_link->camera3_optical_frame)
...

With timestamps as filenames:

rgb/1725904699.141207.jpg
rgb/1725904700.227095.jpg
...

depth/1725904699.141207.png
depth/1725904700.227095.png
...

calib/1725904699.141207_0.yaml
calib/1725904699.141207_1.yaml
calib/1725904699.141207_2.yaml
calib/1725904699.141207_3.yaml
calib/1725904700.227095_0.yaml
calib/1725904700.227095_1.yaml
calib/1725904700.227095_2.yaml
calib/1725904700.227095_3.yaml
...

where rgb and depth images are concatenated images of all the cameras (like shown in rtabmap viewer).

The calibration for each camera would look like this (using same format than ROS camera info, exported from OpenCV in yaml format). One important thing that is not standard is the local_transform 3x4 transformation matrix. This defines the pose of each camera accordingly to base frame (e.g., base_link), including optical rotation.

1725904699.141207_0.yaml

%YAML:1.0
---
camera_name: "1725904699.141207_0"
image_width: 640
image_height: 400
camera_matrix:
   rows: 3
   cols: 3
   data: [ 2.8432208251953125e+02, 0., 3.1247360229492188e+02, 0.,
       2.8412954711914062e+02, 2.0666313171386719e+02, 0., 0., 1. ]
distortion_coefficients:
   rows: 1
   cols: 8
   data: [ 3.4420207142829895e-01, -3.3460360020399094e-02,
       -2.1579016174655408e-04, 2.2867985535413027e-05,
       -1.2446481268852949e-03, 6.7570692300796509e-01,
       -1.4408667339012027e-03, -7.9864347353577614e-03 ]
distortion_model: rational_polynomial
rectification_matrix:
   rows: 3
   cols: 3
   data: [ 9.9997033525948409e-01, -1.7602703616384570e-03,
       7.4986698346391047e-03, 1.7500374888413319e-03,
       9.9999752890452498e-01, 1.3709681364426384e-03,
       -7.5010645792873420e-03, -1.3578045137015477e-03,
       9.9997094477643700e-01 ]
projection_matrix:
   rows: 3
   cols: 4
   data: [ 2.5173199438139457e+02, 0., 2.9296909332275391e+02, 0., 0.,
       2.5173199438139457e+02, 2.1775932693481445e+02, 0., 0., 0., 1.,
       0. ]
local_transform:
   rows: 3
   cols: 4
   data: [ 9.99997258e-01, -2.33603851e-03, -1.06091837e-10,
       3.74998972e-02, -1.52673292e-05, -6.53559202e-03, 9.99978662e-01,
       3.05999428e-01, -2.33598845e-03, -9.99975920e-01, -6.53560972e-03,
       2.04385877e-01 ]

1725904699.141207_1.yaml

%YAML:1.0
---
camera_name: "1725904699.141207_1"
image_width: 640
image_height: 400
camera_matrix:
   rows: 3
   cols: 3
   data: [ 2.8612139892578125e+02, 0., 3.1450225830078125e+02, 0.,
       2.8590396118164062e+02, 2.0738070678710938e+02, 0., 0., 1. ]
distortion_coefficients:
   rows: 1
   cols: 8
   data: [ 3.0223059654235840e+00, 1.0802420377731323e+00,
       6.9366979005280882e-05, -3.8402460631914437e-05,
       2.7348479256033897e-02, 3.3619515895843506e+00,
       1.9861316680908203e+00, 1.8145091831684113e-01 ]
distortion_model: rational_polynomial
rectification_matrix:
   rows: 3
   cols: 3
   data: [ 9.9990656410456025e-01, 4.0576291920516837e-03,
       1.3053685531397281e-02, -4.0370663002351438e-03,
       9.9999056904951489e-01, -1.6012206951796838e-03,
       -1.3060059582571065e-02, 1.5483724897375616e-03,
       9.9991351495333469e-01 ]
projection_matrix:
   rows: 3
   cols: 4
   data: [ 2.5771455153020753e+02, 0., 2.9026425170898438e+02, 0., 0.,
       2.5771455153020753e+02, 2.2554862213134766e+02, 0., 0., 0., 1.,
       0. ]
local_transform:
   rows: 3
   cols: 4
   data: [ -7.14218113e-05, 4.25133072e-02, 9.99095917e-01,
       4.49997336e-01, -9.99998569e-01, -1.67995563e-03, -1.24274435e-09,
       -3.74999456e-02, 1.67843676e-03, -9.99094486e-01, 4.25133668e-02,
       2.06869781e-01 ]

1725904699.141207_2.yaml

%YAML:1.0
---
camera_name: "1725904699.141207_2"
image_width: 640
image_height: 400
camera_matrix:
   rows: 3
   cols: 3
   data: [ 2.8406231689453125e+02, 0., 3.1622885131835938e+02, 0.,
       2.8392382812500000e+02, 2.0817411804199219e+02, 0., 0., 1. ]
distortion_coefficients:
   rows: 1
   cols: 8
   data: [ 3.5483765602111816e+00, 9.7399836778640747e-01,
       -1.3552993186749518e-04, 8.4828621766064316e-05,
       1.1834275908768177e-02, 3.8920052051544189e+00,
       2.0302903652191162e+00, 1.2011218070983887e-01 ]
distortion_model: rational_polynomial
rectification_matrix:
   rows: 3
   cols: 3
   data: [ 9.9994720069054110e-01, -1.1169437088930591e-03,
       1.0215099994695959e-02, 1.1548290355903972e-03,
       9.9999247478903364e-01, -3.7036111030126374e-03,
       -1.0210886398791769e-02, 3.7152122489791974e-03,
       9.9994096575592706e-01 ]
projection_matrix:
   rows: 3
   cols: 4
   data: [ 2.5986273791884997e+02, 0., 3.0931185150146484e+02, 0., 0.,
       2.5986273791884997e+02, 2.2803075790405273e+02, 0., 0., 0., 1.,
       0. ]
local_transform:
   rows: 3
   cols: 4
   data: [ -9.99999225e-01, 1.22194772e-03, -5.98029404e-11,
       -3.74999717e-02, 6.59961597e-06, 5.40084625e-03, -9.99985397e-01,
       -3.05999756e-01, -1.22192991e-03, -9.99984682e-01,
       -5.40084997e-03, 2.04676628e-01 ]

1725904699.141207_3.yaml

%YAML:1.0
---
camera_name: "1725904699.141207_3"
image_width: 640
image_height: 400
camera_matrix:
   rows: 3
   cols: 3
   data: [ 2.8491946411132812e+02, 0., 3.0825848388671875e+02, 0.,
       2.8467828369140625e+02, 2.0445513916015625e+02, 0., 0., 1. ]
distortion_coefficients:
   rows: 1
   cols: 8
   data: [ 4.2646196484565735e-01, -1.3780822046101093e-02,
       -3.1901296461001039e-04, 3.2278010621666908e-04,
       -1.6535141039639711e-03, 7.5491762161254883e-01,
       4.6238999813795090e-02, -7.5233546085655689e-03 ]
distortion_model: rational_polynomial
rectification_matrix:
   rows: 3
   cols: 3
   data: [ 9.9998844201433157e-01, 4.8032413437674440e-03,
       2.1144820525623666e-04, -4.8025378421746850e-03,
       9.9998331390747253e-01, -3.2105352986058740e-03,
       -2.2686565289381551e-04, 3.2094827032774836e-03,
       9.9999482386298022e-01 ]
projection_matrix:
   rows: 3
   cols: 4
   data: [ 2.4878580650125062e+02, 0., 3.0457991027832031e+02, 0., 0.,
       2.4878580650125062e+02, 2.1556067657470703e+02, 0., 0., 0., 1.,
       0. ]
local_transform:
   rows: 3
   cols: 4
   data: [ -5.92851165e-06, 3.20213870e-03, -9.99994874e-01,
       -4.50000226e-01, 9.99998271e-01, 1.85145822e-03, 1.24384114e-10,
       3.74999344e-02, 1.85144879e-03, -9.99993145e-01, -3.20214406e-03,
       2.05738813e-01 ]