Added "decimation" parameter to data_throttle and stereo_throttle nodelets. Added cloud processing parameters from point_cloud_xyz to point_cloud_xyzrgb nodelet. Added stereo inputs to point_cloud_xyzrgb.

src/nodelets/data_throttle.cpp

@@ -39,6 +39,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <sensor_msgs/CameraInfo.h>
 
+#include <cv_bridge/cv_bridge.h>
+
+#include <rtabmap/core/util3d.h>
+
 namespace rtabmap_ros
 {
 
@@ -47,9 +51,10 @@ class DataThrottleNodelet : public nodelet::Nodelet
 public:
 	//Constructor
 	DataThrottleNodelet():
-		max_update_rate_(0),
+		rate_(0),
 		approxSync_(0),
-		exactSync_(0)
+		exactSync_(0),
+		decimation_(1)
 	{
 	}
 
@@ -67,7 +72,7 @@ public:
 
 private:
 	ros::Time last_update_;
-	double max_update_rate_;
+	double rate_;
 	virtual void onInit()
 	{
 		ros::NodeHandle& nh = getNodeHandle();
@@ -84,9 +89,17 @@ private:
 
 		int queueSize = 10;
 		bool approxSync = true;
-		private_nh.param("max_rate", max_update_rate_, max_update_rate_);
+		if(private_nh.getParam("max_rate", rate_))
+		{
+			ROS_WARN("\"max_rate\" is now known as \"rate\".");
+		}
+		private_nh.param("rate", rate_, rate_);
 		private_nh.param("queue_size", queueSize, queueSize);
 		private_nh.param("approx_sync", approxSync, approxSync);
+		private_nh.param("decimation", decimation_, decimation_);
+		ROS_ASSERT(decimation_ >= 1);
+		ROS_INFO("Rate=%f Hz", rate_);
+		ROS_INFO("Decimation=%d", decimation_);
 		ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
 
 		if(approxSync)
@@ -104,40 +117,84 @@ private:
 		image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image_in"), 1, hintsDepth);
 		info_sub_.subscribe(rgb_nh, "camera_info_in", 1);
 
-		imagePub_ = rgb_it.advertise("image_out", 10);
-		imageDepthPub_ = depth_it.advertise("image_out", 10);
-		infoPub_ = rgb_nh.advertise<sensor_msgs::CameraInfo>("camera_info_out", 10);
+		imagePub_ = rgb_it.advertise("image_out", 1);
+		imageDepthPub_ = depth_it.advertise("image_out", 1);
+		infoPub_ = rgb_nh.advertise<sensor_msgs::CameraInfo>("camera_info_out", 1);
 	};
 
 	void callback(const sensor_msgs::ImageConstPtr& image,
 			const sensor_msgs::ImageConstPtr& imageDepth,
 			const sensor_msgs::CameraInfoConstPtr& camInfo)
 	{
-		if (max_update_rate_ > 0.0)
+		if (rate_ > 0.0)
 		{
-			NODELET_DEBUG("update set to %f", max_update_rate_);
-			if ( last_update_ + ros::Duration(1.0/max_update_rate_) > ros::Time::now())
+			NODELET_DEBUG("update set to %f", rate_);
+			if ( last_update_ + ros::Duration(1.0/rate_) > ros::Time::now())
 			{
 				NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec());
 				return;
 			}
 		}
 		else
-			NODELET_DEBUG("update_rate unset continuing");
+			NODELET_DEBUG("rate unset continuing");
 
 		last_update_ = ros::Time::now();
 
 		if(imagePub_.getNumSubscribers())
 		{
-			imagePub_.publish(image);
+			if(decimation_ > 1)
+			{
+				cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
+				cv_bridge::CvImage out;
+				out.header = imagePtr->header;
+				out.encoding = imagePtr->encoding;
+				out.image = rtabmap::util3d::decimate(imagePtr->image, decimation_);
+				imagePub_.publish(out.toImageMsg());
+			}
+			else
+			{
+				imagePub_.publish(image);
+			}
 		}
 		if(imageDepthPub_.getNumSubscribers())
 		{
-			imageDepthPub_.publish(imageDepth);
+			if(decimation_ > 1)
+			{
+				cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageDepth);
+				cv_bridge::CvImage out;
+				out.header = imagePtr->header;
+				out.encoding = imagePtr->encoding;
+				out.image = rtabmap::util3d::decimate(imagePtr->image, decimation_);
+				imageDepthPub_.publish(out.toImageMsg());
+			}
+			else
+			{
+				imageDepthPub_.publish(imageDepth);
+			}
 		}
 		if(infoPub_.getNumSubscribers())
 		{
-			infoPub_.publish(camInfo);
+			if(decimation_ > 1)
+			{
+				sensor_msgs::CameraInfo info = *camInfo;
+				info.height /= decimation_;
+				info.width /= decimation_;
+				info.roi.height /= decimation_;
+				info.roi.width /= decimation_;
+				info.K[2]/=float(decimation_); // cx
+				info.K[5]/=float(decimation_); // cy
+				info.K[0]/=float(decimation_); // fx
+				info.K[4]/=float(decimation_); // fy
+				info.P[2]/=float(decimation_); // cx
+				info.P[6]/=float(decimation_); // cy
+				info.P[0]/=float(decimation_); // fx
+				info.P[5]/=float(decimation_); // fy
+				infoPub_.publish(info);
+			}
+			else
+			{
+				infoPub_.publish(camInfo);
+			}
 		}
 	}
 
