Added multicam support for stereo_odometry

src/nodelets/rgbd_odometry.cpp

@@ -113,6 +113,14 @@ public:
 		{
 			delete exactSync4_;
 		}
+		if(approxSync5_)
+		{
+			delete approxSync5_;
+		}
+		if(exactSync5_)
+		{
+			delete exactSync5_;
+		}
 	}
 
 private:
@@ -397,9 +405,7 @@ private:
 		int cameraCount = rgbImages.size();
 		cv::Mat rgb;
 		cv::Mat depth;
-		pcl::PointCloud<pcl::PointXYZ> scanCloud;
 		std::vector<rtabmap::CameraModel> cameraModels;
-		double stampDiff = 0;
 		for(unsigned int i=0; i<rgbImages.size(); ++i)
 		{
 			if(!(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
@@ -488,7 +494,6 @@ private:
 			}
 
 			cv_bridge::CvImageConstPtr ptrDepth = depthImages[i];
-			cv::Mat subDepth = ptrDepth->image;
 
 			// initialize
 			if(rgb.empty())
@@ -497,7 +502,7 @@ private:
 			}
 			if(depth.empty())
 			{
-				depth = cv::Mat(depthHeight, depthWidth*cameraCount, subDepth.type());
+				depth = cv::Mat(depthHeight, depthWidth*cameraCount, ptrDepth->image.type());
 			}
 
 			if(ptrImage->image.type() == rgb.type())
@@ -506,17 +511,17 @@ private:
 			}
 			else
 			{
-				NODELET_ERROR("Some RGB images are not the same type!");
+				NODELET_ERROR("Some RGB images are not the same type! %d vs %d", ptrImage->image.type(), rgb.type());
 				return;
 			}
 
-			if(subDepth.type() == depth.type())
+			if(ptrDepth->image.type() == depth.type())
 			{
-				subDepth.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
+				ptrDepth->image.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
 			}
 			else
 			{
-				NODELET_ERROR("Some Depth images are not the same type! %d vs %d", subDepth.type(), depth.type());
+				NODELET_ERROR("Some Depth images are not the same type! %d vs %d", ptrDepth->image.type(), depth.type());
 				return;
 			}
 

src/nodelets/stereo_odometry.cpp

@@ -44,6 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <cv_bridge/cv_bridge.h>
 
 #include "rtabmap_ros/MsgConversion.h"
+#include <rtabmap_ros/RGBDImages.h>
 
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UTimer.h>
@@ -63,6 +64,12 @@ public:
 		rtabmap_ros::OdometryROS(true, true, false),
 		approxSync_(0),
 		exactSync_(0),
+		approxSync2_(0),
+		exactSync2_(0),
+		approxSync3_(0),
+		exactSync3_(0),
+		approxSync4_(0),
+		exactSync4_(0),
 		queueSize_(5),
 		keepColor_(false)
 	{
@@ -89,10 +96,12 @@ private:
 		bool approxSync = false;
 		bool subscribeRGBD = false;
 		double approxSyncMaxInterval = 0.0;
+		int rgbdCameras;
 		pnh.param("approx_sync", approxSync, approxSync);
 		pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
 		pnh.param("queue_size", queueSize_, queueSize_);
 		pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD);
+		pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras);
 		pnh.param("keep_color", keepColor_, keepColor_);
 
 		NODELET_INFO("StereoOdometry: approx_sync = %s", approxSync?"true":"false");
