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$
*
*/
#ifndef PCL_PPF_REGISTRATION_H_
#define PCL_PPF_REGISTRATION_H_
#include <pcl/registration/boost.h>
#include <pcl/registration/registration.h>
#include <pcl/features/ppf.h>
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(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;
}
};
typedef boost::unordered_multimap<HashKeyStruct, std::pair<size_t, size_t> > FeatureHashMapType;
typedef boost::shared_ptr<FeatureHashMapType> FeatureHashMapTypePtr;
typedef boost::shared_ptr<PPFHashMapSearch> Ptr;
/** \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)
: alpha_m_ ()
, feature_hash_map_ (new FeatureHashMapType)
, internals_initialized_ (false)
, angle_discretization_step_ (angle_discretization_step)
, distance_discretization_step_ (distance_discretization_step)
, max_dist_ (-1.0f)
{
}
/** \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<size_t, size_t> > &indices);
/** \brief Convenience method for returning a copy of the class instance as a boost::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 () { 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 () { return distance_discretization_step_; }
/** \brief Returns the maximum distance found between any feature pair in the given input feature cloud */
inline float
getModelDiameter () { return max_dist_; }
std::vector <std::vector <float> > alpha_m_;
private:
FeatureHashMapTypePtr feature_hash_map_;
bool internals_initialized_;
float angle_discretization_step_, distance_discretization_step_;
float max_dist_;
};
/** \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
*
* \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(Eigen::Affine3f &a_pose, unsigned int &a_votes)
: pose (a_pose),
votes (a_votes)
{}
Eigen::Affine3f pose;
unsigned int votes;
};
typedef std::vector<PoseWithVotes, Eigen::aligned_allocator<PoseWithVotes> > PoseWithVotesList;
/// 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_;
typedef pcl::PointCloud<PointSource> PointCloudSource;
typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr;
typedef pcl::PointCloud<PointTarget> PointCloudTarget;
typedef typename PointCloudTarget::Ptr PointCloudTargetPtr;
typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr;
/** \brief Empty constructor that initializes all the parameters of the algorithm with default values */
PPFRegistration ()
: Registration<PointSource, PointTarget> (),
search_method_ (),
scene_reference_point_sampling_rate_ (5),
clustering_position_diff_threshold_ (0.01f),
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);
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);
/** \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 */
unsigned int scene_reference_point_sampling_rate_;
/** \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_, 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 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<size_t, unsigned int> &a,
const std::pair<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);
};
}
#include <pcl/registration/impl/ppf_registration.hpp>
#endif // PCL_PPF_REGISTRATION_H_
pcl::PPFRegistration::PointCloudTargetConstPtr
PointCloudTarget::ConstPtr PointCloudTargetConstPtr
Definition: ppf_registration.h:177
pcl::PPFRegistration::PoseWithVotes::PoseWithVotes
PoseWithVotes(Eigen::Affine3f &a_pose, unsigned int &a_votes)
Definition: ppf_registration.h:153
pcl::PPFHashMapSearch::makeShared
Ptr makeShared()
Convenience method for returning a copy of the class instance as a boost::shared_ptr.
Definition: ppf_registration.h:109
pcl::PPFHashMapSearch
Definition: ppf_registration.h:51
pcl::PPFRegistration::PoseWithVotes::pose
Eigen::Affine3f pose
Definition: ppf_registration.h:158
pcl
Definition: convolution.h:45
pcl::PPFHashMapSearch::getModelDiameter
float getModelDiameter()
Returns the maximum distance found between any feature pair in the given input feature cloud...
Definition: ppf_registration.h:121
pcl::PPFRegistration::getSearchMethod
PPFHashMapSearch::Ptr getSearchMethod()
Getter function for the search method of the class.
Definition: ppf_registration.h:235
pcl::PPFRegistration::setRotationClusteringThreshold
void setRotationClusteringThreshold(float clustering_rotation_diff_threshold)
Method for setting the rotation clustering parameter.
Definition: ppf_registration.h:208
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:219
pcl::PPFHashMapSearch::alpha_m_
std::vector< std::vector< float > > alpha_m_
Definition: ppf_registration.h:123
pcl::PPFRegistration::PointCloudTargetPtr
PointCloudTarget::Ptr PointCloudTargetPtr
Definition: ppf_registration.h:176
pcl::PPFHashMapSearch::FeatureHashMapType
boost::unordered_multimap< HashKeyStruct, std::pair< size_t, size_t > > FeatureHashMapType
Definition: ppf_registration.h:69
pcl::PPFRegistration::setPositionClusteringThreshold
void setPositionClusteringThreshold(float clustering_position_diff_threshold)
Method for setting the position difference clustering parameter.
Definition: ppf_registration.h:194
pcl::PPFRegistration::PointCloudTarget
pcl::PointCloud< PointTarget > PointCloudTarget
Definition: ppf_registration.h:175
pcl::PointCloud< PointSource >::Ptr
boost::shared_ptr< PointCloud< PointSource > > Ptr
Definition: point_cloud.h:428
pcl::PPFHashMapSearch::Ptr
boost::shared_ptr< PPFHashMapSearch > Ptr
Definition: ppf_registration.h:71
pcl::PPFRegistration::setSearchMethod
void setSearchMethod(PPFHashMapSearch::Ptr search_method)
Function that sets the search method for the algorithm.
Definition: ppf_registration.h:231
pcl::PPFHashMapSearch::getAngleDiscretizationStep
float getAngleDiscretizationStep()
Returns the angle discretization step parameter (the step value between each bin of the hash map for ...
Definition: ppf_registration.h:113
pcl::PPFRegistration::getRotationClusteringThreshold
float getRotationClusteringThreshold()
Returns the parameter defining the rotation clustering threshold.
Definition: ppf_registration.h:213
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:78
pcl::PointCloud::ConstPtr
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
pcl::PPFRegistration::PointCloudSourceConstPtr
PointCloudSource::ConstPtr PointCloudSourceConstPtr
Definition: ppf_registration.h:173
pcl::Registration
Registration represents the base registration class for general purpose, ICP-like methods...
Definition: registration.h:62
pcl::PointCloud< PointSource >
pcl::PPFHashMapSearch::FeatureHashMapTypePtr
boost::shared_ptr< FeatureHashMapType > FeatureHashMapTypePtr
Definition: ppf_registration.h:70
pcl::PPFRegistration::PoseWithVotes::votes
unsigned int votes
Definition: ppf_registration.h:159
pcl::PPFRegistration::getSceneReferencePointSamplingRate
unsigned int getSceneReferencePointSamplingRate()
Returns the parameter for the scene reference point sampling rate of the algorithm.
Definition: ppf_registration.h:223
pcl::PPFRegistration::getPositionClusteringThreshold
float getPositionClusteringThreshold()
Returns the parameter defining the position difference clustering parameter - distance threshold belo...
Definition: ppf_registration.h:201
pcl::PPFRegistration::PoseWithVotes
Structure for storing a pose (represented as an Eigen::Affine3f) and an integer for counting votes...
Definition: ppf_registration.h:151
pcl::PPFRegistration::PoseWithVotesList
std::vector< PoseWithVotes, Eigen::aligned_allocator< PoseWithVotes > > PoseWithVotesList
Definition: ppf_registration.h:161
pcl::PPFHashMapSearch::getDistanceDiscretizationStep
float getDistanceDiscretizationStep()
Returns the distance discretization step parameter (the step value between each bin of the hash map f...
Definition: ppf_registration.h:117
pcl::PPFRegistration
Class that registers two point clouds based on their sets of PPFSignatures.
Definition: ppf_registration.h:144
pcl::PPFRegistration::PPFRegistration
PPFRegistration()
Empty constructor that initializes all the parameters of the algorithm with default values...
Definition: ppf_registration.h:181
pcl::PPFRegistration::PointCloudSourcePtr
PointCloudSource::Ptr PointCloudSourcePtr
Definition: ppf_registration.h:172
pcl::PPFHashMapSearch::HashKeyStruct::HashKeyStruct
HashKeyStruct(int a, int b, int c, int d)
Definition: ppf_registration.h:61
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:59
pcl::KdTreeFLANN::Ptr
boost::shared_ptr< KdTreeFLANN< PointT > > Ptr
Definition: kdtree_flann.h:89
pcl::PPFRegistration::PointCloudSource
pcl::PointCloud< PointSource > PointCloudSource
Definition: ppf_registration.h:171