@@ -154,6 +211,8 @@ private:
 	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncPolicy;
 	message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
 
+	int decimation_;
+
 };
 
 

src/nodelets/point_cloud_xyz.cpp

@@ -105,7 +105,7 @@ private:
 			approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
 			approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, _1, _2));
 
-			approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityDispPolicy>(MyApproxSyncDisparityDispPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
+			approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
 			approxSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, _1, _2));
 		}
 		else
@@ -113,7 +113,7 @@ private:
 			exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
 			exactSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, _1, _2));
 
-			exactSyncDisparity_ = new message_filters::Synchronizer<MyExactSyncDisparityDispPolicy>(MyExactSyncDisparityDispPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
+			exactSyncDisparity_ = new message_filters::Synchronizer<MyExactSyncDisparityPolicy>(MyExactSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
 			exactSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, _1, _2));
 		}
 
@@ -255,14 +255,14 @@ private:
 	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncDepthPolicy;
 	message_filters::Synchronizer<MyApproxSyncDepthPolicy> * approxSyncDepth_;
 
-	typedef message_filters::sync_policies::ApproximateTime<stereo_msgs::DisparityImage, sensor_msgs::CameraInfo> MyApproxSyncDisparityDispPolicy;
-	message_filters::Synchronizer<MyApproxSyncDisparityDispPolicy> * approxSyncDisparity_;
+	typedef message_filters::sync_policies::ApproximateTime<stereo_msgs::DisparityImage, sensor_msgs::CameraInfo> MyApproxSyncDisparityPolicy;
+	message_filters::Synchronizer<MyApproxSyncDisparityPolicy> * approxSyncDisparity_;
 
 	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncDepthPolicy;
 	message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_;
 
-	typedef message_filters::sync_policies::ExactTime<stereo_msgs::DisparityImage, sensor_msgs::CameraInfo> MyExactSyncDisparityDispPolicy;
-	message_filters::Synchronizer<MyExactSyncDisparityDispPolicy> * exactSyncDisparity_;
+	typedef message_filters::sync_policies::ExactTime<stereo_msgs::DisparityImage, sensor_msgs::CameraInfo> MyExactSyncDisparityPolicy;
+	message_filters::Synchronizer<MyExactSyncDisparityPolicy> * exactSyncDisparity_;
 
 };
 

src/nodelets/point_cloud_xyzrgb.cpp

@@ -42,8 +42,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <image_transport/subscriber_filter.h>
 
 #include <image_geometry/pinhole_camera_model.h>
+#include <image_geometry/stereo_camera_model.h>
 
 #include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/sync_policies/exact_time.h>
 #include <message_filters/subscriber.h>
 
 #include <cv_bridge/cv_bridge.h>
