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 * $Id$ 00037 * 00038 */ 00039 00040 #ifndef PCL_HARRIS_KEYPOINT_2D_IMPL_H_ 00041 #define PCL_HARRIS_KEYPOINT_2D_IMPL_H_ 00042 00043 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00044 template <typename PointInT, typename PointOutT, typename IntensityT> void 00045 pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::setMethod (ResponseMethod method) 00046 { 00047 method_ = method; 00048 } 00049 00050 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00051 template <typename PointInT, typename PointOutT, typename IntensityT> void 00052 pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::setThreshold (float threshold) 00053 { 00054 threshold_= threshold; 00055 } 00056 00057 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00058 template <typename PointInT, typename PointOutT, typename IntensityT> void 00059 pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::setRefine (bool do_refine) 00060 { 00061 refine_ = do_refine; 00062 } 00063 00064 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00065 template <typename PointInT, typename PointOutT, typename IntensityT> void 00066 pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::setNonMaxSupression (bool nonmax) 00067 { 00068 nonmax_ = nonmax; 00069 } 00070 00071 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00072 template <typename PointInT, typename PointOutT, typename IntensityT> void 00073 pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::setWindowWidth (int window_width) 00074 { 00075 window_width_= window_width; 00076 } 00077 00078 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00079 template <typename PointInT, typename PointOutT, typename IntensityT> void 00080 pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::setWindowHeight (int window_height) 00081 { 00082 window_height_= window_height; 00083 } 00084 00085 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00086 template <typename PointInT, typename PointOutT, typename IntensityT> void 00087 pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::setSkippedPixels (int skipped_pixels) 00088 { 00089 skipped_pixels_= skipped_pixels; 00090 } 00091 00092 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00093 template <typename PointInT, typename PointOutT, typename IntensityT> void 00094 pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::setMinimalDistance (int min_distance) 00095 { 00096 min_distance_ = min_distance; 00097 } 00098 00099 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00100 template <typename PointInT, typename PointOutT, typename IntensityT> void 00101 pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::computeSecondMomentMatrix (std::size_t index, float* coefficients) const 00102 { 00103 static const int width = static_cast<int> (input_->width); 00104 static const int height = static_cast<int> (input_->height); 00105 00106 int x = static_cast<int> (index % input_->width); 00107 int y = static_cast<int> (index / input_->width); 00108 unsigned count = 0; 00109 // indices 0 1 2 00110 // coefficients: ixix ixiy iyiy 00111 memset (coefficients, 0, sizeof (float) * 3); 00112 00113 int endx = std::min (width, x + half_window_width_); 00114 int endy = std::min (height, y + half_window_height_); 00115 for (int xx = std::max (0, x - half_window_width_); xx < endx; ++xx) 00116 for (int yy = std::max (0, y - half_window_height_); yy < endy; ++yy) 00117 { 00118 const float& ix = derivatives_rows_ (xx,yy); 00119 const float& iy = derivatives_cols_ (xx,yy); 00120 coefficients[0]+= ix * ix; 00121 coefficients[1]+= ix * iy; 00122 coefficients[2]+= iy * iy; 00123 } 00124 } 00125 00126 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00127 template <typename PointInT, typename PointOutT, typename IntensityT> bool 00128 pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::initCompute () 00129 { 00130 if (!pcl::Keypoint<PointInT, PointOutT>::initCompute ()) 00131 { 00132 PCL_ERROR ("[pcl::%s::initCompute] init failed.!\n", name_.c_str ()); 00133 return (false); 00134 } 00135 00136 if (!input_->isOrganized ()) 00137 { 00138 PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ()); 00139 return (false); 00140 } 00141 00142 if (indices_->size () != input_->size ()) 00143 { 00144 PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support setting indices!\n", name_.c_str ()); 00145 return (false); 00146 } 00147 00148 if ((window_height_%2) == 0) 00149 { 00150 PCL_ERROR ("[pcl::%s::initCompute] Window height must be odd!\n", name_.c_str ()); 00151 return (false); 00152 } 00153 00154 if ((window_width_%2) == 0) 00155 { 00156 PCL_ERROR ("[pcl::%s::initCompute] Window width must be odd!\n", name_.c_str ()); 00157 return (false); 00158 } 00159 00160 if (window_height_ < 3 || window_width_ < 3) 00161 { 00162 PCL_ERROR ("[pcl::%s::initCompute] Window size must be >= 3x3!\n", name_.c_str ()); 00163 return (false); 00164 } 00165 00166 half_window_width_ = window_width_ / 2; 00167 half_window_height_ = window_height_ / 2; 00168 00169 return (true); 00170 } 00171 00172 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00173 template <typename PointInT, typename PointOutT, typename IntensityT> void 00174 pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::detectKeypoints (PointCloudOut &output) 00175 { 00176 derivatives_cols_.resize (input_->width, input_->height); 00177 derivatives_rows_.resize (input_->width, input_->height); 00178 //Compute cloud intensities first derivatives along columns and rows 00179 //!!! nsallem 20120220 : we don't test here for density so if one term in nan the result is nan 00180 int w = static_cast<int> (input_->width) - 1; 00181 int h = static_cast<int> (input_->height) - 1; 00182 // j = 0 --> j-1 out of range ; use 0 00183 // i = 0 --> i-1 out of range ; use 0 00184 derivatives_cols_(0,0) = (intensity_ ((*input_) (0,1)) - intensity_ ((*input_) (0,0))) * 0.5; 00185 derivatives_rows_(0,0) = (intensity_ ((*input_) (1,0)) - intensity_ ((*input_) (0,0))) * 0.5; 00186 00187 // #ifdef _OPENMP 00188 // //#pragma omp parallel for shared (derivatives_cols_, input_) num_threads (threads_) 00189 // #pragma omp parallel for num_threads (threads_) 00190 // #endif 00191 for(int i = 1; i < w; ++i) 00192 { 00193 derivatives_cols_(i,0) = (intensity_ ((*input_) (i,1)) - intensity_ ((*input_) (i,0))) * 0.5; 00194 } 00195 00196 derivatives_rows_(w,0) = (intensity_ ((*input_) (w,0)) - intensity_ ((*input_) (w-1,0))) * 0.5; 00197 derivatives_cols_(w,0) = (intensity_ ((*input_) (w,1)) - intensity_ ((*input_) (w,0))) * 0.5; 00198 00199 // #ifdef _OPENMP 00200 // //#pragma omp parallel for shared (derivatives_cols_, derivatives_rows_, input_) num_threads (threads_) 00201 // #pragma omp parallel for num_threads (threads_) 00202 // #endif 00203 for(int j = 1; j < h; ++j) 00204 { 00205 // i = 0 --> i-1 out of range ; use 0 00206 derivatives_rows_(0,j) = (intensity_ ((*input_) (1,j)) - intensity_ ((*input_) (0,j))) * 0.5; 00207 for(int i = 1; i < w; ++i) 00208 { 00209 // derivative with respect to rows 00210 derivatives_rows_(i,j) = (intensity_ ((*input_) (i+1,j)) - intensity_ ((*input_) (i-1,j))) * 0.5; 00211 00212 // derivative with respect to cols 00213 derivatives_cols_(i,j) = (intensity_ ((*input_) (i,j+1)) - intensity_ ((*input_) (i,j-1))) * 0.5; 00214 } 00215 // i = w --> w+1 out of range ; use w 00216 derivatives_rows_(w,j) = (intensity_ ((*input_) (w,j)) - intensity_ ((*input_) (w-1,j))) * 0.5; 00217 } 00218 00219 // j = h --> j+1 out of range use h 00220 derivatives_cols_(0,h) = (intensity_ ((*input_) (0,h)) - intensity_ ((*input_) (0,h-1))) * 0.5; 00221 derivatives_rows_(0,h) = (intensity_ ((*input_) (1,h)) - intensity_ ((*input_) (0,h))) * 0.5; 00222 00223 // #ifdef _OPENMP 00224 // //#pragma omp parallel for shared (derivatives_cols_, input_) num_threads (threads_) 00225 // #pragma omp parallel for num_threads (threads_) 00226 // #endif 00227 for(int i = 1; i < w; ++i) 00228 { 00229 derivatives_cols_(i,h) = (intensity_ ((*input_) (i,h)) - intensity_ ((*input_) (i,h-1))) * 0.5; 00230 } 00231 derivatives_rows_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w-1,h))) * 0.5; 00232 derivatives_cols_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w,h-1))) * 0.5; 00233 00234 float highest_response_; 00235 00236 switch (method_) 00237 { 00238 case HARRIS: 00239 responseHarris(*response_, highest_response_); 00240 break; 00241 case NOBLE: 00242 responseNoble(*response_, highest_response_); 00243 break; 00244 case LOWE: 00245 responseLowe(*response_, highest_response_); 00246 break; 00247 case TOMASI: 00248 responseTomasi(*response_, highest_response_); 00249 break; 00250 } 00251 00252 if (!nonmax_) 00253 output = *response_; 00254 else 00255 { 00256 threshold_*= highest_response_; 00257 00258 std::sort (indices_->begin (), indices_->end (), 00259 boost::bind (&HarrisKeypoint2D::greaterIntensityAtIndices, this, _1, _2)); 00260 00261 output.clear (); 00262 output.reserve (response_->size()); 00263 std::vector<bool> occupency_map (response_->size (), false); 00264 int width (response_->width); 00265 int height (response_->height); 00266 const int occupency_map_size (occupency_map.size ()); 00267 00268 #ifdef _OPENMP 00269 #pragma omp parallel for shared (output, occupency_map) private (width, height) num_threads(threads_) 00270 #endif 00271 for (int idx = 0; idx < occupency_map_size; ++idx) 00272 { 00273 if (occupency_map[idx] || response_->points [indices_->at (idx)].intensity < threshold_ || !isFinite (response_->points[idx])) 00274 continue; 00275 00276 #ifdef _OPENMP 00277 #pragma omp critical 00278 #endif 00279 output.push_back (response_->at (indices_->at (idx))); 00280 00281 int u_end = std::min (width, indices_->at (idx) % width + min_distance_); 00282 int v_end = std::min (height, indices_->at (idx) / width + min_distance_); 00283 for(int u = std::max (0, indices_->at (idx) % width - min_distance_); u < u_end; ++u) 00284 for(int v = std::max (0, indices_->at (idx) / width - min_distance_); v < v_end; ++v) 00285 occupency_map[v*input_->width+u] = true; 00286 } 00287 00288 // if (refine_) 00289 // refineCorners (output); 00290 00291 output.height = 1; 00292 output.width = static_cast<uint32_t> (output.size()); 00293 } 00294 00295 // we don not change the denseness 00296 output.is_dense = input_->is_dense; 00297 } 00298 00299 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00300 template <typename PointInT, typename PointOutT, typename IntensityT> void 00301 pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::responseHarris (PointCloudOut &output, float& highest_response) const 00302 { 00303 PCL_ALIGN (16) float covar [3]; 00304 output.clear (); 00305 output.resize (input_->size ()); 00306 highest_response = - std::numeric_limits<float>::max (); 00307 const int output_size (output.size ()); 00308 00309 #ifdef _OPENMP 00310 #pragma omp parallel for shared (output, highest_response) private (covar) num_threads(threads_) 00311 #endif 00312 for (int index = 0; index < output_size; ++index) 00313 { 00314 PointOutT& out_point = output.points [index]; 00315 const PointInT &in_point = (*input_).points [index]; 00316 out_point.intensity = 0; 00317 out_point.x = in_point.x; 00318 out_point.y = in_point.y; 00319 out_point.z = in_point.z; 00320 if (isFinite (in_point)) 00321 { 00322 computeSecondMomentMatrix (index, covar); 00323 float trace = covar [0] + covar [2]; 00324 if (trace != 0.f) 00325 { 00326 float det = covar[0] * covar[2] - covar[1] * covar[1]; 00327 out_point.intensity = 0.04f + det - 0.04f * trace * trace; 00328 00329 #ifdef _OPENMP 00330 #pragma omp critical 00331 #endif 00332 highest_response = (out_point.intensity > highest_response) ? out_point.intensity : highest_response; 00333 } 00334 } 00335 } 00336 00337 output.height = input_->height; 00338 output.width = input_->width; 00339 } 00340 00341 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00342 template <typename PointInT, typename PointOutT, typename IntensityT> void 00343 pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::responseNoble (PointCloudOut &output, float& highest_response) const 00344 { 00345 PCL_ALIGN (16) float covar [3]; 00346 output.clear (); 00347 output.resize (input_->size ()); 00348 highest_response = - std::numeric_limits<float>::max (); 00349 const int output_size (output.size ()); 00350 00351 #ifdef _OPENMP 00352 #pragma omp parallel for shared (output, highest_response) private (covar) num_threads(threads_) 00353 #endif 00354 for (size_t index = 0; index < output_size; ++index) 00355 { 00356 PointOutT &out_point = output.points [index]; 00357 const PointInT &in_point = input_->points [index]; 00358 out_point.x = in_point.x; 00359 out_point.y = in_point.y; 00360 out_point.z = in_point.z; 00361 out_point.intensity = 0; 00362 if (isFinite (in_point)) 00363 { 00364 computeSecondMomentMatrix (index, covar); 00365 float trace = covar [0] + covar [2]; 00366 if (trace != 0) 00367 { 00368 float det = covar[0] * covar[2] - covar[1] * covar[1]; 00369 out_point.intensity = det / trace; 00370 00371 #ifdef _OPENMP 00372 #pragma omp critical 00373 #endif 00374 highest_response = (out_point.intensity > highest_response) ? out_point.intensity : highest_response; 00375 } 00376 } 00377 } 00378 00379 output.height = input_->height; 00380 output.width = input_->width; 00381 } 00382 00383 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00384 template <typename PointInT, typename PointOutT, typename IntensityT> void 00385 pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::responseLowe (PointCloudOut &output, float& highest_response) const 00386 { 00387 PCL_ALIGN (16) float covar [3]; 00388 output.clear (); 00389 output.resize (input_->size ()); 00390 highest_response = -std::numeric_limits<float>::max (); 00391 const int output_size (output.size ()); 00392 00393 #ifdef _OPENMP 00394 #pragma omp parallel for shared (output, highest_response) private (covar) num_threads(threads_) 00395 #endif 00396 for (size_t index = 0; index < output_size; ++index) 00397 { 00398 PointOutT &out_point = output.points [index]; 00399 const PointInT &in_point = input_->points [index]; 00400 out_point.x = in_point.x; 00401 out_point.y = in_point.y; 00402 out_point.z = in_point.z; 00403 out_point.intensity = 0; 00404 if (isFinite (in_point)) 00405 { 00406 computeSecondMomentMatrix (index, covar); 00407 float trace = covar [0] + covar [2]; 00408 if (trace != 0) 00409 { 00410 float det = covar[0] * covar[2] - covar[1] * covar[1]; 00411 out_point.intensity = det / (trace * trace); 00412 00413 #ifdef _OPENMP 00414 #pragma omp critical 00415 #endif 00416 highest_response = (out_point.intensity > highest_response) ? out_point.intensity : highest_response; 00417 } 00418 } 00419 } 00420 00421 output.height = input_->height; 00422 output.width = input_->width; 00423 } 00424 00425 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00426 template <typename PointInT, typename PointOutT, typename IntensityT> void 00427 pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::responseTomasi (PointCloudOut &output, float& highest_response) const 00428 { 00429 PCL_ALIGN (16) float covar [3]; 00430 output.clear (); 00431 output.resize (input_->size ()); 00432 highest_response = -std::numeric_limits<float>::max (); 00433 const int output_size (output.size ()); 00434 00435 #ifdef _OPENMP 00436 #pragma omp parallel for shared (output, highest_response) private (covar) num_threads(threads_) 00437 #endif 00438 for (size_t index = 0; index < output_size; ++index) 00439 { 00440 PointOutT &out_point = output.points [index]; 00441 const PointInT &in_point = input_->points [index]; 00442 out_point.x = in_point.x; 00443 out_point.y = in_point.y; 00444 out_point.z = in_point.z; 00445 out_point.intensity = 0; 00446 if (isFinite (in_point)) 00447 { 00448 computeSecondMomentMatrix (index, covar); 00449 // min egenvalue 00450 out_point.intensity = ((covar[0] + covar[2] - sqrt((covar[0] - covar[2])*(covar[0] - covar[2]) + 4 * covar[1] * covar[1])) /2.0f); 00451 00452 #ifdef _OPENMP 00453 #pragma omp critical 00454 #endif 00455 highest_response = (out_point.intensity > highest_response) ? out_point.intensity : highest_response; 00456 } 00457 } 00458 00459 output.height = input_->height; 00460 output.width = input_->width; 00461 } 00462 00463 // ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00464 // template <typename PointInT, typename PointOutT, typename IntensityT> void 00465 // pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::refineCorners (PointCloudOut &corners) const 00466 // { 00467 // std::vector<int> nn_indices; 00468 // std::vector<float> nn_dists; 00469 00470 // Eigen::Matrix2f nnT; 00471 // Eigen::Matrix2f NNT; 00472 // Eigen::Matrix2f NNTInv; 00473 // Eigen::Vector2f NNTp; 00474 // float diff; 00475 // const unsigned max_iterations = 10; 00476 // #ifdef _OPENMP 00477 // #pragma omp parallel for shared (corners) private (nnT, NNT, NNTInv, NNTp, diff, nn_indices, nn_dists) num_threads(threads_) 00478 // #endif 00479 // for (int cIdx = 0; cIdx < static_cast<int> (corners.size ()); ++cIdx) 00480 // { 00481 // unsigned iterations = 0; 00482 // do { 00483 // NNT.setZero(); 00484 // NNTp.setZero(); 00485 // PointInT corner; 00486 // corner.x = corners[cIdx].x; 00487 // corner.y = corners[cIdx].y; 00488 // corner.z = corners[cIdx].z; 00489 // tree_->radiusSearch (corner, search_radius_, nn_indices, nn_dists); 00490 // for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt) 00491 // { 00492 // if (!pcl_isfinite (normals_->points[*iIt].normal_x)) 00493 // continue; 00494 00495 // nnT = normals_->points[*iIt].getNormalVector3fMap () * normals_->points[*iIt].getNormalVector3fMap ().transpose(); 00496 // NNT += nnT; 00497 // NNTp += nnT * surface_->points[*iIt].getVector3fMap (); 00498 // } 00499 // if (invert3x3SymMatrix (NNT, NNTInv) != 0) 00500 // corners[cIdx].getVector3fMap () = NNTInv * NNTp; 00501 00502 // diff = (corners[cIdx].getVector3fMap () - corner.getVector3fMap()).squaredNorm (); 00503 // } while (diff > 1e-6 && ++iterations < max_iterations); 00504 // } 00505 // } 00506 00507 #define PCL_INSTANTIATE_HarrisKeypoint2D(T,U,I) template class PCL_EXPORTS pcl::HarrisKeypoint2D<T,U,I>; 00508 #endif // #ifndef PCL_HARRIS_KEYPOINT_2D_IMPL_H_