correspondence_rejection_features.h
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2011, 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/registration/correspondence_rejection.h>
#include <pcl/point_cloud.h>
#include <pcl/point_representation.h>
#include <pcl/registration/boost.h>
#include <unordered_map>
namespace pcl
{
namespace registration
{
/** \brief CorrespondenceRejectorFeatures implements a correspondence rejection method based on a set of feature
* descriptors. Given an input feature space, the method checks if each feature in the source cloud has a
* correspondence in the target cloud, either by checking the first K (given) point correspondences, or
* by defining a tolerance threshold via a radius in feature space.
* \todo explain this better.
* \author Radu B. Rusu
* \ingroup registration
*/
class PCL_EXPORTS CorrespondenceRejectorFeatures: public CorrespondenceRejector
{
using CorrespondenceRejector::input_correspondences_;
using CorrespondenceRejector::rejection_name_;
using CorrespondenceRejector::getClassName;
public:
using Ptr = shared_ptr<CorrespondenceRejectorFeatures>;
using ConstPtr = shared_ptr<const CorrespondenceRejectorFeatures>;
/** \brief Empty constructor. */
CorrespondenceRejectorFeatures () : max_distance_ (std::numeric_limits<float>::max ())
{
rejection_name_ = "CorrespondenceRejectorFeatures";
}
/** \brief Empty destructor. */
~CorrespondenceRejectorFeatures () {}
/** \brief Get a list of valid correspondences after rejection from the original set of correspondences
* \param[in] original_correspondences the set of initial correspondences given
* \param[out] remaining_correspondences the resultant filtered set of remaining correspondences
*/
void
getRemainingCorrespondences (const pcl::Correspondences& original_correspondences,
pcl::Correspondences& remaining_correspondences) override;
/** \brief Provide a pointer to a cloud of feature descriptors associated with the source point cloud
* \param[in] source_feature a cloud of feature descriptors associated with the source point cloud
* \param[in] key a string that uniquely identifies the feature
*/
template <typename FeatureT> inline void
setSourceFeature (const typename pcl::PointCloud<FeatureT>::ConstPtr &source_feature,
const std::string &key);
/** \brief Get a pointer to the source cloud's feature descriptors, specified by the given \a key
* \param[in] key a string that uniquely identifies the feature (must match the key provided by setSourceFeature)
*/
template <typename FeatureT> inline typename pcl::PointCloud<FeatureT>::ConstPtr
getSourceFeature (const std::string &key);
/** \brief Provide a pointer to a cloud of feature descriptors associated with the target point cloud
* \param[in] target_feature a cloud of feature descriptors associated with the target point cloud
* \param[in] key a string that uniquely identifies the feature
*/
template <typename FeatureT> inline void
setTargetFeature (const typename pcl::PointCloud<FeatureT>::ConstPtr &target_feature,
const std::string &key);
/** \brief Get a pointer to the source cloud's feature descriptors, specified by the given \a key
* \param[in] key a string that uniquely identifies the feature (must match the key provided by setTargetFeature)
*/
template <typename FeatureT> inline typename pcl::PointCloud<FeatureT>::ConstPtr
getTargetFeature (const std::string &key);
/** \brief Set a hard distance threshold in the feature \a FeatureT space, between source and target
* features. Any feature correspondence that is above this threshold will be considered bad and will be
* filtered out.
* \param[in] thresh the distance threshold
* \param[in] key a string that uniquely identifies the feature
*/
template <typename FeatureT> inline void
setDistanceThreshold (double thresh, const std::string &key);
/** \brief Test that all features are valid (i.e., does each key have a valid source cloud, target cloud,
* and search method)
*/
inline bool
hasValidFeatures ();
/** \brief Provide a boost shared pointer to a PointRepresentation to be used when comparing features
* \param[in] key a string that uniquely identifies the feature
* \param[in] fr the point feature representation to be used
*/
template <typename FeatureT> inline void
setFeatureRepresentation (const typename pcl::PointRepresentation<FeatureT>::ConstPtr &fr,
const std::string &key);
protected:
/** \brief Apply the rejection algorithm.
* \param[out] correspondences the set of resultant correspondences.
*/
inline void
applyRejection (pcl::Correspondences &correspondences) override
{
getRemainingCorrespondences (*input_correspondences_, correspondences);
}
/** \brief The maximum distance threshold between two correspondent points in source <-> target. If the
* distance is larger than this threshold, the points will not be ignored in the alignment process.
*/
float max_distance_;
class FeatureContainerInterface
{
public:
/** \brief Empty destructor */
virtual ~FeatureContainerInterface () = default;
virtual bool isValid () = 0;
virtual double getCorrespondenceScore (int index) = 0;
virtual bool isCorrespondenceValid (int index) = 0;
using Ptr = shared_ptr<FeatureContainerInterface>;
};
using FeaturesMap = std::unordered_map<std::string, FeatureContainerInterface::Ptr>;
/** \brief An STL map containing features to use when performing the correspondence search.*/
FeaturesMap features_map_;
/** \brief An inner class containing pointers to the source and target feature clouds
* and the parameters needed to perform the correspondence search. This class extends
* FeatureContainerInterface, which contains abstract methods for any methods that do not depend on the
* FeatureT --- these methods can thus be called from a pointer to FeatureContainerInterface without
* casting to the derived class.
*/
template <typename FeatureT>
class FeatureContainer : public pcl::registration::CorrespondenceRejectorFeatures::FeatureContainerInterface
{
public:
using FeatureCloudConstPtr = typename pcl::PointCloud<FeatureT>::ConstPtr;
using SearchMethod = std::function<int (const pcl::PointCloud<FeatureT> &, int, std::vector<int> &, std::vector<float> &)>;
using PointRepresentationConstPtr = typename pcl::PointRepresentation<FeatureT>::ConstPtr;
FeatureContainer () : thresh_(std::numeric_limits<double>::max ()), feature_representation_()
{
}
/** \brief Empty destructor */
~FeatureContainer () {}
inline void
setSourceFeature (const FeatureCloudConstPtr &source_features)
{
source_features_ = source_features;
}
inline FeatureCloudConstPtr
getSourceFeature ()
{
return (source_features_);
}
inline void
setTargetFeature (const FeatureCloudConstPtr &target_features)
{
target_features_ = target_features;
}
inline FeatureCloudConstPtr
getTargetFeature ()
{
return (target_features_);
}
inline void
setDistanceThreshold (double thresh)
{
thresh_ = thresh;
}
inline bool
isValid () override
{
if (!source_features_ || !target_features_)
return (false);
return (source_features_->points.size () > 0 &&
target_features_->points.size () > 0);
}
/** \brief Provide a boost shared pointer to a PointRepresentation to be used when comparing features
* \param[in] fr the point feature representation to be used
*/
inline void
setFeatureRepresentation (const PointRepresentationConstPtr &fr)
{
feature_representation_ = fr;
}
/** \brief Obtain a score between a pair of correspondences.
* \param[in] index the index to check in the list of correspondences
* \return score the resultant computed score
*/
inline double
getCorrespondenceScore (int index) override
{
// If no feature representation was given, reset to the default implementation for FeatureT
if (!feature_representation_)
feature_representation_.reset (new DefaultFeatureRepresentation<FeatureT>);
// Get the source and the target feature from the list
const FeatureT &feat_src = source_features_->points[index];
const FeatureT &feat_tgt = target_features_->points[index];
// Check if the representations are valid
if (!feature_representation_->isValid (feat_src) || !feature_representation_->isValid (feat_tgt))
{
PCL_ERROR ("[pcl::registration::%s::getCorrespondenceScore] Invalid feature representation given!\n", this->getClassName ().c_str ());
return (std::numeric_limits<double>::max ());
}
// Set the internal feature point representation of choice
Eigen::VectorXf feat_src_ptr = Eigen::VectorXf::Zero (feature_representation_->getNumberOfDimensions ());
feature_representation_->vectorize (FeatureT (feat_src), feat_src_ptr);
Eigen::VectorXf feat_tgt_ptr = Eigen::VectorXf::Zero (feature_representation_->getNumberOfDimensions ());
feature_representation_->vectorize (FeatureT (feat_tgt), feat_tgt_ptr);
// Compute the L2 norm
return ((feat_src_ptr - feat_tgt_ptr).squaredNorm ());
}
/** \brief Check whether the correspondence pair at the given index is valid
* by computing the score and testing it against the user given threshold
* \param[in] index the index to check in the list of correspondences
* \return true if the correspondence is good, false otherwise
*/
inline bool
isCorrespondenceValid (int index) override
{
return (getCorrespondenceScore (index) < thresh_ * thresh_);
}
private:
FeatureCloudConstPtr source_features_, target_features_;
SearchMethod search_method_;
/** \brief The L2 squared Euclidean threshold. */
double thresh_;
/** \brief The internal point feature representation used. */
PointRepresentationConstPtr feature_representation_;
};
};
}
}
#include <pcl/registration/impl/correspondence_rejection_features.hpp>
pcl::registration::CorrespondenceRejector::Ptr
shared_ptr< CorrespondenceRejector > Ptr
Definition: correspondence_rejection.h:61
pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainer::setDistanceThreshold
void setDistanceThreshold(double thresh)
Definition: correspondence_rejection_features.h:220
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainer::isCorrespondenceValid
bool isCorrespondenceValid(int index) override
Check whether the correspondence pair at the given index is valid by computing the score and testing ...
Definition: correspondence_rejection_features.h:281
pcl::registration::CorrespondenceRejector::ConstPtr
shared_ptr< const CorrespondenceRejector > ConstPtr
Definition: correspondence_rejection.h:62
pcl::registration::CorrespondenceRejector::getClassName
const std::string & getClassName() const
Get a string representation of the name of this class.
Definition: correspondence_rejection.h:132
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainer::FeatureCloudConstPtr
typename pcl::PointCloud< FeatureT >::ConstPtr FeatureCloudConstPtr
Definition: correspondence_rejection_features.h:183
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainer::setTargetFeature
void setTargetFeature(const FeatureCloudConstPtr &target_features)
Definition: correspondence_rejection_features.h:208
pcl::registration::CorrespondenceRejectorFeatures::~CorrespondenceRejectorFeatures
~CorrespondenceRejectorFeatures()
Empty destructor.
Definition: correspondence_rejection_features.h:79
pcl::PointRepresentation::ConstPtr
shared_ptr< const PointRepresentation< PointT > > ConstPtr
Definition: point_representation.h:77
pcl::PointCloud< FeatureT >
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainer::getTargetFeature
FeatureCloudConstPtr getTargetFeature()
Definition: correspondence_rejection_features.h:214
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainer::getSourceFeature
FeatureCloudConstPtr getSourceFeature()
Definition: correspondence_rejection_features.h:202
pcl::registration::CorrespondenceRejectorFeatures
CorrespondenceRejectorFeatures implements a correspondence rejection method based on a set of feature...
Definition: correspondence_rejection_features.h:62
pcl::DefaultFeatureRepresentation
DefaulFeatureRepresentation extends PointRepresentation and is intended to be used when defining the ...
Definition: point_representation.h:219
pcl::registration::CorrespondenceRejectorFeatures::features_map_
FeaturesMap features_map_
An STL map containing features to use when performing the correspondence search.
Definition: correspondence_rejection_features.h:171
pcl::registration::CorrespondenceRejectorFeatures::max_distance_
float max_distance_
The maximum distance threshold between two correspondent points in source <-> target.
Definition: correspondence_rejection_features.h:154
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainer
An inner class containing pointers to the source and target feature clouds and the parameters needed ...
Definition: correspondence_rejection_features.h:180
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainerInterface::Ptr
shared_ptr< FeatureContainerInterface > Ptr
Definition: correspondence_rejection_features.h:165
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainer::~FeatureContainer
~FeatureContainer()
Empty destructor.
Definition: correspondence_rejection_features.h:193
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainer::isValid
bool isValid() override
Definition: correspondence_rejection_features.h:226
pcl::registration::CorrespondenceRejectorFeatures::CorrespondenceRejectorFeatures
CorrespondenceRejectorFeatures()
Empty constructor.
Definition: correspondence_rejection_features.h:73
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainer::setSourceFeature
void setSourceFeature(const FeatureCloudConstPtr &source_features)
Definition: correspondence_rejection_features.h:196
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainer::SearchMethod
std::function< int(const pcl::PointCloud< FeatureT > &, int, std::vector< int > &, std::vector< float > &)> SearchMethod
Definition: correspondence_rejection_features.h:184
pcl::registration::CorrespondenceRejectorFeatures::FeaturesMap
std::unordered_map< std::string, FeatureContainerInterface::Ptr > FeaturesMap
Definition: correspondence_rejection_features.h:168
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:416
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainer::getCorrespondenceScore
double getCorrespondenceScore(int index) override
Obtain a score between a pair of correspondences.
Definition: correspondence_rejection_features.h:248
pcl::Correspondences
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Definition: correspondence.h:88
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainerInterface
Definition: correspondence_rejection_features.h:156
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainer::setFeatureRepresentation
void setFeatureRepresentation(const PointRepresentationConstPtr &fr)
Provide a boost shared pointer to a PointRepresentation to be used when comparing features.
Definition: correspondence_rejection_features.h:238
pcl::registration::CorrespondenceRejector::input_correspondences_
CorrespondencesConstPtr input_correspondences_
The input correspondences.
Definition: correspondence_rejection.h:188
pcl::registration::CorrespondenceRejector::rejection_name_
std::string rejection_name_
The name of the rejection method.
Definition: correspondence_rejection.h:185
pcl::registration::CorrespondenceRejectorFeatures::applyRejection
void applyRejection(pcl::Correspondences &correspondences) override
Apply the rejection algorithm.
Definition: correspondence_rejection_features.h:146
PCL_EXPORTS
#define PCL_EXPORTS
Definition: pcl_macros.h:253
pcl::registration::CorrespondenceRejector
CorrespondenceRejector represents the base class for correspondence rejection methods
Definition: correspondence_rejection.h:58
pcl::shared_ptr
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
Definition: pcl_macros.h:90
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainer::PointRepresentationConstPtr
typename pcl::PointRepresentation< FeatureT >::ConstPtr PointRepresentationConstPtr
Definition: correspondence_rejection_features.h:186
pcl::registration::CorrespondenceRejectorFeatures::FeatureContainer::FeatureContainer
FeatureContainer()
Definition: correspondence_rejection_features.h:188