@@ -57,11 +59,28 @@ namespace rtabmap_ros
 class PointCloudXYZRGB : public nodelet::Nodelet
 {
 public:
-	PointCloudXYZRGB() : voxelSize_(0.0), decimation_(1) {}
+	PointCloudXYZRGB() :
+		maxDepth_(0.0),
+		voxelSize_(0.0),
+		decimation_(1),
+		noiseFilterRadius_(0.0),
+		noiseFilterMinNeighbors_(5),
+		approxSyncDepth_(0),
+		approxSyncStereo_(0),
+		exactSyncDepth_(0),
+		exactSyncStereo_(0)
+	{}
 
 	virtual ~PointCloudXYZRGB()
 	{
-		delete sync_;
+		if(approxSyncDepth_)
+			delete approxSyncDepth_;
+		if(approxSyncStereo_)
+			delete approxSyncStereo_;
+		if(exactSyncDepth_)
+			delete exactSyncDepth_;
+		if(exactSyncStereo_)
+			delete exactSyncStereo_;
 	}
 
 private:
@@ -71,14 +90,37 @@ private:
 		ros::NodeHandle & pnh = getPrivateNodeHandle();
 
 		int queueSize = 10;
+		bool approxSync = true;
+		pnh.param("approx_sync", approxSync, approxSync);
 		pnh.param("queue_size", queueSize, queueSize);
+		pnh.param("max_depth", maxDepth_, maxDepth_);
 		pnh.param("voxel_size", voxelSize_, voxelSize_);
 		pnh.param("decimation", decimation_, decimation_);
+		pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_);
+		pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_);
+
+		ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
 
-		sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
-		sync_->registerCallback(boost::bind(&PointCloudXYZRGB::callback, this, _1, _2, _3));
 		cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1);
 
+		if(approxSync)
+		{
+
+			approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
+			approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZRGB::depthCallback, this, _1, _2, _3));
+
+			approxSyncStereo_ = new message_filters::Synchronizer<MyApproxSyncStereoPolicy>(MyApproxSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
+			approxSyncStereo_->registerCallback(boost::bind(&PointCloudXYZRGB::stereoCallback, this, _1, _2, _3, _4));
+		}
+		else
+		{
+			exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
+			exactSyncDepth_->registerCallback(boost::bind(&PointCloudXYZRGB::depthCallback, this, _1, _2, _3));
+
+			exactSyncStereo_ = new message_filters::Synchronizer<MyExactSyncStereoPolicy>(MyExactSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
+			exactSyncStereo_->registerCallback(boost::bind(&PointCloudXYZRGB::stereoCallback, this, _1, _2, _3, _4));
+		}
+
 		ros::NodeHandle rgb_nh(nh, "rgb");
 		ros::NodeHandle depth_nh(nh, "depth");
 		ros::NodeHandle rgb_pnh(pnh, "rgb");
@@ -91,11 +133,24 @@ 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);
+
+
+		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);
+
+		imageLeft_.subscribe(left_it, left_nh.resolveName("image"), 1, hintsLeft);
+		imageRight_.subscribe(right_it, right_nh.resolveName("image"), 1, hintsRight);
+		cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
+		cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
 	}
 
-
-
-	void callback(
+	void depthCallback(
 			  const sensor_msgs::ImageConstPtr& image,
 			  const sensor_msgs::ImageConstPtr& imageDepth,
 			  const sensor_msgs::CameraInfoConstPtr& cameraInfo)
@@ -133,33 +188,124 @@ private:
 					fy,
 					decimation_);
 
-			if(voxelSize_ > 0.0)
-			{
-				pclCloud = rtabmap::util3d::voxelize<pcl::PointXYZRGB>(pclCloud, voxelSize_);
-			}
 
-			sensor_msgs::PointCloud2 rosCloud;
-			pcl::toROSMsg(*pclCloud, rosCloud);
-			rosCloud.header.stamp = image->header.stamp;
-			rosCloud.header.frame_id = image->header.frame_id;
-
-			//publish the message
-			cloudPub_.publish(rosCloud);
+			processAndPublish(pclCloud, imagePtr->header);
 		}
