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