Odom: Added 5 and 6 cameras internal sync callbacks for convenience.

rtabmap_odom/src/nodelets/rgbd_odometry.cpp

@@ -72,6 +72,8 @@ public:
 		exactSync4_(0),
 		approxSync5_(0),
 		exactSync5_(0),
+		approxSync6_(0),
+		exactSync6_(0),
 		queueSize_(5),
 		keepColor_(false)
 	{
@@ -81,46 +83,18 @@ public:
 	{
 		rgbdSub_.shutdown();
 		rgbdxSub_.shutdown();
-		if(approxSync_)
+		delete approxSync_;
-		{
+		delete exactSync_;
-			delete approxSync_;
+		delete approxSync2_;
-		}
+		delete exactSync2_;
-		if(exactSync_)
+		delete approxSync3_;
-		{
+		delete exactSync3_;
-			delete exactSync_;
+		delete approxSync4_;
-		}
+		delete exactSync4_;
-		if(approxSync2_)
+		delete approxSync5_;
-		{
+		delete exactSync5_;
-			delete approxSync2_;
+		delete approxSync6_;
-		}
+		delete exactSync6_;
-		if(exactSync2_)
-		{
-			delete exactSync2_;
-		}
-		if(approxSync3_)
-		{
-			delete approxSync3_;
-		}
-		if(exactSync3_)
-		{
-			delete exactSync3_;
-		}
-		if(approxSync4_)
-		{
-			delete approxSync4_;
-		}
-		if(exactSync4_)
-		{
-			delete exactSync4_;
-		}
-		if(approxSync5_)
-		{
-			delete approxSync5_;
-		}
-		if(exactSync5_)
-		{
-			delete exactSync5_;
-		}
 	}
 
 private:
@@ -147,10 +121,6 @@ private:
 		{
 			rgbdCameras = 0;
 		}
-		if(rgbdCameras > 5)
-		{
-			NODELET_FATAL("Only 5 cameras maximum supported yet. Set 0 to use rgbd_images input (for which rgbdx_sync node can sync up to 8 cameras).");
-		}
 		pnh.param("keep_color", keepColor_, keepColor_);
 
 		NODELET_INFO("RGBDOdometry: approx_sync    = %s", approxSync?"true":"false");
@@ -181,6 +151,10 @@ private:
 				{
 					rgbd_image5_sub_.subscribe(nh, "rgbd_image4", 1);
 				}
+				if(rgbdCameras >= 6)
+				{
+					rgbd_image6_sub_.subscribe(nh, "rgbd_image5", 1);
+				}
 
 				if(rgbdCameras == 2)
 				{
@@ -308,6 +282,53 @@ private:
 							rgbd_image4_sub_.getTopic().c_str(),
                                                         rgbd_image5_sub_.getTopic().c_str());
 				}
+				else if(rgbdCameras == 6)
+				{
+					if(approxSync)
+					{
+						approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>(
+								MyApproxSync6Policy(queueSize_),
+								rgbd_image1_sub_,
+								rgbd_image2_sub_,
+								rgbd_image3_sub_,
+								rgbd_image4_sub_,
+								rgbd_image5_sub_,
+								rgbd_image6_sub_);
+						if(approxSyncMaxInterval > 0.0)
+							approxSync6_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+						approxSync6_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6));
+					}
+					else
+					{
+						exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>(
+								MyExactSync6Policy(queueSize_),
+								rgbd_image1_sub_,
+								rgbd_image2_sub_,
+								rgbd_image3_sub_,
+								rgbd_image4_sub_,
+								rgbd_image5_sub_,
+								rgbd_image6_sub_);
+						exactSync6_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6));
+					}
+					subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s \\\n  %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(),
+							rgbd_image5_sub_.getTopic().c_str(),
+							rgbd_image6_sub_.getTopic().c_str());
+				}
+				else
+				{
+					NODELET_FATAL("%s doesn't support more than 6 cameras (rgbd_cameras=%d) with "
+								  "internal synchronization interface, set rgbd_cameras=0 and use "
+								  "rgbd_images input topic instead for more cameras (for which "
+								  "rgbdx_sync node can sync up to 8 cameras).",
+								  getName().c_str(), rgbdCameras);
+				}
 
 			}
 			else if(rgbdCameras == 0)
