Point Cloud Library (PCL)  1.3.1
harris_keypoint3D.hpp
Go to the documentation of this file.
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 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines