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-2012, 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_RECOGNITION_HOUGH_3D_IMPL_H_ 00041 #define PCL_RECOGNITION_HOUGH_3D_IMPL_H_ 00042 00043 #include <pcl/recognition/cg/hough_3d.h> 00044 #include <pcl/registration/correspondence_types.h> 00045 #include <pcl/registration/correspondence_rejection_sample_consensus.h> 00046 //#include <pcl/sample_consensus/ransac.h> 00047 //#include <pcl/sample_consensus/sac_model_registration.h> 00048 #include <pcl/features/normal_3d.h> 00049 #include <pcl/features/board.h> 00050 00051 00052 template<typename PointModelT, typename PointSceneT, typename PointModelRfT, typename PointSceneRfT> 00053 template<typename PointType, typename PointRfType> void 00054 pcl::Hough3DGrouping<PointModelT, PointSceneT, PointModelRfT, PointSceneRfT>::computeRf (const boost::shared_ptr<const pcl::PointCloud<PointType> > &input, pcl::PointCloud<PointRfType> &rf) 00055 { 00056 if (local_rf_search_radius_ == 0) 00057 { 00058 PCL_WARN ("[pcl::Hough3DGrouping::computeRf()] Warning! Reference frame search radius not set. Computing with default value. Results might be incorrect, algorithm might be slow.\n"); 00059 local_rf_search_radius_ = static_cast<float> (hough_bin_size_); 00060 } 00061 pcl::PointCloud<Normal>::Ptr normal_cloud (new pcl::PointCloud<Normal> ()); 00062 NormalEstimation<PointType, Normal> norm_est; 00063 norm_est.setInputCloud (input); 00064 if (local_rf_normals_search_radius_ <= 0.0f) 00065 { 00066 norm_est.setKSearch (15); 00067 } 00068 else 00069 { 00070 norm_est.setRadiusSearch (local_rf_normals_search_radius_); 00071 } 00072 norm_est.compute (*normal_cloud); 00073 00074 BOARDLocalReferenceFrameEstimation<PointType, Normal, PointRfType> rf_est; 00075 rf_est.setInputCloud (input); 00076 rf_est.setInputNormals (normal_cloud); 00077 rf_est.setFindHoles (true); 00078 rf_est.setRadiusSearch (local_rf_search_radius_); 00079 rf_est.compute (rf); 00080 } 00081 00082 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00083 template<typename PointModelT, typename PointSceneT, typename PointModelRfT, typename PointSceneRfT> bool 00084 pcl::Hough3DGrouping<PointModelT, PointSceneT, PointModelRfT, PointSceneRfT>::train () 00085 { 00086 if (!input_) 00087 { 00088 PCL_ERROR ("[pcl::Hough3DGrouping::train()] Error! Input cloud not set.\n"); 00089 return (false); 00090 } 00091 00092 if (!input_rf_) 00093 { 00094 ModelRfCloudPtr new_input_rf (new ModelRfCloud ()); 00095 computeRf (input_, *new_input_rf); 00096 input_rf_ = new_input_rf; 00097 //PCL_ERROR( 00098 // "[pcl::Hough3DGrouping::train()] Error! Input reference frame not set.\n"); 00099 //return (false); 00100 } 00101 00102 if (input_->size () != input_rf_->size ()) 00103 { 00104 PCL_ERROR ("[pcl::Hough3DGrouping::train()] Error! Input cloud size != Input RF cloud size.\n"); 00105 return (false); 00106 } 00107 00108 model_votes_.clear (); 00109 model_votes_.resize (input_->size ()); 00110 00111 // compute model centroid 00112 Eigen::Vector3f centroid (0, 0, 0); 00113 for (size_t i = 0; i < input_->size (); ++i) 00114 { 00115 centroid += input_->at (i).getVector3fMap (); 00116 } 00117 centroid /= static_cast<float> (input_->size ()); 00118 00119 // compute model votes 00120 for (size_t i = 0; i < input_->size (); ++i) 00121 { 00122 Eigen::Vector3f x_ax ((*input_rf_)[i].x_axis[0], (*input_rf_)[i].x_axis[1], (*input_rf_)[i].x_axis[2]); 00123 Eigen::Vector3f y_ax ((*input_rf_)[i].y_axis[0], (*input_rf_)[i].y_axis[1], (*input_rf_)[i].y_axis[2]); 00124 Eigen::Vector3f z_ax ((*input_rf_)[i].z_axis[0], (*input_rf_)[i].z_axis[1], (*input_rf_)[i].z_axis[2]); 00125 00126 model_votes_[i].x () = x_ax.dot (centroid - input_->at (i).getVector3fMap ()); 00127 model_votes_[i].y () = y_ax.dot (centroid - input_->at (i).getVector3fMap ()); 00128 model_votes_[i].z () = z_ax.dot (centroid - input_->at (i).getVector3fMap ()); 00129 } 00130 00131 needs_training_ = false; 00132 return (true); 00133 } 00134 00135 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00136 template<typename PointModelT, typename PointSceneT, typename PointModelRfT, typename PointSceneRfT> bool 00137 pcl::Hough3DGrouping<PointModelT, PointSceneT, PointModelRfT, PointSceneRfT>::houghVoting () 00138 { 00139 if (needs_training_) 00140 { 00141 if (!train ())//checks input and input_rf 00142 return (false); 00143 } 00144 00145 //if (!scene_) 00146 //{ 00147 // PCL_ERROR( 00148 // "[pcl::Hough3DGrouping::recognizeModelInstances()] Error! Scene cloud not set.\n"); 00149 // return (false); 00150 //} 00151 00152 if (!scene_rf_) 00153 { 00154 ModelRfCloudPtr new_scene_rf (new ModelRfCloud ()); 00155 computeRf (scene_, *new_scene_rf); 00156 scene_rf_ = new_scene_rf; 00157 //PCL_ERROR( 00158 // "[pcl::Hough3DGrouping::recognizeModelInstances()] Error! Scene reference frame not set.\n"); 00159 //return (false); 00160 } 00161 00162 if (scene_->size () != scene_rf_->size ()) 00163 { 00164 PCL_ERROR ("[pcl::Hough3DGrouping::recognizeModelInstances()] Error! Scene cloud size != Scene RF cloud size.\n"); 00165 return (false); 00166 } 00167 00168 if (!model_scene_corrs_) 00169 { 00170 PCL_ERROR ("[pcl::Hough3DGrouping::recognizeModelInstances()] Error! Correspondences not set, please set them before calling again this function.\n"); 00171 return (false); 00172 } 00173 00174 int n_matches = static_cast<int> (model_scene_corrs_->size ()); 00175 if (n_matches == 0) 00176 { 00177 return (false); 00178 } 00179 00180 std::vector<Eigen::Vector3d> scene_votes (n_matches); 00181 Eigen::Vector3d d_min, d_max, bin_size; 00182 00183 d_min.setConstant (std::numeric_limits<double>::max ()); 00184 d_max.setConstant (-std::numeric_limits<double>::max ()); 00185 bin_size.setConstant (hough_bin_size_); 00186 00187 float max_distance = -std::numeric_limits<float>::max (); 00188 00189 // Calculating 3D Hough space dimensions and vote position for each match 00190 for (int i=0; i< n_matches; ++i) 00191 { 00192 int scene_index = model_scene_corrs_->at (i).index_match; 00193 int model_index = model_scene_corrs_->at (i).index_query; 00194 00195 const Eigen::Vector3f& scene_point = scene_->at (scene_index).getVector3fMap (); 00196 const PointSceneRfT& scene_point_rf = scene_rf_->at (scene_index); 00197 00198 Eigen::Vector3f scene_point_rf_x (scene_point_rf.x_axis[0], scene_point_rf.x_axis[1], scene_point_rf.x_axis[2]); 00199 Eigen::Vector3f scene_point_rf_y (scene_point_rf.y_axis[0], scene_point_rf.y_axis[1], scene_point_rf.y_axis[2]); 00200 Eigen::Vector3f scene_point_rf_z (scene_point_rf.z_axis[0], scene_point_rf.z_axis[1], scene_point_rf.z_axis[2]); 00201 00202 //const Eigen::Vector3f& model_point = input_->at (model_index).getVector3fMap (); 00203 const Eigen::Vector3f& model_point_vote = model_votes_[model_index]; 00204 00205 scene_votes[i].x () = scene_point_rf_x[0] * model_point_vote.x () + scene_point_rf_y[0] * model_point_vote.y () + scene_point_rf_z[0] * model_point_vote.z () + scene_point.x (); 00206 scene_votes[i].y () = scene_point_rf_x[1] * model_point_vote.x () + scene_point_rf_y[1] * model_point_vote.y () + scene_point_rf_z[1] * model_point_vote.z () + scene_point.y (); 00207 scene_votes[i].z () = scene_point_rf_x[2] * model_point_vote.x () + scene_point_rf_y[2] * model_point_vote.y () + scene_point_rf_z[2] * model_point_vote.z () + scene_point.z (); 00208 00209 if (scene_votes[i].x () < d_min.x ()) 00210 d_min.x () = scene_votes[i].x (); 00211 if (scene_votes[i].x () > d_max.x ()) 00212 d_max.x () = scene_votes[i].x (); 00213 00214 if (scene_votes[i].y () < d_min.y ()) 00215 d_min.y () = scene_votes[i].y (); 00216 if (scene_votes[i].y () > d_max.y ()) 00217 d_max.y () = scene_votes[i].y (); 00218 00219 if (scene_votes[i].z () < d_min.z ()) 00220 d_min.z () = scene_votes[i].z (); 00221 if (scene_votes[i].z () > d_max.z ()) 00222 d_max.z () = scene_votes[i].z (); 00223 00224 // Calculate max distance for interpolated votes 00225 if (use_interpolation_ && max_distance < model_scene_corrs_->at (i).distance) 00226 { 00227 max_distance = model_scene_corrs_->at (i).distance; 00228 } 00229 } 00230 00231 // Hough Voting 00232 hough_space_.reset (new pcl::recognition::HoughSpace3D (d_min, bin_size, d_max)); 00233 00234 for (int i = 0; i < n_matches; ++i) 00235 { 00236 double weight = 1.0; 00237 if (use_distance_weight_ && max_distance != 0) 00238 { 00239 weight = 1.0 - (model_scene_corrs_->at (i).distance / max_distance); 00240 } 00241 if (use_interpolation_) 00242 { 00243 hough_space_->voteInt (scene_votes[i], weight, i); 00244 } 00245 else 00246 { 00247 hough_space_->vote (scene_votes[i], weight, i); 00248 } 00249 } 00250 00251 hough_space_initialized_ = true; 00252 00253 return (true); 00254 } 00255 00256 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00257 template<typename PointModelT, typename PointSceneT, typename PointModelRfT, typename PointSceneRfT> void 00258 pcl::Hough3DGrouping<PointModelT, PointSceneT, PointModelRfT, PointSceneRfT>::clusterCorrespondences (std::vector<Correspondences> &model_instances) 00259 { 00260 model_instances.clear (); 00261 found_transformations_.clear (); 00262 00263 if (!hough_space_initialized_ && !houghVoting ()) 00264 { 00265 return; 00266 } 00267 00268 // Finding max bins and voters 00269 std::vector<double> max_values; 00270 std::vector<std::vector<int> > max_ids; 00271 00272 hough_space_->findMaxima (hough_threshold_, max_values, max_ids); 00273 00274 // Insert maximas into result vector, after Ransac correspondence rejection 00275 // Temp copy of scene cloud with the type cast to ModelT in order to use Ransac 00276 PointCloudPtr temp_scene_cloud_ptr (new PointCloud); 00277 pcl::copyPointCloud<PointSceneT, PointModelT> (*scene_, *temp_scene_cloud_ptr); 00278 00279 pcl::registration::CorrespondenceRejectorSampleConsensus<PointModelT> corr_rejector; 00280 corr_rejector.setMaximumIterations (10000); 00281 corr_rejector.setInlierThreshold (hough_bin_size_); 00282 corr_rejector.setInputSource (input_); 00283 corr_rejector.setInputTarget (temp_scene_cloud_ptr); 00284 00285 for (size_t j = 0; j < max_values.size (); ++j) 00286 { 00287 Correspondences temp_corrs, filtered_corrs; 00288 for (size_t i = 0; i < max_ids[j].size (); ++i) 00289 { 00290 temp_corrs.push_back (model_scene_corrs_->at (max_ids[j][i])); 00291 } 00292 // RANSAC filtering 00293 corr_rejector.getRemainingCorrespondences (temp_corrs, filtered_corrs); 00294 // Save transformations for recognize 00295 found_transformations_.push_back (corr_rejector.getBestTransformation ()); 00296 00297 model_instances.push_back (filtered_corrs); 00298 } 00299 } 00300 00301 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00302 //template<typename PointModelT, typename PointSceneT, typename PointModelRfT, typename PointSceneRfT> bool 00303 //pcl::Hough3DGrouping<PointModelT, PointSceneT, PointModelRfT, PointSceneRfT>::getTransformMatrix (const PointCloudConstPtr &scene_cloud, const Correspondences &corrs, Eigen::Matrix4f &transform) 00304 //{ 00305 // std::vector<int> model_indices; 00306 // std::vector<int> scene_indices; 00307 // pcl::registration::getQueryIndices (corrs, model_indices); 00308 // pcl::registration::getMatchIndices (corrs, scene_indices); 00309 // 00310 // typename pcl::SampleConsensusModelRegistration<PointModelT>::Ptr model (new pcl::SampleConsensusModelRegistration<PointModelT> (input_, model_indices)); 00311 // model->setInputTarget (scene_cloud, scene_indices); 00312 // 00313 // pcl::RandomSampleConsensus<PointModelT> ransac (model); 00314 // ransac.setDistanceThreshold (hough_bin_size_); 00315 // ransac.setMaxIterations (10000); 00316 // if (!ransac.computeModel ()) 00317 // return (false); 00318 // 00319 // // Transform model coefficients from vectorXf to matrix4f 00320 // Eigen::VectorXf coeffs; 00321 // ransac.getModelCoefficients (coeffs); 00322 // 00323 // transform.row (0) = coeffs.segment<4> (0); 00324 // transform.row (1) = coeffs.segment<4> (4); 00325 // transform.row (2) = coeffs.segment<4> (8); 00326 // transform.row (3) = coeffs.segment<4> (12); 00327 // 00328 // return (true); 00329 //} 00330 00331 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00332 template <typename PointModelT, typename PointSceneT, typename PointModelRfT, typename PointSceneRfT> bool 00333 pcl::Hough3DGrouping<PointModelT, PointSceneT, PointModelRfT, PointSceneRfT>::recognize ( 00334 std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations) 00335 { 00336 std::vector<pcl::Correspondences> model_instances; 00337 return (this->recognize (transformations, model_instances)); 00338 } 00339 00340 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00341 template <typename PointModelT, typename PointSceneT, typename PointModelRfT, typename PointSceneRfT> bool 00342 pcl::Hough3DGrouping<PointModelT, PointSceneT, PointModelRfT, PointSceneRfT>::recognize ( 00343 std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations, std::vector<pcl::Correspondences> &clustered_corrs) 00344 { 00345 transformations.clear (); 00346 if (!this->initCompute ()) 00347 { 00348 PCL_ERROR ("[pcl::Hough3DGrouping::recognize()] Error! Model cloud or Scene cloud not set, please set them before calling again this function.\n"); 00349 return (false); 00350 } 00351 00352 clusterCorrespondences (clustered_corrs); 00353 00354 transformations = found_transformations_; 00355 00356 //// Temp copy of scene cloud with the type cast to ModelT in order to use Ransac 00357 //PointCloudPtr temp_scene_cloud_ptr (new PointCloud); 00358 //pcl::copyPointCloud<PointSceneT, PointModelT> (*scene_, *temp_scene_cloud_ptr); 00359 00360 //for (size_t i = 0; i < model_instances.size (); ++i) 00361 //{ 00362 // Eigen::Matrix4f curr_transf; 00363 // if (getTransformMatrix (temp_scene_cloud_ptr, model_instances[i], curr_transf)) 00364 // transformations.push_back (curr_transf); 00365 //} 00366 00367 this->deinitCompute (); 00368 return (true); 00369 } 00370 00371 00372 #define PCL_INSTANTIATE_Hough3DGrouping(T,ST,RFT,SRFT) template class PCL_EXPORTS pcl::Hough3DGrouping<T,ST,RFT,SRFT>; 00373 00374 #endif // PCL_RECOGNITION_HOUGH_3D_IMPL_H_