From b0a773e1f6dac6d966a36f0f8be0517e8d61904d Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 7 Sep 2026 21:14:33 -0700 Subject: [PATCH] camera info conversion from ROS: detect null K R P correctly (#1448) * camera info conversion from ROS: detect null K R P correctly * Updated deskew based on ros2 tests * clenup ros1 main page and ci job name --- .github/workflows/{noetic-pr.yml => ros1.yml} | 2 +- README.md | 22 ---- .../rtabmap_conversions/MsgConversion.h | 7 +- rtabmap_conversions/src/MsgConversion.cpp | 124 +++++++++++++----- rtabmap_odom/src/nodelets/icp_odometry.cpp | 6 +- 5 files changed, 98 insertions(+), 63 deletions(-) rename .github/workflows/{noetic-pr.yml => ros1.yml} (98%) diff --git a/.github/workflows/noetic-pr.yml b/.github/workflows/ros1.yml similarity index 98% rename from .github/workflows/noetic-pr.yml rename to .github/workflows/ros1.yml index 710247cc..4c095991 100644 --- a/.github/workflows/noetic-pr.yml +++ b/.github/workflows/ros1.yml @@ -1,4 +1,4 @@ -name: noetic-pr +name: ros1 on: pull_request: diff --git a/README.md b/README.md index 8a22a85b..e67809f4 100644 --- a/README.md +++ b/README.md @@ -16,11 +16,6 @@ For the RTAB-Map libraries and standalone application, visit [RTAB-Map's home pa Build Status
Build Status - - ROS 2 - Build Status - - @@ -33,23 +28,6 @@ For the RTAB-Map libraries and standalone application, visit [RTAB-Map's home pa Noetic Build Status - - ROS 2 - Humble - Build Status - - - Iron - Build Status - - - Jazzy - Build Status - - - Rolling - Build Status - Docker diff --git a/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h b/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h index ac2777a8..68daffcb 100644 --- a/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h +++ b/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h @@ -284,10 +284,15 @@ bool deskew( double waitForTransform, bool slerp = false); +/** Deskew a point cloud using a constant velocity model. + * @param input cloud with a per-point time channel ("t", "time", "stamps" or "timestamp") + * @param output deskewed cloud, expressed in the frame at input's header stamp + * @param velocity twist of the sensor frame (m/s and rad/s) + * @return false if the cloud has no usable time channel or velocity is null + */ bool deskew( const sensor_msgs::PointCloud2 & input, sensor_msgs::PointCloud2 & output, - double previousStamp, const rtabmap::Transform & velocity); } diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 512384f1..74c6e8bc 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -796,9 +796,12 @@ rtabmap::CameraModel cameraModelFromROS( const sensor_msgs::CameraInfo & camInfo, const rtabmap::Transform & localTransform) { + // Note: K, R and P are fixed-size arrays in the ROS message (boost::array), so they + // are never empty and their size is always right. An unset matrix is signalled by + // all-zero content instead: K[0] and P[0] hold the focal length, which is always + // non-zero for a valid calibration, and an unset rectification matrix is all zeros. cv:: Mat K; - UASSERT(camInfo.K.empty() || camInfo.K.size() == 9); - if(!camInfo.K.empty()) + if(camInfo.K[0] != 0.0) { K = cv::Mat(3, 3, CV_64FC1); memcpy(K.data, camInfo.K.elems, 9*sizeof(double)); @@ -825,17 +828,22 @@ rtabmap::CameraModel cameraModelFromROS( } } + // R is a rotation matrix, so any of its elements can legitimately be zero: only + // an entirely zero matrix means "not set". cv:: Mat R; - UASSERT(camInfo.R.empty() || camInfo.R.size() == 9); - if(!camInfo.R.empty()) + bool rIsSet = false; + for(size_t i=0; !rIsSet && i0 when constant velocity model is used!"); - return false; - } - if(velocity.isNull()) { ROS_ERROR("velocity should be valid when constant velocity model is used!"); @@ -3069,8 +3070,23 @@ bool deskew_impl( } else if(lastStamp == firstStamp) { - ROS_ERROR("First and last stamps in the scan are the same (%f) (header=%f)!", lastStamp.toSec(), input.header.stamp.toSec()); - return false; + // There is no time spread across the scan, so there is nothing to correct. This + // happens when the driver doesn't fill the per-point time channel, and also when + // the cloud has already been deskewed: deskewing zeroes that channel to mark it. + // Pass the cloud through unchanged so that deskewing twice is a no-op rather than + // a failure that makes the caller drop the frame. + static bool warned = false; + if(!warned) + { + ROS_WARN("First and last stamps in the scan are the same (%f) (header=%f), the " + "cloud is returned unchanged. Either the time channel is not filled by " + "the driver, or the cloud has already been deskewed. This warning is " + "only shown once.", + lastStamp.toSec(), input.header.stamp.toSec()); + warned = true; + } + output = input; + return true; } std::string errorMsg; @@ -3121,23 +3137,19 @@ bool deskew_impl( float vx,vy,vz, vroll,vpitch,vyaw; velocity.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw); - // We need three poses: - // 1- The pose of base frame in odom frame at first stamp - // 2- The pose of base frame in odom frame at msg stamp - // 3- The pose of base frame in odom frame at last stamp - UASSERT(firstStamp.toSec() >= previousStamp); - UASSERT(lastStamp.toSec() > previousStamp); - double dt1 = firstStamp.toSec() - previousStamp; - double dt2 = input.header.stamp.toSec() - previousStamp; - double dt3 = lastStamp.toSec() - previousStamp; - - rtabmap::Transform p1(vx*dt1, vy*dt1, vz*dt1, vroll*dt1, vpitch*dt1, vyaw*dt1); - rtabmap::Transform p2(vx*dt2, vy*dt2, vz*dt2, vroll*dt2, vpitch*dt2, vyaw*dt2); - rtabmap::Transform p3(vx*dt3, vy*dt3, vz*dt3, vroll*dt3, vpitch*dt3, vyaw*dt3); + // Integrate the velocity directly from the stamp of the msg, which is the + // frame the deskewed cloud is expressed in. Going through a third, earlier + // reference pose and composing it away would give the same answer for a pure + // translation, but not for a rotation: Transform() scales roll/pitch/yaw + // linearly instead of using the twist exponential, so the composition only + // cancels in the small-angle limit. Keeping dt bounded by the scan duration + // is where that approximation is at its best. + double dt1 = firstStamp.toSec() - input.header.stamp.toSec(); + double dt3 = lastStamp.toSec() - input.header.stamp.toSec(); // First and last poses are relative to stamp of the msg - firstPose = p2.inverse() * p1; - lastPose = p2.inverse() * p3; + firstPose = rtabmap::Transform(vx*dt1, vy*dt1, vz*dt1, vroll*dt1, vpitch*dt1, vyaw*dt1); + lastPose = rtabmap::Transform(vx*dt3, vy*dt3, vz*dt3, vroll*dt3, vpitch*dt3, vyaw*dt3); } if(firstPose.isNull()) @@ -3164,6 +3176,7 @@ bool deskew_impl( output = input; ros::Time stamp; + bool clampWarned = false; // reported once per cloud, see the clamp below UTimer processingTime; if(timeOnColumns) { @@ -3209,7 +3222,27 @@ bool deskew_impl( rtabmap::Transform transform; if(slerp) { - transform = firstPose.interpolate((stamp-firstStamp).toSec() / scanTime, lastPose); + // The ordering check only compares the first and last samples, so a stamp + // outside [firstStamp, lastStamp] can slip through. Clamp it: extrapolating + // would throw the point far beyond the sweep. + double ratio = (stamp-firstStamp).toSec() / scanTime; + if(ratio < 0.0 || ratio > 1.0) + { + // Warned once per cloud rather than once per process: the timestamp + // channel is corrupted, which is a serious upstream problem worth + // reporting on every affected scan, but not once per point. + if(!clampWarned) + { + ROS_WARN("A point has a stamp (%f) outside the first (%f) and last (%f) " + "stamps of the scan, its correction is clamped to the closest end " + "of the sweep. The timestamp channel of the input cloud is likely " + "corrupted. Only the first such point of this cloud is reported.", + stamp.toSec(), firstStamp.toSec(), lastStamp.toSec()); + clampWarned = true; + } + ratio = ratio<0.0?0.0:1.0; + } + transform = firstPose.interpolate(float(ratio), lastPose); } else { @@ -3303,7 +3336,27 @@ bool deskew_impl( rtabmap::Transform transform; if(slerp) { - transform = firstPose.interpolate((stamp-firstStamp).toSec() / scanTime, lastPose); + // The ordering check only compares the first and last samples, so a stamp + // outside [firstStamp, lastStamp] can slip through. Clamp it: extrapolating + // would throw the point far beyond the sweep. + double ratio = (stamp-firstStamp).toSec() / scanTime; + if(ratio < 0.0 || ratio > 1.0) + { + // Warned once per cloud rather than once per process: the timestamp + // channel is corrupted, which is a serious upstream problem worth + // reporting on every affected scan, but not once per point. + if(!clampWarned) + { + ROS_WARN("A point has a stamp (%f) outside the first (%f) and last (%f) " + "stamps of the scan, its correction is clamped to the closest end " + "of the sweep. The timestamp channel of the input cloud is likely " + "corrupted. Only the first such point of this cloud is reported.", + stamp.toSec(), firstStamp.toSec(), lastStamp.toSec()); + clampWarned = true; + } + ratio = ratio<0.0?0.0:1.0; + } + transform = firstPose.interpolate(float(ratio), lastPose); } else { @@ -3365,16 +3418,15 @@ bool deskew( double waitForTransform, bool slerp) { - return deskew_impl(input, output, fixedFrameId, &tfBuffer, waitForTransform, slerp, rtabmap::Transform(), 0); + return deskew_impl(input, output, fixedFrameId, &tfBuffer, waitForTransform, slerp, rtabmap::Transform()); } bool deskew( const sensor_msgs::PointCloud2 & input, sensor_msgs::PointCloud2 & output, - double previousStamp, const rtabmap::Transform & velocity) { - return deskew_impl(input, output, "", 0, 0, true, velocity, previousStamp); + return deskew_impl(input, output, "", 0, 0, true, velocity); } } diff --git a/rtabmap_odom/src/nodelets/icp_odometry.cpp b/rtabmap_odom/src/nodelets/icp_odometry.cpp index a6583ca7..d795e311 100644 --- a/rtabmap_odom/src/nodelets/icp_odometry.cpp +++ b/rtabmap_odom/src/nodelets/icp_odometry.cpp @@ -396,7 +396,7 @@ private: { // deskew with constant velocity model (we are in frameId) sensor_msgs::PointCloud2 scanOutDeskewed; - if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp().toSec(), velocityGuess())) + if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, velocityGuess())) { ROS_ERROR("Failed to deskew input cloud, aborting odometry update!"); return; @@ -421,7 +421,7 @@ private: { // deskew with constant velocity model sensor_msgs::PointCloud2 scanOutDeskewed; - if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp().toSec(), velocityGuess())) + if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, velocityGuess())) { ROS_ERROR("Failed to deskew input cloud, aborting odometry update!"); return; @@ -660,7 +660,7 @@ private: } sensor_msgs::PointCloud2::Ptr cloudDeskewed(new sensor_msgs::PointCloud2); - if(!rtabmap_conversions::deskew(*cloudPtr, *cloudDeskewed, previousStamp().toSec(), velocityGuess())) + if(!rtabmap_conversions::deskew(*cloudPtr, *cloudDeskewed, velocityGuess())) { ROS_ERROR("Failed to deskew input cloud, aborting odometry update!"); return;