Point Cloud Library (PCL)  1.14.1-dev
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 
53 template<typename PointModelT, typename PointSceneT, typename PointModelRfT, typename PointSceneRfT>
54 template<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 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
84 template<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  {
95  ModelRfCloudPtr new_input_rf (new ModelRfCloud ());
96  computeRf<PointModelT, PointModelRfT> (input_, *new_input_rf);
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 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
137 template<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  {
155  ModelRfCloudPtr new_scene_rf (new ModelRfCloud ());
156  computeRf<PointSceneT, PointSceneRfT> (scene_, *new_scene_rf);
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 
206  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 ();
207  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 ();
208  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 ();
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
233  hough_space_.reset (new pcl::recognition::HoughSpace3D (d_min, bin_size, d_max));
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 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
258 template<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
277  PointCloudPtr temp_scene_cloud_ptr (new PointCloud);
278  pcl::copyPointCloud (*scene_, *temp_scene_cloud_ptr);
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  {
288  Correspondences temp_corrs, filtered_corrs;
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 
298  model_instances.push_back (filtered_corrs);
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 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
333 template <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 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
342 template <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.empty());
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_
BOARDLocalReferenceFrameEstimation implements the BOrder Aware Repeatable Directions algorithm for lo...
Definition: board.h:59
void setFindHoles(bool find_holes)
Sets whether holes in the margin of the support, for each point, are searched and accounted for in th...
Definition: board.h:102
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset.
Definition: feature.h:339
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
Definition: feature.h:198
void setKSearch(int k)
Set the number of k nearest neighbors to use for the feature estimation.
Definition: feature.h:184
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition: feature.hpp:194
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
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point.
Definition: normal_3d.h:244
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
Definition: normal_3d.h:328
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:65
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
HoughSpace3D is a 3D voting space.
Definition: hough_3d.h:58
CorrespondenceRejectorSampleConsensus implements a correspondence rejection using Random Sample Conse...
virtual void setInputSource(const PointCloudConstPtr &cloud)
Provide a source point cloud dataset (must contain XYZ data!)
Eigen::Matrix4f getBestTransformation()
Get the best transformation after RANSAC rejection.
virtual void setInputTarget(const PointCloudConstPtr &cloud)
Provide a target point cloud dataset (must contain XYZ data!)
void setInlierThreshold(double threshold)
Set the maximum distance between corresponding points.
void setMaximumIterations(int max_iterations)
Set the maximum number of iterations.
void getRemainingCorrespondences(const pcl::Correspondences &original_correspondences, pcl::Correspondences &remaining_correspondences) override
Get a list of valid correspondences after rejection from the original set of correspondences.
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:142
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences