joint_icp.h
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2009-2012, 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.
*
*/
#pragma once
// PCL includes
#include <pcl/registration/icp.h>
namespace pcl {
/** \brief @b JointIterativeClosestPoint extends ICP to multiple frames which
* share the same transform. This is particularly useful when solving for
* camera extrinsics using multiple observations. When given a single pair of
* clouds, this reduces to vanilla ICP.
*
* \author Stephen Miller
* \ingroup registration
*/
template <typename PointSource, typename PointTarget, typename Scalar = float>
class JointIterativeClosestPoint
: public IterativeClosestPoint<PointSource, PointTarget, Scalar> {
public:
using PointCloudSource = typename IterativeClosestPoint<PointSource,
PointTarget,
Scalar>::PointCloudSource;
using PointCloudSourcePtr = typename PointCloudSource::Ptr;
using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
using PointCloudTarget = typename IterativeClosestPoint<PointSource,
PointTarget,
Scalar>::PointCloudTarget;
using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
using KdTree = pcl::search::KdTree<PointTarget>;
using KdTreePtr = typename KdTree::Ptr;
using KdTreeReciprocal = pcl::search::KdTree<PointSource>;
using KdTreeReciprocalPtr = typename KdTree::Ptr;
using PointIndicesPtr = PointIndices::Ptr;
using PointIndicesConstPtr = PointIndices::ConstPtr;
using Ptr = shared_ptr<JointIterativeClosestPoint<PointSource, PointTarget, Scalar>>;
using ConstPtr =
shared_ptr<const JointIterativeClosestPoint<PointSource, PointTarget, Scalar>>;
using CorrespondenceEstimation =
pcl::registration::CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>;
using CorrespondenceEstimationPtr = typename CorrespondenceEstimation::Ptr;
using CorrespondenceEstimationConstPtr = typename CorrespondenceEstimation::ConstPtr;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::reg_name_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::getClassName;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::setInputSource;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::input_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::indices_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::target_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::nr_iterations_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::max_iterations_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
previous_transformation_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::final_transformation_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::transformation_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
transformation_epsilon_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::converged_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::corr_dist_threshold_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::inlier_threshold_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
min_number_correspondences_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::update_visualizer_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
euclidean_fitness_epsilon_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::correspondences_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
transformation_estimation_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
correspondence_estimation_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
correspondence_rejectors_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
use_reciprocal_correspondence_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::convergence_criteria_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::source_has_normals_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::target_has_normals_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::need_source_blob_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::need_target_blob_;
using Matrix4 =
typename IterativeClosestPoint<PointSource, PointTarget, Scalar>::Matrix4;
/** \brief Empty constructor. */
JointIterativeClosestPoint()
{
IterativeClosestPoint<PointSource, PointTarget, Scalar>();
reg_name_ = "JointIterativeClosestPoint";
};
/** \brief Empty destructor */
~JointIterativeClosestPoint() {}
/** \brief Provide a pointer to the input source
* (e.g., the point cloud that we want to align to the target)
*/
void
setInputSource(const PointCloudSourceConstPtr& /*cloud*/) override
{
PCL_WARN("[pcl::%s::setInputSource] Warning; JointIterativeClosestPoint expects "
"multiple clouds. Please use addInputSource.\n",
getClassName().c_str());
return;
}
/** \brief Add a source cloud to the joint solver
*
* \param[in] cloud source cloud
*/
inline void
addInputSource(const PointCloudSourceConstPtr& cloud)
{
// Set the parent InputSource, just to get all cached values (e.g. the existence of
// normals).
if (sources_.empty())
IterativeClosestPoint<PointSource, PointTarget, Scalar>::setInputSource(cloud);
sources_.push_back(cloud);
}
/** \brief Provide a pointer to the input target
* (e.g., the point cloud that we want to align to the target)
*/
void
setInputTarget(const PointCloudTargetConstPtr& /*cloud*/) override
{
PCL_WARN("[pcl::%s::setInputTarget] Warning; JointIterativeClosestPoint expects "
"multiple clouds. Please use addInputTarget.\n",
getClassName().c_str());
return;
}
/** \brief Add a target cloud to the joint solver
*
* \param[in] cloud target cloud
*/
inline void
addInputTarget(const PointCloudTargetConstPtr& cloud)
{
// Set the parent InputTarget, just to get all cached values (e.g. the existence of
// normals).
if (targets_.empty())
IterativeClosestPoint<PointSource, PointTarget, Scalar>::setInputTarget(cloud);
targets_.push_back(cloud);
}
/** \brief Add a manual correspondence estimator
* If you choose to do this, you must add one for each
* input source / target pair. They do not need to have trees
* or input clouds set ahead of time.
*
* \param[in] ce Correspondence estimation
*/
inline void
addCorrespondenceEstimation(CorrespondenceEstimationPtr ce)
{
correspondence_estimations_.push_back(ce);
}
/** \brief Reset my list of input sources
*/
inline void
clearInputSources()
{
sources_.clear();
}
/** \brief Reset my list of input targets
*/
inline void
clearInputTargets()
{
targets_.clear();
}
/** \brief Reset my list of correspondence estimation methods.
*/
inline void
clearCorrespondenceEstimations()
{
correspondence_estimations_.clear();
}
protected:
/** \brief Rigid transformation computation method with initial guess.
* \param output the transformed input point cloud dataset using the rigid
* transformation found \param guess the initial guess of the transformation to
* compute
*/
void
computeTransformation(PointCloudSource& output, const Matrix4& guess) override;
/** \brief Looks at the Estimators and Rejectors and determines whether their
* blob-setter methods need to be called */
void
determineRequiredBlobData() override;
std::vector<PointCloudSourceConstPtr> sources_;
std::vector<PointCloudTargetConstPtr> targets_;
std::vector<CorrespondenceEstimationPtr> correspondence_estimations_;
};
} // namespace pcl
#include <pcl/registration/impl/joint_icp.hpp>
pcl::IterativeClosestPoint
IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm.
Definition: icp.h:97
pcl::IterativeClosestPoint::Matrix4
typename Registration< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
Definition: icp.h:142
pcl::IterativeClosestPoint< PointSource, PointTarget, float >::use_reciprocal_correspondence_
bool use_reciprocal_correspondence_
The correspondence type used for correspondence estimation.
Definition: icp.h:305
pcl::IterativeClosestPoint::setInputTarget
void setInputTarget(const PointCloudTargetConstPtr &cloud) override
Provide a pointer to the input target (e.g., the point cloud that we want to align to the target)
Definition: icp.h:240
pcl::IterativeClosestPoint::setInputSource
void setInputSource(const PointCloudSourceConstPtr &cloud) override
Provide a pointer to the input source (e.g., the point cloud that we want to align to the target)
Definition: icp.h:207
pcl::JointIterativeClosestPoint
JointIterativeClosestPoint extends ICP to multiple frames which share the same transform.
Definition: joint_icp.h:54
pcl::JointIterativeClosestPoint::targets_
std::vector< PointCloudTargetConstPtr > targets_
Definition: joint_icp.h:242
pcl::JointIterativeClosestPoint::PointCloudSourcePtr
typename PointCloudSource::Ptr PointCloudSourcePtr
Definition: joint_icp.h:59
pcl::JointIterativeClosestPoint::clearCorrespondenceEstimations
void clearCorrespondenceEstimations()
Reset my list of correspondence estimation methods.
Definition: joint_icp.h:222
pcl::JointIterativeClosestPoint::ConstPtr
shared_ptr< const JointIterativeClosestPoint< PointSource, PointTarget, Scalar > > ConstPtr
Definition: joint_icp.h:79
pcl::JointIterativeClosestPoint::addInputSource
void addInputSource(const PointCloudSourceConstPtr &cloud)
Add a source cloud to the joint solver.
Definition: joint_icp.h:155
pcl::JointIterativeClosestPoint::determineRequiredBlobData
void determineRequiredBlobData() override
Looks at the Estimators and Rejectors and determines whether their blob-setter methods need to be cal...
Definition: joint_icp.hpp:296
pcl::JointIterativeClosestPoint::PointCloudTarget
typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::PointCloudTarget PointCloudTarget
Definition: joint_icp.h:64
pcl::JointIterativeClosestPoint::KdTreeReciprocalPtr
typename KdTree::Ptr KdTreeReciprocalPtr
Definition: joint_icp.h:72
pcl::JointIterativeClosestPoint::computeTransformation
void computeTransformation(PointCloudSource &output, const Matrix4 &guess) override
Rigid transformation computation method with initial guess.
Definition: joint_icp.hpp:49
pcl::JointIterativeClosestPoint::PointCloudTargetConstPtr
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
Definition: joint_icp.h:66
pcl::JointIterativeClosestPoint::PointIndicesPtr
PointIndices::Ptr PointIndicesPtr
Definition: joint_icp.h:74
pcl::JointIterativeClosestPoint::clearInputSources
void clearInputSources()
Reset my list of input sources.
Definition: joint_icp.h:206
pcl::JointIterativeClosestPoint::PointCloudTargetPtr
typename PointCloudTarget::Ptr PointCloudTargetPtr
Definition: joint_icp.h:65
pcl::JointIterativeClosestPoint::sources_
std::vector< PointCloudSourceConstPtr > sources_
Definition: joint_icp.h:241
pcl::JointIterativeClosestPoint::CorrespondenceEstimationPtr
typename CorrespondenceEstimation::Ptr CorrespondenceEstimationPtr
Definition: joint_icp.h:83
pcl::JointIterativeClosestPoint::PointCloudSource
typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::PointCloudSource PointCloudSource
Definition: joint_icp.h:58
pcl::JointIterativeClosestPoint::addCorrespondenceEstimation
void addCorrespondenceEstimation(CorrespondenceEstimationPtr ce)
Add a manual correspondence estimator If you choose to do this, you must add one for each input sourc...
Definition: joint_icp.h:198
pcl::JointIterativeClosestPoint::PointIndicesConstPtr
PointIndices::ConstPtr PointIndicesConstPtr
Definition: joint_icp.h:75
pcl::JointIterativeClosestPoint::JointIterativeClosestPoint
JointIterativeClosestPoint()
Empty constructor.
Definition: joint_icp.h:129
pcl::JointIterativeClosestPoint::setInputTarget
void setInputTarget(const PointCloudTargetConstPtr &) override
Provide a pointer to the input target (e.g., the point cloud that we want to align to the target)
Definition: joint_icp.h:168
pcl::JointIterativeClosestPoint::CorrespondenceEstimationConstPtr
typename CorrespondenceEstimation::ConstPtr CorrespondenceEstimationConstPtr
Definition: joint_icp.h:84
pcl::JointIterativeClosestPoint::Ptr
shared_ptr< JointIterativeClosestPoint< PointSource, PointTarget, Scalar > > Ptr
Definition: joint_icp.h:77
pcl::JointIterativeClosestPoint::KdTreePtr
typename KdTree::Ptr KdTreePtr
Definition: joint_icp.h:69
pcl::JointIterativeClosestPoint::correspondence_estimations_
std::vector< CorrespondenceEstimationPtr > correspondence_estimations_
Definition: joint_icp.h:243
pcl::JointIterativeClosestPoint::PointCloudSourceConstPtr
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
Definition: joint_icp.h:60
pcl::JointIterativeClosestPoint::~JointIterativeClosestPoint
~JointIterativeClosestPoint()
Empty destructor.
Definition: joint_icp.h:136
pcl::JointIterativeClosestPoint::Matrix4
typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
Definition: joint_icp.h:126
pcl::JointIterativeClosestPoint::clearInputTargets
void clearInputTargets()
Reset my list of input targets.
Definition: joint_icp.h:214
pcl::JointIterativeClosestPoint::setInputSource
void setInputSource(const PointCloudSourceConstPtr &) override
Provide a pointer to the input source (e.g., the point cloud that we want to align to the target)
Definition: joint_icp.h:142
pcl::JointIterativeClosestPoint::addInputTarget
void addInputTarget(const PointCloudTargetConstPtr &cloud)
Add a target cloud to the joint solver.
Definition: joint_icp.h:181
pcl::Registration< PointSource, PointTarget, float >::reg_name_
std::string reg_name_
The registration method name.
Definition: registration.h:558
pcl::Registration< PointSource, PointTarget, float >::getClassName
const std::string & getClassName() const
Abstract class get name method.
Definition: registration.h:495
pcl::Registration< PointSource, PointTarget, float >::correspondence_estimation_
CorrespondenceEstimationPtr correspondence_estimation_
A CorrespondenceEstimation object, used to estimate correspondences between the source and the target...
Definition: registration.h:639
pcl::Registration< PointSource, PointTarget, float >::previous_transformation_
Matrix4 previous_transformation_
The previous transformation matrix estimated by the registration method (used internally).
Definition: registration.h:590
pcl::Registration< PointSource, PointTarget, float >::transformation_estimation_
TransformationEstimationPtr transformation_estimation_
A TransformationEstimation object, used to calculate the 4x4 rigid transformation.
Definition: registration.h:635
pcl::Registration< PointSource, PointTarget, float >::euclidean_fitness_epsilon_
double euclidean_fitness_epsilon_
The maximum allowed Euclidean error between two consecutive steps in the ICP loop,...
Definition: registration.h:607
pcl::Registration< PointSource, PointTarget, float >::transformation_epsilon_
double transformation_epsilon_
The maximum difference between two consecutive transformations in order to consider convergence (user...
Definition: registration.h:595
pcl::Registration< PointSource, PointTarget, float >::correspondence_rejectors_
std::vector< CorrespondenceRejectorPtr > correspondence_rejectors_
The list of correspondence rejectors to use.
Definition: registration.h:642
pcl::Registration< PointSource, PointTarget, float >::min_number_correspondences_
int min_number_correspondences_
The minimum number of correspondences that the algorithm needs before attempting to estimate the tran...
Definition: registration.h:628
pcl::registration::CorrespondenceEstimationBase
Abstract CorrespondenceEstimationBase class.
Definition: correspondence_estimation.h:60
pcl::registration::CorrespondenceEstimationBase::ConstPtr
shared_ptr< const CorrespondenceEstimationBase< PointSource, PointTarget, Scalar > > ConstPtr
Definition: correspondence_estimation.h:65
pcl::registration::CorrespondenceEstimationBase::Ptr
shared_ptr< CorrespondenceEstimationBase< PointSource, PointTarget, Scalar > > Ptr
Definition: correspondence_estimation.h:63
pcl::search::KdTree
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62
pcl::search::KdTree::Ptr
shared_ptr< KdTree< PointT, Tree > > Ptr
Definition: kdtree.h:75
pcl
Definition: convolution.h:46
pcl::PointIndices::Ptr
shared_ptr< ::pcl::PointIndices > Ptr
Definition: PointIndices.h:13
pcl::PointIndices::ConstPtr
shared_ptr< const ::pcl::PointIndices > ConstPtr
Definition: PointIndices.h:14