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_SUSAN_IMPL_HPP_ 00039 #define PCL_SUSAN_IMPL_HPP_ 00040 00041 #include <pcl/keypoints/susan.h> 00042 #include <pcl/features/normal_3d.h> 00043 #include <pcl/features/integral_image_normal.h> 00044 00045 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00046 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void 00047 pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::setNonMaxSupression (bool nonmax) 00048 { 00049 nonmax_ = nonmax; 00050 } 00051 00052 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00053 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void 00054 pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::setGeometricValidation (bool validate) 00055 { 00056 geometric_validation_ = validate; 00057 } 00058 00059 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00060 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void 00061 pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::setRadius (float radius) 00062 { 00063 search_radius_ = radius; 00064 } 00065 00066 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00067 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void 00068 pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::setDistanceThreshold (float distance_threshold) 00069 { 00070 distance_threshold_ = distance_threshold; 00071 } 00072 00073 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00074 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void 00075 pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::setAngularThreshold (float angular_threshold) 00076 { 00077 angular_threshold_ = angular_threshold; 00078 } 00079 00080 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00081 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void 00082 pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::setIntensityThreshold (float intensity_threshold) 00083 { 00084 intensity_threshold_ = intensity_threshold; 00085 } 00086 00087 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00088 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void 00089 pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::setNormals (const PointCloudNConstPtr &normals) 00090 { 00091 normals_ = normals; 00092 } 00093 00094 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00095 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void 00096 pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::setSearchSurface (const PointCloudInConstPtr &cloud) 00097 { 00098 surface_ = cloud; 00099 normals_.reset (new pcl::PointCloud<NormalT>); 00100 } 00101 00102 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00103 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void 00104 pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::setNumberOfThreads (unsigned int nr_threads) 00105 { 00106 threads_ = nr_threads; 00107 } 00108 00109 00110 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00111 // template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void 00112 // pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::USAN (const PointInT& nucleus, 00113 // const NormalT& nucleus_normal, 00114 // const std::vector<int>& neighbors, 00115 // const float& t, 00116 // float& response, 00117 // Eigen::Vector3f& centroid) const 00118 // { 00119 // float area = 0; 00120 // response = 0; 00121 // float x0 = nucleus.x; 00122 // float y0 = nucleus.y; 00123 // float z0 = nucleus.z; 00124 // //xx xy xz yy yz zz 00125 // std::vector<float> coefficients(6); 00126 // memset (&coefficients[0], 0, sizeof (float) * 6); 00127 // for (std::vector<int>::const_iterator index = neighbors.begin(); index != neighbors.end(); ++index) 00128 // { 00129 // if (pcl_isfinite (normals_->points[*index].normal_x)) 00130 // { 00131 // Eigen::Vector3f diff = normals_->points[*index].getNormal3fMap () - nucleus_normal.getNormal3fMap (); 00132 // float c = diff.norm () / t; 00133 // c = -1 * pow (c, 6.0); 00134 // c = exp (c); 00135 // Eigen::Vector3f xyz = surface_->points[*index].getVector3fMap (); 00136 // centroid += c * xyz; 00137 // area += c; 00138 // coefficients[0] += c * (x0 - xyz.x) * (x0 - xyz.x); 00139 // coefficients[1] += c * (x0 - xyz.x) * (y0 - xyz.y); 00140 // coefficients[2] += c * (x0 - xyz.x) * (z0 - xyz.z); 00141 // coefficients[3] += c * (y0 - xyz.y) * (y0 - xyz.y); 00142 // coefficients[4] += c * (y0 - xyz.y) * (z0 - xyz.z); 00143 // coefficients[5] += c * (z0 - xyz.z) * (z0 - xyz.z); 00144 // } 00145 // } 00146 00147 // if (area > 0) 00148 // { 00149 // centroid /= area; 00150 // if (area < geometric_threshold) 00151 // response = geometric_threshold - area; 00152 // // Look for edge direction 00153 // // X direction 00154 // if ((coefficients[3]/coefficients[0]) < 1 && (coefficients[5]/coefficients[0]) < 1) 00155 // direction = Eigen::Vector3f (1, 0, 0); 00156 // else 00157 // { 00158 // // Y direction 00159 // if ((coefficients[0]/coefficients[3]) < 1 && (coefficients[5]/coefficients[3]) < 1) 00160 // direction = Eigen::Vector3f (0, 1, 0); 00161 // else 00162 // { 00163 // // Z direction 00164 // if ((coefficients[0]/coefficients[5]) < 1 && (coefficients[3]/coefficients[5]) < 1) 00165 // direction = Eigen::Vector3f (0, 1, 0); 00166 // // Diagonal edge 00167 // else 00168 // { 00169 // //XY direction 00170 // if ((coefficients[2]/coeffcients[1]) < 1 && (coeffcients[4]/coeffcients[1]) < 1) 00171 // { 00172 // if (coeffcients[1] > 0) 00173 // direction = Eigen::Vector3f (1,1,0); 00174 // else 00175 // direction = Eigen::Vector3f (-1,1,0); 00176 // } 00177 // else 00178 // { 00179 // //XZ direction 00180 // if ((coefficients[1]/coeffcients[2]) > 1 && (coeffcients[4]/coeffcients[2]) < 1) 00181 // { 00182 // if (coeffcients[2] > 0) 00183 // direction = Eigen::Vector3f (1,0,1); 00184 // else 00185 // direction = Eigen::Vector3f (-1,0,1); 00186 // } 00187 // //YZ direction 00188 // else 00189 // { 00190 // if (coeffcients[4] > 0) 00191 // direction = Eigen::Vector3f (0,1,1); 00192 // else 00193 // direction = Eigen::Vector3f (0,-1,1); 00194 // } 00195 // } 00196 // } 00197 // } 00198 // } 00199 00200 // // std::size_t max_index = std::distance (coefficients.begin (), max); 00201 // // switch (max_index) 00202 // // { 00203 // // case 0 : direction = Eigen::Vector3f (1, 0, 0); break; 00204 // // case 1 : direction = Eigen::Vector3f (1, 1, 0); break; 00205 // // case 2 : direction = Eigen::Vector3f (1, 0, 1); break; 00206 // // case 3 : direction = Eigen::Vector3f (0, 1, 0); break; 00207 // // case 4 : direction = Eigen::Vector3f (0, 1, 1); break; 00208 // // case 5 : direction = Eigen::Vector3f (0, 0, 1); break; 00209 // // } 00210 // } 00211 // } 00212 00213 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00214 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> bool 00215 pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::initCompute () 00216 { 00217 if (!Keypoint<PointInT, PointOutT>::initCompute ()) 00218 { 00219 PCL_ERROR ("[pcl::%s::initCompute] init failed!\n", name_.c_str ()); 00220 return (false); 00221 } 00222 00223 if (normals_->empty ()) 00224 { 00225 PointCloudNPtr normals (new PointCloudN ()); 00226 normals->reserve (normals->size ()); 00227 if (!surface_->isOrganized ()) 00228 { 00229 pcl::NormalEstimation<PointInT, NormalT> normal_estimation; 00230 normal_estimation.setInputCloud (surface_); 00231 normal_estimation.setRadiusSearch (search_radius_); 00232 normal_estimation.compute (*normals); 00233 } 00234 else 00235 { 00236 IntegralImageNormalEstimation<PointInT, NormalT> normal_estimation; 00237 normal_estimation.setNormalEstimationMethod (pcl::IntegralImageNormalEstimation<PointInT, NormalT>::SIMPLE_3D_GRADIENT); 00238 normal_estimation.setInputCloud (surface_); 00239 normal_estimation.setNormalSmoothingSize (5.0); 00240 normal_estimation.compute (*normals); 00241 } 00242 normals_ = normals; 00243 } 00244 if (normals_->size () != surface_->size ()) 00245 { 00246 PCL_ERROR ("[pcl::%s::initCompute] normals given, but the number of normals does not match the number of input points!\n", name_.c_str ()); 00247 return (false); 00248 } 00249 return (true); 00250 } 00251 00252 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00253 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> bool 00254 pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::isWithinNucleusCentroid (const Eigen::Vector3f& nucleus, 00255 const Eigen::Vector3f& centroid, 00256 const Eigen::Vector3f& nc, 00257 const PointInT& point) const 00258 { 00259 Eigen::Vector3f pc = centroid - point.getVector3fMap (); 00260 Eigen::Vector3f pn = nucleus - point.getVector3fMap (); 00261 Eigen::Vector3f pc_cross_nc = pc.cross (nc); 00262 return ((pc_cross_nc.norm () <= tolerance_) && (pc.dot (nc) >= 0) && (pn.dot (nc) <= 0)); 00263 } 00264 00265 // template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> bool 00266 // pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::isValidQueryPoint3D (int point_index) const 00267 // { 00268 // return (isFinite (surface_->points [point_index]) && 00269 // isFinite (normals_->points [point_index])); 00270 // } 00271 00272 // template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> bool 00273 // pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::isValidQueryPoint2D (int point_index) const 00274 // { 00275 // return (isFinite (surface_->points [point_index])); 00276 // } 00277 00278 // template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> bool 00279 // pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::isWithinSusan2D (int nucleus, int neighbor) const 00280 // { 00281 // return (fabs (intensity_ (surface_->points[nucleus]) - 00282 // intensity_ (surface_->points[neighbor])) <= intensity_threshold_); 00283 // } 00284 00285 // template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> bool 00286 // pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::isWithinSusan3D (int nucleus, int neighbor) const 00287 // { 00288 // Eigen::Vector3f nucleus_normal = normals_->point[nucleus].getVector3fMap (); 00289 // return (1 - nucleus_normal.dot (normals_->points[*index].getNormalVector3fMap ()) <= angular_threshold_); 00290 // } 00291 00292 // template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> bool 00293 // pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::isWithinSusanH (int nucleus, int neighbor) const 00294 // { 00295 // Eigen::Vector3f nucleus_normal = normals_->point[nucleus].getVector3fMap (); 00296 // return ((1 - nucleus_normal.dot (normals_->points[*index].getNormalVector3fMap ()) <= angular_threshold_) || 00297 // (fabs (intensity_ (surface_->points[nucleus]) - intensity_ (surface_->points[neighbor])) <= intensity_threshold_)); 00298 // } 00299 00300 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00301 template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void 00302 pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::detectKeypoints (PointCloudOut &output) 00303 { 00304 boost::shared_ptr<pcl::PointCloud<PointOutT> > response (new pcl::PointCloud<PointOutT> ()); 00305 response->reserve (surface_->size ()); 00306 00307 // Check if the output has a "label" field 00308 label_idx_ = pcl::getFieldIndex<PointOutT> (output, "label", out_fields_); 00309 00310 const int input_size = static_cast<int> (input_->size ()); 00311 //#ifdef _OPENMP 00312 //#pragma omp parallel for shared (response) num_threads(threads_) 00313 //#endif 00314 for (int point_index = 0; point_index < input_size; ++point_index) 00315 { 00316 const PointInT& point_in = input_->points [point_index]; 00317 const NormalT& normal_in = normals_->points [point_index]; 00318 if (!isFinite (point_in) || !isFinite (normal_in)) 00319 continue; 00320 else 00321 { 00322 Eigen::Vector3f nucleus = point_in.getVector3fMap (); 00323 Eigen::Vector3f nucleus_normal = normals_->points [point_index].getNormalVector3fMap (); 00324 float nucleus_intensity = intensity_ (point_in); 00325 std::vector<int> nn_indices; 00326 std::vector<float> nn_dists; 00327 tree_->radiusSearch (point_in, search_radius_, nn_indices, nn_dists); 00328 float area = 0; 00329 Eigen::Vector3f centroid = Eigen::Vector3f::Zero (); 00330 // Exclude nucleus from the usan 00331 std::vector<int> usan; usan.reserve (nn_indices.size () - 1); 00332 for (std::vector<int>::const_iterator index = nn_indices.begin(); index != nn_indices.end(); ++index) 00333 { 00334 if ((*index != point_index) && pcl_isfinite (normals_->points[*index].normal_x)) 00335 { 00336 // if the point fulfill condition 00337 if ((fabs (nucleus_intensity - intensity_ (input_->points[*index])) <= intensity_threshold_) || 00338 (1 - nucleus_normal.dot (normals_->points[*index].getNormalVector3fMap ()) <= angular_threshold_)) 00339 { 00340 ++area; 00341 centroid += input_->points[*index].getVector3fMap (); 00342 usan.push_back (*index); 00343 } 00344 } 00345 } 00346 00347 float geometric_threshold = 0.5f * (static_cast<float> (nn_indices.size () - 1)); 00348 if ((area > 0) && (area < geometric_threshold)) 00349 { 00350 // if no geometric validation required add the point to the response 00351 if (!geometric_validation_) 00352 { 00353 PointOutT point_out; 00354 point_out.getVector3fMap () = point_in.getVector3fMap (); 00355 //point_out.intensity = geometric_threshold - area; 00356 intensity_out_.set (point_out, geometric_threshold - area); 00357 // if a label field is found use it to save the index 00358 if (label_idx_ != -1) 00359 { 00360 // save the index in the cloud 00361 uint32_t label = static_cast<uint32_t> (point_index); 00362 memcpy (reinterpret_cast<char*> (&point_out) + out_fields_[label_idx_].offset, 00363 &label, sizeof (uint32_t)); 00364 } 00365 //#ifdef _OPENMP 00366 //#pragma omp critical 00367 //#endif 00368 response->push_back (point_out); 00369 } 00370 else 00371 { 00372 centroid /= area; 00373 Eigen::Vector3f dist = nucleus - centroid; 00374 // Check the distance <= distance_threshold_ 00375 if (dist.norm () >= distance_threshold_) 00376 { 00377 // point is valid from distance point of view 00378 Eigen::Vector3f nc = centroid - nucleus; 00379 // Check the contiguity 00380 std::vector<int>::const_iterator usan_it = usan.begin (); 00381 for (; usan_it != usan.end (); ++usan_it) 00382 { 00383 if (!isWithinNucleusCentroid (nucleus, centroid, nc, input_->points[*usan_it])) 00384 break; 00385 } 00386 // All points within usan lies on the segment [nucleus centroid] 00387 if (usan_it == usan.end ()) 00388 { 00389 PointOutT point_out; 00390 point_out.getVector3fMap () = point_in.getVector3fMap (); 00391 // point_out.intensity = geometric_threshold - area; 00392 intensity_out_.set (point_out, geometric_threshold - area); 00393 // if a label field is found use it to save the index 00394 if (label_idx_ != -1) 00395 { 00396 // save the index in the cloud 00397 uint32_t label = static_cast<uint32_t> (point_index); 00398 memcpy (reinterpret_cast<char*> (&point_out) + out_fields_[label_idx_].offset, 00399 &label, sizeof (uint32_t)); 00400 } 00401 //#ifdef _OPENMP 00402 //#pragma omp critical 00403 //#endif 00404 response->push_back (point_out); 00405 } 00406 } 00407 } 00408 } 00409 } 00410 } 00411 00412 response->height = 1; 00413 response->width = static_cast<uint32_t> (response->size ()); 00414 00415 if (!nonmax_) 00416 output = *response; 00417 else 00418 { 00419 output.points.clear (); 00420 output.points.reserve (response->points.size()); 00421 00422 //#ifdef _OPENMP 00423 //#pragma omp parallel for shared (output) num_threads(threads_) 00424 //#endif 00425 for (int idx = 0; idx < static_cast<int> (response->points.size ()); ++idx) 00426 { 00427 const PointOutT& point_in = response->points [idx]; 00428 const NormalT& normal_in = normals_->points [idx]; 00429 //const float intensity = response->points[idx].intensity; 00430 const float intensity = intensity_out_ (response->points[idx]); 00431 if (!isFinite (point_in) || !isFinite (normal_in) || (intensity == 0)) 00432 continue; 00433 std::vector<int> nn_indices; 00434 std::vector<float> nn_dists; 00435 tree_->radiusSearch (idx, search_radius_, nn_indices, nn_dists); 00436 bool is_minima = true; 00437 for (std::vector<int>::const_iterator nn_it = nn_indices.begin(); nn_it != nn_indices.end(); ++nn_it) 00438 { 00439 // if (intensity > response->points[*nn_it].intensity) 00440 if (intensity > intensity_out_ (response->points[*nn_it])) 00441 { 00442 is_minima = false; 00443 break; 00444 } 00445 } 00446 if (is_minima) 00447 //#ifdef _OPENMP 00448 //#pragma omp critical 00449 //#endif 00450 output.points.push_back (response->points[idx]); 00451 } 00452 00453 output.height = 1; 00454 output.width = static_cast<uint32_t> (output.points.size()); 00455 } 00456 // we don not change the denseness 00457 output.is_dense = input_->is_dense; 00458 } 00459 00460 #define PCL_INSTANTIATE_SUSAN(T,U,N) template class PCL_EXPORTS pcl::SUSANKeypoint<T,U,N>; 00461 #endif // #ifndef PCL_HARRIS_KEYPOINT_3D_IMPL_H_ 00462