i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-08 10:47:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

rtabmap_util tests and doc (#1450)

Browse Source 
* Initial tests

* more tests

* More in-depth deskew() testing

* slightly less verbose clamping corruption warning

* added tf buffer related tests

* added remaining tests

* Added rosdoc2, improve tests when we require sync of odom stamp and sensor stamp

* cleanup doc

* fixing ci

* rtabmap_util tests and doc

* Added db_player tests

* Added MapsManager tests

* Added map_assembler tests

* Documenting node first draft

* relative links

* Fixed british->usa english style. Reviewed all md files.

* added link to install ros1

* updated badges

* added Iron

* added ubuntu

* added codecov

* updated coverage ci

* fixing rosdep

* updated ci cov job

* ci bump

* fixing cov ci

* small doc cleanup
This commit is contained in:
matlabbe
2026-09-07 21:23:22 -07:00
committed by GitHub
parent f77dda2b58
commit 61edb4ee85
60 changed files with 9443 additions and 192 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_conversions/README.md
+2 -2
View File
@@ -46,7 +46,7 @@ The naming is uniform: `xxxFromROS()` converts a message into an RTAB-Map type,
| Graph | `mapDataFromROS`, `mapGraphFromROS`, `nodeFromROS`, `linkFromROS`, `sensorDataFromROS` (+ `ToROS` variants) |
| Misc | `infoFromROS`, `odomInfoFromROS`, `odomInfoToStatistics`, `imuFromROS`, `userDataFromROS`, `envSensorFromROS`, `landmarksFromROS`, `timestampFromROS`, `timestampToROS` |
Full signatures and per-function notes are in the [API documentation](https://docs.ros.org/en/rolling/p/rtabmap_conversions/) and in [`MsgConversion.h`](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h).
Full signatures and per-function notes are in the [API documentation](https://docs.ros.org/en/jazzy/p/rtabmap_conversions/) and in [`MsgConversion.h`](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h).
## Conventions worth knowing
@@ -66,7 +66,7 @@ colcon test-result --verbose
## Documentation
API documentation is generated with [rosdoc2](https://github.com/ros-infrastructure/rosdoc2) from the Doxygen comments in the public header, and published to [docs.ros.org](https://docs.ros.org/en/rolling/p/rtabmap_conversions/). To build it locally:
API documentation is generated with [rosdoc2](https://github.com/ros-infrastructure/rosdoc2) from the Doxygen comments in the public header, and published to [docs.ros.org](https://docs.ros.org/en/jazzy/p/rtabmap_conversions/). To build it locally:
```bash
rosdoc2 build --package-path rtabmap_conversions --output-directory doc_output
rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h
+13 -3
View File
@@ -176,7 +176,9 @@ void toCvCopy(const rtabmap_msgs::msg::RGBDImage & image, cv_bridge::CvImagePtr
/**
* @brief Extract the RGB and depth images of an RGBDImage message without copying.
*
* The returned images alias the message's buffers, so @p image must outlive them.
* The returned images alias the message's buffers, so @p image must outlive them. Both
* output pointers are always valid; they hold an empty image when the corresponding
* field is not set.
*
* @param[in] image the message to read; its shared pointer keeps the buffers alive
* @param[out] rgb the RGB image
@@ -186,6 +188,10 @@ void toCvShare(const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr & image, cv_br
/**
* @brief Extract the RGB and depth images of an RGBDImage message without copying.
*
* Both output pointers are always valid; they hold an empty image when the corresponding
* field is not set.
*
* @param[in] image the message to read
* @param[in] trackedObject object whose lifetime keeps the message buffers alive
* @param[out] rgb the RGB image
@@ -219,8 +225,12 @@ void rgbdImageToROS(const rtabmap::SensorData & data, rtabmap_msgs::msg::RGBDIma
* The stamp is taken from the top-level `image->header`, and the camera's local
* transform is not carried by the message (callers resolve it from TF).
*
* The depth image is optional: a message carrying only the color image and its camera
* info gives a SensorData with no depth, which is valid.
*
* @param image the message to convert
* @return the converted sensor data
* @return the converted sensor data, empty (SensorData::isValid() false) if the message
* carries no color image or an unsupported encoding
*
* @warning The returned SensorData does **not** copy the pixels: it points into the
* message's own buffers. @p image must therefore outlive it and must not be
@@ -772,7 +782,7 @@ bool convertRGBDMsgs(
/**
* @brief Convert a stereo pair into RTAB-Map inputs.
*
* The left image keeps its colour; the right image is always reduced to mono.
* The left image keeps its color; the right image is always reduced to mono.
*
* @param leftImageMsg left image
* @param rightImageMsg right image
rtabmap_conversions/src/MsgConversion.cpp
+12 -2
View File
@@ -255,6 +255,11 @@ void toCvShare(const rtabmap_msgs::msg::RGBDImage & image, const std::shared_ptr
depth = ptr;
}
}
else
{
// empty
depth = std::make_shared<cv_bridge::CvImage>();
}
}
catch(cv::Exception& e) {
UFATAL("Fatal error while converting rgbd image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what());
@@ -432,7 +437,11 @@ rtabmap::SensorData rgbdImageFromROS(const rtabmap_msgs::msg::RGBDImage::ConstSh
int depthWidth = depthMsg->image.cols;
int depthHeight = depthMsg->image.rows;
// The depth image is optional: a message can legitimately carry only the color
// image and its camera info. Compare the resolutions only when there is a depth
// image, otherwise the ratios divide by zero.
UASSERT_MSG(
depthMsg->image.empty() ||
imageWidth/depthWidth == imageHeight/depthHeight,
uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str());
@@ -448,7 +457,8 @@ rtabmap::SensorData rgbdImageFromROS(const rtabmap_msgs::msg::RGBDImage::ConstSh
imageMsg->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
imageMsg->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) ||
!(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
!(depthMsg->image.empty() ||
depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
depthMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
{
@@ -2707,7 +2717,7 @@ bool convertScanMsg(
scan2dMsg.header.frame_id,
odomFrameId.empty()?frameId:odomFrameId,
rclcpp::Time(scan2dMsg.header.stamp.sec, scan2dMsg.header.stamp.nanosec),
rclcpp::Time(scan2dMsg.header.stamp.sec, scan2dMsg.header.stamp.nanosec) + rclcpp::Duration::from_seconds(scan2dMsg.ranges.size()*scan2dMsg.time_increment),
rclcpp::Time(scan2dMsg.header.stamp.sec, scan2dMsg.header.stamp.nanosec) + rclcpp::Duration::from_seconds((scan2dMsg.ranges.empty()?0:scan2dMsg.ranges.size()-1)*scan2dMsg.time_increment),
tfBuffer,
waitForTransform);
if(tmpT.isNull())
rtabmap_conversions/test/test_msg_conversion.cpp
+59 -8
View File
@@ -1756,7 +1756,7 @@ TEST(MsgConversion, deskewConstantVelocityHeaderAtLastPoint)
EXPECT_NEAR(readField(out, i, 0), expected, 1e-4) << "x of point " << i;
}
// The line is flat to well under a millimetre: that is the deskewing working,
// The line is flat to well under a millimeter: that is the deskewing working,
// independently of which end of the scan the frame is anchored to.
float minX = readField(out, 0, 0);
float maxX = minX;
@@ -2011,7 +2011,7 @@ TEST(MsgConversion, deskewClampsSamplesOutsideTheSweep)
ASSERT_TRUE(deskew(in, out, rtabmap::Transform(kSpeed, 0, 0, 0, 0, 0)));
// Clamped to the last sample's correction, so it lands within the sweep's own range
// rather than metres away. Every other sample is unaffected.
// rather than meters away. Every other sample is unaffected.
const float x = readWallX(out, corrupt, 0, kTimeOnColumns);
EXPECT_GE(x, kWallDistance - 1e-3f);
EXPECT_LE(x, kWallDistance + float(kSpeed * kScanSpan) + 1e-3f)
@@ -2224,6 +2224,57 @@ TEST(MsgConversion, toCvShareReadsCompressedDepth)
EXPECT_EQ(cv::countNonZero(depthPtr->image != depth), 0);
}
TEST(MsgConversion, toCvShareOnAnEmptyMessageGivesEmptyImages)
{
// Both pointers must be valid even when the message carries nothing: callers such as
// rgbdImageFromROS() dereference them unconditionally.
const rtabmap_msgs::msg::RGBDImage msg;
cv_bridge::CvImageConstPtr rgbPtr, depthPtr;
toCvShare(msg, std::shared_ptr<void const>(), rgbPtr, depthPtr);
ASSERT_TRUE(rgbPtr);
ASSERT_TRUE(depthPtr);
EXPECT_TRUE(rgbPtr->image.empty());
EXPECT_TRUE(depthPtr->image.empty());
}
TEST(MsgConversion, rgbdImageFromROSOnAnEmptyMessageIsInvalid)
{
rtabmap_msgs::msg::RGBDImage::SharedPtr msg =
std::make_shared<rtabmap_msgs::msg::RGBDImage>();
msg->header.frame_id = "camera_link";
msg->header.stamp = rclcpp::Time(1000, 0, RCL_ROS_TIME);
const rtabmap::SensorData data = rgbdImageFromROS(msg);
EXPECT_FALSE(data.isValid()) << "an empty message must give empty data, not a crash";
}
TEST(MsgConversion, rgbdImageFromROSWithoutDepthKeepsTheColorImage)
{
// The depth image is optional: color plus camera info is a valid message, and the
// resolution check must not divide by the zero depth width.
rtabmap_msgs::msg::RGBDImage::SharedPtr msg =
std::make_shared<rtabmap_msgs::msg::RGBDImage>();
msg->header.frame_id = "camera_link";
msg->header.stamp = rclcpp::Time(1000, 0, RCL_ROS_TIME);
cv::Mat rgb(8, 8, CV_8UC3, cv::Scalar(10, 20, 30));
cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", rgb).toImageMsg(msg->rgb);
msg->rgb_camera_info.width = 8;
msg->rgb_camera_info.height = 8;
msg->rgb_camera_info.k = {525.0, 0.0, 4.0, 0.0, 525.0, 4.0, 0.0, 0.0, 1.0};
const rtabmap::SensorData data = rgbdImageFromROS(msg);
EXPECT_TRUE(data.isValid());
ASSERT_FALSE(data.imageRaw().empty());
EXPECT_EQ(data.imageRaw().at<cv::Vec3b>(0, 0), cv::Vec3b(10, 20, 30));
EXPECT_TRUE(data.depthRaw().empty());
ASSERT_EQ(data.cameraModels().size(), 1u);
EXPECT_NEAR(data.cameraModels()[0].fx(), 525.0, 1e-9);
}
TEST(MsgConversion, toCvCopyReadsCompressedRgb)
{
const cv::Mat rgb(8, 8, CV_8UC3, cv::Scalar(10, 20, 30));
@@ -2990,7 +3041,7 @@ void addOdomMotion(tf2_ros::Buffer & buffer)
TEST(MsgConversion, convertRGBDMsgsSyncsToOdomStamp)
{
// The image is captured at t=1001 but must be expressed relative to the base frame
// at odomStamp=1000, one metre back.
// at odomStamp=1000, one meter back.
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
const rtabmap::Transform baseToCamera(0.1f, 0.0f, 0.2f, 0.0f, 0.0f, 0.0f);
addTf(*buffer, "base_link", "camera_link", baseToCamera, 1000.0);
@@ -3075,7 +3126,7 @@ TEST(MsgConversion, convertRGBDMsgsPrefersTheDepthStampWhenTheyDiffer)
{
// The RGB and depth stamps of a camera are assumed to be equal. This pins the
// tie-break for when they are not: the depth stamp prevails, since it is the one the
// geometry is synchronized to. Not a behaviour to rely on -- a camera whose two
// geometry is synchronized to. Not a behavior to rely on -- a camera whose two
// stamps disagree is already outside the contract.
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
addTf(*buffer, "base_link", "camera_link", rtabmap::Transform(0.1f, 0, 0, 0, 0, 0), 1000.0);
@@ -3350,25 +3401,25 @@ TEST(MsgConversion, convertStereoMsgProducesAStereoModel)
EXPECT_EQ(right.at<unsigned char>(0, 0), 50);
}
TEST(MsgConversion, convertStereoMsgConvertsColourToMono)
TEST(MsgConversion, convertStereoMsgConvertsColorToMono)
{
const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
addTf(*buffer, "base_link", "left_link", rtabmap::Transform::getIdentity(), 1000.0);
const cv::Mat colour(8, 8, CV_8UC3, cv::Scalar(10, 20, 30));
const cv::Mat color(8, 8, CV_8UC3, cv::Scalar(10, 20, 30));
const cv::Mat mono(8, 8, CV_8UC1, cv::Scalar(50));
cv::Mat left, right;
rtabmap::StereoCameraModel model;
ASSERT_TRUE(convertStereoMsg(
makeImage("left_link", 1000.0, colour, "bgr8"),
makeImage("left_link", 1000.0, color, "bgr8"),
makeImage("right_link", 1000.0, mono, "mono8"),
makeCameraInfo("left_link", 1000.0, 8, 8, 0.0),
makeCameraInfo("right_link", 1000.0, 8, 8, -15.0),
"base_link", "", timestampToROS(1000.0),
left, right, model, *buffer, 0.0, true));
// The left image is kept in colour; the right is always reduced to mono.
// The left image is kept in color; the right is always reduced to mono.
EXPECT_EQ(left.type(), CV_8UC3);
EXPECT_EQ(right.type(), CV_8UC1);
}