Point Cloud Library (PCL)  1.3.1
cvfh.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, 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: cvfh.hpp 3022 2011-11-01 03:42:22Z rusu $
00035  *
00036  */
00037 
00038 #ifndef PCL_FEATURES_IMPL_CVFH_H_
00039 #define PCL_FEATURES_IMPL_CVFH_H_
00040 
00041 #include "pcl/features/cvfh.h"
00042 #include "pcl/features/pfh.h"
00043 
00045 template<typename PointInT, typename PointNT, typename PointOutT>
00046   void
00047   pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::extractEuclideanClustersSmooth (
00048                                                                                      const pcl::PointCloud<
00049                                                                                          pcl::PointNormal> &cloud,
00050                                                                                      const pcl::PointCloud<
00051                                                                                          pcl::PointNormal> &normals,
00052                                                                                      float tolerance,
00053                                                                                      const pcl::search::Search<pcl::PointNormal>::Ptr &tree,
00054                                                                                      std::vector<pcl::PointIndices> &clusters,
00055                                                                                      double eps_angle,
00056                                                                                      unsigned int min_pts_per_cluster,
00057                                                                                      unsigned int max_pts_per_cluster)
00058   {
00059     if (tree->getInputCloud ()->points.size () != cloud.points.size ())
00060     {
00061       PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", (unsigned long)tree->getInputCloud ()->points.size (), (unsigned long)cloud.points.size ());
00062       return;
00063     }
00064     if (cloud.points.size () != normals.points.size ())
00065     {
00066       PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%lu) different than normals (%lu)!\n", (unsigned long)cloud.points.size (), (unsigned long)normals.points.size ());
00067       return;
00068     }
00069 
00070     // Create a bool vector of processed point indices, and initialize it to false
00071     std::vector<bool> processed (cloud.points.size (), false);
00072 
00073     std::vector<int> nn_indices;
00074     std::vector<float> nn_distances;
00075     // Process all points in the indices vector
00076     for (size_t i = 0; i < cloud.points.size (); ++i)
00077     {
00078       if (processed[i])
00079         continue;
00080 
00081       std::vector<unsigned int> seed_queue;
00082       int sq_idx = 0;
00083       seed_queue.push_back (i);
00084 
00085       processed[i] = true;
00086 
00087       while (sq_idx < (int)seed_queue.size ())
00088       {
00089         // Search for sq_idx
00090         if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
00091         {
00092           sq_idx++;
00093           continue;
00094         }
00095 
00096         for (size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx
00097         {
00098           if (processed[nn_indices[j]]) // Has this point been processed before ?
00099             continue;
00100 
00101           //processed[nn_indices[j]] = true;
00102           // [-1;1]
00103 
00104           double dot_p = normals.points[seed_queue[sq_idx]].normal[0] * normals.points[nn_indices[j]].normal[0]
00105               + normals.points[seed_queue[sq_idx]].normal[1] * normals.points[nn_indices[j]].normal[1]
00106               + normals.points[seed_queue[sq_idx]].normal[2] * normals.points[nn_indices[j]].normal[2];
00107 
00108           if (fabs (acos (dot_p)) < eps_angle)
00109           {
00110             processed[nn_indices[j]] = true;
00111             seed_queue.push_back (nn_indices[j]);
00112           }
00113         }
00114 
00115         sq_idx++;
00116       }
00117 
00118       // If this queue is satisfactory, add to the clusters
00119       if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
00120       {
00121         pcl::PointIndices r;
00122         r.indices.resize (seed_queue.size ());
00123         for (size_t j = 0; j < seed_queue.size (); ++j)
00124           r.indices[j] = seed_queue[j];
00125 
00126         std::sort (r.indices.begin (), r.indices.end ());
00127         r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());
00128 
00129         r.header = cloud.header;
00130         clusters.push_back (r); // We could avoid a copy by working directly in the vector
00131       }
00132     }
00133   }
00134 
00136 template<typename PointInT, typename PointNT, typename PointOutT>
00137   void
00138   pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::filterNormalsWithHighCurvature (
00139                                                                                      const pcl::PointCloud<PointNT> & cloud,
00140                                                                                      std::vector<int> & indices_out,
00141                                                                                      std::vector<int> & indices_in,
00142                                                                                      float threshold)
00143   {
00144     indices_out.resize (cloud.points.size ());
00145     indices_in.resize (cloud.points.size ());
00146 
00147     size_t in, out;
00148     in = out = 0;
00149 
00150     for (size_t i = 0; i < cloud.points.size (); i++)
00151     {
00152       if (cloud.points[i].curvature > threshold)
00153       {
00154         indices_out[out] = i;
00155         out++;
00156       }
00157       else
00158       {
00159         indices_in[in] = i;
00160         in++;
00161       }
00162     }
00163 
00164     indices_out.resize (out);
00165     indices_in.resize (in);
00166   }
00167 
00169 template<typename PointInT, typename PointNT, typename PointOutT>
00170   void
00171   pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
00172   {
00173     // Check if input was set
00174     if (!normals_)
00175     {
00176       PCL_ERROR ("[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ());
00177       output.width = output.height = 0;
00178       output.points.clear ();
00179       return;
00180     }
00181     if (normals_->points.size () != surface_->points.size ())
00182     {
00183       PCL_ERROR ("[pcl::%s::computeFeature] The number of points in the input dataset differs from the number of points in the dataset containing the normals!\n", getClassName ().c_str ());
00184       output.width = output.height = 0;
00185       output.points.clear ();
00186       return;
00187     }
00188 
00189     centroids_dominant_orientations_.clear ();
00190 
00191     // ---[ Step 0: remove normals with high curvature
00192     float curv_threshold = curv_threshold_;
00193     std::vector<int> indices_out;
00194     std::vector<int> indices_in;
00195     filterNormalsWithHighCurvature (*normals_, indices_out, indices_in, curv_threshold);
00196 
00197     pcl::PointCloud<pcl::PointNormal>::Ptr filtered (new pcl::PointCloud<pcl::PointNormal> ());
00198 
00199     filtered->width = indices_in.size ();
00200     filtered->height = 1;
00201     filtered->points.resize (filtered->width);
00202 
00203     for (size_t i = 0; i < indices_in.size (); ++i)
00204     {
00205       filtered->points[i].x = surface_->points[indices_in[i]].x;
00206       filtered->points[i].y = surface_->points[indices_in[i]].y;
00207       filtered->points[i].z = surface_->points[indices_in[i]].z;
00208 
00209       filtered->points[i].normal[0] = normals_->points[indices_in[i]].normal[0];
00210       filtered->points[i].normal[1] = normals_->points[indices_in[i]].normal[1];
00211       filtered->points[i].normal[2] = normals_->points[indices_in[i]].normal[2];
00212     }
00213 
00214     // ---[ Step 1a : compute clustering
00215     pcl::PointCloud<pcl::PointNormal>::Ptr normals_filtered_cloud (new pcl::PointCloud<pcl::PointNormal> ());
00216     if (indices_in.size () >= 100) //TODO: parameter
00217     {
00218       normals_filtered_cloud->width = indices_in.size ();
00219       normals_filtered_cloud->height = 1;
00220       normals_filtered_cloud->points.resize (normals_filtered_cloud->width);
00221 
00222       for (size_t i = 0; i < indices_in.size (); ++i)
00223       {
00224         normals_filtered_cloud->points[i].x = surface_->points[indices_in[i]].x;
00225         normals_filtered_cloud->points[i].y = surface_->points[indices_in[i]].y;
00226         normals_filtered_cloud->points[i].z = surface_->points[indices_in[i]].z;
00227 
00228         normals_filtered_cloud->points[i].normal[0] = normals_->points[indices_in[i]].normal[0];
00229         normals_filtered_cloud->points[i].normal[1] = normals_->points[indices_in[i]].normal[1];
00230         normals_filtered_cloud->points[i].normal[2] = normals_->points[indices_in[i]].normal[2];
00231       }
00232     }
00233     else
00234     {
00235       normals_filtered_cloud->width = surface_->size ();
00236       normals_filtered_cloud->height = 1;
00237       normals_filtered_cloud->points.resize (normals_filtered_cloud->width);
00238 
00239       for (size_t i = 0; i < surface_->size (); ++i)
00240       {
00241         normals_filtered_cloud->points[i].x = surface_->points[i].x;
00242         normals_filtered_cloud->points[i].y = surface_->points[i].y;
00243         normals_filtered_cloud->points[i].z = surface_->points[i].z;
00244 
00245         normals_filtered_cloud->points[i].normal[0] = normals_->points[i].normal[0];
00246         normals_filtered_cloud->points[i].normal[1] = normals_->points[i].normal[1];
00247         normals_filtered_cloud->points[i].normal[2] = normals_->points[i].normal[2];
00248       }
00249     }
00250 
00251     //recompute normals normals and use them for clustering!
00252     KdTreePtr normals_tree_filtered (new pcl::search::KdTree<pcl::PointNormal> (false));
00253     normals_tree_filtered->setInputCloud (normals_filtered_cloud);
00254 
00255     NormalEstimator n3d;
00256     n3d.setRadiusSearch (radius_normals_);
00257     n3d.setSearchMethod (normals_tree_filtered);
00258     n3d.setInputCloud (normals_filtered_cloud);
00259     n3d.compute (*normals_filtered_cloud);
00260 
00261     KdTreePtr normals_tree (new pcl::search::KdTree<pcl::PointNormal> (false));
00262     normals_tree->setInputCloud (normals_filtered_cloud);
00263     std::vector<pcl::PointIndices> clusters;
00264 
00265     extractEuclideanClustersSmooth (*normals_filtered_cloud, *normals_filtered_cloud, cluster_tolerance_, normals_tree,
00266                                     clusters, eps_angle_threshold_, min_points_);
00267 
00268     /*clusters_colored_.reset (new pcl::PointCloud<pcl::PointXYZINormal> ());
00269     clusters_colored_->points.resize(normals_filtered_cloud->points.size());
00270     clusters_colored_->width = clusters_colored_->points.size();
00271     clusters_colored_->height = 1;
00272 
00273     float intensity = 0.1;
00274     int n_points=0;
00275     for (size_t i = 0; i < clusters.size (); ++i) //for each cluster
00276     {
00277       for (size_t j = 0; j < clusters[i].indices.size (); j++)
00278       {
00279         clusters_colored_->points[n_points].intensity = intensity;
00280         clusters_colored_->points[n_points].getVector4fMap() = normals_filtered_cloud->points[clusters[i].indices[j]].getVector4fMap();
00281         clusters_colored_->points[n_points].getNormalVector4fMap() = normals_filtered_cloud->points[clusters[i].indices[j]].getNormalVector4fMap();
00282         clusters_colored_->points[n_points].curvature = normals_filtered_cloud->points[clusters[i].indices[j]].curvature;
00283 
00284         n_points++;
00285       }
00286 
00287       intensity += 0.1;
00288     }
00289 
00290     clusters_colored_->points.resize(n_points);
00291     clusters_colored_->width = n_points;*/
00292 
00293     std::vector<Eigen::Vector3f> dominant_normals;
00294 
00295     VFHEstimator vfh;
00296     vfh.setInputCloud (surface_);
00297     vfh.setInputNormals (normals_);
00298     vfh.setSearchMethod (this->tree_);
00299     vfh.setUseGivenNormal (true);
00300     vfh.setUseGivenCentroid (true);
00301     vfh.setNormalizeBins (normalize_bins_);
00302     vfh.setNormalizeDistance (true);
00303     vfh.setFillSizeComponent (true);
00304     output.height = 1;
00305 
00306     // ---[ Step 1b : check if any dominant cluster was found
00307     if (clusters.size () > 0)
00308     { // ---[ Step 1b.1 : If yes, compute CVFH using the cluster information
00309 
00310       for (size_t i = 0; i < clusters.size (); ++i) //for each cluster
00311       {
00312         Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
00313         Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
00314 
00315         for (size_t j = 0; j < clusters[i].indices.size (); j++)
00316         {
00317           avg_normal += normals_filtered_cloud->points[clusters[i].indices[j]].getNormalVector4fMap ();
00318           avg_centroid += normals_filtered_cloud->points[clusters[i].indices[j]].getVector4fMap ();
00319         }
00320 
00321         avg_normal /= clusters[i].indices.size ();
00322         avg_centroid /= clusters[i].indices.size ();
00323 
00324         Eigen::Vector4f centroid_test;
00325         pcl::compute3DCentroid (*normals_filtered_cloud, centroid_test);
00326         avg_normal.normalize ();
00327 
00328         Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]);
00329         Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
00330 
00331         //append normal and centroid for the clusters
00332         dominant_normals.push_back (avg_norm);
00333         centroids_dominant_orientations_.push_back (avg_dominant_centroid);
00334       }
00335 
00336       //compute modified VFH for all dominant clusters and add them to the list!
00337       output.points.resize (dominant_normals.size ());
00338       output.width = dominant_normals.size ();
00339 
00340       for (size_t i = 0; i < dominant_normals.size (); ++i)
00341       {
00342         //configure VFH computation for CVFH
00343         vfh.setNormalToUse (dominant_normals[i]);
00344         vfh.setCentroidToUse (centroids_dominant_orientations_[i]);
00345         pcl::PointCloud<pcl::VFHSignature308> vfh_signature;
00346         vfh.compute (vfh_signature);
00347         output.points[i] = vfh_signature.points[0];
00348       }
00349     }
00350     else
00351     { // ---[ Step 1b.1 : If no, compute CVFH using all the object points
00352       Eigen::Vector4f avg_centroid;
00353       pcl::compute3DCentroid (*surface_, avg_centroid);
00354       Eigen::Vector3f cloud_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
00355       centroids_dominant_orientations_.push_back (cloud_centroid);
00356 
00357       //configure VFH computation for CVFH using all object points
00358       vfh.setCentroidToUse (cloud_centroid);
00359       vfh.setUseGivenNormal (false);
00360 
00361       pcl::PointCloud<pcl::VFHSignature308> vfh_signature;
00362       vfh.compute (vfh_signature);
00363 
00364       output.points.resize (1);
00365       output.width = 1;
00366 
00367       output.points[0] = vfh_signature.points[0];
00368     }
00369   }
00370 
00371 #define PCL_INSTANTIATE_CVFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::CVFHEstimation<T,NT,OutT>;
00372 
00373 #endif    // PCL_FEATURES_IMPL_VFH_H_ 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines