Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
geometric_consistency.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2012, Willow Garage, Inc.
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 * $Id$
37 *
38 */
39
40#ifndef PCL_RECOGNITION_GEOMETRIC_CONSISTENCY_IMPL_H_
41#define PCL_RECOGNITION_GEOMETRIC_CONSISTENCY_IMPL_H_
42
43#include <pcl/recognition/cg/geometric_consistency.h>
44#include <pcl/registration/correspondence_types.h>
45#include <pcl/registration/correspondence_rejection_sample_consensus.h>
46#include <pcl/common/io.h>
47
48//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
49inline bool
50gcCorrespSorter (pcl::Correspondence i, pcl::Correspondence j)
51{
52 return (i.distance < j.distance);
53}
54
55//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
56template<typename PointModelT, typename PointSceneT> void
58{
59 model_instances.clear ();
60 found_transformations_.clear ();
61
62 if (!model_scene_corrs_)
63 {
64 PCL_ERROR(
65 "[pcl::GeometricConsistencyGrouping::clusterCorrespondences()] Error! Correspondences not set, please set them before calling again this function.\n");
66 return;
67 }
68
69 CorrespondencesPtr sorted_corrs (new Correspondences (*model_scene_corrs_));
70
71 std::sort (sorted_corrs->begin (), sorted_corrs->end (), gcCorrespSorter);
72
73 model_scene_corrs_ = sorted_corrs;
74
75 std::vector<int> consensus_set;
76 std::vector<bool> taken_corresps (model_scene_corrs_->size (), false);
77
78 Eigen::Vector3f dist_ref, dist_trg;
79
80 //temp copy of scene cloud with the type cast to ModelT in order to use Ransac
81 PointCloudPtr temp_scene_cloud_ptr (new PointCloud ());
82 pcl::copyPointCloud (*scene_, *temp_scene_cloud_ptr);
83
85 corr_rejector.setMaximumIterations (10000);
86 corr_rejector.setInlierThreshold (gc_size_);
87 corr_rejector.setInputSource(input_);
88 corr_rejector.setInputTarget (temp_scene_cloud_ptr);
89
90 for (std::size_t i = 0; i < model_scene_corrs_->size (); ++i)
91 {
92 if (taken_corresps[i])
93 continue;
94
95 consensus_set.clear ();
96 consensus_set.push_back (static_cast<int> (i));
97
98 for (std::size_t j = 0; j < model_scene_corrs_->size (); ++j)
99 {
100 if ( j != i && !taken_corresps[j])
101 {
102 //Let's check if j fits into the current consensus set
103 bool is_a_good_candidate = true;
104 for (const int &k : consensus_set)
105 {
106 int scene_index_k = model_scene_corrs_->at (k).index_match;
107 int model_index_k = model_scene_corrs_->at (k).index_query;
108 int scene_index_j = model_scene_corrs_->at (j).index_match;
109 int model_index_j = model_scene_corrs_->at (j).index_query;
110
111 const Eigen::Vector3f& scene_point_k = scene_->at (scene_index_k).getVector3fMap ();
112 const Eigen::Vector3f& model_point_k = input_->at (model_index_k).getVector3fMap ();
113 const Eigen::Vector3f& scene_point_j = scene_->at (scene_index_j).getVector3fMap ();
114 const Eigen::Vector3f& model_point_j = input_->at (model_index_j).getVector3fMap ();
115
116 dist_ref = scene_point_k - scene_point_j;
117 dist_trg = model_point_k - model_point_j;
118
119 double distance = std::abs (dist_ref.norm () - dist_trg.norm ());
120
121 if (distance > gc_size_)
122 {
123 is_a_good_candidate = false;
124 break;
125 }
126 }
127
128 if (is_a_good_candidate)
129 consensus_set.push_back (static_cast<int> (j));
130 }
131 }
132
133 if (static_cast<int> (consensus_set.size ()) > gc_threshold_)
134 {
135 Correspondences temp_corrs, filtered_corrs;
136 for (const int &j : consensus_set)
137 {
138 temp_corrs.push_back (model_scene_corrs_->at (j));
139 taken_corresps[ j ] = true;
140 }
141 //ransac filtering
142 corr_rejector.getRemainingCorrespondences (temp_corrs, filtered_corrs);
143 //save transformations for recognize
144 found_transformations_.push_back (corr_rejector.getBestTransformation ());
145
146 model_instances.push_back (filtered_corrs);
147 }
148 }
149}
150
151//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
152template<typename PointModelT, typename PointSceneT> bool
154 std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations)
155{
156 std::vector<pcl::Correspondences> model_instances;
157 return (this->recognize (transformations, model_instances));
158}
159
160//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
161template<typename PointModelT, typename PointSceneT> bool
163 std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations, std::vector<pcl::Correspondences> &clustered_corrs)
164{
165 transformations.clear ();
166 if (!this->initCompute ())
167 {
168 PCL_ERROR(
169 "[pcl::GeometricConsistencyGrouping::recognize()] Error! Model cloud or Scene cloud not set, please set them before calling again this function.\n");
170 return (false);
171 }
172
173 clusterCorrespondences (clustered_corrs);
174
175 transformations = found_transformations_;
176
177 this->deinitCompute ();
178 return (true);
179}
180
181#define PCL_INSTANTIATE_GeometricConsistencyGrouping(T,ST) template class PCL_EXPORTS pcl::GeometricConsistencyGrouping<T,ST>;
182
183#endif // PCL_RECOGNITION_GEOMETRIC_CONSISTENCY_IMPL_H_
void clusterCorrespondences(std::vector< Correspondences > &model_instances) override
Cluster the input correspondences in order to distinguish between different instances of the model in...
bool recognize(std::vector< Eigen::Matrix4f, Eigen::aligned_allocator< Eigen::Matrix4f > > &transformations)
The main function, recognizes instances of the model into the scene set by the user.
PointCloud represents the base class in PCL for storing collections of 3D points.
CorrespondenceRejectorSampleConsensus implements a correspondence rejection using Random Sample Conse...
virtual void setInputSource(const PointCloudConstPtr &cloud)
Provide a source point cloud dataset (must contain XYZ data!)
Eigen::Matrix4f getBestTransformation()
Get the best transformation after RANSAC rejection.
virtual void setInputTarget(const PointCloudConstPtr &cloud)
Provide a target point cloud dataset (must contain XYZ data!)
void setInlierThreshold(double threshold)
Set the maximum distance between corresponding points.
void setMaximumIterations(int max_iterations)
Set the maximum number of iterations.
void getRemainingCorrespondences(const pcl::Correspondences &original_correspondences, pcl::Correspondences &remaining_correspondences) override
Get a list of valid correspondences after rejection from the original set of correspondences.
void copyPointCloud(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Copy all the fields from a given point cloud into a new point cloud.
Definition io.hpp:144
shared_ptr< Correspondences > CorrespondencesPtr
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Correspondence represents a match between two entities (e.g., points, descriptors,...