41 #include <pcl/registration/correspondence_rejection.h>
42 #include <pcl/point_cloud.h>
45 namespace registration {
62 template <
typename SourceT,
typename TargetT>
69 using Ptr = shared_ptr<CorrespondenceRejectorPoly<SourceT, TargetT>>;
70 using ConstPtr = shared_ptr<const CorrespondenceRejectorPoly<SourceT, TargetT>>;
84 , similarity_threshold_(0.75f)
85 , similarity_threshold_squared_(0.75f * 0.75f)
87 rejection_name_ =
"CorrespondenceRejectorPoly";
133 setInputSource(cloud);
149 setInputTarget(cloud);
158 cardinality_ = cardinality;
167 return (cardinality_);
177 similarity_threshold_ = similarity_threshold;
178 similarity_threshold_squared_ = similarity_threshold * similarity_threshold;
187 return (similarity_threshold_);
196 iterations_ = iterations;
205 return (iterations_);
220 return (thresholdEdgeLength(corr[idx[0]].index_query,
221 corr[idx[1]].index_query,
222 corr[idx[0]].index_match,
223 corr[idx[1]].index_match,
224 similarity_threshold_squared_));
227 for (
int i = 0; i < cardinality_; ++i) {
228 if (!thresholdEdgeLength(corr[idx[i]].index_query,
229 corr[idx[(i + 1) % cardinality_]].index_query,
230 corr[idx[i]].index_match,
231 corr[idx[(i + 1) % cardinality_]].index_match,
232 similarity_threshold_squared_)) {
253 std::vector<int> idx(cardinality_);
254 for (
int i = 0; i < cardinality_; ++i) {
255 corr[i].index_query = source_indices[i];
256 corr[i].index_match = target_indices[i];
260 return (thresholdPolygon(corr, idx));
270 getRemainingCorrespondences(*input_correspondences_, correspondences);
279 inline std::vector<int>
283 std::vector<bool> sampled(n,
false);
286 std::vector<int> result;
290 const int idx = (std::rand() % n);
297 result.push_back(idx);
299 }
while (samples < k);
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 =
336 computeSquaredDistance((*input_)[index_query_1], (*input_)[index_query_2]);
338 const float dist_tgt =
339 computeSquaredDistance((*target_)[index_match_1], (*target_)[index_match_2]);
341 const float edge_sim =
342 (dist_src < dist_tgt ? dist_src / dist_tgt : dist_tgt / dist_src);
344 return (edge_sim >= simsq);
357 computeHistogram(
const std::vector<float>& data,
float lower,
float upper,
int bins);
365 findThresholdOtsu(
const std::vector<int>& histogram);
389 #include <pcl/registration/impl/correspondence_rejection_poly.hpp>
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
CorrespondenceRejector represents the base class for correspondence rejection methods
shared_ptr< const CorrespondenceRejector > ConstPtr
CorrespondencesConstPtr input_correspondences_
The input correspondences.
const std::string & getClassName() const
Get a string representation of the name of this class.
shared_ptr< CorrespondenceRejector > Ptr
std::string rejection_name_
The name of the rejection method.
CorrespondenceRejectorPoly implements a correspondence rejection method that exploits low-level and p...
float similarity_threshold_squared_
Squared value if similarity_threshold_, only for internal use.
bool thresholdPolygon(const pcl::Correspondences &corr, const std::vector< int > &idx)
Polygonal rejection of a single polygon, indexed by a subset of correspondences.
typename PointCloudSource::Ptr PointCloudSourcePtr
float similarity_threshold_
Lower edge length threshold in [0,1] used for verifying polygon similarities, where 1 is a perfect ma...
float computeSquaredDistance(const SourceT &p1, const TargetT &p2)
Squared Euclidean distance between two points using the members x, y and z.
int getIterations()
Get the number of iterations.
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
PointCloudSourceConstPtr input_
The input point cloud dataset.
float getSimilarityThreshold()
Get the similarity threshold between edge lengths.
void setTargetPoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
Method for setting the target cloud.
bool thresholdPolygon(const pcl::Indices &source_indices, const pcl::Indices &target_indices)
Polygonal rejection of a single polygon, indexed by two point index vectors.
void applyRejection(pcl::Correspondences &correspondences) override
Apply the rejection algorithm.
void setCardinality(int cardinality)
Set the polygon cardinality.
void setSimilarityThreshold(float similarity_threshold)
Set the similarity threshold in [0,1[ between edge lengths, where 1 is a perfect match.
void setIterations(int iterations)
Set the number of iterations.
bool thresholdEdgeLength(int index_query_1, int index_query_2, int index_match_1, int index_match_2, float simsq)
Edge length similarity thresholding.
CorrespondenceRejectorPoly()
Empty constructor.
typename PointCloudTarget::Ptr PointCloudTargetPtr
PointCloudTargetConstPtr target_
The input point cloud dataset target.
void setInputTarget(const PointCloudTargetConstPtr &target)
Provide a target point cloud dataset (must contain XYZ data!), used to compute the correspondence dis...
void setSourcePoints(pcl::PCLPointCloud2::ConstPtr cloud2) override
Blob method for setting the source cloud.
void setInputSource(const PointCloudSourceConstPtr &cloud)
Provide a source 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.
std::vector< int > getUniqueRandomIndices(int n, int k)
Get k unique random indices in range {0,...,n-1} (sampling without replacement)
bool requiresSourcePoints() const override
See if this rejector requires source points.
int iterations_
Number of iterations to run.
int getCardinality()
Get the polygon cardinality.
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
int cardinality_
The polygon cardinality used during rejection.
void fromPCLPointCloud2(const pcl::PCLPointCloud2 &msg, pcl::PointCloud< PointT > &cloud, const MsgFieldMap &field_map, const std::uint8_t *msg_data)
Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud<T> object using a field_map.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
IndicesAllocator<> Indices
Type used for indices in PCL.
shared_ptr< const ::pcl::PCLPointCloud2 > ConstPtr