Point Cloud Library (PCL) 1.13.0
Loading...
Searching...
No Matches
hv_papazov.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2011, Willow Garage, Inc.
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of Willow Garage, Inc. nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 */
36
37#pragma once
38#include <pcl/recognition/hv/hv_papazov.h>
39
40///////////////////////////////////////////////////////////////////////////////////////////////////
41template<typename ModelT, typename SceneT>
42 void
44 {
45
46 //clear stuff
47 recognition_models_.clear ();
48 graph_id_model_map_.clear ();
49 conflict_graph_.clear ();
50 explained_by_RM_.clear ();
51 points_explained_by_rm_.clear ();
52
53 // initialize mask...
54 mask_.resize (complete_models_.size ());
55 for (std::size_t i = 0; i < complete_models_.size (); i++)
56 mask_[i] = true;
57
58 // initialize explained_by_RM
59 explained_by_RM_.resize (scene_cloud_downsampled_->size ());
60 points_explained_by_rm_.resize (scene_cloud_downsampled_->size ());
61
62 // initialize model
63 for (std::size_t m = 0; m < complete_models_.size (); m++)
64 {
65 RecognitionModelPtr recog_model (new RecognitionModel);
66 // voxelize model cloud
67 recog_model->cloud_.reset (new pcl::PointCloud<ModelT>);
68 recog_model->complete_cloud_.reset (new pcl::PointCloud<ModelT>);
69 recog_model->id_ = static_cast<int> (m);
70
71 pcl::VoxelGrid<ModelT> voxel_grid;
72 voxel_grid.setInputCloud (visible_models_[m]);
73 voxel_grid.setLeafSize (resolution_, resolution_, resolution_);
74 voxel_grid.filter (*(recog_model->cloud_));
75
76 pcl::VoxelGrid<ModelT> voxel_grid_complete;
77 voxel_grid_complete.setInputCloud (complete_models_[m]);
78 voxel_grid_complete.setLeafSize (resolution_, resolution_, resolution_);
79 voxel_grid_complete.filter (*(recog_model->complete_cloud_));
80
81 std::vector<int> explained_indices;
82 std::vector<int> outliers;
83 pcl::Indices nn_indices;
84 std::vector<float> nn_distances;
85
86 for (std::size_t i = 0; i < recog_model->cloud_->size (); i++)
87 {
88 if (!scene_downsampled_tree_->radiusSearch ((*recog_model->cloud_)[i], inliers_threshold_, nn_indices, nn_distances,
89 std::numeric_limits<int>::max ()))
90 {
91 outliers.push_back (static_cast<int> (i));
92 }
93 else
94 {
95 for (std::size_t k = 0; k < nn_distances.size (); k++)
96 {
97 explained_indices.push_back (nn_indices[k]); //nn_indices[k] points to the scene
98 }
99 }
100 }
101
102 std::sort (explained_indices.begin (), explained_indices.end ());
103 explained_indices.erase (std::unique (explained_indices.begin (), explained_indices.end ()), explained_indices.end ());
104
105 recog_model->bad_information_ = static_cast<int> (outliers.size ());
106
107 if ((static_cast<float> (recog_model->bad_information_) / static_cast<float> (recog_model->complete_cloud_->size ()))
108 <= penalty_threshold_ && (static_cast<float> (explained_indices.size ())
109 / static_cast<float> (recog_model->complete_cloud_->size ())) >= support_threshold_)
110 {
111 recog_model->explained_ = explained_indices;
112 recognition_models_.push_back (recog_model);
113
114 // update explained_by_RM_, add 1
115 for (const int &explained_index : explained_indices)
116 {
117 explained_by_RM_[explained_index]++;
118 points_explained_by_rm_[explained_index].push_back (recog_model);
119 }
120 }
121 else
122 {
123 mask_[m] = false; // the model didn't survive the sequential check...
124 }
125 }
126 }
127
128///////////////////////////////////////////////////////////////////////////////////////////////////
129template<typename ModelT, typename SceneT>
130 void
132 {
133 // iterate over all vertices of the graph and check if they have a better neighbour, then remove that vertex
134 using VertexIterator = typename boost::graph_traits<Graph>::vertex_iterator;
135 VertexIterator vi, vi_end;
136 boost::tie (vi, vi_end) = boost::vertices (conflict_graph_);
137
138 for (auto next = vi; next != vi_end; next++)
139 {
140 const typename Graph::vertex_descriptor v = boost::vertex (*next, conflict_graph_);
141 typename boost::graph_traits<Graph>::adjacency_iterator ai;
142 typename boost::graph_traits<Graph>::adjacency_iterator ai_end;
143
144 auto current = std::static_pointer_cast<RecognitionModel> (graph_id_model_map_[int (v)]);
145
146 bool a_better_one = false;
147 for (boost::tie (ai, ai_end) = boost::adjacent_vertices (v, conflict_graph_); (ai != ai_end) && !a_better_one; ++ai)
148 {
149 auto neighbour = std::static_pointer_cast<RecognitionModel> (graph_id_model_map_[int (*ai)]);
150 if ((neighbour->explained_.size () >= current->explained_.size ()) && mask_[neighbour->id_])
151 {
152 a_better_one = true;
153 }
154 }
155
156 if (a_better_one)
157 {
158 mask_[current->id_] = false;
159 }
160 }
161 }
162
163///////////////////////////////////////////////////////////////////////////////////////////////////
164template<typename ModelT, typename SceneT>
165 void
167 {
168 // create vertices for the graph
169 for (std::size_t i = 0; i < (recognition_models_.size ()); i++)
170 {
171 const typename Graph::vertex_descriptor v = boost::add_vertex (recognition_models_[i], conflict_graph_);
172 graph_id_model_map_[int (v)] = std::static_pointer_cast<RecognitionModel> (recognition_models_[i]);
173 }
174
175 // iterate over the remaining models and check for each one if there is a conflict with another one
176 for (std::size_t i = 0; i < recognition_models_.size (); i++)
177 {
178 for (std::size_t j = i; j < recognition_models_.size (); j++)
179 {
180 if (i != j)
181 {
182 float n_conflicts = 0.f;
183 // count scene points explained by both models
184 for (std::size_t k = 0; k < explained_by_RM_.size (); k++)
185 {
186 if (explained_by_RM_[k] > 1)
187 {
188 // this point could be a conflict
189 bool i_found = false;
190 bool j_found = false;
191 bool both_found = false;
192 for (std::size_t kk = 0; (kk < points_explained_by_rm_[k].size ()) && !both_found; kk++)
193 {
194 if (points_explained_by_rm_[k][kk]->id_ == recognition_models_[i]->id_)
195 i_found = true;
196
197 if (points_explained_by_rm_[k][kk]->id_ == recognition_models_[j]->id_)
198 j_found = true;
199
200 if (i_found && j_found)
201 both_found = true;
202 }
203
204 if (both_found)
205 n_conflicts += 1.f;
206 }
207 }
208
209 // check if number of points is big enough to create a conflict
210 bool add_conflict = false;
211 add_conflict = ((n_conflicts / static_cast<float> (recognition_models_[i]->complete_cloud_->size ())) > conflict_threshold_size_)
212 || ((n_conflicts / static_cast<float> (recognition_models_[j]->complete_cloud_->size ())) > conflict_threshold_size_);
213
214 if (add_conflict)
215 {
216 boost::add_edge (i, j, conflict_graph_);
217 }
218 }
219 }
220 }
221 }
222
223///////////////////////////////////////////////////////////////////////////////////////////////////
224template<typename ModelT, typename SceneT>
225 void
227 {
228 initialize ();
229 buildConflictGraph ();
230 nonMaximaSuppresion ();
231 }
232
233#define PCL_INSTANTIATE_PapazovHV(T1,T2) template class PCL_EXPORTS pcl::PapazovHV<T1,T2>;
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
Definition filter.h:121
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition pcl_base.hpp:65
A hypothesis verification method proposed in "An Efficient RANSAC for 3D Object Recognition in Noisy ...
Definition hv_papazov.h:56
void verify() override
PointCloud represents the base class in PCL for storing collections of 3D points.
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
Definition voxel_grid.h:177
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
Definition voxel_grid.h:221
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133