@@ -105,12 +114,133 @@ private:
 		std::string subscribedTopicsMsg;
 		if(subscribeRGBD)
 		{
-			rgbdSub_ = nh.subscribe("rgbd_image", 1, &StereoOdometry::callbackRGBD, this);
+			if(rgbdCameras >= 2)
+			{
+				rgbd_image1_sub_.subscribe(nh, "rgbd_image0", 1);
+				rgbd_image2_sub_.subscribe(nh, "rgbd_image1", 1);
+				if(rgbdCameras >= 3)
+				{
+					rgbd_image3_sub_.subscribe(nh, "rgbd_image2", 1);
+				}
+				if(rgbdCameras >= 4)
+				{
+					rgbd_image4_sub_.subscribe(nh, "rgbd_image3", 1);
+				}
 
-			subscribedTopicsMsg =
-					uFormat("\n%s subscribed to:\n   %s",
-					getName().c_str(),
-					rgbdSub_.getTopic().c_str());
+				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(ros::Duration(approxSyncMaxInterval));
+						approxSync2_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2));
+					}
+					else
+					{
+						exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
+								MyExactSync2Policy(queueSize_),
+								rgbd_image1_sub_,
+								rgbd_image2_sub_);
+						exactSync2_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2));
+					}
+					subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s \\\n   %s",
+							getName().c_str(),
+							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(ros::Duration(approxSyncMaxInterval));
+						approxSync3_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD3, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
+					}
+					else
+					{
+						exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
+								MyExactSync3Policy(queueSize_),
+								rgbd_image1_sub_,
+								rgbd_image2_sub_,
+								rgbd_image3_sub_);
+						exactSync3_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD3, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
+					}
+					subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s \\\n   %s \\\n   %s",
+							getName().c_str(),
+							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(ros::Duration(approxSyncMaxInterval));
+						approxSync4_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD4, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::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(boost::bind(&StereoOdometry::callbackRGBD4, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
+					}
+					subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s \\\n   %s \\\n   %s \\\n   %s",
+							getName().c_str(),
+							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
+				{
+					ROS_FATAL("%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.", getName().c_str(), rgbdCameras);
+				}
+
+			}
+			else if(rgbdCameras == 0)
+			{
+				rgbdxSub_ = nh.subscribe("rgbd_images", 1, &StereoOdometry::callbackRGBDX, this);
+
+				subscribedTopicsMsg =
+						uFormat("\n%s subscribed to:\n   %s",
+						getName().c_str(),
+						rgbdxSub_.getTopic().c_str());
+			}
+			else
+			{
+				rgbdSub_ = nh.subscribe("rgbd_image", 1, &StereoOdometry::callbackRGBD, this);
+
+				subscribedTopicsMsg =
+						uFormat("\n%s subscribed to:\n   %s",
+						getName().c_str(),
+						rgbdSub_.getTopic().c_str());
+			}
 		}
 		else
 		{
@@ -166,57 +296,90 @@ private:
 		uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "0"));
 	}
 
-	void callback(
-			const sensor_msgs::ImageConstPtr& imageRectLeft,
-			const sensor_msgs::ImageConstPtr& imageRectRight,
-			const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft,
-			const sensor_msgs::CameraInfoConstPtr& cameraInfoRight)
+	void commonCallback(
+			const std::vector<cv_bridge::CvImageConstPtr> & leftImages,
+			const std::vector<cv_bridge::CvImageConstPtr> & rightImages,
+			const std::vector<sensor_msgs::CameraInfo>& leftCameraInfos,
+			const std::vector<sensor_msgs::CameraInfo>& rightCameraInfos)
 	{
-		callbackCalled();
-		if(!this->isPaused())
+		ROS_ASSERT(rgbImages.size() > 0 && rgbImages.size() == depthImages.size() && rgbImages.size() == cameraInfos.size());
+		ros::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(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
-				 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
-				 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
-				 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
-				 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
-				 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
-				 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0) ||
-				!(imageRectRight->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
-				  imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
-				  imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
-				  imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
-				  imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
-				  imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
-				  imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0))
+			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))
 			{
 				NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8,rgba8,bgra8 (mono8 recommended), received types are %s (left) and %s (right)",
-						imageRectLeft->encoding.c_str(), imageRectRight->encoding.c_str());
+						leftImages[i]->encoding.c_str(), rightImages[i]->encoding.c_str());
 				return;
 			}
 
-			ros::Time stamp = imageRectLeft->header.stamp>imageRectRight->header.stamp?imageRectLeft->header.stamp:imageRectRight->header.stamp;
+			ros::Time stamp = leftImages[i]->header.stamp>rightImages[i]->header.stamp?leftImages[i]->header.stamp:rightImages[i]->header.stamp;
 
-			Transform localTransform = getTransform(this->frameId(), imageRectLeft->header.frame_id, stamp);
+			if(i == 0)
+			{
+				higherStamp = stamp;
+			}
+			else if(stamp > higherStamp)
+			{
+				higherStamp = stamp;
+			}
+
+			Transform localTransform = getTransform(this->frameId(), leftImages[i]->header.frame_id, stamp);
 			if(localTransform.isNull())
 			{
 				return;
 			}
 
