37 #ifndef PCL_RECOGNITION_IMPL_HV_GO_HPP_
38 #define PCL_RECOGNITION_IMPL_HV_GO_HPP_
40 #include <pcl/recognition/hv/hv_go.h>
47 template<
typename Po
intT,
typename NormalT>
50 unsigned int min_pts_per_cluster,
unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())
55 PCL_ERROR(
"[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset\n");
58 if (cloud.
size () != normals.
size ())
60 PCL_ERROR(
"[pcl::extractEuclideanClusters] Number of points in the input point cloud different than normals!\n");
65 std::vector<bool> processed (cloud.
size (),
false);
67 std::vector<int> nn_indices;
68 std::vector<float> nn_distances;
70 int size =
static_cast<int> (cloud.
size ());
71 for (
int i = 0; i < size; ++i)
76 std::vector<unsigned int> seed_queue;
78 seed_queue.push_back (i);
82 while (sq_idx <
static_cast<int> (seed_queue.size ()))
85 if (normals[seed_queue[sq_idx]].curvature > curvature_threshold)
92 if (!tree->
radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
98 for (std::size_t j = 1; j < nn_indices.size (); ++j)
100 if (processed[nn_indices[j]])
103 if (normals[nn_indices[j]].curvature > curvature_threshold)
111 double dot_p = normals[seed_queue[sq_idx]].normal[0] * normals[nn_indices[j]].normal[0]
112 + normals[seed_queue[sq_idx]].normal[1] * normals[nn_indices[j]].normal[1]
113 + normals[seed_queue[sq_idx]].normal[2] * normals[nn_indices[j]].normal[2];
115 if (std::abs (std::acos (dot_p)) < eps_angle)
117 processed[nn_indices[j]] =
true;
126 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
129 r.
indices.resize (seed_queue.size ());
130 for (std::size_t j = 0; j < seed_queue.size (); ++j)
137 clusters.push_back (r);
142 template<
typename ModelT,
typename SceneT>
150 updateExplainedVector (recognition_models_[changed]->explained_, recognition_models_[changed]->explained_distances_, explained_by_RM_,
151 explained_by_RM_distance_weighted, 1.f);
152 updateUnexplainedVector (recognition_models_[changed]->unexplained_in_neighborhood, recognition_models_[changed]->unexplained_in_neighborhood_weights,
153 unexplained_by_RM_neighboorhods, recognition_models_[changed]->explained_, explained_by_RM_, 1.f);
154 updateCMDuplicity(recognition_models_[changed]->complete_cloud_occupancy_indices_, complete_cloud_occupancy_by_RM_, 1.f);
158 updateExplainedVector (recognition_models_[changed]->explained_, recognition_models_[changed]->explained_distances_, explained_by_RM_,
159 explained_by_RM_distance_weighted, -1.f);
160 updateUnexplainedVector (recognition_models_[changed]->unexplained_in_neighborhood, recognition_models_[changed]->unexplained_in_neighborhood_weights,
161 unexplained_by_RM_neighboorhods, recognition_models_[changed]->explained_, explained_by_RM_, -1.f);
162 updateCMDuplicity(recognition_models_[changed]->complete_cloud_occupancy_indices_, complete_cloud_occupancy_by_RM_, -1.f);
166 int duplicity = getDuplicity ();
167 float good_info = getExplainedValue ();
169 float unexplained_info = getPreviousUnexplainedValue ();
170 float bad_info =
static_cast<float> (getPreviousBadInfo ())
171 + (recognition_models_[changed]->outliers_weight_ *
static_cast<float> (recognition_models_[changed]->bad_information_)) * sign;
173 setPreviousBadInfo (bad_info);
175 int n_active_hyp = 0;
176 for(
const bool &i : active) {
181 float duplicity_cm =
static_cast<float> (getDuplicityCM ()) * w_occupied_multiple_cm_;
182 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);
186 template<
typename ModelT,
typename SceneT>
190 recognition_models_.clear ();
191 unexplained_by_RM_neighboorhods.clear ();
192 explained_by_RM_distance_weighted.clear ();
193 explained_by_RM_.clear ();
196 complete_cloud_occupancy_by_RM_.clear ();
199 mask_.resize (complete_models_.size ());
200 for (std::size_t i = 0; i < complete_models_.size (); i++)
203 indices_.resize (complete_models_.size ());
205 NormalEstimator_ n3d;
209 normals_tree->setInputCloud (scene_cloud_downsampled_);
211 n3d.setRadiusSearch (radius_normals_);
212 n3d.setSearchMethod (normals_tree);
213 n3d.setInputCloud (scene_cloud_downsampled_);
214 n3d.compute (*scene_normals_);
218 for (std::size_t i = 0; i < scene_normals_->size (); ++i)
220 if (!std::isfinite ((*scene_normals_)[i].normal_x) || !std::isfinite ((*scene_normals_)[i].normal_y)
221 || !std::isfinite ((*scene_normals_)[i].normal_z))
224 (*scene_normals_)[j] = (*scene_normals_)[i];
225 (*scene_cloud_downsampled_)[j] = (*scene_cloud_downsampled_)[i];
230 scene_normals_->points.resize (j);
231 scene_normals_->width = j;
232 scene_normals_->height = 1;
234 scene_cloud_downsampled_->points.resize (j);
235 scene_cloud_downsampled_->width = j;
236 scene_cloud_downsampled_->height = 1;
238 explained_by_RM_.resize (scene_cloud_downsampled_->size (), 0);
239 explained_by_RM_distance_weighted.resize (scene_cloud_downsampled_->size (), 0.f);
240 unexplained_by_RM_neighboorhods.resize (scene_cloud_downsampled_->size (), 0.f);
247 scene_downsampled_tree_->setInputCloud (scene_cloud_downsampled_);
249 std::vector<pcl::PointIndices> clusters;
250 double eps_angle_threshold = 0.2;
252 float curvature_threshold = 0.045f;
254 extractEuclideanClustersSmooth<SceneT, pcl::Normal> (*scene_cloud_downsampled_, *scene_normals_, inliers_threshold_ * 2.f, scene_downsampled_tree_,
255 clusters, eps_angle_threshold, curvature_threshold, min_points);
258 clusters_cloud_->points.resize (scene_cloud_downsampled_->size ());
259 clusters_cloud_->width = scene_cloud_downsampled_->width;
260 clusters_cloud_->height = 1;
262 for (std::size_t i = 0; i < scene_cloud_downsampled_->size (); i++)
265 p.getVector3fMap () = (*scene_cloud_downsampled_)[i].getVector3fMap ();
267 (*clusters_cloud_)[i] = p;
270 float intens_incr = 100.f /
static_cast<float> (clusters.size ());
271 float intens = intens_incr;
272 for (
const auto &cluster : clusters)
274 for (
const auto &vertex : cluster.indices)
276 (*clusters_cloud_)[vertex].
intensity = intens;
279 intens += intens_incr;
286 recognition_models_.resize (complete_models_.size ());
288 for (
int i = 0; i < static_cast<int> (complete_models_.size ()); i++)
291 recognition_models_[valid].reset (
new RecognitionModel ());
292 if(addModel (visible_models_[i], complete_models_[i], recognition_models_[valid])) {
298 recognition_models_.resize(valid);
299 indices_.resize(valid);
303 ModelT min_pt_all, max_pt_all;
304 min_pt_all.x = min_pt_all.y = min_pt_all.z = std::numeric_limits<float>::max ();
305 max_pt_all.x = max_pt_all.y = max_pt_all.z = (std::numeric_limits<float>::max () - 0.001f) * -1;
307 for (std::size_t i = 0; i < recognition_models_.size (); i++)
309 ModelT min_pt, max_pt;
311 if (min_pt.x < min_pt_all.x)
312 min_pt_all.x = min_pt.x;
314 if (min_pt.y < min_pt_all.y)
315 min_pt_all.y = min_pt.y;
317 if (min_pt.z < min_pt_all.z)
318 min_pt_all.z = min_pt.z;
320 if (max_pt.x > max_pt_all.x)
321 max_pt_all.x = max_pt.x;
323 if (max_pt.y > max_pt_all.y)
324 max_pt_all.y = max_pt.y;
326 if (max_pt.z > max_pt_all.z)
327 max_pt_all.z = max_pt.z;
330 int size_x, size_y, size_z;
331 size_x =
static_cast<int> (std::ceil (std::abs (max_pt_all.x - min_pt_all.x) / res_occupancy_grid_)) + 1;
332 size_y =
static_cast<int> (std::ceil (std::abs (max_pt_all.y - min_pt_all.y) / res_occupancy_grid_)) + 1;
333 size_z =
static_cast<int> (std::ceil (std::abs (max_pt_all.z - min_pt_all.z) / res_occupancy_grid_)) + 1;
335 complete_cloud_occupancy_by_RM_.resize (size_x * size_y * size_z, 0);
337 for (std::size_t i = 0; i < recognition_models_.size (); i++)
340 std::map<int, bool> banned;
341 std::map<int, bool>::iterator banned_it;
343 for (
const auto& point: *complete_models_[indices_[i]])
345 const int pos_x =
static_cast<int> (std::floor ((point.x - min_pt_all.x) / res_occupancy_grid_));
346 const int pos_y =
static_cast<int> (std::floor ((point.y - min_pt_all.y) / res_occupancy_grid_));
347 const int pos_z =
static_cast<int> (std::floor ((point.z - min_pt_all.z) / res_occupancy_grid_));
349 const int idx = pos_z * size_x * size_y + pos_y * size_x + pos_x;
350 banned_it = banned.find (idx);
351 if (banned_it == banned.end ())
353 complete_cloud_occupancy_by_RM_[idx]++;
354 recognition_models_[i]->complete_cloud_occupancy_indices_.push_back (idx);
362 #pragma omp parallel for \
364 schedule(dynamic, 4) \
365 num_threads(omp_get_num_procs())
366 for (
int j = 0; j < static_cast<int> (recognition_models_.size ()); j++)
367 computeClutterCue (recognition_models_[j]);
373 for (std::size_t i = 0; i < recognition_models_.size (); i++)
374 cc_[0].push_back (
static_cast<int> (i));
378 template<
typename ModelT,
typename SceneT>
383 std::vector<RecognitionModelPtr> recognition_models_copy;
384 recognition_models_copy = recognition_models_;
386 recognition_models_.clear ();
388 for (
const int &cc_index : cc_indices)
390 recognition_models_.push_back (recognition_models_copy[cc_index]);
393 for (std::size_t j = 0; j < recognition_models_.size (); j++)
395 RecognitionModelPtr recog_model = recognition_models_[j];
396 for (std::size_t i = 0; i < recog_model->explained_.size (); i++)
398 explained_by_RM_[recog_model->explained_[i]]++;
399 explained_by_RM_distance_weighted[recog_model->explained_[i]] += recog_model->explained_distances_[i];
404 for (std::size_t i = 0; i < recog_model->unexplained_in_neighborhood.size (); i++)
406 unexplained_by_RM_neighboorhods[recog_model->unexplained_in_neighborhood[i]] += recog_model->unexplained_in_neighborhood_weights[i];
411 int occupied_multiple = 0;
412 for(std::size_t i=0; i < complete_cloud_occupancy_by_RM_.size(); i++) {
413 if(complete_cloud_occupancy_by_RM_[i] > 1) {
414 occupied_multiple+=complete_cloud_occupancy_by_RM_[i];
418 setPreviousDuplicityCM(occupied_multiple);
423 float good_information_ = getTotalExplainedInformation (explained_by_RM_, explained_by_RM_distance_weighted, &duplicity);
424 float bad_information_ = 0;
425 float unexplained_in_neighboorhod = getUnexplainedInformationInNeighborhood (unexplained_by_RM_neighboorhods, explained_by_RM_);
427 for (std::size_t i = 0; i < initial_solution.size (); i++)
429 if (initial_solution[i])
430 bad_information_ += recognition_models_[i]->outliers_weight_ *
static_cast<float> (recognition_models_[i]->bad_information_);
433 setPreviousExplainedValue (good_information_);
434 setPreviousDuplicity (duplicity);
435 setPreviousBadInfo (bad_information_);
436 setPreviousUnexplainedValue (unexplained_in_neighboorhod);
439 model.cost_ =
static_cast<mets::gol_type
> ((good_information_ - bad_information_
440 -
static_cast<float> (duplicity)
441 -
static_cast<float> (occupied_multiple) * w_occupied_multiple_cm_
442 -
static_cast<float> (recognition_models_.size ())
443 - unexplained_in_neighboorhod) * -1.f);
445 model.setSolution (initial_solution);
446 model.setOptimizer (
this);
447 SAModel best (model);
449 move_manager neigh (
static_cast<int> (cc_indices.size ()));
451 mets::best_ever_solution best_recorder (best);
452 mets::noimprove_termination_criteria noimprove (max_iterations_);
453 mets::linear_cooling linear_cooling;
454 mets::simulated_annealing<move_manager> sa (model, best_recorder, neigh, noimprove, linear_cooling, initial_temp_, 1e-7, 2);
455 sa.setApplyAndEvaluate(
true);
462 best_seen_ =
static_cast<const SAModel&
> (best_recorder.best_seen ());
463 for (std::size_t i = 0; i < best_seen_.solution_.size (); i++)
465 initial_solution[i] = best_seen_.solution_[i];
468 recognition_models_ = recognition_models_copy;
473 template<
typename ModelT,
typename SceneT>
479 for (
int c = 0; c < n_cc_; c++)
483 std::vector<bool> subsolution (cc_[c].size (),
true);
484 SAOptimize (cc_[c], subsolution);
485 for (std::size_t i = 0; i < subsolution.size (); i++)
487 mask_[indices_[cc_[c][i]]] = (subsolution[i]);
492 template<
typename ModelT,
typename SceneT>
500 float size_model = resolution_;
503 voxel_grid.
setLeafSize (size_model, size_model, size_model);
504 voxel_grid.
filter (*(recog_model->cloud_));
508 voxel_grid2.
setLeafSize (size_model, size_model, size_model);
509 voxel_grid2.
filter (*(recog_model->complete_cloud_));
514 for (
auto& point: *(recog_model->cloud_))
516 if (!isXYZFinite (point))
519 (*recog_model->cloud_)[j] = point;
523 recog_model->cloud_->points.resize (j);
524 recog_model->cloud_->width = j;
525 recog_model->cloud_->height = 1;
528 if (recog_model->cloud_->points.empty ())
530 PCL_WARN(
"The model cloud has no points..\n");
542 n3d.
compute (*(recog_model->normals_));
546 for (std::size_t i = 0; i < recog_model->normals_->size (); ++i)
551 (*recog_model->normals_)[j] = (*recog_model->normals_)[i];
552 (*recog_model->cloud_)[j] = (*recog_model->cloud_)[i];
556 recog_model->normals_->points.resize (j);
557 recog_model->normals_->width = j;
558 recog_model->normals_->height = 1;
560 recog_model->cloud_->points.resize (j);
561 recog_model->cloud_->width = j;
562 recog_model->cloud_->height = 1;
564 std::vector<int> explained_indices;
565 std::vector<float> outliers_weight;
566 std::vector<float> explained_indices_distances;
568 std::vector<int> nn_indices;
569 std::vector<float> nn_distances;
571 std::map<int, std::shared_ptr<std::vector<std::pair<int, float>>>> model_explains_scene_points;
573 outliers_weight.resize (recog_model->cloud_->size ());
574 recog_model->outlier_indices_.resize (recog_model->cloud_->size ());
577 for (std::size_t i = 0; i < recog_model->cloud_->size (); i++)
579 if (!scene_downsampled_tree_->radiusSearch ((*recog_model->cloud_)[i], inliers_threshold_, nn_indices, nn_distances, std::numeric_limits<int>::max ()))
582 outliers_weight[o] = regularizer_;
583 recog_model->outlier_indices_[o] =
static_cast<int> (i);
587 for (std::size_t k = 0; k < nn_distances.size (); k++)
589 std::pair<int, float> pair = std::make_pair (i, nn_distances[k]);
590 auto it = model_explains_scene_points.find (nn_indices[k]);
591 if (it == model_explains_scene_points.end ())
593 std::shared_ptr<std::vector<std::pair<int, float>>> vec (
new std::vector<std::pair<int, float>> ());
594 vec->push_back (pair);
595 model_explains_scene_points[nn_indices[k]] = vec;
598 it->second->push_back (pair);
604 outliers_weight.resize (o);
605 recog_model->outlier_indices_.resize (o);
607 recog_model->outliers_weight_ = (std::accumulate (outliers_weight.begin (), outliers_weight.end (), 0.f) /
static_cast<float> (outliers_weight.size ()));
608 if (outliers_weight.empty ())
609 recog_model->outliers_weight_ = 1.f;
616 for (
auto it = model_explains_scene_points.cbegin (); it != model_explains_scene_points.cend (); it++, p++)
618 std::size_t closest = 0;
619 float min_d = std::numeric_limits<float>::min ();
620 for (std::size_t i = 0; i < it->second->size (); i++)
622 if (it->second->at (i).second > min_d)
624 min_d = it->second->at (i).second;
629 float d = it->second->at (closest).second;
630 float d_weight = -(d * d / (inliers_threshold_)) + 1;
634 Eigen::Vector3f scene_p_normal = (*scene_normals_)[it->first].getNormalVector3fMap ();
635 Eigen::Vector3f model_p_normal =
636 (*recog_model->normals_)[it->second->at(closest).first].getNormalVector3fMap();
637 float dotp = scene_p_normal.dot (model_p_normal) * 1.f;
642 explained_indices.push_back (it->first);
643 explained_indices_distances.push_back (d_weight * dotp);
647 recog_model->bad_information_ =
static_cast<int> (recog_model->outlier_indices_.size ());
648 recog_model->explained_ = explained_indices;
649 recog_model->explained_distances_ = explained_indices_distances;
654 template<
typename ModelT,
typename SceneT>
660 float rn_sqr = radius_neighborhood_GO_ * radius_neighborhood_GO_;
661 std::vector<int> nn_indices;
662 std::vector<float> nn_distances;
664 std::vector < std::pair<int, int> > neighborhood_indices;
665 for (
int i = 0; i < static_cast<int> (recog_model->explained_.size ()); i++)
667 if (scene_downsampled_tree_->radiusSearch ((*scene_cloud_downsampled_)[recog_model->explained_[i]], radius_neighborhood_GO_, nn_indices,
668 nn_distances, std::numeric_limits<int>::max ()))
670 for (std::size_t k = 0; k < nn_distances.size (); k++)
672 if (nn_indices[k] != i)
673 neighborhood_indices.emplace_back (nn_indices[k], i);
679 std::sort (neighborhood_indices.begin (), neighborhood_indices.end (),
680 [] (
const auto& p1,
const auto& p2) { return p1.first < p2.first; });
683 neighborhood_indices.erase (
684 std::unique (neighborhood_indices.begin (), neighborhood_indices.end (),
685 [] (
const auto& p1,
const auto& p2) { return p1.first == p2.first; }), neighborhood_indices.end ());
688 std::vector<int> exp_idces (recog_model->explained_);
689 std::sort (exp_idces.begin (), exp_idces.end ());
691 recog_model->unexplained_in_neighborhood.resize (neighborhood_indices.size ());
692 recog_model->unexplained_in_neighborhood_weights.resize (neighborhood_indices.size ());
696 for (
const auto &neighborhood_index : neighborhood_indices)
698 if ((j < exp_idces.size ()) && (neighborhood_index.first == exp_idces[j]))
706 recog_model->unexplained_in_neighborhood[p] = neighborhood_index.first;
708 if ((*clusters_cloud_)[recog_model->explained_[neighborhood_index.second]].intensity != 0.f
709 && ((*clusters_cloud_)[recog_model->explained_[neighborhood_index.second]].intensity
710 == (*clusters_cloud_)[neighborhood_index.first].intensity))
713 recog_model->unexplained_in_neighborhood_weights[p] = clutter_regularizer_;
719 float d =
static_cast<float> (pow (
720 ((*scene_cloud_downsampled_)[recog_model->explained_[neighborhood_index.second]].getVector3fMap ()
721 - (*scene_cloud_downsampled_)[neighborhood_index.first].getVector3fMap ()).norm (), 2));
722 float d_weight = -(d / rn_sqr) + 1;
725 Eigen::Vector3f scene_p_normal = (*scene_normals_)[neighborhood_index.first].getNormalVector3fMap ();
726 Eigen::Vector3f model_p_normal = (*scene_normals_)[recog_model->explained_[neighborhood_index.second]].getNormalVector3fMap ();
727 float dotp = scene_p_normal.dot (model_p_normal);
732 recog_model->unexplained_in_neighborhood_weights[p] = d_weight * dotp;
738 recog_model->unexplained_in_neighborhood_weights.resize (p);
739 recog_model->unexplained_in_neighborhood.resize (p);
743 #define PCL_INSTANTIATE_GoHV(T1,T2) template class PCL_EXPORTS pcl::GlobalHypothesesVerification<T1,T2>;