38 #ifndef PCL_REGISTRATION_CORRESPONDENCE_REJECTION_POLY_H_
39 #define PCL_REGISTRATION_CORRESPONDENCE_REJECTION_POLY_H_
41 #include <pcl/registration/correspondence_rejection.h>
42 #include <pcl/point_cloud.h>
46 namespace registration
63 template <
typename SourceT,
typename TargetT>
71 typedef boost::shared_ptr<CorrespondenceRejectorPoly>
Ptr;
72 typedef boost::shared_ptr<const CorrespondenceRejectorPoly>
ConstPtr;
86 , similarity_threshold_ (0.75f)
87 , similarity_threshold_squared_ (0.75f * 0.75f)
89 rejection_name_ =
"CorrespondenceRejectorPoly";
115 PCL_WARN (
"[pcl::registration::%s::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.\n",
116 getClassName ().c_str ());
138 PointCloudSourcePtr cloud (
new PointCloudSource);
140 setInputSource (cloud);
152 PointCloudTargetPtr cloud (
new PointCloudTarget);
154 setInputTarget (cloud);
163 cardinality_ = cardinality;
172 return (cardinality_);
182 similarity_threshold_ = similarity_threshold;
183 similarity_threshold_squared_ = similarity_threshold * similarity_threshold;
192 return (similarity_threshold_);
201 iterations_ = iterations;
210 return (iterations_);
221 if (cardinality_ == 2)
223 return (thresholdEdgeLength (corr[ idx[0] ].index_query, corr[ idx[1] ].index_query,
224 corr[ idx[0] ].index_match, corr[ idx[1] ].index_match,
229 for (
int i = 0; i < cardinality_; ++i)
230 if (!thresholdEdgeLength (corr[ idx[i] ].index_query, corr[ idx[(i+1)%cardinality_] ].index_query,
231 corr[ idx[i] ].index_match, corr[ idx[(i+1)%cardinality_] ].index_match,
232 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>
385 #endif // PCL_REGISTRATION_CORRESPONDENCE_REJECTION_POLY_H_
bool requiresTargetPoints() const
See if this rejector requires a target cloud.
PointCloudSource::Ptr PointCloudSourcePtr
void fromPCLPointCloud2(const pcl::PCLPointCloud2 &msg, pcl::PointCloud< PointT > &cloud, const MsgFieldMap &field_map)
Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud object using a field_map...
void applyRejection(pcl::Correspondences &correspondences)
Apply the rejection algorithm.
void setSimilarityThreshold(float similarity_threshold)
Set the similarity threshold in [0,1[ between edge lengths, where 1 is a perfect match.
boost::shared_ptr< PointCloud< PointT > > Ptr
int getCardinality()
Get the polygon cardinality.
CorrespondenceRejectorPoly()
Empty constructor.
PointCloudTarget::ConstPtr PointCloudTargetConstPtr
This file defines compatibility wrappers for low level I/O functions.
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 ...
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.
pcl::PointCloud< SourceT > PointCloudSource
void setSourcePoints(pcl::PCLPointCloud2::ConstPtr cloud2)
Blob method for setting the source cloud.
bool requiresSourcePoints() const
See if this rejector requires source points.
bool thresholdEdgeLength(int index_query_1, int index_query_2, int index_match_1, int index_match_2, float simsq)
Edge length similarity thresholding.
PointCloudSourceConstPtr input_
The input point cloud dataset.
void setInputCloud(const PointCloudSourceConstPtr &cloud)
Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence dis...
void setInputSource(const PointCloudSourceConstPtr &cloud)
Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence dis...
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
const std::string & getClassName() const
Get a string representation of the name of this class.
PointCloudSource::ConstPtr PointCloudSourceConstPtr
int getIterations()
Get the number of iterations.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
boost::shared_ptr< ::pcl::PCLPointCloud2 const > ConstPtr
float similarity_threshold_
Lower edge length threshold in [0,1] used for verifying polygon similarities, where 1 is a perfect ma...
pcl::PointCloud< TargetT > PointCloudTarget
int cardinality_
The polygon cardinality used during rejection.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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...
int iterations_
Number of iterations to run.
boost::shared_ptr< const CorrespondenceRejectorPoly > ConstPtr
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.
void setTargetPoints(pcl::PCLPointCloud2::ConstPtr cloud2)
Method for setting the target cloud.
std::string rejection_name_
The name of the rejection method.
void setCardinality(int cardinality)
Set the polygon cardinality.
PointCloudTarget::Ptr PointCloudTargetPtr
void setInputTarget(const PointCloudTargetConstPtr &target)
Provide a target point cloud dataset (must contain XYZ data!), used to compute the correspondence dis...
void setIterations(int iterations)
Set the number of iterations.
boost::shared_ptr< CorrespondenceRejectorPoly > Ptr
float similarity_threshold_squared_
Squared value if similarity_threshold_, only for internal use.