ported rtabmap_ros_pkg_split to ros2

rtabmap_odom/CMakeLists.txt

@@ -0,0 +1,128 @@
+cmake_minimum_required(VERSION 3.5)
+project(rtabmap_odom)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# To suppress PCL_ROOT warning
+if(POLICY CMP0074)
+    cmake_policy(SET CMP0074 NEW)
+endif()
+
+find_package(ament_cmake_ros REQUIRED)
+find_package(cv_bridge REQUIRED)
+find_package(image_geometry REQUIRED)
+find_package(laser_geometry REQUIRED)
+find_package(message_filters REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(pcl_ros REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(rtabmap_conversions REQUIRED)
+find_package(rtabmap_msgs REQUIRED)
+find_package(rtabmap_util REQUIRED)
+
+include_directories(
+  ${CMAKE_CURRENT_SOURCE_DIR}/include
+)
+
+SET(Libraries
+   cv_bridge
+   image_geometry
+   laser_geometry
+   message_filters
+   nav_msgs
+   pcl_conversions
+   pcl_ros
+   rclcpp
+   sensor_msgs
+   rtabmap_conversions
+   rtabmap_msgs
+   rtabmap_util
+)
+
+###########
+## Build ##
+###########
+
+SET(rtabmap_odom_lib_src
+   src/OdometryROS.cpp
+)
+  
+SET(rtabmap_odom_plugins_lib_src
+   src/nodelets/rgbd_odometry.cpp
+   src/nodelets/stereo_odometry.cpp
+   src/nodelets/icp_odometry.cpp
+)
+
+############################
+## Declare a cpp library
+############################
+add_library(rtabmap_odom SHARED ${rtabmap_odom_lib_src})
+target_include_directories(rtabmap_odom
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
+add_library(rtabmap_odom_plugins SHARED ${rtabmap_odom_plugins_lib_src})
+
+ament_target_dependencies(rtabmap_odom ${Libraries})
+ament_target_dependencies(rtabmap_odom_plugins ${Libraries})
+
+target_link_libraries(rtabmap_odom_plugins rtabmap_odom)
+
+rclcpp_components_register_nodes(rtabmap_odom_plugins "rtabmap_odom::RGBDOdometry")
+rclcpp_components_register_nodes(rtabmap_odom_plugins "rtabmap_odom::StereoOdometry")
+rclcpp_components_register_nodes(rtabmap_odom_plugins "rtabmap_odom::ICPOdometry")
+
+
+add_executable(rtabmap_rgbd_odometry src/RGBDOdometryNode.cpp)
+ament_target_dependencies(rtabmap_rgbd_odometry ${Libraries})
+target_link_libraries(rtabmap_rgbd_odometry rtabmap_odom_plugins)
+set_target_properties(rtabmap_rgbd_odometry PROPERTIES OUTPUT_NAME "rgbd_odometry")
+
+add_executable(rtabmap_stereo_odometry src/StereoOdometryNode.cpp)
+ament_target_dependencies(rtabmap_stereo_odometry ${Libraries})
+target_link_libraries(rtabmap_stereo_odometry rtabmap_odom_plugins)
+set_target_properties(rtabmap_stereo_odometry PROPERTIES OUTPUT_NAME "stereo_odometry")
+
+add_executable(rtabmap_icp_odometry src/ICPOdometryNode.cpp)
+ament_target_dependencies(rtabmap_icp_odometry ${Libraries})
+target_link_libraries(rtabmap_icp_odometry rtabmap_odom_plugins)
+set_target_properties(rtabmap_icp_odometry PROPERTIES OUTPUT_NAME "icp_odometry")
+
+#############
+## Install ##
+#############
+
+ament_export_dependencies(${Libraries})
+ament_export_include_directories(include)
+ament_export_targets(${PROJECT_NAME})          # To include downstream with targets
+ament_export_libraries(rtabmap_odom rtabmap_odom_plugins) # To include downstream without targets
+
+install(TARGETS 
+   rtabmap_odom
+   rtabmap_odom_plugins 
+   EXPORT ${PROJECT_NAME}
+   ARCHIVE DESTINATION lib
+   LIBRARY DESTINATION lib
+   RUNTIME DESTINATION bin
+)
+
+install(TARGETS 
+   rtabmap_rgbd_odometry 
+   rtabmap_icp_odometry
+   rtabmap_stereo_odometry
+   DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+   DESTINATION include
+   FILES_MATCHING PATTERN "*.h"
+)
+
+ament_package()

rtabmap_odom/include/rtabmap_odom/OdometryROS.h

@@ -0,0 +1,174 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef ODOMETRYROS_H_
+#define ODOMETRYROS_H_
+
+#include "rclcpp/rclcpp.hpp"
+
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <std_srvs/srv/empty.hpp>
+#include <std_msgs/msg/header.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <rtabmap_msgs/msg/odom_info.hpp>
+#include <rtabmap_msgs/msg/rgbd_image.hpp>
+#include <rtabmap_msgs/srv/reset_pose.hpp>
+#include <rtabmap/core/SensorData.h>
+#include <rtabmap/core/Parameters.h>
+
+#include <boost/thread.hpp>
+
+#include "rtabmap_util/ULogToRosout.h"
+
+namespace rtabmap {
+class Odometry;
+}
+
+namespace rtabmap_odom {
+
+class OdometryROS : public rclcpp::Node
+{
+
+public:
+	explicit OdometryROS(const rclcpp::NodeOptions & options);
+	explicit OdometryROS(const std::string & name, const rclcpp::NodeOptions & options);
+	virtual ~OdometryROS();
+
+	void processData(rtabmap::SensorData & data, const std_msgs::msg::Header & header);
+
+	void resetOdom(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+	void resetToPose(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<rtabmap_msgs::srv::ResetPose::Request>, std::shared_ptr<rtabmap_msgs::srv::ResetPose::Response>);
+	void pause(const std::shared_ptr<rmw_request_id_t>,	const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+	void resume(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+	void setLogDebug(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+	void setLogInfo(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+	void setLogWarn(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+	void setLogError(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+
+	const std::string & frameId() const {return frameId_;}
+	const std::string & odomFrameId() const {return odomFrameId_;}
+	const std::string & guessFrameId() const {return guessFrameId_;}
+	const rtabmap::ParametersMap & parameters() const {return parameters_;}
+	bool isPaused() const {return paused_;}
+
+protected:
+	void init(bool stereoParams, bool visParams, bool icpParams);
+	void startWarningThread(const std::string & subscribedTopicsMsg, bool approxSync);
+	void callbackCalled() {callbackCalled_ = true;}
+	rmw_qos_reliability_policy_t qos() const {return qos_;}
+
+	virtual void flushCallbacks() {};
+	tf2_ros::Buffer & tfBuffer() {return *tfBuffer_;}
+	const double & waitForTransform() const {return waitForTransform_;}
+	rtabmap::Transform velocityGuess() const;
+	double previousStamp() const {return previousStamp_;}
+	virtual void postProcessData(const rtabmap::SensorData & /*data*/, const std_msgs::msg::Header & /*header*/) const {}
+
+private:
+	void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync);
+
+	virtual void updateParameters(rtabmap::ParametersMap &) {}
+	virtual void onOdomInit() {}
+
+	void callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg);
+	void reset(const rtabmap::Transform & pose = rtabmap::Transform::getIdentity());
+
+private:
+	rtabmap::Odometry * odometry_;
+	std::thread * warningThread_;
+	std::string subscribedTopicsMsg_;
+	bool callbackCalled_;
+
+	// parameters
+	std::string frameId_;
+	std::string odomFrameId_;
+	std::string groundTruthFrameId_;
+	std::string groundTruthBaseFrameId_;
+	std::string guessFrameId_;
+	double guessMinTranslation_;
+	double guessMinRotation_;
+	double guessMinTime_;
+	bool publishTf_;
+	double waitForTransform_;
+	bool publishNullWhenLost_;
+	rmw_qos_reliability_policy_t qos_;
+	rtabmap::ParametersMap parameters_;
+
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odomPub_;
+	rclcpp::Publisher<rtabmap_msgs::msg::OdomInfo>::SharedPtr odomInfoPub_;
+	rclcpp::Publisher<rtabmap_msgs::msg::OdomInfo>::SharedPtr odomInfoLitePub_;
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr odomLocalMap_;
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr odomLocalScanMap_;
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr odomLastFrame_;
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr odomRgbdImagePub_;
+
+	rclcpp::Service<std_srvs::srv::Empty>::SharedPtr resetSrv_;
+	rclcpp::Service<rtabmap_msgs::srv::ResetPose>::SharedPtr resetToPoseSrv_;
+	rclcpp::Service<std_srvs::srv::Empty>::SharedPtr pauseSrv_;
+	rclcpp::Service<std_srvs::srv::Empty>::SharedPtr resumeSrv_;
+	rclcpp::Service<std_srvs::srv::Empty>::SharedPtr setLogDebugSrv_;
+	rclcpp::Service<std_srvs::srv::Empty>::SharedPtr setLogInfoSrv_;
+	rclcpp::Service<std_srvs::srv::Empty>::SharedPtr setLogWarnSrv_;
+	rclcpp::Service<std_srvs::srv::Empty>::SharedPtr setLogErrorSrv_;
+
+	std::shared_ptr<tf2_ros::TransformBroadcaster> tfBroadcaster_;
+	std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
+	std::shared_ptr<tf2_ros::TransformListener> tfListener_;
+	rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imuSub_;
+
+	bool paused_;
+	int resetCountdown_;
+	int resetCurrentCount_;
+	bool stereoParams_;
+	bool visParams_;
+	bool icpParams_;
+	rtabmap::Transform guess_;
+	rtabmap::Transform guessPreviousPose_;
+	double previousStamp_;
+	double expectedUpdateRate_;
+	double maxUpdateRate_;
+	double minUpdateRate_;
+	int odomStrategy_;
+	bool waitIMUToinit_;
+	bool imuProcessed_;
+	std::map<double, rtabmap::IMU> imus_;
+	std::pair<rtabmap::SensorData, std_msgs::msg::Header > bufferedData_;
+	std::string configPath_;
+	rtabmap::Transform initialPose_;
+
+	rtabmap_util::ULogToRosout ulogToRosout_;
+};
+
+}
+
+#endif

rtabmap_odom/include/rtabmap_odom/icp_odometry.hpp

@@ -0,0 +1,88 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rclcpp/rclcpp.hpp"
+
+#include <rtabmap_odom/OdometryROS.h>
+#include <rtabmap_odom/visibility.h>
+
+//#include <pluginlib/class_list_macros.h>
+//#include <pluginlib/class_loader.hpp>
+
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+//#include "rtabmap_ros/PluginInterface.h"
+
+
+using namespace rtabmap;
+
+namespace rtabmap_odom
+{
+
+class ICPOdometry : public rtabmap_odom::OdometryROS
+{
+public:
+	RTABMAP_ODOM_PUBLIC
+	explicit ICPOdometry(const rclcpp::NodeOptions & options);
+
+	virtual ~ICPOdometry();
+
+private:
+	virtual void updateParameters(rtabmap::ParametersMap &);
+	virtual void onOdomInit();
+
+	void callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scanMsg);
+	void callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr pointCloudMsg);
+
+protected:
+	virtual void flushCallbacks();
+	void postProcessData(const SensorData & data, const std_msgs::msg::Header & header) const;
+
+private:
+	rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
+	rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_sub_;
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr filtered_scan_pub_;
+	int scanCloudMaxPoints_;
+	bool scanCloudIs2d_;
+	int scanDownsamplingStep_;
+	double scanRangeMin_;
+	double scanRangeMax_;
+	double scanVoxelSize_;
+	int scanNormalK_;
+	double scanNormalRadius_;
+	double scanNormalGroundUp_;
+	bool deskewing_;
+	bool deskewingSlerp_;
+	//std::vector<std::shared_ptr<rtabmap_ros::PluginInterface> > plugins_;
+	//pluginlib::ClassLoader<rtabmap_ros::PluginInterface> plugin_loader_;
+	bool scanReceived_ = false;
+	bool cloudReceived_ = false;
+
+};
+
+}

rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp

@@ -0,0 +1,138 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rclcpp/rclcpp.hpp"
+
+#include <rtabmap_odom/OdometryROS.h>
+#include <rtabmap_odom/visibility.h>
+
+#include <message_filters/subscriber.h>
+#include <message_filters/time_synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+
+#include <image_transport/image_transport.hpp>
+#include <image_transport/subscriber_filter.hpp>
+
+#include <sensor_msgs/msg/image.hpp>
+#include <rtabmap_msgs/msg/rgbd_image.hpp>
+#include <rtabmap_msgs/msg/rgbd_images.hpp>
+
+#include <cv_bridge/cv_bridge.h>
+
+namespace rtabmap_odom
+{
+
+class RGBDOdometry : public rtabmap_odom::OdometryROS
+{
+public:
+	RTABMAP_ODOM_PUBLIC
+	explicit RGBDOdometry(const rclcpp::NodeOptions & options);
+	virtual ~RGBDOdometry();
+
+private:
+	virtual void updateParameters(rtabmap::ParametersMap & parameters);
+	virtual void onOdomInit();
+
+	void commonCallback(
+				const std::vector<cv_bridge::CvImageConstPtr> & rgbImages,
+				const std::vector<cv_bridge::CvImageConstPtr> & depthImages,
+				const std::vector<sensor_msgs::msg::CameraInfo>& cameraInfos);
+
+	void callback(
+			const sensor_msgs::msg::Image::ConstSharedPtr image,
+			const sensor_msgs::msg::Image::ConstSharedPtr depth,
+			const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo);
+
+	void callbackRGBDX(
+			const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr images);
+
+	void callbackRGBD(
+			const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image);
+
+	void callbackRGBD2(
+			const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
+			const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2);
+
+	void callbackRGBD3(
+			const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
+			const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
+			const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3);
+
+	void callbackRGBD4(
+			const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
+			const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
+			const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
+			const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4);
+
+	void callbackRGBD5(
+			const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
+			const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
+			const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
+			const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
+			const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5);
+
+protected:
+	virtual void flushCallbacks();
+
+private:
+	image_transport::SubscriberFilter image_mono_sub_;
+	image_transport::SubscriberFilter image_depth_sub_;
+	message_filters::Subscriber<sensor_msgs::msg::CameraInfo> info_sub_;
+
+	rclcpp::Subscription<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdSub_;
+	rclcpp::Subscription<rtabmap_msgs::msg::RGBDImages>::SharedPtr rgbdxSub_;
+	message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image1_sub_;
+	message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image2_sub_;
+	message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image3_sub_;
+	message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image4_sub_;
+	message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image5_sub_;
+
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo> MyApproxSyncPolicy;
+	message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo> MyExactSyncPolicy;
+	message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
+	typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync2Policy;
+	message_filters::Synchronizer<MyApproxSync2Policy> * approxSync2_;
+	typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync2Policy;
+	message_filters::Synchronizer<MyExactSync2Policy> * exactSync2_;
+	typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync3Policy;
+	message_filters::Synchronizer<MyApproxSync3Policy> * approxSync3_;
+	typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync3Policy;
+	message_filters::Synchronizer<MyExactSync3Policy> * exactSync3_;
+	typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync4Policy;
+	message_filters::Synchronizer<MyApproxSync4Policy> * approxSync4_;
+	typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync4Policy;
+	message_filters::Synchronizer<MyExactSync4Policy> * exactSync4_;
+	typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync5Policy;
+	message_filters::Synchronizer<MyApproxSync5Policy> * approxSync5_;
+	typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync5Policy;
+	message_filters::Synchronizer<MyExactSync5Policy> * exactSync5_;
+	int queueSize_;
+	bool keepColor_;
+};
+
+}

rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp

@@ -0,0 +1,127 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rclcpp/rclcpp.hpp"
+
+#include <rtabmap_odom/OdometryROS.h>
+#include <rtabmap_odom/visibility.h>
+
+#include <message_filters/subscriber.h>
+#include <message_filters/time_synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+
+#include <image_transport/image_transport.hpp>
+#include <image_transport/subscriber_filter.hpp>
+
+#include <cv_bridge/cv_bridge.h>
+#include <sensor_msgs/msg/image.hpp>
+#include <rtabmap_msgs/msg/rgbd_image.hpp>
+#include <rtabmap_msgs/msg/rgbd_images.hpp>
+
+namespace rtabmap_odom
+{
+
+class StereoOdometry : public rtabmap_odom::OdometryROS
+{
+public:
+	RTABMAP_ODOM_PUBLIC
+	StereoOdometry(const rclcpp::NodeOptions & options);
+	virtual ~StereoOdometry();
+
+private:
+	virtual void updateParameters(rtabmap::ParametersMap & parameters);
+	virtual void onOdomInit();
+
+	void 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);
+
+	void callback(
+			const sensor_msgs::msg::Image::ConstSharedPtr imageRectLeft,
+			const sensor_msgs::msg::Image::ConstSharedPtr imageRectRight,
+			const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoLeft,
+			const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoRight);
+
+	void callbackRGBD(
+			const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image);
+	void callbackRGBDX(
+			const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr images);
+	void callbackRGBD2(
+			const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
+			const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2);
+	void callbackRGBD3(
+			const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
+			const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
+			const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3);
+	void callbackRGBD4(
+			const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
+			const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
+			const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
+			const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4);
+
+
+protected:
+	virtual void flushCallbacks();
+
+private:
+	image_transport::SubscriberFilter imageRectLeft_;
+	image_transport::SubscriberFilter imageRectRight_;
+	message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoLeft_;
+	message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoRight_;
+
+	rclcpp::Subscription<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdSub_;
+	rclcpp::Subscription<rtabmap_msgs::msg::RGBDImages>::SharedPtr rgbdxSub_;
+	message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image1_sub_;
+	message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image2_sub_;
+	message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image3_sub_;
+	message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image4_sub_;
+
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo> MyApproxSyncPolicy;
+	message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo> MyExactSyncPolicy;
+	message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
+	typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync2Policy;
+	message_filters::Synchronizer<MyApproxSync2Policy> * approxSync2_;
+	typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync2Policy;
+	message_filters::Synchronizer<MyExactSync2Policy> * exactSync2_;
+	typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync3Policy;
+	message_filters::Synchronizer<MyApproxSync3Policy> * approxSync3_;
+	typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync3Policy;
+	message_filters::Synchronizer<MyExactSync3Policy> * exactSync3_;
+	typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync4Policy;
+	message_filters::Synchronizer<MyApproxSync4Policy> * approxSync4_;
+	typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync4Policy;
+	message_filters::Synchronizer<MyExactSync4Policy> * exactSync4_;
+
+	int queueSize_;
+	bool keepColor_;
+};
+
+}
+

rtabmap_odom/include/rtabmap_odom/visibility.h

@@ -0,0 +1,58 @@
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RTABMAP_ODOM__VISIBILITY_CONTROL_H_
+#define RTABMAP_ODOM__VISIBILITY_CONTROL_H_
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+  #ifdef __GNUC__
+    #define RTABMAP_ODOM_EXPORT __attribute__ ((dllexport))
+    #define RTABMAP_ODOM_IMPORT __attribute__ ((dllimport))
+  #else
+    #define RTABMAP_ODOM_EXPORT __declspec(dllexport)
+    #define RTABMAP_ODOM_IMPORT __declspec(dllimport)
+  #endif
+  #ifdef RTABMAP_ODOM_BUILDING_DLL
+    #define RTABMAP_ODOM_PUBLIC RTABMAP_ODOM_EXPORT
+  #else
+    #define RTABMAP_ODOM_PUBLIC RTABMAP_ODOM_IMPORT
+  #endif
+  #define RTABMAP_ODOM_PUBLIC_TYPE RTABMAP_ODOM_PUBLIC
+  #define RTABMAP_ODOM_LOCAL
+#else
+  #define RTABMAP_ODOM_EXPORT __attribute__ ((visibility("default")))
+  #define RTABMAP_ODOM_IMPORT
+  #if __GNUC__ >= 4
+    #define RTABMAP_ODOM_PUBLIC __attribute__ ((visibility("default")))
+    #define RTABMAP_ODOM_LOCAL  __attribute__ ((visibility("hidden")))
+  #else
+    #define RTABMAP_ODOM_PUBLIC
+    #define RTABMAP_ODOM_LOCAL
+  #endif
+  #define RTABMAP_ODOM_PUBLIC_TYPE
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // RTABMAP_ODOM__VISIBILITY_CONTROL_H_

rtabmap_odom/package.xml

@@ -0,0 +1,34 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rtabmap_odom</name>
+  <version>0.1.0</version>
+  <description>RTAB-Map's odometry package.</description>
+  <maintainer email="[email protected]">Mathieu Labbe</maintainer>
+  <author>Mathieu Labbe</author>
+  <license>BSD</license>
+  <url type="bugtracker">https://github.com/introlab/rtabmap_ros/issues</url>
+  <url type="repository">https://github.com/introlab/rtabmap_ros</url>
+
+  <buildtool_depend>ament_cmake_ros</buildtool_depend>
+
+  <depend>cv_bridge</depend>
+  <depend>image_geometry</depend>
+  <depend>laser_geometry</depend>
+  <depend>message_filters</depend>
+  <depend>nav_msgs</depend>
+  <depend>pcl_conversions</depend>
+  <depend>pcl_ros</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+  <depend>rtabmap_conversions</depend>
+  <depend>rtabmap_msgs</depend>
+  <depend>rtabmap_util</depend>
+  
+  <export>
+	<build_type>ament_cmake</build_type>
+  </export>
+
+</package>

rtabmap_odom/src/ICPOdometryNode.cpp

@@ -0,0 +1,76 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap_odom/icp_odometry.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+
+int main(int argc, char **argv)
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+
+	// process "--params" argument
+	std::vector<std::string> arguments;
+	for(int i=1;i<argc;++i)
+	{
+		if(strcmp(argv[i], "--params") == 0)
+		{
+			rtabmap::ParametersMap parametersOdom = rtabmap::Parameters::getDefaultOdometryParameters(false, false, true);
+			for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
+			{
+				std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
+				std::cout <<
+						str <<
+						std::setw(60 - str.size()) <<
+						" [" <<
+						rtabmap::Parameters::getDescription(iter->first).c_str() <<
+						"]" <<
+						std::endl;
+			}
+			UWARN("Node will now exit after showing default odometry parameters because "
+					 "argument \"--params\" is detected!");
+			exit(0);
+		}
+		else if(strcmp(argv[i], "--udebug") == 0)
+		{
+			ULogger::setLevel(ULogger::kDebug);
+		}
+		else if(strcmp(argv[i], "--uinfo") == 0)
+		{
+			ULogger::setLevel(ULogger::kInfo);
+		}
+		arguments.push_back(argv[i]);
+	}
+
+	rclcpp::init(argc, argv);
+	rclcpp::NodeOptions options;
+	options.arguments(arguments);
+	rclcpp::spin(std::make_shared<rtabmap_odom::ICPOdometry>(options));
+	rclcpp::shutdown();
+	return 0;
+}

rtabmap_odom/src/RGBDOdometryNode.cpp

@@ -0,0 +1,76 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap_odom/rgbd_odometry.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+
+int main(int argc, char **argv)
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+
+	// process "--params" argument
+	std::vector<std::string> arguments;
+	for(int i=1;i<argc;++i)
+	{
+		if(strcmp(argv[i], "--params") == 0)
+		{
+			rtabmap::ParametersMap parametersOdom = rtabmap::Parameters::getDefaultOdometryParameters(false);
+			for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
+			{
+				std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
+				std::cout <<
+						str <<
+						std::setw(60 - str.size()) <<
+						" [" <<
+						rtabmap::Parameters::getDescription(iter->first).c_str() <<
+						"]" <<
+						std::endl;
+			}
+			UWARN("Node will now exit after showing default odometry parameters because "
+					 "argument \"--params\" is detected!");
+			exit(0);
+		}
+		else if(strcmp(argv[i], "--udebug") == 0)
+		{
+			ULogger::setLevel(ULogger::kDebug);
+		}
+		else if(strcmp(argv[i], "--uinfo") == 0)
+		{
+			ULogger::setLevel(ULogger::kInfo);
+		}
+		arguments.push_back(argv[i]);
+	}
+
+	rclcpp::init(argc, argv);
+	rclcpp::NodeOptions options;
+	options.arguments(arguments);
+	rclcpp::spin(std::make_shared<rtabmap_odom::RGBDOdometry>(options));
+	rclcpp::shutdown();
+	return 0;
+}

rtabmap_odom/src/StereoOdometryNode.cpp

@@ -0,0 +1,76 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap_odom/stereo_odometry.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+int main(int argc, char **argv)
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+
+	// process "--params" argument
+	std::vector<std::string> arguments;
+	for(int i=1;i<argc;++i)
+	{
+		if(strcmp(argv[i], "--params") == 0)
+		{
+			rtabmap::ParametersMap parametersOdom = rtabmap::Parameters::getDefaultOdometryParameters(true);
+			for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
+			{
+				std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
+				std::cout <<
+						str <<
+						std::setw(60 - str.size()) <<
+						" [" <<
+						rtabmap::Parameters::getDescription(iter->first).c_str() <<
+						"]" <<
+						std::endl;
+			}
+			UWARN("Node will now exit after showing default odometry parameters because "
+					 "argument \"--params\" is detected!");
+			exit(0);
+		}
+		else if(strcmp(argv[i], "--udebug") == 0)
+		{
+			ULogger::setLevel(ULogger::kDebug);
+		}
+		else if(strcmp(argv[i], "--uinfo") == 0)
+		{
+			ULogger::setLevel(ULogger::kInfo);
+		}
+		arguments.push_back(argv[i]);
+
+	}
+
+	rclcpp::init(argc, argv);
+	rclcpp::NodeOptions options;
+	options.arguments(arguments);
+	rclcpp::spin(std::make_shared<rtabmap_odom::StereoOdometry>(options));
+	rclcpp::shutdown();
+	return 0;
+}

rtabmap_odom/src/nodelets/icp_odometry.cpp

@@ -0,0 +1,820 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap_odom/icp_odometry.hpp>
+
+#include <laser_geometry/laser_geometry.hpp>
+
+#include <pcl_conversions/pcl_conversions.h>
+
+#include "rtabmap_conversions/MsgConversion.h"
+
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util3d_surface.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/util2d.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UStl.h>
+
+using namespace rtabmap;
+
+namespace rtabmap_odom
+{
+
+ICPOdometry::ICPOdometry(const rclcpp::NodeOptions & options) :
+	OdometryROS("icp_odometry", options),
+	scanCloudMaxPoints_(0),
+	scanCloudIs2d_(false),
+	scanDownsamplingStep_(1),
+	scanRangeMin_(0),
+	scanRangeMax_(0),
+	scanVoxelSize_(0.0),
+	scanNormalK_(0),
+	scanNormalRadius_(0.0),
+	scanNormalGroundUp_(0.0),
+	deskewing_(false),
+	deskewingSlerp_(false),
+	scanReceived_(false),
+	cloudReceived_(false)
+{
+	OdometryROS::init(false, false, true);
+}
+
+ICPOdometry::~ICPOdometry()
+{
+	//plugins_.clear();
+}
+
+void ICPOdometry::onOdomInit()
+{
+	scanCloudMaxPoints_ = this->declare_parameter("scan_cloud_max_points", scanCloudMaxPoints_);
+	scanCloudIs2d_ = this->declare_parameter("scan_cloud_is_2d", scanCloudIs2d_);
+	scanDownsamplingStep_ = this->declare_parameter("scan_downsampling_step", scanDownsamplingStep_);
+	scanRangeMin_ = this->declare_parameter("scan_range_min", scanRangeMin_);
+	scanRangeMax_ = this->declare_parameter("scan_range_max", scanRangeMax_);
+	scanVoxelSize_ = this->declare_parameter("scan_voxel_size", scanVoxelSize_);
+	scanNormalK_ = this->declare_parameter("scan_normal_k", scanNormalK_);
+	scanNormalRadius_ = this->declare_parameter("scan_normal_radius", scanNormalRadius_);
+	scanNormalGroundUp_ = this->declare_parameter("scan_normal_ground_up", scanNormalGroundUp_);
+	deskewing_ = this->declare_parameter("deskewing", deskewing_);
+	deskewingSlerp_ = this->declare_parameter("deskewing_slerp", deskewingSlerp_);
+
+	RCLCPP_INFO(this->get_logger(), "IcpOdometry: qos                    = %d", (int)qos());
+	RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_cloud_max_points  = %d", scanCloudMaxPoints_);
+	RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_cloud_is_2d       = %s", scanCloudIs2d_?"true":"false");
+	RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_downsampling_step = %d", scanDownsamplingStep_);
+	RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_range_min         = %f m", scanRangeMin_);
+	RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_range_max         = %f m", scanRangeMax_);
+	RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_voxel_size        = %f m", scanVoxelSize_);
+	RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_k          = %d", scanNormalK_);
+	RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_radius     = %f m", scanNormalRadius_);
+	RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_ground_up  = %f", scanNormalGroundUp_);
+	RCLCPP_INFO(this->get_logger(), "IcpOdometry: deskewing              = %s", deskewing_?"true":"false");
+	RCLCPP_INFO(this->get_logger(), "IcpOdometry: deskewing_slerp        = %s", deskewingSlerp_?"true":"false");
+
+	scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>("scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackScan, this, std::placeholders::_1));
+	cloud_sub_ = create_subscription<sensor_msgs::msg::PointCloud2>("scan_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackCloud, this, std::placeholders::_1));
+
+	filtered_scan_pub_ = create_publisher<sensor_msgs::msg::PointCloud2>("odom_filtered_input_scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()));
+}
+
+void ICPOdometry::updateParameters(ParametersMap & parameters)
+{
+	//make sure we are using Reg/Strategy=0
+	ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy());
+	if(iter != parameters.end() && iter->second.compare("1") != 0)
+	{
+		RCLCPP_WARN(this->get_logger(), "ICP odometry works only with \"Reg/Strategy\"=1. Ignoring value %s.", iter->second.c_str());
+	}
+	uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "1"));
+
+	iter = parameters.find(Parameters::kIcpDownsamplingStep());
+	if(iter != parameters.end())
+	{
+		int value = uStr2Int(iter->second);
+		if(value > 1)
+		{
+			if(!this->has_parameter("scan_downsampling_step"))
+			{
+				RCLCPP_WARN(this->get_logger(), "IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_downsampling_step\" for convenience. \"%s\" is set to 1.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str());
+				scanDownsamplingStep_ = value;
+				iter->second = "1";
+			}
+			else
+			{
+				RCLCPP_WARN(this->get_logger(), "IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_downsampling_step\" are set.", iter->first.c_str());
+			}
+		}
+	}
+	iter = parameters.find(Parameters::kIcpRangeMin());
+	if(iter != parameters.end())
+	{
+		float value = uStr2Float(iter->second);
+		if(value != 0.0f)
+		{
+			if(!this->has_parameter("scan_range_min"))
+			{
+				RCLCPP_WARN(this->get_logger(), "IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_range_min\" for convenience. \"%s\" is set to 0.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str());
+				scanRangeMin_ = value;
+				iter->second = "0";
+			}
+			else
+			{
+				RCLCPP_WARN(this->get_logger(), "IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_range_min\" are set.", iter->first.c_str());
+			}
+		}
+	}
+	iter = parameters.find(Parameters::kIcpRangeMax());
+	if(iter != parameters.end())
+	{
+		float value = uStr2Float(iter->second);
+		if(value != 0.0f)
+		{
+			if(!this->has_parameter("scan_range_max"))
+			{
+				RCLCPP_WARN(this->get_logger(), "IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_range_max\" for convenience. \"%s\" is set to 0.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str());
+				scanRangeMax_ = value;
+				iter->second = "0";
+			}
+			else
+			{
+				RCLCPP_WARN(this->get_logger(), "IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_range_max\" are set.", iter->first.c_str());
+			}
+		}
+	}
+	iter = parameters.find(Parameters::kIcpVoxelSize());
+	if(iter != parameters.end())
+	{
+		float value = uStr2Float(iter->second);
+		if(value != 0.0f)
+		{
+			if(!this->has_parameter("scan_voxel_size"))
+			{
+				RCLCPP_WARN(this->get_logger(), "IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_voxel_size\" for convenience. \"%s\" is set to 0.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str());
+				scanVoxelSize_ = value;
+				iter->second = "0";
+			}
+			else
+			{
+				RCLCPP_WARN(this->get_logger(), "IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_voxel_size\" are set.", iter->first.c_str());
+			}
+		}
+	}
+	else if(this->has_parameter("scan_voxel_size"))
+	{
+		RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_voxel_size is set (%f), setting %s to 0", scanVoxelSize_, Parameters::kIcpVoxelSize().c_str());
+		parameters.insert(ParametersPair(Parameters::kIcpVoxelSize(), "0"));
+	}
+	iter = parameters.find(Parameters::kIcpPointToPlaneK());
+	if(iter != parameters.end())
+	{
+		int value = uStr2Int(iter->second);
+		if(value != 0)
+		{
+			if(!this->has_parameter("scan_normal_k"))
+			{
+				RCLCPP_WARN(this->get_logger(), "IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_k\" for convenience.", iter->second.c_str(), iter->first.c_str());
+				scanNormalK_ = value;
+			}
+			else
+			{
+				RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_k is set (%d), setting %s to same value.", scanNormalK_, Parameters::kIcpPointToPlaneK().c_str());
+				iter->second = uNumber2Str(scanNormalK_);
+			}
+		}
+	}
+	else if(this->has_parameter("scan_normal_k"))
+	{
+		RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_k is set (%d), setting %s to same value.", scanNormalK_, Parameters::kIcpPointToPlaneK().c_str());
+		parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneK(), uNumber2Str(scanNormalK_)));
+	}
+	iter = parameters.find(Parameters::kIcpPointToPlaneRadius());
+	if(iter != parameters.end())
+	{
+		float value = uStr2Float(iter->second);
+		if(value != 0.0f)
+		{
+			if(!this->has_parameter("scan_normal_radius"))
+			{
+				RCLCPP_WARN(this->get_logger(), "IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_radius\" for convenience.", iter->second.c_str(), iter->first.c_str());
+				scanNormalRadius_ = value;
+			}
+			else
+			{
+				RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_radius is set (%f), setting %s to same value.", scanNormalRadius_, Parameters::kIcpPointToPlaneRadius().c_str());
+				iter->second = uNumber2Str(scanNormalK_);
+			}
+		}
+	}
+	else if(this->has_parameter("scan_normal_radius"))
+	{
+		RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_radius is set (%f), setting %s to same value.", scanNormalRadius_, Parameters::kIcpPointToPlaneRadius().c_str());
+		parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneRadius(), uNumber2Str(scanNormalRadius_)));
+	}
+	iter = parameters.find(Parameters::kIcpPointToPlaneGroundNormalsUp());
+	if(iter != parameters.end())
+	{
+		float value = uStr2Float(iter->second);
+		if(value != 0.0f)
+		{
+			if(!this->has_parameter("scan_normal_ground_up"))
+			{
+				RCLCPP_WARN(get_logger(), "IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_ground_up\" for convenience.", iter->second.c_str(), iter->first.c_str());
+				scanNormalGroundUp_ = value;
+			}
+			else
+			{
+				RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_ground_up is set (%f), setting %s to same value.", scanNormalGroundUp_, Parameters::kIcpPointToPlaneGroundNormalsUp().c_str());
+				iter->second = uNumber2Str(scanNormalK_);
+			}
+		}
+	}
+	else if(this->has_parameter("scan_normal_ground_up"))
+	{
+		RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_ground_up is set (%f), setting %s to same value.", scanNormalGroundUp_, Parameters::kIcpPointToPlaneGroundNormalsUp().c_str());
+		parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneGroundNormalsUp(), uNumber2Str(scanNormalGroundUp_)));
+	}
+}
+
+void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scanMsg)
+{
+	if(cloudReceived_)
+	{
+		RCLCPP_ERROR(this->get_logger(), "%s is already receiving clouds on \"%s\", but also "
+				"just received a scan on \"%s\". Both subscribers cannot be "
+				"used at the same time! Disabling scan subscriber.",
+				get_name(), cloud_sub_->get_topic_name(), scan_sub_->get_topic_name());
+		scan_sub_.reset();
+		return;
+	}
+	scanReceived_ = true;
+	if(this->isPaused())
+	{
+		return;
+	}
+
+	// make sure the frame of the laser is updated too
+	Transform localScanTransform = rtabmap_conversions::getTransform(this->frameId(),
+			scanMsg->header.frame_id,
+			scanMsg->header.stamp,
+			tfBuffer(), waitForTransform());
+	if(localScanTransform.isNull())
+	{
+		RCLCPP_ERROR(this->get_logger(), "TF of received laser scan topic at time %fs is not set, aborting odometry update.", rtabmap_conversions::timestampFromROS(scanMsg->header.stamp));
+		return;
+	}
+
+	//transform in frameId_ frame
+	sensor_msgs::msg::PointCloud2 scanOut;
+	laser_geometry::LaserProjection projection;
+	if(deskewing_ && (!guessFrameId().empty() || (frameId().compare(scanMsg->header.frame_id) != 0)))
+	{
+		// make sure the frame of the laser is updated during the whole scan time
+		rtabmap::Transform tmpT = rtabmap_conversions::getTransform(
+				scanMsg->header.frame_id,
+				guessFrameId().empty()?frameId():guessFrameId(),
+				scanMsg->header.stamp,
+				rclcpp::Time(scanMsg->header.stamp.sec, scanMsg->header.stamp.nanosec) + rclcpp::Duration::from_seconds(scanMsg->ranges.size()*scanMsg->time_increment),
+				this->tfBuffer(),
+				this->waitForTransform());
+		if(tmpT.isNull())
+		{
+			return;
+		}
+
+		projection.transformLaserScanToPointCloud(
+			guessFrameId().empty()?frameId():guessFrameId(),
+			*scanMsg,
+			scanOut,
+			this->tfBuffer(),
+			laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Timestamp);
+
+		if(guessFrameId().empty() && previousStamp() > 0 && !velocityGuess().isNull())
+		{
+			// deskew with constant velocity model (we are in frameId)
+			sensor_msgs::msg::PointCloud2 scanOutDeskewed;
+			if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp(), velocityGuess()))
+			{
+				RCLCPP_ERROR(this->get_logger(), "Failed to deskew input cloud, aborting odometry update!");
+				return;
+			}
+			scanOut = scanOutDeskewed;
+		}
+
+		rtabmap::Transform t = rtabmap_conversions::getTransform(scanMsg->header.frame_id, scanOut.header.frame_id, scanMsg->header.stamp, tfBuffer(), waitForTransform());
+		if(t.isNull())
+		{
+			RCLCPP_ERROR(this->get_logger(), "Cannot transform back projected scan from \"%s\" frame to \"%s\" frame at time %fs.",
+					scanOut.header.frame_id.c_str(), scanMsg->header.frame_id.c_str(), rtabmap_conversions::timestampFromROS(scanMsg->header.stamp));
+			return;
+		}
+
+		sensor_msgs::msg::PointCloud2 scanOutDeskewed;
+		rtabmap_conversions::transformPointCloud(t.toEigen4f(), scanOut, scanOutDeskewed);
+		scanOut = scanOutDeskewed;
+	}
+	else
+	{
+		projection.projectLaser(*scanMsg, scanOut, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Timestamp);
+
+		if(deskewing_ && previousStamp() > 0 && !velocityGuess().isNull())
+		{
+			// deskew with constant velocity model
+			sensor_msgs::msg::PointCloud2 scanOutDeskewed;
+			if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp(), velocityGuess()))
+			{
+				RCLCPP_ERROR(this->get_logger(), "Failed to deskew input cloud, aborting odometry update!");
+				return;
+			}
+			scanOut = scanOutDeskewed;
+		}
+	}
+
+	bool hasIntensity = false;
+	for(unsigned int i=0; i<scanOut.fields.size(); ++i)
+	{
+		if(scanOut.fields[i].name.compare("intensity") == 0)
+		{
+			if(scanOut.fields[i].datatype == sensor_msgs::msg::PointField::FLOAT32)
+			{
+				hasIntensity = true;
+			}
+			else
+			{
+				static bool warningShown = false;
+				if(!warningShown)
+				{
+					RCLCPP_WARN(get_logger(), "The input scan cloud has an \"intensity\" field "
+							"but the datatype (%d) is not supported. Intensity will be ignored. "
+							"This message is only shown once.", scanOut.fields[i].datatype);
+					warningShown = true;
+				}
+			}
+		}
+	}
+
+	pcl::PointCloud<pcl::PointXYZI>::Ptr pclScanI(new pcl::PointCloud<pcl::PointXYZI>);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
+
+	if(hasIntensity)
+	{
+		pcl::fromROSMsg(scanOut, *pclScanI);
+		pclScanI->is_dense = true;
+	}
+	else
+	{
+		pcl::fromROSMsg(scanOut, *pclScan);
+		pclScan->is_dense = true;
+	}
+
+	LaserScan scan;
+	int maxLaserScans = (int)scanMsg->ranges.size();
+	if(!pclScan->empty() || !pclScanI->empty())
+	{
+		if(scanDownsamplingStep_ > 1)
+		{
+			if(hasIntensity)
+			{
+				pclScanI = util3d::downsample(pclScanI, scanDownsamplingStep_);
+			}
+			else
+			{
+				pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
+			}
+			maxLaserScans /= scanDownsamplingStep_;
+		}
+		if(scanVoxelSize_ > 0.0f)
+		{
+			float pointsBeforeFiltering;
+			float pointsAfterFiltering;
+			if(hasIntensity)
+			{
+				pointsBeforeFiltering = (float)pclScanI->size();
+				pclScanI = util3d::voxelize(pclScanI, scanVoxelSize_);
+				pointsAfterFiltering = (float)pclScanI->size();
+			}
+			else
+			{
+				pointsBeforeFiltering = (float)pclScan->size();
+				pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
+				pointsAfterFiltering = (float)pclScan->size();
+			}
+			float ratio = pointsAfterFiltering / pointsBeforeFiltering;
+			maxLaserScans = int(float(maxLaserScans) * ratio);
+		}
+		if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
+		{
+			//compute normals
+			pcl::PointCloud<pcl::Normal>::Ptr normals;
+			if(scanVoxelSize_ > 0.0f)
+			{
+				if(hasIntensity)
+				{
+					normals = util3d::computeNormals2D(pclScanI, scanNormalK_, scanNormalRadius_);
+				}
+				else
+				{
+					normals = util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
+				}
+			}
+			else
+			{
+				if(hasIntensity)
+				{
+					normals = util3d::computeFastOrganizedNormals2D(pclScanI, scanNormalK_, scanNormalRadius_);
+				}
+				else
+				{
+					normals = util3d::computeFastOrganizedNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
+				}
+			}
+			pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScanINormal;
+			pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal;
+			if(hasIntensity)
+			{
+				pclScanINormal.reset(new pcl::PointCloud<pcl::PointXYZINormal>);
+				pcl::concatenateFields(*pclScanI, *normals, *pclScanINormal);
+				scan = util3d::laserScan2dFromPointCloud(*pclScanINormal);
+			}
+			else
+			{
+				pclScanNormal.reset(new pcl::PointCloud<pcl::PointNormal>);
+				pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
+				scan = util3d::laserScan2dFromPointCloud(*pclScanNormal);
+			}
+		}
+		else
+		{
+			if(hasIntensity)
+			{
+				scan = util3d::laserScan2dFromPointCloud(*pclScanI);
+			}
+			else
+			{
+				scan = util3d::laserScan2dFromPointCloud(*pclScan);
+			}
+		}
+	}
+
+	if(scanRangeMin_ > 0 || scanRangeMax_ > 0)
+	{
+		scan = util3d::rangeFiltering(scan, scanRangeMin_, scanRangeMax_);
+	}
+
+	rtabmap::SensorData data(
+			LaserScan(scan,
+					maxLaserScans,
+					scanRangeMax_>0&&scanRangeMax_<scanMsg->range_max?scanRangeMax_:scanMsg->range_max,
+					localScanTransform),
+			cv::Mat(),
+			cv::Mat(),
+			CameraModel(),
+			0,
+			rtabmap_conversions::timestampFromROS(scanMsg->header.stamp));
+
+	this->processData(data, scanMsg->header);
+}
+
+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());
+
+	if(scanReceived_)
+	{
+		RCLCPP_ERROR(this->get_logger(), "%s is already receiving scans on \"%s\", but also "
+				"just received a cloud on \"%s\". Both subscribers cannot be "
+				"used at the same time! Disabling cloud subscriber.",
+				this->get_name(), scan_sub_->get_topic_name(), cloud_sub_->get_topic_name());
+		cloud_sub_.reset();
+		return;
+	}
+	cloudReceived_ = true;
+	if(this->isPaused())
+	{
+		return;
+	}
+
+	std::shared_ptr<sensor_msgs::msg::PointCloud2> cloudMsg(new sensor_msgs::msg::PointCloud2);
+	*cloudMsg = *pointCloudMsg;
+
+	rtabmap::Transform localScanTransform = rtabmap_conversions::getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp, this->tfBuffer(), this->waitForTransform());
+	if(localScanTransform.isNull())
+	{
+		RCLCPP_ERROR(this->get_logger(), "TF of received scan cloud at time %fs is not set, aborting rtabmap update.", rtabmap_conversions::timestampFromROS(cloudMsg->header.stamp));
+		return;
+	}
+
+	if(deskewing_)
+	{
+		if(!guessFrameId().empty())
+		{
+			// deskew with TF
+			if(!rtabmap_conversions::deskew(*pointCloudMsg, *cloudMsg, guessFrameId(), tfBuffer(), waitForTransform(), deskewingSlerp_))
+			{
+				RCLCPP_ERROR(this->get_logger(), "Failed to deskew input cloud, aborting odometry update!");
+				return;
+			}
+		}
+		else if(previousStamp() > 0 && !velocityGuess().isNull())
+		{
+			// deskew with constant velocity model
+			bool alreadyInBaseFrame = frameId().compare(pointCloudMsg->header.frame_id) == 0;
+			std::shared_ptr<sensor_msgs::msg::PointCloud2> cloudInBaseFrame;
+			std::shared_ptr<sensor_msgs::msg::PointCloud2> cloudPtr = cloudMsg;
+			if(!alreadyInBaseFrame)
+			{
+				// transform in base frame
+				rtabmap::Transform t = rtabmap_conversions::getTransform(frameId(), pointCloudMsg->header.frame_id, pointCloudMsg->header.stamp, tfBuffer(), waitForTransform());
+				if(t.isNull())
+				{
+					RCLCPP_ERROR(this->get_logger(), "Cannot transform cloud from \"%s\" frame to \"%s\" frame at time %fs.",
+							pointCloudMsg->header.frame_id.c_str(), frameId().c_str(), rtabmap_conversions::timestampFromROS(pointCloudMsg->header.stamp));
+					return;
+				}
+
+				cloudInBaseFrame.reset(new sensor_msgs::msg::PointCloud2);
+				rtabmap_conversions::transformPointCloud(t.toEigen4f(), *pointCloudMsg, *cloudInBaseFrame);
+				cloudPtr = cloudInBaseFrame;
+			}
+
+			std::shared_ptr<sensor_msgs::msg::PointCloud2> cloudDeskewed(new sensor_msgs::msg::PointCloud2);
+			if(!rtabmap_conversions::deskew(*cloudPtr, *cloudDeskewed, previousStamp(), velocityGuess()))
+			{
+				RCLCPP_ERROR(this->get_logger(), "Failed to deskew input cloud, aborting odometry update!");
+				return;
+			}
+
+			if(!alreadyInBaseFrame)
+			{
+				// put back in scan frame
+				rtabmap::Transform t = rtabmap_conversions::getTransform(pointCloudMsg->header.frame_id, frameId(), pointCloudMsg->header.stamp, tfBuffer(), waitForTransform());
+				if(t.isNull())
+				{
+					RCLCPP_ERROR(this->get_logger(), "Cannot transform cloud from \"%s\" frame to \"%s\" frame at time %fs.",
+							frameId().c_str(), pointCloudMsg->header.frame_id.c_str(), rtabmap_conversions::timestampFromROS(pointCloudMsg->header.stamp));
+					return;
+				}
+				rtabmap_conversions::transformPointCloud(t.toEigen4f(), *cloudDeskewed, *cloudMsg);
+			}
+			else
+			{
+				cloudMsg = cloudDeskewed;
+			}
+		}
+	}
+
+	LaserScan scan;
+	bool hasNormals = false;
+	bool hasIntensity = false;
+	bool is3D = false;
+	for(unsigned int i=0; i<cloudMsg->fields.size(); ++i)
+	{
+		if(scanVoxelSize_ == 0.0f && cloudMsg->fields[i].name.compare("normal_x") == 0)
+		{
+			hasNormals = true;
+		}
+		if(cloudMsg->fields[i].name.compare("z") == 0 && !scanCloudIs2d_)
+		{
+			is3D = true;
+		}
+		if(cloudMsg->fields[i].name.compare("intensity") == 0)
+		{
+			if(cloudMsg->fields[i].datatype == sensor_msgs::msg::PointField::FLOAT32)
+			{
+				hasIntensity = true;
+			}
+			else
+			{
+				static bool warningShown = false;
+				if(!warningShown)
+				{
+					RCLCPP_WARN(this->get_logger(), "The input scan cloud has an \"intensity\" field "
+							"but the datatype (%d) is not supported. Intensity will be ignored. "
+							"This message is only shown once.", cloudMsg->fields[i].datatype);
+					warningShown = true;
+				}
+			}
+		}
+	}
+
+	if(scanCloudMaxPoints_ == 0 && cloudMsg->height > 1)
+	{
+		scanCloudMaxPoints_ = cloudMsg->height * cloudMsg->width;
+		RCLCPP_WARN(this->get_logger(), "IcpOdometry: \"scan_cloud_max_points\" is not set but input "
+				"cloud is not dense, for convenience it will be set to %d (%dx%d)",
+				scanCloudMaxPoints_, cloudMsg->width, cloudMsg->height);
+	}
+	else if(cloudMsg->height > 1 && scanCloudMaxPoints_ < int(cloudMsg->height * cloudMsg->width))
+	{
+		RCLCPP_WARN(this->get_logger(), "IcpOdometry: \"scan_cloud_max_points\" is set to %d but input "
+				"cloud is not dense and has a size of %d (%dx%d), setting to this later size.",
+				scanCloudMaxPoints_, cloudMsg->width *cloudMsg->height, cloudMsg->width, cloudMsg->height);
+		scanCloudMaxPoints_ = cloudMsg->width *cloudMsg->height;
+	}
+	int maxLaserScans = scanCloudMaxPoints_;
+
+	if(hasNormals && hasIntensity)
+	{
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZINormal>);
+		pcl::fromROSMsg(*cloudMsg, *pclScan);
+		if(pclScan->size() && scanDownsamplingStep_ > 1)
+		{
+			pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
+			if(pclScan->height>1)
+			{
+				maxLaserScans = pclScan->height * pclScan->width;
+			}
+			else
+			{
+				maxLaserScans /= scanDownsamplingStep_;
+			}
+		}
+		scan = is3D?util3d::laserScanFromPointCloud(*pclScan):util3d::laserScan2dFromPointCloud(*pclScan);
+	}
+	else if(hasNormals)
+	{
+		pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
+		pcl::fromROSMsg(*cloudMsg, *pclScan);
+		if(pclScan->size() && scanDownsamplingStep_ > 1)
+		{
+			pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
+			if(pclScan->height>1)
+			{
+				maxLaserScans = pclScan->height * pclScan->width;
+			}
+			else
+			{
+				maxLaserScans /= scanDownsamplingStep_;
+			}
+		}
+		scan = is3D?util3d::laserScanFromPointCloud(*pclScan):util3d::laserScan2dFromPointCloud(*pclScan);
+	}
+	else if(hasIntensity)
+	{
+		pcl::PointCloud<pcl::PointXYZI>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZI>);
+		pcl::fromROSMsg(*cloudMsg, *pclScan);
+		if(pclScan->size() && scanDownsamplingStep_ > 1)
+		{
+			pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
+			if(pclScan->height>1)
+			{
+				maxLaserScans = pclScan->height * pclScan->width;
+			}
+			else
+			{
+				maxLaserScans /= scanDownsamplingStep_;
+			}
+		}
+		if(!pclScan->is_dense)
+		{
+			pclScan = util3d::removeNaNFromPointCloud(pclScan);
+		}
+
+		if(pclScan->size())
+		{
+			if(scanVoxelSize_ > 0.0f)
+			{
+				float pointsBeforeFiltering = (float)pclScan->size();
+				pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
+				float ratio = float(pclScan->size()) / pointsBeforeFiltering;
+				maxLaserScans = int(float(maxLaserScans) * ratio);
+			}
+			if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
+			{
+				//compute normals
+				pcl::PointCloud<pcl::Normal>::Ptr normals = is3D?
+						util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_):
+						util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
+				pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointXYZINormal>);
+				pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
+				scan = is3D?util3d::laserScanFromPointCloud(*pclScanNormal):util3d::laserScan2dFromPointCloud(*pclScanNormal);
+			}
+			else
+			{
+				scan = is3D?util3d::laserScanFromPointCloud(*pclScan):util3d::laserScan2dFromPointCloud(*pclScan);
+			}
+		}
+	}
+	else
+	{
+		pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::fromROSMsg(*cloudMsg, *pclScan);
+		if(pclScan->size() && scanDownsamplingStep_ > 1)
+		{
+			pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
+			if(pclScan->height>1)
+			{
+				maxLaserScans = pclScan->height * pclScan->width;
+			}
+			else
+			{
+				maxLaserScans /= scanDownsamplingStep_;
+			}
+
+		}
+		if(!pclScan->is_dense)
+		{
+			pclScan = util3d::removeNaNFromPointCloud(pclScan);
+		}
+
+		if(pclScan->size())
+		{
+			if(scanVoxelSize_ > 0.0f)
+			{
+				float pointsBeforeFiltering = (float)pclScan->size();
+				pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
+				float ratio = float(pclScan->size()) / pointsBeforeFiltering;
+				maxLaserScans = int(float(maxLaserScans) * ratio);
+			}
+			if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
+			{
+				//compute normals
+				pcl::PointCloud<pcl::Normal>::Ptr normals = is3D?
+						util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_):
+						util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
+				pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
+				pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
+				scan = is3D?util3d::laserScanFromPointCloud(*pclScanNormal):util3d::laserScan2dFromPointCloud(*pclScanNormal);
+			}
+			else
+			{
+				scan = is3D?util3d::laserScanFromPointCloud(*pclScan):util3d::laserScan2dFromPointCloud(*pclScan);
+			}
+		}
+	}
+
+	LaserScan laserScan(scan,
+			maxLaserScans,
+			0,
+			localScanTransform);
+	if(scanRangeMin_ > 0 || scanRangeMax_ > 0)
+	{
+		laserScan = util3d::rangeFiltering(laserScan, scanRangeMin_, scanRangeMax_);
+	}
+	if(!laserScan.isEmpty() && laserScan.hasNormals() && !laserScan.is2d() && scanNormalGroundUp_)
+	{
+		laserScan = util3d::adjustNormalsToViewPoint(laserScan, Eigen::Vector3f(0,0,10), (float)scanNormalGroundUp_);
+	}
+
+	rtabmap::SensorData data(
+			laserScan,
+			cv::Mat(),
+			cv::Mat(),
+			CameraModel(),
+			0,
+			rtabmap_conversions::timestampFromROS(cloudMsg->header.stamp));
+
+	this->processData(data, cloudMsg->header);
+}
+
+void ICPOdometry::flushCallbacks()
+{
+	// flush callbacks
+}
+
+void ICPOdometry::postProcessData(const SensorData & data, const std_msgs::msg::Header & header) const
+{
+	if(filtered_scan_pub_->get_subscription_count())
+	{
+		sensor_msgs::msg::PointCloud2::UniquePtr msg(new sensor_msgs::msg::PointCloud2);
+		pcl_conversions::fromPCL(*rtabmap::util3d::laserScanToPointCloud2(data.laserScanRaw()), *msg);
+		msg->header = header;
+		filtered_scan_pub_->publish(std::move(msg));
+	}
+}
+
+}
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_odom::ICPOdometry)

