rtabmap_odom tests and doc (#1456)

* rtabmap_odom tests and doc * opengv note * added ci checks or humble-latest flaky dep cmake errors * Added real data tests for rgbd_odom and stereo_odom * added real data for icp_odometry's deskewing test * fixing json cmake error on lyrical/rolling * test 2d icp odom deskewing branch * first review of existing OdometryROS tests * testing with imu used as guess * tested imu arrivals sync * Fixed odom reset on right pose when guess frame id is used * fixing header errors in ci >=lyrical * Added support for input rgbd_image topic with features for odom, added multicam rgbd_odometry test * Added stereo odom support for features-only frames. Added multicam stereo tests. * forcing latest rtabmap version * updated OdometryROS API * ci: dont build non-latest docker in pull requests * splitting docker jobs * doc edit * Making publish_null_when_lost:=false continous when guess is provided (using guess covariance when we cannot register yet) * updated stereo doc * ficing rolling ci (rviz Ogre header) * Added test coverage of alll rgbd_image callbacks * fixing rolling ci * making docker ci build/run the tests on pull requests * fixing ros2 ci testing * improved sync callback coverage * improving stereo_odometry test coverage * improved icp_odometry test coverage * lyrical voxel_grid ptr error * make multicam tests working as well without opengv * removing deps of missing packages on rolling * PCL empty cloud conversion compiler errors fix * fixing icp_odometry test failure on ci witohut libpointmatcher * fixing nav2 costmap plugin build on lyrical * joining thread when exiting * updating icp test to work the same on pcl 1.15 (lyrical) * Fix parallel tests seg fault --------- Co-authored-by: mathieu86 <[email protected]>
2026-10-05 01:07:49 +08:00 · 2026-09-21 17:02:45 -07:00
parent 73c98f87a8
commit 11edc01d6a
91 changed files with 9736 additions and 350 deletions
@@ -25,6 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

+#include <rtabmap_conversions/PointCloudConversion.h>
 #include "rtabmap_odom/OdometryROS.h"

 #include <sensor_msgs/msg/image.hpp>
@@ -61,6 +62,44 @@ using namespace rtabmap;

 namespace rtabmap_odom {

+namespace {
+
+/**
+ * @brief The covariance of a pose that came from the guess frame instead of registration.
+ *
+ * Used wherever the guess is what the published pose rests on: a frame the odometry did
+ * not update because it had not moved enough, and the frame that restarts the map after
+ * a reset. Nothing was measured in either case, so the confidence is the one the guess
+ * was declared to have rather than anything the registration computed.
+ */
+cv::Mat guessCovariance(double linearVariance, double angularVariance)
+{
+	cv::Mat covariance = cv::Mat::zeros(6,6,CV_64FC1);
+	covariance.at<double>(0,0) = linearVariance;  // xx
+	covariance.at<double>(1,1) = linearVariance;  // yy
+	covariance.at<double>(2,2) = linearVariance;  // zz
+	covariance.at<double>(3,3) = angularVariance; // rr
+	covariance.at<double>(4,4) = angularVariance; // pp
+	covariance.at<double>(5,5) = angularVariance; // yawyaw
+	return covariance;
+}
+
+/**
+ * @brief The velocity a motion implies, for a frame with no registration to measure one.
+ *
+ * Named apart from the guess itself so that it can be called where a `guessVelocity`
+ * variable is in scope.
+ */
+rtabmap::Transform velocityFrom(const rtabmap::Transform & motion, double dt)
+{
+	UASSERT(dt > 0.0);
+	float x,y,z,roll,pitch,yaw;
+	motion.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+	return rtabmap::Transform(x/dt, y/dt, z/dt, roll/dt, pitch/dt, yaw/dt);
+}
+
+}  // namespace
+
 OdometryROS::OdometryROS(const rclcpp::NodeOptions & options) :
 		OdometryROS("odometry", options)
 	{}
@@ -83,6 +122,7 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
 	publishNullWhenLost_(true),
 	publishCompressedSensorData_(false),
 	qos_(RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT),
+	bufferedDataToProcess_(false),
 	paused_(false),
 	resetCountdown_(0),
 	resetCurrentCount_(0),
@@ -194,6 +234,17 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
 				"are the same frame (value=\"%s\"). \"guess_frame_id\" is disabled.", odomFrameId_.c_str());
 		guessFrameId_.clear();
 	}
+	if(!publishNullWhenLost_ && guessFrameId_.empty() && publishTf_)
+	{
+		RCLCPP_ERROR(this->get_logger(), "\"publish_null_when_lost\" is false, but nothing can "
+				"say where odometry restarts after being lost: \"guess_frame_id\" is not set and "
+				"\"publish_tf\" is true, so the %s->%s fallback returns this node's own pose. "
+				"Whatever the robot did while lost will be silently dropped from the trajectory "
+				"and mapped across. Set \"guess_frame_id\", or set \"publish_tf\" to false if "
+				"another node (e.g. robot_localization) publishes %s->%s, or leave "
+				"\"publish_null_when_lost\" true.",
+				odomFrameId_.c_str(), frameId_.c_str(), odomFrameId_.c_str(), frameId_.c_str());
+	}
 	RCLCPP_INFO(this->get_logger(), "Odometry: frame_id               = %s", frameId_.c_str());
 	RCLCPP_INFO(this->get_logger(), "Odometry: odom_frame_id          = %s", odomFrameId_.c_str());
 	RCLCPP_INFO(this->get_logger(), "Odometry: publish_tf             = %s", publishTf_?"true":"false");
@@ -628,8 +679,18 @@ void OdometryROS::processData()
 		imuProcessed_ = true;
 	}

+	// Whether this is a frame at all, as opposed to an IMU-only update. Neither the image
+	// nor the features answer that on their own: a frame that brings its own features has
+	// no image, and a frame of an empty scene has no feature. The calibration does, being
+	// there whenever a camera produced the data -- the same rule RTAB-Map's own
+	// Odometry::process() applies before registering anything.
+	const bool isFrame = !data.imageRaw().empty() ||
+			!data.cameraModels().empty() ||
+			!data.stereoCameraModels().empty() ||
+			!data.laserScanRaw().isEmpty();
+
 	Transform groundTruth;
-	if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
+	if(isFrame)
 	{
 		// Detect time jump in the past
 		double clockNow = now().seconds();
@@ -688,7 +749,7 @@ void OdometryROS::processData()
 		{
 			groundTruth = rtabmap_conversions::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, header.stamp, *tfBuffer_, waitForTransform_);

-			if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
+			if(isFrame)
 			{
 				// Use only XYZ to handle the case odometry was previously initialized with IMU,
 				// we assume that the ground truth contains also a real initial orientation
@@ -716,39 +777,6 @@ void OdometryROS::processData()
 		}
 	}

