From 9a2d96b99e23352dbca56e5e35432377474b2438 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@f169173b-cf89-36c8-b27e-44dbe73f0c83>
Date: Wed, 11 Jun 2014 15:42:41 +0000
Subject: [PATCH] ros-pkg: Added decimation parameter to point_cloud_xyzrgb
 nodelet

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1344 f169173b-cf89-36c8-b27e-44dbe73f0c83
---
 launch/tests/test_pointxyzrgb.launch | 20 ++++++++++++
 src/nodelets/point_cloud_xyzrgb.cpp  | 47 +++++++++++++++-------------
 2 files changed, 45 insertions(+), 22 deletions(-)
 create mode 100644 launch/tests/test_pointxyzrgb.launch

diff --git a/launch/tests/test_pointxyzrgb.launch b/launch/tests/test_pointxyzrgb.launch
new file mode 100644
index 00000000..c7eb52ea
--- /dev/null
+++ b/launch/tests/test_pointxyzrgb.launch
@@ -0,0 +1,20 @@
+
+<launch>
+   
+  
+  <!-- sync cloud with odometry and voxelize the point cloud (for fast visualization in rviz) -->
+  <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap/point_cloud_xyzrgb">
+    <remap from="rgb/image"       to="camera/rgb/image_rect_color"/>
+    <remap from="depth/image"     to="camera/depth_registered/image_raw"/>
+    <remap from="rgb/camera_info" to="camera/depth_registered/camera_info"/>
+    <remap from="cloud"           to="/voxel_cloud" />
+    
+    <param name="queue_size" type="int" value="30"/>
+    <param name="voxel_size" type="double" value="0.02"/>
+    <param name="decimation" type="int" value="2"/>
+
+    <param name="rgb/image_transport" type="string" value="compressed"/>
+    <param name="depth/image_transport" type="string" value="compressedDepth"/>
+  </node>
+
+</launch>
diff --git a/src/nodelets/point_cloud_xyzrgb.cpp b/src/nodelets/point_cloud_xyzrgb.cpp
index f8991533..b179a9a1 100644
--- a/src/nodelets/point_cloud_xyzrgb.cpp
+++ b/src/nodelets/point_cloud_xyzrgb.cpp
@@ -35,7 +35,7 @@ namespace rtabmap
 class PointCloudXYZRGB : public nodelet::Nodelet
 {
 public:
-	PointCloudXYZRGB() : voxelSize_(0.0) {}
+	PointCloudXYZRGB() : voxelSize_(0.0), decimation_(1) {}
 
 	virtual ~PointCloudXYZRGB()
 	{
@@ -51,6 +51,7 @@ private:
 		int queueSize = 10;
 		pnh.param("queue_size", queueSize, queueSize);
 		pnh.param("voxel_size", voxelSize_, voxelSize_);
+		pnh.param("decimation", decimation_, decimation_);
 
 		sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
 		sync_->registerCallback(boost::bind(&PointCloudXYZRGB::callback, this, _1, _2, _3));
@@ -87,29 +88,30 @@ private:
 			return;
 		}
 
-
-		cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
-		cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth);
-
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
-		pclCloud = rtabmap::util3d::cloudFromDepthRGB(
-				imagePtr->image,
-				imageDepthPtr->image,
-				cameraInfo->K[2],
-				cameraInfo->K[5],
-				cameraInfo->K[0],
-				cameraInfo->K[4]);
-
-		if(voxelSize_ > 0.0)
-		{
-			pclCloud = rtabmap::util3d::voxelize(pclCloud, voxelSize_);
-		}
-
-		//*********************
-		// Publish Map
-		//*********************
 		if(cloudPub_.getNumSubscribers())
 		{
+			cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
+			cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth);
+
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
+			pclCloud = rtabmap::util3d::cloudFromDepthRGB(
+					imagePtr->image,
+					imageDepthPtr->image,
+					cameraInfo->K[2],
+					cameraInfo->K[5],
+					cameraInfo->K[0],
+					cameraInfo->K[4],
+					decimation_);
+
+			if(voxelSize_ > 0.0)
+			{
+				pclCloud = rtabmap::util3d::voxelize(pclCloud, voxelSize_);
+			}
+
+			//*********************
+			// Publish Map
+			//*********************
+
 			sensor_msgs::PointCloud2 rosCloud;
 			pcl::toROSMsg(*pclCloud, rosCloud);
 			rosCloud.header.stamp = image->header.stamp;
@@ -123,6 +125,7 @@ private:
 private:
 
 	double voxelSize_;
+	int decimation_;
 	ros::Publisher cloudPub_;
 
 	image_transport::SubscriberFilter imageSub_;