rtabmap_odom/src/nodelets/rgbd_odometry.cpp

@@ -0,0 +1,763 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap_odom/rgbd_odometry.hpp>
+
+#include <image_geometry/stereo_camera_model.h>
+
+#include <sensor_msgs/image_encodings.hpp>
+
+#include "rtabmap_conversions/MsgConversion.h"
+#include <rtabmap_msgs/msg/rgbd_images.hpp>
+
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util2d.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UStl.h>
+
+using namespace rtabmap;
+
+namespace rtabmap_odom
+{
+
+RGBDOdometry::RGBDOdometry(const rclcpp::NodeOptions & options) :
+		OdometryROS("rgbd_odometry", options),
+		approxSync_(0),
+		exactSync_(0),
+		approxSync2_(0),
+		exactSync2_(0),
+		approxSync3_(0),
+		exactSync3_(0),
+		approxSync4_(0),
+		exactSync4_(0),
+		approxSync5_(0),
+		exactSync5_(0),
+		queueSize_(5),
+		keepColor_(false)
+{
+	OdometryROS::init(false, true, false);
+}
+
+RGBDOdometry::~RGBDOdometry()
+{
+	delete approxSync_;
+	delete exactSync_;
+	delete approxSync2_;
+	delete exactSync2_;
+	delete approxSync3_;
+	delete exactSync3_;
+	delete approxSync4_;
+	delete exactSync4_;
+	delete approxSync5_;
+	delete exactSync5_;
+}
+
+void RGBDOdometry::onOdomInit()
+{
+	int rgbdCameras = 1;
+	bool approxSync = true;
+	bool subscribeRGBD = false;
+	double approxSyncMaxInterval = 0.0;
+	approxSync = this->declare_parameter("approx_sync", approxSync);
+	approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
+	queueSize_ = this->declare_parameter("queue_size", queueSize_);
+	int qosCamInfo = this->declare_parameter("qos_camera_info", (int)qos());
+	subscribeRGBD = this->declare_parameter("subscribe_rgbd", subscribeRGBD);
+	rgbdCameras = this->declare_parameter("rgbd_cameras", rgbdCameras);
+	if(rgbdCameras <= 0)
+	{
+		rgbdCameras = 1;
+	}
+	if(rgbdCameras > 5)
+	{
+		RCLCPP_FATAL(this->get_logger(), "Only 5 cameras maximum supported yet. Set 0 to use rgbd_images input (for which rgbdx_sync node can sync up to 8 cameras).");
+	}
+	keepColor_ = this->declare_parameter("keep_color", keepColor_);
+
+	RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync    = %s", approxSync?"true":"false");
+	if(approxSync)
+		RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval);
+	RCLCPP_INFO(this->get_logger(), "RGBDOdometry: queue_size     = %d", queueSize_);
+	RCLCPP_INFO(this->get_logger(), "RGBDOdometry: qos            = %d", (int)qos());
+	RCLCPP_INFO(this->get_logger(), "RGBDOdometry: qos_camera_info = %d", qosCamInfo);
+	RCLCPP_INFO(this->get_logger(), "RGBDOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
+	RCLCPP_INFO(this->get_logger(), "RGBDOdometry: rgbd_cameras   = %d", rgbdCameras);
+	RCLCPP_INFO(this->get_logger(), "RGBDOdometry: keep_color     = %s", keepColor_?"true":"false");
+
+	std::string subscribedTopicsMsg;
+	if(subscribeRGBD)
+	{
+		if(rgbdCameras >= 2)
+		{
+			rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
+			rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
+			if(rgbdCameras >= 3)
+			{
+				rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
+			}
+			if(rgbdCameras >= 4)
+			{
+				rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
+			}
+			if(rgbdCameras >= 5)
+			{
+				rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
+			}
+
+			if(rgbdCameras == 2)
+			{
+				if(approxSync)
+				{
+					approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
+							MyApproxSync2Policy(queueSize_),
+							rgbd_image1_sub_,
+							rgbd_image2_sub_);
+					if(approxSyncMaxInterval > 0.0)
+						approxSync2_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
+					approxSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
+				}
+				else
+				{
+					exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
+							MyExactSync2Policy(queueSize_),
+							rgbd_image1_sub_,
+							rgbd_image2_sub_);
+					exactSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
+				}
+				subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s,\n   %s",
+						get_name(),
+						approxSync?"approx":"exact",
+						approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
+						rgbd_image1_sub_.getSubscriber()->get_topic_name(),
+						rgbd_image2_sub_.getSubscriber()->get_topic_name());
+			}
+			else if(rgbdCameras == 3)
+			{
+				if(approxSync)
+				{
+					approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
+							MyApproxSync3Policy(queueSize_),
+							rgbd_image1_sub_,
+							rgbd_image2_sub_,
+							rgbd_image3_sub_);
+					if(approxSyncMaxInterval > 0.0)
+							approxSync3_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
+					approxSync3_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD3, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+				}
+				else
+				{
+					exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
+							MyExactSync3Policy(queueSize_),
+							rgbd_image1_sub_,
+							rgbd_image2_sub_,
+							rgbd_image3_sub_);
+					exactSync3_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD3, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+				}
+				subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s,\n   %s,\n   %s",
+						get_name(),
+						approxSync?"approx":"exact",
+						approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
+						rgbd_image1_sub_.getSubscriber()->get_topic_name(),
+						rgbd_image2_sub_.getSubscriber()->get_topic_name(),
+						rgbd_image3_sub_.getSubscriber()->get_topic_name());
+			}
+			else if(rgbdCameras == 4)
+			{
+				if(approxSync)
+				{
+					approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
+							MyApproxSync4Policy(queueSize_),
+							rgbd_image1_sub_,
+							rgbd_image2_sub_,
+							rgbd_image3_sub_,
+							rgbd_image4_sub_);
+					if(approxSyncMaxInterval > 0.0)
+						approxSync4_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
+					approxSync4_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
+				}
+				else
+				{
+					exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
+							MyExactSync4Policy(queueSize_),
+							rgbd_image1_sub_,
+							rgbd_image2_sub_,
+							rgbd_image3_sub_,
+							rgbd_image4_sub_);
+					exactSync4_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
+				}
+				subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s,\n   %s,\n   %s,\n   %s",
+						get_name(),
+						approxSync?"approx":"exact",
+						approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
+						rgbd_image1_sub_.getSubscriber()->get_topic_name(),
+						rgbd_image2_sub_.getSubscriber()->get_topic_name(),
+						rgbd_image3_sub_.getSubscriber()->get_topic_name(),
+						rgbd_image4_sub_.getSubscriber()->get_topic_name());
+			}
+			else if(rgbdCameras == 5)
+			{
+				if(approxSync)
+				{
+					approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>(
+							MyApproxSync5Policy(queueSize_),
+							rgbd_image1_sub_,
+							rgbd_image2_sub_,
+							rgbd_image3_sub_,
+							rgbd_image4_sub_,
+							rgbd_image5_sub_);
+					if(approxSyncMaxInterval > 0.0)
+						approxSync5_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
+					approxSync5_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
+				}
+				else
+				{
+					exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>(
+							MyExactSync5Policy(queueSize_),
+							rgbd_image1_sub_,
+							rgbd_image2_sub_,
+							rgbd_image3_sub_,
+							rgbd_image4_sub_,
+							rgbd_image5_sub_);
+					exactSync5_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
+				}
+				subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s \\\n  %s \\\n  %s \\\n   %s \\\n   %s",
+						get_name(),
+						approxSync?"approx":"exact",
+						approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
+						rgbd_image1_sub_.getSubscriber()->get_topic_name(),
+						rgbd_image2_sub_.getSubscriber()->get_topic_name(),
+						rgbd_image3_sub_.getSubscriber()->get_topic_name(),
+						rgbd_image4_sub_.getSubscriber()->get_topic_name(),
+						rgbd_image5_sub_.getSubscriber()->get_topic_name());
+			}
+		}
+		else if(rgbdCameras == 0)
+		{
+			rgbdxSub_ = create_subscription<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBDX, this, std::placeholders::_1));
+
+			subscribedTopicsMsg = uFormat("\n%s subscribed to:\n   %s",
+					get_name(),
+					rgbdxSub_->get_topic_name());
+		}
+		else
+		{
+			rgbdSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBD, this, std::placeholders::_1));
+
+			subscribedTopicsMsg =
+					uFormat("\n%s subscribed to:\n   %s",
+					get_name(),
+					rgbdSub_->get_topic_name());
+		}
+	}
+	else
+	{
+		image_transport::TransportHints hints(this);
+		image_mono_sub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
+		image_depth_sub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
+		info_sub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
+
+		if(approxSync)
+		{
+			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
+			if(approxSyncMaxInterval > 0.0)
+				approxSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
+			approxSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+		}
+		else
+		{
+			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
+			exactSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+		}
+
+		subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s,\n   %s,\n   %s",
+				get_name(),
+				approxSync?"approx":"exact",
+				approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
+				image_mono_sub_.getSubscriber().getTopic().c_str(),
+				image_depth_sub_.getSubscriber().getTopic().c_str(),
+				info_sub_.getSubscriber()->get_topic_name());
+	}
+	this->startWarningThread(subscribedTopicsMsg, approxSync);
+}
+
+void RGBDOdometry::updateParameters(ParametersMap & parameters)
+{
+	//make sure we are using Reg/Strategy=0
+	ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy());
+	if(iter != parameters.end() && iter->second.compare("0") != 0)
+	{
+		RCLCPP_WARN(this->get_logger(), "RGBD odometry works only with \"Reg/Strategy\"=0. Ignoring value %s.", iter->second.c_str());
+	}
+	uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "0"));
+
+	int estimationType = Parameters::defaultVisEstimationType();
+	Parameters::parse(parameters, Parameters::kVisEstimationType(), estimationType);
+	int rgbdCameras = 1;
+	bool subscribeRGBD = false;
+	this->get_parameter("subscribe_rgbd", subscribeRGBD);
+	this->get_parameter("rgbd_cameras", rgbdCameras);
+	if(subscribeRGBD && rgbdCameras> 1 && estimationType>0)
+	{
+		RCLCPP_WARN(this->get_logger(), "Setting \"%s\" parameter to 0 (%d is not supported "
+				"for multi-cameras) as \"subscribe_rgbd\" is "
+				"true and \"rgbd_cameras\">1. Set \"%s\" to 0 to suppress this warning.",
+				Parameters::kVisEstimationType().c_str(),
+				estimationType,
+				Parameters::kVisEstimationType().c_str());
+		uInsert(parameters, ParametersPair(Parameters::kVisEstimationType(), "0"));
+	}
+}
+
+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)
+{
+	UASSERT(rgbImages.size() > 0 && rgbImages.size() == depthImages.size() && rgbImages.size() == cameraInfos.size());
+	rclcpp::Time higherStamp;
+	int imageWidth = rgbImages[0]->image.cols;
+	int imageHeight = rgbImages[0]->image.rows;
+	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());
+
+	int cameraCount = rgbImages.size();
+	cv::Mat rgb;
+	cv::Mat depth;
+	std::vector<rtabmap::CameraModel> cameraModels;
+	for(unsigned int i=0; i<rgbImages.size(); ++i)
+	{
+		if(!(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 ||
+			 depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 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",
+				rgbImages[i]->encoding.c_str(),
+				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());
+
+		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;
+
+		if(i == 0)
+		{
+			higherStamp = stamp;
+		}
+		else if(stamp > higherStamp)
+		{
+			higherStamp = stamp;
+		}
+
+		Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), rgbImages[i]->header.frame_id, stamp, tfBuffer(), waitForTransform());
+		if(localTransform.isNull())
+		{
+			return;
+		}
+
+		if(i>0)
+		{
+			double stampDiff = fabs(rtabmap_conversions::timestampFromROS(rgbImages[i]->header.stamp) - rtabmap_conversions::timestampFromROS(rgbImages[i-1]->header.stamp));
+			if(stampDiff > 1.0/60.0)
+			{
+				static bool warningShown = false;
+				if(!warningShown)
+				{
+					RCLCPP_WARN(this->get_logger(), "The time difference between cameras %d and %d is "
+							"high (diff=%fs, cam%d=%fs, cam%d=%fs). You may want "
+							"to set approx_sync_max_interval to reject bad synchronizations or use "
+							"approx_sync=false if streams have all the exact same timestamp. This "
+							"message is only printed once.",
+							i-1, i,
+							stampDiff,
+							i-1, rtabmap_conversions::timestampFromROS(rgbImages[i-1]->header.stamp),
+							i, rtabmap_conversions::timestampFromROS(rgbImages[i]->header.stamp));
+					warningShown = true;
+				}
+			}
+		}
+
+		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(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");
+			}
+		}
+
+		cv_bridge::CvImageConstPtr ptrDepth = depthImages[i];
+
+		// 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;
+		}
+
+		cameraModels.push_back(rtabmap_conversions::cameraModelFromROS(cameraInfos[i], localTransform));
+	}
+
+	rtabmap::SensorData data(
+			rgb,
+			depth,
+			cameraModels,
+			0,
+			rtabmap_conversions::timestampFromROS(higherStamp));
+
+	std_msgs::msg::Header header;
+	header.stamp = higherStamp;
+	header.frame_id = rgbImages.size()==1?rgbImages[0]->header.frame_id:"";
+	this->processData(data, header);
+}
+
+void RGBDOdometry::callback(
+		const sensor_msgs::msg::Image::ConstSharedPtr image,
+		const sensor_msgs::msg::Image::ConstSharedPtr depth,
+		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
+{
+	callbackCalled();
+	if(!this->isPaused())
+	{
+		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
+		std::vector<cv_bridge::CvImageConstPtr> depthMsgs(1);
+		std::vector<sensor_msgs::msg::CameraInfo> infoMsgs;
+		imageMsgs[0] = cv_bridge::toCvShare(image);
+		depthMsgs[0] = cv_bridge::toCvShare(depth);
+		infoMsgs.push_back(*cameraInfo);
+
+		double stampDiff = fabs(rtabmap_conversions::timestampFromROS(image->header.stamp) - rtabmap_conversions::timestampFromROS(depth->header.stamp));
+		if(stampDiff > 0.020)
+		{
+			RCLCPP_WARN(this->get_logger(), "The time difference between rgb and depth frames is "
+					"high (diff=%fs, rgb=%fs, depth=%fs). You may want "
+					"to set approx_sync_max_interval lower than 0.02s to reject spurious bad synchronizations or use "
+					"approx_sync=false if streams have all the exact same timestamp.",
+					stampDiff,
+					rtabmap_conversions::timestampFromROS(image->header.stamp),
+					rtabmap_conversions::timestampFromROS(depth->header.stamp));
+		}
+
+		this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+	}
+}
+
+void RGBDOdometry::callbackRGBDX(
+		const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr images)
+{
+	callbackCalled();
+	if(!this->isPaused())
+	{
+		if(images->rgbd_images.empty())
+		{
+			RCLCPP_ERROR(this->get_logger(), "Input topic \"%s\" doesn't contain any image(s)!", rgbdxSub_->get_topic_name());
+			return;
+		}
+		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;
+		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);
+		}
+
+		this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+	}
+}
+
+void RGBDOdometry::callbackRGBD(
+		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image)
+{
+	callbackCalled();
+	if(!this->isPaused())
+	{
+		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
+		std::vector<cv_bridge::CvImageConstPtr> depthMsgs(1);
+		std::vector<sensor_msgs::msg::CameraInfo> infoMsgs;
+		rtabmap_conversions::toCvShare(image, imageMsgs[0], depthMsgs[0]);
+		infoMsgs.push_back(image->rgb_camera_info);
+
+		this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+	}
+}
+
+void RGBDOdometry::callbackRGBD2(
+		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
+		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2)
+{
+	callbackCalled();
+	if(!this->isPaused())
+	{
+		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(2);
+		std::vector<cv_bridge::CvImageConstPtr> depthMsgs(2);
+		std::vector<sensor_msgs::msg::CameraInfo> infoMsgs;
+		rtabmap_conversions::toCvShare(image, imageMsgs[0], depthMsgs[0]);
+		rtabmap_conversions::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
+		infoMsgs.push_back(image->rgb_camera_info);
+		infoMsgs.push_back(image2->rgb_camera_info);
+
+		this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+	}
+}
+
+void RGBDOdometry::callbackRGBD3(
+		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
+		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
+		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3)
+{
+	callbackCalled();
+	if(!this->isPaused())
+	{
+		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(3);
+		std::vector<cv_bridge::CvImageConstPtr> depthMsgs(3);
+		std::vector<sensor_msgs::msg::CameraInfo> infoMsgs;
+		rtabmap_conversions::toCvShare(image, imageMsgs[0], depthMsgs[0]);
+		rtabmap_conversions::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
+		rtabmap_conversions::toCvShare(image3, imageMsgs[2], depthMsgs[2]);
+		infoMsgs.push_back(image->rgb_camera_info);
+		infoMsgs.push_back(image2->rgb_camera_info);
+		infoMsgs.push_back(image3->rgb_camera_info);
+
+		this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+	}
+}
+
+void RGBDOdometry::callbackRGBD4(
+		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
+		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
+		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
+		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4)
+{
+	callbackCalled();
+	if(!this->isPaused())
+	{
+		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(4);
+		std::vector<cv_bridge::CvImageConstPtr> depthMsgs(4);
+		std::vector<sensor_msgs::msg::CameraInfo> infoMsgs;
+		rtabmap_conversions::toCvShare(image, imageMsgs[0], depthMsgs[0]);
+		rtabmap_conversions::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
+		rtabmap_conversions::toCvShare(image3, imageMsgs[2], depthMsgs[2]);
+		rtabmap_conversions::toCvShare(image4, imageMsgs[3], depthMsgs[3]);
+		infoMsgs.push_back(image->rgb_camera_info);
+		infoMsgs.push_back(image2->rgb_camera_info);
+		infoMsgs.push_back(image3->rgb_camera_info);
+		infoMsgs.push_back(image4->rgb_camera_info);
+
+		this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+	}
+}
+
+void RGBDOdometry::callbackRGBD5(
+		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
+		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
+		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
+		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
+		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5)
+{
+	callbackCalled();
+	if(!this->isPaused())
+	{
+		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(5);
+		std::vector<cv_bridge::CvImageConstPtr> depthMsgs(5);
+		std::vector<sensor_msgs::msg::CameraInfo> infoMsgs;
+		rtabmap_conversions::toCvShare(image, imageMsgs[0], depthMsgs[0]);
+		rtabmap_conversions::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
+		rtabmap_conversions::toCvShare(image3, imageMsgs[2], depthMsgs[2]);
+		rtabmap_conversions::toCvShare(image4, imageMsgs[3], depthMsgs[3]);
+		rtabmap_conversions::toCvShare(image5, imageMsgs[4], depthMsgs[4]);
+		infoMsgs.push_back(image->rgb_camera_info);
+		infoMsgs.push_back(image2->rgb_camera_info);
+		infoMsgs.push_back(image3->rgb_camera_info);
+		infoMsgs.push_back(image4->rgb_camera_info);
+		infoMsgs.push_back(image5->rgb_camera_info);
+
+		this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+	}
+}
+
+void RGBDOdometry::flushCallbacks()
+{
+	// flush callbacks
+	if(approxSync_)
+	{
+		delete approxSync_;
+		approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
+		approxSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+	}
+	if(exactSync_)
+	{
+		delete exactSync_;
+		exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
+		exactSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+	}
+	if(approxSync2_)
+	{
+		delete approxSync2_;
+		approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
+				MyApproxSync2Policy(queueSize_),
+				rgbd_image1_sub_,
+				rgbd_image2_sub_);
+		approxSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
+	}
+	if(exactSync2_)
+	{
+		delete exactSync2_;
+		exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
+				MyExactSync2Policy(queueSize_),
+				rgbd_image1_sub_,
+				rgbd_image2_sub_);
+		exactSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
+	}
+	if(approxSync3_)
+	{
+		delete approxSync3_;
+		approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
+				MyApproxSync3Policy(queueSize_),
+				rgbd_image1_sub_,
+				rgbd_image2_sub_,
+				rgbd_image3_sub_);
+		approxSync3_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD3, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+	}
+	if(exactSync3_)
+	{
+		delete exactSync3_;
+		exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
+				MyExactSync3Policy(queueSize_),
+				rgbd_image1_sub_,
+				rgbd_image2_sub_,
+				rgbd_image3_sub_);
+		exactSync3_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD3, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+	}
+	if(approxSync4_)
+	{
+		delete approxSync4_;
+		approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
+				MyApproxSync4Policy(queueSize_),
+				rgbd_image1_sub_,
+				rgbd_image2_sub_,
+				rgbd_image3_sub_,
+				rgbd_image4_sub_);
+		approxSync4_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
+	}
+	if(exactSync4_)
+	{
+		delete exactSync4_;
+		exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
+				MyExactSync4Policy(queueSize_),
+				rgbd_image1_sub_,
+				rgbd_image2_sub_,
+				rgbd_image3_sub_,
+				rgbd_image4_sub_);
+		exactSync4_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
+	}
+	if(approxSync5_)
+	{
+		delete approxSync5_;
+		approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>(
+				MyApproxSync5Policy(queueSize_),
+				rgbd_image1_sub_,
+				rgbd_image2_sub_,
+				rgbd_image3_sub_,
+				rgbd_image4_sub_,
+				rgbd_image5_sub_);
+		approxSync5_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
+	}
+	if(exactSync5_)
+	{
+		delete exactSync5_;
+		exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>(
+				MyExactSync5Policy(queueSize_),
+				rgbd_image1_sub_,
+				rgbd_image2_sub_,
+				rgbd_image3_sub_,
+				rgbd_image4_sub_,
+				rgbd_image5_sub_);
+		exactSync5_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
+	}
+}
+
+}
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_odom::RGBDOdometry)

