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