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_HARRIS_KEYPOINT_3D_IMPL_H_ 00039 #define PCL_HARRIS_KEYPOINT_3D_IMPL_H_ 00040 00041 #include <pcl/keypoints/harris_3d.h> 00042 #include <pcl/common/io.h> 00043 #include <pcl/filters/passthrough.h> 00044 #include <pcl/filters/extract_indices.h> 00045 #include <pcl/features/normal_3d.h> 00046 #include <pcl/features/integral_image_normal.h> 00047 #include <pcl/common/time.h> 00048 #include <pcl/common/centroid.h> 00049 #ifdef HAVE_SSE_EXTENSIONS 00050 #include <xmmintrin.h> 00051 #endif 00052 00053 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00054 template <typename PointInT, typename PointOutT, typename NormalT> void 00055 pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::setMethod (ResponseMethod method) 00056 { 00057 method_ = method; 00058 } 00059 00060 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00061 template <typename PointInT, typename PointOutT, typename NormalT> void 00062 pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::setThreshold (float threshold) 00063 { 00064 threshold_= threshold; 00065 } 00066 00067 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00068 template <typename PointInT, typename PointOutT, typename NormalT> void 00069 pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::setRadius (float radius) 00070 { 00071 search_radius_ = radius; 00072 } 00073 00074 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00075 template <typename PointInT, typename PointOutT, typename NormalT> void 00076 pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::setRefine (bool do_refine) 00077 { 00078 refine_ = do_refine; 00079 } 00080 00081 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00082 template <typename PointInT, typename PointOutT, typename NormalT> void 00083 pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::setNonMaxSupression (bool nonmax) 00084 { 00085 nonmax_ = nonmax; 00086 } 00087 00088 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00089 template <typename PointInT, typename PointOutT, typename NormalT> void 00090 pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::setNormals (const PointCloudNConstPtr &normals) 00091 { 00092 normals_ = normals; 00093 } 00094 00095 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00096 template <typename PointInT, typename PointOutT, typename NormalT> void 00097 pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::calculateNormalCovar (const std::vector<int>& neighbors, float* coefficients) const 00098 { 00099 unsigned count = 0; 00100 // indices 0 1 2 3 4 5 6 7 00101 // coefficients: xx xy xz ?? yx yy yz ?? 00102 #ifdef HAVE_SSE_EXTENSIONS 00103 // accumulator for xx, xy, xz 00104 __m128 vec1 = _mm_setzero_ps(); 00105 // accumulator for yy, yz, zz 00106 __m128 vec2 = _mm_setzero_ps(); 00107 00108 __m128 norm1; 00109 00110 __m128 norm2; 00111 00112 float zz = 0; 00113 00114 for (std::vector<int>::const_iterator iIt = neighbors.begin(); iIt != neighbors.end(); ++iIt) 00115 { 00116 if (pcl_isfinite (normals_->points[*iIt].normal_x)) 00117 { 00118 // nx, ny, nz, h 00119 norm1 = _mm_load_ps (&(normals_->points[*iIt].normal_x)); 00120 00121 // nx, nx, nx, nx 00122 norm2 = _mm_set1_ps (normals_->points[*iIt].normal_x); 00123 00124 // nx * nx, nx * ny, nx * nz, nx * h 00125 norm2 = _mm_mul_ps (norm1, norm2); 00126 00127 // accumulate 00128 vec1 = _mm_add_ps (vec1, norm2); 00129 00130 // ny, ny, ny, ny 00131 norm2 = _mm_set1_ps (normals_->points[*iIt].normal_y); 00132 00133 // ny * nx, ny * ny, ny * nz, ny * h 00134 norm2 = _mm_mul_ps (norm1, norm2); 00135 00136 // accumulate 00137 vec2 = _mm_add_ps (vec2, norm2); 00138 00139 zz += normals_->points[*iIt].normal_z * normals_->points[*iIt].normal_z; 00140 ++count; 00141 } 00142 } 00143 if (count > 0) 00144 { 00145 norm2 = _mm_set1_ps (float(count)); 00146 vec1 = _mm_div_ps (vec1, norm2); 00147 vec2 = _mm_div_ps (vec2, norm2); 00148 _mm_store_ps (coefficients, vec1); 00149 _mm_store_ps (coefficients + 4, vec2); 00150 coefficients [7] = zz / float(count); 00151 } 00152 else 00153 memset (coefficients, 0, sizeof (float) * 8); 00154 #else 00155 memset (coefficients, 0, sizeof (float) * 8); 00156 for (std::vector<int>::const_iterator iIt = neighbors.begin(); iIt != neighbors.end(); ++iIt) 00157 { 00158 if (pcl_isfinite (normals_->points[*iIt].normal_x)) 00159 { 00160 coefficients[0] += normals_->points[*iIt].normal_x * normals_->points[*iIt].normal_x; 00161 coefficients[1] += normals_->points[*iIt].normal_x * normals_->points[*iIt].normal_y; 00162 coefficients[2] += normals_->points[*iIt].normal_x * normals_->points[*iIt].normal_z; 00163 00164 coefficients[5] += normals_->points[*iIt].normal_y * normals_->points[*iIt].normal_y; 00165 coefficients[6] += normals_->points[*iIt].normal_y * normals_->points[*iIt].normal_z; 00166 coefficients[7] += normals_->points[*iIt].normal_z * normals_->points[*iIt].normal_z; 00167 00168 ++count; 00169 } 00170 } 00171 if (count > 0) 00172 { 00173 float norm = 1.0 / float (count); 00174 coefficients[0] *= norm; 00175 coefficients[1] *= norm; 00176 coefficients[2] *= norm; 00177 coefficients[5] *= norm; 00178 coefficients[6] *= norm; 00179 coefficients[7] *= norm; 00180 } 00181 #endif 00182 } 00183 00184 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00185 template <typename PointInT, typename PointOutT, typename NormalT> bool 00186 pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::initCompute () 00187 { 00188 if (!Keypoint<PointInT, PointOutT>::initCompute ()) 00189 { 00190 PCL_ERROR ("[pcl::%s::initCompute] init failed!\n", name_.c_str ()); 00191 return (false); 00192 } 00193 00194 if (method_ < 1 || method_ > 5) 00195 { 00196 PCL_ERROR ("[pcl::%s::initCompute] method (%d) must be in [1..5]!\n", name_.c_str (), method_); 00197 return (false); 00198 } 00199 00200 if (!normals_) 00201 { 00202 PointCloudNPtr normals (new PointCloudN ()); 00203 normals->reserve (normals->size ()); 00204 if (!surface_->isOrganized ()) 00205 { 00206 pcl::NormalEstimation<PointInT, NormalT> normal_estimation; 00207 normal_estimation.setInputCloud (surface_); 00208 normal_estimation.setRadiusSearch (search_radius_); 00209 normal_estimation.compute (*normals); 00210 } 00211 else 00212 { 00213 IntegralImageNormalEstimation<PointInT, NormalT> normal_estimation; 00214 normal_estimation.setNormalEstimationMethod (pcl::IntegralImageNormalEstimation<PointInT, NormalT>::SIMPLE_3D_GRADIENT); 00215 normal_estimation.setInputCloud (surface_); 00216 normal_estimation.setNormalSmoothingSize (5.0); 00217 normal_estimation.compute (*normals); 00218 } 00219 normals_ = normals; 00220 } 00221 if (normals_->size () != surface_->size ()) 00222 { 00223 PCL_ERROR ("[pcl::%s::initCompute] normals given, but the number of normals does not match the number of input points!\n", name_.c_str (), method_); 00224 return (false); 00225 } 00226 return (true); 00227 } 00228 00229 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00230 template <typename PointInT, typename PointOutT, typename NormalT> void 00231 pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloudOut &output) 00232 { 00233 boost::shared_ptr<pcl::PointCloud<PointOutT> > response (new pcl::PointCloud<PointOutT> ()); 00234 00235 response->points.reserve (input_->points.size()); 00236 00237 switch (method_) 00238 { 00239 case HARRIS: 00240 responseHarris(*response); 00241 break; 00242 case NOBLE: 00243 responseNoble(*response); 00244 break; 00245 case LOWE: 00246 responseLowe(*response); 00247 break; 00248 case CURVATURE: 00249 responseCurvature(*response); 00250 break; 00251 case TOMASI: 00252 responseTomasi(*response); 00253 break; 00254 } 00255 00256 if (!nonmax_) 00257 { 00258 output = *response; 00259 // we do not change the denseness in this case 00260 output.is_dense = input_->is_dense; 00261 } 00262 else 00263 { 00264 output.points.clear (); 00265 output.points.reserve (response->points.size()); 00266 00267 #ifdef _OPENMP 00268 #pragma omp parallel for shared (output) num_threads(threads_) 00269 #endif 00270 for (int idx = 0; idx < static_cast<int> (response->points.size ()); ++idx) 00271 { 00272 if (!isFinite (response->points[idx]) || 00273 !pcl_isfinite (response->points[idx].intensity) || 00274 response->points[idx].intensity < threshold_) 00275 continue; 00276 00277 std::vector<int> nn_indices; 00278 std::vector<float> nn_dists; 00279 tree_->radiusSearch (idx, search_radius_, nn_indices, nn_dists); 00280 bool is_maxima = true; 00281 for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt) 00282 { 00283 if (response->points[idx].intensity < response->points[*iIt].intensity) 00284 { 00285 is_maxima = false; 00286 break; 00287 } 00288 } 00289 if (is_maxima) 00290 #ifdef _OPENMP 00291 #pragma omp critical 00292 #endif 00293 output.points.push_back (response->points[idx]); 00294 } 00295 00296 if (refine_) 00297 refineCorners (output); 00298 00299 output.height = 1; 00300 output.width = static_cast<uint32_t> (output.points.size()); 00301 output.is_dense = true; 00302 } 00303 } 00304 00305 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00306 template <typename PointInT, typename PointOutT, typename NormalT> void 00307 pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::responseHarris (PointCloudOut &output) const 00308 { 00309 PCL_ALIGN (16) float covar [8]; 00310 output.resize (input_->size ()); 00311 #ifdef _OPENMP 00312 #pragma omp parallel for shared (output) private (covar) num_threads(threads_) 00313 #endif 00314 for (int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx) 00315 { 00316 const PointInT& pointIn = input_->points [pIdx]; 00317 output [pIdx].intensity = 0.0; //std::numeric_limits<float>::quiet_NaN (); 00318 if (isFinite (pointIn)) 00319 { 00320 std::vector<int> nn_indices; 00321 std::vector<float> nn_dists; 00322 tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists); 00323 calculateNormalCovar (nn_indices, covar); 00324 00325 float trace = covar [0] + covar [5] + covar [7]; 00326 if (trace != 0) 00327 { 00328 float det = covar [0] * covar [5] * covar [7] + 2.0f * covar [1] * covar [2] * covar [6] 00329 - covar [2] * covar [2] * covar [5] 00330 - covar [1] * covar [1] * covar [7] 00331 - covar [6] * covar [6] * covar [0]; 00332 00333 output [pIdx].intensity = 0.04f + det - 0.04f * trace * trace; 00334 } 00335 } 00336 output [pIdx].x = pointIn.x; 00337 output [pIdx].y = pointIn.y; 00338 output [pIdx].z = pointIn.z; 00339 } 00340 output.height = input_->height; 00341 output.width = input_->width; 00342 } 00343 00344 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00345 template <typename PointInT, typename PointOutT, typename NormalT> void 00346 pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::responseNoble (PointCloudOut &output) const 00347 { 00348 PCL_ALIGN (16) float covar [8]; 00349 output.resize (input_->size ()); 00350 #ifdef _OPENMP 00351 #pragma omp parallel for shared (output) private (covar) num_threads(threads_) 00352 #endif 00353 for (int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx) 00354 { 00355 const PointInT& pointIn = input_->points [pIdx]; 00356 output [pIdx].intensity = 0.0; 00357 if (isFinite (pointIn)) 00358 { 00359 std::vector<int> nn_indices; 00360 std::vector<float> nn_dists; 00361 tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists); 00362 calculateNormalCovar (nn_indices, covar); 00363 float trace = covar [0] + covar [5] + covar [7]; 00364 if (trace != 0) 00365 { 00366 float det = covar [0] * covar [5] * covar [7] + 2.0f * covar [1] * covar [2] * covar [6] 00367 - covar [2] * covar [2] * covar [5] 00368 - covar [1] * covar [1] * covar [7] 00369 - covar [6] * covar [6] * covar [0]; 00370 00371 output [pIdx].intensity = det / trace; 00372 } 00373 } 00374 output [pIdx].x = pointIn.x; 00375 output [pIdx].y = pointIn.y; 00376 output [pIdx].z = pointIn.z; 00377 } 00378 output.height = input_->height; 00379 output.width = input_->width; 00380 } 00381 00382 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00383 template <typename PointInT, typename PointOutT, typename NormalT> void 00384 pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::responseLowe (PointCloudOut &output) const 00385 { 00386 PCL_ALIGN (16) float covar [8]; 00387 output.resize (input_->size ()); 00388 #ifdef _OPENMP 00389 #pragma omp parallel for shared (output) private (covar) num_threads(threads_) 00390 #endif 00391 for (int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx) 00392 { 00393 const PointInT& pointIn = input_->points [pIdx]; 00394 output [pIdx].intensity = 0.0; 00395 if (isFinite (pointIn)) 00396 { 00397 std::vector<int> nn_indices; 00398 std::vector<float> nn_dists; 00399 tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists); 00400 calculateNormalCovar (nn_indices, covar); 00401 float trace = covar [0] + covar [5] + covar [7]; 00402 if (trace != 0) 00403 { 00404 float det = covar [0] * covar [5] * covar [7] + 2.0f * covar [1] * covar [2] * covar [6] 00405 - covar [2] * covar [2] * covar [5] 00406 - covar [1] * covar [1] * covar [7] 00407 - covar [6] * covar [6] * covar [0]; 00408 00409 output [pIdx].intensity = det / (trace * trace); 00410 } 00411 } 00412 output [pIdx].x = pointIn.x; 00413 output [pIdx].y = pointIn.y; 00414 output [pIdx].z = pointIn.z; 00415 } 00416 output.height = input_->height; 00417 output.width = input_->width; 00418 } 00419 00420 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00421 template <typename PointInT, typename PointOutT, typename NormalT> void 00422 pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::responseCurvature (PointCloudOut &output) const 00423 { 00424 PointOutT point; 00425 for (unsigned idx = 0; idx < input_->points.size(); ++idx) 00426 { 00427 point.x = input_->points[idx].x; 00428 point.y = input_->points[idx].y; 00429 point.z = input_->points[idx].z; 00430 point.intensity = normals_->points[idx].curvature; 00431 output.points.push_back(point); 00432 } 00433 // does not change the order 00434 output.height = input_->height; 00435 output.width = input_->width; 00436 } 00437 00438 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00439 template <typename PointInT, typename PointOutT, typename NormalT> void 00440 pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::responseTomasi (PointCloudOut &output) const 00441 { 00442 PCL_ALIGN (16) float covar [8]; 00443 Eigen::Matrix3f covariance_matrix; 00444 output.resize (input_->size ()); 00445 #ifdef _OPENMP 00446 #pragma omp parallel for shared (output) private (covar, covariance_matrix) num_threads(threads_) 00447 #endif 00448 for (int pIdx = 0; pIdx < static_cast<int> (input_->size ()); ++pIdx) 00449 { 00450 const PointInT& pointIn = input_->points [pIdx]; 00451 output [pIdx].intensity = 0.0; 00452 if (isFinite (pointIn)) 00453 { 00454 std::vector<int> nn_indices; 00455 std::vector<float> nn_dists; 00456 tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists); 00457 calculateNormalCovar (nn_indices, covar); 00458 float trace = covar [0] + covar [5] + covar [7]; 00459 if (trace != 0) 00460 { 00461 covariance_matrix.coeffRef (0) = covar [0]; 00462 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = covar [1]; 00463 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = covar [2]; 00464 covariance_matrix.coeffRef (4) = covar [5]; 00465 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = covar [6]; 00466 covariance_matrix.coeffRef (8) = covar [7]; 00467 00468 EIGEN_ALIGN16 Eigen::Vector3f eigen_values; 00469 pcl::eigen33(covariance_matrix, eigen_values); 00470 output [pIdx].intensity = eigen_values[0]; 00471 } 00472 } 00473 output [pIdx].x = pointIn.x; 00474 output [pIdx].y = pointIn.y; 00475 output [pIdx].z = pointIn.z; 00476 } 00477 output.height = input_->height; 00478 output.width = input_->width; 00479 } 00480 00481 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00482 template <typename PointInT, typename PointOutT, typename NormalT> void 00483 pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::refineCorners (PointCloudOut &corners) const 00484 { 00485 Eigen::Matrix3f nnT; 00486 Eigen::Matrix3f NNT; 00487 Eigen::Matrix3f NNTInv; 00488 Eigen::Vector3f NNTp; 00489 float diff; 00490 const unsigned max_iterations = 10; 00491 #ifdef _OPENMP 00492 #pragma omp parallel for shared (corners) private (nnT, NNT, NNTInv, NNTp, diff) num_threads(threads_) 00493 #endif 00494 for (int cIdx = 0; cIdx < static_cast<int> (corners.size ()); ++cIdx) 00495 { 00496 unsigned iterations = 0; 00497 do { 00498 NNT.setZero(); 00499 NNTp.setZero(); 00500 PointInT corner; 00501 corner.x = corners[cIdx].x; 00502 corner.y = corners[cIdx].y; 00503 corner.z = corners[cIdx].z; 00504 std::vector<int> nn_indices; 00505 std::vector<float> nn_dists; 00506 tree_->radiusSearch (corner, search_radius_, nn_indices, nn_dists); 00507 for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt) 00508 { 00509 if (!pcl_isfinite (normals_->points[*iIt].normal_x)) 00510 continue; 00511 00512 nnT = normals_->points[*iIt].getNormalVector3fMap () * normals_->points[*iIt].getNormalVector3fMap ().transpose(); 00513 NNT += nnT; 00514 NNTp += nnT * surface_->points[*iIt].getVector3fMap (); 00515 } 00516 if (invert3x3SymMatrix (NNT, NNTInv) != 0) 00517 corners[cIdx].getVector3fMap () = NNTInv * NNTp; 00518 00519 diff = (corners[cIdx].getVector3fMap () - corner.getVector3fMap()).squaredNorm (); 00520 } while (diff > 1e-6 && ++iterations < max_iterations); 00521 } 00522 } 00523 00524 #define PCL_INSTANTIATE_HarrisKeypoint3D(T,U,N) template class PCL_EXPORTS pcl::HarrisKeypoint3D<T,U,N>; 00525 #endif // #ifndef PCL_HARRIS_KEYPOINT_3D_IMPL_H_ 00526