-			double stampDiff = fabs(imageRectLeft->header.stamp.toSec() - imageRectRight->header.stamp.toSec());
-			if(stampDiff > 0.010)
+			if(i>0)
 			{
-				NODELET_WARN("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,
-						imageRectLeft->header.stamp.toSec(),
-						imageRectRight->header.stamp.toSec());
+				double stampDiff = fabs(leftImages[i]->header.stamp.toSec() - leftImages[i-1]->header.stamp.toSec());
+				if(stampDiff > 1.0/60.0)
+				{
+					static bool warningShown = false;
+					if(!warningShown)
+					{
+						NODELET_WARN("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, leftImages[i-1]->header.stamp.toSec(),
+								i, leftImages[i]->header.stamp.toSec());
+						warningShown = true;
+					}
+				}
 			}
 
 			int quality = -1;
-			if(imageRectLeft->data.size() && imageRectRight->data.size())
+			if(!leftImages[i]->image.empty() && !rightImages[i]->image.empty())
 			{
 				bool alreadyRectified = true;
 				Parameters::parse(parameters(), Parameters::kRtabmapImagesAlreadyRectified(), alreadyRectified);
@@ -224,15 +387,15 @@ private:
 				if(!alreadyRectified)
 				{
 					stereoTransform = getTransform(
-							cameraInfoRight->header.frame_id,
-							cameraInfoLeft->header.frame_id,
-							cameraInfoLeft->header.stamp);
+							rightCameraInfos[i].header.frame_id,
+							leftCameraInfos[i].header.frame_id,
+							leftCameraInfos[i].header.stamp);
 					if(stereoTransform.isNull())
 					{
 						NODELET_ERROR("Parameter %s is false but we cannot get TF between the two cameras! (between frames %s and %s)",
 								Parameters::kRtabmapImagesAlreadyRectified().c_str(),
-								cameraInfoRight->header.frame_id.c_str(),
-								cameraInfoLeft->header.frame_id.c_str());
+								rightCameraInfos[i].header.frame_id.c_str(),
+								leftCameraInfos[i].header.frame_id.c_str());
 						return;
 					}
 					else if(stereoTransform.isIdentity())
@@ -241,20 +404,20 @@ private:
 								"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(),
-								cameraInfoRight->header.frame_id.c_str(),
-								cameraInfoLeft->header.frame_id.c_str());
+								rightCameraInfos[i].header.frame_id.c_str(),
+								leftCameraInfos[i].header.frame_id.c_str());
 						return;
 					}
 				}
 
-				rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*cameraInfoLeft, *cameraInfoRight, localTransform, stereoTransform);
+				rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(leftCameraInfos[i], rightCameraInfos[i], localTransform, stereoTransform);
 
 				if(stereoModel.baseline() == 0 && alreadyRectified)
 				{
 					stereoTransform = getTransform(
-							cameraInfoLeft->header.frame_id,
-							cameraInfoRight->header.frame_id,
-							cameraInfoLeft->header.stamp);
+							leftCameraInfos[i].header.frame_id,
+							rightCameraInfos[i].header.frame_id,
+							leftCameraInfos[i].header.stamp);
 
 					if(!stereoTransform.isNull() && stereoTransform.x()>0)
 					{
@@ -265,7 +428,7 @@ private:
 									"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(),
-									cameraInfoRight->header.frame_id.c_str(), cameraInfoLeft->header.frame_id.c_str(), stereoTransform.x());
+									rightCameraInfos[i].header.frame_id.c_str(), leftCameraInfos[i].header.frame_id.c_str(), stereoTransform.x());
 							warned = true;
 						}
 						stereoModel = rtabmap::StereoCameraModel(
@@ -298,34 +461,113 @@ private:
 						shown = true;
 					}
 				}
-				cv_bridge::CvImagePtr ptrImageLeft = cv_bridge::toCvCopy(imageRectLeft,
-						imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 ||
-						imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8)==0?"":
-							keepColor_ && imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0?"bgr8":"mono8");
-				cv_bridge::CvImagePtr ptrImageRight = cv_bridge::toCvCopy(imageRectRight,
-						imageRectRight->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 ||
-						imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8)==0?"":"mono8");
 
-				UTimer stepTimer;
-				//
-				UDEBUG("localTransform = %s", localTransform.prettyPrint().c_str());
-				rtabmap::SensorData data(
-						ptrImageLeft->image,
-						ptrImageRight->image,
-						stereoModel,
-						0,
-						rtabmap_ros::timestampFromROS(stamp));
+				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");
+					}
+				}
 
