handle_detector_nan.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: transform_pointcloud.cpp 30719 2010-07-09 20:28:41Z rusu $
00035  *
00036  */
00037 
00046 // ROS core
00047 #include <ros/ros.h>
00048 
00049 //pcl
00050 #include "pcl/common/common.h"
00051 #include "pcl/io/pcd_io.h"
00052 #include "pcl/point_types.h"
00053 #include <pcl/features/normal_3d.h>
00054 #include <tf/transform_listener.h>
00055 #include "pcl/sample_consensus/method_types.h"
00056 #include "pcl/sample_consensus/model_types.h"
00057 #include <pcl/segmentation/sac_segmentation.h>
00058 #include "pcl/filters/extract_indices.h"
00059 #include <pcl/ros/conversions.h>
00060 #include <pcl/common/angles.h>
00061 #include "pcl/segmentation/extract_clusters.h"
00062 #include <pcl/surface/convex_hull.h>
00063 #include "pcl_ros/transforms.h"
00064 //pcl_ros
00065 #include <pcl_ros/publisher.h>
00066 //opencv
00067 #include "opencv2/highgui/highgui.hpp"
00068 #include "opencv2/imgproc/imgproc.hpp"
00069 //bullet
00070 #include "LinearMath/btVector3.h"
00071 //3D-2D projection
00072 #include <sensor_msgs/CameraInfo.h>
00073 #include <image_geometry/pinhole_camera_model.h>
00074 //for marker pose estimation
00075 #include "visualization_msgs/Marker.h"
00076 //local service
00077 #include <handle_detection2D/getHandlesNAN.h>
00078 
00079 //for find function
00080 #include <algorithm>
00081 
00082 typedef pcl::PointXYZRGB PointT;
00083 typedef pcl::PointCloud<PointT> PointCloud;
00084 typedef PointCloud::Ptr PointCloudPtr;
00085 typedef PointCloud::Ptr PointCloudPtr;
00086 typedef PointCloud::ConstPtr PointCloudConstPtr;
00087 typedef pcl::KdTree<PointT>::Ptr KdTreePtr;
00088 
00089 //for find function
00090 #include <algorithm>
00091 using namespace std;
00092 using namespace cv;
00093 class HandleDetectorNANNode
00094 {
00095 protected:
00096   ros::NodeHandle nh_;
00097 
00098 public:
00099   string output_cloud_topic_, input_cloud_topic_, output_marker_topic_;
00100 
00101   ros::Subscriber sub_;
00102   ros::ServiceServer service_;
00103   ros::Publisher marker_pub_;
00104   pcl_ros::Publisher<PointT> pub_;
00105   sensor_msgs::PointCloud2 output_cloud_;
00106 
00107   Mat nan_image, dst, canny_img, roi_image;
00108   pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcl_cloud_;
00109   pcl::PointCloud<pcl::PointXYZRGB>::Ptr biggest_plane_cloud_;
00110   pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> n3d_;   //Normal estimation
00111   KdTreePtr normals_tree_, clusters_tree_;
00112   pcl::KdTree<pcl::PointXYZ>::Ptr handles_tree_;
00113   pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg_;               // Planar segmentation object
00114   pcl::EuclideanClusterExtraction<PointT> cluster_;
00115   pcl::EuclideanClusterExtraction<pcl::PointXYZ> h_cluster_;
00116   double eps_angle_, seg_prob_, sac_distance_, normal_distance_weight_, base_link_kinect_head_rgb_optical_frame_;
00117   double handle_length_, x_handle_position_offset_, z_handle_position_;
00118   int k_, max_iter_, min_table_inliers_, nr_cluster_;
00119   double plane_cluster_tolerance_;
00120   int plane_cluster_min_size_, plane_cluster_max_size_;
00121   tf::TransformListener tf_listener_;
00122   bool save_image_, save_cloud_;
00123   sensor_msgs::CameraInfoConstPtr cam_info;
00124   sensor_msgs::ImageConstPtr image_ptr;
00125   image_geometry::PinholeCameraModel cam_model_;
00126   pcl::PCDWriter pcd_writer_;
00127   pcl::ConvexHull<pcl::PointXYZ> chull_;
00128   std::string kinect_rgb_optical_frame_;
00130   HandleDetectorNANNode  (ros::NodeHandle &n) : nh_(n)
00131   {
00132     // Maximum number of outgoing messages to be queued for delivery to subscribers = 1
00133     nh_.param("input_cloud_topic", input_cloud_topic_, std::string("/camera/rgb/points"));
00134     output_cloud_topic_ = input_cloud_topic_ + "_plane";
00135 //    nh_.param("to_frame", to_frame_, std::string("base_link"));
00136     
00137     nh_.param("output_marker_topic", output_marker_topic_, std::string("handle_centroid"));
00138     marker_pub_ = nh_.advertise<visualization_msgs::Marker>(output_marker_topic_, 0 );
00139 
00140     //    sub_ = nh_.subscribe (input_cloud_topic_, 1,  &HandleDetectorNANNode::cloud_cb, this);
00141     service_ = nh_.advertiseService("get_handles_nan", &HandleDetectorNANNode::cloud_cb, this);
00142     ROS_INFO ("[%s:] Listening for incoming data on topic %s", getName ().c_str (), nh_.resolveName (input_cloud_topic_).c_str ());
00143     pub_.advertise (nh_, output_cloud_topic_, 1);
00144     ROS_INFO ("[%s:] Will be publishing data on topic %s.", getName ().c_str (), nh_.resolveName (output_cloud_topic_).c_str ());
00145     nan_image = Mat::zeros (480, 640, CV_8UC1);
00146     roi_image = Mat::zeros (480, 640, CV_8UC1);
00147     pcl_cloud_.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
00148     biggest_plane_cloud_.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
00149     //3D handle length
00150     nh_.param("handle_length", handle_length_, 0.1);
00151     //for normal search
00152     nh_.param("k", k_, 30);
00153     nh_.param("save_image", save_image_, false);
00154     nh_.param("save_cloud", save_cloud_, false);
00155     normals_tree_ = boost::make_shared<pcl::KdTreeFLANN<PointT> > ();
00156     n3d_.setKSearch (k_);
00157     n3d_.setSearchMethod (normals_tree_);
00158     //plane extraction
00159     nh_.param("sac_distance", sac_distance_, 0.02);
00160     nh_.param("normal_distance_weight", normal_distance_weight_, 0.05);
00161     nh_.param("max_iter", max_iter_, 500);
00162     nh_.param("eps_angle", eps_angle_, 15.0);
00163     nh_.param("seg_prob", seg_prob_, 0.99);
00164     nh_.param("min_table_inliers", min_table_inliers_, 15000);
00165     nh_.param("nr_cluster", nr_cluster_, 1);
00166     seg_.setDistanceThreshold (sac_distance_);
00167     seg_.setMaxIterations (max_iter_);
00168     seg_.setNormalDistanceWeight (normal_distance_weight_);
00169     seg_.setOptimizeCoefficients (true);
00170     seg_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
00171     seg_.setEpsAngle(pcl::deg2rad(eps_angle_));
00172     seg_.setMethodType (pcl::SAC_RANSAC);
00173     seg_.setProbability (seg_prob_);
00174     //clustering of planes
00175     nh_.param("plane_cluster_tolerance", plane_cluster_tolerance_, 0.03);
00176     nh_.param("plane_cluster_min_size", plane_cluster_min_size_, 200);
00177     cluster_.setClusterTolerance (plane_cluster_tolerance_);
00178     cluster_.setSpatialLocator(0);
00179     cluster_.setMinClusterSize (plane_cluster_min_size_);
00180     //    cluster_.setMaxClusterSize (object_cluster_max_size_);
00181     clusters_tree_ = boost::make_shared<pcl::KdTreeFLANN<PointT> > ();
00182     clusters_tree_->setEpsilon (1);
00183     cluster_.setSearchMethod (clusters_tree_);
00184 
00185     //clustering of handles in NAN cloud
00186     handles_tree_ = boost::make_shared<pcl::KdTreeFLANN<pcl::PointXYZ> > ();
00187     handles_tree_->setEpsilon (1);
00188     h_cluster_.setSearchMethod (handles_tree_);
00189     h_cluster_.setClusterTolerance (0.02);
00190     h_cluster_.setSpatialLocator(0);
00191     h_cluster_.setMinClusterSize (500);
00192     h_cluster_.setMaxClusterSize (4000);
00193 
00194     //ADA handle offset from the supporting plane
00195     nh_.param("x_handle_position_offset", x_handle_position_offset_, 0.03);
00196     //expected height of handle in base_link
00197     nh_.param("z_handle_position", z_handle_position_, 0.75);
00198     nh_.param("kinect_rgb_optical_frame", kinect_rgb_optical_frame_, std::string("/openni_rgb_optical_frame"));
00199   }
00200 
00202   // lineLength2D ()
00203   double lineLength2D (int x1, int y1, int x2, int y2)
00204     {
00205       double dist  = sqrt ((x1-x2) * (x1-x2) + (y1-y2) * (y1-y2));
00206       return dist;
00207     }
00208   double lineLength2D (double x1, double y1, double x2, double y2)
00209     {
00210       double dist  = sqrt ((x1-x2) * (x1-x2) + (y1-y2) * (y1-y2));
00211       return dist;
00212     }
00213 
00215 
00216   std::string getName () const { return ("HandleDetectorNAN"); }
00217 
00218   int findBiggestPlane(pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcl_cloud, Rect & roi, pcl::PointIndices & indices_biggest_plane)
00219   {
00220     //transform point cloud into base_link
00221     bool found_transform = tf_listener_.waitForTransform(pcl_cloud->header.frame_id, "base_link",
00222                                                          pcl_cloud->header.stamp, ros::Duration(1.0));
00223     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_temp (new pcl::PointCloud<pcl::PointXYZRGB>());
00224     if (found_transform)
00225     {
00226       //ROS_ASSERT_MSG(found_transform, "Could not transform to camera frame");
00227       tf::StampedTransform transform;
00228       tf_listener_.lookupTransform("base_link", pcl_cloud->header.frame_id, pcl_cloud->header.stamp, transform);
00229       pcl_ros::transformPointCloud(*pcl_cloud, *cloud_temp, transform);
00230       pcl_cloud = cloud_temp;
00231       pcl_cloud->header.frame_id = "base_link";
00232       pcl_cloud->header.stamp = ros::Time::now();
00233     }
00234     else
00235       {
00236         ROS_INFO("[%s:] No transform found between %s and base_link", getName().c_str(), pcl_cloud->header.frame_id.c_str());
00237         return -1;
00238       }
00239       //Estimate Point Normals
00240       pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>());
00241       n3d_.setInputCloud (pcl_cloud);
00242       n3d_.compute (*cloud_normals);
00243 
00244       //find transform between base_link and kinect_head_rgb_optical_frame frames
00245       // bool found_transform = tf_listener_.waitForTransform("kinect_head_rgb_optical_frame", "base_link",
00246       //                                                      pcl_cloud->header.stamp, ros::Duration(1.0));
00247       // tf::StampedTransform transform;
00248       // if (found_transform)
00249       // {
00250       //   tf_listener_.lookupTransform("kinect_head_rgb_optical_frame", "base_link", pcl_cloud->header.stamp, transform);
00251       //   double yaw, pitch, roll;
00252       //   transform.getBasis().getEulerZYX(yaw, pitch, roll);
00253       //   ROS_INFO("[%s:] Transform X: %f Y: %f Z: %f R: %f P: %f Y: %f", getName().c_str(), transform.getOrigin().getX(), 
00254       //            transform.getOrigin().getY(), transform.getOrigin().getZ(), roll, pitch, yaw);
00255       //   base_link_kinect_head_rgb_optical_frame_ = pitch;
00256       // }
00257       // else
00258       // {
00259       //   ROS_INFO("[%s:] No transform found between %s and base_link", getName().c_str(), pcl_cloud->header.frame_id.c_str());
00260       //   return -1;
00261       // }
00262 
00263       //Segment the biggest furniture_face plane
00264       //z axis in Kinect frame
00265       btVector3 axis(1.0, 0.0, 0.0);
00266       //rotate axis around x in Kinect frame for an angle between base_link and kinect_head_rgb_optical_frame
00267       //btVector3 axis2 = axis.rotate(btVector3(1.0, 0.0, 0.0), -base_link_kinect_head_rgb_optical_frame_);
00268       //ROS_INFO_STREAM("[" << getName() << ":]" <<" axis: " << axis2.getX() << " " << axis2.getY() << " " 
00269       //              << axis2.getZ());
00270       seg_.setAxis (Eigen::Vector3f(axis.getX(), axis.getY(), axis.getZ()));
00271       pcl::ModelCoefficients::Ptr table_coeff (new pcl::ModelCoefficients ());
00272       pcl::PointIndices::Ptr table_inliers (new pcl::PointIndices ());
00273       seg_.setInputCloud (pcl_cloud);
00274       seg_.setInputNormals (cloud_normals);
00275       seg_.segment (*table_inliers, *table_coeff);
00276       ROS_INFO ("[%s] Cabinet face model: [%f, %f, %f, %f] with %d inliers.", getName ().c_str (), 
00277                 table_coeff->values[0], table_coeff->values[1], table_coeff->values[2], table_coeff->values[3], 
00278                 (int)table_inliers->indices.size ());
00279 
00280       if ((int)table_inliers->indices.size () <= min_table_inliers_)
00281       {
00282         ROS_ERROR ("[%s:] Cabinet face has to few inliers", getName().c_str());
00283         return -1;
00284       }
00285 
00286       //Extract the biggest cluster correponding to above inliers
00287       std::vector<pcl::PointIndices> clusters;
00288       cluster_.setInputCloud (pcl_cloud);
00289       cluster_.setIndices(table_inliers);
00290       cluster_.extract (clusters);
00291 
00292       ROS_INFO("[%s:] Found clusters %ld ", getName().c_str(), clusters.size());
00293       PointCloudPtr biggest_face (new PointCloud());
00294       if (int(clusters.size()) >= nr_cluster_)
00295       {
00296         for (int i = 0; i < nr_cluster_; i++)
00297         {
00298           pcl::copyPointCloud (*pcl_cloud, clusters[i], *biggest_face);
00299           indices_biggest_plane = clusters[i];
00300         }
00301       }
00302       else
00303       {
00304         ROS_ERROR("[%s:] Only %ld clusters found with size > %d points", getName().c_str(), clusters.size(), 
00305                   plane_cluster_min_size_);
00306         return -1;
00307       }
00308       ROS_INFO("[%s:] Found biggest face with %ld points", getName().c_str(), biggest_face->points.size());
00309 
00310       biggest_plane_cloud_->header = biggest_face->header = pcl_cloud->header;
00311       biggest_plane_cloud_ = biggest_face;
00312 
00313 //       //getMinMax
00314 //       double min_x, min_y, max_x, max_y;
00315 //       // min_x = min_y = numeric_limits<double>::max();
00316 //       min_x = min_y = 1000.0;
00317 //       max_x = max_y = -1000.0;
00318       
00319 //       size_t in_min_x, in_min_y, in_max_x, in_max_y;
00320 //       in_min_x = in_min_y = in_max_x = in_max_y = 0;
00321 //       for (size_t i = 0; i < clusters[0].indices.size(); ++i)
00322 //       {
00323 //         if (pcl_cloud->points[clusters[0].indices[i]].x > max_x)
00324 //         {
00325 //           max_x = pcl_cloud->points[clusters[0].indices[i]].x;
00326 //           in_max_x = clusters[0].indices[i];
00327 //         }
00328 //         if (pcl_cloud->points[clusters[0].indices[i]].x < min_x)
00329 //         {
00330 //           min_x = pcl_cloud->points[clusters[0].indices[i]].x;
00331 //           in_min_x = clusters[0].indices[i];
00332 //         }
00333 //         if (pcl_cloud->points[clusters[0].indices[i]].y < min_y)
00334 //         {
00335 //           min_y = pcl_cloud->points[clusters[0].indices[i]].y;
00336 //           in_min_y = clusters[0].indices[i];
00337 //         }
00338 //         if (pcl_cloud->points[clusters[0].indices[i]].y > max_y)
00339 //         {
00340 //           max_y = pcl_cloud->points[clusters[0].indices[i]].y;
00341 //           in_max_y = clusters[0].indices[i];
00342 //         }
00343 //       }
00344 //       ROS_INFO_STREAM(getName() << " min x, y: " << min_x << " " << min_y);
00345 //       ROS_INFO_STREAM(getName() << " max x, y: " << max_x << " " << max_y);
00346 
00347 
00348 //       ROS_INFO_STREAM(getName() << " in min x, y: " << in_min_x << " " << in_min_y);
00349 //       ROS_INFO_STREAM(getName() << " in max x, y: " << in_max_x << " " << in_max_y);
00350 
00351       
00352 //       int x1 = 0;
00353 //       int y1 = 0;
00354 //       int x2 = 0;
00355 //       int y2 = 0;
00356 //       for (size_t i = 0; i <  pcl_cloud->height; ++i)
00357 //       {
00358 //         // go over rows          
00359 //         for (size_t j = 0; j < pcl_cloud->width; ++j)
00360 //         {
00361 //           if ((i * 640 + j) == in_min_x) 
00362 //             x1 = j;
00363 //           if ((i * 640 + j) == in_min_y) 
00364 //             y1 = i;
00365 //           if ((i * 640 + j) == in_max_x) 
00366 //             x2 = j;
00367 //           if ((i * 640 + j) == in_max_y) 
00368 //             y2 = i;
00369 //         }
00370 //       }
00371 // //      ROS_INFO_STREAM(getName() << " coordinates x1, y1, x2, y2: " << x1 << " " << y1 << " " << x2 << " " << y2);
00372 //       roi.x = x1;
00373 //       roi.y = y1;
00374 //       roi.width = x2 - x1;
00375 //       roi.height = y2 - y1;
00376       //For Debug
00377       pub_.publish(*biggest_face);
00378       ROS_INFO_STREAM(getName() << " Published biggest face with " << biggest_face->points.size() << " points.");
00379       return 0;  
00380     }
00381 
00383   // project3DPointsToPixels (!)
00384   void project3DPointsToPixels (std::vector <cv::Point2d> & projected_points)
00385   {
00386     projected_points.resize(0);
00387     //project point cloud to image
00388     cam_info = ros::topic::waitForMessage<sensor_msgs::CameraInfo>("/kinect_head/camera/rgb/camera_info");
00389     cam_model_.fromCameraInfo(cam_info);
00390 
00391     for (unsigned long i = 0; i < biggest_plane_cloud_->points.size(); i++)
00392       {
00393         cv::Point3d pt_cv(biggest_plane_cloud_->points[i].x, biggest_plane_cloud_->points[i].y,
00394                           biggest_plane_cloud_->points[i].z);
00395         cv::Point2d uv;
00396         cam_model_.project3dToPixel(pt_cv, uv);
00397         projected_points.push_back(uv);
00398         ROS_DEBUG_STREAM("points projected: " << round(uv.x) << " " << round(uv.y));
00399       }
00400   }
00402   // drawNANs (!)
00403   void drawNANs(cv::Mat & nan_image, const Rect roi, double padding = 0.0)
00404   {
00405       // go over cols
00406     for (size_t i = 0; i <  pcl_cloud_->height; i++)
00407     {
00408       // go over rows          
00409       for (size_t j = 0; j < pcl_cloud_->width; j++)
00410       {
00411         if (isnan (pcl_cloud_->points[i * 640 + j].x) ||
00412             isnan (pcl_cloud_->points[i * 640 + j].y) ||
00413             isnan (pcl_cloud_->points[i * 640 + j].z))
00414         {
00415           // if ((int)j > (roi.x + roi.width * padding) && (int)j < (roi.x + roi.width) && 
00416           //     (int)i > (roi.y - roi.width * padding) && (int)i < (roi.y + roi.height))
00417             nan_image.at <uint8_t>(i, j) = 255;
00418         }
00419       }
00420     }
00421   }
00422 
00423  void drawROI(cv::Mat & nan_image, pcl::PointIndices & roi_indices)
00424   {
00425       // go over cols
00426     for (size_t i = 0; i <  pcl_cloud_->height; i++)
00427     {
00428       // go over rows          
00429       for (size_t j = 0; j < pcl_cloud_->width; j++)
00430         {
00431           vector<int>::iterator it = find(roi_indices.indices.begin(), roi_indices.indices.end(), i * 640 + j);
00432             if (it != roi_indices.indices.end()) 
00433            {
00434               // found it
00435               //if (isnan (pcl_cloud_->points[i * 640 + j].x) ||
00436                   // isnan (pcl_cloud_->points[i * 640 + j].y) ||
00437                   // isnan (pcl_cloud_->points[i * 640 + j].z))
00438                 //      {
00439                   // if ((int)j > (roi.x + roi.width * padding) && (int)j < (roi.x + roi.width) && 
00440                   //     (int)i > (roi.y - roi.width * padding) && (int)i < (roi.y + roi.height))
00441                   nan_image.at <uint8_t>(i, j) = 255;
00442                   //    }
00443               } 
00444                else 
00445                {
00446               //does not exist
00447               continue;
00448                }
00449       }
00450     }
00451   }
00452 
00453   void findLines (cv::Mat & nan_image, vector<Vec4i> & lines)
00454   {
00455     int min_line_length = 30;
00456     int max_line_gap = 10;
00457     HoughLinesP(nan_image, lines, 1, CV_PI/180, 100, min_line_length, max_line_gap );
00458   }
00459 
00460     void erodeDilate (cv::Mat & nan_image)
00461   {
00462     int dilate_iter = 5;
00463     int erode_iter = 3;
00464     dilate(nan_image, nan_image, Mat(), Point(-1,-1), dilate_iter);
00465     erode(nan_image, nan_image, Mat(), Point(-1,-1), erode_iter);
00466     dilate(nan_image, nan_image, Mat(), Point(-1,-1), dilate_iter);
00467     erode(nan_image, nan_image, Mat(), Point(-1,-1), erode_iter);
00468   }
00469 
00470   void drawLines(const cv::Mat & nan_image, cv::Mat & dst, const vector<Vec4i> & lines, int max_line_length)
00471   {
00472     cvtColor(nan_image, dst, CV_GRAY2BGR);
00473     for( size_t i = 0; i < lines.size(); i++ )
00474       {
00475         Vec4i l = lines[i];
00476         float length = lineLength2D (l[0], l[1], l[2], l[3]);
00477         //float angle = atan2 (l[3] - l[1], l[2] - l[0]) * 180 / CV_PI;
00478         //if (length < 180 && (((80.0 < angle) && (angle < 110.0)) || ((-1.0 < angle) && (angle < 1.0))))
00479         //{
00480         cv::Scalar color(std::rand() % 256, std::rand() % 256, std::rand() % 256);
00481         if (length < max_line_length)
00482           line(dst, Point(l[0], l[1]), Point(l[2], l[3]), color, 3, CV_AA);
00483         //TODO:
00484         //calculate orientation and check for it
00485         //cerr << "angle: " << angle << endl;
00486         //cerr << "length: " << length << endl;
00487         //}
00488         //TODO: cluster lines and compute average x and y
00489       }
00490   }
00491 
00493   // drawProjectedPoints (!)
00494   void drawProjectedPoints(std::vector <cv::Point2d> & projected_points, cv::Mat & nan_image, cv::Mat & dst)
00495   {
00496     cvtColor(nan_image, dst, CV_GRAY2BGR);
00497     for (uint i = 0; i < projected_points.size(); i++)
00498       circle(dst, cvPoint(projected_points[i].x,  projected_points[i].y), 5, cvScalar(0, 255, 0));
00499   }
00500 
00502   // createNaNPointCloud (!)
00503   void createNaNPointCloud (cv::Mat & nan_image, pcl::PointCloud<pcl::PointXYZ>::Ptr nan_cloud)
00504   {
00505     for (int i = 0; i < nan_image.rows; i++)
00506       for (int j = 0; j < nan_image.cols; j++)
00507         {
00508           if (nan_image.at<uint8_t>(i, j) == 255)
00509             {
00510               pcl::PointXYZ p;
00511               p.x = (double)j/1000.0;
00512               p.y = (double)i/1000.0;
00513               p.z = 0.0;
00514               nan_cloud->points.push_back(p);
00515             }
00516         }
00517   }
00518 
00520   // createNaNPointCloud (!)
00521   void findHandlesInNanPointCloud (pcl::PointCloud<pcl::PointXYZ>::Ptr nan_cloud, std::vector<pcl::PointIndices> & handle_clusters)
00522   {
00523     h_cluster_.setInputCloud (nan_cloud);
00524     //      cluster_.setIndices(table_inliers);
00525     h_cluster_.extract (handle_clusters);
00526     ROS_INFO("%s Found %ld handle candidates", getName().c_str(), handle_clusters.size());
00527   }
00528     
00530   // saveHandleClusters (!)
00531   void saveHandleClusters (std::vector<pcl::PointIndices> & handle_clusters, pcl::PointCloud<pcl::PointXYZ>::Ptr nan_cloud)
00532   {
00533     pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
00534     for (uint i = 0; i < handle_clusters.size(); i++)
00535       {
00536         pcl::copyPointCloud (*nan_cloud, handle_clusters[i], *cluster_cloud);
00537         std::stringstream ss;
00538         ss << ros::Time::now() << ".pcd";
00539         ROS_INFO ("[%s:] Cluster cloud saved to %s with points %ld", getName().c_str(), ss.str ().c_str (), cluster_cloud->points.size());
00540         pcd_writer_.write(ss.str (), *cluster_cloud);
00541         cluster_cloud->points.clear();
00542         sleep(0.5);
00543       }
00544   }
00545 
00547   // recoverCentroidPose (!)
00548   void recoverCentroidPose(std::vector<pcl::PointIndices> & handle_clusters, pcl::PointCloud<pcl::PointXYZ>::Ptr nan_cloud,
00549                            std::vector<geometry_msgs::PointStamped> & vector_point_avg)
00550   {
00551     pcl::PointCloud<pcl::PointXYZ> cloud_hull;
00552     pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
00553     //chull_.setInputCloud (boost::make_shared<PointCloud> (cloud_projected));
00554     int count = 0;
00555     int count_hull_points = 0;
00556     //    std::vector<geometry_msgs::PointStamped> vector_point_avg;
00557     geometry_msgs::PointStamped point_avg;
00558     for (uint i = 0; i < handle_clusters.size(); i++)
00559       {
00560         //      chull_.setIndices (boost::make_shared<pcl::PointIndices> (handle_clusters[i]));
00561         pcl::copyPointCloud (*nan_cloud, handle_clusters[i], *cluster_cloud);
00562         chull_.setInputCloud(cluster_cloud);
00563         chull_.reconstruct (cloud_hull);
00564         point_avg.point.x = point_avg.point.y = point_avg.point.z = 0.0;
00565         cerr << "hull: " << count << endl;
00566         for (uint j = 0; j < cloud_hull.points.size(); j++)
00567           {
00568             // cerr << "x, y: " << cloud_hull.points[j].x * 1000 << " " << cloud_hull.points[j].y * 1000 << endl;
00569             // cerr << "p.x: " << pcl_cloud_->at(cloud_hull.points[j].x * 1000, cloud_hull.points[j].y * 1000).x << " ";
00570             // cerr << " p.y: " << pcl_cloud_->at(cloud_hull.points[j].x * 1000, cloud_hull.points[j].y * 1000).y << " ";
00571             // cerr << " p.z: " << pcl_cloud_->at(cloud_hull.points[j].x * 1000, cloud_hull.points[j].y * 1000).z << endl;
00572             if (!isnan (pcl_cloud_->at(cloud_hull.points[j].x * 1000, cloud_hull.points[j].y * 1000).x) ||
00573                 !isnan (pcl_cloud_->at(cloud_hull.points[j].x * 1000, cloud_hull.points[j].y * 1000).y) ||
00574                 !isnan (pcl_cloud_->at(cloud_hull.points[j].x * 1000, cloud_hull.points[j].y * 1000).x))
00575               {
00576                 point_avg.point.x = point_avg.point.x + pcl_cloud_->at(cloud_hull.points[j].x * 1000, cloud_hull.points[j].y * 1000).x;
00577                 point_avg.point.y = point_avg.point.y + pcl_cloud_->at(cloud_hull.points[j].x * 1000, cloud_hull.points[j].y * 1000).y;
00578                 point_avg.point.z = point_avg.point.z + pcl_cloud_->at(cloud_hull.points[j].x * 1000, cloud_hull.points[j].y * 1000).z;
00579                 count_hull_points++;
00580               }
00581           }
00582         if (count_hull_points != 0)
00583           {
00584             point_avg.point.x = point_avg.point.x/(double)count_hull_points;
00585             point_avg.point.y = point_avg.point.y/(double)count_hull_points;
00586             point_avg.point.z = point_avg.point.z/(double)count_hull_points;
00587             count_hull_points = 0;
00588             point_avg.header.frame_id = pcl_cloud_->header.frame_id;
00589             point_avg.header.stamp = ros::Time::now();
00590             vector_point_avg.push_back(point_avg);
00591           }
00592         // std::stringstream ss;
00593         // ss << "hull_" << count << ".pcd";
00594         // ROS_INFO ("[%s:] Hull cloud saved to %s", getName().c_str(), ss.str ().c_str ());
00595         // pcd_writer_.write(ss.str (), cloud_hull);
00596         // cloud_hull.points.clear();
00597         // cluster_cloud->points.clear();
00598         count++;
00599       }
00600     //    publishMarker (vector_point_avg);
00601   }
00602 
00603   void publishMarker (std::vector<geometry_msgs::PointStamped> & vector_point_avg)
00604   {
00605     visualization_msgs::Marker marker;
00606     for (uint i = 0; i < vector_point_avg.size(); i++)
00607       {
00608         marker.header.frame_id = vector_point_avg[i].header.frame_id;
00609         marker.header.stamp = ros::Time::now();
00610         marker.ns = "HandleCentroid";
00611         marker.id = i;
00612         marker.type = visualization_msgs::Marker::SPHERE;
00613         marker.action = visualization_msgs::Marker::ADD;
00614         marker.pose.position.x = vector_point_avg[i].point.x;
00615         marker.pose.position.y = vector_point_avg[i].point.y;
00616         marker.pose.position.z = vector_point_avg[i].point.z;
00617         marker.pose.orientation.x = 0;
00618         marker.pose.orientation.y = 0;
00619         marker.pose.orientation.z = 0;
00620         marker.pose.orientation.w = 1;
00621         marker.scale.x = .1;
00622         marker.scale.y = .1;
00623         marker.scale.z = .1;
00624         marker.color.a = 0.5;
00625         marker.color.r = 0.0;
00626         marker.color.g = 1.0;
00627         marker.color.b = 0.0;
00628         std::cerr << getName() << " MARKER COMPUTED, WITH FRAME " << marker.header.frame_id << std::endl;
00629         std::cerr << getName() << " Handle position " << vector_point_avg[i].point.x << " " << vector_point_avg[i].point.y
00630                   << " " << vector_point_avg[i].point.z << std::endl;
00631         marker_pub_.publish(marker);
00632       }
00633   }
00634 
00636   // getHandleImageLength (!)
00637   void getHandleImageLength (std::vector<geometry_msgs::PointStamped> & vector_point_avg,     
00638                              std::vector<double> & expected_handle_image_length)
00639   {
00640     cam_info = ros::topic::waitForMessage<sensor_msgs::CameraInfo>("/kinect_head/camera/rgb/camera_info");
00641     cam_model_.fromCameraInfo(cam_info);
00642 
00643     for (unsigned long i = 0; i < vector_point_avg.size(); i++)
00644       {
00645         cv::Point3d pt_cv1(0, handle_length_, vector_point_avg[i].point.z);
00646         cv::Point3d pt_cv2(0, 0.0, vector_point_avg[i].point.z);
00647         cv::Point2d uv1, uv2;
00648         cam_model_.project3dToPixel(pt_cv1, uv1);
00649         cam_model_.project3dToPixel(pt_cv2, uv2);
00650         cerr << getName() << "Z: " << vector_point_avg[i].point.z <<  " Line length expected: " << lineLength2D(uv1.x, uv1.y, uv2.x, uv2.y) << endl;
00651         expected_handle_image_length.push_back(lineLength2D(uv1.x, uv1.y, uv2.x, uv2.y));
00652       }
00653   }
00654 
00656     // rejectFalsePositives (!)
00657   void rejectFalsePositives (std::vector<geometry_msgs::PointStamped> & vector_point_avg)
00658   {
00659     // pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
00660     // for (uint i = 0; i < handle_clusters.size(); i++)
00661     //   {
00662     //  //      chull_.setIndices (boost::make_shared<pcl::PointIndices> (handle_clusters[i]));
00663     //  pcl::copyPointCloud (*nan_cloud, handle_clusters[i], *cluster_cloud);
00664     //  pcl::PointXYZ point_max, point_min;
00665     //  getMinMax3D (*cluster_cloud, point_min, point_max);
00666     //  point_min.x = point_max.x = 0.0;
00667     //  cerr << getName() << " point_min: " << point_min.x << " " << point_min.y << endl;
00668     //  cerr << getName() << " point_max: " << point_max.x << " " << point_max.y << endl;
00669     //  cerr << getName() << " Line length actual: " << 1000 * lineLength2D(point_min.x, point_min.y, point_max.x, point_max.y) << endl;
00670     //  double min_allowed_len = 1000 * lineLength2D(point_min.x, point_min.y, point_max.x, point_max.y) * 0.7;
00671     //  double max_allowed_len = 1000 * lineLength2D(point_min.x, point_min.y, point_max.x, point_max.y) * 1.3;
00672           
00673     //  if (expected_handle_image_length[i] > min_allowed_len && expected_handle_image_length[i] < max_allowed_len)
00674     //    {
00675     //      true_pos.push_back(i); 
00676     //    }
00677     //   }
00678     //substract the ADS offset and filter out the poses with the wrong Z
00679     std::vector<geometry_msgs::PointStamped> temp = vector_point_avg;
00680     vector_point_avg.clear();
00681     for (uint i = 0; i < temp.size(); i++)
00682       {
00683         if (((z_handle_position_ - 0.1) < temp[i].point.z) && ((z_handle_position_ + 0.1) > temp[i].point.z ))
00684           {
00685             temp[i].point.x -= x_handle_position_offset_;
00686             vector_point_avg.push_back (temp[i]);
00687           }
00688       }
00689   }
00690 
00691   void transformPoints (std::vector<geometry_msgs::PointStamped> & vector_point_avg, std::string frame="base_link")
00692   {
00693     for (uint i = 0; i < vector_point_avg.size(); i++)
00694       {
00695         bool found_transform = tf_listener_.waitForTransform(vector_point_avg[i].header.frame_id, frame,
00696                                                     vector_point_avg[i].header.stamp, ros::Duration(10.0));
00697         if (found_transform)
00698           {
00699             tf_listener_.transformPoint(frame, vector_point_avg[i], vector_point_avg[i]);
00700           }
00701         else
00702           {
00703             ROS_ERROR("No transform for point %d", i);
00704             return;
00705           }
00706       }
00707   }
00708     
00710   // cloud_cb (!)
00711   //  void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& pc)
00712   bool cloud_cb (handle_detection2D::getHandlesNAN::Request & req, 
00713                  handle_detection2D::getHandlesNAN::Response & res)
00714   {
00715     sensor_msgs::PointCloud2ConstPtr pc;
00716     pc = ros::topic::waitForMessage<sensor_msgs::PointCloud2> ("/kinect_head/camera/rgb/points");
00717     pcl::fromROSMsg(*pc, *pcl_cloud_);
00718     ROS_INFO_STREAM("[" << getName ().c_str () << ":] Got PC with height: width: " << pcl_cloud_->height << " " 
00719                     <<  pcl_cloud_->width << " frame: " << pcl_cloud_->header.frame_id);
00720     
00721     //image of nans
00722     nan_image = Mat::zeros (480, 640, CV_8UC1);
00723     //find roi from 3D plane
00724     Rect roi;
00725 
00726     pcl::PointIndices indices_biggest_plane;
00727     if (findBiggestPlane(pcl_cloud_, roi, indices_biggest_plane) < 0)
00728       return false;
00729     
00730     ROS_INFO_STREAM(getName() << " indices_biggest_plane with size:  " << indices_biggest_plane.indices.size());
00731     // ROS_INFO_STREAM(getName() << " coordinates x1, y1, x2, y2: " << roi.x << " " << roi.y << " " 
00732     //                 << roi.x + roi.width << " " << roi.y + roi.height);
00733 
00734     //project points to 3D
00735     std::vector <cv::Point2d> projected_points;
00736     //    project3DPointsToPixels (projected_points);
00737 
00738     //draw projected points
00739     // drawProjectedPoints (projected_points, nan_image, dst);
00740 
00741     //TODO: find convex contour and draw nan points only inside of it
00742 
00743     //TODO: average over 10 measurements
00744 
00745     //draw NANs on the image
00746     drawNANs(nan_image, roi);
00747     drawROI(roi_image, indices_biggest_plane);
00748 
00749     //drawHandles
00750     
00751     // circle(dst, cvPoint(roi.x,  roi.y), 7, cvScalar(255, 0, 0));
00752     // circle(dst, cvPoint(roi.x + roi.width,  roi.y+roi.height), 7, cvScalar(255, 0, 0));
00753 
00754     erodeDilate (nan_image);
00755     erodeDilate (roi_image);
00756     cv::Mat intersection;
00757     cv::bitwise_and(nan_image, roi_image, intersection);
00758     //     imshow("roi_image", roi_image);
00759     // waitKey();
00760     // imshow("nan_image", nan_image);
00761     //    imshow("intersection", intersection);
00762     //waitKey();
00763 
00764 
00765 
00766     //    cv::Canny(nan_image, canny_img, 50, 200, 3);
00767     //TODO: get the line length in image from the depth
00768 
00769     //vector<Vec4i> lines;
00770     //findLines(canny_img, lines);
00771 
00772     //int max_line_length = 100;
00773     //drawLines(canny_img, dst, lines, max_line_length);
00774     
00775     //createNaN point cloud
00776     pcl::PointCloud<pcl::PointXYZ>::Ptr nan_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
00777     createNaNPointCloud (intersection, nan_cloud);
00778 
00779     std::vector<pcl::PointIndices> handle_clusters;
00780     findHandlesInNanPointCloud (nan_cloud, handle_clusters);
00781     //saveHandleClusters (handle_clusters, nan_cloud);
00782 
00783     std::vector<geometry_msgs::PointStamped> vector_point_avg;
00784     recoverCentroidPose(handle_clusters, nan_cloud, vector_point_avg);
00785 
00786     transformPoints(vector_point_avg, "base_link");
00787 
00788     rejectFalsePositives(vector_point_avg);
00789     //response to service call
00790     for (uint i = 0; i < vector_point_avg.size(); i++)
00791       {
00792         res.handle_poses.push_back (vector_point_avg[i]);
00793       }
00794 
00795     //    std::vector<double> expected_handle_image_length;
00796     //    getHandleImageLength(vector_point_avg, expected_handle_image_length);
00797     
00798     // std::vector<int> true_pos;
00799     // rejectFalsePositives (handle_clusters, expected_handle_image_length, nan_cloud, true_pos);
00800     
00801     // std::vector<geometry_msgs::PointStamped> vector_point_avg_minus_fp;
00802     // for (uint i = 0; i < true_pos.size(); i++)
00803     //   {
00804     //  vector_point_avg_minus_fp.push_back(vector_point_avg[true_pos[i]]);
00805     //   }
00806     
00807     //publishMarker(vector_point_avg_minus_fp);
00808     publishMarker(vector_point_avg);
00809      if (save_cloud_)
00810       {
00811         std::stringstream ss;
00812         ros::Time time = ros::Time::now();
00813         ss << "original_" << time << ".pcd";
00814         ROS_INFO ("[%s:] Cloud saved to %s", getName().c_str(), ss.str ().c_str ());
00815         pcd_writer_.write(ss.str (), *pcl_cloud_);
00816         //pcd_writer_.write(ss.str (), *biggest_plane_cloud_);
00817         ss.str("");
00818         ss << "nan_cloud_" << time << ".pcd";
00819         ROS_INFO ("[%s:] Cloud saved to %s", getName().c_str(), ss.str ().c_str ());
00820         pcd_writer_.write(ss.str (), *nan_cloud);
00821       }
00822 
00823     if (save_image_)
00824       {
00825         std::stringstream ss;
00826         ss << ros::Time::now() << ".png";
00827         ROS_INFO ("[%s:] Image saved to %s", getName().c_str(), ss.str ().c_str ());
00828         imwrite(ss.str(), dst);
00829       }
00830 
00831     nan_cloud->points.clear();
00832     //    imshow("nan_image", nan_image);
00833     //    imshow("dst", dst);
00834     //waitKey(10);
00835     // sleep(10);
00836     return true;
00837   }
00838 };
00839 
00840 int main (int argc, char** argv)
00841 {
00842   ros::init (argc, argv, "handle_detector_nan");
00843   ros::NodeHandle n("~");
00844   HandleDetectorNANNode tp(n);
00845   ros::spin ();
00846   return (0);
00847 }
00848 
00849 /* ]--- */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


handle_detection2D
Author(s): Dejan Pangercic
autogenerated on Sun Oct 6 2013 12:07:08