Loading...
Searching...
No Matches
ppf_registration.h
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Alexandru-Eugen Ichim
* Willow Garage, Inc
* Copyright (c) 2012-, Open Perception, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder(s) nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id$
*
*/
#pragma once
#include <pcl/features/ppf.h>
#include <pcl/registration/registration.h>
#include <unordered_map>
namespace pcl {
class PCL_EXPORTS PPFHashMapSearch {
public:
/** \brief Data structure to hold the information for the key in the feature hash map
* of the PPFHashMapSearch class \note It uses multiple pair levels in order to enable
* the usage of the boost::hash function which has the std::pair implementation (i.e.,
* does not require a custom hash function)
*/
struct HashKeyStruct : public std::pair<int, std::pair<int, std::pair<int, int>>> {
HashKeyStruct() = default;
HashKeyStruct(int a, int b, int c, int d)
{
this->first = a;
this->second.first = b;
this->second.second.first = c;
this->second.second.second = d;
}
std::size_t
operator()(const HashKeyStruct& s) const noexcept
{
const std::size_t h1 = std::hash<int>{}(s.first);
const std::size_t h2 = std::hash<int>{}(s.second.first);
const std::size_t h3 = std::hash<int>{}(s.second.second.first);
const std::size_t h4 = std::hash<int>{}(s.second.second.second);
return h1 ^ (h2 << 1) ^ (h3 << 2) ^ (h4 << 3);
}
};
using FeatureHashMapType =
std::unordered_multimap<HashKeyStruct,
std::pair<std::size_t, std::size_t>,
HashKeyStruct>;
using FeatureHashMapTypePtr = shared_ptr<FeatureHashMapType>;
using Ptr = shared_ptr<PPFHashMapSearch>;
using ConstPtr = shared_ptr<const PPFHashMapSearch>;
/** \brief Constructor for the PPFHashMapSearch class which sets the two step
* parameters for the enclosed data structure \param angle_discretization_step the
* step value between each bin of the hash map for the angular values \param
* distance_discretization_step the step value between each bin of the hash map for
* the distance values
*/
PPFHashMapSearch(float angle_discretization_step = 12.0f / 180.0f *
static_cast<float>(M_PI),
float distance_discretization_step = 0.01f)
: feature_hash_map_(new FeatureHashMapType)
, angle_discretization_step_(angle_discretization_step)
, distance_discretization_step_(distance_discretization_step)
{}
/** \brief Method that sets the feature cloud to be inserted in the hash map
* \param feature_cloud a const smart pointer to the PPFSignature feature cloud
*/
void
setInputFeatureCloud(PointCloud<PPFSignature>::ConstPtr feature_cloud);
/** \brief Function for finding the nearest neighbors for the given feature inside the
* discretized hash map \param f1 The 1st value describing the query PPFSignature
* feature \param f2 The 2nd value describing the query PPFSignature feature \param f3
* The 3rd value describing the query PPFSignature feature \param f4 The 4th value
* describing the query PPFSignature feature \param indices a vector of pair indices
* representing the feature pairs that have been found in the bin corresponding to the
* query feature
*/
void
nearestNeighborSearch(float& f1,
float& f2,
float& f3,
float& f4,
std::vector<std::pair<std::size_t, std::size_t>>& indices);
/** \brief Convenience method for returning a copy of the class instance as a
* shared_ptr */
Ptr
makeShared()
{
return Ptr(new PPFHashMapSearch(*this));
}
/** \brief Returns the angle discretization step parameter (the step value between
* each bin of the hash map for the angular values) */
inline float
getAngleDiscretizationStep() const
{
return angle_discretization_step_;
}
/** \brief Returns the distance discretization step parameter (the step value between
* each bin of the hash map for the distance values) */
inline float
getDistanceDiscretizationStep() const
{
return distance_discretization_step_;
}
/** \brief Returns the maximum distance found between any feature pair in the given
* input feature cloud */
inline float
getModelDiameter() const
{
return max_dist_;
}
std::vector<std::vector<float>> alpha_m_;
private:
FeatureHashMapTypePtr feature_hash_map_;
bool internals_initialized_{false};
float angle_discretization_step_, distance_discretization_step_;
float max_dist_{-1.0f};
};
/** \brief Class that registers two point clouds based on their sets of PPFSignatures.
* Please refer to the following publication for more details:
* B. Drost, M. Ulrich, N. Navab, S. Ilic
* Model Globally, Match Locally: Efficient and Robust 3D Object Recognition
* 2010 IEEE Conference on Computer Vision and Pattern Recognition (CVPR)
* 13-18 June 2010, San Francisco, CA
*
* \note This class works in tandem with the PPFEstimation class
* \ingroup registration
*
* \author Alexandru-Eugen Ichim
*/
template <typename PointSource, typename PointTarget>
class PPFRegistration : public Registration<PointSource, PointTarget> {
public:
/** \brief Structure for storing a pose (represented as an Eigen::Affine3f) and an
* integer for counting votes \note initially used std::pair<Eigen::Affine3f, unsigned
* int>, but it proved problematic because of the Eigen structures alignment problems
* - std::pair does not have a custom allocator
*/
struct PoseWithVotes {
PoseWithVotes(const Eigen::Affine3f& a_pose, unsigned int& a_votes)
: pose(a_pose), votes(a_votes)
{}
Eigen::Affine3f pose;
unsigned int votes;
};
using PoseWithVotesList =
std::vector<PoseWithVotes, Eigen::aligned_allocator<PoseWithVotes>>;
/// input_ is the model cloud
using Registration<PointSource, PointTarget>::input_;
/// target_ is the scene cloud
using Registration<PointSource, PointTarget>::target_;
using Registration<PointSource, PointTarget>::converged_;
using Registration<PointSource, PointTarget>::final_transformation_;
using Registration<PointSource, PointTarget>::transformation_;
using PointCloudSource = pcl::PointCloud<PointSource>;
using PointCloudSourcePtr = typename PointCloudSource::Ptr;
using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
using PointCloudTarget = pcl::PointCloud<PointTarget>;
using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
/** \brief Empty constructor that initializes all the parameters of the algorithm with
* default values */
PPFRegistration()
: Registration<PointSource, PointTarget>()
, clustering_rotation_diff_threshold_(20.0f / 180.0f * static_cast<float>(M_PI))
{}
/** \brief Method for setting the position difference clustering parameter
* \param clustering_position_diff_threshold distance threshold below which two poses
* are considered close enough to be in the same cluster (for the clustering phase of
* the algorithm)
*/
inline void
setPositionClusteringThreshold(float clustering_position_diff_threshold)
{
clustering_position_diff_threshold_ = clustering_position_diff_threshold;
}
/** \brief Returns the parameter defining the position difference clustering parameter
* - distance threshold below which two poses are considered close enough to be in the
* same cluster (for the clustering phase of the algorithm)
*/
inline float
getPositionClusteringThreshold()
{
return clustering_position_diff_threshold_;
}
/** \brief Method for setting the rotation clustering parameter
* \param clustering_rotation_diff_threshold rotation difference threshold below which
* two poses are considered to be in the same cluster (for the clustering phase of the
* algorithm)
*/
inline void
setRotationClusteringThreshold(float clustering_rotation_diff_threshold)
{
clustering_rotation_diff_threshold_ = clustering_rotation_diff_threshold;
}
/** \brief Returns the parameter defining the rotation clustering threshold
*/
inline float
getRotationClusteringThreshold()
{
return clustering_rotation_diff_threshold_;
}
/** \brief Method for setting the scene reference point sampling rate
* \param scene_reference_point_sampling_rate sampling rate for the scene reference
* point
*/
inline void
setSceneReferencePointSamplingRate(unsigned int scene_reference_point_sampling_rate)
{
scene_reference_point_sampling_rate_ = scene_reference_point_sampling_rate;
}
/** \brief Returns the parameter for the scene reference point sampling rate of the
* algorithm */
inline unsigned int
getSceneReferencePointSamplingRate()
{
return scene_reference_point_sampling_rate_;
}
/** \brief Function that sets the search method for the algorithm
* \note Right now, the only available method is the one initially proposed by
* the authors - by using a hash map with discretized feature vectors
* \param search_method smart pointer to the search method to be set
*/
inline void
setSearchMethod(PPFHashMapSearch::Ptr search_method)
{
search_method_ = search_method;
}
/** \brief Getter function for the search method of the class */
inline PPFHashMapSearch::Ptr
getSearchMethod()
{
return search_method_;
}
/** \brief Provide a pointer to the input target (e.g., the point cloud that we want
* to align the input source to) \param cloud the input point cloud target
*/
void
setInputTarget(const PointCloudTargetConstPtr& cloud) override;
/** \brief Returns the most promising pose candidates, after clustering. The pose with
* the most votes is the result of the registration. It may make sense to check the
* next best pose candidates if the registration did not give the right result, or if
* there are more than one correct results. You need to call the align function before
* this one.
*/
inline PoseWithVotesList
getBestPoseCandidates()
{
return best_pose_candidates;
}
private:
/** \brief Method that calculates the transformation between the input_ and target_
* point clouds, based on the PPF features */
void
computeTransformation(PointCloudSource& output,
const Eigen::Matrix4f& guess) override;
/** \brief the search method that is going to be used to find matching feature pairs
*/
PPFHashMapSearch::Ptr search_method_;
/** \brief parameter for the sampling rate of the scene reference points */
uindex_t scene_reference_point_sampling_rate_{5};
/** \brief position and rotation difference thresholds below which two
* poses are considered to be in the same cluster (for the clustering phase of the
* algorithm) */
float clustering_position_diff_threshold_{0.01f}, clustering_rotation_diff_threshold_;
/** \brief use a kd-tree with range searches of range max_dist to skip an O(N) pass
* through the point cloud */
typename pcl::KdTreeFLANN<PointTarget>::Ptr scene_search_tree_;
/** \brief List with the most promising pose candidates, after clustering. The pose
* with the most votes is returned as the registration result. */
PoseWithVotesList best_pose_candidates;
/** \brief static method used for the std::sort function to order two PoseWithVotes
* instances by their number of votes*/
static bool
poseWithVotesCompareFunction(const PoseWithVotes& a, const PoseWithVotes& b);
/** \brief static method used for the std::sort function to order two pairs <index,
* votes> by the number of votes (unsigned integer value) */
static bool
clusterVotesCompareFunction(const std::pair<std::size_t, unsigned int>& a,
const std::pair<std::size_t, unsigned int>& b);
/** \brief Method that clusters a set of given poses by using the clustering
* thresholds and their corresponding number of votes (see publication for more
* details) */
void
clusterPoses(PoseWithVotesList& poses, PoseWithVotesList& result);
/** \brief Method that checks whether two poses are close together - based on the
* clustering threshold parameters of the class */
bool
posesWithinErrorBounds(Eigen::Affine3f& pose1,
Eigen::Affine3f& pose2,
float& position_diff,
float& rotation_diff_angle);
};
} // namespace pcl
#include <pcl/registration/impl/ppf_registration.hpp>
pcl::KdTreeFLANN::Ptr
shared_ptr< KdTreeFLANN< PointT, Dist > > Ptr
Definition kdtree_flann.h:151
pcl::PCLBase< PointSource >::input_
PointCloudConstPtr input_
The input point cloud dataset.
Definition pcl_base.h:147
pcl::PPFHashMapSearch
Definition ppf_registration.h:49
pcl::PPFHashMapSearch::getAngleDiscretizationStep
float getAngleDiscretizationStep() const
Returns the angle discretization step parameter (the step value between each bin of the hash map for ...
Definition ppf_registration.h:131
pcl::PPFHashMapSearch::alpha_m_
std::vector< std::vector< float > > alpha_m_
Definition ppf_registration.h:152
pcl::PPFHashMapSearch::FeatureHashMapTypePtr
shared_ptr< FeatureHashMapType > FeatureHashMapTypePtr
Definition ppf_registration.h:81
pcl::PPFHashMapSearch::FeatureHashMapType
std::unordered_multimap< HashKeyStruct, std::pair< std::size_t, std::size_t >, HashKeyStruct > FeatureHashMapType
Definition ppf_registration.h:80
pcl::PPFHashMapSearch::Ptr
shared_ptr< PPFHashMapSearch > Ptr
Definition ppf_registration.h:82
pcl::PPFHashMapSearch::ConstPtr
shared_ptr< const PPFHashMapSearch > ConstPtr
Definition ppf_registration.h:83
pcl::PPFHashMapSearch::makeShared
Ptr makeShared()
Convenience method for returning a copy of the class instance as a shared_ptr.
Definition ppf_registration.h:123
pcl::PPFHashMapSearch::PPFHashMapSearch
PPFHashMapSearch(float angle_discretization_step=12.0f/180.0f *static_cast< float >(M_PI), float distance_discretization_step=0.01f)
Constructor for the PPFHashMapSearch class which sets the two step parameters for the enclosed data s...
Definition ppf_registration.h:91
pcl::PPFHashMapSearch::getDistanceDiscretizationStep
float getDistanceDiscretizationStep() const
Returns the distance discretization step parameter (the step value between each bin of the hash map f...
Definition ppf_registration.h:139
pcl::PPFHashMapSearch::nearestNeighborSearch
void nearestNeighborSearch(float &f1, float &f2, float &f3, float &f4, std::vector< std::pair< std::size_t, std::size_t > > &indices)
Function for finding the nearest neighbors for the given feature inside the discretized hash map.
pcl::PPFHashMapSearch::setInputFeatureCloud
void setInputFeatureCloud(PointCloud< PPFSignature >::ConstPtr feature_cloud)
Method that sets the feature cloud to be inserted in the hash map.
pcl::PPFHashMapSearch::getModelDiameter
float getModelDiameter() const
Returns the maximum distance found between any feature pair in the given input feature cloud.
Definition ppf_registration.h:147
pcl::PPFRegistration
Class that registers two point clouds based on their sets of PPFSignatures.
Definition ppf_registration.h:175
pcl::PPFRegistration::PointCloudSourcePtr
typename PointCloudSource::Ptr PointCloudSourcePtr
Definition ppf_registration.h:202
pcl::PPFRegistration::getSceneReferencePointSamplingRate
unsigned int getSceneReferencePointSamplingRate()
Returns the parameter for the scene reference point sampling rate of the algorithm.
Definition ppf_registration.h:269
pcl::PPFRegistration::getRotationClusteringThreshold
float getRotationClusteringThreshold()
Returns the parameter defining the rotation clustering threshold.
Definition ppf_registration.h:251
pcl::PPFRegistration::PointCloudTargetPtr
typename PointCloudTarget::Ptr PointCloudTargetPtr
Definition ppf_registration.h:206
pcl::PPFRegistration::setRotationClusteringThreshold
void setRotationClusteringThreshold(float clustering_rotation_diff_threshold)
Method for setting the rotation clustering parameter.
Definition ppf_registration.h:243
pcl::PPFRegistration::getSearchMethod
PPFHashMapSearch::Ptr getSearchMethod()
Getter function for the search method of the class.
Definition ppf_registration.h:287
pcl::PPFRegistration::PointCloudSourceConstPtr
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
Definition ppf_registration.h:203
pcl::PPFRegistration::PPFRegistration
PPFRegistration()
Empty constructor that initializes all the parameters of the algorithm with default values.
Definition ppf_registration.h:211
pcl::PPFRegistration::PointCloudSource
pcl::PointCloud< PointSource > PointCloudSource
Definition ppf_registration.h:201
pcl::PPFRegistration::setSceneReferencePointSamplingRate
void setSceneReferencePointSamplingRate(unsigned int scene_reference_point_sampling_rate)
Method for setting the scene reference point sampling rate.
Definition ppf_registration.h:261
pcl::PPFRegistration::getBestPoseCandidates
PoseWithVotesList getBestPoseCandidates()
Returns the most promising pose candidates, after clustering.
Definition ppf_registration.h:305
pcl::PPFRegistration::PoseWithVotesList
std::vector< PoseWithVotes, Eigen::aligned_allocator< PoseWithVotes > > PoseWithVotesList
Definition ppf_registration.h:191
pcl::PPFRegistration::getPositionClusteringThreshold
float getPositionClusteringThreshold()
Returns the parameter defining the position difference clustering parameter.
Definition ppf_registration.h:232
pcl::PPFRegistration::PointCloudTargetConstPtr
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
Definition ppf_registration.h:207
pcl::PPFRegistration::setPositionClusteringThreshold
void setPositionClusteringThreshold(float clustering_position_diff_threshold)
Method for setting the position difference clustering parameter.
Definition ppf_registration.h:222
pcl::PPFRegistration::setInputTarget
void setInputTarget(const PointCloudTargetConstPtr &cloud) override
Provide a pointer to the input target (e.g., the point cloud that we want to align the input source t...
Definition ppf_registration.hpp:53
pcl::PPFRegistration::setSearchMethod
void setSearchMethod(PPFHashMapSearch::Ptr search_method)
Function that sets the search method for the algorithm.
Definition ppf_registration.h:280
pcl::PointCloud< PointSource >
pcl::PointCloud< PointSource >::Ptr
shared_ptr< PointCloud< PointSource > > Ptr
Definition point_cloud.h:413
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition point_cloud.h:414
pcl::Registration
Registration represents the base registration class for general purpose, ICP-like methods.
Definition registration.h:57
pcl::Registration::final_transformation_
Matrix4 final_transformation_
The final transformation matrix estimated by the registration method after N iterations.
Definition registration.h:573
pcl::Registration::transformation_
Matrix4 transformation_
The transformation matrix estimated by the registration method.
Definition registration.h:576
pcl::Registration::converged_
bool converged_
Holds internal convergence state, given user parameters.
Definition registration.h:613
pcl::Registration::target_
PointCloudTargetConstPtr target_
The input point cloud dataset target.
Definition registration.h:569
pcl
Definition convolution.h:46
pcl::uindex_t
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
Definition types.h:120
M_PI
#define M_PI
Definition pcl_macros.h:201
pcl::PPFHashMapSearch::HashKeyStruct
Data structure to hold the information for the key in the feature hash map of the PPFHashMapSearch cl...
Definition ppf_registration.h:56
pcl::PPFHashMapSearch::HashKeyStruct::HashKeyStruct
HashKeyStruct(int a, int b, int c, int d)
Definition ppf_registration.h:59
pcl::PPFHashMapSearch::HashKeyStruct::operator()
std::size_t operator()(const HashKeyStruct &s) const noexcept
Definition ppf_registration.h:68
pcl::PPFHashMapSearch::HashKeyStruct::HashKeyStruct
HashKeyStruct()=default
pcl::PPFRegistration::PoseWithVotes
Structure for storing a pose (represented as an Eigen::Affine3f) and an integer for counting votes.
Definition ppf_registration.h:182
pcl::PPFRegistration::PoseWithVotes::votes
unsigned int votes
Definition ppf_registration.h:188
pcl::PPFRegistration::PoseWithVotes::pose
Eigen::Affine3f pose
Definition ppf_registration.h:187
pcl::PPFRegistration::PoseWithVotes::PoseWithVotes
PoseWithVotes(const Eigen::Affine3f &a_pose, unsigned int &a_votes)
Definition ppf_registration.h:183