point_cloud_xyz[rgb]: Fixed roi ratios when camera_info is not the same resolution than depth image.

src/nodelets/point_cloud_xyz.cpp

@@ -205,12 +205,12 @@ private:
 	}
 
 	void callback(
-			  const sensor_msgs::ImageConstPtr& depth,
+			  const sensor_msgs::ImageConstPtr& depthMsg,
 			  const sensor_msgs::CameraInfoConstPtr& cameraInfo)
 	{
-		if(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 &&
+		if(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 &&
-		   depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0 &&
+		   depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0 &&
-		   depth->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0)
+		   depthMsg->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0)
 		{
 			NODELET_ERROR("Input type depth=32FC1,16UC1,MONO16");
 			return;
@@ -220,28 +220,68 @@ private:
 		{
 			ros::WallTime time = ros::WallTime::now();
 
-			cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth);
+			cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depthMsg);
 			cv::Rect roi = rtabmap::util2d::computeRoi(imageDepthPtr->image, roiRatios_);
 
-			image_geometry::PinholeCameraModel model;
+			rtabmap::CameraModel model = cameraModelFromROS(*cameraInfo);
-			model.fromCameraInfo(*cameraInfo);
 
 			pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
-			rtabmap::CameraModel m(
+
-					model.fx(),
+			cv::Mat depth = imageDepthPtr->image;
-					model.fy(),
+			if( roiRatios_.size() == 4 &&
-					model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols),
+				((roiRatios_[0] > 0.0f && roiRatios_[0] <= 1.0f) ||
-					model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows));
+				 (roiRatios_[1] > 0.0f && roiRatios_[1] <= 1.0f) ||
+				 (roiRatios_[2] > 0.0f && roiRatios_[2] <= 1.0f) ||
+				 (roiRatios_[3] > 0.0f && roiRatios_[3] <= 1.0f)))
+			{
+				cv::Rect roiDepth = rtabmap::util2d::computeRoi(depth, roiRatios_);
+				cv::Rect roiRgb;
+				if(model.imageWidth() && model.imageHeight())
+				{
+					roiRgb = rtabmap::util2d::computeRoi(model.imageSize(), roiRatios_);
+				}
+				if(	roiDepth.width%decimation_==0 &&
+					roiDepth.height%decimation_==0 &&
+					(roiRgb.width != 0 ||
+					   (roiRgb.width%decimation_==0 &&
+						roiRgb.height%decimation_==0)))
+				{
+					depth = cv::Mat(depth, roiDepth);
+					if(model.imageWidth() != 0 && model.imageHeight() != 0)
+					{
+						model = model.roi(roiRgb);
+					}
+					else
+					{
+						model = model.roi(roiDepth);
+					}
+				}
+				else
+				{
+					NODELET_ERROR("Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
+						  "dimension (depth=%dx%d rgb=%dx%d) cannot be divided exactly "
+						  "by decimation parameter (%d). Ignoring ROI ratios...",
+						  roiRatios_[0],
+						  roiRatios_[1],
+						  roiRatios_[2],
+						  roiRatios_[3],
+						  roiDepth.width,
+						  roiDepth.height,
+						  roiRgb.width,
+						  roiRgb.height,
+						  decimation_);
+				}
+			}
 
 			pcl::IndicesPtr indices(new std::vector<int>);
 			pclCloud = rtabmap::util3d::cloudFromDepth(
-					cv::Mat(imageDepthPtr->image, roi),
+					depth,
-					m,
+					model,
 					decimation_,
 					maxDepth_,
 					minDepth_,
 					indices.get());
-			processAndPublish(pclCloud, indices, depth->header);
+			processAndPublish(pclCloud, indices, depthMsg->header);
 
 			NODELET_DEBUG("point_cloud_xyz from depth time = %f s", (ros::WallTime::now() - time).toSec());
 		}
@@ -276,6 +316,7 @@ private:
 
 			pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
 			rtabmap::CameraModel leftModel = rtabmap_ros::cameraModelFromROS(*cameraInfo);
