2011/ros11: process_kinect_range2.cpp

File process_kinect_range2.cpp, 14.1 KB (added by cteo, 7 years ago)

simple kinect code that processes point clouds with PCL

Line 
1/*
2Sample code to show how we can subscribe to the Kinect /camera/rgb/points topic containing PCL 2.0 data (with RGB)
3and use the PCL library to:
41) display the data (pcl_visualization) - with RGB and just depth
52) limit data to a predefined 'z' (depth)
63) publish the filtered point cloud
7
8Author: Ching L. Teo
9Date: 05/30/2011
10*/
11// standard includes
12#include<stdexcept>
13// ROS core
14#include <ros/ros.h>
15#include <boost/thread/mutex.hpp>
16#include <pcl_ros/point_cloud.h>
17#include <image_transport/image_transport.h>
18#include <sensor_msgs/Image.h>
19#include <stereo_msgs/DisparityImage.h>
20#include <sensor_msgs/CameraInfo.h>
21#include <sensor_msgs/image_encodings.h>
22// PCL includes
23#include <pcl/io/io.h>
24#include <pcl/io/pcd_io.h>
25#include <pcl/point_types.h>
26#include <pcl/range_image/range_image_planar.h>
27#include <pcl/filters/passthrough.h>
28#include <pcl/common/common_headers.h>
29#include <pcl_visualization/pcl_visualizer.h>
30//openCV
31#include <opencv2/imgproc/imgproc.hpp>
32#include <opencv2/highgui/highgui.hpp>
33#include <opencv2/core/core.hpp>
34// cv_bridge
35#include <cv_bridge/cv_bridge.h>
36
37
38using namespace std;
39using namespace pcl;
40using namespace pcl_visualization;
41using namespace cv;
42namespace enc = sensor_msgs::image_encodings;
43
44class ProcessKinectRange
45{       
46
47public:
48        // constructor
49        ProcessKinectRange(int argc, char** argv) : it(node_handle)
50        {
51                angular_resolution = 0.3f; // Resolution of the range image
52                noise_level = 0.0f;         // Distance in which the z-buffer averages instead of taking the minimum
53                min_range  = 0.0f;          // Minimum range for the range image creation
54                border_size = 0;            // Additional border around the range image
55                viz_type = 1;               // 0 - depth+pcl, 1 - rgb+pcl, 2 - depth image only
56                coordinate_frame = RangeImage::CAMERA_FRAME;
57                viewer = new PCLVisualizer("Live viewer - point cloud");
58                range_image = new RangeImage;
59                sensor_pose = Eigen::Affine3f::Identity();
60
61                // check command line inputs
62                ProcessCommandLine(argc, argv);
63
64
65                //check input topic
66                if (node_handle.resolveName("input")=="/input")
67                {       
68                        std::cerr << "Did you forget input:=<your Kinect PCL topic>?\n";
69                        exit(0);
70                }
71                if (node_handle.resolveName("input2")=="/input2" && viz_type == 2)
72                {       
73                        std::cerr << "Did you forget input2:=<your Kinect Depth image topic>?\n";
74                        exit(0);
75                }
76
77                // set-up attributes + subscriber
78                angular_resolution = deg2rad (angular_resolution);     
79                subscriber = node_handle.subscribe ("input", 1, &ProcessKinectRange::cloud_msg_cb, this);
80                subscriber_depth = it.subscribe ("input2", 1, &ProcessKinectRange::depth_msg_cb, this); // receive depth images directly from Kinect
81                //set-up publishers -- a templated point cloud publisher (will convert from pcl::PointCloud to sensor_msgs::PointCloud2 automatically)
82                publisher = node_handle.advertise <pcl::PointCloud<pcl::PointXYZRGB> >("/umd_image_proc/PointCloud2_umd_processKinectRange2", 1);
83                publisher_img = it.advertise ("/umd_image_proc/image_umd_processKinectRange2", 1);
84        }
85
86        // destructor
87        ~ProcessKinectRange()
88        {
89        }
90       
91        // call backs for subscriber
92        void cloud_msg_cb (const sensor_msgs::PointCloud2ConstPtr& msg)
93        {
94          m.lock ();
95          cloud_msg = msg;
96          //std::cout << "Received cloud msg: " << "header=" << msg->header << "width="<<msg->width <<", height="<<msg->height<<".\n";
97          m.unlock ();
98         
99        // if point clouds are to be processed, else skip
100         if(viz_type!=2)
101                ProcessCloudMessage();
102
103        }
104        void depth_msg_cb (const sensor_msgs::ImageConstPtr& msg)
105        {
106          m.lock ();
107          kinect_depthimg_msg = msg;
108          m.unlock ();
109
110          if(viz_type == 2)
111                ProcessDepthMessage();
112
113        }
114
115private:
116        // ros attributes
117        ros::NodeHandle node_handle;
118        ros::Subscriber subscriber;
119        ros::Publisher  publisher;
120        image_transport::ImageTransport it; // should always use ImageTransport to subscribe and publish images
121        image_transport::Subscriber subscriber_depth;
122        image_transport::Publisher publisher_img;
123        PCLVisualizer* viewer;
124        RangeImage* range_image;
125
126        // other attributes
127        float angular_resolution;       // Resolution of the range image
128        float   noise_level;            // Distance in which the z-buffer averages instead of taking the minimum
129        float   min_range;              // Minimum range for the range image creation
130        int     border_size;            // Additional border around the range image
131        int     viz_type;               // the type of points expected: 0 - no rgb, 1 - otherwise
132        RangeImage::CoordinateFrame coordinate_frame;
133        sensor_msgs::PointCloud2ConstPtr cloud_msg, old_cloud_msg;
134        sensor_msgs::ImageConstPtr kinect_depthimg_msg, old_kinect_depthimg_msg;
135        boost::mutex m;
136        Eigen::Affine3f sensor_pose;
137
138        // Main internal function to process the point cloud message
139        void ProcessCloudMessage()
140        {
141
142                usleep (10000);
143                viewer->spinOnce (10);
144                // if no new messages appear, just stop processing
145                if (!cloud_msg || cloud_msg == old_cloud_msg)
146                        return;
147                old_cloud_msg = cloud_msg;
148               
149                // get message from ROS and convert to point cloud
150                PointCloud<PointXYZRGB> point_cloud;
151                fromROSMsg(*cloud_msg, point_cloud);
152
153                // if the sensor point cloud provides "far range data", use it to compute the sensor_pose       
154                PointCloud<PointWithViewpoint> far_ranges;
155                RangeImage::extractFarRanges(*cloud_msg, far_ranges);
156                if (pcl::getFieldIndex(*cloud_msg, "vp_x")>=0)
157                {
158                        PointCloud<PointWithViewpoint> tmp_pc;
159                        fromROSMsg(*cloud_msg, tmp_pc);
160                        Eigen::Vector3f average_viewpoint = RangeImage::getAverageViewPoint(tmp_pc);
161                        sensor_pose = Eigen::Translation3f(average_viewpoint) * sensor_pose;
162                }
163               
164                //ROS_INFO("(h,w)=%d,%d.\n", point_cloud.height, point_cloud.width);     
165               
166                // For efficieny, all functions in PCL work with PointCloud<PointT>::Ptr, so we extract them from the real point cloud
167                PointCloud<PointXYZRGB>::Ptr point_cloud_ptr (new PointCloud<PointXYZRGB>(point_cloud));
168                PointCloud<PointXYZRGB>::Ptr point_cloudfilt_ptr (new PointCloud<PointXYZRGB>); // filtered pc
169
170                // Filter clouds in Z dimension (min, max)
171                FilterPointCloudZ(point_cloud_ptr, point_cloudfilt_ptr, 0.0f, 10.0f);
172
173                //ROS_INFO("(pco_h, pco_w)=%d,%d.\n", (*point_cloud_ptr).height, (*point_cloud_ptr).width);     
174                //ROS_INFO("(h,w)=%d,%d.\n", (*point_cloudfilt_ptr).height, (*point_cloudfilt_ptr).width);     
175                               
176                // visualize
177                VisualizePointCloud(point_cloudfilt_ptr);
178                //VisualizePointCloud(point_cloud_ptr);
179
180                //publish filtered point cloud
181                publisher.publish((*point_cloudfilt_ptr));     
182/*//OLD CODE
183                // publish range_image
184                if(viz_type==0)
185                {
186
187                        //ROS_INFO("(rimg_h, rimg_w)=%d,%d.\n", range_image->width , range_image->height); 
188                        double rval_tmp;                       
189                        IplImage *tmp = cvCreateImage(cvSize(range_image->width , range_image->height), IPL_DEPTH_32F, 1);
190                        IplImage *t2 = cvCreateImage(cvSize(range_image->width , range_image->height), IPL_DEPTH_8U, 1); // for visualization only
191
192                        // Get iplImages from RangeImage
193                        // NOTE (06/08/11): Kinect Produces depth images directly...see active_seg_2D code
194                        cvSetZero(tmp);
195                        cvSetZero(t2);
196
197                        for(int p_x = 0; p_x < tmp->width; p_x++)
198                          for(int p_y = 0; p_y < tmp->height; p_y++)
199                          {
200                            //tmp->imageData[p_y*tmp->widthStep + p_x] = (range_image->getPoint(p_x, p_y).range > 0.0 ? 255 : 0);
201                            rval_tmp = range_image->getPoint(p_x, p_y).range;
202                            //ROS_INFO("rval: %g", rval_tmp);
203                            if(rval_tmp > 0.0f){
204                                // ROS_INFO("here");
205                                cvSet2D(tmp, p_y, p_x, cvScalar(rval_tmp));
206                                if(rval_tmp > 3.0f)
207                                        t2->imageData[p_y*t2->widthStep + p_x] = 255;
208                                else if(rval_tmp > 2.0f)
209                                        t2->imageData[p_y*t2->widthStep + p_x] = 200;
210                                else if(rval_tmp > 1.0f)
211                                        t2->imageData[p_y*t2->widthStep + p_x] = 150;
212                                else if(rval_tmp > 0.5f)
213                                        t2->imageData[p_y*t2->widthStep + p_x] = 100;
214                                else
215                                        t2->imageData[p_y*t2->widthStep + p_x] = 50;
216                            }
217                            else{
218                                cvSet2D(tmp, p_y, p_x, cvScalar(0.0));
219                                t2->imageData[p_y*t2->widthStep + p_x] = 0;
220                                //ROS_INFO("r_val=%g\n",rval_tmp);
221                            }
222                          }
223
224                        //DEBUG
225                        double minval, maxval;
226                        cvMinMaxLoc(tmp, &minval, &maxval);
227                        ROS_INFO("min: %g, max: %g", minval, maxval);
228
229                        // visualize
230                        cv::namedWindow("range image");
231                        cv::imshow("range image", t2);
232                        cv::waitKey(3);
233                       
234                        // convert to cv::Mat
235                        cv::Mat rangeMat(tmp);         
236
237                        // copy and publish
238                        cv_bridge::CvImage cvres_out;
239                        cvres_out.header = cloud_msg->header;
240                        //cout<< cvres_out.header;
241                        cvres_out.encoding = enc::TYPE_32FC1;
242                        cvres_out.image = rangeMat;
243                        publisher_img.publish(cvres_out.toImageMsg());
244
245                        // release memory
246                        cvReleaseImage(&tmp);
247                        cvReleaseImage(&t2);
248                }
249*/
250                       
251        }
252
253        // Main function to process kinect depth messages
254        void ProcessDepthMessage()
255        {
256                if (!kinect_depthimg_msg || kinect_depthimg_msg == old_kinect_depthimg_msg)
257                        return;
258                old_kinect_depthimg_msg = kinect_depthimg_msg;
259
260                // convert message from ROS to openCV
261                cv_bridge::CvImagePtr cv_depthptr;
262                try
263                {
264                        cv_depthptr = cv_bridge::toCvCopy(kinect_depthimg_msg, enc::TYPE_32FC1);
265                }
266                catch (cv_bridge::Exception& e)
267                {
268                        ROS_ERROR("cv_bridge exception: %s", e.what());
269                        return;
270                }
271
272                // convert to IplImage
273                IplImage ipl_depth = cv_depthptr->image;
274        /*     
275                //clean up the depth image - no negatives or NAN
276                IplImage* ipl_depth_clean = cvCreateImage(cvGetSize(&ipl_depth), IPL_DEPTH_32F,1);
277                double rval_tmp;
278                for(int p_x = 0; p_x < ipl_depth.width; p_x++)
279                  for(int p_y = 0; p_y < ipl_depth.height; p_y++)
280                  {
281                    rval_tmp = cvGet2D(&ipl_depth, p_y,p_x).val[0];
282                    if(rval_tmp > 0.0f){
283                        cvSet2D(ipl_depth_clean, p_y, p_x, cvScalar(rval_tmp));
284                    }
285                    else{
286                        cvSet2D(ipl_depth_clean, p_y, p_x, cvScalar(0.0));
287                    }
288                  }
289        */
290
291
292                 // filter depth image - no negatives or NAN
293                cv::Mat depth_clean(cv_depthptr->image.rows, cv_depthptr->image.cols, CV_32FC1);
294                cv::Mat img(cv_depthptr->image.rows, cv_depthptr->image.cols, CV_8UC1);
295                for(int i = 0; i < cv_depthptr->image.rows; i++)
296                {
297                        float* Di = cv_depthptr->image.ptr<float>(i);
298                        float* Ii = depth_clean.ptr<float>(i);
299                        char* Ivi = img.ptr<char>(i);
300                                for(int j = 0; j < cv_depthptr->image.cols; j++)
301                                {   
302                                   if(Di[j] > 0.0f){                                   
303                                        Ii[j] = Di[j];
304                                        Ivi[j] = (char) (255*((Di[j])/(5.5))); // some suitable values..
305                                   }
306                                    else{
307                                        Ii[j] = 0.0f;
308                                        Ivi[j] = 0;
309                                    }
310                                }   
311                }
312
313                // copy and publish depth image
314                cv_bridge::CvImage cvres_out;
315                cvres_out.header = cv_depthptr->header;
316                //cout<< cvres_out.header;
317                cvres_out.encoding = enc::TYPE_32FC1;
318                cvres_out.image = depth_clean;
319                publisher_img.publish(cvres_out.toImageMsg());
320
321                // display
322                cv::imshow("range image", img);
323                cv::waitKey(3);
324
325               
326        }
327
328        // Filter Point Cloud
329        void FilterPointCloudZ(PointCloud<PointXYZRGB>::Ptr& pc, PointCloud<PointXYZRGB>::Ptr& pc_f, float minZ, float maxZ)
330        {
331                PassThrough<PointXYZRGB> filt;
332                filt.setInputCloud(pc);
333                filt.setFilterFieldName("z"); // filter z dimension
334                filt.setFilterLimits(minZ, maxZ);
335                filt.setKeepOrganized(true); // Important: to keep the "image-structure" after filtering, if not it becomes 1D (and sparse)
336                filt.filter(*pc_f);             
337        }
338
339        // Visualize Point Clouds
340        void VisualizePointCloud(PointCloud<PointXYZRGB>::Ptr& pc)
341        {
342                viewer->removePointCloud ("range image cloud");
343               
344                if(viz_type == 0)
345                {
346                       
347                        range_image->createFromPointCloud(*pc, angular_resolution, deg2rad(360.0f), deg2rad(180.0f),
348                                                sensor_pose, coordinate_frame, noise_level, min_range, border_size);
349                               
350                        // display DEPTH point cloud
351                        pcl_visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> color_handler_cloud(*range_image,200, 200, 200);
352                        viewer->addPointCloud (*range_image, color_handler_cloud, "range image cloud");
353
354                }
355                else
356                {
357
358                        // display RGB point cloud
359                        pcl_visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> color_handler_cloud(*pc);
360                        viewer->addPointCloud (*pc, color_handler_cloud, "range image cloud");
361
362                        // get a sample data point from the point cloud
363                        PointXYZRGB tpoint;                     
364                        tpoint = pc->at(200,200); // sample image point
365                        std::vector<uint8_t> rgb_t(4); // hold 4 elements
366                        extract_pcl_rgb(tpoint, rgb_t);
367                        // x, y, z in meters. x right, y down, z forward from camera center, r, g, b:(1 - 255)
368                        ROS_INFO("(x,y,z)=%g,%g,%g, (r,g,b)=%d,%d,%d", tpoint.x, tpoint.y, tpoint.z, rgb_t[0],rgb_t[1],rgb_t[2]);
369                       
370
371                }       
372        }
373       
374        // function to extract rgb stored in pcl PointXYZRGB
375        void extract_pcl_rgb(pcl::PointXYZRGB& point_t, std::vector<uint8_t>& rgb_v)
376        {
377                // extract color values
378                uint32_t rgb_val_;
379                memcpy(&rgb_val_, &(point_t.rgb), sizeof(float));
380
381                uint8_t garbage_ = (uint8_t)((rgb_val_ >> 24) & 0x000000ff);
382                uint8_t r_ = (uint8_t)((rgb_val_ >> 16) & 0x000000ff);
383                uint8_t g_ = (uint8_t)((rgb_val_ >> 8) & 0x000000ff);
384                uint8_t b_ = (uint8_t)((rgb_val_) & 0x000000ff);
385
386                // save to rgb vector
387                rgb_v[0]=r_;
388                rgb_v[1]=g_;
389                rgb_v[2]=b_;
390                rgb_v[3]= garbage_;
391
392        }
393
394        void ProcessCommandLine(int argc, char** argv)
395        {
396                 for (char c; (c = getopt (argc, argv, "t:hc:r:")) != -1; ) {
397                    switch (c) {
398                      case 'c':
399                        coordinate_frame = RangeImage::CoordinateFrame (strtol (optarg, NULL, 0));
400                        break;
401                      case 'r':
402                      {
403                        angular_resolution = strtod (optarg, NULL);
404                        cout << PVARN(angular_resolution);
405                        break;
406                      }
407                      case 't':
408                      {
409                        viz_type = strtol (optarg, NULL, 0);
410                        if (viz_type < 0  ||  viz_type > 2)
411                        {
412                          cout << "VizType "<<viz_type<<" is unknown.\n";
413                          exit (0);
414                        }
415                        if (viz_type != 2)
416                                cout << "Receiving "<<(viz_type==0 ? "depth point clouds" : "depth+RGB point clouds")<<".\n";
417                        else
418                                cout << "Processing Depth image only.\n";
419                        break;
420                      }
421                      case 'h':
422                      default:
423                        printUsage (argv[0]);
424                        exit (0);
425                    }
426                  }
427        }       
428
429        void printUsage (const char* progName)
430        {
431          cout << "\n\nUsage: "<<progName<<" [options] input:=<yourInput>\n\n"
432               << "Options:\n"
433               << "-------------------------------------------\n"
434               << "-c <int>     source coordinate frame (default "<<coordinate_frame<<")\n"
435               << "-t           0 - 3D points, 1 - 3D+RGB points (default), 2 - depth image only.\n"
436               << "-r <float>   angular resolution in degrees (default "<<angular_resolution<<")\n"
437               << "-h           this help\n"
438               << "\n\n";
439        }
440};
441
442int main(int argc, char** argv)
443{
444
445  ros::init (argc, argv, "umd_processKinectRange2");
446  ProcessKinectRange kc(argc, argv);
447  ros::spin();
448  return 0;
449 
450}