mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-09 11:17:03 +08:00
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]>
This commit is contained in:
matlabbe
@@ -0,0 +1,79 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#ifndef RTABMAP_ODOM_BAG_PLAYBACK_HPP_
|
||||
#define RTABMAP_ODOM_BAG_PLAYBACK_HPP_
|
||||
|
||||
#include <rclcpp/serialization.hpp>
|
||||
#include <rosbag2_cpp/reader.hpp>
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "test_data.hpp"
|
||||
|
||||
/**
|
||||
* @file
|
||||
* @brief Reads recorded messages out of a bag, for tests that need real sensor input.
|
||||
*
|
||||
* The messages are read and replayed by the test itself rather than by `ros2 bag play`:
|
||||
* no second process, no wall-clock pacing, and the test controls exactly when each
|
||||
* message reaches the node. What the recording provides is the part that cannot be
|
||||
* written by hand -- a real driver's cloud layout and a dense TF history around it.
|
||||
*/
|
||||
|
||||
namespace rtabmap_odom_test {
|
||||
|
||||
/**
|
||||
* @brief The Ouster recording in test/data/lidar; see that directory's README.
|
||||
*
|
||||
* Two sweeps half a mast turn apart, from a platform that never moves: the only thing
|
||||
* between them is the mast's rotation, which TF describes in full.
|
||||
*/
|
||||
inline std::string ousterHalfTurnBag()
|
||||
{
|
||||
return testDataRoot() + "/lidar/ouster_half_turn";
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Every message recorded on @p topic, deserialized.
|
||||
*
|
||||
* Returns an empty vector if the bag or the topic is missing, which the caller is
|
||||
* expected to assert on -- a silently empty fixture would make a test pass for the wrong
|
||||
* reason.
|
||||
*/
|
||||
template <typename MsgT>
|
||||
std::vector<MsgT> readBagMessages(const std::string & bagPath, const std::string & topic)
|
||||
{
|
||||
std::vector<MsgT> messages;
|
||||
rosbag2_cpp::Reader reader;
|
||||
try
|
||||
{
|
||||
reader.open(bagPath);
|
||||
}
|
||||
catch(const std::exception & e)
|
||||
{
|
||||
return messages;
|
||||
}
|
||||
|
||||
rclcpp::Serialization<MsgT> serialization;
|
||||
while(reader.has_next())
|
||||
{
|
||||
const std::shared_ptr<rosbag2_storage::SerializedBagMessage> message = reader.read_next();
|
||||
if(message->topic_name != topic)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
rclcpp::SerializedMessage serialized(*message->serialized_data);
|
||||
MsgT deserialized;
|
||||
serialization.deserialize_message(&serialized, &deserialized);
|
||||
messages.push_back(deserialized);
|
||||
}
|
||||
return messages;
|
||||
}
|
||||
|
||||
} // namespace rtabmap_odom_test
|
||||
|
||||
#endif /* RTABMAP_ODOM_BAG_PLAYBACK_HPP_ */
|
||||
@@ -0,0 +1,319 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#ifndef RTABMAP_ODOM_CAMERA_RIG_HPP_
|
||||
#define RTABMAP_ODOM_CAMERA_RIG_HPP_
|
||||
|
||||
#include <geometry_msgs/msg/transform_stamped.hpp>
|
||||
|
||||
#include <rtabmap_msgs/msg/rgbd_images.hpp>
|
||||
|
||||
#include <rtabmap_conversions/MsgConversion.h>
|
||||
|
||||
#include <rtabmap/core/CameraModel.h>
|
||||
#include <rtabmap/core/Compression.h>
|
||||
#include <rtabmap/core/util3d_transforms.h>
|
||||
|
||||
#include <opencv2/core/core.hpp>
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <random>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "msg_builders.hpp"
|
||||
|
||||
/**
|
||||
* @file
|
||||
* @brief A camera rig in a world of points, for the multi-camera odometry tests.
|
||||
*
|
||||
* Several cameras looking outward from one body is the case that cannot be covered with
|
||||
* recorded frames: it needs a calibrated rig, a scene all of them can see, and ground
|
||||
* truth for where the body went. So the scene is made rather than recorded -- points
|
||||
* scattered around the rig, projected into each camera at each pose along a known
|
||||
* trajectory, which is the approach of RTAB-Map's own multi-camera tests
|
||||
* (`EstimateMotion3DTo2DMultiCam*` in corelib/test/test_util3d_motion_estimation.cpp and
|
||||
* the four-camera rig of test_optimizer.cpp).
|
||||
*
|
||||
* What comes out is the *features*, not imagery: keypoints, their 3D positions and a
|
||||
* descriptor per point, which is what a camera driver doing its own feature extraction
|
||||
* would publish on `rgbd_images`, and all it needs to publish. The frames carry no image
|
||||
* at all, so a run that recovers the trajectory can only have done it from them.
|
||||
*/
|
||||
|
||||
namespace rtabmap_odom_test {
|
||||
|
||||
/// Descriptors are matched between frames, so the same point has to keep the same one.
|
||||
const int kRigDescriptorSize = 32;
|
||||
|
||||
/**
|
||||
* @brief Cameras looking outward from one body, and the points they see.
|
||||
*
|
||||
* The cameras are spread evenly around the body and mounted on its rim, so opposite
|
||||
* cameras are a real distance apart rather than sharing an optical centre.
|
||||
*/
|
||||
struct CameraRig
|
||||
{
|
||||
int width = 0;
|
||||
int height = 0;
|
||||
double fx = 0.0;
|
||||
|
||||
/// base_link -> camera i, optical frame, in the order the cameras are published.
|
||||
std::vector<rtabmap::Transform> localTransforms;
|
||||
std::vector<std::string> frameIds;
|
||||
|
||||
/// The world: points in the odometry frame, and one descriptor row per point.
|
||||
std::vector<cv::Point3f> points;
|
||||
cv::Mat descriptors;
|
||||
|
||||
size_t cameras() const { return localTransforms.size(); }
|
||||
|
||||
/// Camera @p i as RTAB-Map sees it, intrinsics and mounting together.
|
||||
rtabmap::CameraModel model(size_t i) const
|
||||
{
|
||||
return rtabmap::CameraModel(fx, fx, width/2.0, height/2.0,
|
||||
localTransforms[i], 0, cv::Size(width, height));
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief A rig of @p cameras cameras in a world of @p numPoints points.
|
||||
*
|
||||
* The horizontal field of view is 360/cameras degrees, up to 90, so the cameras tile as
|
||||
* much of the circle as they can without overlapping: a point is then seen by at most one
|
||||
* of them, which keeps every descriptor unique within a frame. Two cameras seeing the same
|
||||
* point would put two identical descriptors in the same frame, and the ratio test that
|
||||
* accepts a match only when the best candidate is clearly better than the second would
|
||||
* throw both away.
|
||||
*
|
||||
* The points sit in a box wider than the trajectory, minus a hole around it: something
|
||||
* closer than @p minRange would swing through a camera's field of view, or behind it,
|
||||
* over the course of the run.
|
||||
*/
|
||||
inline CameraRig makeCameraRig(
|
||||
int cameras = 4,
|
||||
int numPoints = 300,
|
||||
float boxXY = 8.0f,
|
||||
float boxZ = 1.5f,
|
||||
float minRange = 2.5f,
|
||||
int width = 160,
|
||||
int height = 120,
|
||||
float rimRadius = 0.175f,
|
||||
float rimHeight = 0.05f,
|
||||
uint32_t seed = 7)
|
||||
{
|
||||
CameraRig rig;
|
||||
rig.width = width;
|
||||
rig.height = height;
|
||||
// Half the horizontal field of view spans half the angle between two cameras, capped
|
||||
// at 45 degrees: one or two cameras would otherwise be asked for a 360 or 180 degree
|
||||
// view, which no pinhole model has. The cap only widens the gaps between cameras, so
|
||||
// a point is still seen by at most one of them.
|
||||
rig.fx = (width/2.0) / std::tan(std::min(M_PI/double(cameras), M_PI/4.0));
|
||||
|
||||
for(int i=0; i<cameras; ++i)
|
||||
{
|
||||
const float yaw = 2.0f*float(M_PI)*float(i)/float(cameras);
|
||||
rig.localTransforms.push_back(
|
||||
rtabmap::Transform(rimRadius*std::cos(yaw), rimRadius*std::sin(yaw), rimHeight,
|
||||
0.0f, 0.0f, yaw)
|
||||
* rtabmap::CameraModel::opticalRotation());
|
||||
rig.frameIds.push_back("camera" + std::to_string(i));
|
||||
}
|
||||
|
||||
std::mt19937 rng(seed);
|
||||
std::uniform_real_distribution<float> distXY(-boxXY, boxXY);
|
||||
std::uniform_real_distribution<float> distZ(-boxZ, boxZ);
|
||||
std::normal_distribution<float> distDescriptor(0.0f, 1.0f);
|
||||
|
||||
rig.descriptors = cv::Mat(numPoints, kRigDescriptorSize, CV_32FC1);
|
||||
for(int i=0; i<numPoints; )
|
||||
{
|
||||
const cv::Point3f point(distXY(rng), distXY(rng), distZ(rng));
|
||||
if(std::sqrt(point.x*point.x + point.y*point.y) < minRange)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
rig.points.push_back(point);
|
||||
for(int c=0; c<kRigDescriptorSize; ++c)
|
||||
{
|
||||
rig.descriptors.at<float>(i, c) = distDescriptor(rng);
|
||||
}
|
||||
++i;
|
||||
}
|
||||
return rig;
|
||||
}
|
||||
|
||||
/// The mounting of each camera, as the rig's driver would publish it on /tf_static.
|
||||
inline std::vector<geometry_msgs::msg::TransformStamped> cameraRigTransforms(
|
||||
const CameraRig & rig, const rclcpp::Time & stamp,
|
||||
const std::string & baseFrame = "base_link")
|
||||
{
|
||||
std::vector<geometry_msgs::msg::TransformStamped> transforms;
|
||||
for(size_t i=0; i<rig.cameras(); ++i)
|
||||
{
|
||||
geometry_msgs::msg::TransformStamped tf;
|
||||
tf.header.stamp = stamp;
|
||||
tf.header.frame_id = baseFrame;
|
||||
tf.child_frame_id = rig.frameIds[i];
|
||||
rtabmap_conversions::transformToGeometryMsg(rig.localTransforms[i], tf.transform);
|
||||
transforms.push_back(tf);
|
||||
}
|
||||
return transforms;
|
||||
}
|
||||
|
||||
/// What one camera of the rig saw: its keypoints, their 3D points and their descriptors.
|
||||
struct RigObservations
|
||||
{
|
||||
std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > keyPoints;
|
||||
std::vector<std::vector<rtabmap_msgs::msg::Point3f> > points;
|
||||
std::vector<cv::Mat> descriptors;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief What the rig sees from @p pose, one entry per camera.
|
||||
*
|
||||
* Each point is given to the first camera that has it in view, so no point is reported
|
||||
* twice. The keypoints are in their own camera's image, the 3D points in their own
|
||||
* camera's optical frame, and the descriptors in the order of the keypoints -- which is
|
||||
* how a driver publishes them, and what the node has to reassemble.
|
||||
*/
|
||||
inline RigObservations observeCameraRig(const CameraRig & rig, const rtabmap::Transform & pose)
|
||||
{
|
||||
const size_t cameras = rig.cameras();
|
||||
std::vector<rtabmap::CameraModel> models;
|
||||
std::vector<rtabmap::Transform> worldToCamera;
|
||||
for(size_t i=0; i<cameras; ++i)
|
||||
{
|
||||
models.push_back(rig.model(i));
|
||||
worldToCamera.push_back((pose * rig.localTransforms[i]).inverse());
|
||||
}
|
||||
|
||||
RigObservations seen;
|
||||
seen.keyPoints.resize(cameras);
|
||||
seen.points.resize(cameras);
|
||||
seen.descriptors.resize(cameras);
|
||||
|
||||
for(size_t p=0; p<rig.points.size(); ++p)
|
||||
{
|
||||
for(size_t i=0; i<cameras; ++i)
|
||||
{
|
||||
const cv::Point3f inCamera =
|
||||
rtabmap::util3d::transformPoint(rig.points[p], worldToCamera[i]);
|
||||
if(inCamera.z <= 0.1f)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
float u = 0.0f;
|
||||
float v = 0.0f;
|
||||
models[i].reproject(inCamera.x, inCamera.y, inCamera.z, u, v);
|
||||
// Truncation would let a point just off the left or top edge through, at a
|
||||
// negative pixel the odometry drops later, so the bounds are checked directly.
|
||||
if(u < 0.0f || v < 0.0f || u >= float(rig.width) || v >= float(rig.height))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
rtabmap_msgs::msg::KeyPoint keyPoint;
|
||||
keyPoint.pt.x = u;
|
||||
keyPoint.pt.y = v;
|
||||
keyPoint.size = 3;
|
||||
keyPoint.response = 1.0f;
|
||||
seen.keyPoints[i].push_back(keyPoint);
|
||||
|
||||
rtabmap_msgs::msg::Point3f point;
|
||||
point.x = inCamera.x;
|
||||
point.y = inCamera.y;
|
||||
point.z = inCamera.z;
|
||||
seen.points[i].push_back(point);
|
||||
|
||||
seen.descriptors[i].push_back(rig.descriptors.row(int(p)));
|
||||
break;
|
||||
}
|
||||
}
|
||||
return seen;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief The rig's observations from @p pose as RGB-D frames, one per camera.
|
||||
*
|
||||
* @p withImages attaches a blank image to each camera. There is nothing to find in it,
|
||||
* but the odometry only takes the paths that touch images when one is there.
|
||||
*/
|
||||
inline rtabmap_msgs::msg::RGBDImages cameraRigFrame(
|
||||
const CameraRig & rig, const rtabmap::Transform & pose, double stamp,
|
||||
bool withImages = false)
|
||||
{
|
||||
const RigObservations seen = observeCameraRig(rig, pose);
|
||||
|
||||
rtabmap_msgs::msg::RGBDImages msg;
|
||||
msg.header.stamp = stampOf(stamp);
|
||||
msg.header.frame_id = rig.frameIds[0];
|
||||
for(size_t i=0; i<rig.cameras(); ++i)
|
||||
{
|
||||
rtabmap_msgs::msg::RGBDImage image;
|
||||
image.header.stamp = msg.header.stamp;
|
||||
image.header.frame_id = rig.frameIds[i];
|
||||
// No image at all by default, neither color nor depth: this frame is its
|
||||
// calibration and its features, which is everything a camera doing its own
|
||||
// extraction has to send.
|
||||
if(withImages)
|
||||
{
|
||||
image.rgb = makeImage(rig.frameIds[i], stamp,
|
||||
cv::Mat::zeros(rig.height, rig.width, CV_8UC1), "mono8");
|
||||
}
|
||||
image.rgb_camera_info = makeCameraInfo(
|
||||
rig.frameIds[i], stamp, rig.width, rig.height, 0.0, rig.fx);
|
||||
image.depth_camera_info = image.rgb_camera_info;
|
||||
image.key_points = seen.keyPoints[i];
|
||||
image.points = seen.points[i];
|
||||
image.descriptors = rtabmap::compressData(seen.descriptors[i]);
|
||||
msg.rgbd_images.push_back(image);
|
||||
}
|
||||
return msg;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief The same observations as stereo frames: left camera plus a right one @p baseline
|
||||
* to its side.
|
||||
*
|
||||
* `RGBDImage` carries a stereo pair as its color and depth fields, so this differs from
|
||||
* the RGB-D frames above only in the second calibration, whose `P(0,3)` is what tells the
|
||||
* node how far apart the two cameras are. The features belong to the left image either
|
||||
* way, which is where a stereo pipeline finds them.
|
||||
*/
|
||||
inline rtabmap_msgs::msg::RGBDImages cameraRigStereoFrame(
|
||||
const CameraRig & rig, const rtabmap::Transform & pose, double stamp,
|
||||
double baseline = 0.12, bool withImages = false)
|
||||
{
|
||||
rtabmap_msgs::msg::RGBDImages msg = cameraRigFrame(rig, pose, stamp, withImages);
|
||||
for(size_t i=0; i<msg.rgbd_images.size(); ++i)
|
||||
{
|
||||
msg.rgbd_images[i].depth_camera_info = makeCameraInfo(
|
||||
rig.frameIds[i], stamp, rig.width, rig.height, -baseline*rig.fx, rig.fx);
|
||||
if(withImages)
|
||||
{
|
||||
msg.rgbd_images[i].depth = makeImage(rig.frameIds[i], stamp,
|
||||
cv::Mat::zeros(rig.height, rig.width, CV_8UC1), "mono8");
|
||||
}
|
||||
}
|
||||
return msg;
|
||||
}
|
||||
|
||||
/// How many features @p frame carries, all cameras together.
|
||||
inline size_t cameraRigFeatureCount(const rtabmap_msgs::msg::RGBDImages & frame)
|
||||
{
|
||||
size_t count = 0;
|
||||
for(size_t i=0; i<frame.rgbd_images.size(); ++i)
|
||||
{
|
||||
count += frame.rgbd_images[i].key_points.size();
|
||||
}
|
||||
return count;
|
||||
}
|
||||
|
||||
} // namespace rtabmap_odom_test
|
||||
|
||||
#endif /* RTABMAP_ODOM_CAMERA_RIG_HPP_ */
|
||||
@@ -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
|
||||
Binary file not shown.
@@ -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 ]
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 120 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 114 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 77 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 80 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 58 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 59 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 54 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 54 KiB |
@@ -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. ]
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 67 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 66 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 65 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 64 KiB |
@@ -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. ]
|
||||
@@ -0,0 +1,362 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#ifndef RTABMAP_ODOM_MSG_BUILDERS_HPP_
|
||||
#define RTABMAP_ODOM_MSG_BUILDERS_HPP_
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <sensor_msgs/msg/camera_info.hpp>
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <sensor_msgs/msg/laser_scan.hpp>
|
||||
#include <sensor_msgs/msg/point_cloud2.hpp>
|
||||
#include <sensor_msgs/image_encodings.hpp>
|
||||
#include <nav_msgs/msg/odometry.hpp>
|
||||
#include <rtabmap_msgs/msg/odom_info.hpp>
|
||||
#include <rtabmap_msgs/msg/rgbd_image.hpp>
|
||||
#include <rtabmap_msgs/msg/scan_descriptor.hpp>
|
||||
#include <rtabmap_msgs/msg/sensor_data.hpp>
|
||||
#include <rtabmap_msgs/msg/user_data.hpp>
|
||||
|
||||
#include <opencv2/core/core.hpp>
|
||||
#ifdef PRE_ROS_IRON
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#else
|
||||
#include <cv_bridge/cv_bridge.hpp>
|
||||
#endif
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace rtabmap_odom_test {
|
||||
|
||||
/// A ROS time from a double, the way sensor stamps are written throughout these tests.
|
||||
inline rclcpp::Time stampOf(double seconds)
|
||||
{
|
||||
return rclcpp::Time(
|
||||
int32_t(seconds), uint32_t((seconds - int32_t(seconds)) * 1e9), RCL_ROS_TIME);
|
||||
}
|
||||
|
||||
/// A rectified pinhole CameraInfo; @p tx is P(0,3), non-zero for a stereo right camera.
|
||||
inline sensor_msgs::msg::CameraInfo makeCameraInfo(
|
||||
const std::string & frameId, double stamp, int width = 8, int height = 8,
|
||||
double tx = 0.0, double fx = 100.0)
|
||||
{
|
||||
sensor_msgs::msg::CameraInfo info;
|
||||
info.header.frame_id = frameId;
|
||||
info.header.stamp = stampOf(stamp);
|
||||
info.width = width;
|
||||
info.height = height;
|
||||
info.distortion_model = "plumb_bob";
|
||||
info.d = {0.0, 0.0, 0.0, 0.0, 0.0};
|
||||
info.k = {fx, 0.0, width/2.0, 0.0, fx, height/2.0, 0.0, 0.0, 1.0};
|
||||
info.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
|
||||
info.p = {fx, 0.0, width/2.0, tx, 0.0, fx, height/2.0, 0.0, 0.0, 0.0, 1.0, 0.0};
|
||||
return info;
|
||||
}
|
||||
|
||||
inline sensor_msgs::msg::Image makeImage(
|
||||
const std::string & frameId, double stamp,
|
||||
const cv::Mat & image, const std::string & encoding)
|
||||
{
|
||||
std_msgs::msg::Header header;
|
||||
header.frame_id = frameId;
|
||||
header.stamp = stampOf(stamp);
|
||||
sensor_msgs::msg::Image msg;
|
||||
cv_bridge::CvImage(header, encoding, image).toImageMsg(msg);
|
||||
return msg;
|
||||
}
|
||||
|
||||
/// A bgr8 color image of a single flat color.
|
||||
inline sensor_msgs::msg::Image makeRgbImage(
|
||||
const std::string & frameId, double stamp, int width = 8, int height = 8,
|
||||
const cv::Scalar & color = cv::Scalar(10, 20, 30))
|
||||
{
|
||||
return makeImage(frameId, stamp, cv::Mat(height, width, CV_8UC3, color), "bgr8");
|
||||
}
|
||||
|
||||
/// A 16UC1 depth image in millimeters, the encoding the RGB-D drivers publish.
|
||||
inline sensor_msgs::msg::Image makeDepthImage(
|
||||
const std::string & frameId, double stamp, int width = 8, int height = 8,
|
||||
uint16_t millimeters = 1500)
|
||||
{
|
||||
return makeImage(frameId, stamp,
|
||||
cv::Mat(height, width, CV_16UC1, cv::Scalar(millimeters)), "16UC1");
|
||||
}
|
||||
|
||||
/// A mono8 image, used as a stereo left or right frame.
|
||||
inline sensor_msgs::msg::Image makeMonoImage(
|
||||
const std::string & frameId, double stamp, int width = 8, int height = 8,
|
||||
uint8_t value = 60)
|
||||
{
|
||||
return makeImage(frameId, stamp,
|
||||
cv::Mat(height, width, CV_8UC1, cv::Scalar(value)), "mono8");
|
||||
}
|
||||
|
||||
/// An RGB-D message with raw bgr8 color and 16UC1 depth, as rgbd_sync publishes it.
|
||||
inline rtabmap_msgs::msg::RGBDImage makeRGBDImage(
|
||||
const std::string & frameId, double stamp, int width = 8, int height = 8,
|
||||
const cv::Scalar & rgbColor = cv::Scalar(10, 20, 30), uint16_t depthValue = 1500)
|
||||
{
|
||||
rtabmap_msgs::msg::RGBDImage msg;
|
||||
msg.header.frame_id = frameId;
|
||||
msg.header.stamp = stampOf(stamp);
|
||||
msg.rgb = makeRgbImage(frameId, stamp, width, height, rgbColor);
|
||||
msg.depth = makeDepthImage(frameId, stamp, width, height, depthValue);
|
||||
msg.rgb_camera_info = makeCameraInfo(frameId, stamp, width, height);
|
||||
msg.depth_camera_info = makeCameraInfo(frameId, stamp, width, height);
|
||||
return msg;
|
||||
}
|
||||
|
||||
/// A flat LaserScan of @p count equal ranges over 180 degrees.
|
||||
inline sensor_msgs::msg::LaserScan makeLaserScan(
|
||||
const std::string & frameId, double stamp, size_t count = 10, float range = 2.0f)
|
||||
{
|
||||
sensor_msgs::msg::LaserScan scan;
|
||||
scan.header.frame_id = frameId;
|
||||
scan.header.stamp = stampOf(stamp);
|
||||
scan.angle_min = -M_PI_2;
|
||||
scan.angle_max = M_PI_2;
|
||||
scan.angle_increment = count > 1 ? float(M_PI / double(count - 1)) : float(M_PI);
|
||||
scan.time_increment = 0.0f;
|
||||
scan.scan_time = 0.1f;
|
||||
scan.range_min = 0.1f;
|
||||
scan.range_max = 10.0f;
|
||||
scan.ranges.assign(count, range);
|
||||
return scan;
|
||||
}
|
||||
|
||||
/// A dense unorganized XYZ float cloud, the shape a 3D lidar driver publishes.
|
||||
inline sensor_msgs::msg::PointCloud2 makeXYZCloud(
|
||||
const std::string & frameId, double stamp,
|
||||
const std::vector<cv::Point3f> & points)
|
||||
{
|
||||
sensor_msgs::msg::PointCloud2 cloud;
|
||||
cloud.header.frame_id = frameId;
|
||||
cloud.header.stamp = stampOf(stamp);
|
||||
cloud.height = 1;
|
||||
cloud.width = points.size();
|
||||
cloud.is_bigendian = false;
|
||||
cloud.is_dense = true;
|
||||
|
||||
cloud.fields.resize(3);
|
||||
const char * names[3] = {"x", "y", "z"};
|
||||
for(int i=0; i<3; ++i)
|
||||
{
|
||||
cloud.fields[i].name = names[i];
|
||||
cloud.fields[i].offset = 4 * i;
|
||||
cloud.fields[i].datatype = sensor_msgs::msg::PointField::FLOAT32;
|
||||
cloud.fields[i].count = 1;
|
||||
}
|
||||
cloud.point_step = 12;
|
||||
cloud.row_step = cloud.point_step * cloud.width;
|
||||
cloud.data.resize(cloud.row_step * cloud.height);
|
||||
|
||||
for(size_t i=0; i<points.size(); ++i)
|
||||
{
|
||||
float * p = reinterpret_cast<float *>(&cloud.data[i * cloud.point_step]);
|
||||
p[0] = points[i].x;
|
||||
p[1] = points[i].y;
|
||||
p[2] = points[i].z;
|
||||
}
|
||||
return cloud;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief An XYZ cloud carrying the optional fields a 3D lidar may add.
|
||||
*
|
||||
* `intensity` and the `normal_*`/`curvature` group each change which PCL point type the
|
||||
* odometry converts the cloud into, so a driver that sends them takes a different path
|
||||
* through the node than one that sends plain XYZ. `t` is the per-point offset from the
|
||||
* header stamp that deskewing needs, spread evenly over @p sweep seconds.
|
||||
*/
|
||||
inline sensor_msgs::msg::PointCloud2 makeCloudWithFields(
|
||||
const std::string & frameId, double stamp,
|
||||
const std::vector<cv::Point3f> & points,
|
||||
bool withIntensity, bool withNormals,
|
||||
const cv::Point3f & normal = cv::Point3f(0, 0, 1),
|
||||
bool withTime = false, float sweep = 0.01f)
|
||||
{
|
||||
sensor_msgs::msg::PointCloud2 cloud;
|
||||
cloud.header.frame_id = frameId;
|
||||
cloud.header.stamp = stampOf(stamp);
|
||||
cloud.height = 1;
|
||||
cloud.width = points.size();
|
||||
cloud.is_bigendian = false;
|
||||
cloud.is_dense = true;
|
||||
|
||||
std::vector<std::string> names = {"x", "y", "z"};
|
||||
if(withIntensity)
|
||||
{
|
||||
names.push_back("intensity");
|
||||
}
|
||||
if(withNormals)
|
||||
{
|
||||
names.push_back("normal_x");
|
||||
names.push_back("normal_y");
|
||||
names.push_back("normal_z");
|
||||
names.push_back("curvature");
|
||||
}
|
||||
if(withTime)
|
||||
{
|
||||
names.push_back("t");
|
||||
}
|
||||
cloud.fields.resize(names.size());
|
||||
for(size_t i=0; i<names.size(); ++i)
|
||||
{
|
||||
cloud.fields[i].name = names[i];
|
||||
cloud.fields[i].offset = uint32_t(4 * i);
|
||||
cloud.fields[i].datatype = sensor_msgs::msg::PointField::FLOAT32;
|
||||
cloud.fields[i].count = 1;
|
||||
}
|
||||
cloud.point_step = uint32_t(4 * names.size());
|
||||
cloud.row_step = cloud.point_step * cloud.width;
|
||||
cloud.data.resize(size_t(cloud.row_step) * cloud.height);
|
||||
|
||||
for(size_t i=0; i<points.size(); ++i)
|
||||
{
|
||||
float * p = reinterpret_cast<float *>(&cloud.data[i * cloud.point_step]);
|
||||
size_t f = 0;
|
||||
p[f++] = points[i].x;
|
||||
p[f++] = points[i].y;
|
||||
p[f++] = points[i].z;
|
||||
if(withIntensity)
|
||||
{
|
||||
p[f++] = float(i % 256);
|
||||
}
|
||||
if(withNormals)
|
||||
{
|
||||
p[f++] = normal.x;
|
||||
p[f++] = normal.y;
|
||||
p[f++] = normal.z;
|
||||
p[f++] = 0.0f; // curvature
|
||||
}
|
||||
if(withTime)
|
||||
{
|
||||
p[f++] = points.size() > 1 ?
|
||||
sweep * float(i) / float(points.size() - 1) : 0.0f;
|
||||
}
|
||||
}
|
||||
return cloud;
|
||||
}
|
||||
|
||||
/// A small cloud on a line, enough to tell one scan from another.
|
||||
inline sensor_msgs::msg::PointCloud2 makeScanCloud(
|
||||
const std::string & frameId, double stamp, size_t count = 4)
|
||||
{
|
||||
std::vector<cv::Point3f> points;
|
||||
points.reserve(count);
|
||||
for(size_t i=0; i<count; ++i)
|
||||
{
|
||||
points.push_back(cv::Point3f(1.0f + float(i), 0.0f, 0.0f));
|
||||
}
|
||||
return makeXYZCloud(frameId, stamp, points);
|
||||
}
|
||||
|
||||
/// A ScanDescriptor carrying a 2D scan, a 3D scan, or both, and optionally a descriptor.
|
||||
inline rtabmap_msgs::msg::ScanDescriptor makeScanDescriptor(
|
||||
const std::string & frameId, double stamp,
|
||||
bool with2d = true, bool with3d = false, bool withGlobalDescriptor = false)
|
||||
{
|
||||
rtabmap_msgs::msg::ScanDescriptor msg;
|
||||
msg.header.frame_id = frameId;
|
||||
msg.header.stamp = stampOf(stamp);
|
||||
if(with2d)
|
||||
{
|
||||
msg.scan = makeLaserScan(frameId, stamp);
|
||||
}
|
||||
if(with3d)
|
||||
{
|
||||
msg.scan_cloud = makeScanCloud(frameId, stamp);
|
||||
}
|
||||
if(withGlobalDescriptor)
|
||||
{
|
||||
// Only "not empty" matters here: consumers pass the payload straight to
|
||||
// RTAB-Map, which is what knows how to decode it.
|
||||
msg.global_descriptor.header = msg.header;
|
||||
msg.global_descriptor.data = {1, 2, 3, 4};
|
||||
}
|
||||
return msg;
|
||||
}
|
||||
|
||||
/// An identity-pose odometry message at @p x meters along the x axis.
|
||||
inline nav_msgs::msg::Odometry makeOdometry(
|
||||
const std::string & frameId, double stamp, double x = 0.0,
|
||||
const std::string & childFrameId = "base_link")
|
||||
{
|
||||
nav_msgs::msg::Odometry msg;
|
||||
msg.header.frame_id = frameId;
|
||||
msg.header.stamp = stampOf(stamp);
|
||||
msg.child_frame_id = childFrameId;
|
||||
msg.pose.pose.position.x = x;
|
||||
msg.pose.pose.orientation.w = 1.0;
|
||||
return msg;
|
||||
}
|
||||
|
||||
inline rtabmap_msgs::msg::OdomInfo makeOdomInfo(
|
||||
const std::string & frameId, double stamp, int inliers = 50)
|
||||
{
|
||||
rtabmap_msgs::msg::OdomInfo msg;
|
||||
msg.header.frame_id = frameId;
|
||||
msg.header.stamp = stampOf(stamp);
|
||||
msg.inliers = inliers;
|
||||
msg.matches = inliers;
|
||||
return msg;
|
||||
}
|
||||
|
||||
/// A SensorData carrying one RGB-D camera, as rtabmap_odom republishes it.
|
||||
inline rtabmap_msgs::msg::SensorData makeSensorData(
|
||||
const std::string & frameId, double stamp, int width = 8, int height = 8)
|
||||
{
|
||||
rtabmap_msgs::msg::SensorData msg;
|
||||
msg.header.frame_id = frameId;
|
||||
msg.header.stamp = stampOf(stamp);
|
||||
msg.left = makeRgbImage(frameId, stamp, width, height);
|
||||
msg.right = makeDepthImage(frameId, stamp, width, height);
|
||||
msg.left_camera_info.push_back(makeCameraInfo(frameId, stamp, width, height));
|
||||
msg.right_camera_info.push_back(makeCameraInfo(frameId, stamp, width, height));
|
||||
geometry_msgs::msg::Transform localTransform;
|
||||
localTransform.rotation.w = 1.0;
|
||||
msg.local_transform.push_back(localTransform);
|
||||
return msg;
|
||||
}
|
||||
|
||||
/// An uncompressed user data matrix (several rows, so it is not taken as compressed).
|
||||
inline rtabmap_msgs::msg::UserData makeUserData(
|
||||
const std::string & frameId, double stamp)
|
||||
{
|
||||
rtabmap_msgs::msg::UserData msg;
|
||||
msg.header.frame_id = frameId;
|
||||
msg.header.stamp = stampOf(stamp);
|
||||
msg.rows = 2;
|
||||
msg.cols = 2;
|
||||
msg.type = CV_8UC1;
|
||||
msg.data = {1, 2, 3, 4};
|
||||
return msg;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Reads the x/y/z of a point from any FLOAT32 xyz cloud.
|
||||
*
|
||||
* Looks the offsets up in the field list rather than assuming they are 0/4/8.
|
||||
*/
|
||||
inline cv::Point3f readXYZ(const sensor_msgs::msg::PointCloud2 & cloud, size_t index)
|
||||
{
|
||||
uint32_t xOffset = 0, yOffset = 4, zOffset = 8;
|
||||
for(size_t i=0; i<cloud.fields.size(); ++i)
|
||||
{
|
||||
if(cloud.fields[i].name == "x") { xOffset = cloud.fields[i].offset; }
|
||||
else if(cloud.fields[i].name == "y") { yOffset = cloud.fields[i].offset; }
|
||||
else if(cloud.fields[i].name == "z") { zOffset = cloud.fields[i].offset; }
|
||||
}
|
||||
const unsigned char * base = &cloud.data[index * cloud.point_step];
|
||||
return cv::Point3f(
|
||||
*reinterpret_cast<const float *>(base + xOffset),
|
||||
*reinterpret_cast<const float *>(base + yOffset),
|
||||
*reinterpret_cast<const float *>(base + zOffset));
|
||||
}
|
||||
|
||||
} // namespace rtabmap_odom_test
|
||||
|
||||
#endif /* RTABMAP_ODOM_MSG_BUILDERS_HPP_ */
|
||||
@@ -0,0 +1,256 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef RTABMAP_ODOM_NODE_TEST_UTILS_HPP_
|
||||
#define RTABMAP_ODOM_NODE_TEST_UTILS_HPP_
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
#include <chrono>
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace rtabmap_odom_test {
|
||||
|
||||
/**
|
||||
* @brief Brings rclcpp up once for the whole test binary.
|
||||
*
|
||||
* Registered as a gtest global environment so it runs before the first test and shuts
|
||||
* down after the last one, which keeps gtest_main usable.
|
||||
*/
|
||||
class RclcppEnvironment : public ::testing::Environment
|
||||
{
|
||||
public:
|
||||
void SetUp() override
|
||||
{
|
||||
if(!rclcpp::ok())
|
||||
{
|
||||
rclcpp::init(0, nullptr);
|
||||
}
|
||||
}
|
||||
void TearDown() override
|
||||
{
|
||||
if(rclcpp::ok())
|
||||
{
|
||||
rclcpp::shutdown();
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
/// Registers RclcppEnvironment. Call once at file scope in each test binary.
|
||||
inline ::testing::Environment * registerRclcppEnvironment()
|
||||
{
|
||||
static ::testing::Environment * const env =
|
||||
::testing::AddGlobalTestEnvironment(new RclcppEnvironment);
|
||||
return env;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Base fixture for driving a node under test over real ROS topics.
|
||||
*
|
||||
* The node under test and a helper node share one single-threaded executor, so
|
||||
* publishing, the node's callback and the assertion all happen on the same thread and
|
||||
* the tests stay deterministic. No launch files and no separate processes are involved:
|
||||
* everything runs in the gtest binary.
|
||||
*/
|
||||
class NodeTest : public ::testing::Test
|
||||
{
|
||||
protected:
|
||||
void SetUp() override
|
||||
{
|
||||
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
|
||||
helper_ = std::make_shared<rclcpp::Node>("rtabmap_odom_test_helper");
|
||||
executor_->add_node(helper_);
|
||||
}
|
||||
|
||||
void TearDown() override
|
||||
{
|
||||
for(const rclcpp::Node::SharedPtr & node : nodes_)
|
||||
{
|
||||
executor_->remove_node(node);
|
||||
}
|
||||
nodes_.clear();
|
||||
executor_->remove_node(helper_);
|
||||
helper_.reset();
|
||||
executor_.reset();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Adds a node under test to the shared executor and keeps it alive for the test.
|
||||
*
|
||||
* The wait is for tf2_ros, not for anything the test does with the node.
|
||||
* ~TransformListener cancels its worker's executor and joins it without ordering the
|
||||
* cancel after the worker reached spin(), so a node dropped microseconds after it was
|
||||
* built -- which a test that only reads a parameter back does -- hangs the binary for
|
||||
* good (ros2/geometry2#517). The window is a few instructions wide and nothing here
|
||||
* can observe that thread, so this buys time instead. Drop it once #752 lands.
|
||||
*/
|
||||
template <typename NodeT>
|
||||
std::shared_ptr<NodeT> addNode(const std::shared_ptr<NodeT> & node)
|
||||
{
|
||||
executor_->add_node(node);
|
||||
nodes_.push_back(node);
|
||||
spinFor(std::chrono::milliseconds(50));
|
||||
return node;
|
||||
}
|
||||
|
||||
/// The helper node, used to publish inputs and subscribe to outputs.
|
||||
rclcpp::Node::SharedPtr helper() { return helper_; }
|
||||
|
||||
/**
|
||||
* @brief Spins until @p done returns true, or the timeout elapses.
|
||||
* @return true if @p done became true
|
||||
*/
|
||||
bool spinUntil(
|
||||
const std::function<bool()> & done,
|
||||
std::chrono::milliseconds timeout = std::chrono::milliseconds(5000))
|
||||
{
|
||||
const std::chrono::steady_clock::time_point deadline =
|
||||
std::chrono::steady_clock::now() + timeout;
|
||||
while(rclcpp::ok() && std::chrono::steady_clock::now() < deadline)
|
||||
{
|
||||
if(done())
|
||||
{
|
||||
return true;
|
||||
}
|
||||
executor_->spin_once(std::chrono::milliseconds(10));
|
||||
}
|
||||
return done();
|
||||
}
|
||||
|
||||
/// Spins for a fixed duration, for the "nothing should happen" assertions.
|
||||
void spinFor(std::chrono::milliseconds duration)
|
||||
{
|
||||
const std::chrono::steady_clock::time_point deadline =
|
||||
std::chrono::steady_clock::now() + duration;
|
||||
while(rclcpp::ok() && std::chrono::steady_clock::now() < deadline)
|
||||
{
|
||||
executor_->spin_once(std::chrono::milliseconds(10));
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Waits until @p publisher has at least @p count matched subscriptions.
|
||||
*
|
||||
* Publishing before the node under test has discovered the topic silently drops the
|
||||
* message, which is the most common cause of a flaky in-process node test.
|
||||
*/
|
||||
template <typename PublisherT>
|
||||
bool waitForSubscriber(const PublisherT & publisher, size_t count = 1)
|
||||
{
|
||||
return spinUntil([&]() { return publisher->get_subscription_count() >= count; });
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Waits until @p subscription sees at least one publisher.
|
||||
*
|
||||
* Every node here publishes only when it has subscribers, so the test's subscription
|
||||
* has to be discovered before the input is sent.
|
||||
*/
|
||||
template <typename SubscriptionT>
|
||||
bool waitForPublisher(const SubscriptionT & subscription, size_t count = 1)
|
||||
{
|
||||
return spinUntil([&]() { return subscription->get_publisher_count() >= count; });
|
||||
}
|
||||
|
||||
/// Collects every message received on @p topic, for later assertions.
|
||||
template <typename MsgT>
|
||||
struct Collector
|
||||
{
|
||||
typename rclcpp::Subscription<MsgT>::SharedPtr subscription;
|
||||
std::vector<typename MsgT::ConstSharedPtr> messages;
|
||||
size_t size() const { return messages.size(); }
|
||||
bool empty() const { return messages.empty(); }
|
||||
|
||||
/**
|
||||
* @brief The last (first) message received.
|
||||
*
|
||||
* A test that reads these without having waited for the topic it is reading --
|
||||
* having waited for a different one, say -- gets a legible failure rather than a
|
||||
* segmentation fault: std::vector::back() on an empty vector dereferences
|
||||
* nullptr-1, which crashes the whole binary and takes the rest of its tests with
|
||||
* it. gtest turns the exception into a failure of the test that threw it.
|
||||
*/
|
||||
const MsgT & back() const { return *checked(messages.empty()?0:&messages.back()); }
|
||||
const MsgT & front() const { return *checked(messages.empty()?0:&messages.front()); }
|
||||
|
||||
private:
|
||||
const typename MsgT::ConstSharedPtr & checked(
|
||||
const typename MsgT::ConstSharedPtr * msg) const
|
||||
{
|
||||
if(msg == 0)
|
||||
{
|
||||
throw std::out_of_range(
|
||||
std::string("nothing was received on \"") +
|
||||
(subscription?subscription->get_topic_name():"?") +
|
||||
"\", so there is no message to read: wait for it to arrive first");
|
||||
}
|
||||
return *msg;
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Subscribes the helper node to @p topic and records everything it receives.
|
||||
*
|
||||
* The callback holds the collector weakly. Capturing it by shared_ptr would close a
|
||||
* cycle -- collector owns the subscription, the subscription owns the callback, the
|
||||
* callback owns the collector -- and neither would ever be freed. A subscription that
|
||||
* outlives its test keeps the helper node's rcl handle alive with it, which leaves the
|
||||
* node's rosout publisher registered and greets the next test with "Publisher already
|
||||
* registered for node name: 'rtabmap_odom_test_helper'".
|
||||
*/
|
||||
template <typename MsgT>
|
||||
std::shared_ptr<Collector<MsgT>> collect(
|
||||
const std::string & topic, const rclcpp::QoS & qos = rclcpp::QoS(10))
|
||||
{
|
||||
std::shared_ptr<Collector<MsgT>> collector = std::make_shared<Collector<MsgT>>();
|
||||
std::weak_ptr<Collector<MsgT>> weak = collector;
|
||||
collector->subscription = helper_->create_subscription<MsgT>(
|
||||
topic, qos,
|
||||
[weak](const typename MsgT::ConstSharedPtr msg) {
|
||||
if(std::shared_ptr<Collector<MsgT>> collector = weak.lock())
|
||||
{
|
||||
collector->messages.push_back(msg);
|
||||
}
|
||||
});
|
||||
return collector;
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
|
||||
rclcpp::Node::SharedPtr helper_;
|
||||
std::vector<rclcpp::Node::SharedPtr> nodes_;
|
||||
};
|
||||
|
||||
} // namespace rtabmap_odom_test
|
||||
|
||||
#endif /* RTABMAP_ODOM_NODE_TEST_UTILS_HPP_ */
|
||||
@@ -0,0 +1,115 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#ifndef RTABMAP_ODOM_SCAN_SCENES_HPP_
|
||||
#define RTABMAP_ODOM_SCAN_SCENES_HPP_
|
||||
|
||||
#include <opencv2/core/core.hpp>
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
#include <vector>
|
||||
|
||||
namespace rtabmap_odom_test {
|
||||
|
||||
/**
|
||||
* A 3D corner -- floor plus two walls -- so all six degrees of freedom are constrained.
|
||||
*
|
||||
* Ported from makeCorner3D() in RTAB-Map's corelib/test/test_odometry.cpp. The jitter is
|
||||
* not decoration: a perfectly flat lattice gives degenerate per-point normals and ICP
|
||||
* finds no correspondences at all.
|
||||
*/
|
||||
inline std::vector<cv::Point3f> corner3D(
|
||||
const cv::Point3f & offset = cv::Point3f(0,0,0),
|
||||
float length = 4.0f, int pointsPerSurface = 400, uint64_t seed = 0xC0FFEE)
|
||||
{
|
||||
cv::RNG rng(seed);
|
||||
const float half = 0.5f * length;
|
||||
std::vector<cv::Point3f> points;
|
||||
points.reserve(3 * pointsPerSurface);
|
||||
for(int i=0; i<pointsPerSurface; ++i) // floor z=-half
|
||||
{
|
||||
points.push_back(cv::Point3f(rng.uniform(-half, half), rng.uniform(-half, half),
|
||||
-half + float(rng.gaussian(0.005))) - offset);
|
||||
}
|
||||
for(int i=0; i<pointsPerSurface; ++i) // wall x=-half
|
||||
{
|
||||
points.push_back(cv::Point3f(-half + float(rng.gaussian(0.005)),
|
||||
rng.uniform(-half, half), rng.uniform(-half, half)) - offset);
|
||||
}
|
||||
for(int i=0; i<pointsPerSurface; ++i) // wall y=-half
|
||||
{
|
||||
points.push_back(cv::Point3f(rng.uniform(-half, half),
|
||||
-half + float(rng.gaussian(0.005)), rng.uniform(-half, half)) - offset);
|
||||
}
|
||||
return points;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief The same corner seen after the sensor has turned @p yaw about z.
|
||||
*
|
||||
* The corner does not move; the sensor does, so in the sensor's own frame every point
|
||||
* turns the other way. This is what a scan taken after a rotation looks like.
|
||||
*/
|
||||
inline std::vector<cv::Point3f> corner3DTurned(double yaw)
|
||||
{
|
||||
const double c = std::cos(-yaw);
|
||||
const double s = std::sin(-yaw);
|
||||
std::vector<cv::Point3f> points = corner3D();
|
||||
for(cv::Point3f & p : points)
|
||||
{
|
||||
const float x = p.x;
|
||||
p.x = float(c * x - s * p.y);
|
||||
p.y = float(s * x + c * p.y);
|
||||
}
|
||||
return points;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Range to a 2D corner from a sensor at (@p sensorX, @p sensorY) looking along +x.
|
||||
*
|
||||
* Two perpendicular walls, one ahead and one to the left. A single wall would leave the
|
||||
* motion along it unobservable and ICP would settle wherever it started; the corner pins
|
||||
* both axes and the heading.
|
||||
*
|
||||
* @return the nearer wall along the ray, or 0 if the ray reaches neither
|
||||
*/
|
||||
inline float corner2DRange(
|
||||
double sensorX, double sensorY, double angle,
|
||||
float frontWall = 5.0f, float leftWall = 3.0f)
|
||||
{
|
||||
const double dx = std::cos(angle);
|
||||
const double dy = std::sin(angle);
|
||||
double best = 0.0;
|
||||
if(dx > 1e-6)
|
||||
{
|
||||
best = (frontWall - sensorX) / dx;
|
||||
}
|
||||
if(dy > 1e-6)
|
||||
{
|
||||
const double toLeft = (leftWall - sensorY) / dy;
|
||||
best = (best <= 0.0 || toLeft < best) ? toLeft : best;
|
||||
}
|
||||
return float(best);
|
||||
}
|
||||
|
||||
/**
|
||||
* The ICP settings RTAB-Map's own odometry tests use for this scene: point-to-point, no
|
||||
* voxelization, and a correspondence ratio low enough for a synthetic scan.
|
||||
*/
|
||||
inline std::vector<rclcpp::Parameter> icpTestParameters()
|
||||
{
|
||||
return {
|
||||
rclcpp::Parameter("Icp/PointToPlane", "false"),
|
||||
rclcpp::Parameter("scan_voxel_size", 0.0),
|
||||
rclcpp::Parameter("Icp/CorrespondenceRatio", "0.1"),
|
||||
};
|
||||
}
|
||||
|
||||
} // namespace rtabmap_odom_test
|
||||
|
||||
#endif /* RTABMAP_ODOM_SCAN_SCENES_HPP_ */
|
||||
@@ -0,0 +1,268 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#ifndef RTABMAP_ODOM_TEST_DATA_HPP_
|
||||
#define RTABMAP_ODOM_TEST_DATA_HPP_
|
||||
|
||||
#include <geometry_msgs/msg/transform.hpp>
|
||||
#include <sensor_msgs/msg/camera_info.hpp>
|
||||
|
||||
#include <tf2/LinearMath/Matrix3x3.hpp>
|
||||
#include <tf2/LinearMath/Quaternion.hpp>
|
||||
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include <opencv2/imgcodecs.hpp>
|
||||
|
||||
#include <fstream>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "msg_builders.hpp"
|
||||
|
||||
/**
|
||||
* @file
|
||||
* @brief Real frames for the odometry node tests, from test/data.
|
||||
*
|
||||
* Visual odometry needs imagery it can actually track: synthetic noise gives a detector
|
||||
* corners that match nothing between frames, so a generated "motion" tells us only that
|
||||
* the node did not crash. These loaders hand the tests the same stereo pairs and RGB-D
|
||||
* frames that RTAB-Map registers in corelib/test/test_odometry.cpp, which lets a ROS test
|
||||
* assert that a plausible transform came out the other end.
|
||||
*
|
||||
* See test/data/README.md for where the files come from and why they are vendored.
|
||||
*/
|
||||
|
||||
namespace rtabmap_odom_test {
|
||||
|
||||
/// Root of the vendored fixtures, set by CMake to test/data in the source tree.
|
||||
inline std::string testDataRoot()
|
||||
{
|
||||
return std::string(RTABMAP_ODOM_TEST_DATA_ROOT);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Opens a ROS camera calibration file with OpenCV's parser.
|
||||
*
|
||||
* The files are the ROS flavour: the format line is commented out (`#%YAML:1.0`) so that
|
||||
* a plain YAML parser -- camera_info_manager, rosparam, PyYAML -- accepts them, since
|
||||
* `%YAML:1.0` is not a valid YAML directive and makes those parsers fail. cv::FileStorage
|
||||
* wants the directive, so it is put back here, in memory, and the file on disk stays
|
||||
* loadable by both.
|
||||
*
|
||||
* @return a closed FileStorage if the file is missing
|
||||
*/
|
||||
inline cv::FileStorage openCalibration(const std::string & path)
|
||||
{
|
||||
std::ifstream file(path.c_str());
|
||||
if(!file.is_open())
|
||||
{
|
||||
return cv::FileStorage();
|
||||
}
|
||||
std::ostringstream buffer;
|
||||
buffer << file.rdbuf();
|
||||
std::string text = buffer.str();
|
||||
|
||||
const std::string rosHeader = "#%YAML:1.0";
|
||||
if(text.compare(0, rosHeader.size(), rosHeader) == 0)
|
||||
{
|
||||
text = "%YAML:1.0\n---" + text.substr(rosHeader.size());
|
||||
}
|
||||
return cv::FileStorage(text, cv::FileStorage::READ | cv::FileStorage::MEMORY);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Reads a rows/cols/data matrix from a calibration file node.
|
||||
*
|
||||
* ROS calibration files have no OpenCV `dt` field, so `>> cv::Mat` cannot read them;
|
||||
* the elements are taken one by one instead, as RTAB-Map's CameraModel::load does.
|
||||
*
|
||||
* @return an empty vector if the node is missing or malformed
|
||||
*/
|
||||
inline std::vector<double> readCalibrationMatrix(
|
||||
cv::FileStorage & fs, const std::string & name, int rows, int cols)
|
||||
{
|
||||
const cv::FileNode node = fs[name];
|
||||
if(node.empty())
|
||||
{
|
||||
return std::vector<double>();
|
||||
}
|
||||
std::vector<double> data;
|
||||
node["data"] >> data;
|
||||
if((int)node["rows"] != rows || (int)node["cols"] != cols ||
|
||||
data.size() != size_t(rows * cols))
|
||||
{
|
||||
return std::vector<double>();
|
||||
}
|
||||
return data;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief A CameraInfo from a ROS calibration file.
|
||||
*
|
||||
* The RGB-D files carry only camera_matrix, so R falls back to identity and P to [K|0] --
|
||||
* which is what a driver publishes for an already-rectified monocular camera anyway.
|
||||
*/
|
||||
inline sensor_msgs::msg::CameraInfo cameraInfoFromCalibration(
|
||||
const std::string & path, const std::string & frameId, double stamp)
|
||||
{
|
||||
sensor_msgs::msg::CameraInfo info;
|
||||
info.header.frame_id = frameId;
|
||||
info.header.stamp = stampOf(stamp);
|
||||
|
||||
cv::FileStorage fs = openCalibration(path);
|
||||
if(!fs.isOpened())
|
||||
{
|
||||
return info;
|
||||
}
|
||||
info.width = uint32_t((int)fs["image_width"]);
|
||||
info.height = uint32_t((int)fs["image_height"]);
|
||||
info.distortion_model = fs["distortion_model"].isString() ?
|
||||
(std::string)fs["distortion_model"] : std::string("plumb_bob");
|
||||
|
||||
const std::vector<double> k = readCalibrationMatrix(fs, "camera_matrix", 3, 3);
|
||||
const std::vector<double> r = readCalibrationMatrix(fs, "rectification_matrix", 3, 3);
|
||||
const std::vector<double> p = readCalibrationMatrix(fs, "projection_matrix", 3, 4);
|
||||
const std::vector<double> d = readCalibrationMatrix(fs, "distortion_coefficients", 1, 5);
|
||||
|
||||
info.d.assign(5, 0.0);
|
||||
for(size_t i=0; i<d.size() && i<info.d.size(); ++i)
|
||||
{
|
||||
info.d[i] = d[i];
|
||||
}
|
||||
for(size_t i=0; i<9; ++i)
|
||||
{
|
||||
info.k[i] = k.empty() ? 0.0 : k[i];
|
||||
info.r[i] = r.empty() ? (i%4 == 0 ? 1.0 : 0.0) : r[i];
|
||||
}
|
||||
for(size_t i=0; i<12; ++i)
|
||||
{
|
||||
info.p[i] = p.empty() ? (i%4 == 3 ? 0.0 : info.k[(i/4)*3 + i%4]) : p[i];
|
||||
}
|
||||
return info;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Stereo pairs: data/stereo, from one 640x480 rig at 20 Hz.
|
||||
//
|
||||
// "rect" holds rectified pairs (frames 50 and 60), what the nodes expect on
|
||||
// left/image_rect by default. "raw" holds pairs straight off the camera (frames 420 and
|
||||
// 425), for the Rtabmap/ImagesAlreadyRectified path.
|
||||
//
|
||||
// Each set carries the calibration that describes it, and the two are not
|
||||
// interchangeable: the rectified one has no distortion and an identity rectification
|
||||
// matrix, because that is what rectification leaves behind, while the raw one carries the
|
||||
// lens's real plumb_bob coefficients and the rotation into the rectified frame. Handing
|
||||
// the rectified calibration to a raw pair claims a distortion-free lens the images do not
|
||||
// have.
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
/// "rect" or "raw"; a rectified pair unless a test says otherwise.
|
||||
enum StereoSet { kRectified, kRaw };
|
||||
|
||||
inline std::string stereoSetDir(StereoSet set)
|
||||
{
|
||||
return testDataRoot() + (set == kRaw ? "/stereo/raw" : "/stereo/rect");
|
||||
}
|
||||
|
||||
/// The left image of a pair, in color as a stereo driver publishes it (bgr8).
|
||||
inline cv::Mat stereoLeftImage(const std::string & name, StereoSet set = kRectified)
|
||||
{
|
||||
return cv::imread(stereoSetDir(set) + "/left/" + name + ".jpg", cv::IMREAD_COLOR);
|
||||
}
|
||||
|
||||
/// The right image of a pair, grayscale (mono8), which is all the matcher uses.
|
||||
inline cv::Mat stereoRightImage(const std::string & name, StereoSet set = kRectified)
|
||||
{
|
||||
return cv::imread(stereoSetDir(set) + "/right/" + name + ".jpg", cv::IMREAD_GRAYSCALE);
|
||||
}
|
||||
|
||||
inline sensor_msgs::msg::CameraInfo stereoLeftInfo(
|
||||
const std::string & frameId, double stamp, StereoSet set = kRectified)
|
||||
{
|
||||
return cameraInfoFromCalibration(stereoSetDir(set) + "/stereo_left.yaml", frameId, stamp);
|
||||
}
|
||||
|
||||
/// The right camera's P(0,3) is -fx * baseline, which is where the stereo scale comes from.
|
||||
inline sensor_msgs::msg::CameraInfo stereoRightInfo(
|
||||
const std::string & frameId, double stamp, StereoSet set = kRectified)
|
||||
{
|
||||
return cameraInfoFromCalibration(stereoSetDir(set) + "/stereo_right.yaml", frameId, stamp);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief The right camera's pose in the left camera's frame, from the rig's stereo
|
||||
* calibration (data/stereo/raw/stereo_pose.yaml).
|
||||
*
|
||||
* Stereo calibration stores the transform the other way round -- a point in left
|
||||
* coordinates maps to the right camera as X_right = R * X_left + T -- so this inverts it
|
||||
* into what TF wants to publish, camera_left -> camera_right. Feeding it to TF is what
|
||||
* lets Rtabmap/ImagesAlreadyRectified:=false rectify the pair against the rig's real
|
||||
* extrinsics, small inter-camera rotation included, rather than an assumed ideal baseline.
|
||||
*
|
||||
* @return an identity transform if the file is missing
|
||||
*/
|
||||
inline geometry_msgs::msg::Transform stereoRightInLeftFrame()
|
||||
{
|
||||
geometry_msgs::msg::Transform transform;
|
||||
transform.rotation.w = 1.0;
|
||||
|
||||
cv::FileStorage fs = openCalibration(stereoSetDir(kRaw) + "/stereo_pose.yaml");
|
||||
if(!fs.isOpened())
|
||||
{
|
||||
return transform;
|
||||
}
|
||||
const std::vector<double> r = readCalibrationMatrix(fs, "rotation_matrix", 3, 3);
|
||||
const std::vector<double> t = readCalibrationMatrix(fs, "translation_matrix", 3, 1);
|
||||
if(r.empty() || t.empty())
|
||||
{
|
||||
return transform;
|
||||
}
|
||||
|
||||
// (R, T) -> (R', -R'T)
|
||||
const tf2::Matrix3x3 rotation(
|
||||
r[0], r[1], r[2],
|
||||
r[3], r[4], r[5],
|
||||
r[6], r[7], r[8]);
|
||||
const tf2::Matrix3x3 inverse = rotation.transpose();
|
||||
const tf2::Vector3 translation = inverse * -tf2::Vector3(t[0], t[1], t[2]);
|
||||
|
||||
tf2::Quaternion q;
|
||||
inverse.getRotation(q);
|
||||
transform.rotation.x = q.x();
|
||||
transform.rotation.y = q.y();
|
||||
transform.rotation.z = q.z();
|
||||
transform.rotation.w = q.w();
|
||||
transform.translation.x = translation.x();
|
||||
transform.translation.y = translation.y();
|
||||
transform.translation.z = translation.z();
|
||||
return transform;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// RGB-D frames: data/rgbd, frames "17" and "154".
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
inline cv::Mat rgbdColorImage(const std::string & name)
|
||||
{
|
||||
return cv::imread(testDataRoot() + "/rgbd/rgb/" + name + ".jpg", cv::IMREAD_COLOR);
|
||||
}
|
||||
|
||||
/// 16-bit millimetres, the encoding the RGB-D drivers publish (16UC1).
|
||||
inline cv::Mat rgbdDepthImage(const std::string & name)
|
||||
{
|
||||
return cv::imread(testDataRoot() + "/rgbd/depth/" + name + ".png", cv::IMREAD_UNCHANGED);
|
||||
}
|
||||
|
||||
inline sensor_msgs::msg::CameraInfo rgbdInfo(
|
||||
const std::string & name, const std::string & frameId, double stamp)
|
||||
{
|
||||
return cameraInfoFromCalibration(
|
||||
testDataRoot() + "/rgbd/calib/" + name + ".yaml", frameId, stamp);
|
||||
}
|
||||
|
||||
} // namespace rtabmap_odom_test
|
||||
|
||||
#endif /* RTABMAP_ODOM_TEST_DATA_HPP_ */
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,800 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved. (BSD-3-Clause, see the repository root.)
|
||||
*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <tf2_ros/static_transform_broadcaster.hpp>
|
||||
|
||||
#include <std_srvs/srv/empty.hpp>
|
||||
|
||||
#include <rtabmap_msgs/msg/odom_info.hpp>
|
||||
#include <rtabmap_msgs/msg/rgbd_images.hpp>
|
||||
|
||||
#include <rtabmap_odom/rgbd_odometry.hpp>
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include <rtabmap/core/Version.h>
|
||||
|
||||
#include "camera_rig.hpp"
|
||||
#include "msg_builders.hpp"
|
||||
#include "node_test_utils.hpp"
|
||||
#include "test_data.hpp"
|
||||
|
||||
namespace rtabmap_odom_test {
|
||||
|
||||
namespace {
|
||||
|
||||
::testing::Environment * const kRclcppEnv = registerRclcppEnvironment();
|
||||
|
||||
/**
|
||||
* The frames that carry a scene come from test/data/rgbd -- the same two RGB-D frames
|
||||
* RTAB-Map registers in corelib/test/test_odometry.cpp. These tests assert the ROS-level
|
||||
* contract (which topics are subscribed, what is published, how the parameters wire up)
|
||||
* on real input rather than the accuracy of the registration, which is that test's
|
||||
* business.
|
||||
*/
|
||||
const char * const kFrame = "17";
|
||||
const char * const kLaterFrame = "154"; // much further along the sequence
|
||||
|
||||
/// The blank scene the "lost tracking" tests need is synthetic: there is nothing to see in it.
|
||||
const int kBlankWidth = 160;
|
||||
const int kBlankHeight = 120;
|
||||
|
||||
double translationNorm(const nav_msgs::msg::Odometry & odom)
|
||||
{
|
||||
const geometry_msgs::msg::Point & p = odom.pose.pose.position;
|
||||
return std::sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
|
||||
}
|
||||
|
||||
/// The angle of the pose's rotation, in radians.
|
||||
double rotationAngle(const nav_msgs::msg::Odometry & odom)
|
||||
{
|
||||
const geometry_msgs::msg::Quaternion & q = odom.pose.pose.orientation;
|
||||
return 2.0 * std::acos(std::min(1.0, std::fabs(q.w)));
|
||||
}
|
||||
|
||||
/// rtabmap_odom marks a pose it does not trust with a 9999 covariance rather than staying silent.
|
||||
bool isLost(const nav_msgs::msg::Odometry & odom)
|
||||
{
|
||||
return odom.pose.covariance[0] >= 9999.0;
|
||||
}
|
||||
|
||||
class RgbdOdometryTest : public NodeTest
|
||||
{
|
||||
protected:
|
||||
void publishSensorTf()
|
||||
{
|
||||
staticTf_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*helper());
|
||||
geometry_msgs::msg::TransformStamped tf;
|
||||
tf.header.stamp = helper()->now();
|
||||
tf.header.frame_id = "base_link";
|
||||
tf.child_frame_id = "camera";
|
||||
tf.transform.rotation.w = 1.0;
|
||||
staticTf_->sendTransform(tf);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Waits until the node under test has subscribed to /tf_static.
|
||||
*
|
||||
* The fixture sends the sensor transform before the node exists, so the node's TF
|
||||
* listener only sees it as the retained transient-local message it gets on discovery.
|
||||
* Publishing an image before that arrives makes the node drop the frame after its
|
||||
* 100 ms wait_for_transform, for no reason the test can see.
|
||||
*/
|
||||
bool waitForTfListener()
|
||||
{
|
||||
if(!staticTf_)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
return spinUntil([&]() { return helper()->count_subscribers("/tf_static") >= 1; });
|
||||
}
|
||||
|
||||
std::shared_ptr<rtabmap_odom::RGBDOdometry> makeNode(
|
||||
std::vector<rclcpp::Parameter> params = {})
|
||||
{
|
||||
// Defaults first, so a test that passes the same parameter overrides them.
|
||||
//
|
||||
// always_process_most_recent_frame:=false is what the node itself recommends for
|
||||
// data that arrives faster than its stamps: these tests publish a whole sequence
|
||||
// back to back with stamps a tenth of a second apart, and when the executor is
|
||||
// slow enough that two of them land in the same spin -- a loaded CI runner, a
|
||||
// single core -- the node drops the second as a replay glitch and the test waits
|
||||
// for a message that will never come. It also keeps processing on the calling
|
||||
// thread instead of the node's worker, which is what makes these tests observable
|
||||
// at all: the odometry is finished by the time the publish returns.
|
||||
std::vector<rclcpp::Parameter> all = {
|
||||
rclcpp::Parameter("frame_id", "base_link"),
|
||||
rclcpp::Parameter("publish_tf", false),
|
||||
rclcpp::Parameter("always_process_most_recent_frame", false),
|
||||
};
|
||||
all.insert(all.end(), params.begin(), params.end());
|
||||
rclcpp::NodeOptions options;
|
||||
options.parameter_overrides(all);
|
||||
std::shared_ptr<rtabmap_odom::RGBDOdometry> node =
|
||||
addNode(std::make_shared<rtabmap_odom::RGBDOdometry>(options));
|
||||
waitForTfListener();
|
||||
return node;
|
||||
}
|
||||
|
||||
/// Calls one of the node's std_srvs/Empty services and waits for the answer.
|
||||
bool callEmptyService(const std::string & name)
|
||||
{
|
||||
rclcpp::Client<std_srvs::srv::Empty>::SharedPtr client =
|
||||
helper()->create_client<std_srvs::srv::Empty>("/rgbd_odometry/" + name);
|
||||
if(!spinUntil([&]() { return client->service_is_ready(); }))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
std::shared_future<std_srvs::srv::Empty::Response::SharedPtr> future =
|
||||
client->async_send_request(
|
||||
std::make_shared<std_srvs::srv::Empty::Request>()).future.share();
|
||||
return spinUntil([&]() {
|
||||
return future.wait_for(std::chrono::seconds(0)) == std::future_status::ready; });
|
||||
}
|
||||
|
||||
/// Where each camera of a rig is mounted, as its driver would publish it once.
|
||||
void publishRigTf(const CameraRig & rig)
|
||||
{
|
||||
staticTf_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*helper());
|
||||
staticTf_->sendTransform(cameraRigTransforms(rig, helper()->now()));
|
||||
}
|
||||
|
||||
/// One frame from test/data/rgbd, as rgbd_sync would deliver it: bgr8 plus 16UC1 millimetres.
|
||||
rtabmap_msgs::msg::RGBDImage makeFrame(const std::string & name, double stamp)
|
||||
{
|
||||
const cv::Mat rgb = rgbdColorImage(name);
|
||||
const cv::Mat depth = rgbdDepthImage(name);
|
||||
EXPECT_FALSE(rgb.empty()) << "test/data/rgbd/rgb/" << name << ".jpg missing";
|
||||
EXPECT_FALSE(depth.empty()) << "test/data/rgbd/depth/" << name << ".png missing";
|
||||
|
||||
rtabmap_msgs::msg::RGBDImage msg;
|
||||
msg.header.frame_id = "camera";
|
||||
msg.header.stamp = stampOf(stamp);
|
||||
msg.rgb = makeImage("camera", stamp, rgb, "bgr8");
|
||||
msg.depth = makeImage("camera", stamp, depth, "16UC1");
|
||||
msg.rgb_camera_info = rgbdInfo(name, "camera", stamp);
|
||||
msg.depth_camera_info = msg.rgb_camera_info;
|
||||
return msg;
|
||||
}
|
||||
|
||||
/// A scene with nothing in it: no features to detect, no motion to recover.
|
||||
rtabmap_msgs::msg::RGBDImage makeBlankFrame(double stamp)
|
||||
{
|
||||
rtabmap_msgs::msg::RGBDImage msg;
|
||||
msg.header.frame_id = "camera";
|
||||
msg.header.stamp = stampOf(stamp);
|
||||
msg.rgb = makeImage("camera", stamp,
|
||||
cv::Mat::zeros(kBlankHeight, kBlankWidth, CV_8UC1), "mono8");
|
||||
msg.depth = makeImage("camera", stamp,
|
||||
cv::Mat(kBlankHeight, kBlankWidth, CV_32FC1, cv::Scalar(2.0f)), "32FC1");
|
||||
msg.rgb_camera_info = makeCameraInfo("camera", stamp, kBlankWidth, kBlankHeight);
|
||||
msg.depth_camera_info = msg.rgb_camera_info;
|
||||
return msg;
|
||||
}
|
||||
|
||||
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> staticTf_;
|
||||
};
|
||||
|
||||
/// The vendored calibration has to survive the trip through CameraInfo, or nothing below means anything.
|
||||
TEST_F(RgbdOdometryTest, the_test_calibration_describes_the_camera)
|
||||
{
|
||||
const sensor_msgs::msg::CameraInfo info = rgbdInfo(kFrame, "camera", 1.0);
|
||||
|
||||
ASSERT_EQ(640u, info.width);
|
||||
ASSERT_EQ(480u, info.height);
|
||||
EXPECT_DOUBLE_EQ(525.0, info.k[0]);
|
||||
EXPECT_DOUBLE_EQ(525.0, info.k[4]);
|
||||
// No projection_matrix in the file: P falls back to [K|0], an already-rectified camera.
|
||||
EXPECT_DOUBLE_EQ(info.k[0], info.p[0]);
|
||||
EXPECT_DOUBLE_EQ(0.0, info.p[3]);
|
||||
}
|
||||
|
||||
/// By default the node takes the three raw camera topics.
|
||||
TEST_F(RgbdOdometryTest, subscribes_to_the_raw_camera_topics_by_default)
|
||||
{
|
||||
publishSensorTf();
|
||||
makeNode();
|
||||
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb =
|
||||
helper()->create_publisher<sensor_msgs::msg::Image>("rgb/image", 10);
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
|
||||
helper()->create_publisher<sensor_msgs::msg::Image>("depth/image", 10);
|
||||
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
|
||||
helper()->create_publisher<sensor_msgs::msg::CameraInfo>("rgb/camera_info", 10);
|
||||
|
||||
EXPECT_TRUE(waitForSubscriber(rgb));
|
||||
EXPECT_TRUE(waitForSubscriber(depth));
|
||||
EXPECT_TRUE(waitForSubscriber(info));
|
||||
}
|
||||
|
||||
/// A synchronized set of the three raw topics produces one odometry message, at the origin.
|
||||
TEST_F(RgbdOdometryTest, publishes_odom_for_a_synchronized_raw_frame)
|
||||
{
|
||||
publishSensorTf();
|
||||
std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
|
||||
collect<nav_msgs::msg::Odometry>("odom");
|
||||
makeNode();
|
||||
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb =
|
||||
helper()->create_publisher<sensor_msgs::msg::Image>("rgb/image", 10);
|
||||
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
|
||||
helper()->create_publisher<sensor_msgs::msg::Image>("depth/image", 10);
|
||||
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
|
||||
helper()->create_publisher<sensor_msgs::msg::CameraInfo>("rgb/camera_info", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(rgb));
|
||||
ASSERT_TRUE(waitForSubscriber(depth));
|
||||
ASSERT_TRUE(waitForSubscriber(info));
|
||||
|
||||
// Identical stamps, so this works under either synchronization policy.
|
||||
const rtabmap_msgs::msg::RGBDImage frame = makeFrame(kFrame, 1.0);
|
||||
rgb->publish(frame.rgb);
|
||||
depth->publish(frame.depth);
|
||||
info->publish(frame.rgb_camera_info);
|
||||
|
||||
ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }));
|
||||
EXPECT_EQ("odom", odom->back().header.frame_id);
|
||||
EXPECT_EQ("base_link", odom->back().child_frame_id);
|
||||
// The first frame has nothing to register against: it defines the origin.
|
||||
EXPECT_NEAR(0.0, translationNorm(odom->back()), 1e-9);
|
||||
}
|
||||
|
||||
/// subscribe_rgbd swaps the three topics for one pre-synchronized RGBDImage.
|
||||
TEST_F(RgbdOdometryTest, subscribe_rgbd_takes_a_single_rgbd_image_topic)
|
||||
{
|
||||
publishSensorTf();
|
||||
std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
|
||||
collect<nav_msgs::msg::Odometry>("odom");
|
||||
makeNode({rclcpp::Parameter("subscribe_rgbd", true)});
|
||||
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
|
||||
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
|
||||
pub->publish(makeFrame(kFrame, 1.0));
|
||||
EXPECT_TRUE(spinUntil([&]() { return !odom->empty(); }));
|
||||
}
|
||||
|
||||
/**
|
||||
* The same frame twice: the registration runs on real features and depth, and the only
|
||||
* answer consistent with the input is "I have not moved". A node that mangles the depth
|
||||
* units or the calibration on the way into RTAB-Map fails here, where the textureless
|
||||
* scenes below cannot tell the difference.
|
||||
*/
|
||||
TEST_F(RgbdOdometryTest, registers_a_repeated_frame_as_no_motion)
|
||||
{
|
||||
publishSensorTf();
|
||||
std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
|
||||
collect<nav_msgs::msg::Odometry>("odom");
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::OdomInfo>> info =
|
||||
collect<rtabmap_msgs::msg::OdomInfo>("odom_info");
|
||||
makeNode({rclcpp::Parameter("subscribe_rgbd", true)});
|
||||
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
|
||||
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
ASSERT_TRUE(waitForPublisher(info->subscription));
|
||||
|
||||
pub->publish(makeFrame(kFrame, 1.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }));
|
||||
pub->publish(makeFrame(kFrame, 1.1));
|
||||
// Both collectors: odom and odom_info are published separately, and the assertions
|
||||
// below compare the second of each.
|
||||
ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2 && info->size() >= 2; }));
|
||||
|
||||
const nav_msgs::msg::Odometry & second = odom->back();
|
||||
ASSERT_FALSE(isLost(second)) << "lost tracking on a frame identical to the previous one";
|
||||
EXPECT_GT(info->back().features, 20) << "no features found in a real scene";
|
||||
EXPECT_GT(info->back().inliers, 20)
|
||||
<< "too few inliers (matches=" << info->back().matches << ")";
|
||||
// Exactly zero on this build, in both translation and rotation; a millimetre and a
|
||||
// milliradian leave room for a backend that answers with rounding noise instead.
|
||||
EXPECT_LT(translationNorm(second), 0.001) << "motion reported between identical frames";
|
||||
EXPECT_LT(rotationAngle(second), 0.001) << "rotation reported between identical frames";
|
||||
}
|
||||
|
||||
/**
|
||||
* Frames 17 and 154 are far apart in the sequence, so losing tracking is a legitimate
|
||||
* outcome; what must hold is that the node's answer agrees with itself -- either a pose
|
||||
* it stands behind, of a plausible size, or one flagged as unusable.
|
||||
*/
|
||||
TEST_F(RgbdOdometryTest, stays_consistent_between_two_distant_frames)
|
||||
{
|
||||
publishSensorTf();
|
||||
std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
|
||||
collect<nav_msgs::msg::Odometry>("odom");
|
||||
makeNode({rclcpp::Parameter("subscribe_rgbd", true)});
|
||||
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
|
||||
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
|
||||
pub->publish(makeFrame(kFrame, 1.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }));
|
||||
pub->publish(makeFrame(kLaterFrame, 1.1));
|
||||
ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2; }));
|
||||
|
||||
const nav_msgs::msg::Odometry & second = odom->back();
|
||||
if(!isLost(second))
|
||||
{
|
||||
// 0.41 to 0.46 m over ten runs here. The bound stays a plausibility check rather
|
||||
// than a fit: how far apart these two frames land is the registration's business,
|
||||
// and this test's claim is only that the answer is not nonsense.
|
||||
EXPECT_LT(translationNorm(second), 2.0)
|
||||
<< "implausible jump of " << translationNorm(second) << " m";
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Two to six cameras, each on its own numbered topic.
|
||||
*
|
||||
* Above six the node has no synchronizer for it and says to use rgbd_cameras:=0 with the
|
||||
* rgbd_images topic instead, so six is where this stops. Only the subscriptions are
|
||||
* checked: one numbered topic per camera, none left behind.
|
||||
*/
|
||||
class RgbdOdometryCamerasTest :
|
||||
public RgbdOdometryTest,
|
||||
public ::testing::WithParamInterface<int>
|
||||
{
|
||||
};
|
||||
|
||||
TEST_P(RgbdOdometryCamerasTest, subscribes_to_one_numbered_topic_per_camera)
|
||||
{
|
||||
const int cameras = GetParam();
|
||||
publishSensorTf();
|
||||
makeNode({rclcpp::Parameter("subscribe_rgbd", true),
|
||||
rclcpp::Parameter("rgbd_cameras", cameras)});
|
||||
|
||||
// All of them first, so they are discovered together rather than one wait after another.
|
||||
std::vector<rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr> publishers;
|
||||
for(int i=0; i<cameras; ++i)
|
||||
{
|
||||
publishers.push_back(helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>(
|
||||
"rgbd_image" + std::to_string(i), 10));
|
||||
}
|
||||
|
||||
for(int i=0; i<cameras; ++i)
|
||||
{
|
||||
EXPECT_TRUE(waitForSubscriber(publishers[i]))
|
||||
<< "rgbd_cameras:=" << cameras << " left rgbd_image" << i << " unsubscribed";
|
||||
}
|
||||
}
|
||||
|
||||
INSTANTIATE_TEST_SUITE_P(
|
||||
RgbdCameras,
|
||||
RgbdOdometryCamerasTest,
|
||||
::testing::Range(2, 7),
|
||||
[](const ::testing::TestParamInfo<int> & info) {
|
||||
return std::to_string(info.param) + "_cameras";
|
||||
});
|
||||
|
||||
/// rgbd_cameras:=0 takes any number of cameras in one RGBDImages message.
|
||||
TEST_F(RgbdOdometryTest, rgbd_cameras_zero_takes_an_rgbd_images_topic)
|
||||
{
|
||||
publishSensorTf();
|
||||
makeNode({rclcpp::Parameter("subscribe_rgbd", true),
|
||||
rclcpp::Parameter("rgbd_cameras", 0)});
|
||||
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImages>::SharedPtr pub =
|
||||
helper()->create_publisher<rtabmap_msgs::msg::RGBDImages>("rgbd_images", 10);
|
||||
|
||||
EXPECT_TRUE(waitForSubscriber(pub));
|
||||
}
|
||||
|
||||
/// This node matches by nearest stamp unless told otherwise; stereo_odometry does not.
|
||||
TEST_F(RgbdOdometryTest, approx_sync_is_on_by_default)
|
||||
{
|
||||
publishSensorTf();
|
||||
std::shared_ptr<rtabmap_odom::RGBDOdometry> node = makeNode();
|
||||
|
||||
EXPECT_TRUE(node->get_parameter("approx_sync").as_bool());
|
||||
}
|
||||
|
||||
/**
|
||||
* A textureless scene is the documented failure: there is nothing to match, so the frame
|
||||
* is lost and the node says so with a null pose rather than publishing nothing.
|
||||
* See "When it loses track" in doc/rgbd_odometry.md.
|
||||
*/
|
||||
TEST_F(RgbdOdometryTest, reports_lost_with_a_null_pose_on_a_textureless_scene)
|
||||
{
|
||||
publishSensorTf();
|
||||
std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
|
||||
collect<nav_msgs::msg::Odometry>("odom");
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::OdomInfo>> info =
|
||||
collect<rtabmap_msgs::msg::OdomInfo>("odom_info");
|
||||
makeNode({rclcpp::Parameter("subscribe_rgbd", true)});
|
||||
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
|
||||
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
ASSERT_TRUE(waitForPublisher(info->subscription));
|
||||
|
||||
pub->publish(makeBlankFrame(1.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }));
|
||||
pub->publish(makeBlankFrame(1.1));
|
||||
ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2 && info->size() >= 2; }));
|
||||
|
||||
// Nothing to register against: no features, and the pose carries the "do not use me"
|
||||
// covariance rather than the node going silent.
|
||||
EXPECT_EQ(0, info->back().features);
|
||||
EXPECT_TRUE(isLost(odom->back()));
|
||||
}
|
||||
|
||||
/// publish_null_when_lost:=false makes the node go silent instead.
|
||||
TEST_F(RgbdOdometryTest, publishes_nothing_when_lost_if_null_publishing_is_off)
|
||||
{
|
||||
publishSensorTf();
|
||||
std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
|
||||
collect<nav_msgs::msg::Odometry>("odom");
|
||||
makeNode({rclcpp::Parameter("subscribe_rgbd", true),
|
||||
rclcpp::Parameter("publish_null_when_lost", false)});
|
||||
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
|
||||
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
|
||||
pub->publish(makeBlankFrame(1.0));
|
||||
pub->publish(makeBlankFrame(1.1));
|
||||
spinFor(std::chrono::milliseconds(1500));
|
||||
|
||||
EXPECT_TRUE(odom->empty());
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* keep_color decides what reaches RTAB-Map from a color image, and therefore what the
|
||||
* node republishes: the matcher works in grayscale, so the color is dropped unless asked
|
||||
* for. Same contract as stereo_odometry, checked here because the doc states it of this
|
||||
* node too.
|
||||
*/
|
||||
TEST_F(RgbdOdometryTest, keeps_the_image_in_color_when_asked)
|
||||
{
|
||||
publishSensorTf();
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> frames =
|
||||
collect<rtabmap_msgs::msg::RGBDImage>("odom_rgbd_image");
|
||||
makeNode({rclcpp::Parameter("subscribe_rgbd", true),
|
||||
rclcpp::Parameter("keep_color", true)});
|
||||
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
|
||||
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
ASSERT_TRUE(waitForPublisher(frames->subscription));
|
||||
|
||||
pub->publish(makeFrame(kFrame, 1.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !frames->empty(); }));
|
||||
EXPECT_EQ("bgr8", frames->back().rgb.encoding);
|
||||
}
|
||||
|
||||
/// Off by default: what reaches RTAB-Map, and comes back out, is grayscale.
|
||||
TEST_F(RgbdOdometryTest, converts_the_image_to_grayscale_by_default)
|
||||
{
|
||||
publishSensorTf();
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> frames =
|
||||
collect<rtabmap_msgs::msg::RGBDImage>("odom_rgbd_image");
|
||||
std::shared_ptr<rtabmap_odom::RGBDOdometry> node =
|
||||
makeNode({rclcpp::Parameter("subscribe_rgbd", true)});
|
||||
EXPECT_FALSE(node->get_parameter("keep_color").as_bool());
|
||||
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
|
||||
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
ASSERT_TRUE(waitForPublisher(frames->subscription));
|
||||
|
||||
pub->publish(makeFrame(kFrame, 1.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !frames->empty(); }));
|
||||
EXPECT_EQ("mono8", frames->back().rgb.encoding);
|
||||
}
|
||||
|
||||
/**
|
||||
* The two feature topics the lidar node cannot fill: both are built from the frame's
|
||||
* visual words, so they carry content only on the visual paths. `odom_local_map` is the
|
||||
* map the frame was registered against, `odom_last_frame` the frame's own features, both
|
||||
* in the odom frame.
|
||||
*/
|
||||
TEST_F(RgbdOdometryTest, publishes_the_feature_map_and_the_frame_that_registered_against_it)
|
||||
{
|
||||
publishSensorTf();
|
||||
std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
|
||||
collect<nav_msgs::msg::Odometry>("odom");
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> localMap =
|
||||
collect<sensor_msgs::msg::PointCloud2>("odom_local_map");
|
||||
std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> lastFrame =
|
||||
collect<sensor_msgs::msg::PointCloud2>("odom_last_frame");
|
||||
makeNode({rclcpp::Parameter("subscribe_rgbd", true)});
|
||||
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
|
||||
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
ASSERT_TRUE(waitForPublisher(localMap->subscription));
|
||||
ASSERT_TRUE(waitForPublisher(lastFrame->subscription));
|
||||
|
||||
pub->publish(makeFrame(kFrame, 1.0));
|
||||
ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }));
|
||||
pub->publish(makeFrame(kFrame, 1.1));
|
||||
// All three collectors: the clouds are published after the odometry of the same frame,
|
||||
// so waiting for odom alone leaves them one spin behind.
|
||||
ASSERT_TRUE(spinUntil([&]() {
|
||||
return odom->size() >= 2 && !localMap->empty() && !lastFrame->empty(); }));
|
||||
|
||||
// 534 features on this frame, in both, expressed in the odometry frame.
|
||||
ASSERT_FALSE(localMap->empty()) << "no feature map was published";
|
||||
ASSERT_FALSE(lastFrame->empty()) << "no frame features were published";
|
||||
EXPECT_GT(localMap->back().width, 0u);
|
||||
EXPECT_GT(lastFrame->back().width, 0u);
|
||||
EXPECT_EQ("odom", lastFrame->back().header.frame_id)
|
||||
<< "these are published in the odometry frame, not the sensor's";
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief A four-camera rig, driven a metre through a world of points.
|
||||
*
|
||||
* The frames carry their features -- keypoints, 3D points, descriptors -- and no image at
|
||||
* all, as a driver that does its own extraction publishes them. So the trajectory below
|
||||
* can only come from the features: the control test that follows runs the same frames
|
||||
* with them stripped off, and it finds nothing.
|
||||
*
|
||||
* Both estimation types the multi-camera case supports are run. Vis/EstimationType=0
|
||||
* aligns the two sets of 3D points, which needs nothing extra; =1 solves a PnP across
|
||||
* all four cameras at once, which RTAB-Map hands to OpenGV and cannot do without it.
|
||||
*/
|
||||
class RgbdOdometryRigTest :
|
||||
public RgbdOdometryTest,
|
||||
public ::testing::WithParamInterface<int>
|
||||
{
|
||||
};
|
||||
|
||||
TEST_P(RgbdOdometryRigTest, recovers_the_trajectory_of_a_rig_from_the_features_it_is_given)
|
||||
{
|
||||
const int estimationType = GetParam();
|
||||
#ifndef RTABMAP_OPENGV
|
||||
if(estimationType == 1)
|
||||
{
|
||||
GTEST_SKIP() << "a multi-camera PnP is solved by OpenGV, which RTAB-Map was built without";
|
||||
}
|
||||
#endif
|
||||
|
||||
const CameraRig rig = makeCameraRig();
|
||||
publishRigTf(rig);
|
||||
std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
|
||||
collect<nav_msgs::msg::Odometry>("odom");
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::OdomInfo>> info =
|
||||
collect<rtabmap_msgs::msg::OdomInfo>("odom_info");
|
||||
makeNode({rclcpp::Parameter("subscribe_rgbd", true),
|
||||
rclcpp::Parameter("rgbd_cameras", 0),
|
||||
rclcpp::Parameter("Vis/EstimationType", std::to_string(estimationType))});
|
||||
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImages>::SharedPtr pub =
|
||||
helper()->create_publisher<rtabmap_msgs::msg::RGBDImages>("rgbd_images", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
ASSERT_TRUE(waitForPublisher(info->subscription));
|
||||
|
||||
// A metre forward, ten centimetres at a time.
|
||||
const int frames = 11;
|
||||
rtabmap_msgs::msg::RGBDImages lastFrame;
|
||||
for(int i=0; i<frames; ++i)
|
||||
{
|
||||
lastFrame = cameraRigFrame(rig, rtabmap::Transform(0.1f*i, 0, 0, 0, 0, 0), 1.0 + 0.1*i);
|
||||
ASSERT_EQ(rig.cameras(), lastFrame.rgbd_images.size());
|
||||
pub->publish(lastFrame);
|
||||
// Both collectors: odom and odom_info are published separately, and the feature
|
||||
// count asserted below is read from the odom_info of this same frame.
|
||||
ASSERT_TRUE(spinUntil([&]() {
|
||||
return odom->size() >= size_t(i+1) && info->size() >= size_t(i+1); }))
|
||||
<< "nothing came back for frame " << i;
|
||||
}
|
||||
|
||||
const nav_msgs::msg::Odometry & last = odom->back();
|
||||
ASSERT_FALSE(isLost(last)) << "lost tracking on a rig that sees the whole scene";
|
||||
EXPECT_NEAR(1.0, last.pose.pose.position.x, 0.05)
|
||||
<< "the rig travelled a metre along x";
|
||||
EXPECT_NEAR(0.0, last.pose.pose.position.y, 0.05);
|
||||
EXPECT_NEAR(0.0, last.pose.pose.position.z, 0.05);
|
||||
EXPECT_NEAR(0.0, rotationAngle(last), 0.05) << "the rig never turned";
|
||||
|
||||
// The frame's own features, reassembled from the four cameras and used as they are.
|
||||
// A couple can go missing on the way: RTAB-Map drops a feature whose descriptor lands
|
||||
// on the same visual word as another one of the same frame, both being ambiguous then
|
||||
// (the `count(*iter) == 1` guards in RegistrationVis). What matters here is that the
|
||||
// number is the frame's own and not zero, which is all a blank image could give.
|
||||
const int sent = int(cameraRigFeatureCount(lastFrame));
|
||||
EXPECT_LE(info->back().features, sent);
|
||||
EXPECT_GT(info->back().features, sent - 10)
|
||||
<< "the node did not use the features the frame came with";
|
||||
}
|
||||
|
||||
INSTANTIATE_TEST_SUITE_P(
|
||||
EstimationTypes,
|
||||
RgbdOdometryRigTest,
|
||||
::testing::Values(0, 1),
|
||||
[](const ::testing::TestParamInfo<int> & info) {
|
||||
return info.param == 0 ? std::string("3d_to_3d") : std::string("pnp_across_cameras");
|
||||
});
|
||||
|
||||
/**
|
||||
* The control for the test above: the same frames with the features stripped off. What is
|
||||
* left is four calibrations and nothing to see, which the node is right to process -- an
|
||||
* empty scene is still a frame -- and right to report lost.
|
||||
*/
|
||||
TEST_F(RgbdOdometryRigTest, the_same_frames_without_their_features_have_nothing_to_track)
|
||||
{
|
||||
const CameraRig rig = makeCameraRig();
|
||||
publishRigTf(rig);
|
||||
std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
|
||||
collect<nav_msgs::msg::Odometry>("odom");
|
||||
std::shared_ptr<Collector<rtabmap_msgs::msg::OdomInfo>> info =
|
||||
collect<rtabmap_msgs::msg::OdomInfo>("odom_info");
|
||||
makeNode({rclcpp::Parameter("subscribe_rgbd", true),
|
||||
rclcpp::Parameter("rgbd_cameras", 0)});
|
||||
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImages>::SharedPtr pub =
|
||||
helper()->create_publisher<rtabmap_msgs::msg::RGBDImages>("rgbd_images", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
ASSERT_TRUE(waitForPublisher(info->subscription));
|
||||
|
||||
for(int i=0; i<2; ++i)
|
||||
{
|
||||
rtabmap_msgs::msg::RGBDImages frame =
|
||||
cameraRigFrame(rig, rtabmap::Transform(0.1f*i, 0, 0, 0, 0, 0), 1.0 + 0.1*i);
|
||||
for(size_t c=0; c<frame.rgbd_images.size(); ++c)
|
||||
{
|
||||
frame.rgbd_images[c].key_points.clear();
|
||||
frame.rgbd_images[c].points.clear();
|
||||
frame.rgbd_images[c].descriptors.clear();
|
||||
}
|
||||
pub->publish(frame);
|
||||
ASSERT_TRUE(spinUntil([&]() { return odom->size() >= size_t(i+1); }));
|
||||
}
|
||||
|
||||
ASSERT_TRUE(spinUntil([&]() { return info->size() >= 2; }));
|
||||
EXPECT_EQ(0, info->back().features) << "features appeared from a frame that has none";
|
||||
EXPECT_TRUE(isLost(odom->back()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Odom/ImageDecimation shrinks the image before registering it, and scales the
|
||||
* calibration to match. Features that arrived with the frame are placed in the full size
|
||||
* image, so they have to be brought down with it: read against a calibration half their
|
||||
* scale, a rig's keypoints land in the wrong camera altogether.
|
||||
*
|
||||
* The frames here carry a blank image for the decimation to have something to work on,
|
||||
* and the trajectory has to come out the same as without it.
|
||||
*/
|
||||
TEST_F(RgbdOdometryRigTest, decimation_brings_the_given_features_down_with_the_image)
|
||||
{
|
||||
#ifndef RTABMAP_OPENGV
|
||||
// Only a PnP reads the keypoints this test is about; a 3D-to-3D registration would
|
||||
// come out right even with every one of them misplaced.
|
||||
GTEST_SKIP() << "a multi-camera PnP is solved by OpenGV, which RTAB-Map was built without";
|
||||
#endif
|
||||
|
||||
const CameraRig rig = makeCameraRig();
|
||||
publishRigTf(rig);
|
||||
std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
|
||||
collect<nav_msgs::msg::Odometry>("odom");
|
||||
makeNode({rclcpp::Parameter("subscribe_rgbd", true),
|
||||
rclcpp::Parameter("rgbd_cameras", 0),
|
||||
rclcpp::Parameter("Odom/ImageDecimation", "2")});
|
||||
|
||||
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImages>::SharedPtr pub =
|
||||
helper()->create_publisher<rtabmap_msgs::msg::RGBDImages>("rgbd_images", 10);
|
||||
ASSERT_TRUE(waitForSubscriber(pub));
|
||||
|
||||
const int frames = 11;
|
||||
for(int i=0; i<frames; ++i)
|
||||
{
|
||||
pub->publish(cameraRigFrame(rig, rtabmap::Transform(0.1f*i, 0, 0, 0, 0, 0),
|
||||
1.0 + 0.1*i, /*withImages=*/true));
|
||||
ASSERT_TRUE(spinUntil([&]() { return odom->size() >= size_t(i+1); }))
|
||||
<< "nothing came back for frame " << i;
|
||||
}
|
||||
|
||||
const nav_msgs::msg::Odometry & last = odom->back();
|
||||
ASSERT_FALSE(isLost(last)) << "the features were lost on the way into the decimated frame";
|
||||
EXPECT_NEAR(1.0, last.pose.pose.position.x, 0.05);
|
||||
EXPECT_NEAR(0.0, last.pose.pose.position.y, 0.05);
|
||||
EXPECT_NEAR(0.0, last.pose.pose.position.z, 0.05);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief The same rig on the numbered topics, one to six cameras.
|
||||
*
|
||||
* `rgbd_cameras:=N` subscribes to N topics and synchronizes them with a callback of its
|
||||
* own per N, six of them in all. The test above drives the `rgbd_cameras:=0` one; these
|
||||
* drive the rest, by publishing each camera of the rig on its own topic and asking for
|
||||
* the same metre back.
|
||||
*
|
||||
* Each of them runs both estimation types, except that a multi-camera PnP needs OpenGV
|
||||
* and is skipped when RTAB-Map was built without it. A single camera does not, so that
|
||||
* one is run either way.
|
||||
*/
|
||||
class RgbdOdometryRigCamerasTest :
|
||||
public RgbdOdometryTest,
|
||||
public ::testing::WithParamInterface<std::tuple<int, bool, int>>
|
||||
{
|
||||
};
|
||||
|
||||
TEST_P(RgbdOdometryRigCamerasTest, recovers_the_trajectory_from_the_numbered_topics)
|
||||
{
|
||||
const int cameras = std::get<0>(GetParam());
|
||||
const bool approxSync = std::get<1>(GetParam());
|
||||
const int estimationType = std::get<2>(GetParam());
|
||||
#ifndef RTABMAP_OPENGV
|
||||
if(estimationType == 1 && cameras > 1)
|
||||
{
|
||||
GTEST_SKIP() << "a multi-camera PnP is solved by OpenGV, which RTAB-Map was built without";
|
||||
}
|
||||
#endif
|
||||
|
||||
const CameraRig rig = makeCameraRig(cameras);
|
||||
publishRigTf(rig);
|
||||
std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
|
||||
collect<nav_msgs::msg::Odometry>("odom");
|
||||
makeNode({rclcpp::Parameter("subscribe_rgbd", true),
|
||||
rclcpp::Parameter("rgbd_cameras", cameras),
|
||||
rclcpp::Parameter("approx_sync", approxSync),
|
||||
rclcpp::Parameter("Vis/EstimationType", std::to_string(estimationType))});
|
||||
|
||||
// One camera listens on rgbd_image, more than one on rgbd_image0..N-1.
|
||||
std::vector<rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr> publishers;
|
||||
for(int i=0; i<cameras; ++i)
|
||||
{
|
||||
publishers.push_back(helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>(
|
||||
cameras == 1 ? "rgbd_image" : "rgbd_image" + std::to_string(i), 10));
|
||||
}
|
||||
for(int i=0; i<cameras; ++i)
|
||||
{
|
||||
ASSERT_TRUE(waitForSubscriber(publishers[i])) << "camera " << i << " has no subscriber";
|
||||
}
|
||||
|
||||
const int frames = 11;
|
||||
for(int i=0; i<frames; ++i)
|
||||
{
|
||||
const rtabmap_msgs::msg::RGBDImages frame =
|
||||
cameraRigFrame(rig, rtabmap::Transform(0.1f*i, 0, 0, 0, 0, 0), 1.0 + 0.1*i);
|
||||
ASSERT_EQ(size_t(cameras), frame.rgbd_images.size());
|
||||
for(int c=0; c<cameras; ++c)
|
||||
{
|
||||
publishers[c]->publish(frame.rgbd_images[c]);
|
||||
}
|
||||
ASSERT_TRUE(spinUntil([&]() { return odom->size() >= size_t(i+1); }))
|
||||
<< "nothing came back for frame " << i;
|
||||
}
|
||||
|
||||
const nav_msgs::msg::Odometry & last = odom->back();
|
||||
ASSERT_FALSE(isLost(last)) << "lost tracking with " << cameras << " camera(s)";
|
||||
EXPECT_NEAR(1.0, last.pose.pose.position.x, 0.05) << "the rig travelled a metre along x";
|
||||
EXPECT_NEAR(0.0, last.pose.pose.position.y, 0.05);
|
||||
EXPECT_NEAR(0.0, last.pose.pose.position.z, 0.05);
|
||||
|
||||
// A reset tears the synchronizer down and builds it again -- one per camera count,
|
||||
// and a different one for each of the two sync policies. Frames have to keep arriving
|
||||
// through the new one.
|
||||
ASSERT_TRUE(callEmptyService("reset_odom"));
|
||||
const size_t beforeReset = odom->size();
|
||||
const rtabmap_msgs::msg::RGBDImages frame =
|
||||
cameraRigFrame(rig, rtabmap::Transform(1.1f, 0, 0, 0, 0, 0), 2.1);
|
||||
for(int c=0; c<cameras; ++c)
|
||||
{
|
||||
publishers[c]->publish(frame.rgbd_images[c]);
|
||||
}
|
||||
EXPECT_TRUE(spinUntil([&]() { return odom->size() > beforeReset; }))
|
||||
<< "nothing came back after the reset rebuilt the synchronizer";
|
||||
}
|
||||
|
||||
INSTANTIATE_TEST_SUITE_P(
|
||||
RgbdCameras,
|
||||
RgbdOdometryRigCamerasTest,
|
||||
::testing::Combine(::testing::Range(1, 7), ::testing::Bool(), ::testing::Values(0, 1)),
|
||||
[](const ::testing::TestParamInfo<std::tuple<int, bool, int>> & info) {
|
||||
const int cameras = std::get<0>(info.param);
|
||||
return std::to_string(cameras) + (cameras == 1 ? "_camera_" : "_cameras_") +
|
||||
(std::get<1>(info.param) ? "approx_sync_" : "exact_sync_") +
|
||||
(std::get<2>(info.param) == 0 ? "3d_to_3d" : "pnp");
|
||||
});
|
||||
|
||||
} // namespace
|
||||
} // namespace rtabmap_odom_test
|
||||
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user