37 #ifndef PCL_RECOGNITION_IMPL_HV_GO_HPP_
38 #define PCL_RECOGNITION_IMPL_HV_GO_HPP_
40 #include <pcl/recognition/hv/hv_go.h>
41 #include <pcl/common/common.h>
42 #include <pcl/common/time.h>
43 #include <pcl/point_types.h>
48 template<
typename Po
intT,
typename NormalT>
51 unsigned int min_pts_per_cluster,
unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())
56 PCL_ERROR(
"[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset\n");
59 if (cloud.
size () != normals.
size ())
61 PCL_ERROR(
"[pcl::extractEuclideanClusters] Number of points in the input point cloud different than normals!\n");
68 std::vector<bool> processed (cloud.
size (),
false);
71 std::vector<float> nn_distances;
73 int size =
static_cast<int> (cloud.
size ());
74 for (
int i = 0; i < size; ++i)
79 std::vector<unsigned int> seed_queue;
81 seed_queue.push_back (i);
85 while (sq_idx < static_cast<int> (seed_queue.size ()))
88 if (normals[seed_queue[sq_idx]].curvature > curvature_threshold)
95 if (!tree->
radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
101 for (std::size_t j = nn_start_idx; j < nn_indices.size (); ++j)
103 if (processed[nn_indices[j]])
106 if (normals[nn_indices[j]].curvature > curvature_threshold)
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];
118 if (std::abs (std::acos (dot_p)) < eps_angle)
120 processed[nn_indices[j]] =
true;
129 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
132 r.
indices.resize (seed_queue.size ());
133 for (std::size_t j = 0; j < seed_queue.size (); ++j)
140 clusters.push_back (r);
145 template<
typename ModelT,
typename SceneT>
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);
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);
169 int duplicity = getDuplicity ();
170 float good_info = getExplainedValue ();
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;
176 setPreviousBadInfo (bad_info);
178 int n_active_hyp = 0;
179 for(
const bool i : active) {
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);
189 template<
typename ModelT,
typename SceneT>
193 recognition_models_.clear ();
194 unexplained_by_RM_neighboorhods.clear ();
195 explained_by_RM_distance_weighted.clear ();
196 explained_by_RM_.clear ();
199 complete_cloud_occupancy_by_RM_.clear ();
202 mask_.resize (complete_models_.size ());
203 for (std::size_t i = 0; i < complete_models_.size (); i++)
206 indices_.resize (complete_models_.size ());
208 NormalEstimator_ n3d;
212 normals_tree->setInputCloud (scene_cloud_downsampled_);
214 n3d.setRadiusSearch (radius_normals_);
215 n3d.setSearchMethod (normals_tree);
216 n3d.setInputCloud (scene_cloud_downsampled_);
217 n3d.compute (*scene_normals_);
221 for (std::size_t i = 0; i < scene_normals_->size (); ++i)
223 if (!std::isfinite ((*scene_normals_)[i].normal_x) || !std::isfinite ((*scene_normals_)[i].normal_y)
224 || !std::isfinite ((*scene_normals_)[i].normal_z))
227 (*scene_normals_)[j] = (*scene_normals_)[i];
228 (*scene_cloud_downsampled_)[j] = (*scene_cloud_downsampled_)[i];
233 scene_normals_->points.resize (j);
234 scene_normals_->width = j;
235 scene_normals_->height = 1;
237 scene_cloud_downsampled_->points.resize (j);
238 scene_cloud_downsampled_->width = j;
239 scene_cloud_downsampled_->height = 1;
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);
250 scene_downsampled_tree_->setInputCloud (scene_cloud_downsampled_);
252 std::vector<pcl::PointIndices> clusters;
253 double eps_angle_threshold = 0.2;
255 float curvature_threshold = 0.045f;
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);
261 clusters_cloud_->points.resize (scene_cloud_downsampled_->size ());
262 clusters_cloud_->width = scene_cloud_downsampled_->width;
263 clusters_cloud_->height = 1;
265 for (std::size_t i = 0; i < scene_cloud_downsampled_->size (); i++)
268 p.getVector3fMap () = (*scene_cloud_downsampled_)[i].getVector3fMap ();
270 (*clusters_cloud_)[i] = p;
273 float intens_incr = 100.f /
static_cast<float> (clusters.size ());
274 float intens = intens_incr;
275 for (
const auto &cluster : clusters)
277 for (
const auto &vertex : cluster.indices)
279 (*clusters_cloud_)[vertex].
intensity = intens;
282 intens += intens_incr;
289 recognition_models_.resize (complete_models_.size ());
291 for (
int i = 0; i < static_cast<int> (complete_models_.size ()); i++)
294 recognition_models_[valid].reset (
new RecognitionModel ());
295 if(addModel (visible_models_[i], complete_models_[i], recognition_models_[valid])) {
301 recognition_models_.resize(valid);
302 indices_.resize(valid);
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;
310 for (std::size_t i = 0; i < recognition_models_.size (); i++)
312 ModelT min_pt, max_pt;
314 if (min_pt.x < min_pt_all.x)
315 min_pt_all.x = min_pt.x;
317 if (min_pt.y < min_pt_all.y)
318 min_pt_all.y = min_pt.y;
320 if (min_pt.z < min_pt_all.z)
321 min_pt_all.z = min_pt.z;
323 if (max_pt.x > max_pt_all.x)
324 max_pt_all.x = max_pt.x;
326 if (max_pt.y > max_pt_all.y)
327 max_pt_all.y = max_pt.y;
329 if (max_pt.z > max_pt_all.z)
330 max_pt_all.z = max_pt.z;
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;
338 complete_cloud_occupancy_by_RM_.resize (size_x * size_y * size_z, 0);
340 for (std::size_t i = 0; i < recognition_models_.size (); i++)
343 std::map<int, bool> banned;
344 std::map<int, bool>::iterator banned_it;
346 for (
const auto& point: *complete_models_[indices_[i]])
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_));
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 ())
356 complete_cloud_occupancy_by_RM_[idx]++;
357 recognition_models_[i]->complete_cloud_occupancy_indices_.push_back (idx);
365 #pragma omp parallel for \
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]);
376 for (std::size_t i = 0; i < recognition_models_.size (); i++)
377 cc_[0].push_back (static_cast<int> (i));
381 template<
typename ModelT,
typename SceneT>
386 std::vector<RecognitionModelPtr> recognition_models_copy;
387 recognition_models_copy = recognition_models_;
389 recognition_models_.clear ();
391 for (
const int &cc_index : cc_indices)
393 recognition_models_.push_back (recognition_models_copy[cc_index]);
396 for (std::size_t j = 0; j < recognition_models_.size (); j++)
398 RecognitionModelPtr recog_model = recognition_models_[j];
399 for (std::size_t i = 0; i < recog_model->explained_.size (); i++)
401 explained_by_RM_[recog_model->explained_[i]]++;
402 explained_by_RM_distance_weighted[recog_model->explained_[i]] += recog_model->explained_distances_[i];
407 for (std::size_t i = 0; i < recog_model->unexplained_in_neighborhood.size (); i++)
409 unexplained_by_RM_neighboorhods[recog_model->unexplained_in_neighborhood[i]] += recog_model->unexplained_in_neighborhood_weights[i];
414 int occupied_multiple = 0;
415 for(
const auto& i : complete_cloud_occupancy_by_RM_) {
417 occupied_multiple+=i;
421 setPreviousDuplicityCM(occupied_multiple);
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_);
430 for (std::size_t i = 0; i < initial_solution.size (); i++)
432 if (initial_solution[i])
433 bad_information_ += recognition_models_[i]->outliers_weight_ *
static_cast<float> (recognition_models_[i]->bad_information_);
436 setPreviousExplainedValue (good_information_);
437 setPreviousDuplicity (duplicity);
438 setPreviousBadInfo (bad_information_);
439 setPreviousUnexplainedValue (unexplained_in_neighboorhod);
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);
448 model.setSolution (initial_solution);
449 model.setOptimizer (
this);
450 SAModel best (model);
452 move_manager neigh (static_cast<int> (cc_indices.size ()));
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);
465 best_seen_ =
static_cast<const SAModel&
> (best_recorder.best_seen ());
466 for (std::size_t i = 0; i < best_seen_.solution_.size (); i++)
468 initial_solution[i] = best_seen_.solution_[i];
471 recognition_models_ = recognition_models_copy;
476 template<
typename ModelT,
typename SceneT>
482 for (
int c = 0; c < n_cc_; c++)
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++)
490 mask_[indices_[cc_[c][i]]] = (subsolution[i]);
495 template<
typename ModelT,
typename SceneT>
503 float size_model = resolution_;
506 voxel_grid.
setLeafSize (size_model, size_model, size_model);
507 voxel_grid.
filter (*(recog_model->cloud_));
511 voxel_grid2.
setLeafSize (size_model, size_model, size_model);
512 voxel_grid2.
filter (*(recog_model->complete_cloud_));
517 for (
auto& point: *(recog_model->cloud_))
519 if (!isXYZFinite (point))
522 (*recog_model->cloud_)[j] = point;
526 recog_model->cloud_->points.resize (j);
527 recog_model->cloud_->width = j;
528 recog_model->cloud_->height = 1;
531 if (recog_model->cloud_->points.empty ())
533 PCL_WARN(
"The model cloud has no points..\n");
545 n3d.
compute (*(recog_model->normals_));
549 for (std::size_t i = 0; i < recog_model->normals_->size (); ++i)
554 (*recog_model->normals_)[j] = (*recog_model->normals_)[i];
555 (*recog_model->cloud_)[j] = (*recog_model->cloud_)[i];
559 recog_model->normals_->points.resize (j);
560 recog_model->normals_->width = j;
561 recog_model->normals_->height = 1;
563 recog_model->cloud_->points.resize (j);
564 recog_model->cloud_->width = j;
565 recog_model->cloud_->height = 1;
567 std::vector<int> explained_indices;
568 std::vector<float> outliers_weight;
569 std::vector<float> explained_indices_distances;
572 std::vector<float> nn_distances;
574 std::map<int, std::shared_ptr<std::vector<std::pair<int, float>>>> model_explains_scene_points;
576 outliers_weight.resize (recog_model->cloud_->size ());
577 recog_model->outlier_indices_.resize (recog_model->cloud_->size ());
580 for (std::size_t i = 0; i < recog_model->cloud_->size (); i++)
582 if (!scene_downsampled_tree_->radiusSearch ((*recog_model->cloud_)[i], inliers_threshold_, nn_indices, nn_distances, std::numeric_limits<int>::max ()))
585 outliers_weight[o] = regularizer_;
586 recog_model->outlier_indices_[o] =
static_cast<int> (i);
590 for (std::size_t k = 0; k < nn_distances.size (); k++)
592 std::pair<int, float> pair = std::make_pair (i, nn_distances[k]);
593 auto it = model_explains_scene_points.find (nn_indices[k]);
594 if (it == model_explains_scene_points.end ())
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;
601 it->second->push_back (pair);
607 outliers_weight.resize (o);
608 recog_model->outlier_indices_.resize (o);
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;
619 for (
auto it = model_explains_scene_points.cbegin (); it != model_explains_scene_points.cend (); it++, p++)
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++)
625 if (it->second->at (i).second > min_d)
627 min_d = it->second->at (i).second;
632 float d = it->second->at (closest).second;
633 float d_weight = -(d * d / (inliers_threshold_)) + 1;
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;
645 explained_indices.push_back (it->first);
646 explained_indices_distances.push_back (d_weight * dotp);
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;
657 template<
typename ModelT,
typename SceneT>
663 float rn_sqr = radius_neighborhood_GO_ * radius_neighborhood_GO_;
665 std::vector<float> nn_distances;
667 std::vector < std::pair<int, int> > neighborhood_indices;
668 for (
pcl::index_t i = 0; i < static_cast<pcl::index_t> (recog_model->explained_.size ()); i++)
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 ()))
673 for (std::size_t k = 0; k < nn_distances.size (); k++)
675 if (nn_indices[k] != i)
676 neighborhood_indices.emplace_back (nn_indices[k], i);
682 std::sort (neighborhood_indices.begin (), neighborhood_indices.end (),
683 [] (
const auto& p1,
const auto& p2) {
return p1.first < p2.first; });
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 ());
691 std::vector<int> exp_idces (recog_model->explained_);
692 std::sort (exp_idces.begin (), exp_idces.end ());
694 recog_model->unexplained_in_neighborhood.resize (neighborhood_indices.size ());
695 recog_model->unexplained_in_neighborhood_weights.resize (neighborhood_indices.size ());
699 for (
const auto &neighborhood_index : neighborhood_indices)
701 if ((j < exp_idces.size ()) && (neighborhood_index.first == exp_idces[j]))
709 recog_model->unexplained_in_neighborhood[p] = neighborhood_index.first;
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))
716 recog_model->unexplained_in_neighborhood_weights[p] = clutter_regularizer_;
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;
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);
735 recog_model->unexplained_in_neighborhood_weights[p] = d_weight * dotp;
741 recog_model->unexplained_in_neighborhood_weights.resize (p);
742 recog_model->unexplained_in_neighborhood.resize (p);
746 #define PCL_INSTANTIATE_GoHV(T1,T2) template class PCL_EXPORTS pcl::GlobalHypothesesVerification<T1,T2>;
shared_ptr< KdTree< PointT, Tree > > Ptr
virtual bool getSortedResults()
Gets whether the results should be sorted (ascending in the distance) or not Otherwise the results ma...
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.
shared_ptr< Indices > IndicesPtr
Class to measure the time spent in a scope.
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data...
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point...
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...
virtual PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
IndicesAllocator<> Indices
Type used for indices in PCL.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
pcl::PCLHeader header
The point cloud header.
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
constexpr bool isNormalFinite(const PointT &) noexcept
shared_ptr< const PointCloud< PointT > > ConstPtr
shared_ptr< pcl::search::Search< PointT > > Ptr
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in using th...
A hypothesis verification method proposed in "A Global Hypotheses Verification Method for 3D Object R...
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.