Point Cloud Library (PCL)  1.7.1
hv_go.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2012 Aitor Aldoma, Federico Tombari
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 #include <pcl/recognition/hv/hv_go.h>
38 #include <numeric>
39 #include <pcl/common/time.h>
40 #include <pcl/point_types.h>
41 
42 template<typename PointT, typename NormalT>
43 inline void extractEuclideanClustersSmooth(const typename pcl::PointCloud<PointT> &cloud, const typename pcl::PointCloud<NormalT> &normals, float tolerance,
44  const typename pcl::search::Search<PointT>::Ptr &tree, std::vector<pcl::PointIndices> &clusters, double eps_angle, float curvature_threshold,
45  unsigned int min_pts_per_cluster, unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())
46 {
47 
48  if (tree->getInputCloud ()->points.size () != cloud.points.size ())
49  {
50  PCL_ERROR("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset\n");
51  return;
52  }
53  if (cloud.points.size () != normals.points.size ())
54  {
55  PCL_ERROR("[pcl::extractEuclideanClusters] Number of points in the input point cloud different than normals!\n");
56  return;
57  }
58 
59  // Create a bool vector of processed point indices, and initialize it to false
60  std::vector<bool> processed (cloud.points.size (), false);
61 
62  std::vector<int> nn_indices;
63  std::vector<float> nn_distances;
64  // Process all points in the indices vector
65  int size = static_cast<int> (cloud.points.size ());
66  for (int i = 0; i < size; ++i)
67  {
68  if (processed[i])
69  continue;
70 
71  std::vector<unsigned int> seed_queue;
72  int sq_idx = 0;
73  seed_queue.push_back (i);
74 
75  processed[i] = true;
76 
77  while (sq_idx < static_cast<int> (seed_queue.size ()))
78  {
79 
80  if (normals.points[seed_queue[sq_idx]].curvature > curvature_threshold)
81  {
82  sq_idx++;
83  continue;
84  }
85 
86  // Search for sq_idx
87  if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
88  {
89  sq_idx++;
90  continue;
91  }
92 
93  for (size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx
94  {
95  if (processed[nn_indices[j]]) // Has this point been processed before ?
96  continue;
97 
98  if (normals.points[nn_indices[j]].curvature > curvature_threshold)
99  {
100  continue;
101  }
102 
103  //processed[nn_indices[j]] = true;
104  // [-1;1]
105 
106  double dot_p = normals.points[seed_queue[sq_idx]].normal[0] * normals.points[nn_indices[j]].normal[0]
107  + normals.points[seed_queue[sq_idx]].normal[1] * normals.points[nn_indices[j]].normal[1]
108  + normals.points[seed_queue[sq_idx]].normal[2] * normals.points[nn_indices[j]].normal[2];
109 
110  if (fabs (acos (dot_p)) < eps_angle)
111  {
112  processed[nn_indices[j]] = true;
113  seed_queue.push_back (nn_indices[j]);
114  }
115  }
116 
117  sq_idx++;
118  }
119 
120  // If this queue is satisfactory, add to the clusters
121  if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
122  {
124  r.indices.resize (seed_queue.size ());
125  for (size_t j = 0; j < seed_queue.size (); ++j)
126  r.indices[j] = seed_queue[j];
127 
128  std::sort (r.indices.begin (), r.indices.end ());
129  r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());
130 
131  r.header = cloud.header;
132  clusters.push_back (r); // We could avoid a copy by working directly in the vector
133  }
134  }
135 }
136 
137 template<typename ModelT, typename SceneT>
138 mets::gol_type pcl::GlobalHypothesesVerification<ModelT, SceneT>::evaluateSolution(const std::vector<bool> & active, int changed)
139 {
140  float sign = 1.f;
141  //update explained_by_RM
142  if (active[changed])
143  {
144  //it has been activated
145  updateExplainedVector (recognition_models_[changed]->explained_, recognition_models_[changed]->explained_distances_, explained_by_RM_,
146  explained_by_RM_distance_weighted, 1.f);
147  updateUnexplainedVector (recognition_models_[changed]->unexplained_in_neighborhood, recognition_models_[changed]->unexplained_in_neighborhood_weights,
148  unexplained_by_RM_neighboorhods, recognition_models_[changed]->explained_, explained_by_RM_, 1.f);
149  updateCMDuplicity(recognition_models_[changed]->complete_cloud_occupancy_indices_, complete_cloud_occupancy_by_RM_, 1.f);
150  } else
151  {
152  //it has been deactivated
153  updateExplainedVector (recognition_models_[changed]->explained_, recognition_models_[changed]->explained_distances_, explained_by_RM_,
154  explained_by_RM_distance_weighted, -1.f);
155  updateUnexplainedVector (recognition_models_[changed]->unexplained_in_neighborhood, recognition_models_[changed]->unexplained_in_neighborhood_weights,
156  unexplained_by_RM_neighboorhods, recognition_models_[changed]->explained_, explained_by_RM_, -1.f);
157  updateCMDuplicity(recognition_models_[changed]->complete_cloud_occupancy_indices_, complete_cloud_occupancy_by_RM_, -1.f);
158  sign = -1.f;
159  }
160 
161  int duplicity = getDuplicity ();
162  float good_info = getExplainedValue ();
163 
164  float unexplained_info = getPreviousUnexplainedValue ();
165  float bad_info = static_cast<float> (getPreviousBadInfo ())
166  + (recognition_models_[changed]->outliers_weight_ * static_cast<float> (recognition_models_[changed]->bad_information_)) * sign;
167 
168  setPreviousBadInfo (bad_info);
169 
170  int n_active_hyp = 0;
171  for(size_t i=0; i < active.size(); i++) {
172  if(active[i])
173  n_active_hyp++;
174  }
175 
176  float duplicity_cm = static_cast<float> (getDuplicityCM ()) * w_occupied_multiple_cm_;
177  return static_cast<mets::gol_type> ((good_info - bad_info - static_cast<float> (duplicity) - unexplained_info - duplicity_cm - static_cast<float> (n_active_hyp)) * -1.f); //return the dual to our max problem
178 }
179 
180 ///////////////////////////////////////////////////////////////////////////////////////////////////
181 template<typename ModelT, typename SceneT>
183 {
184  //clear stuff
185  recognition_models_.clear ();
186  unexplained_by_RM_neighboorhods.clear ();
187  explained_by_RM_distance_weighted.clear ();
188  explained_by_RM_.clear ();
189  mask_.clear ();
190  indices_.clear (),
191  complete_cloud_occupancy_by_RM_.clear ();
192 
193  // initialize mask to false
194  mask_.resize (complete_models_.size ());
195  for (size_t i = 0; i < complete_models_.size (); i++)
196  mask_[i] = false;
197 
198  indices_.resize (complete_models_.size ());
199 
200  NormalEstimator_ n3d;
201  scene_normals_.reset (new pcl::PointCloud<pcl::Normal> ());
202 
204  normals_tree->setInputCloud (scene_cloud_downsampled_);
205 
206  n3d.setRadiusSearch (radius_normals_);
207  n3d.setSearchMethod (normals_tree);
208  n3d.setInputCloud (scene_cloud_downsampled_);
209  n3d.compute (*scene_normals_);
210 
211  //check nans...
212  int j = 0;
213  for (size_t i = 0; i < scene_normals_->points.size (); ++i)
214  {
215  if (!pcl_isfinite (scene_normals_->points[i].normal_x) || !pcl_isfinite (scene_normals_->points[i].normal_y)
216  || !pcl_isfinite (scene_normals_->points[i].normal_z))
217  continue;
218 
219  scene_normals_->points[j] = scene_normals_->points[i];
220  scene_cloud_downsampled_->points[j] = scene_cloud_downsampled_->points[i];
221 
222  j++;
223  }
224 
225  scene_normals_->points.resize (j);
226  scene_normals_->width = j;
227  scene_normals_->height = 1;
228 
229  scene_cloud_downsampled_->points.resize (j);
230  scene_cloud_downsampled_->width = j;
231  scene_cloud_downsampled_->height = 1;
232 
233  explained_by_RM_.resize (scene_cloud_downsampled_->points.size (), 0);
234  explained_by_RM_distance_weighted.resize (scene_cloud_downsampled_->points.size (), 0.f);
235  unexplained_by_RM_neighboorhods.resize (scene_cloud_downsampled_->points.size (), 0.f);
236 
237  //compute segmentation of the scene if detect_clutter_
238  if (detect_clutter_)
239  {
240  //initialize kdtree for search
241  scene_downsampled_tree_.reset (new pcl::search::KdTree<SceneT>);
242  scene_downsampled_tree_->setInputCloud (scene_cloud_downsampled_);
243 
244  std::vector<pcl::PointIndices> clusters;
245  double eps_angle_threshold = 0.2;
246  int min_points = 20;
247  float curvature_threshold = 0.045f;
248 
249  extractEuclideanClustersSmooth<SceneT, pcl::Normal> (*scene_cloud_downsampled_, *scene_normals_, inliers_threshold_ * 2.f, scene_downsampled_tree_,
250  clusters, eps_angle_threshold, curvature_threshold, min_points);
251 
252  clusters_cloud_.reset (new pcl::PointCloud<pcl::PointXYZI>);
253  clusters_cloud_->points.resize (scene_cloud_downsampled_->points.size ());
254  clusters_cloud_->width = scene_cloud_downsampled_->width;
255  clusters_cloud_->height = 1;
256 
257  for (size_t i = 0; i < scene_cloud_downsampled_->points.size (); i++)
258  {
259  pcl::PointXYZI p;
260  p.getVector3fMap () = scene_cloud_downsampled_->points[i].getVector3fMap ();
261  p.intensity = 0.f;
262  clusters_cloud_->points[i] = p;
263  }
264 
265  float intens_incr = 100.f / static_cast<float> (clusters.size ());
266  float intens = intens_incr;
267  for (size_t i = 0; i < clusters.size (); i++)
268  {
269  for (size_t j = 0; j < clusters[i].indices.size (); j++)
270  {
271  clusters_cloud_->points[clusters[i].indices[j]].intensity = intens;
272  }
273 
274  intens += intens_incr;
275  }
276  }
277 
278  //compute cues
279  {
280  pcl::ScopeTime tcues ("Computing cues");
281  recognition_models_.resize (complete_models_.size ());
282  int valid = 0;
283  for (int i = 0; i < static_cast<int> (complete_models_.size ()); i++)
284  {
285  //create recognition model
286  recognition_models_[valid].reset (new RecognitionModel ());
287  if(addModel (visible_models_[i], complete_models_[i], recognition_models_[valid])) {
288  indices_[valid] = i;
289  valid++;
290  }
291  }
292 
293  recognition_models_.resize(valid);
294  indices_.resize(valid);
295  }
296 
297  //compute the bounding boxes for the models
298  ModelT min_pt_all, max_pt_all;
299  min_pt_all.x = min_pt_all.y = min_pt_all.z = std::numeric_limits<float>::max ();
300  max_pt_all.x = max_pt_all.y = max_pt_all.z = (std::numeric_limits<float>::max () - 0.001f) * -1;
301 
302  for (size_t i = 0; i < recognition_models_.size (); i++)
303  {
304  ModelT min_pt, max_pt;
305  pcl::getMinMax3D (*complete_models_[indices_[i]], min_pt, max_pt);
306  if (min_pt.x < min_pt_all.x)
307  min_pt_all.x = min_pt.x;
308 
309  if (min_pt.y < min_pt_all.y)
310  min_pt_all.y = min_pt.y;
311 
312  if (min_pt.z < min_pt_all.z)
313  min_pt_all.z = min_pt.z;
314 
315  if (max_pt.x > max_pt_all.x)
316  max_pt_all.x = max_pt.x;
317 
318  if (max_pt.y > max_pt_all.y)
319  max_pt_all.y = max_pt.y;
320 
321  if (max_pt.z > max_pt_all.z)
322  max_pt_all.z = max_pt.z;
323  }
324 
325  int size_x, size_y, size_z;
326  size_x = static_cast<int> (std::ceil (std::abs (max_pt_all.x - min_pt_all.x) / res_occupancy_grid_)) + 1;
327  size_y = static_cast<int> (std::ceil (std::abs (max_pt_all.y - min_pt_all.y) / res_occupancy_grid_)) + 1;
328  size_z = static_cast<int> (std::ceil (std::abs (max_pt_all.z - min_pt_all.z) / res_occupancy_grid_)) + 1;
329 
330  complete_cloud_occupancy_by_RM_.resize (size_x * size_y * size_z, 0);
331 
332  for (size_t i = 0; i < recognition_models_.size (); i++)
333  {
334 
335  std::map<int, bool> banned;
336  std::map<int, bool>::iterator banned_it;
337 
338  for (size_t j = 0; j < complete_models_[indices_[i]]->points.size (); j++)
339  {
340  int pos_x, pos_y, pos_z;
341  pos_x = static_cast<int> (std::floor ((complete_models_[indices_[i]]->points[j].x - min_pt_all.x) / res_occupancy_grid_));
342  pos_y = static_cast<int> (std::floor ((complete_models_[indices_[i]]->points[j].y - min_pt_all.y) / res_occupancy_grid_));
343  pos_z = static_cast<int> (std::floor ((complete_models_[indices_[i]]->points[j].z - min_pt_all.z) / res_occupancy_grid_));
344 
345  int idx = pos_z * size_x * size_y + pos_y * size_x + pos_x;
346  banned_it = banned.find (idx);
347  if (banned_it == banned.end ())
348  {
349  complete_cloud_occupancy_by_RM_[idx]++;
350  recognition_models_[i]->complete_cloud_occupancy_indices_.push_back (idx);
351  banned[idx] = true;
352  }
353  }
354  }
355 
356  {
357  pcl::ScopeTime tcues ("Computing clutter cues");
358 #pragma omp parallel for schedule(dynamic, 4) num_threads(omp_get_num_procs())
359  for (int j = 0; j < static_cast<int> (recognition_models_.size ()); j++)
360  computeClutterCue (recognition_models_[j]);
361  }
362 
363  cc_.clear ();
364  n_cc_ = 1;
365  cc_.resize (n_cc_);
366  for (size_t i = 0; i < recognition_models_.size (); i++)
367  cc_[0].push_back (static_cast<int> (i));
368 
369 }
370 
371 template<typename ModelT, typename SceneT>
372 void pcl::GlobalHypothesesVerification<ModelT, SceneT>::SAOptimize(std::vector<int> & cc_indices, std::vector<bool> & initial_solution)
373 {
374 
375  //temporal copy of recogniton_models_
376  std::vector < boost::shared_ptr<RecognitionModel> > recognition_models_copy;
377  recognition_models_copy = recognition_models_;
378 
379  recognition_models_.clear ();
380 
381  for (size_t j = 0; j < cc_indices.size (); j++)
382  {
383  recognition_models_.push_back (recognition_models_copy[cc_indices[j]]);
384  }
385 
386  for (size_t j = 0; j < recognition_models_.size (); j++)
387  {
388  boost::shared_ptr < RecognitionModel > recog_model = recognition_models_[j];
389  for (size_t i = 0; i < recog_model->explained_.size (); i++)
390  {
391  explained_by_RM_[recog_model->explained_[i]]++;
392  explained_by_RM_distance_weighted[recog_model->explained_[i]] += recog_model->explained_distances_[i];
393  }
394 
395  if (detect_clutter_)
396  {
397  for (size_t i = 0; i < recog_model->unexplained_in_neighborhood.size (); i++)
398  {
399  unexplained_by_RM_neighboorhods[recog_model->unexplained_in_neighborhood[i]] += recog_model->unexplained_in_neighborhood_weights[i];
400  }
401  }
402  }
403 
404  int occupied_multiple = 0;
405  for(size_t i=0; i < complete_cloud_occupancy_by_RM_.size(); i++) {
406  if(complete_cloud_occupancy_by_RM_[i] > 1) {
407  occupied_multiple+=complete_cloud_occupancy_by_RM_[i];
408  }
409  }
410 
411  setPreviousDuplicityCM(occupied_multiple);
412  //do optimization
413  //Define model SAModel, initial solution is all models activated
414 
415  int duplicity;
416  float good_information_ = getTotalExplainedInformation (explained_by_RM_, explained_by_RM_distance_weighted, &duplicity);
417  float bad_information_ = 0;
418  float unexplained_in_neighboorhod = getUnexplainedInformationInNeighborhood (unexplained_by_RM_neighboorhods, explained_by_RM_);
419 
420  for (size_t i = 0; i < initial_solution.size (); i++)
421  {
422  if (initial_solution[i])
423  bad_information_ += recognition_models_[i]->outliers_weight_ * static_cast<float> (recognition_models_[i]->bad_information_);
424  }
425 
426  setPreviousExplainedValue (good_information_);
427  setPreviousDuplicity (duplicity);
428  setPreviousBadInfo (bad_information_);
429  setPreviousUnexplainedValue (unexplained_in_neighboorhod);
430 
431  SAModel model;
432  model.cost_ = static_cast<mets::gol_type> ((good_information_ - bad_information_
433  - static_cast<float> (duplicity)
434  - static_cast<float> (occupied_multiple) * w_occupied_multiple_cm_
435  - static_cast<float> (recognition_models_.size ())
436  - unexplained_in_neighboorhod) * -1.f);
437 
438  model.setSolution (initial_solution);
439  model.setOptimizer (this);
440  SAModel best (model);
441 
442  move_manager neigh (static_cast<int> (cc_indices.size ()));
443 
444  mets::best_ever_solution best_recorder (best);
445  mets::noimprove_termination_criteria noimprove (max_iterations_);
446  mets::linear_cooling linear_cooling;
447  mets::simulated_annealing<move_manager> sa (model, best_recorder, neigh, noimprove, linear_cooling, initial_temp_, 1e-7, 2);
448  //sa.setApplyAndEvaluate(true);
449 
450  {
451  pcl::ScopeTime t ("SA search...");
452  sa.search ();
453  }
454 
455  best_seen_ = static_cast<const SAModel&> (best_recorder.best_seen ());
456  for (size_t i = 0; i < best_seen_.solution_.size (); i++)
457  {
458  initial_solution[i] = best_seen_.solution_[i];
459  }
460 
461  recognition_models_ = recognition_models_copy;
462 
463 }
464 
465 ///////////////////////////////////////////////////////////////////////////////////////////////////
466 template<typename ModelT, typename SceneT>
468 {
469  initialize ();
470 
471  //for each connected component, find the optimal solution
472  for (int c = 0; c < n_cc_; c++)
473  {
474  //TODO: Check for trivial case...
475  //TODO: Check also the number of hypotheses and use exhaustive enumeration if smaller than 10
476  std::vector<bool> subsolution (cc_[c].size (), true);
477  SAOptimize (cc_[c], subsolution);
478  for (size_t i = 0; i < subsolution.size (); i++)
479  {
480  mask_[indices_[cc_[c][i]]] = (subsolution[i]);
481  }
482  }
483 }
484 
485 template<typename ModelT, typename SceneT>
487  typename pcl::PointCloud<ModelT>::ConstPtr & complete_model, boost::shared_ptr<RecognitionModel> & recog_model)
488 {
489  //voxelize model cloud
490  recog_model->cloud_.reset (new pcl::PointCloud<ModelT> ());
491  recog_model->complete_cloud_.reset (new pcl::PointCloud<ModelT> ());
492 
493  float size_model = resolution_;
494  pcl::VoxelGrid<ModelT> voxel_grid;
495  voxel_grid.setInputCloud (model);
496  voxel_grid.setLeafSize (size_model, size_model, size_model);
497  voxel_grid.filter (*(recog_model->cloud_));
498 
499  pcl::VoxelGrid<ModelT> voxel_grid2;
500  voxel_grid2.setInputCloud (complete_model);
501  voxel_grid2.setLeafSize (size_model, size_model, size_model);
502  voxel_grid2.filter (*(recog_model->complete_cloud_));
503 
504  {
505  //check nans...
506  int j = 0;
507  for (size_t i = 0; i < recog_model->cloud_->points.size (); ++i)
508  {
509  if (!pcl_isfinite (recog_model->cloud_->points[i].x) || !pcl_isfinite (recog_model->cloud_->points[i].y)
510  || !pcl_isfinite (recog_model->cloud_->points[i].z))
511  continue;
512 
513  recog_model->cloud_->points[j] = recog_model->cloud_->points[i];
514  j++;
515  }
516 
517  recog_model->cloud_->points.resize (j);
518  recog_model->cloud_->width = j;
519  recog_model->cloud_->height = 1;
520  }
521 
522  if (recog_model->cloud_->points.size () <= 0)
523  {
524  PCL_WARN("The model cloud has no points..\n");
525  return false;
526  }
527 
528  //compute normals unless given (now do it always...)
530  typedef typename pcl::NormalEstimation<ModelT, pcl::Normal> NormalEstimator_;
531  NormalEstimator_ n3d;
532  recog_model->normals_.reset (new pcl::PointCloud<pcl::Normal> ());
533  normals_tree->setInputCloud (recog_model->cloud_);
534  n3d.setRadiusSearch (radius_normals_);
535  n3d.setSearchMethod (normals_tree);
536  n3d.setInputCloud ((recog_model->cloud_));
537  n3d.compute (*(recog_model->normals_));
538 
539  //check nans...
540  int j = 0;
541  for (size_t i = 0; i < recog_model->normals_->points.size (); ++i)
542  {
543  if (!pcl_isfinite (recog_model->normals_->points[i].normal_x) || !pcl_isfinite (recog_model->normals_->points[i].normal_y)
544  || !pcl_isfinite (recog_model->normals_->points[i].normal_z))
545  continue;
546 
547  recog_model->normals_->points[j] = recog_model->normals_->points[i];
548  recog_model->cloud_->points[j] = recog_model->cloud_->points[i];
549  j++;
550  }
551 
552  recog_model->normals_->points.resize (j);
553  recog_model->normals_->width = j;
554  recog_model->normals_->height = 1;
555 
556  recog_model->cloud_->points.resize (j);
557  recog_model->cloud_->width = j;
558  recog_model->cloud_->height = 1;
559 
560  std::vector<int> explained_indices;
561  std::vector<float> outliers_weight;
562  std::vector<float> explained_indices_distances;
563  std::vector<float> unexplained_indices_weights;
564 
565  std::vector<int> nn_indices;
566  std::vector<float> nn_distances;
567 
568  std::map<int, boost::shared_ptr<std::vector<std::pair<int, float> > > > model_explains_scene_points; //which point i from the scene is explained by a points j_k with dist d_k from the model
569  std::map<int, boost::shared_ptr<std::vector<std::pair<int, float> > > >::iterator it;
570 
571  outliers_weight.resize (recog_model->cloud_->points.size ());
572  recog_model->outlier_indices_.resize (recog_model->cloud_->points.size ());
573 
574  size_t o = 0;
575  for (size_t i = 0; i < recog_model->cloud_->points.size (); i++)
576  {
577  if (!scene_downsampled_tree_->radiusSearch (recog_model->cloud_->points[i], inliers_threshold_, nn_indices, nn_distances, std::numeric_limits<int>::max ()))
578  {
579  //outlier
580  outliers_weight[o] = regularizer_;
581  recog_model->outlier_indices_[o] = static_cast<int> (i);
582  o++;
583  } else
584  {
585  for (size_t k = 0; k < nn_distances.size (); k++)
586  {
587  std::pair<int, float> pair = std::make_pair (i, nn_distances[k]); //i is a index to a model point and then distance
588  it = model_explains_scene_points.find (nn_indices[k]);
589  if (it == model_explains_scene_points.end ())
590  {
591  boost::shared_ptr < std::vector<std::pair<int, float> > > vec (new std::vector<std::pair<int, float> > ());
592  vec->push_back (pair);
593  model_explains_scene_points[nn_indices[k]] = vec;
594  } else
595  {
596  it->second->push_back (pair);
597  }
598  }
599  }
600  }
601 
602  outliers_weight.resize (o);
603  recog_model->outlier_indices_.resize (o);
604 
605  recog_model->outliers_weight_ = (std::accumulate (outliers_weight.begin (), outliers_weight.end (), 0.f) / static_cast<float> (outliers_weight.size ()));
606  if (outliers_weight.size () == 0)
607  recog_model->outliers_weight_ = 1.f;
608 
609  pcl::IndicesPtr indices_scene (new std::vector<int>);
610  //go through the map and keep the closest model point in case that several model points explain a scene point
611 
612  int p = 0;
613 
614  for (it = model_explains_scene_points.begin (); it != model_explains_scene_points.end (); it++, p++)
615  {
616  size_t closest = 0;
617  float min_d = std::numeric_limits<float>::min ();
618  for (size_t i = 0; i < it->second->size (); i++)
619  {
620  if (it->second->at (i).second > min_d)
621  {
622  min_d = it->second->at (i).second;
623  closest = i;
624  }
625  }
626 
627  float d = it->second->at (closest).second;
628  float d_weight = -(d * d / (inliers_threshold_)) + 1;
629 
630  //it->first is index to scene point
631  //using normals to weight inliers
632  Eigen::Vector3f scene_p_normal = scene_normals_->points[it->first].getNormalVector3fMap ();
633  Eigen::Vector3f model_p_normal = recog_model->normals_->points[it->second->at (closest).first].getNormalVector3fMap ();
634  float dotp = scene_p_normal.dot (model_p_normal) * 1.f; //[-1,1] from antiparallel trough perpendicular to parallel
635 
636  if (dotp < 0.f)
637  dotp = 0.f;
638 
639  explained_indices.push_back (it->first);
640  explained_indices_distances.push_back (d_weight * dotp);
641 
642  }
643 
644  recog_model->bad_information_ = static_cast<int> (recog_model->outlier_indices_.size ());
645  recog_model->explained_ = explained_indices;
646  recog_model->explained_distances_ = explained_indices_distances;
647 
648  return true;
649 }
650 
651 template<typename ModelT, typename SceneT>
652 void pcl::GlobalHypothesesVerification<ModelT, SceneT>::computeClutterCue(boost::shared_ptr<RecognitionModel> & recog_model)
653 {
654  if (detect_clutter_)
655  {
656 
657  float rn_sqr = radius_neighborhood_GO_ * radius_neighborhood_GO_;
658  std::vector<int> nn_indices;
659  std::vector<float> nn_distances;
660 
661  std::vector < std::pair<int, int> > neighborhood_indices; //first is indices to scene point and second is indices to explained_ scene points
662  for (int i = 0; i < static_cast<int> (recog_model->explained_.size ()); i++)
663  {
664  if (scene_downsampled_tree_->radiusSearch (scene_cloud_downsampled_->points[recog_model->explained_[i]], radius_neighborhood_GO_, nn_indices,
665  nn_distances, std::numeric_limits<int>::max ()))
666  {
667  for (size_t k = 0; k < nn_distances.size (); k++)
668  {
669  if (nn_indices[k] != i)
670  neighborhood_indices.push_back (std::make_pair (nn_indices[k], i));
671  }
672  }
673  }
674 
675  //sort neighborhood indices by id
676  std::sort (neighborhood_indices.begin (), neighborhood_indices.end (),
677  boost::bind (&std::pair<int, int>::first, _1) < boost::bind (&std::pair<int, int>::first, _2));
678 
679  //erase duplicated unexplained points
680  neighborhood_indices.erase (
681  std::unique (neighborhood_indices.begin (), neighborhood_indices.end (),
682  boost::bind (&std::pair<int, int>::first, _1) == boost::bind (&std::pair<int, int>::first, _2)), neighborhood_indices.end ());
683 
684  //sort explained points
685  std::vector<int> exp_idces (recog_model->explained_);
686  std::sort (exp_idces.begin (), exp_idces.end ());
687 
688  recog_model->unexplained_in_neighborhood.resize (neighborhood_indices.size ());
689  recog_model->unexplained_in_neighborhood_weights.resize (neighborhood_indices.size ());
690 
691  size_t p = 0;
692  size_t j = 0;
693  for (size_t i = 0; i < neighborhood_indices.size (); i++)
694  {
695  if ((j < exp_idces.size ()) && (neighborhood_indices[i].first == exp_idces[j]))
696  {
697  //this index is explained by the hypothesis so ignore it, advance j
698  j++;
699  } else
700  {
701  //indices_in_nb[i] < exp_idces[j]
702  //recog_model->unexplained_in_neighborhood.push_back(neighborhood_indices[i]);
703  recog_model->unexplained_in_neighborhood[p] = neighborhood_indices[i].first;
704 
705  if (clusters_cloud_->points[recog_model->explained_[neighborhood_indices[i].second]].intensity != 0.f
706  && (clusters_cloud_->points[recog_model->explained_[neighborhood_indices[i].second]].intensity
707  == clusters_cloud_->points[neighborhood_indices[i].first].intensity))
708  {
709 
710  recog_model->unexplained_in_neighborhood_weights[p] = clutter_regularizer_;
711 
712  } else
713  {
714  //neighborhood_indices[i].first gives the index to the scene point and second to the explained scene point by the model causing this...
715  //calculate weight of this clutter point based on the distance of the scene point and the model point causing it
716  float d = static_cast<float> (pow (
717  (scene_cloud_downsampled_->points[recog_model->explained_[neighborhood_indices[i].second]].getVector3fMap ()
718  - scene_cloud_downsampled_->points[neighborhood_indices[i].first].getVector3fMap ()).norm (), 2));
719  float d_weight = -(d / rn_sqr) + 1; //points that are close have a strong weight*/
720 
721  //using normals to weight clutter points
722  Eigen::Vector3f scene_p_normal = scene_normals_->points[neighborhood_indices[i].first].getNormalVector3fMap ();
723  Eigen::Vector3f model_p_normal = scene_normals_->points[recog_model->explained_[neighborhood_indices[i].second]].getNormalVector3fMap ();
724  float dotp = scene_p_normal.dot (model_p_normal); //[-1,1] from antiparallel trough perpendicular to parallel
725 
726  if (dotp < 0)
727  dotp = 0.f;
728 
729  recog_model->unexplained_in_neighborhood_weights[p] = d_weight * dotp;
730  }
731  p++;
732  }
733  }
734 
735  recog_model->unexplained_in_neighborhood_weights.resize (p);
736  recog_model->unexplained_in_neighborhood.resize (p);
737  }
738 }
739 
740 #define PCL_INSTANTIATE_GoHV(T1,T2) template class PCL_EXPORTS pcl::GlobalHypothesesVerification<T1,T2>;
boost::shared_ptr< KdTree< PointT > > Ptr
Definition: kdtree.h:79
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point...
Definition: normal_3d.h:197
boost::shared_ptr< pcl::search::Search< PointT > > Ptr
Definition: search.h:80
void getMinMax3D(const pcl::PointCloud< PointT > &cloud, PointT &min_pt, PointT &max_pt)
Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud...
Definition: common.hpp:212
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:407
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: normal_3d.h:286
boost::shared_ptr< std::vector< int > > IndicesPtr
Definition: pcl_base.h:60
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:66
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data...
Definition: voxel_grid.h:178
virtual int radiusSearch(const PointT &point, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const =0
Search for all the nearest neighbors of the query point in a given radius.
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
Definition: voxel_grid.h:222
virtual PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
Definition: search.h:124
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
Definition: filter.h:131
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
Class to measure the time spent in a scope.
Definition: time.h:114
std::vector< int > indices
Definition: PointIndices.h:19
A hypothesis verification method proposed in "A Global Hypotheses Verification Method for 3D Object R...
Definition: hv_go.h:27
PointCloud represents the base class in PCL for storing collections of 3D points. ...
::pcl::PCLHeader header
Definition: PointIndices.h:17