Point Cloud Library (PCL)
1.7.0
|
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 00047 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00054 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00061 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00068 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00075 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00082 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00089 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00096 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00103 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00147 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00197 ////////////////////////////////////////////////////////////////////////////////////////////// 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 00291 ////////////////////////////////////////////////////////////////////////////////////////////// 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_ */