249 lines
9.7 KiB
C
249 lines
9.7 KiB
C
|
/*
|
||
|
|
* 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>
|