Added roi_ratios to point_cloud_xyzrgb nodelet

src/nodelets/obstacles_detection.cpp

@@ -305,7 +305,7 @@ private:
 			projObstaclesPub_.publish(rosCloud);
 		}
 
-		//NODELET_INFO("Obstacles segmentation time = %f s", (ros::WallTime::now() - time).toSec());
+		NODELET_DEBUG("Obstacles segmentation time = %f s", (ros::WallTime::now() - time).toSec());
 	}
 
 private:

src/nodelets/point_cloud_xyz.cpp

@@ -127,31 +127,34 @@ private:
 
 		//parse roi (region of interest)
 		roiRatios_.resize(4, 0);
-		std::list<std::string> strValues = uSplit(roiStr, ' ');
+		if(!roiStr.empty())
-		if(strValues.size() != 4)
 		{
-			ROS_ERROR("The number of values must be 4 (\"roi_ratios\"=\"%s\")", roiStr.c_str());
+			std::list<std::string> strValues = uSplit(roiStr, ' ');
-		}
+			if(strValues.size() != 4)
-		else
-		{
-			std::vector<float> tmpValues(4);
-			unsigned int i=0;
-			for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
 			{
-				tmpValues[i] = uStr2Float(*jter);
+				ROS_ERROR("The number of values must be 4 (\"roi_ratios\"=\"%s\")", roiStr.c_str());
-				++i;
-			}
-
-			if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
-				tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
-				tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
-				tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
-			{
-				roiRatios_ = tmpValues;
 			}
 			else
 			{
-				ROS_ERROR("The roi ratios are not valid (\"roi_ratios\"=\"%s\")", roiStr.c_str());
+				std::vector<float> tmpValues(4);
+				unsigned int i=0;
+				for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
+				{
+					tmpValues[i] = uStr2Float(*jter);
+					++i;
+				}
+
+				if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
+					tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
+					tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
+					tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
+				{
+					roiRatios_ = tmpValues;
+				}
+				else
+				{
+					ROS_ERROR("The roi ratios are not valid (\"roi_ratios\"=\"%s\")", roiStr.c_str());
+				}
 			}
 		}
 
@@ -202,22 +205,25 @@ private:
 
 		if(cloudPub_.getNumSubscribers())
 		{
+			ros::WallTime time = ros::WallTime::now();
+
 			cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth);
 			cv::Rect roi = rtabmap::Feature2D::computeRoi(imageDepthPtr->image, roiRatios_);
-			cv::Mat image(imageDepthPtr->image, roi);
 
 			image_geometry::PinholeCameraModel model;
 			model.fromCameraInfo(*cameraInfo);
 
 			pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
 			pclCloud = rtabmap::util3d::cloudFromDepth(
-					image,
+					cv::Mat(imageDepthPtr->image, roi),
 					model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols),
 					model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows),
 					model.fx(),
 					model.fy(),
 					decimation_);
 			processAndPublish(pclCloud, depth->header);
+
+			NODELET_DEBUG("point_cloud_xyz from depth time = %f s", (ros::WallTime::now() - time).toSec());
 		}
 	}
 
@@ -244,6 +250,8 @@ private:
 
 		if(cloudPub_.getNumSubscribers())
 		{
+			ros::WallTime time = ros::WallTime::now();
+
 			cv::Rect roi = rtabmap::Feature2D::computeRoi(disparity, roiRatios_);
 
 			pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
@@ -255,6 +263,8 @@ private:
 					decimation_);
 
 			processAndPublish(pclCloud, disparityMsg->header);
+
+			NODELET_DEBUG("point_cloud_xyz from disparity time = %f s", (ros::WallTime::now() - time).toSec());
 		}
 	}
 

src/nodelets/point_cloud_xyzrgb.cpp

@@ -55,6 +55,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/util3d_filtering.h"
+#include "rtabmap/core/Features2d.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/UStl.h"
 
 namespace rtabmap_ros
 {
@@ -95,6 +98,7 @@ private:
 
 		int queueSize = 10;
 		bool approxSync = true;
+		std::string roiStr;
 		pnh.param("approx_sync", approxSync, approxSync);
 		pnh.param("queue_size", queueSize, queueSize);
 		pnh.param("max_depth", maxDepth_, maxDepth_);
@@ -103,6 +107,40 @@ private:
 		pnh.param("decimation", decimation_, decimation_);
 		pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_);
 		pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_);
