Point Cloud Library (PCL)
1.3.1
|
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_keypoint3D.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 00047 00049 template <typename PointInT, typename PointOutT> void 00050 pcl::HarrisKeypoint3D<PointInT, PointOutT>::setMethod (ResponseMethod method) 00051 { 00052 method_ = method; 00053 } 00054 00055 template <typename PointInT, typename PointOutT> void 00056 pcl::HarrisKeypoint3D<PointInT, PointOutT>::setThreshold (float threshold) 00057 { 00058 threshold_= threshold; 00059 } 00060 00061 template <typename PointInT, typename PointOutT> void 00062 pcl::HarrisKeypoint3D<PointInT, PointOutT>::setRadius (float radius) 00063 { 00064 radius_ = radius; 00065 } 00066 00067 template <typename PointInT, typename PointOutT> void 00068 pcl::HarrisKeypoint3D<PointInT, PointOutT>::setNonMaxSupression (bool nonmax) 00069 { 00070 nonmax_ = nonmax; 00071 } 00072 00074 template <typename PointInT, typename PointOutT> void 00075 pcl::HarrisKeypoint3D<PointInT, PointOutT>::detectKeypoints (PointCloudOut &output) 00076 { 00077 boost::shared_ptr<pcl::PointCloud<PointInT> > cloud (new pcl::PointCloud<PointInT> ()); 00078 pcl::PassThrough<PointInT> pass_; 00079 #if 0 00080 if (indices_->empty () || indices_->size() == indices_->size () == (input_->width * input_->height) || 00081 indices_->size () == input_->points.size ()) 00082 { 00083 pass_.setInputCloud (input_); 00084 } 00085 else 00086 { 00087 boost::shared_ptr<pcl::PointCloud<PointInT> > sub_cloud (new pcl::PointCloud<PointInT> ()); 00088 pcl::ExtractIndices<PointInT> extract; 00089 extract.setIndices (indices_); 00090 extract.setInputCloud (input_); 00091 extract.filter (*sub_cloud); 00092 std::cout << "sub_cloud size: " << sub_cloud->points.size () << std::endl; 00093 pass_.setInputCloud (sub_cloud); 00094 } 00095 #else 00096 pass_.setInputCloud (input_); 00097 #endif 00098 00099 pass_.filter (*cloud); 00100 // estimate normals 00101 boost::shared_ptr<pcl::PointCloud<pcl::Normal> > normals (new pcl::PointCloud<Normal> ()); 00102 pcl::NormalEstimation<PointInT, pcl::Normal> normal_estimation; 00103 normal_estimation.setInputCloud(cloud); 00104 normal_estimation.setRadiusSearch(radius_); 00105 normal_estimation.compute (*normals); 00106 00107 boost::shared_ptr<pcl::PointCloud<PointOutT> > response (new pcl::PointCloud<PointOutT> ()); 00108 switch (method_) 00109 { 00110 case HARRIS: 00111 responseHarris(cloud, normals, *response); 00112 break; 00113 case NOBLE: 00114 responseNoble(cloud, normals, *response); 00115 break; 00116 case LOWE: 00117 responseLowe(cloud, normals, *response); 00118 break; 00119 case CURVATURE: 00120 responseCurvature(cloud, normals, *response); 00121 break; 00122 case TOMASI: 00123 responseTomasi(cloud, normals, *response); 00124 break; 00125 } 00126 00127 // just return the response 00128 if (!nonmax_) 00129 output = *response; 00130 else 00131 { 00132 output.points.clear (); 00133 output.points.reserve (response->points.size()); 00134 std::vector<int> nn_indices; 00135 std::vector<float> nn_dists; 00136 pcl::search::KdTree<pcl::PointXYZI> response_search; 00137 response_search.setInputCloud(response); 00138 for (size_t idx = 0; idx < response->points.size(); ++idx) 00139 { 00140 response_search.radiusSearch (idx, radius_, nn_indices, nn_dists); 00141 bool is_maxima = true; 00142 for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt) 00143 { 00144 if (response->points[idx].intensity < response->points[*iIt].intensity) 00145 { 00146 is_maxima = false; 00147 break; 00148 } 00149 } 00150 if (is_maxima) 00151 output.points.push_back (response->points[idx]); 00152 } 00153 00154 output.height = 1; 00155 output.width = output.points.size(); 00156 } 00157 } 00158 00159 template <typename PointInT, typename PointOutT> void 00160 pcl::HarrisKeypoint3D<PointInT, PointOutT>::responseHarris (typename PointCloudIn::ConstPtr input, pcl::PointCloud<Normal>::ConstPtr normals, PointCloudOut &output) const 00161 { 00162 output.points.clear (); 00163 output.points.reserve (input->points.size()); 00164 00165 std::vector<int> nn_indices; 00166 std::vector<float> nn_dists; 00167 pcl::search::KdTree<PointInT> search; 00168 search.setInputCloud(input); 00169 00170 PointOutT point; 00171 for (typename PointCloudIn::const_iterator pointIt = input->begin(); pointIt != input->end(); ++pointIt) 00172 //for (std::vector<int>::const_iterator idxIt = indices_->begin(); idxIt != indices_->end(); ++idxIt) 00173 { 00174 search.radiusSearch (*pointIt, radius_, nn_indices, nn_dists); 00175 00176 Eigen::Matrix3f covariance_matrix; 00177 covariance_matrix.setZero(); 00178 for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt) 00179 { 00180 const Eigen::Vector3f* vec = reinterpret_cast<const Eigen::Vector3f*> (&(normals->at(*iIt).normal_x)); 00181 covariance_matrix += (*vec) * (vec->transpose()); 00182 } 00183 point.x = pointIt->x; 00184 point.y = pointIt->y; 00185 point.z = pointIt->z; 00186 point.intensity = covariance_matrix.determinant () - 0.04 * covariance_matrix.trace () * covariance_matrix.trace (); 00187 output.points.push_back(point); 00188 } 00189 output.height = 1; 00190 output.width = output.points.size (); 00191 } 00192 00193 template <typename PointInT, typename PointOutT> void 00194 pcl::HarrisKeypoint3D<PointInT, PointOutT>::responseNoble (typename PointCloudIn::ConstPtr input, pcl::PointCloud<Normal>::ConstPtr normals, PointCloudOut &output) const 00195 { 00196 output.points.clear (); 00197 output.points.reserve (input->points.size()); 00198 00199 std::vector<int> nn_indices; 00200 std::vector<float> nn_dists; 00201 pcl::search::KdTree<PointInT> search; 00202 search.setInputCloud(input); 00203 00204 PointOutT point; 00205 for (typename PointCloudIn::const_iterator pointIt = input->begin(); pointIt != input->end(); ++pointIt) 00206 { 00207 search.radiusSearch (*pointIt, radius_, nn_indices, nn_dists); 00208 00209 Eigen::Matrix3f covariance_matrix; 00210 covariance_matrix.setZero(); 00211 for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt) 00212 { 00213 const Eigen::Vector3f* vec = reinterpret_cast<const Eigen::Vector3f*> (&(normals->at(*iIt).normal_x)); 00214 covariance_matrix += (*vec) * (vec->transpose()); 00215 } 00216 point.x = pointIt->x; 00217 point.y = pointIt->y; 00218 point.z = pointIt->z; 00219 point.intensity = covariance_matrix.determinant () / covariance_matrix.trace (); 00220 output.points.push_back(point); 00221 } 00222 output.height = 1; 00223 output.width = output.points.size (); 00224 } 00225 00226 template <typename PointInT, typename PointOutT> void 00227 pcl::HarrisKeypoint3D<PointInT, PointOutT>::responseLowe (typename PointCloudIn::ConstPtr input, pcl::PointCloud<Normal>::ConstPtr normals, PointCloudOut &output) const 00228 { 00229 output.points.clear (); 00230 output.points.reserve (input->points.size()); 00231 00232 std::vector<int> nn_indices; 00233 std::vector<float> nn_dists; 00234 pcl::search::KdTree<PointInT> search; 00235 search.setInputCloud(input); 00236 00237 PointOutT point; 00238 for (typename PointCloudIn::const_iterator pointIt = input->begin(); pointIt != input->end(); ++pointIt) 00239 { 00240 search.radiusSearch (*pointIt, radius_, nn_indices, nn_dists); 00241 00242 Eigen::Matrix3f covariance_matrix; 00243 covariance_matrix.setZero(); 00244 for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt) 00245 { 00246 const Eigen::Vector3f* vec = reinterpret_cast<const Eigen::Vector3f*> (&(normals->at(*iIt).normal_x)); 00247 covariance_matrix += (*vec) * (vec->transpose()); 00248 } 00249 point.x = pointIt->x; 00250 point.y = pointIt->y; 00251 point.z = pointIt->z; 00252 point.intensity = covariance_matrix.determinant () / (covariance_matrix.trace () * covariance_matrix.trace ()); 00253 output.points.push_back(point); 00254 } 00255 output.height = 1; 00256 output.width = output.points.size (); 00257 } 00258 00259 template <typename PointInT, typename PointOutT> void 00260 pcl::HarrisKeypoint3D<PointInT, PointOutT>::responseCurvature (typename PointCloudIn::ConstPtr input, pcl::PointCloud<Normal>::ConstPtr normals, PointCloudOut &output) const 00261 { 00262 output.points.clear (); 00263 output.points.reserve (input->points.size()); 00264 00265 std::vector<int> nn_indices; 00266 std::vector<float> nn_dists; 00267 pcl::search::KdTree<PointInT> search; 00268 search.setInputCloud(input); 00269 00270 PointOutT point; 00271 for (unsigned idx = 0; idx < input->points.size(); ++idx) 00272 { 00273 search.radiusSearch (idx, radius_, nn_indices, nn_dists); 00274 00275 Eigen::Matrix3f covariance_matrix; 00276 covariance_matrix.setZero(); 00277 for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt) 00278 { 00279 const Eigen::Vector3f* vec = reinterpret_cast<const Eigen::Vector3f*> (&(normals->at(*iIt).normal_x)); 00280 covariance_matrix += (*vec) * (vec->transpose()); 00281 } 00282 point.x = input->points[idx].x; 00283 point.y = input->points[idx].y; 00284 point.z = input->points[idx].z; 00285 point.intensity = (*normals)[idx].curvature; 00286 output.points.push_back(point); 00287 } 00288 output.height = 1; 00289 output.width = output.points.size (); 00290 } 00291 00292 template <typename PointInT, typename PointOutT> void 00293 pcl::HarrisKeypoint3D<PointInT, PointOutT>::responseTomasi (typename PointCloudIn::ConstPtr input, pcl::PointCloud<Normal>::ConstPtr normals, PointCloudOut &output) const 00294 { 00295 output.points.clear (); 00296 output.points.reserve (input->points.size()); 00297 00298 std::vector<int> nn_indices; 00299 std::vector<float> nn_dists; 00300 pcl::search::KdTree<PointInT> search; 00301 search.setInputCloud(input); 00302 00303 PointOutT point; 00304 for (typename PointCloudIn::const_iterator pointIt = input->begin(); pointIt != input->end(); ++pointIt) 00305 { 00306 search.radiusSearch (*pointIt, radius_, nn_indices, nn_dists); 00307 00308 Eigen::Matrix3f covariance_matrix; 00309 covariance_matrix.setZero(); 00310 for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt) 00311 { 00312 const Eigen::Vector3f* vec = reinterpret_cast<const Eigen::Vector3f*> (&(normals->at(*iIt).normal_x)); 00313 covariance_matrix += (*vec) * (vec->transpose()); 00314 } 00315 point.x = pointIt->x; 00316 point.y = pointIt->y; 00317 point.z = pointIt->z; 00318 00319 EIGEN_ALIGN16 Eigen::Vector3f eigen_values; 00320 EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors; 00321 pcl::eigen33(covariance_matrix, eigen_vectors, eigen_values); 00322 point.intensity = eigen_values[0]; 00323 output.points.push_back(point); 00324 } 00325 output.height = 1; 00326 output.width = output.points.size (); 00327 } 00328 00329 #define PCL_INSTANTIATE_HarrisKeypoint3D(T,U) template class PCL_EXPORTS pcl::HarrisKeypoint3D<T,U>; 00330 00331 #endif // #ifndef PCL_HARRIS_KEYPOINT_3D_IMPL_H_ 00332