Updated RGBDImage msg with depth camera info, also made changes so that RGBDImage can be used also to sync stereo images.

2026-10-04 00:37:46 +08:00 · 2018-02-13 21:35:15 -05:00
parent 6b14693a95
commit 438fad643c
24 changed files with 384 additions and 209 deletions
@@ -589,25 +589,39 @@ void CommonDataSubscriber::commonSingleDepthCallback(
 		const rtabmap_ros::UserDataConstPtr & userDataMsg,
 		const cv_bridge::CvImageConstPtr & imageMsg,
 		const cv_bridge::CvImageConstPtr & depthMsg,
-		const sensor_msgs::CameraInfo & cameraInfoMsg,
+		const sensor_msgs::CameraInfo & rgbCameraInfoMsg,
+		const sensor_msgs::CameraInfo & depthCameraInfoMsg,
 		const sensor_msgs::LaserScanConstPtr& scanMsg,
 		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
 {
 	callbackCalled();
-	std::vector<cv_bridge::CvImageConstPtr> imageMsgs;
-	std::vector<cv_bridge::CvImageConstPtr> depthMsgs;
-	std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs;
-	if(imageMsg.get())
+
+	if(depthMsg.get() == 0 ||
+	   depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
+	   depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
+	   depthMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
 	{
-		imageMsgs.push_back(imageMsg);
+		std::vector<cv_bridge::CvImageConstPtr> imageMsgs;
+		std::vector<cv_bridge::CvImageConstPtr> depthMsgs;
+		std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs;
+		if(imageMsg.get())
+		{
+			imageMsgs.push_back(imageMsg);
+		}
+		if(depthMsg.get())
+		{
+			depthMsgs.push_back(depthMsg);
+		}
+		cameraInfoMsgs.push_back(rgbCameraInfoMsg);
+		commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
 	}
-	if(depthMsg.get())
+	else // assuming stereo
 	{
-		depthMsgs.push_back(depthMsg);
+		commonStereoCallback(odomMsg, userDataMsg, imageMsg, depthMsg, rgbCameraInfoMsg, depthCameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
 	}
-	cameraInfoMsgs.push_back(cameraInfoMsg);
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+
+
 }

 } /* namespace rtabmap_ros */
@@ -754,7 +754,7 @@ void CoreWrapper::rgbCallback(
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
 }

 void CoreWrapper::rgbOdomCallback(
@@ -767,7 +767,7 @@ void CoreWrapper::rgbOdomCallback(
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
 }

 bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg)
@@ -1145,10 +1145,11 @@ void CoreWrapper::commonDepthCallbackImpl(

 void CoreWrapper::commonStereoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
-		const sensor_msgs::ImageConstPtr& leftImageMsg,
-		const sensor_msgs::ImageConstPtr& rightImageMsg,
-		const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
-		const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const cv_bridge::CvImageConstPtr& leftImageMsg,
+		const cv_bridge::CvImageConstPtr& rightImageMsg,
+		const sensor_msgs::CameraInfo& leftCamInfoMsg,
+		const sensor_msgs::CameraInfo& rightCamInfoMsg,
 		const sensor_msgs::LaserScanConstPtr& scan2dMsg,
 		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
@@ -1244,9 +1245,9 @@ void CoreWrapper::commonStereoCallback(
 		std::vector<cv_bridge::CvImageConstPtr> rgbImages(1);
 		std::vector<cv_bridge::CvImageConstPtr> depthImages(1);
 		std::vector<sensor_msgs::CameraInfo> cameraInfos(1);
-		rgbImages[0] = cv_bridge::toCvShare(leftImageMsg);
+		rgbImages[0] = leftImageMsg;
 		depthImages[0] = imgDepth;
-		cameraInfos[0] = *leftCamInfoMsg;
+		cameraInfos[0] = leftCamInfoMsg;

 		commonDepthCallbackImpl(odomFrameId, rtabmap_ros::UserDataConstPtr(), rgbImages, depthImages, cameraInfos, scan2dMsg, scan3dMsg, odomInfoMsg);
 		return;
@@ -1300,15 +1301,28 @@ void CoreWrapper::commonStereoCallback(
 		}
 	}

-	cv::Mat userData = userData_;
-	userData_ = cv::Mat();
-
 	Transform groundTruthPose;
 	if(!groundTruthFrameId_.empty())
 	{
 		groundTruthPose = rtabmap_ros::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, lastPoseStamp_, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
 	}

+	cv::Mat userData;
+	if(userDataMsg.get())
+	{
+		userData = rtabmap_ros::userDataFromROS(*userDataMsg);
+		if(!userData_.empty())
+		{
+			NODELET_WARN("Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!");
+			userData_ = cv::Mat();
+		}
+	}
+	else
+	{
+		userData = userData_;
+		userData_ = cv::Mat();
+	}
+
 	SensorData data(scan,
 			LaserScanInfo(
 					scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():scan3dMsg.get() != 0?scanCloudMaxPoints_:0,
@@ -590,10 +590,11 @@ void GuiWrapper::commonDepthCallback(

 void GuiWrapper::commonStereoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
-		const sensor_msgs::ImageConstPtr& leftImageMsg,
-		const sensor_msgs::ImageConstPtr& rightImageMsg,
-		const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
-		const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const cv_bridge::CvImageConstPtr& leftImageMsg,
+		const cv_bridge::CvImageConstPtr& rightImageMsg,
+		const sensor_msgs::CameraInfo& leftCamInfoMsg,
+		const sensor_msgs::CameraInfo& rightCamInfoMsg,
 		const sensor_msgs::LaserScanConstPtr& scan2dMsg,
 		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
@@ -615,7 +616,7 @@ void GuiWrapper::commonStereoCallback(
 		}
 		else
 		{
-			odomHeader = leftCamInfoMsg->header;
+			odomHeader = leftCamInfoMsg.header;
 		}
 		odomHeader.frame_id = odomFrameId_;
 	}
@@ -752,6 +753,7 @@ void GuiWrapper::defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg)
 			cv_bridge::CvImageConstPtr(),
 			cv_bridge::CvImageConstPtr(),
 			sensor_msgs::CameraInfo(),
+			sensor_msgs::CameraInfo(),
 			sensor_msgs::LaserScanConstPtr(),
 			sensor_msgs::PointCloud2ConstPtr(),
 			rtabmap_ros::OdomInfoConstPtr());
@@ -69,8 +69,7 @@ MapsManager::MapsManager() :
 		assembledGround_(new pcl::PointCloud<pcl::PointXYZRGB>),
 		occupancyGrid_(new OccupancyGrid),
 		octomap_(0),
-		octomapTreeDepth_(16),
-		octomapOccupancyThr_(0.5)
+		octomapTreeDepth_(16)
 {
 }

@@ -107,9 +106,7 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s

 #ifdef WITH_OCTOMAP_ROS
 #ifdef RTABMAP_OCTOMAP
-	pnh.param("octomap_occupancy_thr", octomapOccupancyThr_, octomapOccupancyThr_);
-	UASSERT(octomapOccupancyThr_>=0.0 && octomapOccupancyThr_<=1.0);
-	octomap_ = new OctoMap(occupancyGrid_->getCellSize(), octomapOccupancyThr_, occupancyGrid_->isFullUpdate(), occupancyGrid_->getUpdateError());
+	octomap_ = new OctoMap(occupancyGrid_->getCellSize(), 0.5, occupancyGrid_->isFullUpdate(), occupancyGrid_->getUpdateError());
 	pnh.param("octomap_tree_depth", octomapTreeDepth_, octomapTreeDepth_);
 	if(octomapTreeDepth_ > 16)
 	{
@@ -122,7 +119,6 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s
 		octomapTreeDepth_ = 16;
 	}
 	ROS_INFO("%s(maps): octomap_tree_depth         = %d", name.c_str(), octomapTreeDepth_);
-	ROS_INFO("%s(maps): octomap_occupancy_thr      = %f", name.c_str(), octomapOccupancyThr_);
 #endif
 #endif

@@ -242,8 +238,8 @@ void MapsManager::backwardCompatibilityParameters(ros::NodeHandle & pnh, Paramet

 	// moved
 	parameterMoved(pnh, "cloud_decimation", Parameters::kGridDepthDecimation(), parameters);
-	parameterMoved(pnh, "cloud_max_depth", Parameters::kGridDepthMax(), parameters);
-	parameterMoved(pnh, "cloud_min_depth", Parameters::kGridDepthMin(), parameters);
+	parameterMoved(pnh, "cloud_max_depth", Parameters::kGridRangeMax(), parameters);
+	parameterMoved(pnh, "cloud_min_depth", Parameters::kGridRangeMin(), parameters);
 	parameterMoved(pnh, "cloud_voxel_size", Parameters::kGridCellSize(), parameters);
 	parameterMoved(pnh, "cloud_floor_culling_height", Parameters::kGridMaxGroundHeight(), parameters);
 	parameterMoved(pnh, "cloud_ceiling_culling_height", Parameters::kGridMaxObstacleHeight(), parameters);
@@ -270,6 +266,7 @@ void MapsManager::backwardCompatibilityParameters(ros::NodeHandle & pnh, Paramet
 #ifdef WITH_OCTOMAP_ROS
 #ifdef RTABMAP_OCTOMAP
 	parameterMoved(pnh, "octomap_ground_is_obstacle", Parameters::kGridGroundIsObstacle(), parameters);
+	parameterMoved(pnh, "octomap_occupancy_thr", Parameters::kGridGlobalOctoMapOccupancyThr(), parameters);
 #endif
 #endif
 }
@@ -286,7 +283,7 @@ void MapsManager::setParameters(const rtabmap::ParametersMap & parameters)
 		delete octomap_;
 		octomap_ = 0;
 	}
-	octomap_ = new OctoMap(occupancyGrid_->getCellSize(), octomapOccupancyThr_, occupancyGrid_->isFullUpdate(), occupancyGrid_->getUpdateError());
+	octomap_ = new OctoMap(parameters_);
 #endif
 #endif
 }
@@ -192,12 +192,23 @@ void toCvShare(const rtabmap_ros::RGBDImageConstPtr & image, cv_bridge::CvImageC
 	}
 	else if(!image->depthCompressed.data.empty())
 	{
-		cv_bridge::CvImagePtr ptr = boost::make_shared<cv_bridge::CvImage>();
-		ptr->header = image->depthCompressed.header;
-		ptr->image = rtabmap::uncompressImage(image->depthCompressed.data);
-		ROS_ASSERT(ptr->image.empty() || ptr->image.type() == CV_32FC1 || ptr->image.type() == CV_16UC1);
-		ptr->encoding = ptr->image.empty()?"":ptr->image.type() == CV_32FC1?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1;
-		depth = ptr;
+		if(image->depthCompressed.format.compare("jpg")==0)
+		{
+#ifdef CV_BRIDGE_HYDRO
+			ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
+#else
+			depth = cv_bridge::toCvCopy(image->depthCompressed);
+#endif
+		}
+		else
+		{
+			cv_bridge::CvImagePtr ptr = boost::make_shared<cv_bridge::CvImage>();
+			ptr->header = image->depthCompressed.header;
+			ptr->image = rtabmap::uncompressImage(image->depthCompressed.data);
+			ROS_ASSERT(ptr->image.empty() || ptr->image.type() == CV_32FC1 || ptr->image.type() == CV_16UC1);
+			ptr->encoding = ptr->image.empty()?"":ptr->image.type() == CV_32FC1?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1;
+			depth = ptr;
+		}
 	}
 	else
 	{
@@ -1284,10 +1295,10 @@ bool convertRGBDMsgs(
 }

 bool convertStereoMsg(
-		const sensor_msgs::ImageConstPtr& leftImageMsg,
-		const sensor_msgs::ImageConstPtr& rightImageMsg,
-		const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
-		const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
+		const cv_bridge::CvImageConstPtr& leftImageMsg,
+		const cv_bridge::CvImageConstPtr& rightImageMsg,
+		const sensor_msgs::CameraInfo& leftCamInfoMsg,
+		const sensor_msgs::CameraInfo& rightCamInfoMsg,
 		const std::string & frameId,
 		const std::string & odomFrameId,
 		const ros::Time & odomStamp,
@@ -1298,7 +1309,6 @@ bool convertStereoMsg(
 		double waitForTransform)
 {
 	UASSERT(leftImageMsg.get() && rightImageMsg.get());
-	UASSERT(leftCamInfoMsg.get() && rightCamInfoMsg.get());

 	if(!(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
 		leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
@@ -1319,13 +1329,13 @@ bool convertStereoMsg(
 	if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
 	   leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
 	{
-		left = cv_bridge::toCvCopy(leftImageMsg, "mono8")->image;
+		left = cv_bridge::cvtColor(leftImageMsg, "mono8")->image;
 	}
 	else
 	{
-		left = cv_bridge::toCvCopy(leftImageMsg, "bgr8")->image;
+		left = cv_bridge::cvtColor(leftImageMsg, "bgr8")->image;
 	}
-	right = cv_bridge::toCvCopy(rightImageMsg, "mono8")->image;
+	right = cv_bridge::cvtColor(rightImageMsg, "mono8")->image;

 	rtabmap::Transform localTransform = getTransform(frameId, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, listener, waitForTransform);
 	if(localTransform.isNull())
@@ -1353,7 +1363,7 @@ bool convertStereoMsg(
 		}
 	}

-	stereoModel = rtabmap_ros::stereoCameraModelFromROS(*leftCamInfoMsg, *rightCamInfoMsg, localTransform);
+	stereoModel = rtabmap_ros::stereoCameraModelFromROS(leftCamInfoMsg, rightCamInfoMsg, localTransform);

 	if(stereoModel.baseline() > 10.0)
 	{
@@ -40,7 +40,7 @@ void CommonDataSubscriber::depthCallback(
 	sensor_msgs::LaserScanConstPtr scanMsg; // Null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthScan2dCallback(
 		const sensor_msgs::ImageConstPtr& imageMsg,
@@ -52,7 +52,7 @@ void CommonDataSubscriber::depthScan2dCallback(
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthScan3dCallback(
 		const sensor_msgs::ImageConstPtr& imageMsg,
@@ -64,7 +64,7 @@ void CommonDataSubscriber::depthScan3dCallback(
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthInfoCallback(
 		const sensor_msgs::ImageConstPtr& imageMsg,
@@ -76,7 +76,7 @@ void CommonDataSubscriber::depthInfoCallback(
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthScan2dInfoCallback(
 		const sensor_msgs::ImageConstPtr& imageMsg,
@@ -88,7 +88,7 @@ void CommonDataSubscriber::depthScan2dInfoCallback(
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthScan3dInfoCallback(
 		const sensor_msgs::ImageConstPtr& imageMsg,
@@ -100,7 +100,7 @@ void CommonDataSubscriber::depthScan3dInfoCallback(
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
 }

 // RGB + Depth + Odom
@@ -114,7 +114,7 @@ void CommonDataSubscriber::depthOdomCallback(
 	sensor_msgs::LaserScanConstPtr scanMsg; // Null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthOdomScan2dCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -126,7 +126,7 @@ void CommonDataSubscriber::depthOdomScan2dCallback(
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthOdomScan3dCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -138,7 +138,7 @@ void CommonDataSubscriber::depthOdomScan3dCallback(
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthOdomInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -150,7 +150,7 @@ void CommonDataSubscriber::depthOdomInfoCallback(
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthOdomScan2dInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -162,7 +162,7 @@ void CommonDataSubscriber::depthOdomScan2dInfoCallback(
 {
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthOdomScan3dInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -174,7 +174,7 @@ void CommonDataSubscriber::depthOdomScan3dInfoCallback(
 {
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
 }

 // RGB + Depth + User Data
@@ -188,7 +188,7 @@ void CommonDataSubscriber::depthDataCallback(
 	sensor_msgs::LaserScanConstPtr scanMsg; // Null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthDataScan2dCallback(
 		const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -200,7 +200,7 @@ void CommonDataSubscriber::depthDataScan2dCallback(
 	nav_msgs::OdometryConstPtr odomMsg; // null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthDataScan3dCallback(
 		const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -212,7 +212,7 @@ void CommonDataSubscriber::depthDataScan3dCallback(
 	nav_msgs::OdometryConstPtr odomMsg; // null
 	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthDataInfoCallback(
 		const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -224,7 +224,7 @@ void CommonDataSubscriber::depthDataInfoCallback(
 	nav_msgs::OdometryConstPtr odomMsg; // null
 	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthDataScan2dInfoCallback(
 		const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -236,7 +236,7 @@ void CommonDataSubscriber::depthDataScan2dInfoCallback(
 {
 	nav_msgs::OdometryConstPtr odomMsg; // null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthDataScan3dInfoCallback(
 		const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -248,7 +248,7 @@ void CommonDataSubscriber::depthDataScan3dInfoCallback(
 {
 	nav_msgs::OdometryConstPtr odomMsg; // null
 	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
 }

 // RGB + Depth + Odom + User Data
@@ -262,7 +262,7 @@ void CommonDataSubscriber::depthOdomDataCallback(
 	sensor_msgs::LaserScanConstPtr scanMsg; // Null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthOdomDataScan2dCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -274,7 +274,7 @@ void CommonDataSubscriber::depthOdomDataScan2dCallback(
 {
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthOdomDataScan3dCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -286,7 +286,7 @@ void CommonDataSubscriber::depthOdomDataScan3dCallback(
 {
 	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthOdomDataInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -298,7 +298,7 @@ void CommonDataSubscriber::depthOdomDataInfoCallback(
 {
 	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthOdomDataScan2dInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -310,7 +310,7 @@ void CommonDataSubscriber::depthOdomDataScan2dInfoCallback(
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
 {
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthOdomDataScan3dInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -322,7 +322,7 @@ void CommonDataSubscriber::depthOdomDataScan3dInfoCallback(
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
 {
 	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
 }

 void CommonDataSubscriber::setupDepthCallbacks(
@@ -44,7 +44,7 @@ void CommonDataSubscriber::rgbdCallback(
 	sensor_msgs::LaserScanConstPtr scanMsg; // Null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbdScan2dCallback(
 		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -57,7 +57,7 @@ void CommonDataSubscriber::rgbdScan2dCallback(
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbdScan3dCallback(
 		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -70,7 +70,7 @@ void CommonDataSubscriber::rgbdScan3dCallback(
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	sensor_msgs::LaserScanConstPtr scanMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbdInfoCallback(
 		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -83,7 +83,7 @@ void CommonDataSubscriber::rgbdInfoCallback(
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	sensor_msgs::LaserScanConstPtr scanMsg; // Null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbdScan2dInfoCallback(
 		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -96,7 +96,7 @@ void CommonDataSubscriber::rgbdScan2dInfoCallback(
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbdScan3dInfoCallback(
 		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
@@ -109,7 +109,7 @@ void CommonDataSubscriber::rgbdScan3dInfoCallback(
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	sensor_msgs::LaserScanConstPtr scanMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
 }

 // 1 RGBD camera + Odom
@@ -124,7 +124,7 @@ void CommonDataSubscriber::rgbdOdomCallback(
 	sensor_msgs::LaserScanConstPtr scanMsg; // Null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbdOdomScan2dCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -137,7 +137,7 @@ void CommonDataSubscriber::rgbdOdomScan2dCallback(
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbdOdomScan3dCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -150,7 +150,7 @@ void CommonDataSubscriber::rgbdOdomScan3dCallback(
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	sensor_msgs::LaserScanConstPtr scanMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbdOdomInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -163,7 +163,7 @@ void CommonDataSubscriber::rgbdOdomInfoCallback(
 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	sensor_msgs::LaserScanConstPtr scanMsg; // Null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbdOdomScan2dInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -176,7 +176,7 @@ void CommonDataSubscriber::rgbdOdomScan2dInfoCallback(

 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbdOdomScan3dInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -189,7 +189,7 @@ void CommonDataSubscriber::rgbdOdomScan3dInfoCallback(

 	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	sensor_msgs::LaserScanConstPtr scanMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
 }

 // 1 RGBD camera + User Data
@@ -204,7 +204,7 @@ void CommonDataSubscriber::rgbdDataCallback(
 	sensor_msgs::LaserScanConstPtr scanMsg; // Null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbdDataScan2dCallback(
 		const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -217,7 +217,7 @@ void CommonDataSubscriber::rgbdDataScan2dCallback(
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbdDataScan3dCallback(
 		const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -230,7 +230,7 @@ void CommonDataSubscriber::rgbdDataScan3dCallback(
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	sensor_msgs::LaserScanConstPtr scanMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbdDataInfoCallback(
 		const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -243,7 +243,7 @@ void CommonDataSubscriber::rgbdDataInfoCallback(
 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	sensor_msgs::LaserScanConstPtr scanMsg; // Null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbdDataScan2dInfoCallback(
 		const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -256,7 +256,7 @@ void CommonDataSubscriber::rgbdDataScan2dInfoCallback(

 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbdDataScan3dInfoCallback(
 		const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -269,7 +269,7 @@ void CommonDataSubscriber::rgbdDataScan3dInfoCallback(

 	nav_msgs::OdometryConstPtr odomMsg; // Null
 	sensor_msgs::LaserScanConstPtr scanMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
 }

 // 1 RGBD camera + Odom + User Data
@@ -284,7 +284,7 @@ void CommonDataSubscriber::rgbdOdomDataCallback(
 	sensor_msgs::LaserScanConstPtr scanMsg; // Null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbdOdomDataScan2dCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -297,7 +297,7 @@ void CommonDataSubscriber::rgbdOdomDataScan2dCallback(

 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbdOdomDataScan3dCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -310,7 +310,7 @@ void CommonDataSubscriber::rgbdOdomDataScan3dCallback(

 	sensor_msgs::LaserScanConstPtr scanMsg; // Null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbdOdomDataInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -323,7 +323,7 @@ void CommonDataSubscriber::rgbdOdomDataInfoCallback(

 	sensor_msgs::LaserScanConstPtr scanMsg; // Null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbdOdomDataScan2dInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -336,7 +336,7 @@ void CommonDataSubscriber::rgbdOdomDataScan2dInfoCallback(
 	rtabmap_ros::toCvShare(image1Msg, rgb, depth);

 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbdOdomDataScan3dInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -349,7 +349,7 @@ void CommonDataSubscriber::rgbdOdomDataScan3dInfoCallback(
 	rtabmap_ros::toCvShare(image1Msg, rgb, depth);

 	sensor_msgs::LaserScanConstPtr scanMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
 }

 void CommonDataSubscriber::setupRGBDCallbacks(
@@ -39,8 +39,8 @@ namespace rtabmap_ros {
 		rtabmap_ros::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
 		rtabmap_ros::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
 		std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs; \
-		cameraInfoMsgs.push_back(image1Msg->cameraInfo); \
-		cameraInfoMsgs.push_back(image2Msg->cameraInfo);
+		cameraInfoMsgs.push_back(image1Msg->rgbCameraInfo); \
+		cameraInfoMsgs.push_back(image2Msg->rgbCameraInfo);

 // 2 RGBD
 void CommonDataSubscriber::rgbd2Callback(
@@ -40,9 +40,9 @@ namespace rtabmap_ros {
 		rtabmap_ros::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
 		rtabmap_ros::toCvShare(image3Msg, imageMsgs[2], depthMsgs[2]); \
 		std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs; \
-		cameraInfoMsgs.push_back(image1Msg->cameraInfo); \
-		cameraInfoMsgs.push_back(image2Msg->cameraInfo); \
-		cameraInfoMsgs.push_back(image3Msg->cameraInfo);
+		cameraInfoMsgs.push_back(image1Msg->rgbCameraInfo); \
+		cameraInfoMsgs.push_back(image2Msg->rgbCameraInfo); \
+		cameraInfoMsgs.push_back(image3Msg->rgbCameraInfo);

 // 3 RGBD
 void CommonDataSubscriber::rgbd3Callback(
@@ -41,10 +41,10 @@ namespace rtabmap_ros {
 		rtabmap_ros::toCvShare(image3Msg, imageMsgs[2], depthMsgs[2]); \
 		rtabmap_ros::toCvShare(image4Msg, imageMsgs[3], depthMsgs[3]); \
 		std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs; \
-		cameraInfoMsgs.push_back(image1Msg->cameraInfo); \
-		cameraInfoMsgs.push_back(image2Msg->cameraInfo); \
-		cameraInfoMsgs.push_back(image3Msg->cameraInfo); \
-		cameraInfoMsgs.push_back(image4Msg->cameraInfo);
+		cameraInfoMsgs.push_back(image1Msg->rgbCameraInfo); \
+		cameraInfoMsgs.push_back(image2Msg->rgbCameraInfo); \
+		cameraInfoMsgs.push_back(image3Msg->rgbCameraInfo); \
+		cameraInfoMsgs.push_back(image4Msg->rgbCameraInfo);

 // 4 RGBD
 void CommonDataSubscriber::rgbd4Callback(
@@ -38,10 +38,11 @@ void CommonDataSubscriber::stereoCallback(
 {
 	callbackCalled();
 	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	sensor_msgs::LaserScanConstPtr scanMsg; // null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonStereoCallback(odomMsg, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonStereoCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::stereoInfoCallback(
 		const sensor_msgs::ImageConstPtr& leftImageMsg,
@@ -52,9 +53,10 @@ void CommonDataSubscriber::stereoInfoCallback(
 {
 	callbackCalled();
 	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	sensor_msgs::LaserScanConstPtr scan2dMsg; // null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
-	commonStereoCallback(odomMsg, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+	commonStereoCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
 }

 // Stereo + Odom
@@ -66,10 +68,11 @@ void CommonDataSubscriber::stereoOdomCallback(
 		const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg)
 {
 	callbackCalled();
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	sensor_msgs::LaserScanConstPtr scanMsg; // Null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
 	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
-	commonStereoCallback(odomMsg, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonStereoCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::stereoOdomInfoCallback(
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -80,9 +83,10 @@ void CommonDataSubscriber::stereoOdomInfoCallback(
 		const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg)
 {
 	callbackCalled();
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
 	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
 	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
-	commonStereoCallback(odomMsg, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+	commonStereoCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
 }

 void CommonDataSubscriber::setupStereoCallbacks(
@@ -469,7 +469,7 @@ private:
 			std::vector<cv_bridge::CvImageConstPtr> depthMsgs(1);
 			std::vector<sensor_msgs::CameraInfo> infoMsgs;
 			rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]);
-			infoMsgs.push_back(image->cameraInfo);
+			infoMsgs.push_back(image->rgbCameraInfo);

 			this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
 		}
@@ -487,8 +487,8 @@ private:
 			std::vector<sensor_msgs::CameraInfo> infoMsgs;
 			rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]);
 			rtabmap_ros::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
-			infoMsgs.push_back(image->cameraInfo);
-			infoMsgs.push_back(image2->cameraInfo);
+			infoMsgs.push_back(image->rgbCameraInfo);
+			infoMsgs.push_back(image2->rgbCameraInfo);

 			this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
 		}
@@ -508,9 +508,9 @@ private:
 			rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]);
 			rtabmap_ros::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
 			rtabmap_ros::toCvShare(image3, imageMsgs[2], depthMsgs[2]);
-			infoMsgs.push_back(image->cameraInfo);
-			infoMsgs.push_back(image2->cameraInfo);
-			infoMsgs.push_back(image3->cameraInfo);
+			infoMsgs.push_back(image->rgbCameraInfo);
+			infoMsgs.push_back(image2->rgbCameraInfo);
+			infoMsgs.push_back(image3->rgbCameraInfo);

 			this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
 		}
@@ -532,10 +532,10 @@ private:
 			rtabmap_ros::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
 			rtabmap_ros::toCvShare(image3, imageMsgs[2], depthMsgs[2]);
 			rtabmap_ros::toCvShare(image4, imageMsgs[3], depthMsgs[3]);
-			infoMsgs.push_back(image->cameraInfo);
-			infoMsgs.push_back(image2->cameraInfo);
-			infoMsgs.push_back(image3->cameraInfo);
-			infoMsgs.push_back(image4->cameraInfo);
+			infoMsgs.push_back(image->rgbCameraInfo);
+			infoMsgs.push_back(image2->rgbCameraInfo);
+			infoMsgs.push_back(image3->rgbCameraInfo);
+			infoMsgs.push_back(image4->rgbCameraInfo);

 			this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
 		}
@@ -101,13 +101,13 @@ private:

 		if(approxSync)
 		{
-			approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
-			approxSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, _1, _2, _3));
+			approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_, cameraDepthInfoSub_);
+			approxSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, _1, _2, _3, _4));
 		}
 		else
 		{
-			exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
-			exactSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, _1, _2, _3));
+			exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_, cameraDepthInfoSub_);
+			exactSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, _1, _2, _3, _4));
 		}

 		ros::NodeHandle rgb_nh(nh, "rgb");
@@ -122,13 +122,15 @@ private:
 		imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
 		imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
 		cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
+		cameraDepthInfoSub_.subscribe(depth_nh, "camera_info", 1);

-		std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s",
+		std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s",
 							getName().c_str(),
 							approxSync?"approx":"exact",
 							imageSub_.getTopic().c_str(),
 							imageDepthSub_.getTopic().c_str(),
-							cameraInfoSub_.getTopic().c_str());
+							cameraInfoSub_.getTopic().c_str(),
+							cameraDepthInfoSub_.getTopic().c_str());

 		warningThread_ = new boost::thread(boost::bind(&RGBDSync::warningLoop, this, subscribedTopicsMsg, approxSync));
 		NODELET_INFO("%s", subscribedTopicsMsg.c_str());
@@ -156,7 +158,8 @@ private:
 	void callback(
 			  const sensor_msgs::ImageConstPtr& image,
 			  const sensor_msgs::ImageConstPtr& depth,
-			  const sensor_msgs::CameraInfoConstPtr& cameraInfo)
+			  const sensor_msgs::CameraInfoConstPtr& cameraInfo,
+			  const sensor_msgs::CameraInfoConstPtr& cameraDepthInfo)
 	{
 		callbackCalled_ = true;
 		if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers())
@@ -164,7 +167,8 @@ private:
 			rtabmap_ros::RGBDImage msg;
 			msg.header.frame_id = cameraInfo->header.frame_id;
 			msg.header.stamp = image->header.stamp>depth->header.stamp?image->header.stamp:depth->header.stamp;
-			msg.cameraInfo = *cameraInfo;
+			msg.rgbCameraInfo = *cameraInfo;
+			msg.depthCameraInfo = *cameraDepthInfo;

 			if(rgbdImageCompressedPub_.getNumSubscribers())
 			{
@@ -174,17 +178,24 @@ private:
 				imagePtr->toCompressedImageMsg(msgCompressed.rgbCompressed, cv_bridge::JPG);

 				cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth);
-				ROS_ASSERT(imageDepthPtr->image.type() == CV_32FC1 || imageDepthPtr->image.type() == CV_16UC1);
-				msgCompressed.depthCompressed.header = imageDepthPtr->header;
-				if(depthScale_ != 1.0)
+				if(imageDepthPtr->image.type() == CV_32FC1 || imageDepthPtr->image.type() == CV_16UC1)
 				{
-					msgCompressed.depthCompressed.data = rtabmap::compressImage(imageDepthPtr->image*depthScale_, ".png");
+					msgCompressed.depthCompressed.header = imageDepthPtr->header;
+					if(depthScale_ != 1.0)
+					{
+						msgCompressed.depthCompressed.data = rtabmap::compressImage(imageDepthPtr->image*depthScale_, ".png");
+					}
+					else
+					{
+						msgCompressed.depthCompressed.data = rtabmap::compressImage(imageDepthPtr->image, ".png");
+					}
+					msgCompressed.depthCompressed.format = "png";
 				}
 				else
 				{
-					msgCompressed.depthCompressed.data = rtabmap::compressImage(imageDepthPtr->image, ".png");
+					// Assume right stereo image
+					imageDepthPtr->toCompressedImageMsg(msgCompressed.depthCompressed, cv_bridge::JPG);
 				}
-				msgCompressed.depthCompressed.format = "png";

 				rgbdImageCompressedPub_.publish(msgCompressed);
 			}
@@ -218,11 +229,12 @@ private:
 	image_transport::SubscriberFilter imageSub_;
 	image_transport::SubscriberFilter imageDepthSub_;
 	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraDepthInfoSub_;

-	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncDepthPolicy;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyApproxSyncDepthPolicy;
 	message_filters::Synchronizer<MyApproxSyncDepthPolicy> * approxSyncDepth_;

-	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncDepthPolicy;
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncDepthPolicy;
 	message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_;
 };

@@ -85,45 +85,61 @@ private:
 		ros::NodeHandle & pnh = getPrivateNodeHandle();

 		bool approxSync = false;
+		bool subscribeRGBD = false;
 		pnh.param("approx_sync", approxSync, approxSync);
 		pnh.param("queue_size", queueSize_, queueSize_);
+		pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD);

 		NODELET_INFO("StereoOdometry: approx_sync = %s", approxSync?"true":"false");
 		NODELET_INFO("StereoOdometry: queue_size = %d", queueSize_);
+		NODELET_INFO("StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");

-		ros::NodeHandle left_nh(nh, "left");
-		ros::NodeHandle right_nh(nh, "right");
-		ros::NodeHandle left_pnh(pnh, "left");
-		ros::NodeHandle right_pnh(pnh, "right");
-		image_transport::ImageTransport left_it(left_nh);
-		image_transport::ImageTransport right_it(right_nh);
-		image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
-		image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
-
-		imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), 1, hintsLeft);
-		imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight);
-		cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
-		cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
-
-		if(approxSync)
+		std::string subscribedTopicsMsg;
+		if(subscribeRGBD)
 		{
-			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
-			approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
+			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
 		{
-			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
-			exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
+			ros::NodeHandle left_nh(nh, "left");
+			ros::NodeHandle right_nh(nh, "right");
+			ros::NodeHandle left_pnh(pnh, "left");
+			ros::NodeHandle right_pnh(pnh, "right");
+			image_transport::ImageTransport left_it(left_nh);
+			image_transport::ImageTransport right_it(right_nh);
+			image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
+			image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
+
+			imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), 1, hintsLeft);
+			imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight);
+			cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
+			cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
+
+			if(approxSync)
+			{
+				approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+				approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
+			}
+			else
+			{
+				exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+				exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
+			}
+
+
+			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s",
+					getName().c_str(),
+					approxSync?"approx":"exact",
+					imageRectLeft_.getTopic().c_str(),
+					imageRectRight_.getTopic().c_str(),
+					cameraInfoLeft_.getTopic().c_str(),
+					cameraInfoRight_.getTopic().c_str());
 		}
-
-
-		std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s",
-				getName().c_str(),
-				approxSync?"approx":"exact",
-				imageRectLeft_.getTopic().c_str(),
-				imageRectRight_.getTopic().c_str(),
-				cameraInfoLeft_.getTopic().c_str(),
-				cameraInfoRight_.getTopic().c_str());
 		this->startWarningThread(subscribedTopicsMsg, approxSync);
 	}

@@ -216,6 +232,84 @@ private:
 		}
 	}

+	void callbackRGBD(
+			const rtabmap_ros::RGBDImageConstPtr& image)
+	{
+		callbackCalled();
+		if(!this->isPaused())
+		{
+			cv_bridge::CvImageConstPtr imageRectLeft, imageRectRight;
+			rtabmap_ros::toCvShare(image, imageRectLeft, imageRectRight);
+
+			if(!(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) ||
+				!(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))
+			{
+				NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 recommended)");
+				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())
+			{
+				return;
+			}
+
+			ros::WallTime time = ros::WallTime::now();
+
+			int quality = -1;
+			if(!imageRectLeft->image.empty() && !imageRectRight->image.empty())
+			{
+				rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(image->rgbCameraInfo, image->depthCameraInfo, localTransform);
+				if(stereoModel.baseline() <= 0)
+				{
+					NODELET_FATAL("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)
+					{
+						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_bridge::CvImagePtr ptrImageLeft = cv_bridge::cvtColor(imageRectLeft, "mono8");
+				cv_bridge::CvImagePtr ptrImageRight = cv_bridge::cvtColor(imageRectRight, "mono8");
+
+				UTimer stepTimer;
+				//
+				UDEBUG("localTransform = %s", localTransform.prettyPrint().c_str());
+				rtabmap::SensorData data(
+						ptrImageLeft->image,
+						ptrImageRight->image,
+						stereoModel,
+						0,
+						rtabmap_ros::timestampFromROS(stamp));
+
+				this->processData(data, stamp);
+			}
+			else
+			{
+				NODELET_WARN("Odom: input images empty?!?");
+			}
+		}
+	}
+
 protected:
 	virtual void flushCallbacks()
 	{
@@ -243,6 +337,7 @@ private:
 	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_;
 	int queueSize_;
 };