349 lines
13 KiB
C++
349 lines
13 KiB
C++
/*
|
|
* 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)
|
|
, 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<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_;
|
|
|
|
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;
|
|
};
|
|
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>()
|
|
, 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) override;
|
|
|
|
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_;
|
|
|
|
/** \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<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);
|
|
};
|
|
} // namespace pcl
|
|
|
|
#include <pcl/registration/impl/ppf_registration.hpp>
|