-				std_msgs::Header header;
-				header.stamp = stamp;
-				header.frame_id = imageRectLeft->header.frame_id;
-				this->processData(data, header);
+				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(leftImages[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
+				{
+					NODELET_ERROR("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
+				{
+					NODELET_ERROR("Some right images are not the same type! %d vs %d", ptrRight->image.type(), right.type());
+					return;
+				}
+
+				cameraModels.push_back(stereoModel);
 			}
 			else
 			{
-				NODELET_WARN("Odom: input images empty?!?");
+				NODELET_ERROR("Odom: input images empty?!?");
+				return;
 			}
 		}
+
+		//
+		rtabmap::SensorData data(
+				left,
+				right,
+				cameraModels,
+				0,
+				rtabmap_ros::timestampFromROS(higherStamp));
+
+		std_msgs::Header header;
+		header.stamp = higherStamp;
+		header.frame_id = leftImages.size()==1?leftImages[0]->header.frame_id:"";
+		this->processData(data, header);
+	}
+
+	void callback(
+				const sensor_msgs::ImageConstPtr& imageLeft,
+				const sensor_msgs::ImageConstPtr& imageRight,
+				const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft,
+				const sensor_msgs::CameraInfoConstPtr& cameraInfoRight)
+	{
+		callbackCalled();
+		if(!this->isPaused())
+		{
+			std::vector<cv_bridge::CvImageConstPtr> leftMsgs(1);
+			std::vector<cv_bridge::CvImageConstPtr> rightMsgs(1);
+			std::vector<sensor_msgs::CameraInfo> leftInfoMsgs;
+			std::vector<sensor_msgs::CameraInfo> rightInfoMsgs;
+			leftMsgs[0] = cv_bridge::toCvShare(imageLeft);
+			rightMsgs[0] = cv_bridge::toCvShare(imageRight);
+			leftInfoMsgs.push_back(*cameraInfoLeft);
+			rightInfoMsgs.push_back(*cameraInfoRight);
+
+			double stampDiff = fabs(imageLeft->header.stamp.toSec() - imageRight->header.stamp.toSec());
+			if(stampDiff > 0.010)
+			{
+				NODELET_WARN("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,
+						imageLeft->header.stamp.toSec(),
+						imageRight->header.stamp.toSec());
+			}
+
+			this->commonCallback(leftMsgs, leftMsgs, leftInfoMsgs, rightInfoMsgs);
+		}
 	}
 
 	void callbackRGBD(
@@ -334,157 +576,119 @@ private:
 		callbackCalled();
 		if(!this->isPaused())
 		{
-			cv_bridge::CvImageConstPtr imageRectLeft, imageRectRight;
-			rtabmap_ros::toCvShare(image, imageRectLeft, imageRectRight);
+			std::vector<cv_bridge::CvImageConstPtr> leftMsgs(1);
+			std::vector<cv_bridge::CvImageConstPtr> rightMsgs(1);
+			std::vector<sensor_msgs::CameraInfo> leftInfoMsgs;
+			std::vector<sensor_msgs::CameraInfo> rightInfoMsgs;
+			rtabmap_ros::toCvShare(image, leftMsgs[0], rightMsgs[0]);
+			leftInfoMsgs.push_back(image->rgb_camera_info);
+			rightInfoMsgs.push_back(image->depth_camera_info);
 
-			if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
-				 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
-				 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
-				 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
-				 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
-				 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
-				 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0) ||
-				!(imageRectRight->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
-				  imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
-				  imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
-				  imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
-				  imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
-				  imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
-				  imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0))
+			this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		}
+	}
+
+	void callbackRGBDX(
+			const rtabmap_ros::RGBDImagesConstPtr& images)
+	{
+		callbackCalled();
+		if(!this->isPaused())
+		{
+			if(images->rgbd_images.empty())
 			{
-				NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8,rgba8,bgra8 (mono8 recommended), received types are %s (left) and %s (right)",
-						imageRectLeft->encoding.c_str(), imageRectRight->encoding.c_str());
+				NODELET_ERROR("Input topic \"%s\" doesn't contain any image(s)!", rgbdxSub_.getTopic().c_str());
 				return;
 			}
-
-			ros::Time stamp = imageRectLeft->header.stamp>imageRectRight->header.stamp?imageRectLeft->header.stamp:imageRectRight->header.stamp;
-
-			Transform localTransform = getTransform(this->frameId(), imageRectLeft->header.frame_id, stamp);
-			if(localTransform.isNull())
+			std::vector<cv_bridge::CvImageConstPtr> leftMsgs(images->rgbd_images.size());
+			std::vector<cv_bridge::CvImageConstPtr> rightMsgs(images->rgbd_images.size());
+			std::vector<sensor_msgs::CameraInfo> leftInfoMsgs;
+			std::vector<sensor_msgs::CameraInfo> rightInfoMsgs;
+			for(size_t i=0; i<images->rgbd_images.size(); ++i)
 			{
-				return;
+				rtabmap_ros::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);
 			}
 