-	bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_ > 0 && rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_ > 1.0/minUpdateRate_;
-	if(tooOldPreviousData)
-	{
-		RCLCPP_WARN(this->get_logger(), "Odometry lost! Odometry will be reset because last update "
-				"is %fs too old (>%fs, min_update_rate = %f Hz). Previous data stamp is %f while new data stamp is %f.",
-				rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_, 1.0/minUpdateRate_, minUpdateRate_, previousStamp_, rtabmap_conversions::timestampFromROS(header.stamp));
-
-		if(!guess_.isNull())
-		{
-			RCLCPP_WARN(this->get_logger(), "Odometry automatically reset based on latest guess available from TF (%s->%s, moved %s since got lost)!",
-					guessFrameId_.c_str(), frameId_.c_str(), guess_.prettyPrint().c_str());
-			odometry_->reset(odometry_->getPose() * guess_);
-			guess_.setNull();
-			guessPreviousPose_.setNull();
-		}
-		else
-		{
-			// Check TF to see if sensor fusion is used (e.g., the output of robot_localization)
-			Transform tfPose = rtabmap_conversions::getTransform(odomFrameId_, frameId_, header.stamp, *tfBuffer_, waitForTransform_);
-			if(tfPose.isNull())
-			{
-				RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest computed pose!");
-				odometry_->reset(odometry_->getPose());
-			}
-			else
-			{
-				RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest odometry pose available from TF (%s->%s)!",
-						odomFrameId_.c_str(), frameId_.c_str());
-				odometry_->reset(tfPose);
-			}
-		}
-	}
-	
 	bool skipOdometryUpdate = false;

 	rtabmap::Transform pose;
@@ -756,23 +784,36 @@ void OdometryROS::processData()
 	rtabmap::Transform guessVelocity;

 	Transform guessCurrentPose;
+	// Whether the guess has a previous pose to be relative to, which decides how the pose
+	// is seeded from it further down. It is cleared by reset(), so a reset asked for
+	// through a service restarts at the guess frame while an automatic one continues from
+	// the pose it has just carried forward.
+	bool guessIsTheFirstOne = false;
 	if(!guessFrameId_.empty())
 	{
 		guessCurrentPose = rtabmap_conversions::getTransform(guessFrameId_, frameId_, header.stamp, *tfBuffer_, waitForTransform_);

 		Transform previousPose = guessPreviousPose_;
-		if(guessPreviousPose_.isNull())
+		guessIsTheFirstOne = guessPreviousPose_.isNull();
+		if(guessIsTheFirstOne)
 		{
 			previousPose = guessCurrentPose;
-			if(!guessCurrentPose.isNull() && odometry_->getPose().isIdentity())
-			{
-				RCLCPP_INFO(get_logger(), "Odometry: init pose with guess %s", guessCurrentPose.prettyPrint().c_str());
-				odometry_->reset(guessCurrentPose);
-			}
 		}

 		if(!previousPose.isNull() && !guessCurrentPose.isNull())
 		{
+			// What the guess frame says the robot is doing. This is what gets published
+			// for a frame with no registration behind it -- one skipped for not having
+			// moved enough, or one starting a new map, whose twist would otherwise be
+			// unknown although its pose comes from the guess. It is dropped further down
+			// as soon as the registration has a velocity of its own to report.
+			if(previousStamp_ > 0.0 &&
+			   rtabmap_conversions::timestampFromROS(header.stamp) > previousStamp_)
+			{
+				guessVelocity = velocityFrom(previousPose.inverse() * guessCurrentPose,
+						rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_);
+			}
+
 			if(guess_.isNull())
 			{
 				guess_ = previousPose.inverse() * guessCurrentPose;
@@ -791,20 +832,7 @@ void OdometryROS::processData()
 				{
 					// Ignore odometry update, we didn't move enough
 					pose = odometry_->getPose() * guess_;
-					info.reg.covariance = cv::Mat::zeros(6,6,CV_64FC1);
-					info.reg.covariance.at<double>(0,0) = guessLinearVariance_;  // xx
-					info.reg.covariance.at<double>(1,1) = guessLinearVariance_;  // yy
-					info.reg.covariance.at<double>(2,2) = guessLinearVariance_; // zz
-					info.reg.covariance.at<double>(3,3) = guessAngularVariance_; // rr
-					info.reg.covariance.at<double>(4,4) = guessAngularVariance_; // pp
-					info.reg.covariance.at<double>(5,5) = guessAngularVariance_; // yawyaw

-					//set velocity
-					double dt = rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_;
-					UASSERT(dt>0.0);
-					// use part of guess matching dt
-					(previousPose.inverse() * guessCurrentPose).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
-					guessVelocity = rtabmap::Transform(x/dt, y/dt, z/dt, roll/dt, pitch/dt, yaw/dt);
 					skipOdometryUpdate = true;
 				}
 			}
@@ -817,16 +845,117 @@ void OdometryROS::processData()
 		}
 	}

+	// Handled here rather than before the guess is computed: guess_ only holds the motion
+	// since the previous frame once the block above has run, and resetting without it
+	// throws away everything the guess source measured across the gap -- which is exactly
+	// what this reset is supposed to carry over.
+	bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_ > 0 && rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_ > 1.0/minUpdateRate_;
+	if(tooOldPreviousData)
+	{
+		RCLCPP_WARN(this->get_logger(), "Odometry lost! Odometry will be reset because last update "
+				"is %fs too old (>%fs, min_update_rate = %f Hz). Previous data stamp is %f while new data stamp is %f.",
+				rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_, 1.0/minUpdateRate_, minUpdateRate_, previousStamp_, rtabmap_conversions::timestampFromROS(header.stamp));
+
+		if(!guess_.isNull())
+		{
+			RCLCPP_WARN(this->get_logger(), "Odometry automatically reset based on latest guess available from TF (%s->%s, moved %s since got lost)!",
+					guessFrameId_.c_str(), frameId_.c_str(), guess_.prettyPrint().c_str());
+			odometry_->reset(odometry_->getPose() * guess_);
+			// Cleared because it has just been applied: the odometry now starts from a
+			// pose that already includes it, and leaving it would have the registration
+			// apply it a second time on the frame that initialises the new map.
+			// guessPreviousPose_ is kept, so the next frame measures its motion from this
+			// one rather than starting over and losing a frame of it.
+			guess_.setNull();
+		}
+		else
+		{
+			// Check TF to see if sensor fusion is used (e.g., the output of robot_localization)
+			Transform tfPose = rtabmap_conversions::getTransform(odomFrameId_, frameId_, header.stamp, *tfBuffer_, waitForTransform_);
+			if(tfPose.isNull())
+			{
+				RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest computed pose!");
+				odometry_->reset(odometry_->getPose());
+			}
+			else
+			{
+				RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest odometry pose available from TF (%s->%s)!",
+						odomFrameId_.c_str(), frameId_.c_str());
+				odometry_->reset(tfPose);
+			}
+		}
+	}
+
 	// process data
 	rclcpp::Time timeStart = rclcpp::Clock().now();
 	if(!groundTruth.isNull())
 	{
 		data.setGroundTruth(groundTruth);
 	}
