global_rsd.cpp
Go to the documentation of this file.
00001 #include <pcl_cloud_algos/global_rsd.h>
00002 
00003 using namespace std;
00004 using namespace pcl_cloud_algos;
00005 
00006 void GlobalRSD::init (ros::NodeHandle& nh)
00007 {
00008   nh_ = nh;
00009   pub_cloud_vrsd_ = pcl_ros::Publisher<pcl::PointNormal> (nh_, "cloud_vrsd", 1);
00010   pub_cloud_centroids_ = pcl_ros::Publisher<pcl::PointNormal> (nh_, "cloud_centroids", 1);
00011 }
00012 
00013 void GlobalRSD::pre ()
00014 {
00015   nh_.param("point_label", point_label_, point_label_);
00016   nh_.param("width", width_, width_);
00017   nh_.param("step", step_, step_);
00018   nh_.param("min_voxel_pts", min_voxel_pts_, min_voxel_pts_);
00019   nh_.param("publish_cloud_vrsd", publish_cloud_vrsd_, publish_cloud_vrsd_);
00020   nh_.param("publish_cloud_centroids", publish_cloud_centroids_, publish_cloud_centroids_);
00021   nh_.param("min_radius_plane", min_radius_plane_,  min_radius_plane_);
00022   nh_.param("min_radius_noise", min_radius_noise_,  min_radius_noise_);
00023   nh_.param("max_radius_noise", max_radius_noise_,  max_radius_noise_);
00024   nh_.param("max_min_radius_diff", max_min_radius_diff_,  max_min_radius_diff_);
00025   nh_.param("min_radius_edge", min_radius_edge_,  min_radius_edge_);
00026   nh_.param("publish_octree", publish_octree_,  publish_octree_);
00027   cloud_grsd_.points.resize(1);
00028 }
00029 
00030 void GlobalRSD::post ()
00031 {
00032 
00033 }
00034 
00035 std::vector<std::string> GlobalRSD::requires ()
00036 {
00037   std::vector<std::string> requires;
00038   // requires 3D coordinates
00039   requires.push_back("x");
00040   requires.push_back("y");
00041   requires.push_back("z");
00042   // requires normals
00043   requires.push_back("normal");
00044   return requires;
00045 }
00046 
00047 std::vector<std::string> GlobalRSD::provides ()
00048 {
00049   std::vector<std::string> provides;
00050 
00051   // provides features (variable number)
00052   for (int i = 0; i < nr_bins_; i++)
00053   {
00054     char dim_name[16];
00055     sprintf (dim_name, "f%d", i+1);
00056     provides.push_back (dim_name);
00057   }
00058 
00059   // sets point label if required
00060   if (point_label_ != -1)
00061     provides.push_back ("point_label");
00062 
00063   return provides;
00064 }
00065 
00066 std::string GlobalRSD::process (const boost::shared_ptr<const GlobalRSD::InputType>& cloud)
00067  {
00068    int norm = pcl::getFieldIndex (*cloud, "normal_x");
00069    
00070    if (norm == -1)
00071    {
00072      ROS_ERROR ("[GlobalRSD] Provided point cloud does not have normals. Use the normal_estimation or mls_fit first!");
00073      output_valid_ = false;
00074      return std::string("missing normals");
00075    }
00076 
00077    // Timers
00078   ros::Time global_time = ros::Time::now ();
00079   ros::Time ts;
00080   pcl::fromROSMsg(*cloud, cloud_vrsd_);
00081   cloud_centroids_.points.resize(cloud_vrsd_.points.size());    
00082   // Create a fixed-size octree decomposition
00083   ts = ros::Time::now ();
00084   setOctree (cloud_vrsd_, width_, -1); //width_ = width of the octree cell
00085   if (verbosity_level_ > 0) 
00086     ROS_INFO ("[GlobalRSD] kdTree created in %g seconds.", (ros::Time::now () - ts).toSec ());
00087 
00088   // Maximum distance in the user-specified neighborhood
00089   double max_dist = (2*step_+1)*width_*sqrt(3);
00090   double radius = max_dist/2;
00091 
00092   // Make sure that we provide enough points for radius computation:
00093   KdTreePtr tree = boost::make_shared<pcl::KdTreeFLANN<pcl::PointNormal> > ();
00094 
00095   if (step_ == 0)
00096   {
00097     ts = ros::Time::now ();
00099     tree->setInputCloud(boost::make_shared <const pcl::PointCloud<pcl::PointNormal> > (cloud_vrsd_));
00100     if (verbosity_level_ > 0) 
00101       ROS_INFO ("[GlobalRSD] kdTree created in %g seconds.", (ros::Time::now () - ts).toSec ());
00102   }
00103 
00104   // Select only the occupied leaves
00105   std::list<octomap::OcTreeVolume> cells;
00106   octree_->getOccupied(cells, 0);
00107 
00108   // Set surface type for each cell in advance
00109   ts = ros::Time::now ();
00110   int cnt_centroids = 0;
00111   double x_min, y_min, z_min, x_max, y_max, z_max;
00112   octree_->getMetricMin(x_min, y_min, z_min);
00113   octree_->getMetricMax(x_max, y_max, z_max);
00114   for (std::list<octomap::OcTreeVolume>::iterator it_i = cells.begin (); it_i != cells.end (); ++it_i)
00115   {
00116     // Get a cell
00117     octomap::point3d centroid_i;
00118     centroid_i(0) = it_i->first.x();
00119     centroid_i(1) = it_i->first.y();
00120     centroid_i(2) = it_i->first.z();
00121     octomap::OcTreeNodePCL *node_i = octree_->search(centroid_i);
00122 
00123     // Get its contents
00124     vector<int> indices_i = node_i->get3DPointInliers ();
00125     if (indices_i.size () < min_voxel_pts_)
00126       continue;
00127 
00128     // Iterating through neighbors
00129     vector<int> neighbors;
00130     if (step_ == 0)
00131     {
00132       Eigen::Vector4f central_point;
00133       pcl::compute3DCentroid (cloud_vrsd_, indices_i, central_point);
00134       vector<float> sqr_distances;
00135       QueryPoint central_point_pcl(central_point[0], central_point[1], central_point[2]);
00136       tree->radiusSearch (central_point_pcl, radius, neighbors, sqr_distances); // max_nn_ is INT_MAX by default
00137     }
00138     else
00139     {
00140       neighbors = indices_i;
00141       for (int i = step_; i <= step_; i++)
00142       {
00143         for (int j = -step_; j <= step_; j++)
00144         {
00145           for (int k = -step_; k <= step_; k++)
00146           {
00147             // skip current point
00148             if (i==0 && j==0 && k==0)
00149               continue;
00150             // skip inexistent cells
00151             octomap::point3d centroid_neighbor;
00152             centroid_neighbor(0) = centroid_i(0) + i*width_;
00153             centroid_neighbor(1) = centroid_i(1) + j*width_;
00154             centroid_neighbor(2) = centroid_i(2) + k*width_;
00155             if (centroid_neighbor(0)<x_min || centroid_neighbor(1)<y_min || centroid_neighbor(2)<z_min || centroid_neighbor(0)>x_max || centroid_neighbor(1)>y_max || centroid_neighbor(2)>z_max)
00156               continue;
00157             // accumulate indices
00158             octomap::OcTreeNodePCL *node_neighbor = octree_->search(centroid_neighbor);
00159             if (node_neighbor == NULL) // TODO: why does the previous check fail?
00160               continue;
00161             vector<int> ni = node_neighbor->get3DPointInliers ();
00162             neighbors.insert (neighbors.end (), ni.begin (), ni.end ());
00163           } // i
00164         } // j
00165       } // k
00166     }
00167 
00168     // Mark all points with result
00169     int type = setSurfaceType (cloud_vrsd_, &indices_i, &neighbors, max_dist);
00170 
00171     // Mark the node label as well
00172     node_i->label = type;
00173 
00174     // Set up PCD with centroids
00175     cloud_centroids_.points[cnt_centroids].x = centroid_i(0);
00176     cloud_centroids_.points[cnt_centroids].y = centroid_i(1);
00177     cloud_centroids_.points[cnt_centroids].z = centroid_i(2);
00178     cloud_centroids_.points[cnt_centroids].curvature = (float) type;
00179     cnt_centroids++;
00180   }
00181   cloud_centroids_.points.resize (cnt_centroids);
00182   //   for (size_t d = 0; d < cloud_centroids_.channels.size (); d++)
00183   //     cloud_centroids_.channels[d].values.resize (cnt_centroids);
00184   
00185   if (verbosity_level_ > 0) 
00186     ROS_INFO ("[GlobalRSD] Cells annotated in %g seconds.", (ros::Time::now () - ts).toSec ());
00187 
00188   //GLOBAL part
00189   // Initialize transition matrix for counting
00190   vector<vector<int> > transitions (NR_CLASS+1);
00191   for (size_t i = 0; i < transitions.size (); i++)
00192     transitions[i].resize (NR_CLASS+1);
00193 
00195   ts = ros::Time::now ();
00196   float line_p1[3], line_p2[3], box_bounds[6];
00197   for (std::list<octomap::OcTreeVolume>::iterator it_i = cells.begin (); it_i != cells.end (); ++it_i)
00198   {
00199     // Get a cell
00200     octomap::point3d centroid_i;
00201     centroid_i(0) = it_i->first.x();
00202     centroid_i(1) = it_i->first.y();
00203     centroid_i(2) = it_i->first.z();
00204     octomap::OcTreeNodePCL *node_i = octree_->search(centroid_i);
00205 
00206     // Get its contents
00207     vector<int> indices_i = node_i->get3DPointInliers ();
00208     if (indices_i.size () < min_voxel_pts_)
00209       continue;
00210 
00211     // Connect current cell to all the remaining ones
00212     for (std::list<octomap::OcTreeVolume>::iterator it_j = it_i; it_j != cells.end (); ++it_j)
00213     {
00215       if (it_i == it_j)
00216        continue;
00217 
00218       // Get a cell
00219       octomap::point3d centroid_j;
00220       centroid_j(0) = it_j->first.x();
00221       centroid_j(1) = it_j->first.y();
00222       centroid_j(2) = it_j->first.z();
00223       octomap::OcTreeNodePCL *node_j = octree_->search(centroid_j);
00224 
00225       // Get its contents
00226       vector<int> indices_j = node_j->get3DPointInliers ();
00227       if (indices_j.size () < min_voxel_pts_)
00228         continue;
00229 
00230       // Create a paired histogram vector which holds: a) the actual centroid value of the intersected voxel, b) the distance from start_voxel to voxel_i
00231       vector<pair<int, IntersectedLeaf> > histogram_values;
00232 
00233       // Get the leaves along the ray
00234       vector<octomap::point3d> ray;
00235       octree_->computeRay(centroid_i, centroid_j, ray);
00236 
00237       // Iterate over leaves
00238       for (vector<octomap::point3d>::iterator centroid_ray = ray.begin (); centroid_ray != ray.end (); centroid_ray++)
00239       {
00240         // Compute the distance to the start leaf
00241         pair<int, IntersectedLeaf> histogram_pair;
00242         histogram_pair.second.centroid = *centroid_ray;
00243         histogram_pair.second.sqr_distance = _sqr_dist (*centroid_ray, centroid_i);
00244 
00245         // Save the label of the cell
00246         octomap::OcTreeNodePCL *node_ray = octree_->search(*centroid_ray);
00247         if (node_ray == NULL) // @TODO: this should never happen, the octree_ should be fully expanded!
00248           histogram_pair.first = -1;
00249         else
00250           histogram_pair.first = node_ray->label;
00251 
00252         // Save data about cell
00253         histogram_values.push_back (histogram_pair);
00254       }
00255 
00256       // Add the first voxel
00257       pair<int, IntersectedLeaf> histogram_pair1;
00258       histogram_pair1.second.sqr_distance = _sqr_dist (centroid_i, centroid_i); // 0.0
00259       histogram_pair1.second.centroid = centroid_i;
00260       histogram_pair1.first = node_i->label;
00261       //histogram_pair1.first = (int)(cloud_vrsd.channels[regIdx].values[indices_i.at (0)]);
00262       histogram_values.push_back (histogram_pair1);
00263 
00264       // Add the last voxel
00265       pair<int, IntersectedLeaf> histogram_pair2;
00266       histogram_pair2.second.sqr_distance = _sqr_dist (centroid_j, centroid_i); // line length
00267       histogram_pair2.second.centroid = centroid_j;
00268       histogram_pair2.first = node_j->label;
00269       //histogram_pair2.first = (int)(cloud_vrsd.channels[regIdx].values[indices_j.at (0)]);
00270       histogram_values.push_back (histogram_pair2);
00271 
00272       // Sort the histogram according to the distance of the leaves to the start leaf
00273       sort (histogram_values.begin (), histogram_values.end (), histogramElementCompare);
00274 
00275       // Count transitions between the first and last voxel
00276       for (unsigned int hi = 1; hi < histogram_values.size (); hi++)
00277       {
00278         // transition matrix has to be symmetrical
00279         transitions[histogram_values[hi].first+1][histogram_values[hi-1].first+1]++;
00280         transitions[histogram_values[hi-1].first+1][histogram_values[hi].first+1]++;
00281         //cerr << "transition: " << histogram_values[hi].first-EMPTY_VALUE << " => " << histogram_values[hi-1].first-EMPTY_VALUE << endl;
00282       }
00283     }
00284   }
00285   if (verbosity_level_ > 0) 
00286     ROS_INFO ("[GlobalRSD] Transitions counted in %g seconds.", (ros::Time::now () - ts).toSec ());
00287 
00289   if (verbosity_level_ > 1)
00290   {
00291     ROS_INFO ("[GlobalRSD] Transition matrix is:");
00292     for (int i=0; i<NR_CLASS+1; i++)
00293     {
00294       stringstream line;
00295       for (int j=0; j<NR_CLASS+1; j++)
00296         line << " " << transitions[i][j];
00297       ROS_INFO ("%s", line.str ().c_str ());
00298     }
00299   }
00300   int nrf = 0;
00301   for (int i=0; i<NR_CLASS+1; i++)
00302   {
00303     for (int j=i; j<NR_CLASS+1; j++)
00304     {
00305       cloud_grsd_.points[0].histogram[nrf++] = transitions[i][j]; //@TODO: resize point cloud
00306     }
00307   }
00308   //@TODO: Check with Zoli what are following 2 lines for:
00309   //if (point_label_ != -1)
00310   //cloud_grsd_.channels[nr_bins_].values[0] = point_label_;
00311   
00312   // Publish partial results for visualization
00313   if (publish_cloud_centroids_)
00314     pub_cloud_centroids_.publish (cloud_centroids_);
00315   if (publish_cloud_vrsd_)
00316     pub_cloud_vrsd_.publish (cloud_vrsd_);
00317 
00318   // Finish
00319   if (verbosity_level_ > 0) 
00320     ROS_INFO ("[GlobalRSD] Computed features in %g seconds.", (ros::Time::now () - global_time).toSec ());
00321   output_valid_ = true;
00322   return std::string("ok");
00323 }
00324 
00325 boost::shared_ptr<const GlobalRSD::OutputType> GlobalRSD::output ()
00326 {
00327   //  sensor_msgs::PointCloud2 cloud_grsd_msg;
00328   sensor_msgs::PointCloud2 cloud_grsd_msg;
00329   pcl::toROSMsg (cloud_grsd_, cloud_grsd_msg);
00330   return boost::make_shared<sensor_msgs::PointCloud2> (cloud_grsd_msg);
00331 }
00332 
00333 #ifdef CREATE_NODE
00334 int main (int argc, char* argv[])
00335 {
00336   return standalone_node <GlobalRSD> (argc, argv);
00337 }
00338 #endif
 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:48