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]>
2026-10-08 10:47:46 +08:00 · 2026-09-21 17:02:45 -07:00
parent 73c98f87a8
commit 11edc01d6a
91 changed files with 9736 additions and 350 deletions
@@ -0,0 +1,103 @@
+# Test data
+
+Real frames for the odometry node tests, so that they register actual imagery instead of
+synthetic noise. Synthetic textures give a detector corners that match nothing between
+frames, which makes a "motion" that only proves the node did not crash.
+
+Everything here is BSD-3-Clause, same authors and same terms as the rest of this
+repository.
+
+| Path | Origin | Used by |
+| --- | --- | --- |
+| `stereo/rect/{left,right}/{50,60}.jpg` | [RTAB-Map `data/stereo_rect`](https://github.com/introlab/rtabmap/tree/master/data/stereo_rect) | `test_stereo_odometry.cpp` |
+| `stereo/rect/stereo_{left,right}.yaml` | [RTAB-Map `data/stereo_rect`](https://github.com/introlab/rtabmap/tree/master/data/stereo_rect) | `test_stereo_odometry.cpp` |
+| `stereo/raw/{left,right}/{420,425}.jpg` | frames 420 and 425 (21.00 s and 21.25 s at 20 Hz) of the [stereo indoor tutorial](https://github.com/introlab/rtabmap/wiki/Stereo-mapping) test sequence | `test_stereo_odometry.cpp` |
+| `stereo/raw/stereo_{left,right}.yaml`, `stereo/raw/stereo_pose.yaml` | that rig's own calibration (`stereo_tutorial_*`) | `test_stereo_odometry.cpp` |
+| `lidar/ouster_half_turn/` | frames 1613418430.682 and 1613418435.082 of an Ouster recording on a rotating mast | `test_icp_odometry.cpp` |
+| `rgbd/rgb/{17,154}.jpg`, `rgbd/depth/{17,154}.png` | [RTAB-Map `data/rgbd`](https://github.com/introlab/rtabmap/tree/master/data/rgbd) | `test_rgbd_odometry.cpp` |
+| `rgbd/calib/{17,154}.yaml` | [RTAB-Map `data/rgbd`](https://github.com/introlab/rtabmap/tree/master/data/rgbd) | `test_rgbd_odometry.cpp` |
+
+They are vendored rather than read from an RTAB-Map checkout because RTAB-Map reaches us
+as an installed library: the `introlab3it/rtabmap` images used by CI delete the source tree
+after `make install`, and the ROS buildfarm has no network during a build. A copy here is
+what makes these tests run everywhere rather than skip.
+
+## What the frames are
+
+Two frames of each kind is the minimum that says anything: the first initialises the
+odometry at the origin, the second has to be registered against it.
+
+- **`stereo/rect`** -- `50` and `60`, two rectified pairs of the same scene a short motion
+  apart. The estimate comes out at 0.171 m, steady to well under a centimetre across runs.
+  These back `corelib/test/test_odometry.cpp` upstream, so a failure here that also fails
+  there is an RTAB-Map issue rather than a ROS one.
+- **`stereo/raw`** -- `420` and `425`, an unrectified pair a quarter second apart, for the
+  `Rtabmap/ImagesAlreadyRectified:=false` path described in `doc/stereo_odometry.md`, and
+  for pinning what happens when that parameter is left at its default on distorted images.
+  A quarter second of walking forward, ~0.233 m, reproduced to a fraction of a percent
+  across runs. Wider gaps in the same window register too, but not reliably: 21.00 s to
+  22.00 s is ~1.04 m at around 45 inliers and lost tracking outright in one run out of
+  eight, where this pair holds 210 or more.
+- **`lidar/ouster_half_turn`** -- two Ouster sweeps 4.40 s apart, recorded as a ROS 2 mcap
+  bag (`/tf`, `/tf_static`, `/os_cloud_node/points`) rather than as loose files, because
+  what makes them worth keeping is the TF history around them.
+
+  The sensor sits on a mast turning at ~42 deg/s while the platform stays put, so it moves
+  through its own 0.1 s sweep and the cloud comes off the driver skewed. The two sweeps are
+  half a turn apart (184.5 deg), which puts the skew in opposite directions and makes a
+  missing correction obvious. And because the platform never moves -- `base_link` ->
+  `box_link` does not translate by a single millimetre over the whole recording -- the
+  answer is known: the transform between the two is the identity. That is what the test
+  measures against, rather than a value taken from a previous run.
+
+  TF runs from 0.1 s before each sweep to 0.1 s past its end, with nothing in between: the
+  gap holds transforms nobody looks up, and keeping them would have tripled the file.
+- **`rgbd`** -- `17` and `154`, two frames of a hand-held Kinect sequence, far enough apart
+  that losing tracking between them is a legitimate outcome.
+
+In every set the left image is color and the right one grayscale, as the cameras recorded
+them.
+
+Both stereo sets come from the same 640x480 rig, but **not** from the same calibration, and
+the two are not interchangeable:
+
+| | `stereo/rect` | `stereo/raw` |
+| --- | --- | --- |
+| `distortion_coefficients` | zeros | the lens's real plumb_bob values (~-0.34) |
+| `rectification_matrix` | identity | the rotation into the rectified frame |
+| `projection_matrix` fx | 487.61 | 500.22 |
+| baseline (`-Tx/fx`) | 0.1197 m | 0.1197 m |
+
+Rectification is what leaves a calibration with no distortion and an identity rotation, so
+the rectified file describes the output of `stereo_image_proc`, not what the camera
+produced. Handing it to a raw pair claims a distortion-free lens the images do not have:
+doing that costs roughly a third of the inliers (110 against 214) and triples the reported
+standard deviation. `stereo_pose.yaml` holds the rig's measured extrinsics -- ~12 cm along
+x plus a few milliradians of rotation -- which the tests publish as the TF between
+`camera_left` and `camera_right`, the transform the node looks up when it has to rectify
+the pair itself.
+
+## File formats
+
+The images are copied as they are. The calibration files differ from their originals by one
+line: the format directive is commented out (`#%YAML:1.0`, with no `---`), which is the ROS
+flavour of the same file. `%YAML:1.0` is not a valid YAML directive, so plain YAML parsers
+-- `camera_info_manager`, `rosparam`, PyYAML -- reject the original form.
+`test/test_data.hpp` puts the directive back in memory before handing the text to
+`cv::FileStorage`, so the files stay readable by both.
+
+Note that they carry no OpenCV `dt` field either, so `>> cv::Mat` cannot read them;
+`rows`/`cols`/`data` are read element by element, as RTAB-Map's own `CameraModel::load`
+does.
+
+The depth images are 16-bit millimetres. `rgbd/calib/*.yaml` also carries a
+`local_transform` (the optical-frame-to-robot transform RTAB-Map stores with the camera
+model); the ROS nodes take that from TF instead, so the tests publish it as a `base_link`
+-> `camera` static transform rather than reading it here.
+
+## Using them
+
+`test/test_data.hpp` loads these into `cv::Mat`, `sensor_msgs/CameraInfo` and
+`geometry_msgs/Transform`. CMake passes the directory as `RTABMAP_ODOM_TEST_DATA_ROOT`,
+pointing into the source tree: the test binaries are not installed, and neither are these
+files.
@@ -0,0 +1,52 @@
+rosbag2_bagfile_information:
+  compression_format: ''
+  compression_mode: ''
+  custom_data: null
+  duration:
+    nanoseconds: 5066021600
+  files:
+  - duration:
+      nanoseconds: 5066021600
+    message_count: 120
+    path: ouster_half_turn.mcap
+    starting_time:
+      nanoseconds_since_epoch: 1613418430206475184
+  message_count: 120
+  relative_file_paths:
+  - ouster_half_turn.mcap
+  ros_distro: rosbags
+  starting_time:
+    nanoseconds_since_epoch: 1613418430206475184
+  storage_identifier: mcap
+  topics_with_message_count:
+  - message_count: 2
+    topic_metadata:
+      name: /tf_static
+      offered_qos_profiles: "- avoid_ros_namespace_conventions: false\n  deadline:
+        {nsec: 0, sec: 0}\n  depth: 10\n  durability: 1\n  history: 1\n  lifespan:
+        {nsec: 0, sec: 0}\n  liveliness: 1\n  liveliness_lease_duration: {nsec: 0,
+        sec: 0}\n  reliability: 1"
+      serialization_format: cdr
+      type: tf2_msgs/msg/TFMessage
+      type_description_hash: RIHS01_e369d0f05a23ae52508854b66f6aa0437f3449d652e8cbf22d5abe85d020f087
+  - message_count: 116
+    topic_metadata:
+      name: /tf
+      offered_qos_profiles: "- avoid_ros_namespace_conventions: false\n  deadline:
+        {nsec: 0, sec: 0}\n  depth: 10\n  durability: 2\n  history: 1\n  lifespan:
+        {nsec: 0, sec: 0}\n  liveliness: 1\n  liveliness_lease_duration: {nsec: 0,
+        sec: 0}\n  reliability: 1"
+      serialization_format: cdr
+      type: tf2_msgs/msg/TFMessage
+      type_description_hash: RIHS01_e369d0f05a23ae52508854b66f6aa0437f3449d652e8cbf22d5abe85d020f087
+  - message_count: 2
+    topic_metadata:
+      name: /os_cloud_node/points
+      offered_qos_profiles: "- avoid_ros_namespace_conventions: false\n  deadline:
+        {nsec: 0, sec: 0}\n  depth: 10\n  durability: 2\n  history: 1\n  lifespan:
+        {nsec: 0, sec: 0}\n  liveliness: 1\n  liveliness_lease_duration: {nsec: 0,
+        sec: 0}\n  reliability: 1"
+      serialization_format: cdr
+      type: sensor_msgs/msg/PointCloud2
+      type_description_hash: RIHS01_9198cabf7da3796ae6fe19c4cb3bdd3525492988c70522628af5daa124bae2b5
+  version: 8
@@ -0,0 +1,16 @@
+#%YAML:1.0
+camera_name: "154"
+image_width: 640
+image_height: 480
+camera_matrix:
+   rows: 3
+   cols: 3
+   data: [ 525., 0., 3.1950000000000000e+02, 0., 525.,
+       2.3950000000000000e+02, 0., 0., 1. ]
+local_transform:
+   rows: 3
+   cols: 4
+   data: [ -1.09767914e-03, 5.99460304e-02, 9.98201132e-01,
+       4.20193411e-02, -9.99999523e-01, -6.60419464e-05, -1.09562278e-03,
+       -8.86659764e-05, 8.94069672e-08, -9.98201728e-01, 5.99459410e-02,
+       4.28920656e-01 ]
@@ -0,0 +1,16 @@
+#%YAML:1.0
+camera_name: "17"
+image_width: 640
+image_height: 480
+camera_matrix:
+   rows: 3
+   cols: 3
+   data: [ 525., 0., 3.1950000000000000e+02, 0., 525.,
+       2.3950000000000000e+02, 0., 0., 1. ]
+local_transform:
+   rows: 3
+   cols: 4
+   data: [ -1.46162510e-03, 5.99460006e-02, 9.98200655e-01,
+       6.20153509e-02, -9.99999106e-01, -8.77380371e-05, -1.45888329e-03,
+       -1.63501027e-04, -2.98023224e-08, -9.98201728e-01, 5.99459410e-02,
+       4.28920656e-01 ]
@@ -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. ]
@@ -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. ]
@@ -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. ]
@@ -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. ]
@@ -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. ]