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
00041 #include <image_transport/image_transport.h>
00042 #include <image_transport/subscriber_filter.h>
00043
00044 #include <image_geometry/pinhole_camera_model.h>
00045 #include <image_geometry/stereo_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 PointCloudXYZRGB : public nodelet::Nodelet
00060 {
00061 public:
00062 PointCloudXYZRGB() :
00063 maxDepth_(0.0),
00064 voxelSize_(0.0),
00065 decimation_(1),
00066 noiseFilterRadius_(0.0),
00067 noiseFilterMinNeighbors_(5),
00068 approxSyncDepth_(0),
00069 approxSyncStereo_(0),
00070 exactSyncDepth_(0),
00071 exactSyncStereo_(0)
00072 {}
00073
00074 virtual ~PointCloudXYZRGB()
00075 {
00076 if(approxSyncDepth_)
00077 delete approxSyncDepth_;
00078 if(approxSyncStereo_)
00079 delete approxSyncStereo_;
00080 if(exactSyncDepth_)
00081 delete exactSyncDepth_;
00082 if(exactSyncStereo_)
00083 delete exactSyncStereo_;
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
00102 ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
00103
00104 cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1);
00105
00106 if(approxSync)
00107 {
00108
00109 approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
00110 approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZRGB::depthCallback, this, _1, _2, _3));
00111
00112 approxSyncStereo_ = new message_filters::Synchronizer<MyApproxSyncStereoPolicy>(MyApproxSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
00113 approxSyncStereo_->registerCallback(boost::bind(&PointCloudXYZRGB::stereoCallback, this, _1, _2, _3, _4));
00114 }
00115 else
00116 {
00117 exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
00118 exactSyncDepth_->registerCallback(boost::bind(&PointCloudXYZRGB::depthCallback, this, _1, _2, _3));
00119
00120 exactSyncStereo_ = new message_filters::Synchronizer<MyExactSyncStereoPolicy>(MyExactSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
00121 exactSyncStereo_->registerCallback(boost::bind(&PointCloudXYZRGB::stereoCallback, this, _1, _2, _3, _4));
00122 }
00123
00124 ros::NodeHandle rgb_nh(nh, "rgb");
00125 ros::NodeHandle depth_nh(nh, "depth");
00126 ros::NodeHandle rgb_pnh(pnh, "rgb");
00127 ros::NodeHandle depth_pnh(pnh, "depth");
00128 image_transport::ImageTransport rgb_it(rgb_nh);
00129 image_transport::ImageTransport depth_it(depth_nh);
00130 image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
00131 image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
00132
00133 imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
00134 imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
00135 cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
00136
00137
00138 ros::NodeHandle left_nh(nh, "left");
00139 ros::NodeHandle right_nh(nh, "right");
00140 ros::NodeHandle left_pnh(pnh, "left");
00141 ros::NodeHandle right_pnh(pnh, "right");
00142 image_transport::ImageTransport left_it(left_nh);
00143 image_transport::ImageTransport right_it(right_nh);
00144 image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
00145 image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
00146
00147 imageLeft_.subscribe(left_it, left_nh.resolveName("image"), 1, hintsLeft);
00148 imageRight_.subscribe(right_it, right_nh.resolveName("image"), 1, hintsRight);
00149 cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
00150 cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
00151 }
00152
00153 void depthCallback(
00154 const sensor_msgs::ImageConstPtr& image,
00155 const sensor_msgs::ImageConstPtr& imageDepth,
00156 const sensor_msgs::CameraInfoConstPtr& cameraInfo)
00157 {
00158 if(!(image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
00159 image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
00160 image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
00161 image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) &&
00162 !(imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 ||
00163 imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 ||
00164 imageDepth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0))
00165 {
00166 ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
00167 return;
00168 }
00169
00170 if(cloudPub_.getNumSubscribers())
00171 {
00172 cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image, "bgr8");
00173 cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth);
00174
00175 image_geometry::PinholeCameraModel model;
00176 model.fromCameraInfo(*cameraInfo);
00177 float fx = model.fx();
00178 float fy = model.fy();
00179 float cx = model.cx();
00180 float cy = model.cy();
00181
00182 pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
00183 pclCloud = rtabmap::util3d::cloudFromDepthRGB(
00184 imagePtr->image,
00185 imageDepthPtr->image,
00186 cx,
00187 cy,
00188 fx,
00189 fy,
00190 decimation_);
00191
00192
00193 processAndPublish(pclCloud, imagePtr->header);
00194 }
00195 }
00196
00197 void stereoCallback(const sensor_msgs::ImageConstPtr& imageLeft,
00198 const sensor_msgs::ImageConstPtr& imageRight,
00199 const sensor_msgs::CameraInfoConstPtr& camInfoLeft,
00200 const sensor_msgs::CameraInfoConstPtr& camInfoRight)
00201 {
00202 if(!(imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
00203 imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
00204 imageLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
00205 imageLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
00206 !(imageRight->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
00207 imageRight->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
00208 imageRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
00209 imageRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
00210 {
00211 ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (enc=%s)", imageLeft->encoding.c_str());
00212 return;
00213 }
00214
00215 if(cloudPub_.getNumSubscribers())
00216 {
00217 cv_bridge::CvImageConstPtr ptrLeftImage, ptrRightImage;
00218 if(imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
00219 imageLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
00220 {
00221 ptrLeftImage = cv_bridge::toCvShare(imageLeft, "mono8");
00222 }
00223 else
00224 {
00225 ptrLeftImage = cv_bridge::toCvShare(imageLeft, "bgr8");
00226 }
00227 ptrRightImage = cv_bridge::toCvShare(imageRight, "mono8");
00228
00229 image_geometry::StereoCameraModel model;
00230 model.fromCameraInfo(*camInfoLeft, *camInfoRight);
00231
00232 float fx = model.left().fx();
00233 float cx = model.left().cx();
00234 float cy = model.left().cy();
00235 float baseline = model.baseline();
00236
00237 pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
00238 pclCloud = rtabmap::util3d::cloudFromStereoImages(
00239 ptrLeftImage->image,
00240 ptrRightImage->image,
00241 cx,
00242 cy,
00243 fx,
00244 baseline,
00245 decimation_);
00246
00247 processAndPublish(pclCloud, imageLeft->header);
00248 }
00249 }
00250
00251 void processAndPublish(pcl::PointCloud<pcl::PointXYZRGB>::Ptr & pclCloud, const std_msgs::Header & header)
00252 {
00253 if(pclCloud->size() && maxDepth_ > 0)
00254 {
00255 pclCloud = rtabmap::util3d::passThrough<pcl::PointXYZRGB>(pclCloud, "z", 0, maxDepth_);
00256 }
00257
00258 if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
00259 {
00260 pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering<pcl::PointXYZRGB>(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
00261 pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
00262 pcl::copyPointCloud(*pclCloud, *indices, *tmp);
00263 pclCloud = tmp;
00264 }
00265
00266 if(pclCloud->size() && voxelSize_ > 0.0)
00267 {
00268 pclCloud = rtabmap::util3d::voxelize<pcl::PointXYZRGB>(pclCloud, voxelSize_);
00269 }
00270
00271 sensor_msgs::PointCloud2 rosCloud;
00272 pcl::toROSMsg(*pclCloud, rosCloud);
00273 rosCloud.header.stamp = header.stamp;
00274 rosCloud.header.frame_id = header.frame_id;
00275
00276
00277 cloudPub_.publish(rosCloud);
00278 }
00279
00280 private:
00281
00282 double maxDepth_;
00283 double voxelSize_;
00284 int decimation_;
00285 double noiseFilterRadius_;
00286 int noiseFilterMinNeighbors_;
00287
00288 ros::Publisher cloudPub_;
00289
00290 image_transport::SubscriberFilter imageSub_;
00291 image_transport::SubscriberFilter imageDepthSub_;
00292 message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
00293
00294 image_transport::SubscriberFilter imageLeft_;
00295 image_transport::SubscriberFilter imageRight_;
00296 message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
00297 message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
00298
00299 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncDepthPolicy;
00300 message_filters::Synchronizer<MyApproxSyncDepthPolicy> * approxSyncDepth_;
00301
00302 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyApproxSyncStereoPolicy;
00303 message_filters::Synchronizer<MyApproxSyncStereoPolicy> * approxSyncStereo_;
00304
00305 typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncDepthPolicy;
00306 message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_;
00307
00308 typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncStereoPolicy;
00309 message_filters::Synchronizer<MyExactSyncStereoPolicy> * exactSyncStereo_;
00310 };
00311
00312 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudXYZRGB, nodelet::Nodelet);
00313 }
00314