Adding rtabmap_msgs/RGBDImages output topic for odometry

rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h

@@ -59,6 +59,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap_msgs/OdomInfo.h>
 #include <rtabmap_msgs/Info.h>
 #include <rtabmap_msgs/RGBDImage.h>
+#include <rtabmap_msgs/RGBDImages.h>
 #include <rtabmap_msgs/UserData.h>
 
 namespace rtabmap_conversions {
@@ -75,8 +76,17 @@ rtabmap::Transform transformFromPoseMsg(const geometry_msgs::Pose & msg, bool ig
 void toCvCopy(const rtabmap_msgs::RGBDImage & image, cv_bridge::CvImagePtr & rgb, cv_bridge::CvImagePtr & depth);
 void toCvShare(const rtabmap_msgs::RGBDImageConstPtr & image, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth);
 void toCvShare(const rtabmap_msgs::RGBDImage & image, const boost::shared_ptr<void const>& trackedObject, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth);
-void rgbdImageToROS(const rtabmap::SensorData & data, rtabmap_msgs::RGBDImage & msg, const std::string & sensorFrameId);
+void rgbdImageToROS(const rtabmap::SensorData & data,
+	rtabmap_msgs::RGBDImage & msg,
+	const std_msgs::Header & header,
+	bool compressImages,
+	const std::string & compressionFormat = "*.png");
 rtabmap::SensorData rgbdImageFromROS(const rtabmap_msgs::RGBDImageConstPtr & image);
+void rgbdImagesToROS(const rtabmap::SensorData & data, 
+	rtabmap_msgs::RGBDImages & msg,
+	const std::vector<std_msgs::Header> & headers,
+	bool compressImages,
+	const std::string & compressionFormat = "*.png");
 
 // copy data
 void compressedMatToBytes(const cv::Mat & compressed, std::vector<unsigned char> & bytes);

rtabmap_conversions/src/MsgConversion.cpp

@@ -209,11 +209,13 @@ void toCvShare(const rtabmap_msgs::RGBDImage & image, const boost::shared_ptr<vo
 	}
 }
 
-void rgbdImageToROS(const rtabmap::SensorData & data, rtabmap_msgs::RGBDImage & msg, const std::string & sensorFrameId)
+void rgbdImageToROS(const rtabmap::SensorData & data,
+	rtabmap_msgs::RGBDImage & msg,
+	const std_msgs::Header & header,
+	bool compressImages,
+	const std::string & compressionFormat)
 {
-	std_msgs::Header header;
+	msg.header = header;
-	header.frame_id = sensorFrameId;
-	header.stamp = ros::Time(data.stamp());
 	rtabmap::Transform localTransform;
 	if(data.cameraModels().size()>1)
 	{
@@ -237,18 +239,26 @@ void rgbdImageToROS(const rtabmap::SensorData & data, rtabmap_msgs::RGBDImage &
 		localTransform = data.stereoCameraModels()[0].localTransform();
 	}
 
+	if(compressImages && (!data.imageCompressed().empty() || !data.depthOrRightRaw().empty()))
+	{
+		ROS_ERROR("Conversion of compressed SensorData to RGBDImage is not implemented...");
+	}
+
 	if(!data.imageRaw().empty())
 	{
 		cv_bridge::CvImage cvImg;
 		cvImg.header = header;
 		cvImg.image = data.imageRaw();
-		UASSERT(data.imageRaw().type()==CV_8UC1 || data.imageRaw().type()==CV_8UC3);
+		UASSERT(data.imageRaw().type()==CV_8UC1 ||
-		cvImg.encoding = data.imageRaw().type()==CV_8UC1?sensor_msgs::image_encodings::MONO8:sensor_msgs::image_encodings::BGR8;
+		        data.imageRaw().type()==CV_8UC3);
-		cvImg.toImageMsg(msg.rgb);
+		cvImg.encoding = data.imageRaw().type()==CV_8UC1?sensor_msgs::image_encodings::MONO8:
-	}
+						 sensor_msgs::image_encodings::BGR8;
-	else if(!data.imageCompressed().empty())
+		if(compressImages) {
-	{
+			cvImg.toCompressedImageMsg(msg.rgb_compressed, compressionFormat == ".jpg"?cv_bridge::JPG:cv_bridge::PNG);
-		ROS_ERROR("Conversion of compressed SensorData to RGBDImage is not implemented...");
+		}
+		else {
+			cvImg.toImageMsg(msg.rgb);
+		}
 	}
 
 	if(!data.depthOrRightRaw().empty())
@@ -256,13 +266,23 @@ void rgbdImageToROS(const rtabmap::SensorData & data, rtabmap_msgs::RGBDImage &
 		cv_bridge::CvImage cvDepth;
 		cvDepth.header = header;
 		cvDepth.image = data.depthOrRightRaw();
-		UASSERT(data.depthOrRightRaw().type()==CV_8UC1 || data.depthOrRightRaw().type()==CV_16UC1 || data.depthOrRightRaw().type()==CV_32FC1);
+		UASSERT(data.depthOrRightRaw().type()==CV_8UC1 ||
-		cvDepth.encoding = data.depthOrRightRaw().type()==CV_8UC1?sensor_msgs::image_encodings::MONO8:data.depthOrRightRaw().type()==CV_16UC1?sensor_msgs::image_encodings::TYPE_16UC1:sensor_msgs::image_encodings::TYPE_32FC1;
+			    data.depthOrRightRaw().type()==CV_8UC3 ||
-		cvDepth.toImageMsg(msg.depth);
+				data.depthOrRightRaw().type()==CV_16UC1 ||
-	}
+				data.depthOrRightRaw().type()==CV_32FC1);
-	else if(!data.depthOrRightCompressed().empty())
+		cvDepth.encoding = data.depthOrRightRaw().type()==CV_8UC1?sensor_msgs::image_encodings::MONO8:
-	{
+						   data.depthOrRightRaw().type()==CV_8UC3?sensor_msgs::image_encodings::BGR8:
-		ROS_ERROR("Conversion of compressed SensorData to RGBDImage is not implemented...");
+						   data.depthOrRightRaw().type()==CV_16UC1?sensor_msgs::image_encodings::TYPE_16UC1:
+						   sensor_msgs::image_encodings::TYPE_32FC1;
+		if(compressImages) {
+			cvDepth.toCompressedImageMsg(msg.depth_compressed,
+				data.depthOrRightRaw().type()!=CV_16UC1 && 
+				data.depthOrRightRaw().type()!=CV_32FC1 && 
+				compressionFormat == ".jpg"?cv_bridge::JPG:cv_bridge::PNG);
+		}
+		else {
+			cvDepth.toImageMsg(msg.depth);
+		}
 	}
 
 	//convert features
@@ -430,6 +450,136 @@ rtabmap::SensorData rgbdImageFromROS(const rtabmap_msgs::RGBDImageConstPtr & ima
 	return data;
 }
 
+void rgbdImagesToROS(const rtabmap::SensorData & data, 
+	rtabmap_msgs::RGBDImages & msg,
+	const std::vector<std_msgs::Header> & headers,
+	bool compressImages,
+	const std::string & compressionFormat)
+{
+	UASSERT(!headers.empty());
+	msg.header = headers[0];
+
+	if(compressImages && 
+	   ((data.imageRaw().empty() && !data.imageCompressed().empty()) ||
+	    (data.depthOrRightRaw().empty() && !data.depthOrRightCompressed().empty())))
+	{
+		ROS_ERROR("Conversion of compressed SensorData to RGBDImage is not implemented...");
+	}
+
+	cv_bridge::CvImage cvImg;
+	if(!data.imageRaw().empty())
+	{
+		UASSERT(data.imageRaw().type()==CV_8UC1 || 
+				data.imageRaw().type()==CV_8UC3);
+		cvImg.encoding = data.imageRaw().type()==CV_8UC1?sensor_msgs::image_encodings::MONO8:
+						 sensor_msgs::image_encodings::BGR8;
+	}
+	cv_bridge::CvImage cvDepthORRight;
+	if(!data.depthOrRightRaw().empty())
+	{
+		UASSERT(data.depthOrRightRaw().type()==CV_8UC1 ||
+				data.depthOrRightRaw().type()==CV_8UC3 ||
+				data.depthOrRightRaw().type()==CV_16UC1 || 
+				data.depthOrRightRaw().type()==CV_32FC1);
+		cvDepthORRight.encoding = data.depthOrRightRaw().type()==CV_8UC1?sensor_msgs::image_encodings::MONO8:
+						          data.depthOrRightRaw().type()==CV_8UC3?sensor_msgs::image_encodings::BGR8:
+						          data.depthOrRightRaw().type()==CV_16UC1?sensor_msgs::image_encodings::TYPE_16UC1:
+						          sensor_msgs::image_encodings::TYPE_32FC1;
+	}
+
+	if(!data.cameraModels().empty())
+	{
+		//rgb+depth
+		if(data.cameraModels().size() != headers.size())
+		{
+			UERROR("Cannot convert multi-camera data to rgbd images if number of sensor frames are not equal to number of cameras.");
+			return;
+		}
+		msg.rgbd_images.resize(data.cameraModels().size());
+		int subImageWidth = data.imageRaw().cols / data.cameraModels().size();
+		int subDepthWidth = data.depthOrRightRaw().cols / data.cameraModels().size();
+		for(size_t i=0; i<data.cameraModels().size(); ++i) {
+			rtabmap_conversions::cameraModelToROS(data.cameraModels()[i], msg.rgbd_images[i].rgb_camera_info);
+			msg.rgbd_images[i].rgb_camera_info.header = headers[i];
+
+			if(!data.imageRaw().empty())
+			{
+				cvImg.image = cv::Mat(data.imageRaw(), cv::Range::all(), cv::Range(0,subImageWidth));
+				cvImg.header = headers[i];
+				
+				if(compressImages) {
+					cvImg.toCompressedImageMsg(msg.rgbd_images[i].rgb_compressed, compressionFormat == ".jpg"?cv_bridge::JPG:cv_bridge::PNG);
+				}
+				else {
+					cvImg.toImageMsg(msg.rgbd_images[i].rgb);
+				}
+			}
+
+			if(!data.depthOrRightRaw().empty())
+			{
+				cvDepthORRight.image = cv::Mat(data.depthOrRightRaw(), cv::Range::all(), cv::Range(0,subDepthWidth));
+				cvDepthORRight.header = headers[i];
+				
+				if(compressImages) {
+					cvDepthORRight.toCompressedImageMsg(msg.rgbd_images[i].depth_compressed,
+						data.depthOrRightRaw().type()!=CV_16UC1 && 
+						data.depthOrRightRaw().type()!=CV_32FC1 && 
+						compressionFormat == ".jpg"?cv_bridge::JPG:cv_bridge::PNG);
+				}
+				else {
+					cvDepthORRight.toImageMsg(msg.rgbd_images[i].depth);
+				}
+			}
+		}
+		
+	}
+	else if(!data.stereoCameraModels().empty())
+	{
+		//stereo
+		if(data.stereoCameraModels().size() != headers.size())
+		{
+			UERROR("Cannot convert multi-camera data to rgbd images if number of sensor frames are not equal to number of cameras.");
+			return;
+		}
+		msg.rgbd_images.resize(data.stereoCameraModels().size());
+		int subImageWidth = data.imageRaw().cols / data.cameraModels().size();
+		int subRightWidth = data.depthOrRightRaw().cols / data.cameraModels().size();
+		for(size_t i=0; i<data.stereoCameraModels().size(); ++i) {
+			rtabmap_conversions::cameraModelToROS(data.stereoCameraModels()[i].left(), msg.rgbd_images[i].rgb_camera_info);
+			rtabmap_conversions::cameraModelToROS(data.stereoCameraModels()[i].right(), msg.rgbd_images[i].depth_camera_info);
+			msg.rgbd_images[i].rgb_camera_info.header = headers[i];
+			msg.rgbd_images[i].depth_camera_info.header = headers[i];
+
+			if(!data.imageRaw().empty())
+			{
+				cvImg.image = cv::Mat(data.imageRaw(), cv::Range::all(), cv::Range(0,subImageWidth));
+				cvImg.header = headers[i];
+				if(compressImages) {
+					cvImg.toCompressedImageMsg(msg.rgbd_images[i].rgb_compressed, compressionFormat == ".jpg"?cv_bridge::JPG:cv_bridge::PNG);
+				}
+				else {
+					cvImg.toImageMsg(msg.rgbd_images[i].rgb);
+				}
+			}
+
+			if(!data.depthOrRightRaw().empty())
+			{
+				cvDepthORRight.image = cv::Mat(data.depthOrRightRaw(), cv::Range::all(), cv::Range(0,subRightWidth));
+				cvDepthORRight.header = headers[i];
+				
+				if(compressImages) {
+					cvDepthORRight.toCompressedImageMsg(msg.rgbd_images[i].depth_compressed, compressionFormat == ".jpg"?cv_bridge::JPG:cv_bridge::PNG);
+				}
+				else {
+					cvDepthORRight.toImageMsg(msg.rgbd_images[i].depth);
+				}
+			}
+		}
+	}
+
+	// TODO: Could be possible to forward keypoints/descriptors/3d points by splitting them against each RGBDImage msg.
+}
+
 void compressedMatToBytes(const cv::Mat & compressed, std::vector<unsigned char> & bytes)
 {
 	UASSERT(compressed.empty() || compressed.type() == CV_8UC1);

rtabmap_odom/include/rtabmap_odom/OdometryROS.h

@@ -63,7 +63,7 @@ public:
 	OdometryROS(bool stereoParams, bool visParams, bool icpParams);
 	virtual ~OdometryROS();
 
-	void processData(rtabmap::SensorData & data, const std_msgs::Header & header);
+	void processData(rtabmap::SensorData & data, const std_msgs::Header & header, const std::vector<std_msgs::Header> & multiCamHeaders = std::vector<std_msgs::Header>());
 
 	bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool resetToPose(rtabmap_msgs::ResetPose::Request&, rtabmap_msgs::ResetPose::Response&);
@@ -127,6 +127,9 @@ private:
 	ros::Publisher odomLocalScanMap_;
 	ros::Publisher odomLastFrame_;
 	ros::Publisher odomRgbdImagePub_;
+	ros::Publisher odomRgbdImageCompressedPub_;
+	ros::Publisher odomRgbdImagesPub_;
+	ros::Publisher odomRgbdImagesCompressedPub_;
 	ros::Publisher odomSensorDataPub_;
 	ros::Publisher odomSensorDataFeaturesPub_;
 	ros::Publisher odomSensorDataCompressedPub_;
@@ -148,6 +151,7 @@ private:
 	USemaphore dataReady_;
 	rtabmap::SensorData dataToProcess_;
 	std_msgs::Header dataHeaderToProcess_;
+	std::vector<std_msgs::Header> dataMultiCamHeadersToProcess_;
 	bool bufferedDataToProcess_;
 
 	bool paused_;

rtabmap_odom/src/OdometryROS.cpp

@@ -108,6 +108,9 @@ void OdometryROS::onInit()
 	odomLocalScanMap_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_scan_map", 1);
 	odomLastFrame_ = nh.advertise<sensor_msgs::PointCloud2>("odom_last_frame", 1);
 	odomRgbdImagePub_ = nh.advertise<rtabmap_msgs::RGBDImage>("odom_rgbd_image", 1);
+	odomRgbdImageCompressedPub_ = nh.advertise<rtabmap_msgs::RGBDImage>("odom_rgbd_image/compressed", 1);
+	odomRgbdImagesPub_ = nh.advertise<rtabmap_msgs::RGBDImages>("odom_rgbd_images", 1);
+	odomRgbdImagesCompressedPub_ = nh.advertise<rtabmap_msgs::RGBDImages>("odom_rgbd_images/compressed", 1);
 	odomSensorDataPub_ = nh.advertise<rtabmap_msgs::SensorData>("odom_sensor_data/raw", 1);
 	odomSensorDataFeaturesPub_ = nh.advertise<rtabmap_msgs::SensorData>("odom_sensor_data/features", 1);
 	odomSensorDataCompressedPub_ = nh.advertise<rtabmap_msgs::SensorData>("odom_sensor_data/compressed", 1);
@@ -457,7 +460,7 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg)
 	}
 }
 
-void OdometryROS::processData(SensorData & data, const std_msgs::Header & header)
+void OdometryROS::processData(SensorData & data, const std_msgs::Header & header, const std::vector<std_msgs::Header> & multiCamHeaders)
 {
 	//NODELET_WARN("Received image: %f delay=%f", data.stamp(), (ros::Time::now() - header.stamp).toSec());
 	if(dataMutex_.lockTry() == 0)
@@ -468,6 +471,7 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
 		}
 		dataToProcess_ = data;
 		dataHeaderToProcess_ = header;
+		dataMultiCamHeadersToProcess_ = multiCamHeaders;
 		bufferedDataToProcess_ = false;
 		dataReady_.release();
 		dataMutex_.unlock();
@@ -551,12 +555,14 @@ void OdometryROS::mainLoop()
 				(previousClockTime_ - clockNow).toSec());
 			SensorData dataCpy = dataToProcess_;
 			std_msgs::Header headerCpy = dataHeaderToProcess_;
