CorrespondenceRejectorPoly implements a correspondence rejection method that exploits low-level and pose-invariant geometric constraints between two point sets by forming virtual polygons of a user-specifiable cardinality on each model using the input correspondences. These polygons are then checked in a pose-invariant manner (i.e. the side lengths must be approximately equal), and rejection is performed by thresholding these edge lengths. More...
#include <correspondence_rejection_poly.h>
Public Types | |
typedef boost::shared_ptr < const CorrespondenceRejectorPoly > | ConstPtr |
typedef pcl::PointCloud< SourceT > | PointCloudSource |
typedef PointCloudSource::ConstPtr | PointCloudSourceConstPtr |
typedef PointCloudSource::Ptr | PointCloudSourcePtr |
typedef pcl::PointCloud< TargetT > | PointCloudTarget |
typedef PointCloudTarget::ConstPtr | PointCloudTargetConstPtr |
typedef PointCloudTarget::Ptr | PointCloudTargetPtr |
typedef boost::shared_ptr < CorrespondenceRejectorPoly > | Ptr |
Public Member Functions | |
CorrespondenceRejectorPoly () | |
Empty constructor. | |
int | getCardinality () |
Get the polygon cardinality. | |
int | getIterations () |
Get the number of iterations. | |
void | getRemainingCorrespondences (const pcl::Correspondences &original_correspondences, pcl::Correspondences &remaining_correspondences) |
Get a list of valid correspondences after rejection from the original set of correspondences. | |
float | getSimilarityThreshold () |
Get the similarity threshold between edge lengths. | |
void | setCardinality (int cardinality) |
Set the polygon cardinality. | |
void | setInputCloud (const PointCloudSourceConstPtr &cloud) |
Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence distance. | |
void | setInputSource (const PointCloudSourceConstPtr &cloud) |
Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence distance. | |
void | setInputTarget (const PointCloudTargetConstPtr &target) |
Provide a target point cloud dataset (must contain XYZ data!), used to compute the correspondence distance. | |
void | setIterations (int iterations) |
Set the number of iterations. | |
void | setSimilarityThreshold (float similarity_threshold) |
Set the similarity threshold in [0,1[ between edge lengths, where 1 is a perfect match. | |
bool | thresholdPolygon (const pcl::Correspondences &corr, const std::vector< int > &idx) |
Polygonal rejection of a single polygon, indexed by a subset of correspondences. | |
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. | |
Protected Member Functions | |
void | applyRejection (pcl::Correspondences &correspondences) |
Apply the rejection algorithm. | |
std::vector< int > | computeHistogram (const std::vector< float > &data, float lower, float upper, int bins) |
Compute a linear histogram. This function is equivalent to the MATLAB function histc, with the edges set as follows: lower:(upper-lower)/bins:upper | |
float | computeSquaredDistance (const SourceT &p1, const TargetT &p2) |
Squared Euclidean distance between two points using the members x, y and z. | |
int | findThresholdOtsu (const std::vector< int > &histogram) |
Find the optimal value for binary histogram thresholding using Otsu's method. | |
std::vector< int > | getUniqueRandomIndices (int n, int k) |
Get k unique random indices in range {0,...,n-1} (sampling without replacement) | |
bool | thresholdEdgeLength (int index_query_1, int index_query_2, int index_match_1, int index_match_2, float simsq) |
Edge length similarity thresholding. | |
Protected Attributes | |
int | cardinality_ |
The polygon cardinality used during rejection. | |
PointCloudSourceConstPtr | input_ |
The input point cloud dataset. | |
int | iterations_ |
Number of iterations to run. | |
float | similarity_threshold_ |
Lower edge length threshold in [0,1] used for verifying polygon similarities, where 1 is a perfect match. | |
float | similarity_threshold_squared_ |
Squared value if similarity_threshold_, only for internal use. | |
PointCloudTargetConstPtr | target_ |
The input point cloud dataset target. |
CorrespondenceRejectorPoly implements a correspondence rejection method that exploits low-level and pose-invariant geometric constraints between two point sets by forming virtual polygons of a user-specifiable cardinality on each model using the input correspondences. These polygons are then checked in a pose-invariant manner (i.e. the side lengths must be approximately equal), and rejection is performed by thresholding these edge lengths.
If you use this in academic work, please cite:
A. G. Buch, D. Kraft, J.-K. Kämäräinen, H. G. Petersen and N. Krüger. Pose Estimation using Local Structure-Specific Shape and Appearance Context. International Conference on Robotics and Automation (ICRA), 2013.
Definition at line 64 of file correspondence_rejection_poly.h.
typedef boost::shared_ptr<const CorrespondenceRejectorPoly> pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::ConstPtr |
Reimplemented from pcl::registration::CorrespondenceRejector.
Definition at line 72 of file correspondence_rejection_poly.h.
typedef pcl::PointCloud<SourceT> pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::PointCloudSource |
Definition at line 74 of file correspondence_rejection_poly.h.
typedef PointCloudSource::ConstPtr pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::PointCloudSourceConstPtr |
Definition at line 76 of file correspondence_rejection_poly.h.
typedef PointCloudSource::Ptr pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::PointCloudSourcePtr |
Definition at line 75 of file correspondence_rejection_poly.h.
typedef pcl::PointCloud<TargetT> pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::PointCloudTarget |
Definition at line 78 of file correspondence_rejection_poly.h.
typedef PointCloudTarget::ConstPtr pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::PointCloudTargetConstPtr |
Definition at line 80 of file correspondence_rejection_poly.h.
typedef PointCloudTarget::Ptr pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::PointCloudTargetPtr |
Definition at line 79 of file correspondence_rejection_poly.h.
typedef boost::shared_ptr<CorrespondenceRejectorPoly> pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::Ptr |
Reimplemented from pcl::registration::CorrespondenceRejector.
Definition at line 71 of file correspondence_rejection_poly.h.
pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::CorrespondenceRejectorPoly | ( | ) | [inline] |
Empty constructor.
Definition at line 83 of file correspondence_rejection_poly.h.
void pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::applyRejection | ( | pcl::Correspondences & | correspondences | ) | [inline, protected, virtual] |
Apply the rejection algorithm.
[out] | correspondences | the set of resultant correspondences. |
Implements pcl::registration::CorrespondenceRejector.
Definition at line 237 of file correspondence_rejection_poly.h.
std::vector< int > pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::computeHistogram | ( | const std::vector< float > & | data, |
float | lower, | ||
float | upper, | ||
int | bins | ||
) | [protected] |
Compute a linear histogram. This function is equivalent to the MATLAB function histc, with the edges set as follows: lower:(upper-lower)/bins:upper
data | input samples |
lower | lower bound of input samples |
upper | upper bound of input samples |
bins | number of bins in output |
Definition at line 154 of file correspondence_rejection_poly.hpp.
float pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::computeSquaredDistance | ( | const SourceT & | p1, |
const TargetT & | p2 | ||
) | [inline, protected] |
Squared Euclidean distance between two points using the members x, y and z.
p1 | first point |
p2 | second point |
Definition at line 282 of file correspondence_rejection_poly.h.
int pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::findThresholdOtsu | ( | const std::vector< int > & | histogram | ) | [protected] |
Find the optimal value for binary histogram thresholding using Otsu's method.
histogram | input histogram |
Definition at line 173 of file correspondence_rejection_poly.hpp.
int pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::getCardinality | ( | ) | [inline] |
Get the polygon cardinality.
Definition at line 142 of file correspondence_rejection_poly.h.
int pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::getIterations | ( | ) | [inline] |
Get the number of iterations.
Definition at line 180 of file correspondence_rejection_poly.h.
void pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::getRemainingCorrespondences | ( | const pcl::Correspondences & | original_correspondences, |
pcl::Correspondences & | remaining_correspondences | ||
) | [virtual] |
Get a list of valid correspondences after rejection from the original set of correspondences.
[in] | original_correspondences | the set of initial correspondences given |
[out] | remaining_correspondences | the resultant filtered set of remaining correspondences |
Implements pcl::registration::CorrespondenceRejector.
Definition at line 43 of file correspondence_rejection_poly.hpp.
float pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::getSimilarityThreshold | ( | ) | [inline] |
Get the similarity threshold between edge lengths.
Definition at line 162 of file correspondence_rejection_poly.h.
std::vector<int> pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::getUniqueRandomIndices | ( | int | n, |
int | k | ||
) | [inline, protected] |
Get k unique random indices in range {0,...,n-1} (sampling without replacement)
n | upper index range, exclusive |
k | number of unique indices to sample |
Definition at line 249 of file correspondence_rejection_poly.h.
void pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::setCardinality | ( | int | cardinality | ) | [inline] |
Set the polygon cardinality.
cardinality | polygon cardinality |
Definition at line 133 of file correspondence_rejection_poly.h.
void pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::setInputCloud | ( | const PointCloudSourceConstPtr & | cloud | ) | [inline] |
Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence distance.
[in] | cloud | a cloud containing XYZ data |
Definition at line 113 of file correspondence_rejection_poly.h.
void pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::setInputSource | ( | const PointCloudSourceConstPtr & | cloud | ) | [inline] |
Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence distance.
[in] | cloud | a cloud containing XYZ data |
Definition at line 104 of file correspondence_rejection_poly.h.
void pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::setInputTarget | ( | const PointCloudTargetConstPtr & | target | ) | [inline] |
Provide a target point cloud dataset (must contain XYZ data!), used to compute the correspondence distance.
[in] | target | a cloud containing XYZ data |
Definition at line 124 of file correspondence_rejection_poly.h.
void pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::setIterations | ( | int | iterations | ) | [inline] |
Set the number of iterations.
iterations | number of iterations |
Definition at line 171 of file correspondence_rejection_poly.h.
void pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::setSimilarityThreshold | ( | float | similarity_threshold | ) | [inline] |
Set the similarity threshold in [0,1[ between edge lengths, where 1 is a perfect match.
similarity | similarity threshold |
Definition at line 152 of file correspondence_rejection_poly.h.
bool pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::thresholdEdgeLength | ( | int | index_query_1, |
int | index_query_2, | ||
int | index_match_1, | ||
int | index_match_2, | ||
float | simsq | ||
) | [inline, protected] |
Edge length similarity thresholding.
index_query_1 | index of first source vertex |
index_query_2 | index of second source vertex |
index_match_1 | index of first target vertex |
index_match_2 | index of second target vertex |
simsq | squared similarity threshold in [0,1] |
Definition at line 300 of file correspondence_rejection_poly.h.
bool pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::thresholdPolygon | ( | const pcl::Correspondences & | corr, |
const std::vector< int > & | idx | ||
) | [inline] |
Polygonal rejection of a single polygon, indexed by a subset of correspondences.
corr | all correspondences into input_ and target_ |
idx | sampled indices into correspondences, must have a size equal to cardinality_ |
Definition at line 191 of file correspondence_rejection_poly.h.
bool pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::thresholdPolygon | ( | const std::vector< int > & | source_indices, |
const std::vector< int > & | target_indices | ||
) | [inline] |
Polygonal rejection of a single polygon, indexed by two point index vectors.
source_indices | indices of polygon points in input_, must have a size equal to cardinality_ |
target_indices | corresponding indices of polygon points in target_, must have a size equal to cardinality_ |
Definition at line 217 of file correspondence_rejection_poly.h.
int pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::cardinality_ [protected] |
The polygon cardinality used during rejection.
Definition at line 344 of file correspondence_rejection_poly.h.
PointCloudSourceConstPtr pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::input_ [protected] |
The input point cloud dataset.
Definition at line 335 of file correspondence_rejection_poly.h.
int pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::iterations_ [protected] |
Number of iterations to run.
Definition at line 341 of file correspondence_rejection_poly.h.
float pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::similarity_threshold_ [protected] |
Lower edge length threshold in [0,1] used for verifying polygon similarities, where 1 is a perfect match.
Definition at line 347 of file correspondence_rejection_poly.h.
float pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::similarity_threshold_squared_ [protected] |
Squared value if similarity_threshold_, only for internal use.
Definition at line 350 of file correspondence_rejection_poly.h.
PointCloudTargetConstPtr pcl::registration::CorrespondenceRejectorPoly< SourceT, TargetT >::target_ [protected] |
The input point cloud dataset target.
Definition at line 338 of file correspondence_rejection_poly.h.