-			ros::WallTime time = ros::WallTime::now();
+			this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		}
+	}
 
-			int quality = -1;
-			if(!imageRectLeft->image.empty() && !imageRectRight->image.empty())
-			{
-				bool alreadyRectified = true;
-				Parameters::parse(parameters(), Parameters::kRtabmapImagesAlreadyRectified(), alreadyRectified);
-				rtabmap::Transform stereoTransform;
-				if(!alreadyRectified)
-				{
-					stereoTransform = getTransform(
-							image->depth_camera_info.header.frame_id,
-							image->rgb_camera_info.header.frame_id,
-							image->rgb_camera_info.header.stamp);
-					if(stereoTransform.isNull())
-					{
-						NODELET_ERROR("Parameter %s is false but we cannot get TF between the two cameras!", Parameters::kRtabmapImagesAlreadyRectified().c_str());
-						return;
-					}
-				}
+	void callbackRGBD2(
+			const rtabmap_ros::RGBDImageConstPtr& image,
+			const rtabmap_ros::RGBDImageConstPtr& image2)
+	{
+		callbackCalled();
+		if(!this->isPaused())
+		{
+			std::vector<cv_bridge::CvImageConstPtr> leftMsgs(2);
+			std::vector<cv_bridge::CvImageConstPtr> rightMsgs(2);
+			std::vector<sensor_msgs::CameraInfo> leftInfoMsgs;
+			std::vector<sensor_msgs::CameraInfo> rightInfoMsgs;
+			rtabmap_ros::toCvShare(image, leftMsgs[0], rightMsgs[0]);
+			rtabmap_ros::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);
 
-				rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(image->rgb_camera_info, image->depth_camera_info, localTransform);
+			this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		}
+	}
 
-				if(stereoModel.baseline() == 0 && alreadyRectified)
-				{
-					stereoTransform = getTransform(
-							image->rgb_camera_info.header.frame_id,
-							image->depth_camera_info.header.frame_id,
-							image->rgb_camera_info.header.stamp);
+	void callbackRGBD3(
+			const rtabmap_ros::RGBDImageConstPtr& image,
+			const rtabmap_ros::RGBDImageConstPtr& image2,
+			const rtabmap_ros::RGBDImageConstPtr& image3)
+	{
+		callbackCalled();
+		if(!this->isPaused())
+		{
+			std::vector<cv_bridge::CvImageConstPtr> leftMsgs(3);
+			std::vector<cv_bridge::CvImageConstPtr> rightMsgs(3);
+			std::vector<sensor_msgs::CameraInfo> leftInfoMsgs;
+			std::vector<sensor_msgs::CameraInfo> rightInfoMsgs;
+			rtabmap_ros::toCvShare(image, leftMsgs[0], rightMsgs[0]);
+			rtabmap_ros::toCvShare(image2, leftMsgs[1], rightMsgs[1]);
+			rtabmap_ros::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);
 
-					if(!stereoTransform.isNull() && stereoTransform.x()>0)
-					{
-						static bool warned = false;
-						if(!warned)
-						{
-							ROS_WARN("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(),
-									image->depth_camera_info.header.frame_id.c_str(), image->rgb_camera_info.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());
-					}
-				}
+			this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		}
+	}
 