+	// Set when the guess has already been folded into the pose below, so that a reset
+	// later in this frame does not go looking for a fallback that is no longer needed.
+	bool poseCarriedByGuess = false;
+	// Set when this frame starts a new map and the guess frame says where, which is what
+	// makes the trajectory it starts continuous with the one before it.
+	bool initialisedOnGuess = false;
 	if(!skipOdometryUpdate)
 	{
+		// This frame will initialise the odometry's map whenever no frame has been
+		// registered since the last reset -- at startup, after a service reset, or on
+		// recovery from an automatic one. Registration then returns no motion, so the pose
+		// has to be put where the guess says the robot is *before* the frame is processed:
+		// afterwards the map is already anchored in the wrong place, and the next
+		// registration measures the difference against that anchor and takes the correction
+		// straight back out. Resetting here costs nothing, the map being empty either way.
+		//
+		// There are two ways to be right, depending on what the guess can say:
+		initialisedOnGuess = odometry_->framesProcessed() == 0 && !guessCurrentPose.isNull();
+		if(initialisedOnGuess)
+		{
+			if(guessIsTheFirstOne)
+			{
+				// Nothing to be relative to. Adopt the guess source's own coordinates, so
+				// that odometry restarts where the guess says it is rather than at the
+				// origin. A pose asked for explicitly through reset_odom_to_pose is left
+				// alone: only an odometry still sitting at the identity is seeded this way.
+				if(odometry_->getPose().isIdentity())
+				{
+					RCLCPP_INFO(get_logger(), "Odometry: init pose with guess %s",
+							guessCurrentPose.prettyPrint().c_str());
+					odometry_->reset(guessCurrentPose);
+				}
+			}
+			else if(!guess_.isNull() && !guess_.isIdentity())
+			{
+				// There is a previous guess pose, so the guess describes real motion since
+				// the frame before this one -- which an automatic reset has just carried the
+				// pose through. Advance by it and the trajectory stays continuous; drop it
+				// and the new map is anchored a frame behind, once per reset, accumulating.
+				RCLCPP_DEBUG(this->get_logger(), "Odometry: advancing the pose by the guess "
+						"(%s) before the map is initialised, so the motion measured since the "
+						"previous frame is not lost.", guess_.prettyPrint().c_str());
+				odometry_->reset(odometry_->getPose() * guess_);
+				guess_.setNull();
+				poseCarriedByGuess = true;
+			}
+		}
 		pose = odometry_->process(data, guess_, &info);
 	}
+
+	// 9999 on both covariances is how rtabmap is told a frame starts a new map. When the
+	// guess frame says where it starts, and publish_null_when_lost says this consumer
+	// wants poses rather than the news of a reset, it goes out as a continuation instead.
+	const bool publishAsContinuation = initialisedOnGuess && !publishNullWhenLost_ && !pose.isNull();
+	if(skipOdometryUpdate || publishAsContinuation)
+	{
+		// Both rest on the guess rather than on a registration: its confidence, its velocity.
+		info.reg.covariance = guessCovariance(guessLinearVariance_, guessAngularVariance_);
+	}
+	else
+	{
+		// The registration measured this one, so its velocity is the one to publish.
+		guessVelocity.setNull();
+	}
 	if(!pose.isNull())
 	{
 		if(!skipOdometryUpdate) {
@@ -909,11 +1038,12 @@ void OdometryROS::processData()
 			if(setTwist)
 			{
 				float x,y,z,roll,pitch,yaw;
-				if(skipOdometryUpdate) {
-					UASSERT(!guessVelocity.isNull());
-					guessVelocity.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
-				} else {
+				// Whatever is left of the two: the registration's own velocity, or the
+				// guess's where the frame had no registration to give one.
+				if(guessVelocity.isNull()) {
 					odometry_->getVelocityGuess().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+				} else {
+					guessVelocity.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
 				}
 				odom.twist.twist.linear.x = x;
 				odom.twist.twist.linear.y = y;
@@ -931,7 +1061,7 @@ void OdometryROS::processData()
 			odom.twist.covariance.at(35) = setTwist?info.reg.covariance.at<double>(5,5):BAD_COVARIANCE; // yawyaw

 			//publish the message
-			if(setTwist || publishNullWhenLost_)
+			if(setTwist || publishNullWhenLost_ || publishAsContinuation)
 			{
 				odomPub_->publish(odom);
 			}
@@ -953,7 +1083,7 @@ void OdometryROS::processData()
 				cloud.push_back(pt);
 			}
 			sensor_msgs::msg::PointCloud2 cloudMsg;
-			pcl::toROSMsg(cloud, cloudMsg);
+			rtabmap_conversions::toPointCloud2Msg(cloud, cloudMsg);
 			cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image
 			cloudMsg.header.frame_id = odomFrameId_;
 			odomLocalMap_->publish(cloudMsg);
@@ -976,7 +1106,7 @@ void OdometryROS::processData()
 					}

 					sensor_msgs::msg::PointCloud2 cloudMsg;
-					pcl::toROSMsg(cloud, cloudMsg);
+					rtabmap_conversions::toPointCloud2Msg(cloud, cloudMsg);
 					cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image
 					cloudMsg.header.frame_id = odomFrameId_;
 					odomLastFrame_->publish(cloudMsg);
@@ -996,7 +1126,7 @@ void OdometryROS::processData()
 						cloud.push_back(pcl::PointXYZ(pt.x, pt.y, pt.z));
 					}
 					sensor_msgs::msg::PointCloud2 cloudMsg;
-					pcl::toROSMsg(cloud, cloudMsg);
+					rtabmap_conversions::toPointCloud2Msg(cloud, cloudMsg);
 					cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image
 					cloudMsg.header.frame_id = odomFrameId_;
 					odomLastFrame_->publish(cloudMsg);
@@ -1010,22 +1140,22 @@ void OdometryROS::processData()
 			if(info.localScanMap.hasNormals() && info.localScanMap.hasIntensity())
 			{
 				pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud = util3d::laserScanToPointCloudINormal(info.localScanMap, info.localScanMap.localTransform());
-				pcl::toROSMsg(*cloud, cloudMsg);
+				rtabmap_conversions::toPointCloud2Msg(*cloud, cloudMsg);
 			}
 			else if(info.localScanMap.hasNormals())
 			{
 				pcl::PointCloud<pcl::PointNormal>::Ptr cloud = util3d::laserScanToPointCloudNormal(info.localScanMap, info.localScanMap.localTransform());
-				pcl::toROSMsg(*cloud, cloudMsg);
+				rtabmap_conversions::toPointCloud2Msg(*cloud, cloudMsg);
 			}
 			else if(info.localScanMap.hasIntensity())
 			{
 				pcl::PointCloud<pcl::PointXYZI>::Ptr cloud = util3d::laserScanToPointCloudI(info.localScanMap, info.localScanMap.localTransform());
-				pcl::toROSMsg(*cloud, cloudMsg);
+				rtabmap_conversions::toPointCloud2Msg(*cloud, cloudMsg);
 			}
 			else
 			{
 				pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(info.localScanMap, info.localScanMap.localTransform());
-				pcl::toROSMsg(*cloud, cloudMsg);
+				rtabmap_conversions::toPointCloud2Msg(*cloud, cloudMsg);
 			}

 			cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image
@@ -1106,6 +1236,17 @@ void OdometryROS::processData()
 				odometry_->reset(odometry_->getPose() * guess_);
 				guess_.setNull();
 			}
