00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include <ros/ros.h>
00029 #include <pluginlib/class_list_macros.h>
00030 #include <nodelet/nodelet.h>
00031
00032 #include <pcl/point_cloud.h>
00033 #include <pcl/point_types.h>
00034 #include <pcl_conversions/pcl_conversions.h>
00035
00036 #include <sensor_msgs/PointCloud2.h>
00037 #include <sensor_msgs/Image.h>
00038 #include <sensor_msgs/image_encodings.h>
00039 #include <sensor_msgs/CameraInfo.h>
00040 #include <stereo_msgs/DisparityImage.h>
00041
00042 #include <image_transport/image_transport.h>
00043 #include <image_transport/subscriber_filter.h>
00044
00045 #include <image_geometry/pinhole_camera_model.h>
00046
00047 #include <message_filters/sync_policies/approximate_time.h>
00048 #include <message_filters/sync_policies/exact_time.h>
00049 #include <message_filters/subscriber.h>
00050
00051 #include <cv_bridge/cv_bridge.h>
00052 #include <opencv2/highgui/highgui.hpp>
00053
00054 #include "rtabmap/core/util3d.h"
00055
00056 namespace rtabmap_ros
00057 {
00058
00059 class PointCloudXYZ : public nodelet::Nodelet
00060 {
00061 public:
00062 PointCloudXYZ() :
00063 maxDepth_(0.0),
00064 voxelSize_(0.0),
00065 decimation_(1),
00066 noiseFilterRadius_(0.0),
00067 noiseFilterMinNeighbors_(5),
00068 approxSyncDepth_(0),
00069 approxSyncDisparity_(0),
00070 exactSyncDepth_(0),
00071 exactSyncDisparity_(0)
00072 {}
00073
00074 virtual ~PointCloudXYZ()
00075 {
00076 if(approxSyncDepth_)
00077 delete approxSyncDepth_;
00078 if(approxSyncDisparity_)
00079 delete approxSyncDisparity_;
00080 if(exactSyncDepth_)
00081 delete exactSyncDepth_;
00082 if(exactSyncDisparity_)
00083 delete exactSyncDisparity_;
00084 }
00085
00086 private:
00087 virtual void onInit()
00088 {
00089 ros::NodeHandle & nh = getNodeHandle();
00090 ros::NodeHandle & pnh = getPrivateNodeHandle();
00091
00092 int queueSize = 10;
00093 bool approxSync = true;
00094 pnh.param("approx_sync", approxSync, approxSync);
00095 pnh.param("queue_size", queueSize, queueSize);
00096 pnh.param("max_depth", maxDepth_, maxDepth_);
00097 pnh.param("voxel_size", voxelSize_, voxelSize_);
00098 pnh.param("decimation", decimation_, decimation_);
00099 pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_);
00100 pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_);
00101 ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
00102
00103 if(approxSync)
00104 {
00105 approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
00106 approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, _1, _2));
00107
00108 approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
00109 approxSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, _1, _2));
00110 }
00111 else
00112 {
00113 exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
00114 exactSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, _1, _2));
00115
00116 exactSyncDisparity_ = new message_filters::Synchronizer<MyExactSyncDisparityPolicy>(MyExactSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
00117 exactSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, _1, _2));
00118 }
00119
00120 ros::NodeHandle depth_nh(nh, "depth");
00121 ros::NodeHandle depth_pnh(pnh, "depth");
00122 image_transport::ImageTransport depth_it(depth_nh);
00123 image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
00124
00125 imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
00126 cameraInfoSub_.subscribe(depth_nh, "camera_info", 1);
00127
00128 disparitySub_.subscribe(nh, "disparity/image", 1);
00129 disparityCameraInfoSub_.subscribe(nh, "disparity/camera_info", 1);
00130
00131 cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1);
00132 }
00133
00134 void callback(
00135 const sensor_msgs::ImageConstPtr& depth,
00136 const sensor_msgs::CameraInfoConstPtr& cameraInfo)
00137 {
00138 if(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 &&
00139 depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0 &&
00140 depth->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0)
00141 {
00142 ROS_ERROR("Input type depth=32FC1,16UC1,MONO16");
00143 return;
00144 }
00145
00146 if(cloudPub_.getNumSubscribers())
00147 {
00148 cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth);
00149
00150 image_geometry::PinholeCameraModel model;
00151 model.fromCameraInfo(*cameraInfo);
00152 float fx = model.fx();
00153 float fy = model.fy();
00154 float cx = model.cx();
00155 float cy = model.cy();
00156
00157 pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
00158 pclCloud = rtabmap::util3d::cloudFromDepth(
00159 imageDepthPtr->image,
00160 cx,
00161 cy,
00162 fx,
00163 fy,
00164 decimation_);
00165
00166 processAndPublish(pclCloud, depth->header);
00167 }
00168 }
00169
00170 void callbackDisparity(
00171 const stereo_msgs::DisparityImageConstPtr& disparityMsg,
00172 const sensor_msgs::CameraInfoConstPtr& cameraInfo)
00173 {
00174 if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0 &&
00175 disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_16SC1) !=0)
00176 {
00177 ROS_ERROR("Input type must be disparity=32FC1 or 16SC1");
00178 return;
00179 }
00180
00181 cv::Mat disparity;
00182 if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0)
00183 {
00184 disparity = cv::Mat(disparityMsg->image.height, disparityMsg->image.width, CV_32FC1, const_cast<uchar*>(disparityMsg->image.data.data()));
00185 }
00186 else
00187 {
00188 disparity = cv::Mat(disparityMsg->image.height, disparityMsg->image.width, CV_16SC1, const_cast<uchar*>(disparityMsg->image.data.data()));
00189 }
00190
00191 if(cloudPub_.getNumSubscribers())
00192 {
00193 image_geometry::PinholeCameraModel model;
00194 model.fromCameraInfo(*cameraInfo);
00195 float cx = model.cx();
00196 float cy = model.cy();
00197
00198 pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
00199 pclCloud = rtabmap::util3d::cloudFromDisparity(
00200 disparity,
00201 cx,
00202 cy,
00203 disparityMsg->f,
00204 disparityMsg->T,
00205 decimation_);
00206
00207 processAndPublish(pclCloud, disparityMsg->header);
00208 }
00209 }
00210
00211 void processAndPublish(pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud, const std_msgs::Header & header)
00212 {
00213 if(pclCloud->size() && maxDepth_ > 0)
00214 {
00215 pclCloud = rtabmap::util3d::passThrough<pcl::PointXYZ>(pclCloud, "z", 0, maxDepth_);
00216 }
00217
00218 if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
00219 {
00220 pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering<pcl::PointXYZ>(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
00221 pcl::PointCloud<pcl::PointXYZ>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZ>);
00222 pcl::copyPointCloud(*pclCloud, *indices, *tmp);
00223 pclCloud = tmp;
00224 }
00225
00226 if(pclCloud->size() && voxelSize_ > 0.0)
00227 {
00228 pclCloud = rtabmap::util3d::voxelize<pcl::PointXYZ>(pclCloud, voxelSize_);
00229 }
00230
00231 sensor_msgs::PointCloud2 rosCloud;
00232 pcl::toROSMsg(*pclCloud, rosCloud);
00233 rosCloud.header.stamp = header.stamp;
00234 rosCloud.header.frame_id = header.frame_id;
00235
00236
00237 cloudPub_.publish(rosCloud);
00238 }
00239
00240 private:
00241
00242 double maxDepth_;
00243 double voxelSize_;
00244 int decimation_;
00245 double noiseFilterRadius_;
00246 int noiseFilterMinNeighbors_;
00247
00248 ros::Publisher cloudPub_;
00249
00250 image_transport::SubscriberFilter imageDepthSub_;
00251 message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
00252
00253 message_filters::Subscriber<stereo_msgs::DisparityImage> disparitySub_;
00254 message_filters::Subscriber<sensor_msgs::CameraInfo> disparityCameraInfoSub_;
00255
00256 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncDepthPolicy;
00257 message_filters::Synchronizer<MyApproxSyncDepthPolicy> * approxSyncDepth_;
00258
00259 typedef message_filters::sync_policies::ApproximateTime<stereo_msgs::DisparityImage, sensor_msgs::CameraInfo> MyApproxSyncDisparityPolicy;
00260 message_filters::Synchronizer<MyApproxSyncDisparityPolicy> * approxSyncDisparity_;
00261
00262 typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncDepthPolicy;
00263 message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_;
00264
00265 typedef message_filters::sync_policies::ExactTime<stereo_msgs::DisparityImage, sensor_msgs::CameraInfo> MyExactSyncDisparityPolicy;
00266 message_filters::Synchronizer<MyExactSyncDisparityPolicy> * exactSyncDisparity_;
00267
00268 };
00269
00270 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudXYZ, nodelet::Nodelet);
00271 }
00272