+			std::vector<std_msgs::Header> multiCamHeadersCpy = dataMultiCamHeadersToProcess_;
 			ros::Time previousCpy = previousClockTime_;
 			this->reset(odometry_->getPose());
 			if(previousCpy > headerCpy.stamp) {
 				// new frame is using new clock, process it now
 				dataToProcess_ = dataCpy;
 				dataHeaderToProcess_ = headerCpy;
+				dataMultiCamHeadersToProcess_ = multiCamHeadersCpy;
 				dataReady_.release();
 				NODELET_WARN("Odometry: Restarting with frame: %f (clock previous=%f, new=%f)",
 					headerCpy.stamp.toSec(), previousCpy.toSec(), clockNow.toSec());
@@ -1037,18 +1043,50 @@ void OdometryROS::mainLoop()
 
 	postProcessData(data, header);
 
-	if(!data.imageRaw().empty() && odomRgbdImagePub_.getNumSubscribers())
+	if(!data.imageRaw().empty()) {
-	{
+		if(odomRgbdImagePub_.getNumSubscribers() || odomRgbdImageCompressedPub_.getNumSubscribers())
-		if(!header.frame_id.empty())
 		{
-			rtabmap_msgs::RGBDImage msg;
+			if(data.cameraModels().size()<=1 && data.stereoCameraModels().size()<=1)
-			rtabmap_conversions::rgbdImageToROS(data, msg, header.frame_id);
+			{
-			msg.header = header; // use corresponding time stamp to image
+				if(odomRgbdImagePub_.getNumSubscribers()) {
-			odomRgbdImagePub_.publish(msg);
+					rtabmap_msgs::RGBDImage msg;
+					rtabmap_conversions::rgbdImageToROS(data, msg, header, false);
+					odomRgbdImagePub_.publish(msg);
+				}
+				
+				if(odomRgbdImageCompressedPub_.getNumSubscribers())
+				{
+					rtabmap_msgs::RGBDImage msg;
+					rtabmap_conversions::rgbdImageToROS(data, msg, header, true, compressionImgFormat_);
+					odomRgbdImageCompressedPub_.publish(msg);
+				}
+
+			}
+			else
+			{
+				ROS_WARN("Cannot convert SensorData for %s topic because it has more than one camera (%ld). "
+					"Subscribe to %s topic instead to get all cameras.",
+					odomRgbdImagePub_.getTopic().c_str(),
+					std::max(data.cameraModels().size(), data.stereoCameraModels().size()), 
+					odomRgbdImagesPub_.getTopic().c_str());
+			}
 		}
-		else
+
+		if(!dataMultiCamHeadersToProcess_.empty())
 		{
-			ROS_WARN("Sensor frame not set, cannot convert SensorData to RGBDImage");
+			if(odomRgbdImagesPub_.getNumSubscribers()) {
+				rtabmap_msgs::RGBDImages msg;
+				rtabmap_conversions::rgbdImagesToROS(data, msg, dataMultiCamHeadersToProcess_, false);
+				msg.header = header;
+				odomRgbdImagesPub_.publish(msg);
+			}
+			if(odomRgbdImagesCompressedPub_.getNumSubscribers())
+			{
+				rtabmap_msgs::RGBDImages msg;
+				rtabmap_conversions::rgbdImagesToROS(data, msg, dataMultiCamHeadersToProcess_, true, compressionImgFormat_);
+				msg.header = header;
+				odomRgbdImagesCompressedPub_.publish(msg);
+			}
 		}
 	}
 
@@ -1197,6 +1235,7 @@ void OdometryROS::reset(const Transform & pose)
 	imuProcessed_ = false;
 	dataToProcess_ = SensorData();
 	dataHeaderToProcess_ = std_msgs::Header();
+	dataMultiCamHeadersToProcess_.clear();
 	bufferedDataToProcess_ = false;
 	imuMutex_.lock();
 	imus_.clear();

rtabmap_odom/src/nodelets/rgbd_odometry.cpp

@@ -443,6 +443,7 @@ private:
 		cv::Mat rgb;
 		cv::Mat depth;
 		std::vector<rtabmap::CameraModel> cameraModels;
+		std::vector<std_msgs::Header> mutiCamHeaders;
 		for(unsigned int i=0; i<rgbImages.size(); ++i)
 		{
 			if(!(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
@@ -539,7 +540,7 @@ private:
 			}
 			if(depth.empty())
 			{
-				depth = cv::Mat(depthHeight, depthWidth*cameraCount, ptrDepth->image.type());
+				depth = cv::Mat::zeros(depthHeight, depthWidth*cameraCount, ptrDepth->image.type());
 			}
 
 			if(ptrImage->image.type() == rgb.type())
@@ -554,7 +555,8 @@ private:
 
 			if(ptrDepth->image.type() == depth.type())
 			{
-				ptrDepth->image.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
+				//if(i < 4)
+					ptrDepth->image.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
 			}
 			else
 			{
@@ -563,6 +565,7 @@ private:
 			}
 
 			cameraModels.push_back(rtabmap_conversions::cameraModelFromROS(cameraInfos[i], localTransform));
+			mutiCamHeaders.push_back(rgbImages[i]->header);
 		}
 
 		rtabmap::SensorData data(
@@ -575,7 +578,7 @@ private:
 		std_msgs::Header header;
 		header.stamp = higherStamp;
 		header.frame_id = rgbImages.size()==1?rgbImages[0]->header.frame_id:"";
-		this->processData(data, header);
+		this->processData(data, header, mutiCamHeaders);
 	}
 
 	void callback(

rtabmap_odom/src/nodelets/stereo_odometry.cpp

@@ -432,6 +432,7 @@ private:
 		cv::Mat left;
 		cv::Mat right;
 		std::vector<rtabmap::StereoCameraModel> cameraModels;
+		std::vector<std_msgs::Header> mutiCamHeaders;
 		for(unsigned int i=0; i<leftImages.size(); ++i)
 		{
 			if(!(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
@@ -555,6 +556,8 @@ private:
 
 				rtabmap::StereoCameraModel stereoModel = rtabmap_conversions::stereoCameraModelFromROS(leftCameraInfos[i], rightCameraInfos[i], localTransform, stereoTransform);
 
+					std::cout << i << " " << stereoModel << std::endl;
+
 				if( stereoModel.baseline() == 0 &&
 					alreadyRectified &&
 					!rightCameraInfos[i].header.frame_id.empty() &&
@@ -587,6 +590,8 @@ private:
 								stereoTransform.x(),
 								stereoModel.localTransform(),
 								stereoModel.left().imageSize());
+
+								std::cout << i << " B " << stereoModel << std::endl;
 					}
 				}
 
@@ -662,6 +667,7 @@ private:
 				}
 
 				cameraModels.push_back(stereoModel);
+				mutiCamHeaders.push_back(leftImages[i]->header);
 			}
 			else
 			{
@@ -681,7 +687,7 @@ private:
 		std_msgs::Header header;
 		header.stamp = higherStamp;
 		header.frame_id = leftImages.size()==1?leftImages[0]->header.frame_id:"";
-		this->processData(data, header);
+		this->processData(data, header, mutiCamHeaders);
 	}
 
 	void callback(