global_rsd.h
Go to the documentation of this file.
00001 #ifndef CLOUD_ALGOS_GRSD_H
00002 #define CLOUD_ALGOS_GRSD_H
00003 
00004 #include <float.h>
00005 
00006 // Eigen
00007 //#include <Eigen/StdVector>
00008 //#include <Eigen/Array>
00009 
00010 //#include <ros/ros.h>
00011 //#include <sensor_msgs/PointCloud.h>
00012 #include <sensor_msgs/PointCloud2.h>
00013 //#include <sensor_msgs/point_cloud_conversion.h>
00014 #include "octomap/octomap.h"
00015 #include "pcl_to_octree/octree/OcTreePCL.h"
00016 #include "pcl_to_octree/octree/OcTreeNodePCL.h"
00017 #include "pcl_to_octree/octree/OcTreeServerPCL.h"
00018 //#include "octomap_server/octomap_server.h"
00019 #include "pcl/io/io.h"
00020 #include <pcl/io/pcd_io.h>
00021 #include <pcl_cloud_algos/pcl_cloud_algos_point_types.h>
00022 #include <visualization_msgs/MarkerArray.h>
00023 
00024 
00025 //pcl_publisher
00026 #include <pcl_ros/publisher.h>
00027 
00028 // Kd Tree
00029 #include "pcl/kdtree/tree_types.h"
00030 #include "pcl/kdtree/kdtree.h"
00031 #include "pcl/kdtree/kdtree_flann.h"
00032 #include "pcl/kdtree/organized_data.h"
00033 #include "pcl/features/feature.h"
00034 
00035 // Cloud algos
00036 #include <pcl_cloud_algos/cloud_algos.h>
00037 
00038 #define NR_CLASS 5
00039 // TODO: use a map to have surface labels and free space map to indices in the transitions matrix
00040 
00041 #define _sqr(c) ((c)*(c))
00042 #define _sqr_dist(a,b) ( _sqr(((a).x())-((b).x())) + _sqr(((a).y())-((b).y())) + _sqr(((a).z())-((b).z())) )
00043 
00044 namespace pcl_cloud_algos
00045 {
00046 
00047 struct IntersectedLeaf
00048 {
00049   double sqr_distance; // square distance from source node
00050   octomap::point3d centroid; // leaf center coordinates
00051 };
00052 
00053 inline bool
00054   histogramElementCompare (const std::pair<int, IntersectedLeaf> &p1, const std::pair<int, IntersectedLeaf> &p2)
00055 {
00056   return (p1.second.sqr_distance < p2.second.sqr_distance);
00057 }
00058 
00059 class GlobalRSD : public CloudAlgo
00060 {
00061  public:
00062 
00063   // Input/output type
00064   typedef sensor_msgs::PointCloud2 OutputType;
00065   typedef sensor_msgs::PointCloud2 InputType;
00066   typedef pcl::KdTree<pcl::PointNormal>::Ptr KdTreePtr;
00067   // Options
00068   int point_label_; // label of the object if known, and -1 otherwise
00069   double width_; // the width of the OcTree cells
00070   int step_; // specifies how many extra cells in each direction should contribute to local feature
00071   //int rsd_bins_; // number of divisions to create the RSD histogram
00072   int min_voxel_pts_; // minimum number of points in a cell to be processed
00073   bool publish_cloud_centroids_; // should we publish cloud_centroids_?
00074   bool publish_cloud_vrsd_; // should we publish cloud_vrsd_?
00075   //bool surface2curvature_; // should we write the results of the local surface classification as the curvature value in the partial results for visualization?
00076   double min_radius_plane_;
00077   double min_radius_edge_;
00078   double min_radius_noise_, max_radius_noise_;
00079   double max_min_radius_diff_;
00080   bool publish_octree_;
00081   // Intermediary results for convenient access
00082   pcl::PointCloud<pcl::PointNormal> cloud_centroids_; 
00083   pcl::PointCloud<pcl::PointNormal> cloud_vrsd_; 
00084   
00085 
00086   // Topic name to subscribe to
00087   static std::string default_input_topic ()
00088     {return std::string ("cloud_pcd");}
00089 
00090   // Topic name to advertise
00091   static std::string default_output_topic ()
00092     {return std::string ("cloud_grsd");};
00093 
00094   // Node name
00095   static std::string default_node_name () 
00096     {return std::string ("global_rsd_node");};
00097 
00098   // Algorithm methods
00099   void init (ros::NodeHandle&);
00100   void pre  ();
00101   void post ();
00102   std::vector<std::string> requires ();
00103   std::vector<std::string> provides ();
00104   std::string process (const boost::shared_ptr<const InputType>&);
00105   boost::shared_ptr<const OutputType> output ();
00106 
00107   struct QueryPoint : public pcl::PointNormal
00108   {
00109     QueryPoint(float x, float y, float z) {this->x=x; this->y=y; this->z=z;}
00110   };
00111 
00112 
00113   // Constructor-Destructor
00114   GlobalRSD () : CloudAlgo ()
00115   {
00116     point_label_ = -1;
00117     width_ = 0.03;
00118     step_ = 0;
00119     min_voxel_pts_ = 1;
00120     nr_bins_ = (NR_CLASS+1)*(NR_CLASS+2)/2;
00121     min_radius_plane_ = 0.045;
00122     min_radius_noise_ = 0.030, max_radius_noise_ = 0.050;
00123     max_min_radius_diff_ = 0.01;
00124     min_radius_edge_ = 0.030;
00125     publish_octree_ = false;
00126   }
00127   ~GlobalRSD ()
00128   {
00129   }
00130 
00131   ros::Publisher createPublisher (ros::NodeHandle& nh)
00132   {
00133     ros::Publisher p = nh.advertise<OutputType> (default_output_topic (), 5);
00134     return p;
00135   }
00136 
00138   // Compute the min and maximum variation of normal angles by distance and
00139   // estimates local minimum and maximum radius of surface curvature, then
00140   // sets a value defining the surface type
00142   // Surface type value:
00143   //    0 - noise/corner
00144   //    1 - planar
00145   //    2 - cylinder (rim)
00146   //    3 - circle (corner?)
00147   //    4 - edge
00148   inline int
00149     setSurfaceType (pcl::PointCloud<pcl::PointNormal> cloud, std::vector<int> *indices, std::vector<int> *neighbors, double max_dist)
00150   {
00151     // Fixing binning to 5 and plane radius to 0.2
00152     int div_d = 5;
00153     double plane_radius = 0.2;
00154 
00155     // Initialize minimum and maximum angle values in each distance bin
00156     std::vector<std::vector<double> > min_max_angle_by_dist (div_d);
00157     for (int di=0; di<div_d; di++)
00158     {
00159       min_max_angle_by_dist[di].resize (2);
00160       min_max_angle_by_dist[di][0] = +DBL_MAX;
00161       min_max_angle_by_dist[di][1] = -DBL_MAX;
00162     }
00163 
00164     // Compute distance by normal angle distribution for points
00165     for (unsigned int i = 0; i < neighbors->size (); i++)
00166       for (unsigned int j = i; j < neighbors->size (); j++)
00167       {
00168         // compute angle between the two lines going through normals (disregard orientation!)
00169         double cosine = cloud.points[i].normal[0] * cloud.points[j].normal[0] +
00170           cloud.points[i].normal[1] * cloud.points[j].normal[1] +
00171           cloud.points[i].normal[2] * cloud.points[j].normal[2];
00172         if (cosine > 1) 
00173           cosine = 1;
00174         if (cosine < -1) 
00175           cosine = -1;
00176         double angle  = acos (cosine);
00177         if (angle > M_PI/2) 
00178           angle = M_PI - angle;
00179         //cerr << round(RAD2DEG(angle)) << " and ";
00180 
00181         // Compute point to point distance
00182         //@TODO: check neighbors->at(i)
00183         double dist = sqrt ((cloud.points[neighbors->at(i)].x - cloud.points[neighbors->at(j)].x) * (cloud.points[neighbors->at(i)].x - cloud.points[neighbors->at(j)].x) +
00184                             (cloud.points[neighbors->at(i)].y - cloud.points[neighbors->at(j)].y) * (cloud.points[neighbors->at(i)].y - cloud.points[neighbors->at(j)].y) +
00185                             (cloud.points[neighbors->at(i)].z - cloud.points[neighbors->at(j)].z) * (cloud.points[neighbors->at(i)].z - cloud.points[neighbors->at(j)].z));
00186         //cerr << dist << ": ";
00187 
00188         // compute bins and increase
00189         int bin_d = (int) floor (div_d * dist / max_dist);
00190 
00191         // update min-max values for distance bins
00192         if (min_max_angle_by_dist[bin_d][0] > angle) 
00193           min_max_angle_by_dist[bin_d][0] = angle;
00194         if (min_max_angle_by_dist[bin_d][1] < angle) 
00195           min_max_angle_by_dist[bin_d][1] = angle;
00196       }
00197 
00198     // Estimate radius from min and max lines
00199     double Amint_Amin = 0, Amint_d = 0;
00200     double Amaxt_Amax = 0, Amaxt_d = 0;
00201     for (int di=0; di<div_d; di++)
00202     {
00203       //cerr << di << ": " << min_max_angle_by_dist[di][0] << " - " << min_max_angle_by_dist[di][1] << endl;
00204       // combute the members of A'*A*r = A'*D
00205       if (min_max_angle_by_dist[di][1] >= 0)
00206       {
00207         double p_min = min_max_angle_by_dist[di][0];
00208         double p_max = min_max_angle_by_dist[di][1];
00209         //cerr << p_min << " " << p_max << endl;
00210         double f = (di+0.5)*max_dist/div_d;
00211         //cerr << f << endl;
00212         Amint_Amin += p_min * p_min;
00213         Amint_d += p_min * f;
00214         Amaxt_Amax += p_max * p_max;
00215         Amaxt_d += p_max * f;
00216       }
00217     }
00218     //cerr << Amint_Amin << " " << Amint_d << " " << Amaxt_Amax << " " << Amaxt_d << endl;
00219     double max_radius;
00220     if (Amint_Amin == 0) 
00221       max_radius = plane_radius;
00222     else 
00223       max_radius = std::min (Amint_d/Amint_Amin, plane_radius);
00224     double min_radius;
00225     if (Amaxt_Amax == 0) 
00226       min_radius = plane_radius;
00227     else 
00228       min_radius = std::min (Amaxt_d/Amaxt_Amax, plane_radius);
00229     //print_info (stderr, "Estimated minimum and maximum radius is: "); print_value (stderr, "%g - %g\n", min_radius, max_radius);
00230 
00231     // Simple categorization to reduce feature vector size, but should use co-occurance of min-max radius bins
00232     int type;// = EMPTY_VALUE;
00233     if (min_radius > min_radius_plane_) // 0.066
00234       type = 1; // plane
00235     else if ((min_radius < min_radius_noise_) && (max_radius < max_radius_noise_))
00236       type = 0; // noise/corner
00237     else if (max_radius - min_radius < max_min_radius_diff_) // 0.0075
00238       type = 3; // circle (corner?)
00239     else if (min_radius < min_radius_edge_) 
00240       type = 4; // edge
00241     else
00242       type = 2; // cylinder (rim)
00243 
00244     // For safety...
00245     if (type >= NR_CLASS)
00246       type = -1;
00247 
00248     // Return type
00249     return type;
00250   }
00251 
00252   // Sets up the OcTree
00253   void
00254     setOctree (pcl::PointCloud<pcl::PointNormal> pointcloud_msg, double octree_res, int initial_label, double laser_offset = 0, double octree_maxrange = -1)
00255   {
00256     octomap_server::OctomapBinary octree_msg;
00257 
00258     octomap::point3d octomap_3d_point;
00259     octomap::Pointcloud octomap_pointcloud;
00260 
00261 
00262     //Reading from pcl point cloud and saving it into octomap point cloud
00263     for(unsigned int i =0; i < pointcloud_msg.points.size(); i++)
00264     {
00265       octomap_3d_point(0) = pointcloud_msg.points[i].x;
00266       octomap_3d_point(1) = pointcloud_msg.points[i].y;
00267       octomap_3d_point(2) = pointcloud_msg.points[i].z;
00268       octomap_pointcloud.push_back(octomap_3d_point);
00269     }
00270 
00271     // Converting from octomap point cloud to octomap graph
00272     octomap::pose6d offset_trans(0,0,-laser_offset,0,0,0);
00273     octomap::pose6d laser_pose(0,0,laser_offset,0,0,0);
00274     octomap_pointcloud.transform(offset_trans);
00275 
00276 
00277     octomap::ScanGraph* octomap_graph = new octomap::ScanGraph();
00278     octomap_graph->addNode(&octomap_pointcloud, laser_pose);
00279 
00280     //ROS_INFO("Number of points in scene graph: %d", octomap_graph->getNumPoints());
00281 
00282     // Converting from octomap graph to octomap tree (octree)
00283     octree_ = new octomap::OcTreePCL(octree_res);
00284     for (octomap::ScanGraph::iterator scan_it = octomap_graph->begin(); scan_it != octomap_graph->end(); scan_it++)
00285     {
00286       octree_->insertScan(**scan_it, octree_maxrange, false);
00287     }
00288     //publish octree on a topic?
00289     if (publish_octree_)
00290     {
00291       octomap_server::octomapMapToMsg(*octree_, octree_msg);
00292       octree_binary_publisher_.publish(octree_msg);
00293       ROS_INFO("Octree built and published");
00294     }
00295 
00296     std::list<octomap::OcTreeVolume> voxels, leaves;
00297     //octree->getLeafNodes(leaves, level_);
00298     octree_->getLeafNodes(leaves);
00299     std::list<octomap::OcTreeVolume>::iterator it1;
00300     int cnt = 0;
00301 
00302     //find Leaf Nodes' centroids, assign controid coordinates to Leaf Node
00303     for( it1 = leaves.begin(); it1 != leaves.end(); ++it1)
00304     {
00305       ROS_DEBUG("Leaf Node %d : x = %f y = %f z = %f side length = %f ", cnt++, it1->first.x(), it1->first.y(), it1->first.z(), it1->second);
00306       octomap::point3d centroid;
00307       centroid(0) = it1->first.x(),  centroid(1) = it1->first.y(),  centroid(2) = it1->first.z();
00308       octomap::OcTreeNodePCL *octree_node = octree_->search(centroid);
00309       octree_node->setCentroid(centroid);
00310       octree_node->setLabel(initial_label);
00311     }
00312     
00313     //assign points to Leaf Nodes
00314     for(unsigned int i = 0; i < pointcloud_msg.points.size(); i++)
00315     {
00316       octomap_3d_point(0) = pointcloud_msg.points[i].x;
00317       octomap_3d_point(1) = pointcloud_msg.points[i].y;
00318       octomap_3d_point(2) = pointcloud_msg.points[i].z;
00319       octomap::OcTreeNodePCL * octree_node1 = octree_->search(octomap_3d_point);
00320       octree_node1->set3DPointInliers(i);
00321     }
00322   }
00323   //*/
00324 
00325  private:
00326 
00327   // ROS stuff
00328   ros::NodeHandle nh_;
00329   //ros::Publisher pub_cloud_vrsd_;
00330   pcl_ros::Publisher<pcl::PointNormal> pub_cloud_vrsd_;
00331   //ros::Publisher pub_cloud_centroids_;
00332   pcl_ros::Publisher<pcl::PointNormal> pub_cloud_centroids_;
00333   ros::Publisher octree_binary_publisher_;
00334 
00335   // hard-coding these for now through  NR_CLASS
00336   int nr_bins_;
00337 
00338   // ROS messages
00339   //@ TODO Document clearly!!!
00340   pcl::PointCloud<pcl::GRSDSignature21> cloud_grsd_; 
00341 
00342   // OcTree stuff
00343   octomap::OcTreePCL* octree_;
00344 };
00345 
00346 }
00347 #endif
00348 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


pcl_cloud_algos
Author(s): Nico Blodow, Dejan Pangercic, Zoltan-Csaba Marton
autogenerated on Thu May 23 2013 15:44:49