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 <rtabmap_ros/MsgConversion.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
00043 #include <image_transport/image_transport.h>
00044 #include <image_transport/subscriber_filter.h>
00045
00046 #include <image_geometry/pinhole_camera_model.h>
00047 #include <image_geometry/stereo_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 PointCloudXYZRGB : public nodelet::Nodelet
00066 {
00067 public:
00068 PointCloudXYZRGB() :
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 approxSyncStereo_(0),
00077 exactSyncDepth_(0),
00078 exactSyncStereo_(0)
00079 {}
00080
00081 virtual ~PointCloudXYZRGB()
00082 {
00083 if(approxSyncDepth_)
00084 delete approxSyncDepth_;
00085 if(approxSyncStereo_)
00086 delete approxSyncStereo_;
00087 if(exactSyncDepth_)
00088 delete exactSyncDepth_;
00089 if(exactSyncStereo_)
00090 delete exactSyncStereo_;
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 roiRatios_.resize(4, 0);
00114 if(!roiStr.empty())
00115 {
00116 std::list<std::string> strValues = uSplit(roiStr, ' ');
00117 if(strValues.size() != 4)
00118 {
00119 ROS_ERROR("The number of values must be 4 (\"roi_ratios\"=\"%s\")", roiStr.c_str());
00120 }
00121 else
00122 {
00123 std::vector<float> tmpValues(4);
00124 unsigned int i=0;
00125 for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
00126 {
00127 tmpValues[i] = uStr2Float(*jter);
00128 ++i;
00129 }
00130
00131 if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
00132 tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
00133 tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
00134 tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
00135 {
00136 roiRatios_ = tmpValues;
00137 }
00138 else
00139 {
00140 ROS_ERROR("The roi ratios are not valid (\"roi_ratios\"=\"%s\")", roiStr.c_str());
00141 }
00142 }
00143 }
00144
00145 NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
00146
00147 cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1);
00148
00149 if(approxSync)
00150 {
00151
00152 approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
00153 approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZRGB::depthCallback, this, _1, _2, _3));
00154
00155 approxSyncStereo_ = new message_filters::Synchronizer<MyApproxSyncStereoPolicy>(MyApproxSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
00156 approxSyncStereo_->registerCallback(boost::bind(&PointCloudXYZRGB::stereoCallback, this, _1, _2, _3, _4));
00157 }
00158 else
00159 {
00160 exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
00161 exactSyncDepth_->registerCallback(boost::bind(&PointCloudXYZRGB::depthCallback, this, _1, _2, _3));
00162
00163 exactSyncStereo_ = new message_filters::Synchronizer<MyExactSyncStereoPolicy>(MyExactSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
00164 exactSyncStereo_->registerCallback(boost::bind(&PointCloudXYZRGB::stereoCallback, this, _1, _2, _3, _4));
00165 }
00166
00167 ros::NodeHandle rgb_nh(nh, "rgb");
00168 ros::NodeHandle depth_nh(nh, "depth");
00169 ros::NodeHandle rgb_pnh(pnh, "rgb");
00170 ros::NodeHandle depth_pnh(pnh, "depth");
00171 image_transport::ImageTransport rgb_it(rgb_nh);
00172 image_transport::ImageTransport depth_it(depth_nh);
00173 image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
00174 image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
00175
00176 imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
00177 imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
00178 cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
00179
00180
00181 ros::NodeHandle left_nh(nh, "left");
00182 ros::NodeHandle right_nh(nh, "right");
00183 ros::NodeHandle left_pnh(pnh, "left");
00184 ros::NodeHandle right_pnh(pnh, "right");
00185 image_transport::ImageTransport left_it(left_nh);
00186 image_transport::ImageTransport right_it(right_nh);
00187 image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
00188 image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
00189
00190 imageLeft_.subscribe(left_it, left_nh.resolveName("image"), 1, hintsLeft);
00191 imageRight_.subscribe(right_it, right_nh.resolveName("image"), 1, hintsRight);
00192 cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
00193 cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
00194 }
00195
00196 void depthCallback(
00197 const sensor_msgs::ImageConstPtr& image,
00198 const sensor_msgs::ImageConstPtr& imageDepth,
00199 const sensor_msgs::CameraInfoConstPtr& cameraInfo)
00200 {
00201 if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
00202 image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
00203 image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
00204 image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
00205 image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) &&
00206 !(imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 ||
00207 imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 ||
00208 imageDepth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0))
00209 {
00210 NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
00211 return;
00212 }
00213
00214 if(cloudPub_.getNumSubscribers())
00215 {
00216 ros::WallTime time = ros::WallTime::now();
00217
00218 cv_bridge::CvImageConstPtr imagePtr;
00219 if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
00220 {
00221 imagePtr = cv_bridge::toCvShare(image);
00222 }
00223 else if(image->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
00224 image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
00225 {
00226 imagePtr = cv_bridge::toCvShare(image, "mono8");
00227 }
00228 else
00229 {
00230 imagePtr = cv_bridge::toCvShare(image, "bgr8");
00231 }
00232
00233 cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth);
00234
00235 image_geometry::PinholeCameraModel model;
00236 model.fromCameraInfo(*cameraInfo);
00237
00238 ROS_ASSERT(imageDepthPtr->image.cols == imagePtr->image.cols);
00239 ROS_ASSERT(imageDepthPtr->image.rows == imagePtr->image.rows);
00240
00241 pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
00242 cv::Rect roi = rtabmap::Feature2D::computeRoi(imageDepthPtr->image, roiRatios_);
00243
00244 rtabmap::CameraModel m(
00245 model.fx(),
00246 model.fy(),
00247 model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols),
00248 model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows));
00249 pclCloud = rtabmap::util3d::cloudFromDepthRGB(
00250 cv::Mat(imagePtr->image, roi),
00251 cv::Mat(imageDepthPtr->image, roi),
00252 m,
00253 decimation_);
00254
00255
00256 processAndPublish(pclCloud, imagePtr->header);
00257
00258 NODELET_DEBUG("point_cloud_xyzrgb from RGB-D time = %f s", (ros::WallTime::now() - time).toSec());
00259 }
00260 }
00261
00262 void stereoCallback(const sensor_msgs::ImageConstPtr& imageLeft,
00263 const sensor_msgs::ImageConstPtr& imageRight,
00264 const sensor_msgs::CameraInfoConstPtr& camInfoLeft,
00265 const sensor_msgs::CameraInfoConstPtr& camInfoRight)
00266 {
00267 if(!(imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
00268 imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
00269 imageLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
00270 imageLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
00271 !(imageRight->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
00272 imageRight->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
00273 imageRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
00274 imageRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
00275 {
00276 NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (enc=%s)", imageLeft->encoding.c_str());
00277 return;
00278 }
00279
00280 if(cloudPub_.getNumSubscribers())
00281 {
00282 ros::WallTime time = ros::WallTime::now();
00283
00284 cv_bridge::CvImageConstPtr ptrLeftImage, ptrRightImage;
00285 if(imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
00286 imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
00287 {
00288 ptrLeftImage = cv_bridge::toCvShare(imageLeft, "mono8");
00289 }
00290 else
00291 {
00292 ptrLeftImage = cv_bridge::toCvShare(imageLeft, "bgr8");
00293 }
00294 ptrRightImage = cv_bridge::toCvShare(imageRight, "mono8");
00295
00296 if(roiRatios_[0]!=0.0f || roiRatios_[1]!=0.0f || roiRatios_[2]!=0.0f || roiRatios_[3]!=0.0f)
00297 {
00298 ROS_WARN("\"roi_ratios\" set but ignored for stereo images.");
00299 }
00300
00301 pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
00302 pclCloud = rtabmap::util3d::cloudFromStereoImages(
00303 ptrLeftImage->image,
00304 ptrRightImage->image,
00305 rtabmap_ros::stereoCameraModelFromROS(*camInfoLeft, *camInfoRight),
00306 decimation_);
00307
00308 processAndPublish(pclCloud, imageLeft->header);
00309
00310 NODELET_DEBUG("point_cloud_xyzrgb from stereo time = %f s", (ros::WallTime::now() - time).toSec());
00311 }
00312 }
00313
00314 void processAndPublish(pcl::PointCloud<pcl::PointXYZRGB>::Ptr & pclCloud, const std_msgs::Header & header)
00315 {
00316 if(pclCloud->size() && (minDepth_ != 0.0 || maxDepth_ > minDepth_))
00317 {
00318 pclCloud = rtabmap::util3d::passThrough(pclCloud, "z", minDepth_, maxDepth_>minDepth_?maxDepth_:std::numeric_limits<float>::max());
00319 }
00320
00321 if(pclCloud->size() && voxelSize_ > 0.0)
00322 {
00323 pclCloud = rtabmap::util3d::voxelize(pclCloud, voxelSize_);
00324 }
00325
00326
00327 if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
00328 {
00329 if(voxelSize_ <= 0.0 && !(minDepth_ != 0.0 || maxDepth_ > minDepth_))
00330 {
00331
00332 pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud);
00333 }
00334
00335 pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
00336 pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
00337 pcl::copyPointCloud(*pclCloud, *indices, *tmp);
00338 pclCloud = tmp;
00339 }
00340
00341 sensor_msgs::PointCloud2 rosCloud;
00342 pcl::toROSMsg(*pclCloud, rosCloud);
00343 rosCloud.header.stamp = header.stamp;
00344 rosCloud.header.frame_id = header.frame_id;
00345
00346
00347 cloudPub_.publish(rosCloud);
00348 }
00349
00350 private:
00351
00352 double maxDepth_;
00353 double minDepth_;
00354 double voxelSize_;
00355 int decimation_;
00356 double noiseFilterRadius_;
00357 int noiseFilterMinNeighbors_;
00358 std::vector<float> roiRatios_;
00359
00360 ros::Publisher cloudPub_;
00361
00362 image_transport::SubscriberFilter imageSub_;
00363 image_transport::SubscriberFilter imageDepthSub_;
00364 message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
00365
00366 image_transport::SubscriberFilter imageLeft_;
00367 image_transport::SubscriberFilter imageRight_;
00368 message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
00369 message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
00370
00371 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncDepthPolicy;
00372 message_filters::Synchronizer<MyApproxSyncDepthPolicy> * approxSyncDepth_;
00373
00374 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyApproxSyncStereoPolicy;
00375 message_filters::Synchronizer<MyApproxSyncStereoPolicy> * approxSyncStereo_;
00376
00377 typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncDepthPolicy;
00378 message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_;
00379
00380 typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncStereoPolicy;
00381 message_filters::Synchronizer<MyExactSyncStereoPolicy> * exactSyncStereo_;
00382 };
00383
00384 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudXYZRGB, nodelet::Nodelet);
00385 }
00386