+			else if(poseCarriedByGuess)
+			{
+				// The guess was folded into the pose before this frame was processed, so the
+				// pose already covers the motion since the last one. Going to TF for a
+				// fallback here would block for wait_for_transform on every lost frame and
+				// answer a question that has already been answered.
+				RCLCPP_WARN(this->get_logger(), "Odometry automatically reset, carrying the "
+						"latest guess from TF (%s->%s) that was already applied to the pose!",
+						guessFrameId_.c_str(), frameId_.c_str());
+				odometry_->reset(odometry_->getPose());
+			}
 			else
 			{
 				// Check TF to see if sensor fusion is used (e.g., the output of robot_localization)
@@ -1319,6 +1460,11 @@ void OdometryROS::reset(const Transform & pose)
 	UScopeMutex lock(dataMutex_);
 	odometry_->reset(pose);
 	guess_.setNull();
+	// Clearing this is what tells the next frame to restart from the guess frame rather
+	// than continue from here: the seeding step below cannot tell a reset asked for
+	// through a service from one the node decided on its own, and reads this instead. The
+	// automatic resets deliberately leave it alone, so that they carry on from the pose
+	// they just moved.
 	guessPreviousPose_.setNull();
 	previousStamp_ = 0.0;
 	previousClockTime_ = 0.0;
@@ -25,6 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

+#include <rtabmap_conversions/PointCloudConversion.h>
 #include <rtabmap_odom/icp_odometry.hpp>

 #include <laser_geometry/laser_geometry.hpp>
@@ -69,6 +70,7 @@ ICPOdometry::ICPOdometry(const rclcpp::NodeOptions & options) :

 ICPOdometry::~ICPOdometry()
 {
+	this->join(true);
 }

 void ICPOdometry::onOdomInit()
@@ -399,12 +401,12 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan

 	if(hasIntensity)
 	{
-		pcl::fromROSMsg(scanOut, *pclScanI);
+		rtabmap_conversions::fromPointCloud2Msg(scanOut, *pclScanI);
 		pclScanI->is_dense = true;
 	}
 	else
 	{
-		pcl::fromROSMsg(scanOut, *pclScan);
+		rtabmap_conversions::fromPointCloud2Msg(scanOut, *pclScan);
 		pclScan->is_dense = true;
 	}

@@ -519,7 +521,7 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan
 void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr pointCloudMsg)
 {
 	UASSERT_MSG(pointCloudMsg->data.size() == pointCloudMsg->row_step*pointCloudMsg->height,
-			uFormat("data=%d row_step=%d height=%d", pointCloudMsg->data.size(), pointCloudMsg->row_step, pointCloudMsg->height).c_str());
+			uFormat("data=%d row_step=%d height=%d", (int)pointCloudMsg->data.size(), (int)pointCloudMsg->row_step, (int)pointCloudMsg->height).c_str());

 	if(scanReceived_)
 	{
@@ -668,7 +670,7 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
 	if(hasNormals && hasIntensity)
 	{
 		pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZINormal>);
-		pcl::fromROSMsg(*cloudMsg, *pclScan);
+		rtabmap_conversions::fromPointCloud2Msg(*cloudMsg, *pclScan);
 		if(pclScan->size() && scanDownsamplingStep_ > 1)
 		{
 			pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
@@ -686,7 +688,7 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
 	else if(hasNormals)
 	{
 		pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
-		pcl::fromROSMsg(*cloudMsg, *pclScan);
+		rtabmap_conversions::fromPointCloud2Msg(*cloudMsg, *pclScan);
 		if(pclScan->size() && scanDownsamplingStep_ > 1)
 		{
 			pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
@@ -704,7 +706,7 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
 	else if(hasIntensity)
 	{
 		pcl::PointCloud<pcl::PointXYZI>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZI>);
-		pcl::fromROSMsg(*cloudMsg, *pclScan);
+		rtabmap_conversions::fromPointCloud2Msg(*cloudMsg, *pclScan);
 		if(pclScan->size() && scanDownsamplingStep_ > 1)
 		{
 			pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
@@ -750,7 +752,7 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
 	else
 	{
 		pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
-		pcl::fromROSMsg(*cloudMsg, *pclScan);
+		rtabmap_conversions::fromPointCloud2Msg(*cloudMsg, *pclScan);
 		if(pclScan->size() && scanDownsamplingStep_ > 1)
 		{
 			pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap_conversions/MsgConversion.h"
 #include <rtabmap_msgs/msg/rgbd_images.hpp>

+#include <rtabmap/core/Compression.h>
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util2d.h>
 #include <rtabmap/utilite/ULogger.h>
@@ -73,6 +74,8 @@ RGBDOdometry::RGBDOdometry(const rclcpp::NodeOptions & options) :

 RGBDOdometry::~RGBDOdometry()
 {
+	this->join(true);
+
 	delete approxSync_;
 	delete exactSync_;
 	delete approxSync2_;
@@ -435,40 +438,58 @@ void RGBDOdometry::updateParameters(ParametersMap & parameters)
 void RGBDOdometry::commonCallback(
 			const std::vector<cv_bridge::CvImageConstPtr> & rgbImages,
 			const std::vector<cv_bridge::CvImageConstPtr> & depthImages,
-			const std::vector<sensor_msgs::msg::CameraInfo>& cameraInfos)
+			const std::vector<sensor_msgs::msg::CameraInfo>& cameraInfos,
+			const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPointsMsgs,
+			const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3dMsgs,
+			const std::vector<cv::Mat> & localDescriptorsMsgs)
 {
 	UASSERT(rgbImages.size() > 0 && rgbImages.size() == depthImages.size() && rgbImages.size() == cameraInfos.size());
 	rclcpp::Time higherStamp;
 	UASSERT_MSG(rgbImages[0], "RGB image is null!");
-	int imageWidth = rgbImages[0]->image.cols;
-	int imageHeight = rgbImages[0]->image.rows;
 	UASSERT_MSG(depthImages[0], "Depth image is null!");
+
+	// The images are what the local features would otherwise be extracted from, so a frame
+	// that brings its own can leave them out -- which is nearly all of the bandwidth. It
+	// then describes itself with its calibration alone: how big the image would have been,
+	// where the camera is, what it sees. (An RGB-D message with no depth image at all also
+	// used to divide by zero below.)
+	const bool hasRgb = !rgbImages[0]->image.empty();
+	const bool hasDepth = !depthImages[0]->image.empty();
+
+	int imageWidth = hasRgb?rgbImages[0]->image.cols:(int)cameraInfos[0].width;
+	int imageHeight = hasRgb?rgbImages[0]->image.rows:(int)cameraInfos[0].height;
 	int depthWidth = depthImages[0]->image.cols;
 	int depthHeight = depthImages[0]->image.rows;

-	UASSERT_MSG(
-			imageWidth/depthWidth == imageHeight/depthHeight,
-			uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str());
+	if(hasDepth)
+	{
+		UASSERT_MSG(
+				imageWidth/depthWidth == imageHeight/depthHeight,
+				uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str());
+	}

 	int cameraCount = rgbImages.size();
 	cv::Mat rgb;
 	cv::Mat depth;
 	std::vector<rtabmap::CameraModel> cameraModels;
+	std::vector<cv::KeyPoint> keypoints;
+	std::vector<cv::Point3f> points3d;
+	cv::Mat descriptors;
 	for(unsigned int i=0; i<rgbImages.size(); ++i)
 	{
 		UASSERT_MSG(rgbImages[i], uFormat("RGB image is null for camera %d", i).c_str());
 		UASSERT_MSG(depthImages[i], uFormat("Depth image is null for camera %d", i).c_str());
-		if(!(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
+		if((hasRgb && !(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
 			 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
 			 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
 			 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
 			 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
 			 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
 			 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
-			 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) ||
-			!(depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
+			 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0)) ||
+			(hasDepth && !(depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
 			 depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
-			 depthImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
+			 depthImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)))
 		{
 			RCLCPP_ERROR(this->get_logger(), "Input type must be image=mono8,mono16,rgb8,bgr8,bgra8,rgba8 and "
 			"image_depth=32FC1,16UC1,mono16. Current rgb=%s and depth=%s",
@@ -476,20 +497,33 @@ void RGBDOdometry::commonCallback(
 				depthImages[i]->encoding.c_str());
 			return;
 		}
-		UASSERT_MSG(rgbImages[i]->image.cols == imageWidth && rgbImages[i]->image.rows == imageHeight,
-				uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
-						imageWidth,
-						rgbImages[i]->image.cols,
-						imageHeight,
-						rgbImages[i]->image.rows).c_str());
-		UASSERT_MSG(depthImages[i]->image.cols == depthWidth && depthImages[i]->image.rows == depthHeight,
-				uFormat("depthWidth=%d vs %d depthHeight=%d vs %d",
-						depthWidth,
-						depthImages[i]->image.cols,
-						depthHeight,
-						depthImages[i]->image.rows).c_str());
+		if(hasRgb)
+		{
+			UASSERT_MSG(rgbImages[i]->image.cols == imageWidth && rgbImages[i]->image.rows == imageHeight,
+					uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
+							imageWidth,
+							rgbImages[i]->image.cols,
+							imageHeight,
+							rgbImages[i]->image.rows).c_str());
+		}
+		if(hasDepth)
+		{
+			UASSERT_MSG(depthImages[i]->image.cols == depthWidth && depthImages[i]->image.rows == depthHeight,
+					uFormat("depthWidth=%d vs %d depthHeight=%d vs %d",
+							depthWidth,
+							depthImages[i]->image.cols,
+							depthHeight,
+							depthImages[i]->image.rows).c_str());
+		}

-		rclcpp::Time stamp = rtabmap_conversions::timestampFromROS(rgbImages[i]->header.stamp)>rtabmap_conversions::timestampFromROS(depthImages[i]->header.stamp)?rgbImages[i]->header.stamp:depthImages[i]->header.stamp;
+		// An image that is not there carries no header either, so a frame that has none is
+		// stamped and placed by its calibration, which is all it has.
+		const std::string & cameraFrameId = hasRgb?rgbImages[i]->header.frame_id:cameraInfos[i].header.frame_id;
+		rclcpp::Time stamp = cameraInfos[i].header.stamp;
+		if(hasRgb || hasDepth)
+		{
+			stamp = rtabmap_conversions::timestampFromROS(rgbImages[i]->header.stamp)>rtabmap_conversions::timestampFromROS(depthImages[i]->header.stamp)?rgbImages[i]->header.stamp:depthImages[i]->header.stamp;
+		}

 		if(i == 0)
 		{
@@ -500,7 +534,7 @@ void RGBDOdometry::commonCallback(
 			higherStamp = stamp;
 		}

-		Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), rgbImages[i]->header.frame_id, stamp, tfBuffer(), waitForTransform());
+		Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), cameraFrameId, stamp, tfBuffer(), waitForTransform());
 		if(localTransform.isNull())
 		{
 			return;
@@ -528,53 +562,75 @@ void RGBDOdometry::commonCallback(
 			}
 		}

-		cv_bridge::CvImageConstPtr ptrImage = rgbImages[i];
-		if(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
-		   rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
+		if(hasRgb)
 		{
-			if(keepColor_ && rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
+			cv_bridge::CvImageConstPtr ptrImage = rgbImages[i];
+			if(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
+			   rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
 			{
-				ptrImage = cv_bridge::cvtColor(rgbImages[i], "bgr8");
+				if(keepColor_ && rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
+				{
+					ptrImage = cv_bridge::cvtColor(rgbImages[i], "bgr8");
+				}
+				else
+				{
+					ptrImage = cv_bridge::cvtColor(rgbImages[i], "mono8");
+				}
+			}
+
+			// initialize
+			if(rgb.empty())
+			{
+				rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
+			}
+
+			if(ptrImage->image.type() == rgb.type())
+			{
+				ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
 			}
 			else
 			{
-				ptrImage = cv_bridge::cvtColor(rgbImages[i], "mono8");
+				RCLCPP_ERROR(this->get_logger(), "Some RGB images are not the same type! %d vs %d", ptrImage->image.type(), rgb.type());
+				return;
 			}
 		}

-		cv_bridge::CvImageConstPtr ptrDepth = depthImages[i];
+		if(hasDepth)
+		{
+			cv_bridge::CvImageConstPtr ptrDepth = depthImages[i];
+			if(depth.empty())
+			{
+				depth = cv::Mat(depthHeight, depthWidth*cameraCount, ptrDepth->image.type());
+			}

-		// initialize
-		if(rgb.empty())
-		{
-			rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
-		}
-		if(depth.empty())
-		{
-			depth = cv::Mat(depthHeight, depthWidth*cameraCount, ptrDepth->image.type());
-		}
-
-		if(ptrImage->image.type() == rgb.type())
-		{
-			ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
-		}
-		else
-		{
-			RCLCPP_ERROR(this->get_logger(), "Some RGB images are not the same type! %d vs %d", ptrImage->image.type(), rgb.type());
-			return;
-		}
-
-		if(ptrDepth->image.type() == depth.type())
-		{
-			ptrDepth->image.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
-		}
-		else
-		{
-			RCLCPP_ERROR(this->get_logger(), "Some Depth images are not the same type! %d vs %d", ptrDepth->image.type(), depth.type());
-			return;
+			if(ptrDepth->image.type() == depth.type())
+			{
+				ptrDepth->image.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
+			}
+			else
+			{
+				RCLCPP_ERROR(this->get_logger(), "Some Depth images are not the same type! %d vs %d", ptrDepth->image.type(), depth.type());
+				return;
+			}
 		}

 		cameraModels.push_back(rtabmap_conversions::cameraModelFromROS(cameraInfos[i], localTransform));
+
+		// The images of all cameras are stitched side by side above, so the keypoints of
+		// camera i are shifted by as many images as come before it, and their 3D points,
+		// which arrive in that camera's optical frame, are brought back to the base frame.
+		if(localKeyPointsMsgs.size() == rgbImages.size())
+		{
+			rtabmap_conversions::keypointsFromROS(localKeyPointsMsgs[i], keypoints, imageWidth*i);
+		}
+		if(localPoints3dMsgs.size() == rgbImages.size())
+		{
+			rtabmap_conversions::points3fFromROS(localPoints3dMsgs[i], points3d, localTransform);
+		}
+		if(localDescriptorsMsgs.size() == rgbImages.size())
+		{
+			descriptors.push_back(localDescriptorsMsgs[i]);
+		}
 	}

 	rtabmap::SensorData data(
@@ -584,9 +640,30 @@ void RGBDOdometry::commonCallback(
 			0,
 			rtabmap_conversions::timestampFromROS(higherStamp));

+	// Features that came with the frame are used as they are: the odometry then skips
+	// detection, description and the depth lookup that would otherwise rebuild them
+	// (see RegistrationVis, which extracts only when the frame carries no keypoints).
+	// They are dropped rather than trusted if the three of them disagree, as using them
+	// out of step would silently mismatch keypoints with their descriptors or 3D points.
+	if(!keypoints.empty())
+	{
+		if((!points3d.empty() && points3d.size() != keypoints.size()) ||
+		   (!descriptors.empty() && descriptors.rows != (int)keypoints.size()))
+		{
+			RCLCPP_ERROR(this->get_logger(), "Ignoring the local features received with this frame: "
+					"%d keypoints, %d 3D points and %d descriptors, which should be the same count "
+					"(or none at all for the 3D points and the descriptors).",
+					(int)keypoints.size(), (int)points3d.size(), descriptors.rows);
+		}
+		else
+		{
+			data.setFeatures(keypoints, points3d, descriptors);
+		}
+	}
+
 	std_msgs::msg::Header header;
 	header.stamp = higherStamp;
-	header.frame_id = rgbImages.size()==1?rgbImages[0]->header.frame_id:"";
+	header.frame_id = rgbImages.size()==1?(hasRgb?rgbImages[0]->header.frame_id:cameraInfos[0].header.frame_id):"";
 	this->processData(data, header);
 }

@@ -627,6 +704,27 @@ void RGBDOdometry::callback(
 	}
 }

+namespace {
+
+/**
+ * @brief Collects the local features one camera's image carries, cameras in order.
+ *
+ * An image that carries none pushes empty entries rather than nothing, so that the
+ * per-camera indexing still lines up with the images.
+ */
+void appendLocalFeatures(
+		const rtabmap_msgs::msg::RGBDImage & image,
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & keyPoints,
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & points3d,
+		std::vector<cv::Mat> & descriptors)
+{
+	keyPoints.push_back(image.key_points);
+	points3d.push_back(image.points);
+	descriptors.push_back(rtabmap::uncompressData(image.descriptors));
+}
+
+}  // namespace
+
 void RGBDOdometry::callbackRGBDX(
 		const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr images)
 {
@@ -642,13 +740,17 @@ void RGBDOdometry::callbackRGBDX(
 		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(images->rgbd_images.size());
 		std::vector<cv_bridge::CvImageConstPtr> depthMsgs(images->rgbd_images.size());
 		std::vector<sensor_msgs::msg::CameraInfo> infoMsgs;
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
 		for(size_t i=0; i<images->rgbd_images.size(); ++i)
 		{
 			rtabmap_conversions::toCvShare(images->rgbd_images[i], images, imageMsgs[i], depthMsgs[i]);
 			infoMsgs.push_back(images->rgbd_images[i].rgb_camera_info);
+			appendLocalFeatures(images->rgbd_images[i], localKeyPoints, localPoints3d, localDescriptors);
 		}

-		this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+		this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }

@@ -665,7 +767,12 @@ void RGBDOdometry::callbackRGBD(
 		rtabmap_conversions::toCvShare(image, imageMsgs[0], depthMsgs[0]);
 		infoMsgs.push_back(image->rgb_camera_info);

-		this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }

@@ -685,7 +792,13 @@ void RGBDOdometry::callbackRGBD2(
 		infoMsgs.push_back(image->rgb_camera_info);
 		infoMsgs.push_back(image2->rgb_camera_info);

-		this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }

@@ -708,7 +821,14 @@ void RGBDOdometry::callbackRGBD3(
 		infoMsgs.push_back(image2->rgb_camera_info);
 		infoMsgs.push_back(image3->rgb_camera_info);

-		this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }

@@ -734,7 +854,15 @@ void RGBDOdometry::callbackRGBD4(
 		infoMsgs.push_back(image3->rgb_camera_info);
 		infoMsgs.push_back(image4->rgb_camera_info);

-		this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image4, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }

@@ -763,7 +891,16 @@ void RGBDOdometry::callbackRGBD5(
 		infoMsgs.push_back(image4->rgb_camera_info);
 		infoMsgs.push_back(image5->rgb_camera_info);

-		this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image4, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image5, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }

@@ -795,7 +932,17 @@ void RGBDOdometry::callbackRGBD6(
 		infoMsgs.push_back(image5->rgb_camera_info);
 		infoMsgs.push_back(image6->rgb_camera_info);

-		this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image4, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image5, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image6, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }

@@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/Compression.h>
 #include <rtabmap/core/Odometry.h>

 using namespace rtabmap;
@@ -73,6 +74,8 @@ StereoOdometry::StereoOdometry(const rclcpp::NodeOptions & options) :

 StereoOdometry::~StereoOdometry()
 {
+	this->join(true);
+
 	delete approxSync_;
 	delete exactSync_;
 	delete approxSync2_;
@@ -407,29 +410,46 @@ void StereoOdometry::commonCallback(
 		const std::vector<cv_bridge::CvImageConstPtr> & leftImages,
 		const std::vector<cv_bridge::CvImageConstPtr> & rightImages,
 		const std::vector<sensor_msgs::msg::CameraInfo>& leftCameraInfos,
-		const std::vector<sensor_msgs::msg::CameraInfo>& rightCameraInfos)
+		const std::vector<sensor_msgs::msg::CameraInfo>& rightCameraInfos,
+		const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPointsMsgs,
+		const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3dMsgs,
+		const std::vector<cv::Mat> & localDescriptorsMsgs)
 {
 	UASSERT(leftImages.size() > 0 &&
 			leftImages.size() == rightImages.size() &&
 			leftImages.size() == leftCameraInfos.size() &&
 			rightImages.size() == rightCameraInfos.size());
 	rclcpp::Time higherStamp;
-	int leftWidth = leftImages[0]->image.cols;
-	int leftHeight = leftImages[0]->image.rows;
+
+	// The images are what the local features would otherwise be extracted from, so a frame
+	// that brings its own can leave them out -- which is nearly all of the bandwidth. It
+	// then describes itself with its calibration alone: how big the left image would have
+	// been, where the rig is, how far apart the two cameras are.
+	const bool hasImages = !leftImages[0]->image.empty() && !rightImages[0]->image.empty();
+
+	int leftWidth = hasImages?leftImages[0]->image.cols:(int)leftCameraInfos[0].width;
+	int leftHeight = hasImages?leftImages[0]->image.rows:(int)leftCameraInfos[0].height;
 	int rightWidth = rightImages[0]->image.cols;
 	int rightHeight = rightImages[0]->image.rows;

-	UASSERT_MSG(
-			leftWidth == rightWidth && leftHeight == rightHeight,
-		uFormat("left=%dx%d right=%dx%d", leftWidth, leftHeight, rightWidth, rightHeight).c_str());
+	if(hasImages)
+	{
+		UASSERT_MSG(
+				leftWidth == rightWidth && leftHeight == rightHeight,
+			uFormat("left=%dx%d right=%dx%d", leftWidth, leftHeight, rightWidth, rightHeight).c_str());
+	}

 	int cameraCount = leftImages.size();
 	cv::Mat left;
 	cv::Mat right;
 	std::vector<rtabmap::StereoCameraModel> cameraModels;
+	std::vector<cv::KeyPoint> keypoints;
+	std::vector<cv::Point3f> points3d;
+	cv::Mat descriptors;
 	for(unsigned int i=0; i<leftImages.size(); ++i)
 	{
-		if(!(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
+		if(hasImages &&
+		   (!(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
 			 leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
 			 leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
 			 leftImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
@@ -442,14 +462,21 @@ void StereoOdometry::commonCallback(
 			  rightImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
 			  rightImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
 			  rightImages[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
-			  rightImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0))
+			  rightImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0)))
 		{
 			RCLCPP_ERROR(this->get_logger(), "Input type must be image=mono8,mono16,rgb8,bgr8,rgba8,bgra8 (mono8 recommended), received types are %s (left) and %s (right)",
 					leftImages[i]->encoding.c_str(), rightImages[i]->encoding.c_str());
 			return;
 		}

-		rclcpp::Time stamp = rtabmap_conversions::timestampFromROS(leftImages[i]->header.stamp)>rtabmap_conversions::timestampFromROS(rightImages[i]->header.stamp)?leftImages[i]->header.stamp:rightImages[i]->header.stamp;
+		// An image that is not there carries no header either, so a frame that has none is
+		// stamped and placed by its calibration, which is all it has.
+		const std::string & cameraFrameId = hasImages?leftImages[i]->header.frame_id:leftCameraInfos[i].header.frame_id;
+		rclcpp::Time stamp = leftCameraInfos[i].header.stamp;
+		if(hasImages)
+		{
+			stamp = rtabmap_conversions::timestampFromROS(leftImages[i]->header.stamp)>rtabmap_conversions::timestampFromROS(rightImages[i]->header.stamp)?leftImages[i]->header.stamp:rightImages[i]->header.stamp;
+		}

 		if(i == 0)
 		{
@@ -460,7 +487,7 @@ void StereoOdometry::commonCallback(
 			higherStamp = stamp;
 		}

-		Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), leftImages[i]->header.frame_id, stamp, tfBuffer(), waitForTransform());
+		Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), cameraFrameId, stamp, tfBuffer(), waitForTransform());
 		if(localTransform.isNull())
 		{
 			return;
@@ -488,7 +515,13 @@ void StereoOdometry::commonCallback(
 			}
 		}

-		if(!leftImages[i]->image.empty() && !rightImages[i]->image.empty())
+		if(hasImages != (!leftImages[i]->image.empty() && !rightImages[i]->image.empty()))
+		{
+			RCLCPP_ERROR(this->get_logger(), "Odom: camera %d of this frame has images while "
+					"another one doesn't (or the other way around)?!?", i);
+			return;
+		}
+
 		{
 			bool alreadyRectified = true;
 			Parameters::parse(parameters(), Parameters::kRtabmapImagesAlreadyRectified(), alreadyRectified);
@@ -602,62 +635,76 @@ void StereoOdometry::commonCallback(
 					shown = true;
 				}
 			}
-			cv_bridge::CvImageConstPtr ptrLeft = leftImages[i];
-			if(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
-			   leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
+			if(hasImages)
 			{
-				if(keepColor_ && leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
+				cv_bridge::CvImageConstPtr ptrLeft = leftImages[i];
+				if(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
+				   leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
 				{
-					ptrLeft = cv_bridge::cvtColor(leftImages[i], "bgr8");
+					if(keepColor_ && leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
+					{
+						ptrLeft = cv_bridge::cvtColor(leftImages[i], "bgr8");
+					}
+					else
+					{
+						ptrLeft = cv_bridge::cvtColor(leftImages[i], "mono8");
+					}
+				}
+				cv_bridge::CvImageConstPtr ptrRight = rightImages[i];
+				if(rightImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
+				   rightImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
+				{
+					ptrRight = cv_bridge::cvtColor(rightImages[i], "mono8");
+				}
+
+				// initialize
+				if(left.empty())
+				{
+					left = cv::Mat(leftHeight, leftWidth*cameraCount, ptrLeft->image.type());
+				}
+				if(right.empty())
+				{
+					right = cv::Mat(rightHeight, rightWidth*cameraCount, ptrRight->image.type());
+				}
+
+				if(ptrLeft->image.type() == left.type())
+				{
+					ptrLeft->image.copyTo(cv::Mat(left, cv::Rect(i*leftWidth, 0, leftWidth, leftHeight)));
 				}
 				else
 				{
-					ptrLeft = cv_bridge::cvtColor(leftImages[i], "mono8");
+					RCLCPP_ERROR(this->get_logger(), "Some left images are not the same type! %d vs %d", ptrLeft->image.type(), left.type());
+					return;
 				}
-			}
-			cv_bridge::CvImageConstPtr ptrRight = rightImages[i];
-			if(rightImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
-			   rightImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
-			{
-				ptrRight = cv_bridge::cvtColor(rightImages[i], "mono8");
-			}

-			// initialize
-			if(left.empty())
-			{
-				left = cv::Mat(leftHeight, leftWidth*cameraCount, ptrLeft->image.type());
-			}
-			if(right.empty())
-			{
-				right = cv::Mat(rightHeight, rightWidth*cameraCount, ptrRight->image.type());
-			}
-
-			if(ptrLeft->image.type() == left.type())
-			{
-				ptrLeft->image.copyTo(cv::Mat(left, cv::Rect(i*leftWidth, 0, leftWidth, leftHeight)));
-			}
-			else
-			{
-				RCLCPP_ERROR(this->get_logger(), "Some left images are not the same type! %d vs %d", ptrLeft->image.type(), left.type());
-				return;
-			}
-
-			if(ptrRight->image.type() == right.type())
-			{
-				ptrRight->image.copyTo(cv::Mat(right, cv::Rect(i*rightWidth, 0, rightWidth, rightHeight)));
-			}
-			else
-			{
-				RCLCPP_ERROR(this->get_logger(), "Some right images are not the same type! %d vs %d", ptrRight->image.type(), right.type());
-				return;
+				if(ptrRight->image.type() == right.type())
+				{
+					ptrRight->image.copyTo(cv::Mat(right, cv::Rect(i*rightWidth, 0, rightWidth, rightHeight)));
+				}
+				else
+				{
+					RCLCPP_ERROR(this->get_logger(), "Some right images are not the same type! %d vs %d", ptrRight->image.type(), right.type());
+					return;
+				}
 			}

 			cameraModels.push_back(stereoModel);
 		}
-		else
+
+		// The left images of all cameras are stitched side by side above, so the keypoints
+		// of camera i are shifted by as many images as come before it, and their 3D points,
+		// which arrive in that camera's optical frame, are brought back to the base frame.
+		if(localKeyPointsMsgs.size() == leftImages.size())
 		{
-			RCLCPP_ERROR(this->get_logger(), "Odom: input images empty?!?");
-			return;
+			rtabmap_conversions::keypointsFromROS(localKeyPointsMsgs[i], keypoints, leftWidth*i);
+		}
+		if(localPoints3dMsgs.size() == leftImages.size())
+		{
+			rtabmap_conversions::points3fFromROS(localPoints3dMsgs[i], points3d, localTransform);
+		}
+		if(localDescriptorsMsgs.size() == leftImages.size())
+		{
+			descriptors.push_back(localDescriptorsMsgs[i]);
 		}
 	}

@@ -669,9 +716,30 @@ void StereoOdometry::commonCallback(
 			0,
 			rtabmap_conversions::timestampFromROS(higherStamp));

+	// Features that came with the frame are used as they are: the odometry then skips
+	// detection, description and the disparity search that would otherwise rebuild them
+	// (see RegistrationVis, which extracts only when the frame carries no keypoints).
+	// They are dropped rather than trusted if the three of them disagree, as using them
+	// out of step would silently mismatch keypoints with their descriptors or 3D points.
+	if(!keypoints.empty())
+	{
+		if((!points3d.empty() && points3d.size() != keypoints.size()) ||
+		   (!descriptors.empty() && descriptors.rows != (int)keypoints.size()))
+		{
+			RCLCPP_ERROR(this->get_logger(), "Ignoring the local features received with this frame: "
+					"%d keypoints, %d 3D points and %d descriptors, which should be the same count "
+					"(or none at all for the 3D points and the descriptors).",
+					(int)keypoints.size(), (int)points3d.size(), descriptors.rows);
+		}
+		else
+		{
+			data.setFeatures(keypoints, points3d, descriptors);
+		}
+	}
+
 	std_msgs::msg::Header header;
 	header.stamp = higherStamp;
-	header.frame_id = leftImages.size()==1?leftImages[0]->header.frame_id:"";
+	header.frame_id = leftImages.size()==1?(hasImages?leftImages[0]->header.frame_id:leftCameraInfos[0].header.frame_id):"";
 	this->processData(data, header);
 }

@@ -715,6 +783,27 @@ void StereoOdometry::callback(
 	}
 }

+namespace {
+
+/**
+ * @brief Collects the local features one camera's frame carries, cameras in order.
+ *
+ * A frame that carries none pushes empty entries rather than nothing, so that the
+ * per-camera indexing still lines up with the images.
+ */
+void appendLocalFeatures(
+		const rtabmap_msgs::msg::RGBDImage & image,
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & keyPoints,
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & points3d,
+		std::vector<cv::Mat> & descriptors)
+{
+	keyPoints.push_back(image.key_points);
+	points3d.push_back(image.points);
+	descriptors.push_back(rtabmap::uncompressData(image.descriptors));
+}
+
+}  // namespace
+
 void StereoOdometry::callbackRGBD(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image)
 {
@@ -730,7 +819,12 @@ void StereoOdometry::callbackRGBD(
 		leftInfoMsgs.push_back(image->rgb_camera_info);
 		rightInfoMsgs.push_back(image->depth_camera_info);

-		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }

@@ -750,14 +844,18 @@ void StereoOdometry::callbackRGBDX(
 		std::vector<cv_bridge::CvImageConstPtr> rightMsgs(images->rgbd_images.size());
 		std::vector<sensor_msgs::msg::CameraInfo> leftInfoMsgs;
 		std::vector<sensor_msgs::msg::CameraInfo> rightInfoMsgs;
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
 		for(size_t i=0; i<images->rgbd_images.size(); ++i)
 		{
 			rtabmap_conversions::toCvShare(images->rgbd_images[i], images, leftMsgs[i], rightMsgs[i]);
 			leftInfoMsgs.push_back(images->rgbd_images[i].rgb_camera_info);
 			rightInfoMsgs.push_back(images->rgbd_images[i].depth_camera_info);
+			appendLocalFeatures(images->rgbd_images[i], localKeyPoints, localPoints3d, localDescriptors);
 		}

-		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }

@@ -780,7 +878,13 @@ void StereoOdometry::callbackRGBD2(
 		rightInfoMsgs.push_back(image->depth_camera_info);
 		rightInfoMsgs.push_back(image2->depth_camera_info);

-		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }

@@ -807,7 +911,14 @@ void StereoOdometry::callbackRGBD3(
 		rightInfoMsgs.push_back(image2->depth_camera_info);
 		rightInfoMsgs.push_back(image3->depth_camera_info);

-		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }

@@ -838,7 +949,15 @@ void StereoOdometry::callbackRGBD4(
 		rightInfoMsgs.push_back(image3->depth_camera_info);
 		rightInfoMsgs.push_back(image4->depth_camera_info);

-		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image4, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }

@@ -873,7 +992,16 @@ void StereoOdometry::callbackRGBD5(
 		rightInfoMsgs.push_back(image4->depth_camera_info);
 		rightInfoMsgs.push_back(image5->depth_camera_info);

-		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image4, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image5, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }

@@ -912,7 +1040,17 @@ void StereoOdometry::callbackRGBD6(
 		rightInfoMsgs.push_back(image5->depth_camera_info);
 		rightInfoMsgs.push_back(image6->depth_camera_info);

-		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image4, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image5, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image6, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }