iss_3d.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2011, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  */
00037 
00038 #ifndef PCL_ISS_KEYPOINT3D_IMPL_H_
00039 #define PCL_ISS_KEYPOINT3D_IMPL_H_
00040 
00041 #include <pcl/features/boundary.h>
00042 #include <pcl/features/normal_3d.h>
00043 #include <pcl/features/integral_image_normal.h>
00044 
00045 #include <pcl/keypoints/iss_3d.h>
00046 
00048 template<typename PointInT, typename PointOutT, typename NormalT> void
00049 pcl::ISSKeypoint3D<PointInT, PointOutT, NormalT>::setSalientRadius (double salient_radius)
00050 {
00051   salient_radius_ = salient_radius;
00052 }
00053 
00055 template<typename PointInT, typename PointOutT, typename NormalT> void
00056 pcl::ISSKeypoint3D<PointInT, PointOutT, NormalT>::setNonMaxRadius (double non_max_radius)
00057 {
00058   non_max_radius_ = non_max_radius;
00059 }
00060 
00062 template<typename PointInT, typename PointOutT, typename NormalT> void
00063 pcl::ISSKeypoint3D<PointInT, PointOutT, NormalT>::setNormalRadius (double normal_radius)
00064 {
00065   normal_radius_ = normal_radius;
00066 }
00067 
00069 template<typename PointInT, typename PointOutT, typename NormalT> void
00070 pcl::ISSKeypoint3D<PointInT, PointOutT, NormalT>::setBorderRadius (double border_radius)
00071 {
00072   border_radius_ = border_radius;
00073 }
00074 
00076 template<typename PointInT, typename PointOutT, typename NormalT> void
00077 pcl::ISSKeypoint3D<PointInT, PointOutT, NormalT>::setThreshold21 (double gamma_21)
00078 {
00079   gamma_21_ = gamma_21;
00080 }
00081 
00083 template<typename PointInT, typename PointOutT, typename NormalT> void
00084 pcl::ISSKeypoint3D<PointInT, PointOutT, NormalT>::setThreshold32 (double gamma_32)
00085 {
00086   gamma_32_ = gamma_32;
00087 }
00088 
00090 template<typename PointInT, typename PointOutT, typename NormalT> void
00091 pcl::ISSKeypoint3D<PointInT, PointOutT, NormalT>::setMinNeighbors (int min_neighbors)
00092 {
00093   min_neighbors_ = min_neighbors;
00094 }
00095 
00097 template<typename PointInT, typename PointOutT, typename NormalT> void
00098 pcl::ISSKeypoint3D<PointInT, PointOutT, NormalT>::setNormals (const PointCloudNConstPtr &normals)
00099 {
00100   normals_ = normals;
00101 }
00102 
00104 template<typename PointInT, typename PointOutT, typename NormalT> bool*
00105 pcl::ISSKeypoint3D<PointInT, PointOutT, NormalT>::getBoundaryPoints (PointCloudIn &input, double border_radius, float angle_threshold)
00106 {
00107   bool* edge_points = new bool [input.size ()];
00108 
00109   Eigen::Vector4f u = Eigen::Vector4f::Zero ();
00110   Eigen::Vector4f v = Eigen::Vector4f::Zero ();
00111 
00112   pcl::BoundaryEstimation<PointInT, NormalT, pcl::Boundary> boundary_estimator;
00113   boundary_estimator.setInputCloud (input_);
00114 
00115   int index;
00116 #ifdef _OPENMP
00117 #pragma omp parallel for private(u, v) num_threads(threads_)
00118 #endif
00119   for (index = 0; index < int (input.points.size ()); index++)
00120   {
00121     edge_points[index] = false;
00122     PointInT current_point = input.points[index];
00123 
00124     if (pcl::isFinite(current_point))
00125     {
00126       std::vector<int> nn_indices;
00127       std::vector<float> nn_distances;
00128       int n_neighbors;
00129 
00130       this->searchForNeighbors (static_cast<int> (index), border_radius, nn_indices, nn_distances);
00131 
00132       n_neighbors = static_cast<int> (nn_indices.size ());
00133 
00134       if (n_neighbors >= min_neighbors_)
00135       {
00136         boundary_estimator.getCoordinateSystemOnPlane (normals_->points[index], u, v);
00137 
00138         if (boundary_estimator.isBoundaryPoint (input, static_cast<int> (index), nn_indices, u, v, angle_threshold))
00139           edge_points[index] = true;
00140       }
00141     }
00142   }
00143 
00144   return (edge_points);
00145 }
00146 
00148 template<typename PointInT, typename PointOutT, typename NormalT> void
00149 pcl::ISSKeypoint3D<PointInT, PointOutT, NormalT>::getScatterMatrix (const int& current_index, Eigen::Matrix3d &cov_m)
00150 {
00151   const PointInT& current_point = (*input_).points[current_index];
00152 
00153   double central_point[3];
00154   memset(central_point, 0, sizeof(double) * 3);
00155 
00156   central_point[0] = current_point.x;
00157   central_point[1] = current_point.y;
00158   central_point[2] = current_point.z;
00159 
00160   cov_m = Eigen::Matrix3d::Zero ();
00161 
00162   std::vector<int> nn_indices;
00163   std::vector<float> nn_distances;
00164   int n_neighbors;
00165 
00166   this->searchForNeighbors (current_index, salient_radius_, nn_indices, nn_distances);
00167 
00168   n_neighbors = static_cast<int> (nn_indices.size ());
00169 
00170   if (n_neighbors < min_neighbors_)
00171     return;
00172 
00173   double cov[9];
00174   memset(cov, 0, sizeof(double) * 9);
00175 
00176   for (size_t n_idx = 0; n_idx < n_neighbors; n_idx++)
00177   {
00178     const PointInT& n_point = (*input_).points[nn_indices[n_idx]];
00179 
00180     double neigh_point[3];
00181     memset(neigh_point, 0, sizeof(double) * 3);
00182 
00183     neigh_point[0] = n_point.x;
00184     neigh_point[1] = n_point.y;
00185     neigh_point[2] = n_point.z;
00186 
00187     for (int i = 0; i < 3; i++)
00188       for (int j = 0; j < 3; j++)
00189         cov[i * 3 + j] += (neigh_point[i] - central_point[i]) * (neigh_point[j] - central_point[j]);
00190   }
00191 
00192   cov_m << cov[0], cov[1], cov[2],
00193            cov[3], cov[4], cov[5],
00194            cov[6], cov[7], cov[8];
00195 }
00196 
00198 template<typename PointInT, typename PointOutT, typename NormalT> bool
00199 pcl::ISSKeypoint3D<PointInT, PointOutT, NormalT>::initCompute ()
00200 {
00201   if (!Keypoint<PointInT, PointOutT>::initCompute ())
00202   {
00203     PCL_ERROR ("[pcl::%s::initCompute] init failed!\n", name_.c_str ());
00204     return (false);
00205   }
00206   if (salient_radius_ <= 0)
00207   {
00208     PCL_ERROR ("[pcl::%s::initCompute] : the salient radius (%f) must be strict positive!\n",
00209                 name_.c_str (), salient_radius_);
00210     return (false);
00211   }
00212   if (non_max_radius_ <= 0)
00213   {
00214     PCL_ERROR ("[pcl::%s::initCompute] : the non maxima radius (%f) must be strict positive!\n",
00215                 name_.c_str (), non_max_radius_);
00216     return (false);
00217   }
00218   if (gamma_21_ <= 0)
00219   {
00220     PCL_ERROR ("[pcl::%s::initCompute] : the threshold on the ratio between the 2nd and the 1rst eigenvalue (%f) must be strict positive!\n",
00221                 name_.c_str (), gamma_21_);
00222     return (false);
00223   }
00224   if (gamma_32_ <= 0)
00225   {
00226     PCL_ERROR ("[pcl::%s::initCompute] : the threshold on the ratio between the 3rd and the 2nd eigenvalue (%f) must be strict positive!\n",
00227                 name_.c_str (), gamma_32_);
00228     return (false);
00229   }
00230   if (min_neighbors_ <= 0)
00231   {
00232     PCL_ERROR ("[pcl::%s::initCompute] : the minimum number of neighbors (%f) must be strict positive!\n",
00233                 name_.c_str (), min_neighbors_);
00234     return (false);
00235   }
00236 
00237   if (third_eigen_value_)
00238     delete[] third_eigen_value_;
00239 
00240   third_eigen_value_ = new double[input_->size ()];
00241   memset(third_eigen_value_, 0, sizeof(double) * input_->size ());
00242 
00243   if (edge_points_)
00244     delete[] edge_points_;
00245 
00246   if (border_radius_ > 0.0)
00247   {
00248     if (normals_->empty ())
00249     {
00250       if (normal_radius_ <= 0.)
00251       {
00252         PCL_ERROR ("[pcl::%s::initCompute] : the radius used to estimate surface normals (%f) must be positive!\n",
00253         name_.c_str (), normal_radius_);
00254         return (false);
00255       }
00256 
00257       PointCloudNPtr normal_ptr (new PointCloudN ());
00258       if (input_->height == 1 )
00259       {
00260         pcl::NormalEstimation<PointInT, NormalT> normal_estimation;
00261         normal_estimation.setInputCloud (surface_);
00262         normal_estimation.setRadiusSearch (normal_radius_);
00263         normal_estimation.compute (*normal_ptr);
00264       }
00265       else
00266       {
00267         pcl::IntegralImageNormalEstimation<PointInT, NormalT> normal_estimation;
00268         normal_estimation.setNormalEstimationMethod (pcl::IntegralImageNormalEstimation<PointInT, NormalT>::SIMPLE_3D_GRADIENT);
00269         normal_estimation.setInputCloud (surface_);
00270         normal_estimation.setNormalSmoothingSize (5.0);
00271         normal_estimation.compute (*normal_ptr);
00272       }
00273       normals_ = normal_ptr;
00274     }
00275     if (normals_->size () != surface_->size ())
00276     {
00277       PCL_ERROR ("[pcl::%s::initCompute] normals given, but the number of normals does not match the number of input points!\n", name_.c_str ());
00278       return (false);
00279     }
00280   }
00281   else if (border_radius_ < 0.0)
00282   {
00283     PCL_ERROR ("[pcl::%s::initCompute] : the border radius used to estimate boundary points (%f) must be positive!\n",
00284                 name_.c_str (), border_radius_);
00285     return (false);
00286   }
00287 
00288   return (true);
00289 }
00290 
00292 template<typename PointInT, typename PointOutT, typename NormalT> void
00293 pcl::ISSKeypoint3D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloudOut &output)
00294 {
00295   // Make sure the output cloud is empty
00296   output.points.clear ();
00297 
00298   if (border_radius_ > 0.0)
00299     edge_points_ = getBoundaryPoints (*(input_->makeShared ()), border_radius_, angle_threshold_);
00300 
00301   bool* borders = new bool [input_->size()];
00302 
00303   int index;
00304 #ifdef _OPENMP
00305   #pragma omp parallel for num_threads(threads_)
00306 #endif
00307   for (index = 0; index < int (input_->size ()); index++)
00308   {
00309     borders[index] = false;
00310     PointInT current_point = input_->points[index];
00311 
00312     if ((border_radius_ > 0.0) && (pcl::isFinite(current_point)))
00313     {
00314       std::vector<int> nn_indices;
00315       std::vector<float> nn_distances;
00316 
00317       this->searchForNeighbors (static_cast<int> (index), border_radius_, nn_indices, nn_distances);
00318 
00319       for (int j = 0 ; j < nn_indices.size (); j++)
00320       {
00321         if (edge_points_[nn_indices[j]])
00322         {
00323           borders[index] = true;
00324           break;
00325         }
00326       }
00327     }
00328   }
00329 
00330   Eigen::Vector3d *omp_mem = new Eigen::Vector3d[threads_];
00331 
00332   for (int i = 0; i < threads_; i++)
00333     omp_mem[i].setZero (3);
00334 
00335   double *prg_local_mem = new double[input_->size () * 3];
00336   double **prg_mem = new double * [input_->size ()];
00337 
00338   for (int i = 0; i < input_->size (); i++)
00339     prg_mem[i] = prg_local_mem + 3 * i;
00340 
00341 #ifdef _OPENMP
00342   #pragma omp parallel for num_threads(threads_)
00343 #endif
00344   for (index = 0; index < static_cast<int> (input_->size ()); index++)
00345   {
00346 #ifdef _OPENMP
00347     int tid = omp_get_thread_num ();
00348 #else
00349     int tid = 0;
00350 #endif
00351     PointInT current_point = input_->points[index];
00352 
00353     if ((!borders[index]) && pcl::isFinite(current_point))
00354     {
00355       //if the considered point is not a border point and the point is "finite", then compute the scatter matrix
00356       Eigen::Matrix3d cov_m = Eigen::Matrix3d::Zero ();
00357       getScatterMatrix (static_cast<int> (index), cov_m);
00358 
00359       Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver (cov_m);
00360 
00361       const double& e1c = solver.eigenvalues ()[2];
00362       const double& e2c = solver.eigenvalues ()[1];
00363       const double& e3c = solver.eigenvalues ()[0];
00364 
00365       if (!pcl_isfinite (e1c) || !pcl_isfinite (e2c) || !pcl_isfinite (e3c))
00366         continue;
00367 
00368       if (e3c < 0)
00369       {
00370         PCL_WARN ("[pcl::%s::detectKeypoints] : The third eigenvalue is negative! Skipping the point with index %i.\n",
00371                   name_.c_str (), index);
00372         continue;
00373       }
00374 
00375       omp_mem[tid][0] = e2c / e1c;
00376       omp_mem[tid][1] = e3c / e2c;;
00377       omp_mem[tid][2] = e3c;
00378     }
00379 
00380     for (int d = 0; d < omp_mem[tid].size (); d++)
00381         prg_mem[index][d] = omp_mem[tid][d];
00382   }
00383 
00384   for (index = 0; index < int (input_->size ()); index++)
00385   {
00386    if (!borders[index])
00387     {
00388       if ((prg_mem[index][0] < gamma_21_) && (prg_mem[index][1] < gamma_32_))
00389         third_eigen_value_[index] = prg_mem[index][2];
00390     }
00391   }
00392 
00393   bool* feat_max = new bool [input_->size()];
00394   bool is_max;
00395 
00396 #ifdef _OPENMP
00397   #pragma omp parallel for private(is_max) num_threads(threads_)
00398 #endif
00399   for (index = 0; index < int (input_->size ()); index++)
00400   {
00401     feat_max [index] = false;
00402     PointInT current_point = input_->points[index];
00403 
00404     if ((third_eigen_value_[index] > 0.0) && (pcl::isFinite(current_point)))
00405     {
00406       std::vector<int> nn_indices;
00407       std::vector<float> nn_distances;
00408       int n_neighbors;
00409 
00410       this->searchForNeighbors (static_cast<int> (index), non_max_radius_, nn_indices, nn_distances);
00411 
00412       n_neighbors = static_cast<int> (nn_indices.size ());
00413 
00414       if (n_neighbors >= min_neighbors_)
00415       {
00416         is_max = true;
00417 
00418         for (size_t j = 0 ; j < n_neighbors; j++)
00419           if (third_eigen_value_[index] < third_eigen_value_[nn_indices[j]])
00420             is_max = false;
00421         if (is_max)
00422           feat_max[index] = true;
00423       }
00424     }
00425   }
00426 
00427 #ifdef _OPENMP
00428 #pragma omp parallel for shared (output) num_threads(threads_)
00429 #endif
00430   for (index = 0; index < int (input_->size ()); index++)
00431   {
00432     if (feat_max[index])
00433 #ifdef _OPENMP
00434 #pragma omp critical
00435 #endif
00436       output.points.push_back(input_->points[index]);
00437   }
00438 
00439   output.header = input_->header;
00440   output.width = static_cast<uint32_t> (output.points.size ());
00441   output.height = 1;
00442 
00443   // Clear the contents of variables and arrays before the beginning of the next computation.
00444   if (border_radius_ > 0.0)
00445     normals_.reset (new pcl::PointCloud<NormalT>);
00446 
00447   delete[] borders;
00448   delete[] prg_mem;
00449   delete[] prg_local_mem;
00450   delete[] feat_max;
00451 }
00452 
00453 #define PCL_INSTANTIATE_ISSKeypoint3D(T,U,N) template class PCL_EXPORTS pcl::ISSKeypoint3D<T,U,N>;
00454 
00455 #endif /* PCL_ISS_3D_IMPL_H_ */


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:25:03