@@ -700,6 +721,35 @@ private:
 			this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
 		}
 	}
+	void callbackRGBD6(
+			const rtabmap_msgs::RGBDImageConstPtr& image,
+			const rtabmap_msgs::RGBDImageConstPtr& image2,
+			const rtabmap_msgs::RGBDImageConstPtr& image3,
+			const rtabmap_msgs::RGBDImageConstPtr& image4,
+			const rtabmap_msgs::RGBDImageConstPtr& image5,
+			const rtabmap_msgs::RGBDImageConstPtr& image6)
+	{
+		if(!this->isPaused())
+		{
+			std::vector<cv_bridge::CvImageConstPtr> imageMsgs(6);
+			std::vector<cv_bridge::CvImageConstPtr> depthMsgs(6);
+			std::vector<sensor_msgs::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]);
+			rtabmap_conversions::toCvShare(image6, imageMsgs[5], depthMsgs[5]);
+			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);
+			infoMsgs.push_back(image6->rgb_camera_info);
+
+			this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+		}
+	}
 
 protected:
 	virtual void flushCallbacks()
@@ -801,6 +851,32 @@ protected:
                                         rgbd_image5_sub_);
 			exactSync5_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD5, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5));
 		}
+		if(approxSync6_)
+		{
+			delete approxSync6_;
+			approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>(
+					MyApproxSync6Policy(queueSize_),
+					rgbd_image1_sub_,
+					rgbd_image2_sub_,
+					rgbd_image3_sub_,
+					rgbd_image4_sub_,
+					rgbd_image5_sub_,
+					rgbd_image6_sub_);
+			approxSync6_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6));
+		}
+		if(exactSync6_)
+		{
+			delete exactSync6_;
+			exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>(
+					MyExactSync6Policy(queueSize_),
+					rgbd_image1_sub_,
+					rgbd_image2_sub_,
+					rgbd_image3_sub_,
+					rgbd_image4_sub_,
+					rgbd_image5_sub_,
+					rgbd_image6_sub_);
+			exactSync6_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6));
+		}
 	}
 
 private:
@@ -815,6 +891,7 @@ private:
 	message_filters::Subscriber<rtabmap_msgs::RGBDImage> rgbd_image3_sub_;
 	message_filters::Subscriber<rtabmap_msgs::RGBDImage> rgbd_image4_sub_;
 	message_filters::Subscriber<rtabmap_msgs::RGBDImage> rgbd_image5_sub_;
+	message_filters::Subscriber<rtabmap_msgs::RGBDImage> rgbd_image6_sub_;
 
 	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
 	message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
@@ -836,6 +913,10 @@ private:
 	message_filters::Synchronizer<MyApproxSync5Policy> * approxSync5_;
 	typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyExactSync5Policy;
 	message_filters::Synchronizer<MyExactSync5Policy> * exactSync5_;
+	typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyApproxSync6Policy;
+	message_filters::Synchronizer<MyApproxSync6Policy> * approxSync6_;
+	typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyExactSync6Policy;
+	message_filters::Synchronizer<MyExactSync6Policy> * exactSync6_;
 	int queueSize_;
 	bool keepColor_;
 };

rtabmap_odom/src/nodelets/stereo_odometry.cpp

@@ -70,6 +70,10 @@ public:
 		exactSync3_(0),
 		approxSync4_(0),
 		exactSync4_(0),
