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_GEOMETRIC_CONSISTENCY_IMPL_H_ 00041 #define PCL_RECOGNITION_GEOMETRIC_CONSISTENCY_IMPL_H_ 00042 00043 #include <pcl/recognition/cg/geometric_consistency.h> 00044 #include <pcl/registration/correspondence_types.h> 00045 #include <pcl/registration/correspondence_rejection_sample_consensus.h> 00046 #include <pcl/common/io.h> 00047 00048 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00049 bool 00050 gcCorrespSorter (pcl::Correspondence i, pcl::Correspondence j) 00051 { 00052 return (i.distance < j.distance); 00053 } 00054 00055 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00056 template<typename PointModelT, typename PointSceneT> void 00057 pcl::GeometricConsistencyGrouping<PointModelT, PointSceneT>::clusterCorrespondences (std::vector<Correspondences> &model_instances) 00058 { 00059 model_instances.clear (); 00060 found_transformations_.clear (); 00061 00062 if (!model_scene_corrs_) 00063 { 00064 PCL_ERROR( 00065 "[pcl::GeometricConsistencyGrouping::clusterCorrespondences()] Error! Correspondences not set, please set them before calling again this function.\n"); 00066 return; 00067 } 00068 00069 CorrespondencesPtr sorted_corrs (new Correspondences (*model_scene_corrs_)); 00070 00071 std::sort (sorted_corrs->begin (), sorted_corrs->end (), gcCorrespSorter); 00072 00073 model_scene_corrs_ = sorted_corrs; 00074 00075 std::vector<int> consensus_set; 00076 std::vector<bool> taken_corresps (model_scene_corrs_->size (), false); 00077 00078 Eigen::Vector3f dist_ref, dist_trg; 00079 00080 //temp copy of scene cloud with the type cast to ModelT in order to use Ransac 00081 PointCloudPtr temp_scene_cloud_ptr (new PointCloud ()); 00082 pcl::copyPointCloud<PointSceneT, PointModelT> (*scene_, *temp_scene_cloud_ptr); 00083 00084 pcl::registration::CorrespondenceRejectorSampleConsensus<PointModelT> corr_rejector; 00085 corr_rejector.setMaximumIterations (10000); 00086 corr_rejector.setInlierThreshold (gc_size_); 00087 corr_rejector.setInputSource(input_); 00088 corr_rejector.setInputTarget (temp_scene_cloud_ptr); 00089 00090 for (size_t i = 0; i < model_scene_corrs_->size (); ++i) 00091 { 00092 if (taken_corresps[i]) 00093 continue; 00094 00095 consensus_set.clear (); 00096 consensus_set.push_back (static_cast<int> (i)); 00097 00098 for (size_t j = 0; j < model_scene_corrs_->size (); ++j) 00099 { 00100 if ( j != i && !taken_corresps[j]) 00101 { 00102 //Let's check if j fits into the current consensus set 00103 bool is_a_good_candidate = true; 00104 for (size_t k = 0; k < consensus_set.size (); ++k) 00105 { 00106 int scene_index_k = model_scene_corrs_->at (consensus_set[k]).index_match; 00107 int model_index_k = model_scene_corrs_->at (consensus_set[k]).index_query; 00108 int scene_index_j = model_scene_corrs_->at (j).index_match; 00109 int model_index_j = model_scene_corrs_->at (j).index_query; 00110 00111 const Eigen::Vector3f& scene_point_k = scene_->at (scene_index_k).getVector3fMap (); 00112 const Eigen::Vector3f& model_point_k = input_->at (model_index_k).getVector3fMap (); 00113 const Eigen::Vector3f& scene_point_j = scene_->at (scene_index_j).getVector3fMap (); 00114 const Eigen::Vector3f& model_point_j = input_->at (model_index_j).getVector3fMap (); 00115 00116 dist_ref = scene_point_k - scene_point_j; 00117 dist_trg = model_point_k - model_point_j; 00118 00119 double distance = fabs (dist_ref.norm () - dist_trg.norm ()); 00120 00121 if (distance > gc_size_) 00122 { 00123 is_a_good_candidate = false; 00124 break; 00125 } 00126 } 00127 00128 if (is_a_good_candidate) 00129 consensus_set.push_back (static_cast<int> (j)); 00130 } 00131 } 00132 00133 if (static_cast<int> (consensus_set.size ()) > gc_threshold_) 00134 { 00135 Correspondences temp_corrs, filtered_corrs; 00136 for (size_t j = 0; j < consensus_set.size (); j++) 00137 { 00138 temp_corrs.push_back (model_scene_corrs_->at (consensus_set[j])); 00139 taken_corresps[ consensus_set[j] ] = true; 00140 } 00141 //ransac filtering 00142 corr_rejector.getRemainingCorrespondences (temp_corrs, filtered_corrs); 00143 //save transformations for recognize 00144 found_transformations_.push_back (corr_rejector.getBestTransformation ()); 00145 00146 model_instances.push_back (filtered_corrs); 00147 } 00148 } 00149 } 00150 00151 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00152 template<typename PointModelT, typename PointSceneT> bool 00153 pcl::GeometricConsistencyGrouping<PointModelT, PointSceneT>::recognize ( 00154 std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations) 00155 { 00156 std::vector<pcl::Correspondences> model_instances; 00157 return (this->recognize (transformations, model_instances)); 00158 } 00159 00160 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00161 template<typename PointModelT, typename PointSceneT> bool 00162 pcl::GeometricConsistencyGrouping<PointModelT, PointSceneT>::recognize ( 00163 std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations, std::vector<pcl::Correspondences> &clustered_corrs) 00164 { 00165 transformations.clear (); 00166 if (!this->initCompute ()) 00167 { 00168 PCL_ERROR( 00169 "[pcl::GeometricConsistencyGrouping::recognize()] Error! Model cloud or Scene cloud not set, please set them before calling again this function.\n"); 00170 return (false); 00171 } 00172 00173 clusterCorrespondences (clustered_corrs); 00174 00175 transformations = found_transformations_; 00176 00177 this->deinitCompute (); 00178 return (true); 00179 } 00180 00181 #define PCL_INSTANTIATE_GeometricConsistencyGrouping(T,ST) template class PCL_EXPORTS pcl::GeometricConsistencyGrouping<T,ST>; 00182 00183 #endif // PCL_RECOGNITION_GEOMETRIC_CONSISTENCY_IMPL_H_