+		pnh.param("roi_ratios", roiStr, roiStr);
+
+		//parse roi (region of interest)
+		roiRatios_.resize(4, 0);
+		if(!roiStr.empty())
+		{
+			std::list<std::string> strValues = uSplit(roiStr, ' ');
+			if(strValues.size() != 4)
+			{
+				ROS_ERROR("The number of values must be 4 (\"roi_ratios\"=\"%s\")", roiStr.c_str());
+			}
+			else
+			{
+				std::vector<float> tmpValues(4);
+				unsigned int i=0;
+				for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
+				{
+					tmpValues[i] = uStr2Float(*jter);
+					++i;
+				}
+
+				if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
+					tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
+					tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
+					tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
+				{
+					roiRatios_ = tmpValues;
+				}
+				else
+				{
+					ROS_ERROR("The roi ratios are not valid (\"roi_ratios\"=\"%s\")", roiStr.c_str());
+				}
+			}
+		}
 
 		NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
 
@@ -175,6 +213,8 @@ private:
 
 		if(cloudPub_.getNumSubscribers())
 		{
+			ros::WallTime time = ros::WallTime::now();
+
 			cv_bridge::CvImageConstPtr imagePtr;
 			if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
 			{
@@ -194,23 +234,25 @@ private:
 
 			image_geometry::PinholeCameraModel model;
 			model.fromCameraInfo(*cameraInfo);
-			float fx = model.fx();
+
-			float fy = model.fy();
+			ROS_ASSERT(imageDepthPtr->image.cols == imagePtr->image.cols);
-			float cx = model.cx();
+			ROS_ASSERT(imageDepthPtr->image.rows == imagePtr->image.rows);
-			float cy = model.cy();
 
 			pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
+			cv::Rect roi = rtabmap::Feature2D::computeRoi(imageDepthPtr->image, roiRatios_);
 			pclCloud = rtabmap::util3d::cloudFromDepthRGB(
-					imagePtr->image,
+					cv::Mat(imagePtr->image, roi),
-					imageDepthPtr->image,
+					cv::Mat(imageDepthPtr->image, roi),
-					cx,
+					model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols),
-					cy,
+					model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows),
-					fx,
+					model.fx(),
-					fy,
+					model.fy(),
 					decimation_);
 
 
 			processAndPublish(pclCloud, imagePtr->header);
+
+			NODELET_DEBUG("point_cloud_xyzrgb from RGB-D time = %f s", (ros::WallTime::now() - time).toSec());
 		}
 	}
 
@@ -234,6 +276,8 @@ private:
 
 		if(cloudPub_.getNumSubscribers())
 		{
+			ros::WallTime time = ros::WallTime::now();
+
 			cv_bridge::CvImageConstPtr ptrLeftImage, ptrRightImage;
 			if(imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
 				imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
@@ -246,6 +290,11 @@ private:
 			}
 			ptrRightImage = cv_bridge::toCvShare(imageRight, "mono8");
 
+			if(roiRatios_[0]!=0.0f || roiRatios_[1]!=0.0f || roiRatios_[2]!=0.0f || roiRatios_[3]!=0.0f)
+			{
+				ROS_WARN("\"roi_ratios\" set but ignored for stereo images.");
+			}
+
 			pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
 			pclCloud = rtabmap::util3d::cloudFromStereoImages(
 					ptrLeftImage->image,
@@ -254,6 +303,8 @@ private:
 					decimation_);
 
 			processAndPublish(pclCloud, imageLeft->header);
+
+			NODELET_DEBUG("point_cloud_xyzrgb from stereo time = %f s", (ros::WallTime::now() - time).toSec());
 		}
 	}
 
@@ -302,6 +353,7 @@ private:
 	int decimation_;
 	double noiseFilterRadius_;
 	int noiseFilterMinNeighbors_;
+	std::vector<float> roiRatios_;
 
 	ros::Publisher cloudPub_;