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 <rtabmap_ros/MsgConversion.h>
00033
00034 #include <pcl/point_cloud.h>
00035 #include <pcl/point_types.h>
00036 #include <pcl_conversions/pcl_conversions.h>
00037
00038 #include <sensor_msgs/PointCloud2.h>
00039 #include <sensor_msgs/Image.h>
00040 #include <sensor_msgs/image_encodings.h>
00041 #include <sensor_msgs/CameraInfo.h>
00042 #include <stereo_msgs/DisparityImage.h>
00043
00044 #include <image_transport/image_transport.h>
00045 #include <image_transport/subscriber_filter.h>
00046
00047 #include <image_geometry/pinhole_camera_model.h>
00048
00049 #include <message_filters/sync_policies/approximate_time.h>
00050 #include <message_filters/sync_policies/exact_time.h>
00051 #include <message_filters/subscriber.h>
00052
00053 #include <cv_bridge/cv_bridge.h>
00054 #include <opencv2/highgui/highgui.hpp>
00055
00056 #include "rtabmap/core/util2d.h"
00057 #include "rtabmap/core/util3d.h"
00058 #include "rtabmap/core/util3d_filtering.h"
00059 #include "rtabmap/core/util3d_surface.h"
00060 #include "rtabmap/utilite/UConversion.h"
00061 #include "rtabmap/utilite/UStl.h"
00062
00063 namespace rtabmap_ros
00064 {
00065
00066 class PointCloudXYZ : public nodelet::Nodelet
00067 {
00068 public:
00069 PointCloudXYZ() :
00070 maxDepth_(0.0),
00071 minDepth_(0.0),
00072 voxelSize_(0.0),
00073 decimation_(1),
00074 noiseFilterRadius_(0.0),
00075 noiseFilterMinNeighbors_(5),
00076 normalK_(0),
00077 normalRadius_(0.0),
00078 filterNaNs_(false),
00079 approxSyncDepth_(0),
00080 approxSyncDisparity_(0),
00081 exactSyncDepth_(0),
00082 exactSyncDisparity_(0)
00083 {}
00084
00085 virtual ~PointCloudXYZ()
00086 {
00087 if(approxSyncDepth_)
00088 delete approxSyncDepth_;
00089 if(approxSyncDisparity_)
00090 delete approxSyncDisparity_;
00091 if(exactSyncDepth_)
00092 delete exactSyncDepth_;
00093 if(exactSyncDisparity_)
00094 delete exactSyncDisparity_;
00095 }
00096
00097 private:
00098 virtual void onInit()
00099 {
00100 ros::NodeHandle & nh = getNodeHandle();
00101 ros::NodeHandle & pnh = getPrivateNodeHandle();
00102
00103 int queueSize = 10;
00104 bool approxSync = true;
00105 std::string roiStr;
00106 pnh.param("approx_sync", approxSync, approxSync);
00107 pnh.param("queue_size", queueSize, queueSize);
00108 pnh.param("max_depth", maxDepth_, maxDepth_);
00109 pnh.param("min_depth", minDepth_, minDepth_);
00110 pnh.param("voxel_size", voxelSize_, voxelSize_);
00111 pnh.param("decimation", decimation_, decimation_);
00112 pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_);
00113 pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_);
00114 pnh.param("normal_k", normalK_, normalK_);
00115 pnh.param("normal_radius", normalRadius_, normalRadius_);
00116 pnh.param("filter_nans", filterNaNs_, filterNaNs_);
00117 pnh.param("roi_ratios", roiStr, roiStr);
00118
00119
00120 if(pnh.hasParam("cut_left"))
00121 {
00122 ROS_ERROR("\"cut_left\" parameter is replaced by \"roi_ratios\". It will be ignored.");
00123 }
00124 if(pnh.hasParam("cut_right"))
00125 {
00126 ROS_ERROR("\"cut_right\" parameter is replaced by \"roi_ratios\". It will be ignored.");
00127 }
00128 if(pnh.hasParam("special_filter_close_object"))
00129 {
00130 ROS_ERROR("\"special_filter_close_object\" parameter is removed. This kind of processing "
00131 "should be done before or after this nodelet. See old implementation here: "
00132 "https://github.com/introlab/rtabmap_ros/blob/f0026b071c7c54fbcc71df778dd7e17f52f78fc4/src/nodelets/point_cloud_xyz.cpp#L178-L201.");
00133 }
00134
00135
00136 roiRatios_.resize(4, 0);
00137 if(!roiStr.empty())
00138 {
00139 std::list<std::string> strValues = uSplit(roiStr, ' ');
00140 if(strValues.size() != 4)
00141 {
00142 ROS_ERROR("The number of values must be 4 (\"roi_ratios\"=\"%s\")", roiStr.c_str());
00143 }
00144 else
00145 {
00146 std::vector<float> tmpValues(4);
00147 unsigned int i=0;
00148 for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
00149 {
00150 tmpValues[i] = uStr2Float(*jter);
00151 ++i;
00152 }
00153
00154 if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
00155 tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
00156 tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
00157 tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
00158 {
00159 roiRatios_ = tmpValues;
00160 }
00161 else
00162 {
00163 ROS_ERROR("The roi ratios are not valid (\"roi_ratios\"=\"%s\")", roiStr.c_str());
00164 }
00165 }
00166 }
00167
00168 NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
00169
00170 if(approxSync)
00171 {
00172 approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
00173 approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, _1, _2));
00174
00175 approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
00176 approxSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, _1, _2));
00177 }
00178 else
00179 {
00180 exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
00181 exactSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, _1, _2));
00182
00183 exactSyncDisparity_ = new message_filters::Synchronizer<MyExactSyncDisparityPolicy>(MyExactSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
00184 exactSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, _1, _2));
00185 }
00186
00187 ros::NodeHandle depth_nh(nh, "depth");
00188 ros::NodeHandle depth_pnh(pnh, "depth");
00189 image_transport::ImageTransport depth_it(depth_nh);
00190 image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
00191
00192 imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
00193 cameraInfoSub_.subscribe(depth_nh, "camera_info", 1);
00194
00195 disparitySub_.subscribe(nh, "disparity/image", 1);
00196 disparityCameraInfoSub_.subscribe(nh, "disparity/camera_info", 1);
00197
00198 cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1);
00199 }
00200
00201 void callback(
00202 const sensor_msgs::ImageConstPtr& depth,
00203 const sensor_msgs::CameraInfoConstPtr& cameraInfo)
00204 {
00205 if(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 &&
00206 depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0 &&
00207 depth->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0)
00208 {
00209 NODELET_ERROR("Input type depth=32FC1,16UC1,MONO16");
00210 return;
00211 }
00212
00213 if(cloudPub_.getNumSubscribers())
00214 {
00215 ros::WallTime time = ros::WallTime::now();
00216
00217 cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth);
00218 cv::Rect roi = rtabmap::util2d::computeRoi(imageDepthPtr->image, roiRatios_);
00219
00220 image_geometry::PinholeCameraModel model;
00221 model.fromCameraInfo(*cameraInfo);
00222
00223 pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
00224 rtabmap::CameraModel m(
00225 model.fx(),
00226 model.fy(),
00227 model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols),
00228 model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows));
00229
00230 pcl::IndicesPtr indices(new std::vector<int>);
00231 pclCloud = rtabmap::util3d::cloudFromDepth(
00232 cv::Mat(imageDepthPtr->image, roi),
00233 m,
00234 decimation_,
00235 maxDepth_,
00236 minDepth_,
00237 indices.get());
00238 processAndPublish(pclCloud, indices, depth->header);
00239
00240 NODELET_DEBUG("point_cloud_xyz from depth time = %f s", (ros::WallTime::now() - time).toSec());
00241 }
00242 }
00243
00244 void callbackDisparity(
00245 const stereo_msgs::DisparityImageConstPtr& disparityMsg,
00246 const sensor_msgs::CameraInfoConstPtr& cameraInfo)
00247 {
00248 if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0 &&
00249 disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_16SC1) !=0)
00250 {
00251 NODELET_ERROR("Input type must be disparity=32FC1 or 16SC1");
00252 return;
00253 }
00254
00255 cv::Mat disparity;
00256 if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0)
00257 {
00258 disparity = cv::Mat(disparityMsg->image.height, disparityMsg->image.width, CV_32FC1, const_cast<uchar*>(disparityMsg->image.data.data()));
00259 }
00260 else
00261 {
00262 disparity = cv::Mat(disparityMsg->image.height, disparityMsg->image.width, CV_16SC1, const_cast<uchar*>(disparityMsg->image.data.data()));
00263 }
00264
00265 if(cloudPub_.getNumSubscribers())
00266 {
00267 ros::WallTime time = ros::WallTime::now();
00268
00269 cv::Rect roi = rtabmap::util2d::computeRoi(disparity, roiRatios_);
00270
00271 pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
00272 rtabmap::CameraModel leftModel = rtabmap_ros::cameraModelFromROS(*cameraInfo);
00273 rtabmap::StereoCameraModel stereoModel(disparityMsg->f, disparityMsg->f, leftModel.cx()-roiRatios_[0]*double(disparity.cols), leftModel.cy()-roiRatios_[2]*double(disparity.rows), disparityMsg->T);
00274 pcl::IndicesPtr indices(new std::vector<int>);
00275 pclCloud = rtabmap::util3d::cloudFromDisparity(
00276 cv::Mat(disparity, roi),
00277 stereoModel,
00278 decimation_,
00279 maxDepth_,
00280 minDepth_,
00281 indices.get());
00282
00283 processAndPublish(pclCloud, indices, disparityMsg->header);
00284
00285 NODELET_DEBUG("point_cloud_xyz from disparity time = %f s", (ros::WallTime::now() - time).toSec());
00286 }
00287 }
00288
00289 void processAndPublish(pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud, pcl::IndicesPtr & indices, const std_msgs::Header & header)
00290 {
00291 if(indices->size() && voxelSize_ > 0.0)
00292 {
00293 pclCloud = rtabmap::util3d::voxelize(pclCloud, indices, voxelSize_);
00294 }
00295
00296
00297 if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
00298 {
00299 if(pclCloud->is_dense)
00300 {
00301 indices = rtabmap::util3d::radiusFiltering(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
00302 }
00303 else
00304 {
00305 indices = rtabmap::util3d::radiusFiltering(pclCloud, indices, noiseFilterRadius_, noiseFilterMinNeighbors_);
00306 }
00307 pcl::PointCloud<pcl::PointXYZ>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZ>);
00308 pcl::copyPointCloud(*pclCloud, *indices, *tmp);
00309 pclCloud = tmp;
00310 }
00311
00312 sensor_msgs::PointCloud2 rosCloud;
00313 if(pclCloud->size() && (normalK_ > 0 || normalRadius_ > 0.0f))
00314 {
00315
00316 pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_);
00317 pcl::PointCloud<pcl::PointNormal>::Ptr pclCloudNormal(new pcl::PointCloud<pcl::PointNormal>);
00318 pcl::concatenateFields(*pclCloud, *normals, *pclCloudNormal);
00319 if(filterNaNs_)
00320 {
00321 pclCloudNormal = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclCloudNormal);
00322 }
00323 pcl::toROSMsg(*pclCloudNormal, rosCloud);
00324 }
00325 else
00326 {
00327 if(filterNaNs_ && !pclCloud->is_dense)
00328 {
00329 pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud);
00330 }
00331 pcl::toROSMsg(*pclCloud, rosCloud);
00332 }
00333 rosCloud.header.stamp = header.stamp;
00334 rosCloud.header.frame_id = header.frame_id;
00335
00336
00337 cloudPub_.publish(rosCloud);
00338 }
00339
00340 private:
00341
00342 double maxDepth_;
00343 double minDepth_;
00344 double voxelSize_;
00345 int decimation_;
00346 double noiseFilterRadius_;
00347 int noiseFilterMinNeighbors_;
00348 int normalK_;
00349 double normalRadius_;
00350 bool filterNaNs_;
00351 std::vector<float> roiRatios_;
00352
00353 ros::Publisher cloudPub_;
00354
00355 image_transport::SubscriberFilter imageDepthSub_;
00356 message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
00357
00358 message_filters::Subscriber<stereo_msgs::DisparityImage> disparitySub_;
00359 message_filters::Subscriber<sensor_msgs::CameraInfo> disparityCameraInfoSub_;
00360
00361 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncDepthPolicy;
00362 message_filters::Synchronizer<MyApproxSyncDepthPolicy> * approxSyncDepth_;
00363
00364 typedef message_filters::sync_policies::ApproximateTime<stereo_msgs::DisparityImage, sensor_msgs::CameraInfo> MyApproxSyncDisparityPolicy;
00365 message_filters::Synchronizer<MyApproxSyncDisparityPolicy> * approxSyncDisparity_;
00366
00367 typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncDepthPolicy;
00368 message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_;
00369
00370 typedef message_filters::sync_policies::ExactTime<stereo_msgs::DisparityImage, sensor_msgs::CameraInfo> MyExactSyncDisparityPolicy;
00371 message_filters::Synchronizer<MyExactSyncDisparityPolicy> * exactSyncDisparity_;
00372
00373 };
00374
00375 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudXYZ, nodelet::Nodelet);
00376 }
00377