+			UASSERT(disparity.cols == leftModel.imageWidth() && disparity.rows == leftModel.imageHeight());
 			rtabmap::StereoCameraModel stereoModel(disparityMsg->f, disparityMsg->f, leftModel.cx()-roiRatios_[0]*double(disparity.cols), leftModel.cy()-roiRatios_[2]*double(disparity.rows), disparityMsg->T);
 			pcl::IndicesPtr indices(new std::vector<int>);
 			pclCloud = rtabmap::util3d::cloudFromDisparity(

src/nodelets/point_cloud_xyzrgb.cpp

@@ -296,25 +296,51 @@ private:
 
 			cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth);
 
-			image_geometry::PinholeCameraModel model;
+			rtabmap::CameraModel model = cameraModelFromROS(*cameraInfo);
-			model.fromCameraInfo(*cameraInfo);
-
-			ROS_ASSERT(imageDepthPtr->image.cols == imagePtr->image.cols);
-			ROS_ASSERT(imageDepthPtr->image.rows == imagePtr->image.rows);
 
 			pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
-			cv::Rect roi = rtabmap::util2d::computeRoi(imageDepthPtr->image, roiRatios_);
 
-			rtabmap::CameraModel m(
+			cv::Mat rgb = imagePtr->image;
-					model.fx(),
+			cv::Mat depth = imageDepthPtr->image;
-					model.fy(),
+			if( roiRatios_.size() == 4 &&
-					model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols),
+				((roiRatios_[0] > 0.0f && roiRatios_[0] <= 1.0f) ||
-					model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows));
+				 (roiRatios_[1] > 0.0f && roiRatios_[1] <= 1.0f) ||
+				 (roiRatios_[2] > 0.0f && roiRatios_[2] <= 1.0f) ||
+				 (roiRatios_[3] > 0.0f && roiRatios_[3] <= 1.0f)))
+			{
+				cv::Rect roiDepth = rtabmap::util2d::computeRoi(depth, roiRatios_);
+				cv::Rect roiRgb = rtabmap::util2d::computeRoi(rgb, roiRatios_);
+				if(	roiDepth.width%decimation_==0 &&
+					roiDepth.height%decimation_==0 &&
+					roiRgb.width%decimation_==0 &&
+					roiRgb.height%decimation_==0)
+				{
+					depth = cv::Mat(depth, roiDepth);
+					rgb = cv::Mat(rgb, roiRgb);
+					model = model.roi(roiRgb);
+				}
+				else
+				{
+					NODELET_ERROR("Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
+						  "dimension (depth=%dx%d rgb=%dx%d) cannot be divided exactly "
+						  "by decimation parameter (%d). Ignoring ROI ratios...",
+						  roiRatios_[0],
+						  roiRatios_[1],
+						  roiRatios_[2],
+						  roiRatios_[3],
+						  roiDepth.width,
+						  roiDepth.height,
+						  roiRgb.width,
+						  roiRgb.height,
+						  decimation_);
+				}
+			}
+
 			pcl::IndicesPtr indices(new std::vector<int>);
 			pclCloud = rtabmap::util3d::cloudFromDepthRGB(
-					cv::Mat(imagePtr->image, roi),
+					rgb,
-					cv::Mat(imageDepthPtr->image, roi),
+					depth,
-					m,
+					model,
 					decimation_,
 					maxDepth_,
 					minDepth_,
@@ -372,6 +398,8 @@ private:
 
 			pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
 			rtabmap::CameraModel leftModel = rtabmap_ros::cameraModelFromROS(*cameraInfo);
+			UASSERT(disparity.cols == leftModel.imageWidth() && disparity.rows == leftModel.imageHeight());
+			UASSERT(imagePtr->image.cols == leftModel.imageWidth() && imagePtr->image.rows == leftModel.imageHeight());
 			rtabmap::StereoCameraModel stereoModel(imageDisparity->f, imageDisparity->f, leftModel.cx()-roiRatios_[0]*double(disparity.cols), leftModel.cy()-roiRatios_[2]*double(disparity.rows), imageDisparity->T);
 			pcl::IndicesPtr indices(new std::vector<int>);
 			pclCloud = rtabmap::util3d::cloudFromDisparityRGB(
@@ -467,7 +495,8 @@ private:
 						maxDepth_,
 						minDepth_,
 						indices.get(),
-						stereoBMParameters_);
+						stereoBMParameters_,
+						roiRatios_);
 
 				processAndPublish(pclCloud, indices, image->header);
 			}