rtabmap_odom/src/nodelets/stereo_odometry.cpp

@@ -0,0 +1,825 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap_odom/stereo_odometry.hpp>
+
+#include <sensor_msgs/image_encodings.hpp>
+
+#include <image_geometry/stereo_camera_model.h>
+
+#include "rtabmap_conversions/MsgConversion.h"
+#include <rtabmap_msgs/msg/rgbd_images.hpp>
+
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/Odometry.h>
+
+using namespace rtabmap;
+
+namespace rtabmap_odom
+{
+
+StereoOdometry::StereoOdometry(const rclcpp::NodeOptions & options) :
+		rtabmap_odom::OdometryROS("stereo_odometry", options),
+		approxSync_(0),
+		exactSync_(0),
+		approxSync2_(0),
+		exactSync2_(0),
+		approxSync3_(0),
+		exactSync3_(0),
+		approxSync4_(0),
+		exactSync4_(0),
+		queueSize_(5),
+		keepColor_(false)
+{
+	OdometryROS::init(true, true, false);
+}
+
+StereoOdometry::~StereoOdometry()
+{
+	delete approxSync_;
+	delete exactSync_;
+}
+
+void StereoOdometry::onOdomInit()
+{
+	bool approxSync = false;
+	bool subscribeRGBD = false;
+	double approxSyncMaxInterval = 0.0;
+	int rgbdCameras = 1;
+	approxSync = this->declare_parameter("approx_sync", approxSync);
+	approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
+	queueSize_ = this->declare_parameter("queue_size", queueSize_);
+	int qosCamInfo = this->declare_parameter("qos_camera_info", (int)qos());
+	subscribeRGBD = this->declare_parameter("subscribe_rgbd", subscribeRGBD);
+	rgbdCameras = this->declare_parameter("rgbd_cameras", rgbdCameras);
+	keepColor_ = this->declare_parameter("keep_color", keepColor_);
+
+	RCLCPP_INFO(this->get_logger(), "StereoOdometry: approx_sync = %s", approxSync?"true":"false");
+	if(approxSync)
+		RCLCPP_INFO(this->get_logger(), "StereoOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval);
+	RCLCPP_INFO(this->get_logger(), "StereoOdometry: queue_size  = %d", queueSize_);
+	RCLCPP_INFO(this->get_logger(), "RGBDOdometry: qos            = %d", (int)qos());
+	RCLCPP_INFO(this->get_logger(), "RGBDOdometry: qos_camera_info = %d", qosCamInfo);
+	RCLCPP_INFO(this->get_logger(), "StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
+	RCLCPP_INFO(this->get_logger(), "StereoOdometry: keep_color     = %s", keepColor_?"true":"false");
+
+	std::string subscribedTopicsMsg;
+	if(subscribeRGBD)
+	{
+		if(rgbdCameras >= 2)
+		{
+			rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
+			rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
+			if(rgbdCameras >= 3)
+			{
+				rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
+			}
+			if(rgbdCameras >= 4)
+			{
+				rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
+			}
+
+			if(rgbdCameras == 2)
+			{
+				if(approxSync)
+				{
+					approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
+							MyApproxSync2Policy(queueSize_),
+							rgbd_image1_sub_,
+							rgbd_image2_sub_);
+					if(approxSyncMaxInterval > 0.0)
+						approxSync2_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
+					approxSync2_->registerCallback(std::bind(&StereoOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
+				}
+				else
+				{
+					exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
+							MyExactSync2Policy(queueSize_),
+							rgbd_image1_sub_,
+							rgbd_image2_sub_);
+					exactSync2_->registerCallback(std::bind(&StereoOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
+				}
+				subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s \\\n   %s",
+						get_name(),
+						approxSync?"approx":"exact",
+						approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
+						rgbd_image1_sub_.getTopic().c_str(),
+						rgbd_image2_sub_.getTopic().c_str());
+			}
+			else if(rgbdCameras == 3)
+			{
+				if(approxSync)
+				{
+					approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
+							MyApproxSync3Policy(queueSize_),
+							rgbd_image1_sub_,
+							rgbd_image2_sub_,
+							rgbd_image3_sub_);
+					if(approxSyncMaxInterval > 0.0)
+						approxSync3_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
+					approxSync3_->registerCallback(std::bind(&StereoOdometry::callbackRGBD3, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+				}
+				else
+				{
+					exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
+							MyExactSync3Policy(queueSize_),
+							rgbd_image1_sub_,
+							rgbd_image2_sub_,
+							rgbd_image3_sub_);
+					exactSync3_->registerCallback(std::bind(&StereoOdometry::callbackRGBD3, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+				}
+				subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s \\\n   %s \\\n   %s",
+						get_name(),
+						approxSync?"approx":"exact",
+						approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
+						rgbd_image1_sub_.getTopic().c_str(),
+						rgbd_image2_sub_.getTopic().c_str(),
+						rgbd_image3_sub_.getTopic().c_str());
+			}
+			else if(rgbdCameras == 4)
+			{
+				if(approxSync)
+				{
+					approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
+							MyApproxSync4Policy(queueSize_),
+							rgbd_image1_sub_,
+							rgbd_image2_sub_,
+							rgbd_image3_sub_,
+							rgbd_image4_sub_);
+					if(approxSyncMaxInterval > 0.0)
+						approxSync4_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
+					approxSync4_->registerCallback(std::bind(&StereoOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
+				}
+				else
+				{
+					exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
+							MyExactSync4Policy(queueSize_),
+							rgbd_image1_sub_,
+							rgbd_image2_sub_,
+							rgbd_image3_sub_,
+							rgbd_image4_sub_);
+					exactSync4_->registerCallback(std::bind(&StereoOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
+				}
+				subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s \\\n   %s \\\n   %s \\\n   %s",
+						get_name(),
+						approxSync?"approx":"exact",
+						approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
+						rgbd_image1_sub_.getTopic().c_str(),
+						rgbd_image2_sub_.getTopic().c_str(),
+						rgbd_image3_sub_.getTopic().c_str(),
+						rgbd_image4_sub_.getTopic().c_str());
+			}
+			else
+			{
+				RCLCPP_FATAL(this->get_logger(), "%s doesn't support more than 4 cameras (rgbd_cameras=%d) with internal synchronization interface, set rgbd_cameras=0 and use rgbd_images input topic instead for more cameras.", get_name(), rgbdCameras);
+			}
+
+		}
+		else if(rgbdCameras == 0)
+		{
+			rgbdxSub_ = create_subscription<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBDX, this, std::placeholders::_1));
+
+			subscribedTopicsMsg =
+					uFormat("\n%s subscribed to:\n   %s",
+					get_name(),
+					rgbdxSub_->get_topic_name());
+		}
+		else
+		{
+			rgbdSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBD, this, std::placeholders::_1));
+
+			subscribedTopicsMsg =
+					uFormat("\n%s subscribed to:\n   %s",
+					get_name(),
+					rgbdSub_->get_topic_name());
+		}
+	}
+	else
+	{
+		image_transport::TransportHints hints(this);
+		imageRectLeft_.subscribe(this, "left/image_rect", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
+		imageRectRight_.subscribe(this, "right/image_rect", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
+		cameraInfoLeft_.subscribe(this, "left/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
+		cameraInfoRight_.subscribe(this, "right/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
+
+		if(approxSync)
+		{
+			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+			if(approxSyncMaxInterval>0.0)
+				approxSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
+			approxSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
+		}
+		else
+		{
+			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+			exactSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
+		}
+
+		subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s \\\n   %s \\\n   %s \\\n   %s",
+				get_name(),
+				approxSync?"approx":"exact",
+				approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
+				imageRectLeft_.getTopic().c_str(),
+				imageRectRight_.getTopic().c_str(),
+				cameraInfoLeft_.getSubscriber()->get_topic_name(),
+				cameraInfoRight_.getSubscriber()->get_topic_name());
+	}
+
+	this->startWarningThread(subscribedTopicsMsg, approxSync);
+}
+
+void StereoOdometry::updateParameters(ParametersMap & parameters)
+{
+	//make sure we are using Reg/Strategy=0
+	ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy());
+	if(iter != parameters.end() && iter->second.compare("0") != 0)
+	{
+		RCLCPP_WARN(this->get_logger(), "Stereo odometry works only with \"Reg/Strategy\"=0. Ignoring value %s.", iter->second.c_str());
+	}
+	uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "0"));
+}
+
+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)
+{
+	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;
+	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());
+
+	int cameraCount = leftImages.size();
+	cv::Mat left;
+	cv::Mat right;
+	std::vector<rtabmap::StereoCameraModel> cameraModels;
+	for(unsigned int i=0; i<leftImages.size(); ++i)
+	{
+		if(!(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 ||
+			 leftImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
+			 leftImages[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
+			 leftImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0) ||
+			!(rightImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
+			  rightImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
+			  rightImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
+			  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))
+		{
+			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;
+
+		if(i == 0)
+		{
+			higherStamp = stamp;
+		}
+		else if(stamp > higherStamp)
+		{
+			higherStamp = stamp;
+		}
+
+		Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), leftImages[i]->header.frame_id, stamp, tfBuffer(), waitForTransform());
+		if(localTransform.isNull())
+		{
+			return;
+		}
+
+		if(i>0)
+		{
+			double stampDiff = fabs(rtabmap_conversions::timestampFromROS(leftImages[i]->header.stamp) - rtabmap_conversions::timestampFromROS(leftImages[i-1]->header.stamp));
+			if(stampDiff > 1.0/60.0)
+			{
+				static bool warningShown = false;
+				if(!warningShown)
+				{
+					RCLCPP_WARN(this->get_logger(), "The time difference between cameras %d and %d is "
+							"high (diff=%fs, cam%d=%fs, cam%d=%fs). You may want "
+							"to set approx_sync_max_interval to reject bad synchronizations or use "
+							"approx_sync=false if streams have all the exact same timestamp. This "
+							"message is only printed once.",
+							i-1, i,
+							stampDiff,
+							i-1, rtabmap_conversions::timestampFromROS(leftImages[i-1]->header.stamp),
+							i, rtabmap_conversions::timestampFromROS(leftImages[i]->header.stamp));
+					warningShown = true;
+				}
+			}
+		}
+
+		if(!leftImages[i]->image.empty() && !rightImages[i]->image.empty())
+		{
+			bool alreadyRectified = true;
+			Parameters::parse(parameters(), Parameters::kRtabmapImagesAlreadyRectified(), alreadyRectified);
+			rtabmap::Transform stereoTransform;
+			if(!alreadyRectified)
+			{
+				if(rightCameraInfos[i].header.frame_id.empty() || leftCameraInfos[i].header.frame_id.empty())
+				{
+					if(rightCameraInfos[i].p[3] == 0.0 && leftCameraInfos[i].p[3] == 0)
+					{
+						RCLCPP_ERROR(this->get_logger(), "Parameter %s is false but the frame_id in one of the camera_info "
+								"topic is empty, so TF between the cameras cannot be computed!",
+								Parameters::kRtabmapImagesAlreadyRectified().c_str());
+						return;
+					}
+					else
+					{
+						stereoTransform = rtabmap_conversions::getTransform(
+								rightCameraInfos[i].header.frame_id,
+								leftCameraInfos[i].header.frame_id,
+								leftCameraInfos[i].header.stamp,
+								tfBuffer(),
+								waitForTransform());
+						if(stereoTransform.isNull())
+						{
+							RCLCPP_ERROR(this->get_logger(), "Parameter %s is false but we cannot get TF between the two cameras! (between frames %s and %s)",
+									Parameters::kRtabmapImagesAlreadyRectified().c_str(),
+									rightCameraInfos[i].header.frame_id.c_str(),
+									leftCameraInfos[i].header.frame_id.c_str());
+							return;
+						}
+						else if(stereoTransform.isIdentity())
+						{
+							RCLCPP_ERROR(this->get_logger(), "Parameter %s is false but we cannot get a valid TF between the two cameras! "
+									"Identity transform returned between left and right cameras. Verify that if TF between "
+									"the cameras is valid: \"rosrun tf tf_echo %s %s\".",
+									Parameters::kRtabmapImagesAlreadyRectified().c_str(),
+									rightCameraInfos[i].header.frame_id.c_str(),
+									leftCameraInfos[i].header.frame_id.c_str());
+							return;
+						}
+					}
+				}
+
+				rtabmap::StereoCameraModel stereoModel = rtabmap_conversions::stereoCameraModelFromROS(leftCameraInfos[i], rightCameraInfos[i], localTransform, stereoTransform);
+
+				if( stereoModel.baseline() == 0 &&
+					alreadyRectified &&
+					!rightCameraInfos[i].header.frame_id.empty() &&
+					!leftCameraInfos[i].header.frame_id.empty())
+				{
+					stereoTransform = rtabmap_conversions::getTransform(
+							leftCameraInfos[i].header.frame_id,
+							rightCameraInfos[i].header.frame_id,
+							leftCameraInfos[i].header.stamp,
+							tfBuffer(),
+							waitForTransform());
+
+					if(!stereoTransform.isNull() && stereoTransform.x()>0)
+					{
+						static bool warned = false;
+						if(!warned)
+						{
+							RCLCPP_WARN(this->get_logger(), "Parameter %s is false but the frame_id in one of the "
+									"camera_info topic is empty, so TF between the cameras cannot be "
+									"computed! However, the baseline can be computed from the calibration, "
+									"we will use this one instead of TF. This message is only printed once...",
+									Parameters::kRtabmapImagesAlreadyRectified().c_str());
+							warned = true;
+						}
+					}
+				}
+				else
+				{
+					stereoTransform = rtabmap_conversions::getTransform(
+							rightCameraInfos[i].header.frame_id,
+							leftCameraInfos[i].header.frame_id,
+							leftCameraInfos[i].header.stamp,
+							tfBuffer(),
+							waitForTransform());
+					if(stereoTransform.isNull())
+					{
+						RCLCPP_ERROR(this->get_logger(), "Parameter %s is false but we cannot get TF between the two cameras! (between frames %s and %s)",
+								Parameters::kRtabmapImagesAlreadyRectified().c_str(),
+								rightCameraInfos[i].header.frame_id.c_str(),
+								leftCameraInfos[i].header.frame_id.c_str());
+						return;
+					}
+					else if(stereoTransform.isIdentity())
+					{
+						RCLCPP_ERROR(this->get_logger(), "Parameter %s is false but we cannot get a valid TF between the two cameras! "
+								"Identity transform returned between left and right cameras. Verify that if TF between "
+								"the cameras is valid: \"rosrun tf tf_echo %s %s\".",
+								Parameters::kRtabmapImagesAlreadyRectified().c_str(),
+								rightCameraInfos[i].header.frame_id.c_str(),
+								leftCameraInfos[i].header.frame_id.c_str());
+						return;
+					}
+				}
+			}
+
+			rtabmap::StereoCameraModel stereoModel = rtabmap_conversions::stereoCameraModelFromROS(leftCameraInfos[i], rightCameraInfos[i], localTransform, stereoTransform);
+
+			if( stereoModel.baseline() == 0 &&
+				alreadyRectified &&
+				!rightCameraInfos[i].header.frame_id.empty() &&
+				!leftCameraInfos[i].header.frame_id.empty())
+			{
+				stereoTransform = rtabmap_conversions::getTransform(
+						leftCameraInfos[i].header.frame_id,
+						rightCameraInfos[i].header.frame_id,
+						leftCameraInfos[i].header.stamp,
+						tfBuffer(),
+						waitForTransform());
+
+				if(!stereoTransform.isNull() && stereoTransform.x()>0)
+				{
+					static bool warned = false;
+					if(!warned)
+					{
+						RCLCPP_WARN(this->get_logger(), "Right camera info doesn't have Tx set but we are assuming that stereo images are already rectified (see %s parameter). While not "
+								"recommended, we used TF to get the baseline (%s->%s = %fm) for convenience (e.g., D400 ir stereo issue). It is preferred to feed "
+								"a valid right camera info if stereo images are already rectified. This message is only printed once...",
+								rtabmap::Parameters::kRtabmapImagesAlreadyRectified().c_str(),
+								rightCameraInfos[i].header.frame_id.c_str(), leftCameraInfos[i].header.frame_id.c_str(), stereoTransform.x());
+						warned = true;
+					}
+					stereoModel = rtabmap::StereoCameraModel(
+							stereoModel.left().fx(),
+							stereoModel.left().fy(),
+							stereoModel.left().cx(),
+							stereoModel.left().cy(),
+							stereoTransform.x(),
+							stereoModel.localTransform(),
+							stereoModel.left().imageSize());
+				}
+			}
+			if(alreadyRectified && stereoModel.baseline() <= 0)
+			{
+				RCLCPP_ERROR(this->get_logger(), "The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
+						  "setup where the Tx (or P(0,3)) is negative in the right camera info msg.", stereoModel.baseline());
+				return;
+			}
+
+			if(stereoModel.baseline() > 10.0)
+			{
+				static bool shown = false;
+				if(!shown)
+				{
+					RCLCPP_WARN(this->get_logger(), "Detected baseline (%f m) is quite large! Is your "
+							 "right camera_info P(0,3) correctly set? Note that "
+							 "baseline=-P(0,3)/P(0,0). This warning is printed only once.",
+							 stereoModel.baseline());
+					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(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
+			{
+				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;
+			}
+
+			cameraModels.push_back(stereoModel);
+		}
+		else
+		{
+			RCLCPP_ERROR(this->get_logger(), "Odom: input images empty?!?");
+			return;
+		}
+	}
+
+	//
+	rtabmap::SensorData data(
+			left,
+			right,
+			cameraModels,
+			0,
+			rtabmap_conversions::timestampFromROS(higherStamp));
+
+	std_msgs::msg::Header header;
+	header.stamp = higherStamp;
+	header.frame_id = leftImages.size()==1?leftImages[0]->header.frame_id:"";
+	this->processData(data, header);
+}
+
+void StereoOdometry::callback(
+		const sensor_msgs::msg::Image::ConstSharedPtr imageRectLeft,
+		const sensor_msgs::msg::Image::ConstSharedPtr imageRectRight,
+		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoLeft,
+		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoRight)
+{
+	callbackCalled();
+	if(!this->isPaused())
+	{
+		std::vector<cv_bridge::CvImageConstPtr> leftMsgs(1);
+		std::vector<cv_bridge::CvImageConstPtr> rightMsgs(1);
+		std::vector<sensor_msgs::msg::CameraInfo> leftInfoMsgs;
+		std::vector<sensor_msgs::msg::CameraInfo> rightInfoMsgs;
+		leftMsgs[0] = cv_bridge::toCvShare(imageRectLeft);
+		rightMsgs[0] = cv_bridge::toCvShare(imageRectRight);
+		leftInfoMsgs.push_back(*cameraInfoLeft);
+		rightInfoMsgs.push_back(*cameraInfoRight);
+
+		double stampDiff = fabs(rtabmap_conversions::timestampFromROS(imageRectLeft->header.stamp) - rtabmap_conversions::timestampFromROS(imageRectRight->header.stamp));
+		if(stampDiff > 0.010)
+		{
+			RCLCPP_WARN(this->get_logger(), "The time difference between left and right frames is "
+					"high (diff=%fs, left=%fs, right=%fs). If your left and right cameras are hardware "
+					"synchronized, use approx_sync:=false. Otherwise, you may want "
+					"to set approx_sync_max_interval lower than 0.01s to reject spurious bad synchronizations.",
+					stampDiff,
+					rtabmap_conversions::timestampFromROS(imageRectLeft->header.stamp),
+					rtabmap_conversions::timestampFromROS(imageRectRight->header.stamp));
+		}
+
+		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+	}
+}
+
+void StereoOdometry::callbackRGBD(
+		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image)
+{
+	callbackCalled();
+	if(!this->isPaused())
+	{
+		std::vector<cv_bridge::CvImageConstPtr> leftMsgs(1);
+		std::vector<cv_bridge::CvImageConstPtr> rightMsgs(1);
+		std::vector<sensor_msgs::msg::CameraInfo> leftInfoMsgs;
+		std::vector<sensor_msgs::msg::CameraInfo> rightInfoMsgs;
+		rtabmap_conversions::toCvShare(image, leftMsgs[0], rightMsgs[0]);
+		leftInfoMsgs.push_back(image->rgb_camera_info);
+		rightInfoMsgs.push_back(image->depth_camera_info);
+
+		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+	}
+}
+
+void StereoOdometry::callbackRGBDX(
+		const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr images)
+{
+	callbackCalled();
+	if(!this->isPaused())
+	{
+		if(images->rgbd_images.empty())
+		{
+			RCLCPP_ERROR(this->get_logger(), "Input topic \"%s\" doesn't contain any image(s)!", rgbdxSub_->get_topic_name());
+			return;
+		}
+		std::vector<cv_bridge::CvImageConstPtr> leftMsgs(images->rgbd_images.size());
+		std::vector<cv_bridge::CvImageConstPtr> rightMsgs(images->rgbd_images.size());
+		std::vector<sensor_msgs::msg::CameraInfo> leftInfoMsgs;
+		std::vector<sensor_msgs::msg::CameraInfo> rightInfoMsgs;
+		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);
+		}
+
+		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+	}
+}
+
+void StereoOdometry::callbackRGBD2(
+		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
+		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2)
+{
+	callbackCalled();
+	if(!this->isPaused())
+	{
+		std::vector<cv_bridge::CvImageConstPtr> leftMsgs(2);
+		std::vector<cv_bridge::CvImageConstPtr> rightMsgs(2);
+		std::vector<sensor_msgs::msg::CameraInfo> leftInfoMsgs;
+		std::vector<sensor_msgs::msg::CameraInfo> rightInfoMsgs;
+		rtabmap_conversions::toCvShare(image, leftMsgs[0], rightMsgs[0]);
+		rtabmap_conversions::toCvShare(image2, leftMsgs[1], rightMsgs[1]);
+		leftInfoMsgs.push_back(image->rgb_camera_info);
+		leftInfoMsgs.push_back(image2->rgb_camera_info);
+		rightInfoMsgs.push_back(image->depth_camera_info);
+		rightInfoMsgs.push_back(image2->depth_camera_info);
+
+		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+	}
+}
+
+void StereoOdometry::callbackRGBD3(
+		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
+		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
+		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3)
+{
+	callbackCalled();
+	if(!this->isPaused())
+	{
+		std::vector<cv_bridge::CvImageConstPtr> leftMsgs(3);
+		std::vector<cv_bridge::CvImageConstPtr> rightMsgs(3);
+		std::vector<sensor_msgs::msg::CameraInfo> leftInfoMsgs;
+		std::vector<sensor_msgs::msg::CameraInfo> rightInfoMsgs;
+		rtabmap_conversions::toCvShare(image, leftMsgs[0], rightMsgs[0]);
+		rtabmap_conversions::toCvShare(image2, leftMsgs[1], rightMsgs[1]);
+		rtabmap_conversions::toCvShare(image3, leftMsgs[2], rightMsgs[2]);
+		leftInfoMsgs.push_back(image->rgb_camera_info);
+		leftInfoMsgs.push_back(image2->rgb_camera_info);
+		leftInfoMsgs.push_back(image3->rgb_camera_info);
+		rightInfoMsgs.push_back(image->depth_camera_info);
+		rightInfoMsgs.push_back(image2->depth_camera_info);
+		rightInfoMsgs.push_back(image3->depth_camera_info);
+
+		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+	}
+}
+
+void StereoOdometry::callbackRGBD4(
+		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
+		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
+		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
+		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4)
+{
+	callbackCalled();
+	if(!this->isPaused())
+	{
+		std::vector<cv_bridge::CvImageConstPtr> leftMsgs(4);
+		std::vector<cv_bridge::CvImageConstPtr> rightMsgs(4);
+		std::vector<sensor_msgs::msg::CameraInfo> leftInfoMsgs;
+		std::vector<sensor_msgs::msg::CameraInfo> rightInfoMsgs;
+		rtabmap_conversions::toCvShare(image, leftMsgs[0], rightMsgs[0]);
+		rtabmap_conversions::toCvShare(image2, leftMsgs[1], rightMsgs[1]);
+		rtabmap_conversions::toCvShare(image3, leftMsgs[2], rightMsgs[2]);
+		rtabmap_conversions::toCvShare(image4, leftMsgs[3], rightMsgs[3]);
+		leftInfoMsgs.push_back(image->rgb_camera_info);
+		leftInfoMsgs.push_back(image2->rgb_camera_info);
+		leftInfoMsgs.push_back(image3->rgb_camera_info);
+		leftInfoMsgs.push_back(image4->rgb_camera_info);
+		rightInfoMsgs.push_back(image->depth_camera_info);
+		rightInfoMsgs.push_back(image2->depth_camera_info);
+		rightInfoMsgs.push_back(image3->depth_camera_info);
+		rightInfoMsgs.push_back(image4->depth_camera_info);
+
+		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+	}
+}
+
+void StereoOdometry::flushCallbacks()
+{
+	//flush callbacks
+	if(approxSync_)
+	{
+		delete approxSync_;
+		approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+		approxSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
+	}
+	if(exactSync_)
+	{
+		delete exactSync_;
+		exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+		exactSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
+	}
+	if(approxSync2_)
+	{
+		delete approxSync2_;
+		approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
+				MyApproxSync2Policy(queueSize_),
+				rgbd_image1_sub_,
+				rgbd_image2_sub_);
+		approxSync2_->registerCallback(std::bind(&StereoOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
+	}
+	if(exactSync2_)
+	{
+		delete exactSync2_;
+		exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
+				MyExactSync2Policy(queueSize_),
+				rgbd_image1_sub_,
+				rgbd_image2_sub_);
+		exactSync2_->registerCallback(std::bind(&StereoOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
+	}
+	if(approxSync3_)
+	{
+		delete approxSync3_;
+		approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
+				MyApproxSync3Policy(queueSize_),
+				rgbd_image1_sub_,
+				rgbd_image2_sub_,
+				rgbd_image3_sub_);
+		approxSync3_->registerCallback(std::bind(&StereoOdometry::callbackRGBD3, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+	}
+	if(exactSync3_)
+	{
+		delete exactSync3_;
+		exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
+				MyExactSync3Policy(queueSize_),
+				rgbd_image1_sub_,
+				rgbd_image2_sub_,
+				rgbd_image3_sub_);
+		exactSync3_->registerCallback(std::bind(&StereoOdometry::callbackRGBD3, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+	}
+	if(approxSync4_)
+	{
+		delete approxSync4_;
+		approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
+				MyApproxSync4Policy(queueSize_),
+				rgbd_image1_sub_,
+				rgbd_image2_sub_,
+				rgbd_image3_sub_,
+				rgbd_image4_sub_);
+		approxSync4_->registerCallback(std::bind(&StereoOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
+	}
+	if(exactSync4_)
+	{
+		delete exactSync4_;
+		exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
+				MyExactSync4Policy(queueSize_),
+				rgbd_image1_sub_,
+				rgbd_image2_sub_,
+				rgbd_image3_sub_,
+				rgbd_image4_sub_);
+		exactSync4_->registerCallback(std::bind(&StereoOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
+	}
+}
+
+}
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_odom::StereoOdometry)
+