41 #include <pcl/registration/correspondence_rejection.h> 42 #include <pcl/point_cloud.h> 46 namespace registration
63 template <
typename SourceT,
typename TargetT>
71 using Ptr = shared_ptr<CorrespondenceRejectorPoly<SourceT, TargetT> >;
72 using ConstPtr = shared_ptr<const CorrespondenceRejectorPoly<SourceT, TargetT> >;
86 , similarity_threshold_ (0.75f)
87 , similarity_threshold_squared_ (0.75f * 0.75f)
89 rejection_name_ =
"CorrespondenceRejectorPoly";
112 PCL_DEPRECATED(1, 12,
"pcl::registration::CorrespondenceRejectorPoly::setInputCloud is deprecated. Please use setInputSource instead")
139 setInputSource (cloud);
153 setInputTarget (cloud);
162 cardinality_ = cardinality;
171 return (cardinality_);
181 similarity_threshold_ = similarity_threshold;
182 similarity_threshold_squared_ = similarity_threshold * similarity_threshold;
191 return (similarity_threshold_);
200 iterations_ = iterations;
209 return (iterations_);
220 if (cardinality_ == 2)
222 return (thresholdEdgeLength (corr[ idx[0] ].index_query, corr[ idx[1] ].index_query,
223 corr[ idx[0] ].index_match, corr[ idx[1] ].index_match,
227 for (
int i = 0; i < cardinality_; ++i)
229 if (!thresholdEdgeLength (corr[ idx[i] ].index_query, corr[ idx[(i+1)%cardinality_] ].index_query,
230 corr[ idx[i] ].index_match, corr[ idx[(i+1)%cardinality_] ].index_match,
231 similarity_threshold_squared_))
245 thresholdPolygon (
const std::vector<int>& source_indices,
const std::vector<int>& target_indices)
249 std::vector<int> idx (cardinality_);
250 for (
int i = 0; i < cardinality_; ++i)
252 corr[i].index_query = source_indices[i];
253 corr[i].index_match = target_indices[i];
257 return (thresholdPolygon (corr, idx));
267 getRemainingCorrespondences (*input_correspondences_, correspondences);
276 inline std::vector<int>
280 std::vector<bool> sampled (n,
false);
283 std::vector<int> result;
288 const int idx = (std::rand () % n);
296 result.push_back (idx);
312 const float dx = p2.x - p1.x;
313 const float dy = p2.y - p1.y;
314 const float dz = p2.z - p1.z;
316 return (dx*dx + dy*dy + dz*dz);
335 const float dist_src = computeSquaredDistance ((*input_)[index_query_1], (*input_)[index_query_2]);
337 const float dist_tgt = computeSquaredDistance ((*target_)[index_match_1], (*target_)[index_match_2]);
339 const float edge_sim = (dist_src < dist_tgt ? dist_src / dist_tgt : dist_tgt / dist_src);
341 return (edge_sim >= simsq);
353 computeHistogram (
const std::vector<float>& data,
float lower,
float upper,
int bins);
360 findThresholdOtsu (
const std::vector<int>& histogram);
383 #include <pcl/registration/impl/correspondence_rejection_poly.hpp> void fromPCLPointCloud2(const pcl::PCLPointCloud2 &msg, pcl::PointCloud< PointT > &cloud, const MsgFieldMap &field_map)
Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud<T> object using a field_map...
shared_ptr< const CorrespondenceRejector > ConstPtr
void setSimilarityThreshold(float similarity_threshold)
Set the similarity threshold in [0,1[ between edge lengths, where 1 is a perfect match.
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
shared_ptr< PointCloud< PointT > > Ptr
int getCardinality()
Get the polygon cardinality.
void setSourcePoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
Blob method for setting the source cloud.
shared_ptr< CorrespondenceRejector > Ptr
CorrespondenceRejectorPoly()
Empty constructor.
bool thresholdPolygon(const pcl::Correspondences &corr, const std::vector< int > &idx)
Polygonal rejection of a single polygon, indexed by a subset of correspondences.
std::vector< int > getUniqueRandomIndices(int n, int k)
Get k unique random indices in range {0,...,n-1} (sampling without replacement)
CorrespondenceRejector represents the base class for correspondence rejection methods ...
shared_ptr< const ::pcl::PCLPointCloud2 > ConstPtr
bool thresholdPolygon(const std::vector< int > &source_indices, const std::vector< int > &target_indices)
Polygonal rejection of a single polygon, indexed by two point index vectors.
const std::string & getClassName() const
Get a string representation of the name of this class.
bool thresholdEdgeLength(int index_query_1, int index_query_2, int index_match_1, int index_match_2, float simsq)
Edge length similarity thresholding.
typename PointCloudSource::Ptr PointCloudSourcePtr
PointCloudSourceConstPtr input_
The input point cloud dataset.
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
void setInputSource(const PointCloudSourceConstPtr &cloud)
Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence dis...
int getIterations()
Get the number of iterations.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
float similarity_threshold_
Lower edge length threshold in [0,1] used for verifying polygon similarities, where 1 is a perfect ma...
int cardinality_
The polygon cardinality used during rejection.
typename PointCloudTarget::Ptr PointCloudTargetPtr
float getSimilarityThreshold()
Get the similarity threshold between edge lengths.
PointCloudTargetConstPtr target_
The input point cloud dataset target.
CorrespondenceRejectorPoly implements a correspondence rejection method that exploits low-level and p...
void applyRejection(pcl::Correspondences &correspondences) override
Apply the rejection algorithm.
int iterations_
Number of iterations to run.
float computeSquaredDistance(const SourceT &p1, const TargetT &p2)
Squared Euclidean distance between two points using the members x, y and z.
CorrespondencesConstPtr input_correspondences_
The input correspondences.
shared_ptr< const PointCloud< PointT > > ConstPtr
bool requiresSourcePoints() const override
See if this rejector requires source points.
std::string rejection_name_
The name of the rejection method.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
#define PCL_DEPRECATED(Major, Minor, Message)
macro for compatibility across compilers and help remove old deprecated items for the Major...
void setTargetPoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
Method for setting the target cloud.
void setCardinality(int cardinality)
Set the polygon cardinality.
void setInputTarget(const PointCloudTargetConstPtr &target)
Provide a target point cloud dataset (must contain XYZ data!), used to compute the correspondence dis...
bool requiresTargetPoints() const override
See if this rejector requires a target cloud.
void setIterations(int iterations)
Set the number of iterations.
float similarity_threshold_squared_
Squared value if similarity_threshold_, only for internal use.