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/ros1.yml

@@ -1,4 +1,4 @@
-name: noetic-pr
+name: ros1
 
 on:
   pull_request:

README.md

@@ -16,11 +16,6 @@ For the RTAB-Map libraries and standalone application, visit [RTAB-Map's home pa
            <td><a href="https://github.com/introlab/rtabmap_ros/actions/workflows/ros1.yml"><img src="https://github.com/introlab/rtabmap_ros/actions/workflows/ros1.yml/badge.svg" alt="Build Status"/> <br> <a href="https://github.com/introlab/rtabmap_ros/actions/workflows/docker.yml"><img src="https://github.com/introlab/rtabmap_ros/actions/workflows/docker.yml/badge.svg" alt="Build Status"/>
            </td>
         </tr>
-        <tr>
-           <td>ROS 2</td>
-           <td><a href="https://github.com/introlab/rtabmap_ros/actions/workflows/ros2.yml"><img src="https://github.com/introlab/rtabmap_ros/actions/workflows/ros2.yml/badge.svg" alt="Build Status"/>
-           </td>
-        </tr>
      </tbody>
   </table>
  
@@ -33,23 +28,6 @@ For the RTAB-Map libraries and standalone application, visit [RTAB-Map's home pa
             <td>Noetic</td>
             <td><a href="http://build.ros.org/job/Nbin_ufv8_uFv8__rtabmap_ros__ubuntu_focal_arm64__binary/"><img src="http://build.ros.org/buildStatus/icon?job=Nbin_ufv8_uFv8__rtabmap_ros__ubuntu_focal_arm64__binary" alt="Build Status"/></td>
         </tr>
-        <tr>
-            <td rowspan="4">ROS 2</td>
-            <td>Humble</td>
-            <td><a href="http://build.ros2.org/job/Hbin_uJ64__rtabmap_ros__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__rtabmap_ros__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
-        </tr>
-        <tr>
-            <td>Iron</td>
-            <td><a href="http://build.ros2.org/job/Ibin_uJ64__rtabmap_ros__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__rtabmap_ros__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
-        </tr>
-        <tr>
-            <td>Jazzy</td>
-            <td><a href="http://build.ros2.org/job/Jbin_uN64__rtabmap_ros__ubuntu_noble_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Jbin_uN64__rtabmap_ros__ubuntu_noble_amd64__binary" alt="Build Status"/></td>
-        </tr>
-        <tr>
-            <td>Rolling</td>
-            <td><a href="http://build.ros2.org/job/Rbin_uJ64__rtabmap_ros__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__rtabmap_ros__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
-        </tr>
         <tr>
            <td>Docker</td>
            <td>

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);
 
 }

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[0] != 0.0)
-	if(!camInfo.K.empty())
 	{
 		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);
+	bool rIsSet = false;
-	if(!camInfo.R.empty())
+	for(size_t i=0; !rIsSet && i<camInfo.R.size(); ++i)
+	{
+		rIsSet = camInfo.R[i] != 0.0;
+	}
+	if(rIsSet)
 	{
 		R = cv::Mat(3, 3, CV_64FC1);
 		memcpy(R.data, camInfo.R.elems, 9*sizeof(double));
 	}
 
 	cv:: Mat P;
-	UASSERT(camInfo.P.empty() || camInfo.P.size() == 12);
+	if(camInfo.P[0] != 0.0)
-	if(!camInfo.P.empty())
 	{
 		P = cv::Mat(3, 4, CV_64FC1);
 		memcpy(P.data, camInfo.P.elems, 12*sizeof(double));
@@ -2768,8 +2776,7 @@ bool deskew_impl(
 		tf2_ros::Buffer * tfBuffer,
 		double waitForTransform,
 		bool slerp,
-		const rtabmap::Transform & velocity,
+		const rtabmap::Transform & velocity)
-		double previousStamp)
 {
 	if(tfBuffer != 0)
 	{
@@ -2793,12 +2800,6 @@ bool deskew_impl(
 			return false;
 		}
 
-		if(previousStamp <= 0.0)
-		{
-			ROS_ERROR("previousStamp should be >0 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());
+		// There is no time spread across the scan, so there is nothing to correct. This
-		return false;
+		// 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:
+			// Integrate the velocity directly from the stamp of the msg, which is the
-			//  1- The pose of base frame in odom frame at first stamp
+			// frame the deskewed cloud is expressed in. Going through a third, earlier
-			//  2- The pose of base frame in odom frame at msg stamp
+			// reference pose and composing it away would give the same answer for a pure
-			//  3- The pose of base frame in odom frame at last stamp
+			// translation, but not for a rotation: Transform() scales roll/pitch/yaw
-			UASSERT(firstStamp.toSec() >= previousStamp);
+			// linearly instead of using the twist exponential, so the composition only
-			UASSERT(lastStamp.toSec() > previousStamp);
+			// cancels in the small-angle limit. Keeping dt bounded by the scan duration
-			double dt1 = firstStamp.toSec() - previousStamp;
+			// is where that approximation is at its best.
-			double dt2 = input.header.stamp.toSec() - previousStamp;
+			double dt1 = firstStamp.toSec() - input.header.stamp.toSec();
-			double dt3 = lastStamp.toSec() - previousStamp;
+			double dt3 = lastStamp.toSec() - input.header.stamp.toSec();
-
-			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);
 
 			// First and last poses are relative to stamp of the msg
-			firstPose = p2.inverse() * p1;
+			firstPose = rtabmap::Transform(vx*dt1, vy*dt1, vz*dt1, vroll*dt1, vpitch*dt1, vyaw*dt1);
-			lastPose = p2.inverse() * p3;
+			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);
 }
 
 }

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;