-}
+	}
+
+	void stereoCallback(const sensor_msgs::ImageConstPtr& imageLeft,
+			const sensor_msgs::ImageConstPtr& imageRight,
+			const sensor_msgs::CameraInfoConstPtr& camInfoLeft,
+			const sensor_msgs::CameraInfoConstPtr& camInfoRight)
+	{
+		if(!(imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+				imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
+				imageLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+				imageLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
+			!(imageRight->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+				imageRight->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
+				imageRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+				imageRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
+		{
+			ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (enc=%s)", imageLeft->encoding.c_str());
+			return;
+		}
+
+		if(cloudPub_.getNumSubscribers())
+		{
+			cv_bridge::CvImageConstPtr ptrLeftImage, ptrRightImage;
+			if(imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+				imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
+			{
+				ptrLeftImage = cv_bridge::toCvShare(imageLeft, "mono8");
+			}
+			else
+			{
+				ptrLeftImage = cv_bridge::toCvShare(imageLeft, "bgr8");
+			}
+			ptrRightImage = cv_bridge::toCvShare(imageRight, "mono8");
+
+			image_geometry::StereoCameraModel model;
+			model.fromCameraInfo(*camInfoLeft, *camInfoRight);
+
+			float fx = model.left().fx();
+			float cx = model.left().cx();
+			float cy = model.left().cy();
+			float baseline = model.baseline();
+
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
+			pclCloud = rtabmap::util3d::cloudFromStereoImages(
+					ptrLeftImage->image,
+					ptrRightImage->image,
+					cx,
+					cy,
+					fx,
+					baseline,
+					decimation_);
+
+			processAndPublish(pclCloud, imageLeft->header);
+		}
+	}
+
+	void processAndPublish(pcl::PointCloud<pcl::PointXYZRGB>::Ptr & pclCloud, const std_msgs::Header & header)
+	{
+		if(pclCloud->size() && maxDepth_ > 0)
+		{
+			pclCloud = rtabmap::util3d::passThrough<pcl::PointXYZRGB>(pclCloud, "z", 0, maxDepth_);
+		}
+
+		if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
+		{
+			pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering<pcl::PointXYZRGB>(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
+			pcl::copyPointCloud(*pclCloud, *indices, *tmp);
+			pclCloud = tmp;
+		}
+
+		if(pclCloud->size() && voxelSize_ > 0.0)
+		{
+			pclCloud = rtabmap::util3d::voxelize<pcl::PointXYZRGB>(pclCloud, voxelSize_);
+		}
+
+		sensor_msgs::PointCloud2 rosCloud;
+		pcl::toROSMsg(*pclCloud, rosCloud);
+		rosCloud.header.stamp = header.stamp;
+		rosCloud.header.frame_id = header.frame_id;
+
+		//publish the message
+		cloudPub_.publish(rosCloud);
+	}
 
 private:
 
+	double maxDepth_;
 	double voxelSize_;
 	int decimation_;
+	double noiseFilterRadius_;
+	int noiseFilterMinNeighbors_;
+
 	ros::Publisher cloudPub_;
 
 	image_transport::SubscriberFilter imageSub_;
 	image_transport::SubscriberFilter imageDepthSub_;
 	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
 
-	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy;
-	message_filters::Synchronizer<MySyncPolicy> * sync_;
+	image_transport::SubscriberFilter imageLeft_;
+	image_transport::SubscriberFilter imageRight_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
+
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncDepthPolicy;
+	message_filters::Synchronizer<MyApproxSyncDepthPolicy> * approxSyncDepth_;
+
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyApproxSyncStereoPolicy;
+	message_filters::Synchronizer<MyApproxSyncStereoPolicy> * approxSyncStereo_;
+
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncDepthPolicy;
+	message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_;
+
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncStereoPolicy;
+	message_filters::Synchronizer<MyExactSyncStereoPolicy> * exactSyncStereo_;
 };
 
 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudXYZRGB, nodelet::Nodelet);

src/nodelets/stereo_throttle.cpp

@@ -38,6 +38,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <sensor_msgs/CameraInfo.h>
 
+#include <cv_bridge/cv_bridge.h>
+
+#include <rtabmap/core/util3d.h>
+
 namespace rtabmap_ros
 {
 
@@ -46,9 +50,10 @@ class StereoThrottleNodelet : public nodelet::Nodelet
 public:
 	//Constructor
 	StereoThrottleNodelet():
-		max_update_rate_(0),
+		rate_(0),
 		approxSync_(0),
-		exactSync_(0)
+		exactSync_(0),
+		decimation_(1)
 	{
 	}
 
@@ -66,7 +71,7 @@ public:
 
 private:
 	ros::Time last_update_;
-	double max_update_rate_;
+	double rate_;
 	virtual void onInit()
 	{
 		ros::NodeHandle& nh = getNodeHandle();
@@ -84,15 +89,14 @@ private:
 		int queueSize = 5;
 		bool approxSync = false;
 		pnh.param("approx_sync", approxSync, approxSync);
-		pnh.param("rate", max_update_rate_, max_update_rate_);
+		pnh.param("rate", rate_, rate_);
 		pnh.param("queue_size", queueSize, queueSize);
+		pnh.param("decimation", decimation_, decimation_);
+		ROS_ASSERT(decimation_ >= 1);
+		ROS_INFO("Rate=%f Hz", rate_);
+		ROS_INFO("Decimation=%d", decimation_);
 		ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
 
-		if(max_update_rate_ == 0.0)
-		{
-			ROS_WARN("Parameter \"rate\" (Hz) is not set!");
-		}
-
 		if(approxSync)
 		{
 			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
@@ -120,35 +124,101 @@ private:
 			const sensor_msgs::CameraInfoConstPtr& camInfoLeft,
 			const sensor_msgs::CameraInfoConstPtr& camInfoRight)
 	{
-		if (max_update_rate_ > 0.0)
+		if (rate_ > 0.0)
 		{
-			NODELET_DEBUG("update set to %f", max_update_rate_);
-			if ( last_update_ + ros::Duration(1.0/max_update_rate_) > ros::Time::now())
+			NODELET_DEBUG("update set to %f", rate_);
+			if ( last_update_ + ros::Duration(1.0/rate_) > ros::Time::now())
 			{
 				NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec());
 				return;
 			}
 		}
 		else
-			NODELET_DEBUG("update_rate unset continuing");
+			NODELET_DEBUG("rate unset continuing");
 
 		last_update_ = ros::Time::now();
 
 		if(imageLeftPub_.getNumSubscribers())
 		{
-			imageLeftPub_.publish(imageLeft);
+			if(decimation_ > 1)
+			{
+				cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageLeft);
+				cv_bridge::CvImage out;
+				out.header = imagePtr->header;
+				out.encoding = imagePtr->encoding;
+				out.image = rtabmap::util3d::decimate(imagePtr->image, decimation_);
+				imageLeftPub_.publish(out.toImageMsg());
+			}
+			else
+			{
+				imageLeftPub_.publish(imageLeft);
+			}
 		}
 		if(imageRightPub_.getNumSubscribers())
 		{
-			imageRightPub_.publish(imageRight);
+			if(decimation_ > 1)
+			{
+				cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageRight);
+				cv_bridge::CvImage out;
+				out.header = imagePtr->header;
+				out.encoding = imagePtr->encoding;
+				out.image = rtabmap::util3d::decimate(imagePtr->image, decimation_);
+				imageRightPub_.publish(out.toImageMsg());
+			}
+			else
+			{
+				imageRightPub_.publish(imageRight);
+			}
 		}
 		if(infoLeftPub_.getNumSubscribers())
 		{
-			infoLeftPub_.publish(camInfoLeft);
+			if(decimation_ > 1)
+			{
+				sensor_msgs::CameraInfo info = *camInfoLeft;
+				info.height /= decimation_;
+				info.width /= decimation_;
+				info.roi.height /= decimation_;
+				info.roi.width /= decimation_;
+				info.K[2]/=float(decimation_); // cx
+				info.K[5]/=float(decimation_); // cy
+				info.K[0]/=float(decimation_); // fx
+				info.K[4]/=float(decimation_); // fy
+				info.P[2]/=float(decimation_); // cx
+				info.P[6]/=float(decimation_); // cy
+				info.P[0]/=float(decimation_); // fx
+				info.P[5]/=float(decimation_); // fy
+				info.P[3]/=float(decimation_); // Tx
+				infoLeftPub_.publish(info);
+			}
+			else
+			{
+				infoLeftPub_.publish(camInfoLeft);
+			}
 		}
 		if(infoRightPub_.getNumSubscribers())
 		{
-			infoRightPub_.publish(camInfoRight);
+			if(decimation_ > 1)
+			{
+				sensor_msgs::CameraInfo info = *camInfoRight;
+				info.height /= decimation_;
+				info.width /= decimation_;
+				info.roi.height /= decimation_;
+				info.roi.width /= decimation_;
+				info.K[2]/=float(decimation_); // cx
+				info.K[5]/=float(decimation_); // cy
+				info.K[0]/=float(decimation_); // fx
+				info.K[4]/=float(decimation_); // fy
+				info.P[2]/=float(decimation_); // cx
+				info.P[6]/=float(decimation_); // cy
+				info.P[0]/=float(decimation_); // fx
+				info.P[5]/=float(decimation_); // fy
+				info.P[3]/=float(decimation_); // Tx
+				infoRightPub_.publish(info);
+			}
+			else
+			{
+				infoRightPub_.publish(camInfoRight);
+			}
 		}
 	}
 
@@ -167,6 +237,8 @@ private:
 	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncPolicy;
 	message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
 
+	int decimation_;
+
 };