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