37 #ifndef PCL_RECOGNITION_IMPL_HV_GO_HPP_ 38 #define PCL_RECOGNITION_IMPL_HV_GO_HPP_ 40 #include <pcl/recognition/hv/hv_go.h> 42 #include <pcl/common/time.h> 43 #include <pcl/point_types.h> 45 template<
typename Po
intT,
typename NormalT>
48 unsigned int min_pts_per_cluster,
unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())
53 PCL_ERROR(
"[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset\n");
58 PCL_ERROR(
"[pcl::extractEuclideanClusters] Number of points in the input point cloud different than normals!\n");
63 std::vector<bool> processed (cloud.
points.size (),
false);
65 std::vector<int> nn_indices;
66 std::vector<float> nn_distances;
68 int size =
static_cast<int> (cloud.
points.size ());
69 for (
int i = 0; i < size; ++i)
74 std::vector<unsigned int> seed_queue;
76 seed_queue.push_back (i);
80 while (sq_idx < static_cast<int> (seed_queue.size ()))
83 if (normals.
points[seed_queue[sq_idx]].curvature > curvature_threshold)
90 if (!tree->
radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
96 for (
size_t j = 1; j < nn_indices.size (); ++j)
98 if (processed[nn_indices[j]])
101 if (normals.
points[nn_indices[j]].curvature > curvature_threshold)
109 double dot_p = normals.
points[seed_queue[sq_idx]].normal[0] * normals.
points[nn_indices[j]].normal[0]
110 + normals.
points[seed_queue[sq_idx]].normal[1] * normals.
points[nn_indices[j]].normal[1]
111 + normals.
points[seed_queue[sq_idx]].normal[2] * normals.
points[nn_indices[j]].normal[2];
113 if (fabs (acos (dot_p)) < eps_angle)
115 processed[nn_indices[j]] =
true;
116 seed_queue.push_back (nn_indices[j]);
124 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
127 r.
indices.resize (seed_queue.size ());
128 for (
size_t j = 0; j < seed_queue.size (); ++j)
135 clusters.push_back (r);
140 template<
typename ModelT,
typename SceneT>
148 updateExplainedVector (recognition_models_[changed]->explained_, recognition_models_[changed]->explained_distances_, explained_by_RM_,
149 explained_by_RM_distance_weighted, 1.f);
150 updateUnexplainedVector (recognition_models_[changed]->unexplained_in_neighborhood, recognition_models_[changed]->unexplained_in_neighborhood_weights,
151 unexplained_by_RM_neighboorhods, recognition_models_[changed]->explained_, explained_by_RM_, 1.f);
152 updateCMDuplicity(recognition_models_[changed]->complete_cloud_occupancy_indices_, complete_cloud_occupancy_by_RM_, 1.f);
156 updateExplainedVector (recognition_models_[changed]->explained_, recognition_models_[changed]->explained_distances_, explained_by_RM_,
157 explained_by_RM_distance_weighted, -1.f);
158 updateUnexplainedVector (recognition_models_[changed]->unexplained_in_neighborhood, recognition_models_[changed]->unexplained_in_neighborhood_weights,
159 unexplained_by_RM_neighboorhods, recognition_models_[changed]->explained_, explained_by_RM_, -1.f);
160 updateCMDuplicity(recognition_models_[changed]->complete_cloud_occupancy_indices_, complete_cloud_occupancy_by_RM_, -1.f);
164 int duplicity = getDuplicity ();
165 float good_info = getExplainedValue ();
167 float unexplained_info = getPreviousUnexplainedValue ();
168 float bad_info =
static_cast<float> (getPreviousBadInfo ())
169 + (recognition_models_[changed]->outliers_weight_ * static_cast<float> (recognition_models_[changed]->bad_information_)) * sign;
171 setPreviousBadInfo (bad_info);
173 int n_active_hyp = 0;
174 for(
size_t i=0; i < active.size(); i++) {
179 float duplicity_cm =
static_cast<float> (getDuplicityCM ()) * w_occupied_multiple_cm_;
180 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);
184 template<
typename ModelT,
typename SceneT>
188 recognition_models_.clear ();
189 unexplained_by_RM_neighboorhods.clear ();
190 explained_by_RM_distance_weighted.clear ();
191 explained_by_RM_.clear ();
194 complete_cloud_occupancy_by_RM_.clear ();
197 mask_.resize (complete_models_.size ());
198 for (
size_t i = 0; i < complete_models_.size (); i++)
201 indices_.resize (complete_models_.size ());
203 NormalEstimator_ n3d;
207 normals_tree->setInputCloud (scene_cloud_downsampled_);
209 n3d.setRadiusSearch (radius_normals_);
210 n3d.setSearchMethod (normals_tree);
211 n3d.setInputCloud (scene_cloud_downsampled_);
212 n3d.compute (*scene_normals_);
216 for (
size_t i = 0; i < scene_normals_->points.size (); ++i)
218 if (!pcl_isfinite (scene_normals_->points[i].normal_x) || !pcl_isfinite (scene_normals_->points[i].normal_y)
219 || !pcl_isfinite (scene_normals_->points[i].normal_z))
222 scene_normals_->points[j] = scene_normals_->points[i];
223 scene_cloud_downsampled_->points[j] = scene_cloud_downsampled_->points[i];
228 scene_normals_->points.resize (j);
229 scene_normals_->width = j;
230 scene_normals_->height = 1;
232 scene_cloud_downsampled_->points.resize (j);
233 scene_cloud_downsampled_->width = j;
234 scene_cloud_downsampled_->height = 1;
236 explained_by_RM_.resize (scene_cloud_downsampled_->points.size (), 0);
237 explained_by_RM_distance_weighted.resize (scene_cloud_downsampled_->points.size (), 0.f);
238 unexplained_by_RM_neighboorhods.resize (scene_cloud_downsampled_->points.size (), 0.f);
245 scene_downsampled_tree_->setInputCloud (scene_cloud_downsampled_);
247 std::vector<pcl::PointIndices> clusters;
248 double eps_angle_threshold = 0.2;
250 float curvature_threshold = 0.045f;
252 extractEuclideanClustersSmooth<SceneT, pcl::Normal> (*scene_cloud_downsampled_, *scene_normals_, inliers_threshold_ * 2.f, scene_downsampled_tree_,
253 clusters, eps_angle_threshold, curvature_threshold, min_points);
256 clusters_cloud_->points.resize (scene_cloud_downsampled_->points.size ());
257 clusters_cloud_->width = scene_cloud_downsampled_->width;
258 clusters_cloud_->height = 1;
260 for (
size_t i = 0; i < scene_cloud_downsampled_->points.size (); i++)
263 p.getVector3fMap () = scene_cloud_downsampled_->points[i].getVector3fMap ();
265 clusters_cloud_->points[i] = p;
268 float intens_incr = 100.f /
static_cast<float> (clusters.size ());
269 float intens = intens_incr;
270 for (
size_t i = 0; i < clusters.size (); i++)
272 for (
size_t j = 0; j < clusters[i].indices.size (); j++)
274 clusters_cloud_->points[clusters[i].indices[j]].
intensity = intens;
277 intens += intens_incr;
284 recognition_models_.resize (complete_models_.size ());
286 for (
int i = 0; i < static_cast<int> (complete_models_.size ()); i++)
289 recognition_models_[valid].reset (
new RecognitionModel ());
290 if(addModel (visible_models_[i], complete_models_[i], recognition_models_[valid])) {
296 recognition_models_.resize(valid);
297 indices_.resize(valid);
301 ModelT min_pt_all, max_pt_all;
302 min_pt_all.x = min_pt_all.y = min_pt_all.z = std::numeric_limits<float>::max ();
303 max_pt_all.x = max_pt_all.y = max_pt_all.z = (std::numeric_limits<float>::max () - 0.001f) * -1;
305 for (
size_t i = 0; i < recognition_models_.size (); i++)
307 ModelT min_pt, max_pt;
309 if (min_pt.x < min_pt_all.x)
310 min_pt_all.x = min_pt.x;
312 if (min_pt.y < min_pt_all.y)
313 min_pt_all.y = min_pt.y;
315 if (min_pt.z < min_pt_all.z)
316 min_pt_all.z = min_pt.z;
318 if (max_pt.x > max_pt_all.x)
319 max_pt_all.x = max_pt.x;
321 if (max_pt.y > max_pt_all.y)
322 max_pt_all.y = max_pt.y;
324 if (max_pt.z > max_pt_all.z)
325 max_pt_all.z = max_pt.z;
328 int size_x, size_y, size_z;
329 size_x =
static_cast<int> (std::ceil (std::abs (max_pt_all.x - min_pt_all.x) / res_occupancy_grid_)) + 1;
330 size_y =
static_cast<int> (std::ceil (std::abs (max_pt_all.y - min_pt_all.y) / res_occupancy_grid_)) + 1;
331 size_z =
static_cast<int> (std::ceil (std::abs (max_pt_all.z - min_pt_all.z) / res_occupancy_grid_)) + 1;
333 complete_cloud_occupancy_by_RM_.resize (size_x * size_y * size_z, 0);
335 for (
size_t i = 0; i < recognition_models_.size (); i++)
338 std::map<int, bool> banned;
339 std::map<int, bool>::iterator banned_it;
341 for (
size_t j = 0; j < complete_models_[indices_[i]]->points.size (); j++)
343 int pos_x, pos_y, pos_z;
344 pos_x =
static_cast<int> (std::floor ((complete_models_[indices_[i]]->points[j].x - min_pt_all.x) / res_occupancy_grid_));
345 pos_y =
static_cast<int> (std::floor ((complete_models_[indices_[i]]->points[j].y - min_pt_all.y) / res_occupancy_grid_));
346 pos_z =
static_cast<int> (std::floor ((complete_models_[indices_[i]]->points[j].z - min_pt_all.z) / res_occupancy_grid_));
348 int idx = pos_z * size_x * size_y + pos_y * size_x + pos_x;
349 banned_it = banned.find (idx);
350 if (banned_it == banned.end ())
352 complete_cloud_occupancy_by_RM_[idx]++;
353 recognition_models_[i]->complete_cloud_occupancy_indices_.push_back (idx);
361 #pragma omp parallel for schedule(dynamic, 4) num_threads(omp_get_num_procs()) 362 for (
int j = 0; j < static_cast<int> (recognition_models_.size ()); j++)
363 computeClutterCue (recognition_models_[j]);
369 for (
size_t i = 0; i < recognition_models_.size (); i++)
370 cc_[0].push_back (static_cast<int> (i));
374 template<
typename ModelT,
typename SceneT>
379 std::vector < boost::shared_ptr<RecognitionModel> > recognition_models_copy;
380 recognition_models_copy = recognition_models_;
382 recognition_models_.clear ();
384 for (
size_t j = 0; j < cc_indices.size (); j++)
386 recognition_models_.push_back (recognition_models_copy[cc_indices[j]]);
389 for (
size_t j = 0; j < recognition_models_.size (); j++)
391 boost::shared_ptr < RecognitionModel > recog_model = recognition_models_[j];
392 for (
size_t i = 0; i < recog_model->explained_.size (); i++)
394 explained_by_RM_[recog_model->explained_[i]]++;
395 explained_by_RM_distance_weighted[recog_model->explained_[i]] += recog_model->explained_distances_[i];
400 for (
size_t i = 0; i < recog_model->unexplained_in_neighborhood.size (); i++)
402 unexplained_by_RM_neighboorhods[recog_model->unexplained_in_neighborhood[i]] += recog_model->unexplained_in_neighborhood_weights[i];
407 int occupied_multiple = 0;
408 for(
size_t i=0; i < complete_cloud_occupancy_by_RM_.size(); i++) {
409 if(complete_cloud_occupancy_by_RM_[i] > 1) {
410 occupied_multiple+=complete_cloud_occupancy_by_RM_[i];
414 setPreviousDuplicityCM(occupied_multiple);
419 float good_information_ = getTotalExplainedInformation (explained_by_RM_, explained_by_RM_distance_weighted, &duplicity);
420 float bad_information_ = 0;
421 float unexplained_in_neighboorhod = getUnexplainedInformationInNeighborhood (unexplained_by_RM_neighboorhods, explained_by_RM_);
423 for (
size_t i = 0; i < initial_solution.size (); i++)
425 if (initial_solution[i])
426 bad_information_ += recognition_models_[i]->outliers_weight_ *
static_cast<float> (recognition_models_[i]->bad_information_);
429 setPreviousExplainedValue (good_information_);
430 setPreviousDuplicity (duplicity);
431 setPreviousBadInfo (bad_information_);
432 setPreviousUnexplainedValue (unexplained_in_neighboorhod);
435 model.cost_ =
static_cast<mets::gol_type
> ((good_information_ - bad_information_
436 -
static_cast<float> (duplicity)
437 - static_cast<float> (occupied_multiple) * w_occupied_multiple_cm_
438 -
static_cast<float> (recognition_models_.size ())
439 - unexplained_in_neighboorhod) * -1.f);
441 model.setSolution (initial_solution);
442 model.setOptimizer (
this);
443 SAModel best (model);
445 move_manager neigh (static_cast<int> (cc_indices.size ()));
447 mets::best_ever_solution best_recorder (best);
448 mets::noimprove_termination_criteria noimprove (max_iterations_);
449 mets::linear_cooling linear_cooling;
450 mets::simulated_annealing<move_manager> sa (model, best_recorder, neigh, noimprove, linear_cooling, initial_temp_, 1e-7, 2);
451 sa.setApplyAndEvaluate(
true);
458 best_seen_ =
static_cast<const SAModel&
> (best_recorder.best_seen ());
459 for (
size_t i = 0; i < best_seen_.solution_.size (); i++)
461 initial_solution[i] = best_seen_.solution_[i];
464 recognition_models_ = recognition_models_copy;
469 template<
typename ModelT,
typename SceneT>
475 for (
int c = 0; c < n_cc_; c++)
479 std::vector<bool> subsolution (cc_[c].size (),
true);
480 SAOptimize (cc_[c], subsolution);
481 for (
size_t i = 0; i < subsolution.size (); i++)
483 mask_[indices_[cc_[c][i]]] = (subsolution[i]);
488 template<
typename ModelT,
typename SceneT>
496 float size_model = resolution_;
499 voxel_grid.
setLeafSize (size_model, size_model, size_model);
500 voxel_grid.
filter (*(recog_model->cloud_));
504 voxel_grid2.
setLeafSize (size_model, size_model, size_model);
505 voxel_grid2.
filter (*(recog_model->complete_cloud_));
510 for (
size_t i = 0; i < recog_model->cloud_->points.size (); ++i)
512 if (!pcl_isfinite (recog_model->cloud_->points[i].x) || !pcl_isfinite (recog_model->cloud_->points[i].y)
513 || !pcl_isfinite (recog_model->cloud_->points[i].z))
516 recog_model->cloud_->points[j] = recog_model->cloud_->points[i];
520 recog_model->cloud_->points.resize (j);
521 recog_model->cloud_->width = j;
522 recog_model->cloud_->height = 1;
525 if (recog_model->cloud_->points.size () <= 0)
527 PCL_WARN(
"The model cloud has no points..\n");
534 NormalEstimator_ n3d;
540 n3d.
compute (*(recog_model->normals_));
544 for (
size_t i = 0; i < recog_model->normals_->points.size (); ++i)
546 if (!pcl_isfinite (recog_model->normals_->points[i].normal_x) || !pcl_isfinite (recog_model->normals_->points[i].normal_y)
547 || !pcl_isfinite (recog_model->normals_->points[i].normal_z))
550 recog_model->normals_->points[j] = recog_model->normals_->points[i];
551 recog_model->cloud_->points[j] = recog_model->cloud_->points[i];
555 recog_model->normals_->points.resize (j);
556 recog_model->normals_->width = j;
557 recog_model->normals_->height = 1;
559 recog_model->cloud_->points.resize (j);
560 recog_model->cloud_->width = j;
561 recog_model->cloud_->height = 1;
563 std::vector<int> explained_indices;
564 std::vector<float> outliers_weight;
565 std::vector<float> explained_indices_distances;
566 std::vector<float> unexplained_indices_weights;
568 std::vector<int> nn_indices;
569 std::vector<float> nn_distances;
571 std::map<int, boost::shared_ptr<std::vector<std::pair<int, float> > > > model_explains_scene_points;
572 std::map<int, boost::shared_ptr<std::vector<std::pair<int, float> > > >::iterator it;
574 outliers_weight.resize (recog_model->cloud_->points.size ());
575 recog_model->outlier_indices_.resize (recog_model->cloud_->points.size ());
578 for (
size_t i = 0; i < recog_model->cloud_->points.size (); i++)
580 if (!scene_downsampled_tree_->radiusSearch (recog_model->cloud_->points[i], inliers_threshold_, nn_indices, nn_distances, std::numeric_limits<int>::max ()))
583 outliers_weight[o] = regularizer_;
584 recog_model->outlier_indices_[o] =
static_cast<int> (i);
588 for (
size_t k = 0; k < nn_distances.size (); k++)
590 std::pair<int, float> pair = std::make_pair (i, nn_distances[k]);
591 it = model_explains_scene_points.find (nn_indices[k]);
592 if (it == model_explains_scene_points.end ())
594 boost::shared_ptr < std::vector<std::pair<int, float> > > vec (
new std::vector<std::pair<int, float> > ());
595 vec->push_back (pair);
596 model_explains_scene_points[nn_indices[k]] = vec;
599 it->second->push_back (pair);
605 outliers_weight.resize (o);
606 recog_model->outlier_indices_.resize (o);
608 recog_model->outliers_weight_ = (std::accumulate (outliers_weight.begin (), outliers_weight.end (), 0.f) / static_cast<float> (outliers_weight.size ()));
609 if (outliers_weight.size () == 0)
610 recog_model->outliers_weight_ = 1.f;
617 for (it = model_explains_scene_points.begin (); it != model_explains_scene_points.end (); it++, p++)
620 float min_d = std::numeric_limits<float>::min ();
621 for (
size_t i = 0; i < it->second->size (); i++)
623 if (it->second->at (i).second > min_d)
625 min_d = it->second->at (i).second;
630 float d = it->second->at (closest).second;
631 float d_weight = -(d * d / (inliers_threshold_)) + 1;
635 Eigen::Vector3f scene_p_normal = scene_normals_->points[it->first].getNormalVector3fMap ();
636 Eigen::Vector3f model_p_normal = recog_model->normals_->points[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_->points[recog_model->explained_[i]], radius_neighborhood_GO_, nn_indices,
668 nn_distances, std::numeric_limits<int>::max ()))
670 for (
size_t k = 0; k < nn_distances.size (); k++)
672 if (nn_indices[k] != i)
673 neighborhood_indices.push_back (std::make_pair (nn_indices[k], i));
679 std::sort (neighborhood_indices.begin (), neighborhood_indices.end (),
680 boost::bind (&std::pair<int, int>::first, _1) < boost::bind (&std::pair<int, int>::first, _2));
683 neighborhood_indices.erase (
684 std::unique (neighborhood_indices.begin (), neighborhood_indices.end (),
685 boost::bind (&std::pair<int, int>::first, _1) == boost::bind (&std::pair<int, int>::first, _2)), 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 (
size_t i = 0; i < neighborhood_indices.size (); i++)
698 if ((j < exp_idces.size ()) && (neighborhood_indices[i].first == exp_idces[j]))
706 recog_model->unexplained_in_neighborhood[p] = neighborhood_indices[i].first;
708 if (clusters_cloud_->points[recog_model->explained_[neighborhood_indices[i].second]].intensity != 0.f
709 && (clusters_cloud_->points[recog_model->explained_[neighborhood_indices[i].second]].intensity
710 == clusters_cloud_->points[neighborhood_indices[i].first].intensity))
713 recog_model->unexplained_in_neighborhood_weights[p] = clutter_regularizer_;
719 float d =
static_cast<float> (pow (
720 (scene_cloud_downsampled_->points[recog_model->explained_[neighborhood_indices[i].second]].getVector3fMap ()
721 - scene_cloud_downsampled_->points[neighborhood_indices[i].first].getVector3fMap ()).norm (), 2));
722 float d_weight = -(d / rn_sqr) + 1;
725 Eigen::Vector3f scene_p_normal = scene_normals_->points[neighborhood_indices[i].first].getNormalVector3fMap ();
726 Eigen::Vector3f model_p_normal = scene_normals_->points[recog_model->explained_[neighborhood_indices[i].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>;
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
boost::shared_ptr< std::vector< int > > IndicesPtr
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.
Class to measure the time spent in a scope.
std::vector< int > indices
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...
boost::shared_ptr< KdTree< PointT, Tree > > Ptr
boost::shared_ptr< pcl::search::Search< PointT > > Ptr
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...
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
virtual PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
pcl::PCLHeader header
The point cloud header.
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Provide a pointer to the input dataset.
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.
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
A hypothesis verification method proposed in "A Global Hypotheses Verification Method for 3D Object R...
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.