+		approxSync5_(0),
+		exactSync5_(0),
+		approxSync6_(0),
+		exactSync6_(0),
 		queueSize_(5),
 		keepColor_(false)
 	{
@@ -77,14 +81,20 @@ public:
 
 	virtual ~StereoOdometry()
 	{
-		if(approxSync_)
+		rgbdSub_.shutdown();
-		{
+		rgbdxSub_.shutdown();
-			delete approxSync_;
+		delete approxSync_;
-		}
+		delete exactSync_;
-		if(exactSync_)
+		delete approxSync2_;
-		{
+		delete exactSync2_;
-			delete exactSync_;
+		delete approxSync3_;
-		}
+		delete exactSync3_;
+		delete approxSync4_;
+		delete exactSync4_;
+		delete approxSync5_;
+		delete exactSync5_;
+		delete approxSync6_;
+		delete exactSync6_;
 	}
 
 private:
@@ -127,6 +137,14 @@ private:
 				{
 					rgbd_image4_sub_.subscribe(nh, "rgbd_image3", 1);
 				}
+				if(rgbdCameras >= 5)
+				{
+					rgbd_image5_sub_.subscribe(nh, "rgbd_image4", 1);
+				}
+				if(rgbdCameras >= 6)
+				{
+					rgbd_image6_sub_.subscribe(nh, "rgbd_image5", 1);
+				}
 
 				if(rgbdCameras == 2)
 				{
@@ -218,9 +236,88 @@ private:
 							rgbd_image3_sub_.getTopic().c_str(),
 							rgbd_image4_sub_.getTopic().c_str());
 				}
+				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(ros::Duration(approxSyncMaxInterval));
+						approxSync5_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD5, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::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(boost::bind(&StereoOdometry::callbackRGBD5, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5));
+					}
+					subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %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(),
+							rgbd_image5_sub_.getTopic().c_str());
+				}
+				else if(rgbdCameras == 6)
+				{
+					if(approxSync)
+					{
+						approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>(
+								MyApproxSync6Policy(queueSize_),
+								rgbd_image1_sub_,
+								rgbd_image2_sub_,
+								rgbd_image3_sub_,
+								rgbd_image4_sub_,
+								rgbd_image5_sub_,
+								rgbd_image6_sub_);
+						if(approxSyncMaxInterval > 0.0)
+							approxSync6_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+						approxSync6_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6));
+					}
+					else
+					{
+						exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>(
+								MyExactSync6Policy(queueSize_),
+								rgbd_image1_sub_,
+								rgbd_image2_sub_,
+								rgbd_image3_sub_,
+								rgbd_image4_sub_,
+								rgbd_image5_sub_,
+								rgbd_image6_sub_);
+						exactSync6_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6));
+					}
+					subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n   %s \\\n   %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(),
+							rgbd_image5_sub_.getTopic().c_str(),
+							rgbd_image6_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);
+					NODELET_FATAL("%s doesn't support more than 6 cameras (rgbd_cameras=%d) with "
+								  "internal synchronization interface, set rgbd_cameras=0 and use "
+								  "rgbd_images input topic instead for more cameras (for which "
+								  "rgbdx_sync node can sync up to 8 cameras).",
+								  getName().c_str(), rgbdCameras);
 				}
 
 			}
@@ -723,6 +820,76 @@ private:
 		}
 	}
 