-				if(alreadyRectified && stereoModel.baseline() <= 0)
-				{
-					NODELET_ERROR("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;
-				}
+	void callbackRGBD4(
+			const rtabmap_ros::RGBDImageConstPtr& image,
+			const rtabmap_ros::RGBDImageConstPtr& image2,
+			const rtabmap_ros::RGBDImageConstPtr& image3,
+			const rtabmap_ros::RGBDImageConstPtr& image4)
+	{
+		callbackCalled();
+		if(!this->isPaused())
+		{
+			std::vector<cv_bridge::CvImageConstPtr> leftMsgs(4);
+			std::vector<cv_bridge::CvImageConstPtr> rightMsgs(4);
+			std::vector<sensor_msgs::CameraInfo> leftInfoMsgs;
+			std::vector<sensor_msgs::CameraInfo> rightInfoMsgs;
+			rtabmap_ros::toCvShare(image, leftMsgs[0], rightMsgs[0]);
+			rtabmap_ros::toCvShare(image2, leftMsgs[1], rightMsgs[1]);
+			rtabmap_ros::toCvShare(image3, leftMsgs[2], rightMsgs[2]);
+			rtabmap_ros::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);
 
-				if(stereoModel.baseline() > 10.0)
-				{
-					static bool shown = false;
-					if(!shown)
-					{
-						NODELET_WARN("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::Mat left;
-				if(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
-				   imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
-				{
-					if(keepColor_ && imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
-					{
-						left = cv_bridge::cvtColor(imageRectLeft, "bgr8")->image;
-					}
-					else
-					{
-						left = cv_bridge::cvtColor(imageRectLeft, "mono8")->image;
-					}
-				}
-				else
-				{
-					left = imageRectLeft->image.clone();
-				}
-				cv::Mat right;
-				if(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
-				   imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
-				{
-					right = cv_bridge::cvtColor(imageRectRight, "mono8")->image;
-				}
-				else
-				{
-					right = imageRectRight->image.clone();
-				}
-
-				UTimer stepTimer;
-				//
-				UDEBUG("localTransform = %s", localTransform.prettyPrint().c_str());
-				rtabmap::SensorData data(
-						left,
-						right,
-						stereoModel,
-						0,
-						rtabmap_ros::timestampFromROS(stamp));
-
-				std_msgs::Header header;
-				header.stamp = stamp;
-				header.frame_id = image->header.frame_id;
-				this->processData(data, header);
-			}
-			else
-			{
-				NODELET_WARN("Odom: input images empty?!?");
-			}
+			this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
 		}
 	}
 
@@ -504,6 +708,66 @@ protected:
 			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
 			exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
 		}
+		if(approxSync2_)
+		{
+			delete approxSync2_;
+			approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
+					MyApproxSync2Policy(queueSize_),
+					rgbd_image1_sub_,
+					rgbd_image2_sub_);
+			approxSync2_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2));
+		}
+		if(exactSync2_)
+		{
+			delete exactSync2_;
+			exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
+					MyExactSync2Policy(queueSize_),
+					rgbd_image1_sub_,
+					rgbd_image2_sub_);
+			exactSync2_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::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(boost::bind(&StereoOdometry::callbackRGBD3, this, boost::placeholders::_1, boost::placeholders::_2, boost::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(boost::bind(&StereoOdometry::callbackRGBD3, this, boost::placeholders::_1, boost::placeholders::_2, boost::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(boost::bind(&StereoOdometry::callbackRGBD4, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::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(boost::bind(&StereoOdometry::callbackRGBD4, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
+		}
 	}
 
 private:
@@ -511,11 +775,32 @@ private:
 	image_transport::SubscriberFilter imageRectRight_;
 	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
 	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
+
+	ros::Subscriber rgbdSub_;
+	ros::Subscriber rgbdxSub_;
+	message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image1_sub_;
+	message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image2_sub_;
+	message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image3_sub_;
+	message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image4_sub_;
+	message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image5_sub_;
+
 	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
 	message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
 	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncPolicy;
 	message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
-	ros::Subscriber rgbdSub_;
+	typedef message_filters::sync_policies::ApproximateTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyApproxSync2Policy;
+	message_filters::Synchronizer<MyApproxSync2Policy> * approxSync2_;
+	typedef message_filters::sync_policies::ExactTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyExactSync2Policy;
+	message_filters::Synchronizer<MyExactSync2Policy> * exactSync2_;
+	typedef message_filters::sync_policies::ApproximateTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyApproxSync3Policy;
+	message_filters::Synchronizer<MyApproxSync3Policy> * approxSync3_;
+	typedef message_filters::sync_policies::ExactTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyExactSync3Policy;
+	message_filters::Synchronizer<MyExactSync3Policy> * exactSync3_;
+	typedef message_filters::sync_policies::ApproximateTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyApproxSync4Policy;
+	message_filters::Synchronizer<MyApproxSync4Policy> * approxSync4_;
+	typedef message_filters::sync_policies::ExactTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyExactSync4Policy;
+	message_filters::Synchronizer<MyExactSync4Policy> * exactSync4_;
+
 	int queueSize_;
 	bool keepColor_;
 };