Loading...
Searching...
No Matches
correspondence_rejection.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/console/print.h>
#include <pcl/registration/correspondence_sorting.h>
#include <pcl/registration/correspondence_types.h>
#include <pcl/search/kdtree.h>
#include <pcl/point_cloud.h>
namespace pcl {
namespace registration {
/** @b CorrespondenceRejector represents the base class for correspondence rejection
* methods \author Dirk Holz \ingroup registration
*/
class CorrespondenceRejector {
public:
using Ptr = shared_ptr<CorrespondenceRejector>;
using ConstPtr = shared_ptr<const CorrespondenceRejector>;
/** \brief Empty constructor. */
CorrespondenceRejector() = default;
/** \brief Empty destructor. */
virtual ~CorrespondenceRejector() = default;
/** \brief Provide a pointer to the vector of the input correspondences.
* \param[in] correspondences the const shared pointer to a correspondence vector
*/
virtual inline void
setInputCorrespondences(const CorrespondencesConstPtr& correspondences)
{
input_correspondences_ = correspondences;
};
/** \brief Get a pointer to the vector of the input correspondences.
* \return correspondences the const shared pointer to a correspondence vector
*/
inline CorrespondencesConstPtr
getInputCorrespondences()
{
return input_correspondences_;
};
/** \brief Run correspondence rejection
* \param[out] correspondences Vector of correspondences that have not been rejected.
*/
inline void
getCorrespondences(pcl::Correspondences& correspondences)
{
if (!input_correspondences_ || (input_correspondences_->empty()))
return;
applyRejection(correspondences);
}
/** \brief Get a list of valid correspondences after rejection from the original set
* of correspondences. Pure virtual. Compared to \a getCorrespondences this function
* is stateless, i.e., input correspondences do not need to be provided beforehand,
* but are directly provided in the function call.
* \param[in] original_correspondences the set of initial correspondences given
* \param[out] remaining_correspondences the resultant filtered set of remaining
* correspondences
*/
virtual inline void
getRemainingCorrespondences(const pcl::Correspondences& original_correspondences,
pcl::Correspondences& remaining_correspondences) = 0;
/** \brief Determine the indices of query points of
* correspondences that have been rejected, i.e., the difference
* between the input correspondences (set via \a setInputCorrespondences)
* and the given correspondence vector.
* \param[in] correspondences Vector of correspondences after rejection
* \param[out] indices Vector of query point indices of those correspondences
* that have been rejected.
*/
inline void
getRejectedQueryIndices(const pcl::Correspondences& correspondences,
pcl::Indices& indices)
{
if (!input_correspondences_ || input_correspondences_->empty()) {
PCL_WARN("[pcl::registration::%s::getRejectedQueryIndices] Input correspondences "
"not set (lookup of rejected correspondences _not_ possible).\n",
getClassName().c_str());
return;
}
pcl::getRejectedQueryIndices(*input_correspondences_, correspondences, indices);
}
/** \brief Get a string representation of the name of this class. */
inline const std::string&
getClassName() const
{
return (rejection_name_);
}
/** \brief See if this rejector requires source points */
virtual bool
requiresSourcePoints() const
{
return (false);
}
/** \brief Abstract method for setting the source cloud */
virtual void
setSourcePoints(pcl::PCLPointCloud2::ConstPtr /*cloud2*/)
{
PCL_WARN("[pcl::registration::%s::setSourcePoints] This class does not require an "
"input source cloud\n",
getClassName().c_str());
}
/** \brief See if this rejector requires source normals */
virtual bool
requiresSourceNormals() const
{
return (false);
}
/** \brief Abstract method for setting the source normals */
virtual void
setSourceNormals(pcl::PCLPointCloud2::ConstPtr /*cloud2*/)
{
PCL_WARN("[pcl::registration::%s::setSourceNormals] This class does not require "
"input source normals\n",
getClassName().c_str());
}
/** \brief See if this rejector requires a target cloud */
virtual bool
requiresTargetPoints() const
{
return (false);
}
/** \brief Abstract method for setting the target cloud */
virtual void
setTargetPoints(pcl::PCLPointCloud2::ConstPtr /*cloud2*/)
{
PCL_WARN("[pcl::registration::%s::setTargetPoints] This class does not require an "
"input target cloud\n",
getClassName().c_str());
}
/** \brief See if this rejector requires target normals */
virtual bool
requiresTargetNormals() const
{
return (false);
}
/** \brief Abstract method for setting the target normals */
virtual void
setTargetNormals(pcl::PCLPointCloud2::ConstPtr /*cloud2*/)
{
PCL_WARN("[pcl::registration::%s::setTargetNormals] This class does not require "
"input target normals\n",
getClassName().c_str());
}
protected:
/** \brief The name of the rejection method. */
std::string rejection_name_{};
/** \brief The input correspondences. */
CorrespondencesConstPtr input_correspondences_;
/** \brief Abstract rejection method. */
virtual void
applyRejection(Correspondences& correspondences) = 0;
};
/** @b DataContainerInterface provides a generic interface for computing correspondence
* scores between correspondent points in the input and target clouds
* \ingroup registration
*/
class DataContainerInterface {
public:
using Ptr = shared_ptr<DataContainerInterface>;
using ConstPtr = shared_ptr<const DataContainerInterface>;
virtual ~DataContainerInterface() = default;
virtual double
getCorrespondenceScore(int index) = 0;
virtual double
getCorrespondenceScore(const pcl::Correspondence&) = 0;
virtual double
getCorrespondenceScoreFromNormals(const pcl::Correspondence&) = 0;
};
/** @b DataContainer is a container for the input and target point clouds and implements
* the interface to compute correspondence scores between correspondent points in the
* input and target clouds \ingroup registration
*/
template <typename PointT, typename NormalT = pcl::PointNormal>
class DataContainer : public DataContainerInterface {
using PointCloud = pcl::PointCloud<PointT>;
using PointCloudPtr = typename PointCloud::Ptr;
using PointCloudConstPtr = typename PointCloud::ConstPtr;
using KdTreePtr = typename pcl::search::KdTree<PointT>::Ptr;
using Normals = pcl::PointCloud<NormalT>;
using NormalsPtr = typename Normals::Ptr;
using NormalsConstPtr = typename Normals::ConstPtr;
public:
/** \brief Empty constructor. */
DataContainer(bool needs_normals = false)
: input_()
, input_transformed_()
, target_()
, input_normals_()
, input_normals_transformed_()
, target_normals_()
, tree_(new pcl::search::KdTree<PointT>)
, class_name_("DataContainer")
, needs_normals_(needs_normals)
{}
/** \brief Empty destructor */
~DataContainer() override = default;
/** \brief Provide a source point cloud dataset (must contain XYZ
* data!), used to compute the correspondence distance.
* \param[in] cloud a cloud containing XYZ data
*/
inline void
setInputSource(const PointCloudConstPtr& cloud)
{
input_ = cloud;
}
/** \brief Get a pointer to the input point cloud dataset target. */
inline PointCloudConstPtr const
getInputSource()
{
return (input_);
}
/** \brief Provide a target point cloud dataset (must contain XYZ
* data!), used to compute the correspondence distance.
* \param[in] target a cloud containing XYZ data
*/
inline void
setInputTarget(const PointCloudConstPtr& target)
{
target_ = target;
target_cloud_updated_ = true;
}
/** \brief Get a pointer to the input point cloud dataset target. */
inline PointCloudConstPtr const
getInputTarget()
{
return (target_);
}
/** \brief Provide a pointer to the search object used to find correspondences in
* the target cloud.
* \param[in] tree a pointer to the spatial search object.
* \param[in] force_no_recompute If set to true, this tree will NEVER be
* recomputed, regardless of calls to setInputTarget. Only use if you are
* confident that the tree will be set correctly.
*/
inline void
setSearchMethodTarget(const KdTreePtr& tree, bool force_no_recompute = false)
{
tree_ = tree;
force_no_recompute_ = force_no_recompute;
target_cloud_updated_ = true;
}
/** \brief Set the normals computed on the input point cloud
* \param[in] normals the normals computed for the input cloud
*/
inline void
setInputNormals(const NormalsConstPtr& normals)
{
input_normals_ = normals;
}
/** \brief Get the normals computed on the input point cloud */
inline NormalsConstPtr
getInputNormals()
{
return (input_normals_);
}
/** \brief Set the normals computed on the target point cloud
* \param[in] normals the normals computed for the input cloud
*/
inline void
setTargetNormals(const NormalsConstPtr& normals)
{
target_normals_ = normals;
}
/** \brief Get the normals computed on the target point cloud */
inline NormalsConstPtr
getTargetNormals()
{
return (target_normals_);
}
/** \brief Get the correspondence score for a point in the input cloud
* \param[in] index index of the point in the input cloud
*/
inline double
getCorrespondenceScore(int index) override
{
if (target_cloud_updated_ && !force_no_recompute_) {
tree_->setInputCloud(target_);
}
pcl::Indices indices(1);
std::vector<float> distances(1);
if (tree_->nearestKSearch((*input_)[index], 1, indices, distances))
return (distances[0]);
return (std::numeric_limits<double>::max());
}
/** \brief Get the correspondence score for a given pair of correspondent points
* \param[in] corr Correspondent points
*/
inline double
getCorrespondenceScore(const pcl::Correspondence& corr) override
{
// Get the source and the target feature from the list
const PointT& src = (*input_)[corr.index_query];
const PointT& tgt = (*target_)[corr.index_match];
return ((src.getVector4fMap() - tgt.getVector4fMap()).squaredNorm());
}
/** \brief Get the correspondence score for a given pair of correspondent points based
* on the angle between the normals. The normmals for the in put and target clouds
* must be set before using this function \param[in] corr Correspondent points
*/
inline double
getCorrespondenceScoreFromNormals(const pcl::Correspondence& corr) override
{
// assert ( (input_normals_->size () != 0) && (target_normals_->size () != 0) &&
// "Normals are not set for the input and target point clouds");
assert(input_normals_ && target_normals_ &&
"Normals are not set for the input and target point clouds");
const NormalT& src = (*input_normals_)[corr.index_query];
const NormalT& tgt = (*target_normals_)[corr.index_match];
return (static_cast<double>((src.normal[0] * tgt.normal[0]) +
(src.normal[1] * tgt.normal[1]) +
(src.normal[2] * tgt.normal[2])));
}
private:
/** \brief The input point cloud dataset */
PointCloudConstPtr input_;
/** \brief The input transformed point cloud dataset */
PointCloudPtr input_transformed_;
/** \brief The target point cloud dataset. */
PointCloudConstPtr target_;
/** \brief Normals to the input point cloud */
NormalsConstPtr input_normals_;
/** \brief Normals to the input point cloud */
NormalsPtr input_normals_transformed_;
/** \brief Normals to the target point cloud */
NormalsConstPtr target_normals_;
/** \brief A pointer to the spatial search object. */
KdTreePtr tree_;
/** \brief The name of the rejection method. */
std::string class_name_;
/** \brief Should the current data container use normals? */
bool needs_normals_;
/** \brief Variable that stores whether we have a new target cloud, meaning we need to
* pre-process it again. This way, we avoid rebuilding the kd-tree */
bool target_cloud_updated_{true};
/** \brief A flag which, if set, means the tree operating on the target cloud
* will never be recomputed*/
bool force_no_recompute_{false};
/** \brief Get a string representation of the name of this class. */
inline const std::string&
getClassName() const
{
return (class_name_);
}
};
} // namespace registration
} // namespace pcl
pcl::KdTree
KdTree represents the base spatial locator class for kd-tree implementations.
Definition kdtree.h:56
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition point_cloud.h:173
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition point_cloud.h:413
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition point_cloud.h:414
pcl::registration::CorrespondenceRejector
CorrespondenceRejector represents the base class for correspondence rejection methods
Definition correspondence_rejection.h:54
pcl::registration::CorrespondenceRejector::requiresSourceNormals
virtual bool requiresSourceNormals() const
See if this rejector requires source normals.
Definition correspondence_rejection.h:154
pcl::registration::CorrespondenceRejector::CorrespondenceRejector
CorrespondenceRejector()=default
Empty constructor.
pcl::registration::CorrespondenceRejector::setSourceNormals
virtual void setSourceNormals(pcl::PCLPointCloud2::ConstPtr)
Abstract method for setting the source normals.
Definition correspondence_rejection.h:161
pcl::registration::CorrespondenceRejector::getRemainingCorrespondences
virtual void getRemainingCorrespondences(const pcl::Correspondences &original_correspondences, pcl::Correspondences &remaining_correspondences)=0
Get a list of valid correspondences after rejection from the original set of correspondences.
pcl::registration::CorrespondenceRejector::setSourcePoints
virtual void setSourcePoints(pcl::PCLPointCloud2::ConstPtr)
Abstract method for setting the source cloud.
Definition correspondence_rejection.h:145
pcl::registration::CorrespondenceRejector::setTargetNormals
virtual void setTargetNormals(pcl::PCLPointCloud2::ConstPtr)
Abstract method for setting the target normals.
Definition correspondence_rejection.h:192
pcl::registration::CorrespondenceRejector::ConstPtr
shared_ptr< const CorrespondenceRejector > ConstPtr
Definition correspondence_rejection.h:57
pcl::registration::CorrespondenceRejector::applyRejection
virtual void applyRejection(Correspondences &correspondences)=0
Abstract rejection method.
pcl::registration::CorrespondenceRejector::getCorrespondences
void getCorrespondences(pcl::Correspondences &correspondences)
Run correspondence rejection.
Definition correspondence_rejection.h:87
pcl::registration::CorrespondenceRejector::requiresSourcePoints
virtual bool requiresSourcePoints() const
See if this rejector requires source points.
Definition correspondence_rejection.h:138
pcl::registration::CorrespondenceRejector::input_correspondences_
CorrespondencesConstPtr input_correspondences_
The input correspondences.
Definition correspondence_rejection.h:204
pcl::registration::CorrespondenceRejector::getRejectedQueryIndices
void getRejectedQueryIndices(const pcl::Correspondences &correspondences, pcl::Indices &indices)
Determine the indices of query points of correspondences that have been rejected, i....
Definition correspondence_rejection.h:116
pcl::registration::CorrespondenceRejector::Ptr
shared_ptr< CorrespondenceRejector > Ptr
Definition correspondence_rejection.h:56
pcl::registration::CorrespondenceRejector::requiresTargetPoints
virtual bool requiresTargetPoints() const
See if this rejector requires a target cloud.
Definition correspondence_rejection.h:169
pcl::registration::CorrespondenceRejector::requiresTargetNormals
virtual bool requiresTargetNormals() const
See if this rejector requires target normals.
Definition correspondence_rejection.h:185
pcl::registration::CorrespondenceRejector::rejection_name_
std::string rejection_name_
The name of the rejection method.
Definition correspondence_rejection.h:201
pcl::registration::CorrespondenceRejector::~CorrespondenceRejector
virtual ~CorrespondenceRejector()=default
Empty destructor.
pcl::registration::CorrespondenceRejector::getClassName
const std::string & getClassName() const
Get a string representation of the name of this class.
Definition correspondence_rejection.h:131
pcl::registration::CorrespondenceRejector::getInputCorrespondences
CorrespondencesConstPtr getInputCorrespondences()
Get a pointer to the vector of the input correspondences.
Definition correspondence_rejection.h:78
pcl::registration::CorrespondenceRejector::setTargetPoints
virtual void setTargetPoints(pcl::PCLPointCloud2::ConstPtr)
Abstract method for setting the target cloud.
Definition correspondence_rejection.h:176
pcl::registration::CorrespondenceRejector::setInputCorrespondences
virtual void setInputCorrespondences(const CorrespondencesConstPtr &correspondences)
Provide a pointer to the vector of the input correspondences.
Definition correspondence_rejection.h:69
pcl::registration::DataContainer
DataContainer is a container for the input and target point clouds and implements the interface to co...
Definition correspondence_rejection.h:234
pcl::registration::DataContainer::getTargetNormals
NormalsConstPtr getTargetNormals()
Get the normals computed on the target point cloud.
Definition correspondence_rejection.h:339
pcl::registration::DataContainer::setInputTarget
void setInputTarget(const PointCloudConstPtr &target)
Provide a target point cloud dataset (must contain XYZ data!), used to compute the correspondence dis...
Definition correspondence_rejection.h:284
pcl::registration::DataContainer::getInputSource
PointCloudConstPtr const getInputSource()
Get a pointer to the input point cloud dataset target.
Definition correspondence_rejection.h:274
pcl::registration::DataContainer::getCorrespondenceScore
double getCorrespondenceScore(int index) override
Get the correspondence score for a point in the input cloud.
Definition correspondence_rejection.h:348
pcl::registration::DataContainer::getCorrespondenceScore
double getCorrespondenceScore(const pcl::Correspondence &corr) override
Get the correspondence score for a given pair of correspondent points.
Definition correspondence_rejection.h:364
pcl::registration::DataContainer::getInputNormals
NormalsConstPtr getInputNormals()
Get the normals computed on the input point cloud.
Definition correspondence_rejection.h:323
pcl::registration::DataContainer::setInputSource
void setInputSource(const PointCloudConstPtr &cloud)
Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence dis...
Definition correspondence_rejection.h:267
pcl::registration::DataContainer::setTargetNormals
void setTargetNormals(const NormalsConstPtr &normals)
Set the normals computed on the target point cloud.
Definition correspondence_rejection.h:332
pcl::registration::DataContainer::getCorrespondenceScoreFromNormals
double getCorrespondenceScoreFromNormals(const pcl::Correspondence &corr) override
Get the correspondence score for a given pair of correspondent points based on the angle between the ...
Definition correspondence_rejection.h:378
pcl::registration::DataContainer::setSearchMethodTarget
void setSearchMethodTarget(const KdTreePtr &tree, bool force_no_recompute=false)
Provide a pointer to the search object used to find correspondences in the target cloud.
Definition correspondence_rejection.h:305
pcl::registration::DataContainer::DataContainer
DataContainer(bool needs_normals=false)
Empty constructor.
Definition correspondence_rejection.h:247
pcl::registration::DataContainer::setInputNormals
void setInputNormals(const NormalsConstPtr &normals)
Set the normals computed on the input point cloud.
Definition correspondence_rejection.h:316
pcl::registration::DataContainer::getInputTarget
PointCloudConstPtr const getInputTarget()
Get a pointer to the input point cloud dataset target.
Definition correspondence_rejection.h:292
pcl::registration::DataContainer::~DataContainer
~DataContainer() override=default
Empty destructor.
pcl::registration::DataContainerInterface
DataContainerInterface provides a generic interface for computing correspondence scores between corre...
Definition correspondence_rejection.h:215
pcl::registration::DataContainerInterface::getCorrespondenceScore
virtual double getCorrespondenceScore(int index)=0
pcl::registration::DataContainerInterface::~DataContainerInterface
virtual ~DataContainerInterface()=default
pcl::registration::DataContainerInterface::ConstPtr
shared_ptr< const DataContainerInterface > ConstPtr
Definition correspondence_rejection.h:218
pcl::registration::DataContainerInterface::Ptr
shared_ptr< DataContainerInterface > Ptr
Definition correspondence_rejection.h:217
pcl::registration::DataContainerInterface::getCorrespondenceScore
virtual double getCorrespondenceScore(const pcl::Correspondence &)=0
pcl::registration::DataContainerInterface::getCorrespondenceScoreFromNormals
virtual double getCorrespondenceScoreFromNormals(const pcl::Correspondence &)=0
pcl::search::KdTree::Ptr
shared_ptr< KdTree< PointT, Tree > > Ptr
Definition kdtree.h:75
pcl
Definition convolution.h:46
pcl::CorrespondencesConstPtr
shared_ptr< const Correspondences > CorrespondencesConstPtr
Definition correspondence.h:91
pcl::Correspondences
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Definition correspondence.h:89
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
pcl::getRejectedQueryIndices
void getRejectedQueryIndices(const pcl::Correspondences &correspondences_before, const pcl::Correspondences &correspondences_after, Indices &indices, bool presorting_required=true)
Get the query points of correspondences that are present in one correspondence vector but not in the ...
pcl::Correspondence
Correspondence represents a match between two entities (e.g., points, descriptors,...
Definition correspondence.h:61
pcl::Correspondence::index_query
index_t index_query
Index of the query (source) point.
Definition correspondence.h:63
pcl::Correspondence::index_match
index_t index_match
Index of the matching (target) point.
Definition correspondence.h:65
pcl::Normal
A point structure representing normal coordinates and the surface curvature estimate.
Definition point_types.hpp:823
pcl::PCLPointCloud2::ConstPtr
shared_ptr< const ::pcl::PCLPointCloud2 > ConstPtr
Definition PCLPointCloud2.h:36
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition point_types.hpp:623