+	void callbackRGBD5(
+			const rtabmap_msgs::RGBDImageConstPtr& image,
+			const rtabmap_msgs::RGBDImageConstPtr& image2,
+			const rtabmap_msgs::RGBDImageConstPtr& image3,
+			const rtabmap_msgs::RGBDImageConstPtr& image4,
+			const rtabmap_msgs::RGBDImageConstPtr& image5)
+	{
+		if(!this->isPaused())
+		{
+			std::vector<cv_bridge::CvImageConstPtr> leftMsgs(5);
+			std::vector<cv_bridge::CvImageConstPtr> rightMsgs(5);
+			std::vector<sensor_msgs::CameraInfo> leftInfoMsgs;
+			std::vector<sensor_msgs::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]);
+			rtabmap_conversions::toCvShare(image5, leftMsgs[4], rightMsgs[4]);
+			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);
+			leftInfoMsgs.push_back(image5->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);
+			rightInfoMsgs.push_back(image5->depth_camera_info);
+
+			this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		}
+	}
+
+	void callbackRGBD6(
+			const rtabmap_msgs::RGBDImageConstPtr& image,
+			const rtabmap_msgs::RGBDImageConstPtr& image2,
+			const rtabmap_msgs::RGBDImageConstPtr& image3,
+			const rtabmap_msgs::RGBDImageConstPtr& image4,
+			const rtabmap_msgs::RGBDImageConstPtr& image5,
+			const rtabmap_msgs::RGBDImageConstPtr& image6)
+	{
+		if(!this->isPaused())
+		{
+			std::vector<cv_bridge::CvImageConstPtr> leftMsgs(6);
+			std::vector<cv_bridge::CvImageConstPtr> rightMsgs(6);
+			std::vector<sensor_msgs::CameraInfo> leftInfoMsgs;
+			std::vector<sensor_msgs::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]);
+			rtabmap_conversions::toCvShare(image5, leftMsgs[4], rightMsgs[4]);
+			rtabmap_conversions::toCvShare(image6, leftMsgs[5], rightMsgs[5]);
+			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);
+			leftInfoMsgs.push_back(image5->rgb_camera_info);
+			leftInfoMsgs.push_back(image6->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);
+			rightInfoMsgs.push_back(image5->depth_camera_info);
+			rightInfoMsgs.push_back(image6->depth_camera_info);
+
+			this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		}
+	}
+
 protected:
 	virtual void flushCallbacks()
 	{
@@ -799,6 +966,56 @@ protected:
 					rgbd_image4_sub_);
 			exactSync4_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD4, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::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(boost::bind(&StereoOdometry::callbackRGBD5, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::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(boost::bind(&StereoOdometry::callbackRGBD5, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5));
+		}
+		if(approxSync6_)
+		{
+			delete approxSync6_;
+			approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>(
+					MyApproxSync6Policy(queueSize_),
+					rgbd_image1_sub_,
+					rgbd_image2_sub_,
+					rgbd_image3_sub_,
+					rgbd_image4_sub_,
+					rgbd_image5_sub_,
+					rgbd_image6_sub_);
+			approxSync6_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6));
+		}
+		if(exactSync6_)
+		{
+			delete exactSync6_;
+			exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>(
+					MyExactSync6Policy(queueSize_),
+					rgbd_image1_sub_,
+					rgbd_image2_sub_,
+					rgbd_image3_sub_,
+					rgbd_image4_sub_,
+					rgbd_image5_sub_,
+					rgbd_image6_sub_);
+			exactSync6_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6));
+		}
 	}
 
 private:
@@ -814,6 +1031,7 @@ private:
 	message_filters::Subscriber<rtabmap_msgs::RGBDImage> rgbd_image3_sub_;
 	message_filters::Subscriber<rtabmap_msgs::RGBDImage> rgbd_image4_sub_;
 	message_filters::Subscriber<rtabmap_msgs::RGBDImage> rgbd_image5_sub_;
+	message_filters::Subscriber<rtabmap_msgs::RGBDImage> rgbd_image6_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_;
@@ -831,6 +1049,14 @@ private:
 	message_filters::Synchronizer<MyApproxSync4Policy> * approxSync4_;
 	typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyExactSync4Policy;
 	message_filters::Synchronizer<MyExactSync4Policy> * exactSync4_;
+	typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyApproxSync5Policy;
+	message_filters::Synchronizer<MyApproxSync5Policy> * approxSync5_;
+	typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyExactSync5Policy;
+	message_filters::Synchronizer<MyExactSync5Policy> * exactSync5_;
+	typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyApproxSync6Policy;
+	message_filters::Synchronizer<MyApproxSync6Policy> * approxSync6_;
+	typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyExactSync6Policy;
+	message_filters::Synchronizer<MyExactSync6Policy> * exactSync6_;
 
 	int queueSize_;
 	bool keepColor_;