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_6D_IMPL_H_ 00039 #define PCL_HARRIS_KEYPOINT_6D_IMPL_H_ 00040 00041 #include <pcl/keypoints/harris_6d.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/fast_intensity_gradient.h> 00047 #include <pcl/features/intensity_gradient.h> 00048 #include <pcl/features/integral_image_normal.h> 00049 00050 template <typename PointInT, typename PointOutT, typename NormalT> void 00051 pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::setThreshold (float threshold) 00052 { 00053 threshold_= threshold; 00054 } 00055 00056 template <typename PointInT, typename PointOutT, typename NormalT> void 00057 pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::setRadius (float radius) 00058 { 00059 search_radius_ = radius; 00060 } 00061 00062 template <typename PointInT, typename PointOutT, typename NormalT> void 00063 pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::setRefine (bool do_refine) 00064 { 00065 refine_ = do_refine; 00066 } 00067 00068 template <typename PointInT, typename PointOutT, typename NormalT> void 00069 pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::setNonMaxSupression (bool nonmax) 00070 { 00071 nonmax_ = nonmax; 00072 } 00073 00074 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00075 template <typename PointInT, typename PointOutT, typename NormalT> void 00076 pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::calculateCombinedCovar (const std::vector<int>& neighbors, float* coefficients) const 00077 { 00078 memset (coefficients, 0, sizeof (float) * 21); 00079 unsigned count = 0; 00080 for (std::vector<int>::const_iterator iIt = neighbors.begin(); iIt != neighbors.end(); ++iIt) 00081 { 00082 if (pcl_isfinite (normals_->points[*iIt].normal_x) && pcl_isfinite (intensity_gradients_->points[*iIt].gradient [0])) 00083 { 00084 coefficients[ 0] += normals_->points[*iIt].normal_x * normals_->points[*iIt].normal_x; 00085 coefficients[ 1] += normals_->points[*iIt].normal_x * normals_->points[*iIt].normal_y; 00086 coefficients[ 2] += normals_->points[*iIt].normal_x * normals_->points[*iIt].normal_z; 00087 coefficients[ 3] += normals_->points[*iIt].normal_x * intensity_gradients_->points[*iIt].gradient [0]; 00088 coefficients[ 4] += normals_->points[*iIt].normal_x * intensity_gradients_->points[*iIt].gradient [1]; 00089 coefficients[ 5] += normals_->points[*iIt].normal_x * intensity_gradients_->points[*iIt].gradient [2]; 00090 00091 coefficients[ 6] += normals_->points[*iIt].normal_y * normals_->points[*iIt].normal_y; 00092 coefficients[ 7] += normals_->points[*iIt].normal_y * normals_->points[*iIt].normal_z; 00093 coefficients[ 8] += normals_->points[*iIt].normal_y * intensity_gradients_->points[*iIt].gradient [0]; 00094 coefficients[ 9] += normals_->points[*iIt].normal_y * intensity_gradients_->points[*iIt].gradient [1]; 00095 coefficients[10] += normals_->points[*iIt].normal_y * intensity_gradients_->points[*iIt].gradient [2]; 00096 00097 coefficients[11] += normals_->points[*iIt].normal_z * normals_->points[*iIt].normal_z; 00098 coefficients[12] += normals_->points[*iIt].normal_z * intensity_gradients_->points[*iIt].gradient [0]; 00099 coefficients[13] += normals_->points[*iIt].normal_z * intensity_gradients_->points[*iIt].gradient [1]; 00100 coefficients[14] += normals_->points[*iIt].normal_z * intensity_gradients_->points[*iIt].gradient [2]; 00101 00102 coefficients[15] += intensity_gradients_->points[*iIt].gradient [0] * intensity_gradients_->points[*iIt].gradient [0]; 00103 coefficients[16] += intensity_gradients_->points[*iIt].gradient [0] * intensity_gradients_->points[*iIt].gradient [1]; 00104 coefficients[17] += intensity_gradients_->points[*iIt].gradient [0] * intensity_gradients_->points[*iIt].gradient [2]; 00105 00106 coefficients[18] += intensity_gradients_->points[*iIt].gradient [1] * intensity_gradients_->points[*iIt].gradient [1]; 00107 coefficients[19] += intensity_gradients_->points[*iIt].gradient [1] * intensity_gradients_->points[*iIt].gradient [2]; 00108 00109 coefficients[20] += intensity_gradients_->points[*iIt].gradient [2] * intensity_gradients_->points[*iIt].gradient [2]; 00110 00111 ++count; 00112 } 00113 } 00114 if (count > 0) 00115 { 00116 float norm = 1.0 / float (count); 00117 coefficients[ 0] *= norm; 00118 coefficients[ 1] *= norm; 00119 coefficients[ 2] *= norm; 00120 coefficients[ 3] *= norm; 00121 coefficients[ 4] *= norm; 00122 coefficients[ 5] *= norm; 00123 coefficients[ 6] *= norm; 00124 coefficients[ 7] *= norm; 00125 coefficients[ 8] *= norm; 00126 coefficients[ 9] *= norm; 00127 coefficients[10] *= norm; 00128 coefficients[11] *= norm; 00129 coefficients[12] *= norm; 00130 coefficients[13] *= norm; 00131 coefficients[14] *= norm; 00132 coefficients[15] *= norm; 00133 coefficients[16] *= norm; 00134 coefficients[17] *= norm; 00135 coefficients[18] *= norm; 00136 coefficients[19] *= norm; 00137 coefficients[20] *= norm; 00138 } 00139 } 00140 00141 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00142 template <typename PointInT, typename PointOutT, typename NormalT> void 00143 pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloudOut &output) 00144 { 00145 if (normals_->empty ()) 00146 { 00147 normals_->reserve (surface_->size ()); 00148 if (!surface_->isOrganized ()) 00149 { 00150 pcl::NormalEstimation<PointInT, NormalT> normal_estimation; 00151 normal_estimation.setInputCloud (surface_); 00152 normal_estimation.setRadiusSearch (search_radius_); 00153 normal_estimation.compute (*normals_); 00154 } 00155 else 00156 { 00157 IntegralImageNormalEstimation<PointInT, NormalT> normal_estimation; 00158 normal_estimation.setNormalEstimationMethod (pcl::IntegralImageNormalEstimation<PointInT, NormalT>::SIMPLE_3D_GRADIENT); 00159 normal_estimation.setInputCloud (surface_); 00160 normal_estimation.setNormalSmoothingSize (5.0); 00161 normal_estimation.compute (*normals_); 00162 } 00163 } 00164 00165 pcl::PointCloud<pcl::PointXYZI>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZI>); 00166 cloud->resize (surface_->size ()); 00167 #ifdef _OPENMP 00168 #pragma omp parallel for num_threads(threads_) default(shared) 00169 #endif 00170 for (unsigned idx = 0; idx < surface_->size (); ++idx) 00171 { 00172 cloud->points [idx].x = surface_->points [idx].x; 00173 cloud->points [idx].y = surface_->points [idx].y; 00174 cloud->points [idx].z = surface_->points [idx].z; 00175 //grayscale = 0.2989 * R + 0.5870 * G + 0.1140 * B 00176 00177 cloud->points [idx].intensity = 0.00390625 * (0.114 * float(surface_->points [idx].b) + 0.5870 * float(surface_->points [idx].g) + 0.2989 * float(surface_->points [idx].r)); 00178 } 00179 pcl::copyPointCloud (*surface_, *cloud); 00180 00181 IntensityGradientEstimation<PointXYZI, NormalT, IntensityGradient> grad_est; 00182 grad_est.setInputCloud (cloud); 00183 grad_est.setInputNormals (normals_); 00184 grad_est.setRadiusSearch (search_radius_); 00185 grad_est.compute (*intensity_gradients_); 00186 00187 #ifdef _OPENMP 00188 #pragma omp parallel for num_threads(threads_) default (shared) 00189 #endif 00190 for (unsigned idx = 0; idx < intensity_gradients_->size (); ++idx) 00191 { 00192 float len = intensity_gradients_->points [idx].gradient_x * intensity_gradients_->points [idx].gradient_x + 00193 intensity_gradients_->points [idx].gradient_y * intensity_gradients_->points [idx].gradient_y + 00194 intensity_gradients_->points [idx].gradient_z * intensity_gradients_->points [idx].gradient_z ; 00195 00196 // Suat: ToDo: remove this magic number or expose using set/get 00197 if (len > 200.0) 00198 { 00199 len = 1.0 / sqrt (len); 00200 intensity_gradients_->points [idx].gradient_x *= len; 00201 intensity_gradients_->points [idx].gradient_y *= len; 00202 intensity_gradients_->points [idx].gradient_z *= len; 00203 } 00204 else 00205 { 00206 intensity_gradients_->points [idx].gradient_x = 0; 00207 intensity_gradients_->points [idx].gradient_y = 0; 00208 intensity_gradients_->points [idx].gradient_z = 0; 00209 } 00210 } 00211 00212 boost::shared_ptr<pcl::PointCloud<PointOutT> > response (new pcl::PointCloud<PointOutT> ()); 00213 response->points.reserve (input_->points.size()); 00214 responseTomasi(*response); 00215 00216 // just return the response 00217 if (!nonmax_) 00218 { 00219 output = *response; 00220 // we do not change the denseness in this case 00221 output.is_dense = input_->is_dense; 00222 } 00223 else 00224 { 00225 output.points.clear (); 00226 output.points.reserve (response->points.size()); 00227 00228 #ifdef _OPENMP 00229 #pragma omp parallel for num_threads(threads_) default(shared) 00230 #endif 00231 for (size_t idx = 0; idx < response->points.size (); ++idx) 00232 { 00233 if (!isFinite (response->points[idx]) || response->points[idx].intensity < threshold_) 00234 continue; 00235 00236 std::vector<int> nn_indices; 00237 std::vector<float> nn_dists; 00238 tree_->radiusSearch (idx, search_radius_, nn_indices, nn_dists); 00239 bool is_maxima = true; 00240 for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt) 00241 { 00242 if (response->points[idx].intensity < response->points[*iIt].intensity) 00243 { 00244 is_maxima = false; 00245 break; 00246 } 00247 } 00248 if (is_maxima) 00249 #ifdef _OPENMP 00250 #pragma omp critical 00251 #endif 00252 output.points.push_back (response->points[idx]); 00253 } 00254 00255 if (refine_) 00256 refineCorners (output); 00257 00258 output.height = 1; 00259 output.width = static_cast<uint32_t> (output.points.size()); 00260 output.is_dense = true; 00261 } 00262 00263 00264 } 00265 00266 template <typename PointInT, typename PointOutT, typename NormalT> void 00267 pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::responseTomasi (PointCloudOut &output) const 00268 { 00269 // get the 6x6 covar-mat 00270 PointOutT pointOut; 00271 PCL_ALIGN (16) float covar [21]; 00272 Eigen::SelfAdjointEigenSolver <Eigen::Matrix<float, 6, 6> > solver; 00273 Eigen::Matrix<float, 6, 6> covariance; 00274 00275 #ifdef _OPENMP 00276 #pragma omp parallel for default (shared) private (pointOut, covar, covariance, solver) num_threads(threads_) 00277 #endif 00278 for (unsigned pIdx = 0; pIdx < input_->size (); ++pIdx) 00279 { 00280 const PointInT& pointIn = input_->points [pIdx]; 00281 pointOut.intensity = 0.0; //std::numeric_limits<float>::quiet_NaN (); 00282 if (isFinite (pointIn)) 00283 { 00284 std::vector<int> nn_indices; 00285 std::vector<float> nn_dists; 00286 tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists); 00287 calculateCombinedCovar (nn_indices, covar); 00288 00289 float trace = covar [0] + covar [6] + covar [11] + covar [15] + covar [18] + covar [20]; 00290 if (trace != 0) 00291 { 00292 covariance.coeffRef ( 0) = covar [ 0]; 00293 covariance.coeffRef ( 1) = covar [ 1]; 00294 covariance.coeffRef ( 2) = covar [ 2]; 00295 covariance.coeffRef ( 3) = covar [ 3]; 00296 covariance.coeffRef ( 4) = covar [ 4]; 00297 covariance.coeffRef ( 5) = covar [ 5]; 00298 00299 covariance.coeffRef ( 7) = covar [ 6]; 00300 covariance.coeffRef ( 8) = covar [ 7]; 00301 covariance.coeffRef ( 9) = covar [ 8]; 00302 covariance.coeffRef (10) = covar [ 9]; 00303 covariance.coeffRef (11) = covar [10]; 00304 00305 covariance.coeffRef (14) = covar [11]; 00306 covariance.coeffRef (15) = covar [12]; 00307 covariance.coeffRef (16) = covar [13]; 00308 covariance.coeffRef (17) = covar [14]; 00309 00310 covariance.coeffRef (21) = covar [15]; 00311 covariance.coeffRef (22) = covar [16]; 00312 covariance.coeffRef (23) = covar [17]; 00313 00314 covariance.coeffRef (28) = covar [18]; 00315 covariance.coeffRef (29) = covar [19]; 00316 00317 covariance.coeffRef (35) = covar [20]; 00318 00319 covariance.coeffRef ( 6) = covar [ 1]; 00320 00321 covariance.coeffRef (12) = covar [ 2]; 00322 covariance.coeffRef (13) = covar [ 7]; 00323 00324 covariance.coeffRef (18) = covar [ 3]; 00325 covariance.coeffRef (19) = covar [ 8]; 00326 covariance.coeffRef (20) = covar [12]; 00327 00328 covariance.coeffRef (24) = covar [ 4]; 00329 covariance.coeffRef (25) = covar [ 9]; 00330 covariance.coeffRef (26) = covar [13]; 00331 covariance.coeffRef (27) = covar [16]; 00332 00333 covariance.coeffRef (30) = covar [ 5]; 00334 covariance.coeffRef (31) = covar [10]; 00335 covariance.coeffRef (32) = covar [14]; 00336 covariance.coeffRef (33) = covar [17]; 00337 covariance.coeffRef (34) = covar [19]; 00338 00339 solver.compute (covariance); 00340 pointOut.intensity = solver.eigenvalues () [3]; 00341 } 00342 } 00343 00344 pointOut.x = pointIn.x; 00345 pointOut.y = pointIn.y; 00346 pointOut.z = pointIn.z; 00347 #ifdef _OPENMP 00348 #pragma omp critical 00349 #endif 00350 00351 output.points.push_back(pointOut); 00352 } 00353 output.height = input_->height; 00354 output.width = input_->width; 00355 } 00356 00357 template <typename PointInT, typename PointOutT, typename NormalT> void 00358 pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::refineCorners (PointCloudOut &corners) const 00359 { 00360 pcl::search::KdTree<PointInT> search; 00361 search.setInputCloud(surface_); 00362 00363 Eigen::Matrix3f nnT; 00364 Eigen::Matrix3f NNT; 00365 Eigen::Vector3f NNTp; 00366 const Eigen::Vector3f* normal; 00367 const Eigen::Vector3f* point; 00368 float diff; 00369 const unsigned max_iterations = 10; 00370 for (typename PointCloudOut::iterator cornerIt = corners.begin(); cornerIt != corners.end(); ++cornerIt) 00371 { 00372 unsigned iterations = 0; 00373 do { 00374 NNT.setZero(); 00375 NNTp.setZero(); 00376 PointInT corner; 00377 corner.x = cornerIt->x; 00378 corner.y = cornerIt->y; 00379 corner.z = cornerIt->z; 00380 std::vector<int> nn_indices; 00381 std::vector<float> nn_dists; 00382 search.radiusSearch (corner, search_radius_, nn_indices, nn_dists); 00383 for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt) 00384 { 00385 normal = reinterpret_cast<const Eigen::Vector3f*> (&(normals_->points[*iIt].normal_x)); 00386 point = reinterpret_cast<const Eigen::Vector3f*> (&(surface_->points[*iIt].x)); 00387 nnT = (*normal) * (normal->transpose()); 00388 NNT += nnT; 00389 NNTp += nnT * (*point); 00390 } 00391 if (NNT.determinant() != 0) 00392 *(reinterpret_cast<Eigen::Vector3f*>(&(cornerIt->x))) = NNT.inverse () * NNTp; 00393 00394 diff = (cornerIt->x - corner.x) * (cornerIt->x - corner.x) + 00395 (cornerIt->y - corner.y) * (cornerIt->y - corner.y) + 00396 (cornerIt->z - corner.z) * (cornerIt->z - corner.z); 00397 00398 } while (diff > 1e-6 && ++iterations < max_iterations); 00399 } 00400 } 00401 00402 #define PCL_INSTANTIATE_HarrisKeypoint6D(T,U,N) template class PCL_EXPORTS pcl::HarrisKeypoint6D<T,U,N>; 00403 #endif // #ifndef PCL_HARRIS_KEYPOINT_6D_IMPL_H_ 00404