rtabmap_odom tests and doc (#1456)

* rtabmap_odom tests and doc

* opengv note

* added ci checks or humble-latest flaky dep cmake errors

* Added real data tests for rgbd_odom and stereo_odom

* added real data for icp_odometry's deskewing test

* fixing json cmake error on lyrical/rolling

* test 2d icp odom deskewing branch

* first review of existing OdometryROS tests

* testing with imu used as guess

* tested imu arrivals sync

* Fixed odom reset on right pose when guess frame id is used

* fixing header errors in ci >=lyrical

* Added support for input rgbd_image topic with features for odom, added multicam rgbd_odometry test

* Added stereo odom support for features-only frames. Added multicam stereo tests.

* forcing latest rtabmap version

* updated OdometryROS API

* ci: dont build non-latest docker in pull requests

* splitting docker jobs

* doc edit

* Making publish_null_when_lost:=false continous when guess is provided (using guess covariance when we cannot register yet)

* updated stereo doc

* ficing rolling ci (rviz Ogre header)

* Added test coverage of alll rgbd_image callbacks

* fixing rolling ci

* making docker ci build/run the tests on pull requests

* fixing ros2 ci testing

* improved sync callback coverage

* improving stereo_odometry test coverage

* improved icp_odometry test coverage

* lyrical voxel_grid ptr error

* make multicam tests working as well without opengv

* removing deps of missing packages on rolling

* PCL empty cloud  conversion compiler errors fix

* fixing icp_odometry test failure on ci witohut libpointmatcher

* fixing nav2 costmap plugin build on lyrical

* joining thread when exiting

* updating icp test to work the same on pcl 1.15 (lyrical)

* Fix parallel tests seg fault

---------

Co-authored-by: mathieu86 <[email protected]>

rtabmap_odom/test/data/stereo/raw/stereo_left.yaml

@@ -0,0 +1,34 @@
+#%YAML:1.0
+camera_name: stereo_tutorial_left
+image_width: 640
+image_height: 480
+camera_matrix:
+   rows: 3
+   cols: 3
+   data: [ 5.2500741669069100e+02, 0., 3.1964931134995413e+02, 0.,
+       5.2447137699104690e+02, 2.4893842459131687e+02, 0., 0., 1. ]
+distortion_coefficients:
+   rows: 1
+   cols: 5
+   data: [ -3.4256158963391159e-01, 1.5067561187251743e-01,
+       -1.1171618396794343e-03, -1.1496904882258663e-03,
+       -3.4792309849001175e-02 ]
+distortion_model: plumb_bob
+rectification_matrix:
+   rows: 3
+   cols: 3
+   data: [ 9.9847710457215999e-01, -7.5204006718293083e-03,
+       -5.4652678058179430e-02, 7.6102614351676607e-03,
+       9.9997001005750097e-01, 1.4362821762221084e-03,
+       5.4640237610064049e-02, -1.8500160368176528e-03,
+       9.9850439251641721e-01 ]
+projection_matrix:
+   rows: 3
+   cols: 4
+   data: [ 5.0021545003868783e+02, 0., 3.5945997238159180e+02, 0., 0.,
+       5.0021545003868783e+02, 2.4750019836425781e+02, 0., 0., 0., 1.,
+       0. ]
+local_transform:
+   rows: 3
+   cols: 4
+   data: [ 0., 0., 1., 0., -1., 0., 0., 0., 0., -1., 0., 0. ]

rtabmap_odom/test/data/stereo/raw/stereo_pose.yaml

@@ -0,0 +1,30 @@
+#%YAML:1.0
+camera_name: stereo_tutorial
+rotation_matrix:
+   rows: 3
+   cols: 3
+   data: [ 9.9998244174079198e-01, -9.1486052999498408e-04,
+       5.8548475927491656e-03, 8.9587501387632107e-04,
+       9.9999433529188531e-01, 3.2445018261516327e-03,
+       -5.8577826934067389e-03, -3.2391996466791719e-03,
+       9.9997759673282971e-01 ]
+translation_matrix:
+   rows: 3
+   cols: 1
+   data: [ -1.1944376542810328e-01, 8.1410498203477041e-04,
+       7.2368896323297812e-03 ]
+essential_matrix:
+   rows: 3
+   cols: 3
+   data: [ -1.1252198674164329e-05, -7.2394856860325228e-03,
+       7.9060664179560138e-04, 6.5370869431856798e-03,
+       -3.9352294745729046e-04, 1.1948346038335741e-01,
+       -9.2109737277881536e-04, -1.1944234402152067e-01,
+       -3.9230197564821978e-04 ]
+fundamental_matrix:
+   rows: 3
+   cols: 3
+   data: [ -2.5309754099766013e-08, -1.6300536361959590e-05,
+       4.9995535977749410e-03, 1.4720205166478256e-05,
+       -8.8704021902692202e-07, 1.3677019067204871e-01,
+       -4.6630670977502930e-03, -1.3776019369349130e-01, 1. ]

rtabmap_odom/test/data/stereo/raw/stereo_right.yaml

@@ -0,0 +1,34 @@
+#%YAML:1.0
+camera_name: stereo_tutorial_right
+image_width: 640
+image_height: 480
+camera_matrix:
+   rows: 3
+   cols: 3
+   data: [ 5.3173015723615038e+02, 0., 3.0841183762414585e+02, 0.,
+       5.3114393189729469e+02, 2.4247036103612575e+02, 0., 0., 1. ]
+distortion_coefficients:
+   rows: 1
+   cols: 5
+   data: [ -3.2382605112765667e-01, 4.3067676369775459e-02,
+       -1.0212741406694578e-03, 5.1816193644504472e-04,
+       9.9965184406768520e-02 ]
+distortion_model: plumb_bob
+rectification_matrix:
+   rows: 3
+   cols: 3
+   data: [ 9.9814647006952373e-01, -6.8031680948065307e-03,
+       -6.0475955483342586e-02, 6.7037039320888168e-03,
+       9.9997582338248303e-01, -1.8474317621778344e-03,
+       6.0487061768119681e-02, 1.4385945915416094e-03,
+       9.9816794468879888e-01 ]
+projection_matrix:
+   rows: 3
+   cols: 4
+   data: [ 5.0021545003868783e+02, 0., 3.5945997238159180e+02,
+       -5.9858566522579160e+01, 0., 5.0021545003868783e+02,
+       2.4750019836425781e+02, 0., 0., 0., 1., 0. ]
+local_transform:
+   rows: 3
+   cols: 4
+   data: [ 0., 0., 1., 0., -1., 0., 0., 0., 0., -1., 0., 0. ]

rtabmap_odom/test/data/stereo/rect/stereo_left.yaml

@@ -0,0 +1,24 @@
+#%YAML:1.0
+camera_name: stereo_left
+image_width: 640
+image_height: 480
+camera_matrix:
+   rows: 3
+   cols: 3
+   data: [ 4.8760873413085938e+02, 0., 3.1811621093750000e+02, 0.,
+       4.8760873413085938e+02, 2.4944424438476562e+02, 0., 0., 1. ]
+distortion_coefficients:
+   rows: 1
+   cols: 5
+   data: [ 0., 0., 0., 0., 0. ]
+distortion_model: plumb_bob
+rectification_matrix:
+   rows: 3
+   cols: 3
+   data: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
+projection_matrix:
+   rows: 3
+   cols: 4
+   data: [ 4.8760873413085938e+02, 0., 3.1811621093750000e+02, 0., 0.,
+       4.8760873413085938e+02, 2.4944424438476562e+02, 0., 0., 0., 1.,
+       0. ]

rtabmap_odom/test/data/stereo/rect/stereo_right.yaml

@@ -0,0 +1,24 @@
+#%YAML:1.0
+camera_name: stereo_right
+image_width: 640
+image_height: 480
+camera_matrix:
+   rows: 3
+   cols: 3
+   data: [ 4.8760873413085938e+02, 0., 3.1811621093750000e+02, 0.,
+       4.8760873413085938e+02, 2.4944424438476562e+02, 0., 0., 1. ]
+distortion_coefficients:
+   rows: 1
+   cols: 5
+   data: [ 0., 0., 0., 0., 0. ]
+distortion_model: plumb_bob
+rectification_matrix:
+   rows: 3
+   cols: 3
+   data: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
+projection_matrix:
+   rows: 3
+   cols: 4
+   data: [ 4.8760873413085938e+02, 0., 3.1811621093750000e+02,
+       -5.8362700032946350e+01, 0., 4.8760873413085938e+02,
+       2.4944424438476562e+02, 0., 0., 0., 1., 0. ]