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 #pragma once 00038 #include <pcl/recognition/hv/greedy_verification.h> 00039 00040 template<typename ModelT, typename SceneT> 00041 void 00042 pcl::GreedyVerification<ModelT, SceneT>::initialize () 00043 { 00044 //clear stuff 00045 recognition_models_.clear (); 00046 points_explained_by_rm_.clear (); 00047 00048 // initialize mask... 00049 mask_.resize (visible_models_.size ()); 00050 for (size_t i = 0; i < visible_models_.size (); i++) 00051 mask_[i] = false; 00052 00053 // initialize explained_by_RM 00054 points_explained_by_rm_.resize (scene_cloud_downsampled_->points.size ()); 00055 00056 // initalize model 00057 for (size_t m = 0; m < visible_models_.size (); m++) 00058 { 00059 boost::shared_ptr < RecognitionModel > recog_model (new RecognitionModel); 00060 // voxelize model cloud 00061 recog_model->cloud_.reset (new pcl::PointCloud<ModelT>); 00062 recog_model->id_ = static_cast<int> (m); 00063 00064 pcl::VoxelGrid<ModelT> voxel_grid; 00065 voxel_grid.setInputCloud (visible_models_[m]); 00066 voxel_grid.setLeafSize (resolution_, resolution_, resolution_); 00067 voxel_grid.filter (*(recog_model->cloud_)); 00068 00069 std::vector<int> explained_indices; 00070 std::vector<int> outliers; 00071 std::vector<int> nn_indices; 00072 std::vector<float> nn_distances; 00073 00074 for (size_t i = 0; i < recog_model->cloud_->points.size (); i++) 00075 { 00076 if (!scene_downsampled_tree_->radiusSearch (recog_model->cloud_->points[i], inliers_threshold_, nn_indices, nn_distances, 00077 std::numeric_limits<int>::max ())) 00078 { 00079 outliers.push_back (static_cast<int> (i)); 00080 } 00081 else 00082 { 00083 for (size_t k = 0; k < nn_distances.size (); k++) 00084 { 00085 explained_indices.push_back (nn_indices[k]); //nn_indices[k] points to the scene 00086 } 00087 } 00088 } 00089 00090 std::sort (explained_indices.begin (), explained_indices.end ()); 00091 explained_indices.erase (std::unique (explained_indices.begin (), explained_indices.end ()), explained_indices.end ()); 00092 00093 recog_model->bad_information_ = static_cast<int> (outliers.size ()); 00094 recog_model->explained_ = explained_indices; 00095 recog_model->good_information_ = static_cast<int> (explained_indices.size ()); 00096 recog_model->regularizer_ = regularizer_; 00097 00098 recognition_models_.push_back (recog_model); 00099 00100 for (size_t i = 0; i < explained_indices.size (); i++) 00101 { 00102 points_explained_by_rm_[explained_indices[i]].push_back (recog_model); 00103 } 00104 } 00105 00106 sortModels (); 00107 } 00108 00109 template<typename ModelT, typename SceneT> 00110 void 00111 pcl::GreedyVerification<ModelT, SceneT>::verify () 00112 { 00113 initialize (); 00114 00115 std::vector<bool> best_solution_; 00116 best_solution_.resize (recognition_models_.size ()); 00117 00118 for (size_t i = 0; i < recognition_models_.size (); i++) 00119 { 00120 if (static_cast<float> (recognition_models_[i]->good_information_) > (regularizer_ 00121 * static_cast<float> (recognition_models_[i]->bad_information_))) 00122 { 00123 best_solution_[i] = true; 00124 updateGoodInformation (static_cast<int> (i)); 00125 } 00126 else 00127 best_solution_[i] = false; 00128 } 00129 00130 for (size_t i = 0; i < best_solution_.size (); i++) 00131 { 00132 if (best_solution_[i]) 00133 { 00134 mask_[indices_models_[i].index_] = true; 00135 } 00136 else 00137 { 00138 mask_[indices_models_[i].index_] = false; 00139 } 00140 } 00141 } 00142 00143 #define PCL_INSTANTIATE_GreedyVerification(T1,T2) template class PCL_EXPORTS pcl::GreedyVerification<T1,T2>;