Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
hough_3d.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2012, 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 * $Id:$
37 *
38 */
39
40#ifndef PCL_RECOGNITION_HOUGH_3D_IMPL_H_
41#define PCL_RECOGNITION_HOUGH_3D_IMPL_H_
42
43#include <pcl/common/io.h> // for copyPointCloud
44#include <pcl/recognition/cg/hough_3d.h>
45#include <pcl/registration/correspondence_types.h>
46#include <pcl/registration/correspondence_rejection_sample_consensus.h>
47//#include <pcl/sample_consensus/ransac.h>
48//#include <pcl/sample_consensus/sac_model_registration.h>
49#include <pcl/features/normal_3d.h>
50#include <pcl/features/board.h>
51
52
53template<typename PointModelT, typename PointSceneT, typename PointModelRfT, typename PointSceneRfT>
54template<typename PointType, typename PointRfType> void
56{
57 if (local_rf_search_radius_ == 0)
58 {
59 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");
60 local_rf_search_radius_ = static_cast<float> (hough_bin_size_);
61 }
64 norm_est.setInputCloud (input);
65 if (local_rf_normals_search_radius_ <= 0.0f)
66 {
67 norm_est.setKSearch (15);
68 }
69 else
70 {
71 norm_est.setRadiusSearch (local_rf_normals_search_radius_);
72 }
73 norm_est.compute (*normal_cloud);
74
76 rf_est.setInputCloud (input);
77 rf_est.setInputNormals (normal_cloud);
78 rf_est.setFindHoles (true);
79 rf_est.setRadiusSearch (local_rf_search_radius_);
80 rf_est.compute (rf);
81}
82
83////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
84template<typename PointModelT, typename PointSceneT, typename PointModelRfT, typename PointSceneRfT> bool
86{
87 if (!input_)
88 {
89 PCL_ERROR ("[pcl::Hough3DGrouping::train()] Error! Input cloud not set.\n");
90 return (false);
91 }
92
93 if (!input_rf_)
94 {
97 input_rf_ = new_input_rf;
98 //PCL_ERROR(
99 // "[pcl::Hough3DGrouping::train()] Error! Input reference frame not set.\n");
100 //return (false);
101 }
102
103 if (input_->size () != input_rf_->size ())
104 {
105 PCL_ERROR ("[pcl::Hough3DGrouping::train()] Error! Input cloud size != Input RF cloud size.\n");
106 return (false);
107 }
108
109 model_votes_.clear ();
110 model_votes_.resize (input_->size ());
111
112 // compute model centroid
113 Eigen::Vector3f centroid (0, 0, 0);
114 for (std::size_t i = 0; i < input_->size (); ++i)
115 {
116 centroid += input_->at (i).getVector3fMap ();
117 }
118 centroid /= static_cast<float> (input_->size ());
119
120 // compute model votes
121 for (std::size_t i = 0; i < input_->size (); ++i)
122 {
123 Eigen::Vector3f x_ax ((*input_rf_)[i].x_axis[0], (*input_rf_)[i].x_axis[1], (*input_rf_)[i].x_axis[2]);
124 Eigen::Vector3f y_ax ((*input_rf_)[i].y_axis[0], (*input_rf_)[i].y_axis[1], (*input_rf_)[i].y_axis[2]);
125 Eigen::Vector3f z_ax ((*input_rf_)[i].z_axis[0], (*input_rf_)[i].z_axis[1], (*input_rf_)[i].z_axis[2]);
126
127 model_votes_[i].x () = x_ax.dot (centroid - input_->at (i).getVector3fMap ());
128 model_votes_[i].y () = y_ax.dot (centroid - input_->at (i).getVector3fMap ());
129 model_votes_[i].z () = z_ax.dot (centroid - input_->at (i).getVector3fMap ());
130 }
131
132 needs_training_ = false;
133 return (true);
134}
135
136////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
137template<typename PointModelT, typename PointSceneT, typename PointModelRfT, typename PointSceneRfT> bool
139{
140 if (needs_training_)
141 {
142 if (!train ())//checks input and input_rf
143 return (false);
144 }
145
146 //if (!scene_)
147 //{
148 // PCL_ERROR(
149 // "[pcl::Hough3DGrouping::recognizeModelInstances()] Error! Scene cloud not set.\n");
150 // return (false);
151 //}
152
153 if (!scene_rf_)
154 {
157 scene_rf_ = new_scene_rf;
158 //PCL_ERROR(
159 // "[pcl::Hough3DGrouping::recognizeModelInstances()] Error! Scene reference frame not set.\n");
160 //return (false);
161 }
162
163 if (scene_->size () != scene_rf_->size ())
164 {
165 PCL_ERROR ("[pcl::Hough3DGrouping::recognizeModelInstances()] Error! Scene cloud size != Scene RF cloud size.\n");
166 return (false);
167 }
168
169 if (!model_scene_corrs_)
170 {
171 PCL_ERROR ("[pcl::Hough3DGrouping::recognizeModelInstances()] Error! Correspondences not set, please set them before calling again this function.\n");
172 return (false);
173 }
174
175 int n_matches = static_cast<int> (model_scene_corrs_->size ());
176 if (n_matches == 0)
177 {
178 return (false);
179 }
180
181 std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > scene_votes (n_matches);
182 Eigen::Vector3d d_min, d_max, bin_size;
183
184 d_min.setConstant (std::numeric_limits<double>::max ());
185 d_max.setConstant (-std::numeric_limits<double>::max ());
186 bin_size.setConstant (hough_bin_size_);
187
188 float max_distance = -std::numeric_limits<float>::max ();
189
190 // Calculating 3D Hough space dimensions and vote position for each match
191 for (int i=0; i< n_matches; ++i)
192 {
193 int scene_index = model_scene_corrs_->at (i).index_match;
194 int model_index = model_scene_corrs_->at (i).index_query;
195
196 const Eigen::Vector3f& scene_point = scene_->at (scene_index).getVector3fMap ();
197 const PointSceneRfT& scene_point_rf = scene_rf_->at (scene_index);
198
199 Eigen::Vector3f scene_point_rf_x (scene_point_rf.x_axis[0], scene_point_rf.x_axis[1], scene_point_rf.x_axis[2]);
200 Eigen::Vector3f scene_point_rf_y (scene_point_rf.y_axis[0], scene_point_rf.y_axis[1], scene_point_rf.y_axis[2]);
201 Eigen::Vector3f scene_point_rf_z (scene_point_rf.z_axis[0], scene_point_rf.z_axis[1], scene_point_rf.z_axis[2]);
202
203 //const Eigen::Vector3f& model_point = input_->at (model_index).getVector3fMap ();
204 const Eigen::Vector3f& model_point_vote = model_votes_[model_index];
205
209
210 if (scene_votes[i].x () < d_min.x ())
211 d_min.x () = scene_votes[i].x ();
212 if (scene_votes[i].x () > d_max.x ())
213 d_max.x () = scene_votes[i].x ();
214
215 if (scene_votes[i].y () < d_min.y ())
216 d_min.y () = scene_votes[i].y ();
217 if (scene_votes[i].y () > d_max.y ())
218 d_max.y () = scene_votes[i].y ();
219
220 if (scene_votes[i].z () < d_min.z ())
221 d_min.z () = scene_votes[i].z ();
222 if (scene_votes[i].z () > d_max.z ())
223 d_max.z () = scene_votes[i].z ();
224
225 // Calculate max distance for interpolated votes
226 if (use_interpolation_ && max_distance < model_scene_corrs_->at (i).distance)
227 {
228 max_distance = model_scene_corrs_->at (i).distance;
229 }
230 }
231
232 // Hough Voting
234
235 for (int i = 0; i < n_matches; ++i)
236 {
237 double weight = 1.0;
238 if (use_distance_weight_ && max_distance != 0)
239 {
240 weight = 1.0 - (model_scene_corrs_->at (i).distance / max_distance);
241 }
242 if (use_interpolation_)
243 {
244 hough_space_->voteInt (scene_votes[i], weight, i);
245 }
246 else
247 {
248 hough_space_->vote (scene_votes[i], weight, i);
249 }
250 }
251
252 hough_space_initialized_ = true;
253
254 return (true);
255}
256
257////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
258template<typename PointModelT, typename PointSceneT, typename PointModelRfT, typename PointSceneRfT> void
260{
261 model_instances.clear ();
262 found_transformations_.clear ();
263
264 if (!hough_space_initialized_ && !houghVoting ())
265 {
266 return;
267 }
268
269 // Finding max bins and voters
270 std::vector<double> max_values;
271 std::vector<std::vector<int> > max_ids;
272
273 hough_space_->findMaxima (hough_threshold_, max_values, max_ids);
274
275 // Insert maximas into result vector, after Ransac correspondence rejection
276 // Temp copy of scene cloud with the type cast to ModelT in order to use Ransac
279
281 corr_rejector.setMaximumIterations (10000);
282 corr_rejector.setInlierThreshold (hough_bin_size_);
283 corr_rejector.setInputSource (input_);
284 corr_rejector.setInputTarget (temp_scene_cloud_ptr);
285
286 for (std::size_t j = 0; j < max_values.size (); ++j)
287 {
289 for (const int &i : max_ids[j])
290 {
291 temp_corrs.push_back (model_scene_corrs_->at (i));
292 }
293 // RANSAC filtering
294 corr_rejector.getRemainingCorrespondences (temp_corrs, filtered_corrs);
295 // Save transformations for recognize
296 found_transformations_.push_back (corr_rejector.getBestTransformation ());
297
299 }
300}
301
302////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
303//template<typename PointModelT, typename PointSceneT, typename PointModelRfT, typename PointSceneRfT> bool
304//pcl::Hough3DGrouping<PointModelT, PointSceneT, PointModelRfT, PointSceneRfT>::getTransformMatrix (const PointCloudConstPtr &scene_cloud, const Correspondences &corrs, Eigen::Matrix4f &transform)
305//{
306// std::vector<int> model_indices;
307// std::vector<int> scene_indices;
308// pcl::registration::getQueryIndices (corrs, model_indices);
309// pcl::registration::getMatchIndices (corrs, scene_indices);
310//
311// typename pcl::SampleConsensusModelRegistration<PointModelT>::Ptr model (new pcl::SampleConsensusModelRegistration<PointModelT> (input_, model_indices));
312// model->setInputTarget (scene_cloud, scene_indices);
313//
314// pcl::RandomSampleConsensus<PointModelT> ransac (model);
315// ransac.setDistanceThreshold (hough_bin_size_);
316// ransac.setMaxIterations (10000);
317// if (!ransac.computeModel ())
318// return (false);
319//
320// // Transform model coefficients from vectorXf to matrix4f
321// Eigen::VectorXf coeffs;
322// ransac.getModelCoefficients (coeffs);
323//
324// transform.row (0) = coeffs.segment<4> (0);
325// transform.row (1) = coeffs.segment<4> (4);
326// transform.row (2) = coeffs.segment<4> (8);
327// transform.row (3) = coeffs.segment<4> (12);
328//
329// return (true);
330//}
331
332////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
333template <typename PointModelT, typename PointSceneT, typename PointModelRfT, typename PointSceneRfT> bool
335 std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations)
336{
337 std::vector<pcl::Correspondences> model_instances;
338 return (this->recognize (transformations, model_instances));
339}
340
341////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
342template <typename PointModelT, typename PointSceneT, typename PointModelRfT, typename PointSceneRfT> bool
344 std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations, std::vector<pcl::Correspondences> &clustered_corrs)
345{
346 transformations.clear ();
347 if (!this->initCompute ())
348 {
349 PCL_ERROR ("[pcl::Hough3DGrouping::recognize()] Error! Model cloud or Scene cloud not set, please set them before calling again this function.\n");
350 return (false);
351 }
352
353 clusterCorrespondences (clustered_corrs);
354
355 transformations = found_transformations_;
356
357 //// Temp copy of scene cloud with the type cast to ModelT in order to use Ransac
358 //PointCloudPtr temp_scene_cloud_ptr (new PointCloud);
359 //pcl::copyPointCloud (*scene_, *temp_scene_cloud_ptr);
360
361 //for (std::size_t i = 0; i < model_instances.size (); ++i)
362 //{
363 // Eigen::Matrix4f curr_transf;
364 // if (getTransformMatrix (temp_scene_cloud_ptr, model_instances[i], curr_transf))
365 // transformations.push_back (curr_transf);
366 //}
367
368 this->deinitCompute ();
369 return (transformations.size ());
370}
371
372
373#define PCL_INSTANTIATE_Hough3DGrouping(T,ST,RFT,SRFT) template class PCL_EXPORTS pcl::Hough3DGrouping<T,ST,RFT,SRFT>;
374
375#endif // PCL_RECOGNITION_HOUGH_3D_IMPL_H_
Iterator class for point clouds with or without given indices.
ConstCloudIterator(const PointCloud< PointT > &cloud)
std::size_t size() const
Size of the range the iterator is going through.
bool recognize(std::vector< Eigen::Matrix4f, Eigen::aligned_allocator< Eigen::Matrix4f > > &transformations)
The main function, recognizes instances of the model into the scene set by the user.
Definition hough_3d.hpp:334
typename ModelRfCloud::Ptr ModelRfCloudPtr
Definition hough_3d.h:152
bool houghVoting()
The Hough space voting procedure.
Definition hough_3d.hpp:138
bool train()
Call this function after setting the input, the input_rf and the hough_bin_size parameters to perform...
Definition hough_3d.hpp:85
void computeRf(const typename pcl::PointCloud< PointType >::ConstPtr &input, pcl::PointCloud< PointRfType > &rf)
Computes the reference frame for an input cloud.
Definition hough_3d.hpp:55
typename PointCloud::Ptr PointCloudPtr
Definition hough_3d.h:160
void clusterCorrespondences(std::vector< Correspondences > &model_instances) override
Cluster the input correspondences in order to distinguish between different instances of the model in...
Definition hough_3d.hpp:259
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
HoughSpace3D is a 3D voting space.
Definition hough_3d.h:58
CorrespondenceRejectorSampleConsensus implements a correspondence rejection using Random Sample Conse...
void copyPointCloud(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Copy all the fields from a given point cloud into a new point cloud.
Definition io.hpp:144
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences