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 <stereo_msgs/DisparityImage.h>
00044
00045 #include <image_transport/image_transport.h>
00046 #include <image_transport/subscriber_filter.h>
00047
00048 #include <image_geometry/pinhole_camera_model.h>
00049 #include <image_geometry/stereo_camera_model.h>
00050
00051 #include <message_filters/sync_policies/approximate_time.h>
00052 #include <message_filters/sync_policies/exact_time.h>
00053 #include <message_filters/subscriber.h>
00054
00055 #include <cv_bridge/cv_bridge.h>
00056 #include <opencv2/highgui/highgui.hpp>
00057
00058 #include "rtabmap/core/util2d.h"
00059 #include "rtabmap/core/util3d.h"
00060 #include "rtabmap/core/util3d_filtering.h"
00061 #include "rtabmap/core/util3d_surface.h"
00062 #include "rtabmap/utilite/UConversion.h"
00063 #include "rtabmap/utilite/UStl.h"
00064
00065 namespace rtabmap_ros
00066 {
00067
00068 class PointCloudXYZRGB : public nodelet::Nodelet
00069 {
00070 public:
00071 PointCloudXYZRGB() :
00072 maxDepth_(0.0),
00073 minDepth_(0.0),
00074 voxelSize_(0.0),
00075 decimation_(1),
00076 noiseFilterRadius_(0.0),
00077 noiseFilterMinNeighbors_(5),
00078 normalK_(0),
00079 normalRadius_(0.0),
00080 filterNaNs_(false),
00081 approxSyncDepth_(0),
00082 approxSyncDisparity_(0),
00083 approxSyncStereo_(0),
00084 exactSyncDepth_(0),
00085 exactSyncDisparity_(0),
00086 exactSyncStereo_(0)
00087 {}
00088
00089 virtual ~PointCloudXYZRGB()
00090 {
00091 if(approxSyncDepth_)
00092 delete approxSyncDepth_;
00093 if(approxSyncDisparity_)
00094 delete approxSyncDisparity_;
00095 if(approxSyncStereo_)
00096 delete approxSyncStereo_;
00097 if(exactSyncDepth_)
00098 delete exactSyncDepth_;
00099 if(exactSyncDisparity_)
00100 delete exactSyncDisparity_;
00101 if(exactSyncStereo_)
00102 delete exactSyncStereo_;
00103 }
00104
00105 private:
00106 virtual void onInit()
00107 {
00108 ros::NodeHandle & nh = getNodeHandle();
00109 ros::NodeHandle & pnh = getPrivateNodeHandle();
00110
00111 int queueSize = 10;
00112 bool approxSync = true;
00113 std::string roiStr;
00114 pnh.param("approx_sync", approxSync, approxSync);
00115 pnh.param("queue_size", queueSize, queueSize);
00116 pnh.param("max_depth", maxDepth_, maxDepth_);
00117 pnh.param("min_depth", minDepth_, minDepth_);
00118 pnh.param("voxel_size", voxelSize_, voxelSize_);
00119 pnh.param("decimation", decimation_, decimation_);
00120 pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_);
00121 pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_);
00122 pnh.param("normal_k", normalK_, normalK_);
00123 pnh.param("normal_radius", normalRadius_, normalRadius_);
00124 pnh.param("filter_nans", filterNaNs_, filterNaNs_);
00125 pnh.param("roi_ratios", roiStr, roiStr);
00126
00127
00128 roiRatios_.resize(4, 0);
00129 if(!roiStr.empty())
00130 {
00131 std::list<std::string> strValues = uSplit(roiStr, ' ');
00132 if(strValues.size() != 4)
00133 {
00134 ROS_ERROR("The number of values must be 4 (\"roi_ratios\"=\"%s\")", roiStr.c_str());
00135 }
00136 else
00137 {
00138 std::vector<float> tmpValues(4);
00139 unsigned int i=0;
00140 for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
00141 {
00142 tmpValues[i] = uStr2Float(*jter);
00143 ++i;
00144 }
00145
00146 if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
00147 tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
00148 tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
00149 tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
00150 {
00151 roiRatios_ = tmpValues;
00152 }
00153 else
00154 {
00155 ROS_ERROR("The roi ratios are not valid (\"roi_ratios\"=\"%s\")", roiStr.c_str());
00156 }
00157 }
00158 }
00159
00160
00161 stereoBMParameters_ = rtabmap::Parameters::getDefaultParameters("StereoBM");
00162 for(rtabmap::ParametersMap::iterator iter=stereoBMParameters_.begin(); iter!=stereoBMParameters_.end(); ++iter)
00163 {
00164 std::string vStr;
00165 bool vBool;
00166 int vInt;
00167 double vDouble;
00168 if(pnh.getParam(iter->first, vStr))
00169 {
00170 NODELET_INFO("point_cloud_xyzrgb: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
00171 iter->second = vStr;
00172 }
00173 else if(pnh.getParam(iter->first, vBool))
00174 {
00175 NODELET_INFO("point_cloud_xyzrgb: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
00176 iter->second = uBool2Str(vBool);
00177 }
00178 else if(pnh.getParam(iter->first, vDouble))
00179 {
00180 NODELET_INFO("point_cloud_xyzrgb: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
00181 iter->second = uNumber2Str(vDouble);
00182 }
00183 else if(pnh.getParam(iter->first, vInt))
00184 {
00185 NODELET_INFO("point_cloud_xyzrgb: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
00186 iter->second = uNumber2Str(vInt);
00187 }
00188 }
00189
00190 NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
00191
00192 cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1);
00193
00194 rgbdImageSub_ = nh.subscribe("rgbd_image", 1, &PointCloudXYZRGB::rgbdImageCallback, this);
00195
00196 if(approxSync)
00197 {
00198
00199 approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
00200 approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZRGB::depthCallback, this, _1, _2, _3));
00201
00202 approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(queueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_);
00203 approxSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZRGB::disparityCallback, this, _1, _2, _3));
00204
00205 approxSyncStereo_ = new message_filters::Synchronizer<MyApproxSyncStereoPolicy>(MyApproxSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
00206 approxSyncStereo_->registerCallback(boost::bind(&PointCloudXYZRGB::stereoCallback, this, _1, _2, _3, _4));
00207 }
00208 else
00209 {
00210 exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
00211 exactSyncDepth_->registerCallback(boost::bind(&PointCloudXYZRGB::depthCallback, this, _1, _2, _3));
00212
00213 exactSyncDisparity_ = new message_filters::Synchronizer<MyExactSyncDisparityPolicy>(MyExactSyncDisparityPolicy(queueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_);
00214 exactSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZRGB::disparityCallback, this, _1, _2, _3));
00215
00216 exactSyncStereo_ = new message_filters::Synchronizer<MyExactSyncStereoPolicy>(MyExactSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
00217 exactSyncStereo_->registerCallback(boost::bind(&PointCloudXYZRGB::stereoCallback, this, _1, _2, _3, _4));
00218 }
00219
00220 ros::NodeHandle rgb_nh(nh, "rgb");
00221 ros::NodeHandle depth_nh(nh, "depth");
00222 ros::NodeHandle rgb_pnh(pnh, "rgb");
00223 ros::NodeHandle depth_pnh(pnh, "depth");
00224 image_transport::ImageTransport rgb_it(rgb_nh);
00225 image_transport::ImageTransport depth_it(depth_nh);
00226 image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
00227 image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
00228
00229 imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
00230 imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
00231 cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
00232
00233 ros::NodeHandle left_nh(nh, "left");
00234 ros::NodeHandle right_nh(nh, "right");
00235 ros::NodeHandle left_pnh(pnh, "left");
00236 ros::NodeHandle right_pnh(pnh, "right");
00237 image_transport::ImageTransport left_it(left_nh);
00238 image_transport::ImageTransport right_it(right_nh);
00239 image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
00240 image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
00241
00242 imageDisparitySub_.subscribe(nh, "disparity", 1);
00243
00244 imageLeft_.subscribe(left_it, left_nh.resolveName("image"), 1, hintsLeft);
00245 imageRight_.subscribe(right_it, right_nh.resolveName("image"), 1, hintsRight);
00246 cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
00247 cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
00248 }
00249
00250 void depthCallback(
00251 const sensor_msgs::ImageConstPtr& image,
00252 const sensor_msgs::ImageConstPtr& imageDepth,
00253 const sensor_msgs::CameraInfoConstPtr& cameraInfo)
00254 {
00255 if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
00256 image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
00257 image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
00258 image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
00259 image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
00260 image->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
00261 image->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
00262 image->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) ||
00263 !(imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 ||
00264 imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 ||
00265 imageDepth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0))
00266 {
00267 NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
00268 return;
00269 }
00270
00271 if(cloudPub_.getNumSubscribers())
00272 {
00273 ros::WallTime time = ros::WallTime::now();
00274
00275 cv_bridge::CvImageConstPtr imagePtr;
00276 if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
00277 {
00278 imagePtr = cv_bridge::toCvShare(image);
00279 }
00280 else if(image->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
00281 image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
00282 {
00283 imagePtr = cv_bridge::toCvShare(image, "mono8");
00284 }
00285 else
00286 {
00287 imagePtr = cv_bridge::toCvShare(image, "bgr8");
00288 }
00289
00290 cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth);
00291
00292 image_geometry::PinholeCameraModel model;
00293 model.fromCameraInfo(*cameraInfo);
00294
00295 ROS_ASSERT(imageDepthPtr->image.cols == imagePtr->image.cols);
00296 ROS_ASSERT(imageDepthPtr->image.rows == imagePtr->image.rows);
00297
00298 pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
00299 cv::Rect roi = rtabmap::util2d::computeRoi(imageDepthPtr->image, roiRatios_);
00300
00301 rtabmap::CameraModel m(
00302 model.fx(),
00303 model.fy(),
00304 model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols),
00305 model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows));
00306 pcl::IndicesPtr indices(new std::vector<int>);
00307 pclCloud = rtabmap::util3d::cloudFromDepthRGB(
00308 cv::Mat(imagePtr->image, roi),
00309 cv::Mat(imageDepthPtr->image, roi),
00310 m,
00311 decimation_,
00312 maxDepth_,
00313 minDepth_,
00314 indices.get());
00315
00316
00317 processAndPublish(pclCloud, indices, imagePtr->header);
00318
00319 NODELET_DEBUG("point_cloud_xyzrgb from RGB-D time = %f s", (ros::WallTime::now() - time).toSec());
00320 }
00321 }
00322
00323 void disparityCallback(
00324 const sensor_msgs::ImageConstPtr& image,
00325 const stereo_msgs::DisparityImageConstPtr& imageDisparity,
00326 const sensor_msgs::CameraInfoConstPtr& cameraInfo)
00327 {
00328 cv_bridge::CvImageConstPtr imagePtr;
00329 if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
00330 {
00331 imagePtr = cv_bridge::toCvShare(image);
00332 }
00333 else if(image->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
00334 image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
00335 {
00336 imagePtr = cv_bridge::toCvShare(image, "mono8");
00337 }
00338 else
00339 {
00340 imagePtr = cv_bridge::toCvShare(image, "bgr8");
00341 }
00342
00343 if(imageDisparity->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0 &&
00344 imageDisparity->image.encoding.compare(sensor_msgs::image_encodings::TYPE_16SC1) !=0)
00345 {
00346 NODELET_ERROR("Input type must be disparity=32FC1 or 16SC1");
00347 return;
00348 }
00349
00350 cv::Mat disparity;
00351 if(imageDisparity->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0)
00352 {
00353 disparity = cv::Mat(imageDisparity->image.height, imageDisparity->image.width, CV_32FC1, const_cast<uchar*>(imageDisparity->image.data.data()));
00354 }
00355 else
00356 {
00357 disparity = cv::Mat(imageDisparity->image.height, imageDisparity->image.width, CV_16SC1, const_cast<uchar*>(imageDisparity->image.data.data()));
00358 }
00359
00360 if(cloudPub_.getNumSubscribers())
00361 {
00362 ros::WallTime time = ros::WallTime::now();
00363
00364 cv::Rect roi = rtabmap::util2d::computeRoi(disparity, roiRatios_);
00365
00366 pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
00367 rtabmap::CameraModel leftModel = rtabmap_ros::cameraModelFromROS(*cameraInfo);
00368 rtabmap::StereoCameraModel stereoModel(imageDisparity->f, imageDisparity->f, leftModel.cx()-roiRatios_[0]*double(disparity.cols), leftModel.cy()-roiRatios_[2]*double(disparity.rows), imageDisparity->T);
00369 pcl::IndicesPtr indices(new std::vector<int>);
00370 pclCloud = rtabmap::util3d::cloudFromDisparityRGB(
00371 cv::Mat(imagePtr->image, roi),
00372 cv::Mat(disparity, roi),
00373 stereoModel,
00374 decimation_,
00375 maxDepth_,
00376 minDepth_,
00377 indices.get());
00378
00379 processAndPublish(pclCloud, indices, imageDisparity->header);
00380
00381 NODELET_DEBUG("point_cloud_xyzrgb from disparity time = %f s", (ros::WallTime::now() - time).toSec());
00382 }
00383 }
00384
00385 void stereoCallback(const sensor_msgs::ImageConstPtr& imageLeft,
00386 const sensor_msgs::ImageConstPtr& imageRight,
00387 const sensor_msgs::CameraInfoConstPtr& camInfoLeft,
00388 const sensor_msgs::CameraInfoConstPtr& camInfoRight)
00389 {
00390 if(!(imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
00391 imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
00392 imageLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
00393 imageLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
00394 !(imageRight->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
00395 imageRight->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
00396 imageRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
00397 imageRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
00398 {
00399 NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (enc=%s)", imageLeft->encoding.c_str());
00400 return;
00401 }
00402
00403 if(cloudPub_.getNumSubscribers())
00404 {
00405 ros::WallTime time = ros::WallTime::now();
00406
00407 cv_bridge::CvImageConstPtr ptrLeftImage, ptrRightImage;
00408 if(imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
00409 imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
00410 {
00411 ptrLeftImage = cv_bridge::toCvShare(imageLeft, "mono8");
00412 }
00413 else
00414 {
00415 ptrLeftImage = cv_bridge::toCvShare(imageLeft, "bgr8");
00416 }
00417 ptrRightImage = cv_bridge::toCvShare(imageRight, "mono8");
00418
00419 if(roiRatios_[0]!=0.0f || roiRatios_[1]!=0.0f || roiRatios_[2]!=0.0f || roiRatios_[3]!=0.0f)
00420 {
00421 ROS_WARN("\"roi_ratios\" set but ignored for stereo images.");
00422 }
00423
00424 pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
00425 pcl::IndicesPtr indices(new std::vector<int>);
00426 pclCloud = rtabmap::util3d::cloudFromStereoImages(
00427 ptrLeftImage->image,
00428 ptrRightImage->image,
00429 rtabmap_ros::stereoCameraModelFromROS(*camInfoLeft, *camInfoRight),
00430 decimation_,
00431 maxDepth_,
00432 minDepth_,
00433 indices.get(),
00434 stereoBMParameters_);
00435
00436 processAndPublish(pclCloud, indices, imageLeft->header);
00437
00438 NODELET_DEBUG("point_cloud_xyzrgb from stereo time = %f s", (ros::WallTime::now() - time).toSec());
00439 }
00440 }
00441
00442 void rgbdImageCallback(const rtabmap_ros::RGBDImageConstPtr & image)
00443 {
00444 if(cloudPub_.getNumSubscribers())
00445 {
00446 ros::WallTime time = ros::WallTime::now();
00447
00448 rtabmap::SensorData data = rtabmap_ros::rgbdImageFromROS(image);
00449 pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
00450 pcl::IndicesPtr indices(new std::vector<int>);
00451 if(data.isValid())
00452 {
00453 pclCloud = rtabmap::util3d::cloudRGBFromSensorData(
00454 data,
00455 decimation_,
00456 maxDepth_,
00457 minDepth_,
00458 indices.get(),
00459 stereoBMParameters_);
00460
00461 processAndPublish(pclCloud, indices, image->header);
00462 }
00463
00464 NODELET_DEBUG("point_cloud_xyzrgb from rgbd_image time = %f s", (ros::WallTime::now() - time).toSec());
00465 }
00466 }
00467
00468 void processAndPublish(pcl::PointCloud<pcl::PointXYZRGB>::Ptr & pclCloud, pcl::IndicesPtr & indices, const std_msgs::Header & header)
00469 {
00470 if(indices->size() && voxelSize_ > 0.0)
00471 {
00472 pclCloud = rtabmap::util3d::voxelize(pclCloud, indices, voxelSize_);
00473 }
00474
00475
00476 if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
00477 {
00478 if(pclCloud->is_dense)
00479 {
00480 indices = rtabmap::util3d::radiusFiltering(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
00481 }
00482 else
00483 {
00484 indices = rtabmap::util3d::radiusFiltering(pclCloud, indices, noiseFilterRadius_, noiseFilterMinNeighbors_);
00485 }
00486 pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
00487 pcl::copyPointCloud(*pclCloud, *indices, *tmp);
00488 pclCloud = tmp;
00489 }
00490
00491 sensor_msgs::PointCloud2 rosCloud;
00492 if(pclCloud->size() && (normalK_ > 0 || normalRadius_ > 0.0f))
00493 {
00494
00495 pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_);
00496 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pclCloudNormal(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
00497 pcl::concatenateFields(*pclCloud, *normals, *pclCloudNormal);
00498 if(filterNaNs_)
00499 {
00500 pclCloudNormal = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclCloudNormal);
00501 }
00502 pcl::toROSMsg(*pclCloudNormal, rosCloud);
00503 }
00504 else
00505 {
00506 if(filterNaNs_ && !pclCloud->is_dense)
00507 {
00508 pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud);
00509 }
00510 pcl::toROSMsg(*pclCloud, rosCloud);
00511 }
00512 rosCloud.header.stamp = header.stamp;
00513 rosCloud.header.frame_id = header.frame_id;
00514
00515
00516 cloudPub_.publish(rosCloud);
00517 }
00518
00519 private:
00520
00521 double maxDepth_;
00522 double minDepth_;
00523 double voxelSize_;
00524 int decimation_;
00525 double noiseFilterRadius_;
00526 int noiseFilterMinNeighbors_;
00527 int normalK_;
00528 double normalRadius_;
00529 bool filterNaNs_;
00530 std::vector<float> roiRatios_;
00531 rtabmap::ParametersMap stereoBMParameters_;
00532
00533 ros::Publisher cloudPub_;
00534
00535 ros::Subscriber rgbdImageSub_;
00536
00537 image_transport::SubscriberFilter imageSub_;
00538 image_transport::SubscriberFilter imageDepthSub_;
00539 message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
00540
00541 message_filters::Subscriber<stereo_msgs::DisparityImage> imageDisparitySub_;
00542
00543 image_transport::SubscriberFilter imageLeft_;
00544 image_transport::SubscriberFilter imageRight_;
00545 message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
00546 message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
00547
00548 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncDepthPolicy;
00549 message_filters::Synchronizer<MyApproxSyncDepthPolicy> * approxSyncDepth_;
00550
00551 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, stereo_msgs::DisparityImage, sensor_msgs::CameraInfo> MyApproxSyncDisparityPolicy;
00552 message_filters::Synchronizer<MyApproxSyncDisparityPolicy> * approxSyncDisparity_;
00553
00554 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyApproxSyncStereoPolicy;
00555 message_filters::Synchronizer<MyApproxSyncStereoPolicy> * approxSyncStereo_;
00556
00557 typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncDepthPolicy;
00558 message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_;
00559
00560 typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, stereo_msgs::DisparityImage, sensor_msgs::CameraInfo> MyExactSyncDisparityPolicy;
00561 message_filters::Synchronizer<MyExactSyncDisparityPolicy> * exactSyncDisparity_;
00562
00563 typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncStereoPolicy;
00564 message_filters::Synchronizer<MyExactSyncStereoPolicy> * exactSyncStereo_;
00565 };
00566
00567 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudXYZRGB, nodelet::Nodelet);
00568 }
00569