joint_icp.hpp
/*
* 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.
*
*/
#ifndef PCL_REGISTRATION_IMPL_JOINT_ICP_HPP_
#define PCL_REGISTRATION_IMPL_JOINT_ICP_HPP_
#include <pcl/registration/boost.h>
#include <pcl/correspondence.h>
#include <pcl/console/print.h>
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::JointIterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformation (
PointCloudSource &output, const Matrix4 &guess)
{
// Point clouds containing the correspondences of each point in <input, indices>
if (sources_.size () != targets_.size () || sources_.empty () || targets_.empty ())
{
PCL_ERROR ("[pcl::%s::computeTransformation] Must set InputSources and InputTargets to the same, nonzero size!\n",
getClassName ().c_str ());
return;
}
bool manual_correspondence_estimations_set = true;
if (correspondence_estimations_.empty ())
{
manual_correspondence_estimations_set = false;
correspondence_estimations_.resize (sources_.size ());
for (size_t i = 0; i < sources_.size (); i++)
{
correspondence_estimations_[i] = correspondence_estimation_->clone ();
KdTreeReciprocalPtr src_tree (new KdTreeReciprocal);
KdTreePtr tgt_tree (new KdTree);
correspondence_estimations_[i]->setSearchMethodTarget (tgt_tree);
correspondence_estimations_[i]->setSearchMethodSource (src_tree);
}
}
if (correspondence_estimations_.size () != sources_.size ())
{
PCL_ERROR ("[pcl::%s::computeTransform] Must set CorrespondenceEstimations to be the same size as the joint\n",
getClassName ().c_str ());
return;
}
std::vector<PointCloudSourcePtr> inputs_transformed (sources_.size ());
for (size_t i = 0; i < sources_.size (); i++)
{
inputs_transformed[i].reset (new PointCloudSource);
}
nr_iterations_ = 0;
converged_ = false;
// Initialise final transformation to the guessed one
final_transformation_ = guess;
// Make a combined transformed input and output
std::vector<size_t> input_offsets (sources_.size ());
std::vector<size_t> target_offsets (targets_.size ());
PointCloudSourcePtr sources_combined (new PointCloudSource);
PointCloudSourcePtr inputs_transformed_combined (new PointCloudSource);
PointCloudTargetPtr targets_combined (new PointCloudTarget);
size_t input_offset = 0;
size_t target_offset = 0;
for (size_t i = 0; i < sources_.size (); i++)
{
// If the guessed transformation is non identity
if (guess != Matrix4::Identity ())
{
// Apply guessed transformation prior to search for neighbours
this->transformCloud (*sources_[i], *inputs_transformed[i], guess);
}
else
{
*inputs_transformed[i] = *sources_[i];
}
*sources_combined += *sources_[i];
*inputs_transformed_combined += *inputs_transformed[i];
*targets_combined += *targets_[i];
input_offsets[i] = input_offset;
target_offsets[i] = target_offset;
input_offset += inputs_transformed[i]->size ();
target_offset += targets_[i]->size ();
}
transformation_ = Matrix4::Identity ();
// Make blobs if necessary
determineRequiredBlobData ();
// Pass in the default target for the Correspondence Estimation/Rejection code
for (size_t i = 0; i < sources_.size (); i++)
{
correspondence_estimations_[i]->setInputTarget (targets_[i]);
if (correspondence_estimations_[i]->requiresTargetNormals ())
{
PCLPointCloud2::Ptr target_blob (new PCLPointCloud2);
pcl::toPCLPointCloud2 (*targets_[i], *target_blob);
correspondence_estimations_[i]->setTargetNormals (target_blob);
}
}
PCLPointCloud2::Ptr targets_combined_blob (new PCLPointCloud2);
if (!correspondence_rejectors_.empty () && need_target_blob_)
pcl::toPCLPointCloud2 (*targets_combined, *targets_combined_blob);
for (size_t i = 0; i < correspondence_rejectors_.size (); ++i)
{
registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
if (rej->requiresTargetPoints ())
rej->setTargetPoints (targets_combined_blob);
if (rej->requiresTargetNormals () && target_has_normals_)
rej->setTargetNormals (targets_combined_blob);
}
convergence_criteria_->setMaximumIterations (max_iterations_);
convergence_criteria_->setRelativeMSE (euclidean_fitness_epsilon_);
convergence_criteria_->setTranslationThreshold (transformation_epsilon_);
convergence_criteria_->setRotationThreshold (1.0 - transformation_epsilon_);
// Repeat until convergence
std::vector<CorrespondencesPtr> partial_correspondences_ (sources_.size ());
for (size_t i = 0; i < sources_.size (); i++)
{
partial_correspondences_[i].reset (new pcl::Correspondences);
}
do
{
// Save the previously estimated transformation
previous_transformation_ = transformation_;
// Set the source each iteration, to ensure the dirty flag is updated
correspondences_->clear ();
for (size_t i = 0; i < correspondence_estimations_.size (); i++)
{
correspondence_estimations_[i]->setInputSource (inputs_transformed[i]);
// Get blob data if needed
if (correspondence_estimations_[i]->requiresSourceNormals ())
{
PCLPointCloud2::Ptr input_transformed_blob (new PCLPointCloud2);
toPCLPointCloud2 (*inputs_transformed[i], *input_transformed_blob);
correspondence_estimations_[i]->setSourceNormals (input_transformed_blob);
}
// Estimate correspondences on each cloud pair separately
if (use_reciprocal_correspondence_)
{
correspondence_estimations_[i]->determineReciprocalCorrespondences (*partial_correspondences_[i], corr_dist_threshold_);
}
else
{
correspondence_estimations_[i]->determineCorrespondences (*partial_correspondences_[i], corr_dist_threshold_);
}
PCL_DEBUG ("[pcl::%s::computeTransformation] Found %d partial correspondences for cloud [%d]\n",
getClassName ().c_str (),
partial_correspondences_[i]->size (), i);
for (size_t j = 0; j < partial_correspondences_[i]->size (); j++)
{
pcl::Correspondence corr = partial_correspondences_[i]->at (j);
// Update the offsets to be for the combined clouds
corr.index_query += input_offsets[i];
corr.index_match += target_offsets[i];
correspondences_->push_back (corr);
}
}
PCL_DEBUG ("[pcl::%s::computeTransformation] Total correspondences: %d\n", getClassName ().c_str (), correspondences_->size ());
PCLPointCloud2::Ptr inputs_transformed_combined_blob;
if (need_source_blob_)
{
inputs_transformed_combined_blob.reset (new PCLPointCloud2);
toPCLPointCloud2 (*inputs_transformed_combined, *inputs_transformed_combined_blob);
}
CorrespondencesPtr temp_correspondences (new Correspondences (*correspondences_));
for (size_t i = 0; i < correspondence_rejectors_.size (); ++i)
{
PCL_DEBUG ("Applying a correspondence rejector method: %s.\n", correspondence_rejectors_[i]->getClassName ().c_str ());
registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
PCL_DEBUG ("Applying a correspondence rejector method: %s.\n", rej->getClassName ().c_str ());
if (rej->requiresSourcePoints ())
rej->setSourcePoints (inputs_transformed_combined_blob);
if (rej->requiresSourceNormals () && source_has_normals_)
rej->setSourceNormals (inputs_transformed_combined_blob);
rej->setInputCorrespondences (temp_correspondences);
rej->getCorrespondences (*correspondences_);
// Modify input for the next iteration
if (i < correspondence_rejectors_.size () - 1)
*temp_correspondences = *correspondences_;
}
int cnt = correspondences_->size ();
// Check whether we have enough correspondences
if (cnt < min_number_correspondences_)
{
PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
convergence_criteria_->setConvergenceState(pcl::registration::DefaultConvergenceCriteria<Scalar>::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES);
converged_ = false;
break;
}
// Estimate the transform jointly, on a combined correspondence set
transformation_estimation_->estimateRigidTransformation (*inputs_transformed_combined, *targets_combined, *correspondences_, transformation_);
// Tranform the combined data
this->transformCloud (*inputs_transformed_combined, *inputs_transformed_combined, transformation_);
// And all its components
for (size_t i = 0; i < sources_.size (); i++)
{
this->transformCloud (*inputs_transformed[i], *inputs_transformed[i], transformation_);
}
// Obtain the final transformation
final_transformation_ = transformation_ * final_transformation_;
++nr_iterations_;
// Update the vizualization of icp convergence
//if (update_visualizer_ != 0)
// update_visualizer_(output, source_indices_good, *target_, target_indices_good );
converged_ = static_cast<bool> ((*convergence_criteria_));
}
while (!converged_);
PCL_DEBUG ("Transformation is:\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n",
final_transformation_ (0, 0), final_transformation_ (0, 1), final_transformation_ (0, 2), final_transformation_ (0, 3),
final_transformation_ (1, 0), final_transformation_ (1, 1), final_transformation_ (1, 2), final_transformation_ (1, 3),
final_transformation_ (2, 0), final_transformation_ (2, 1), final_transformation_ (2, 2), final_transformation_ (2, 3),
final_transformation_ (3, 0), final_transformation_ (3, 1), final_transformation_ (3, 2), final_transformation_ (3, 3));
// For fitness checks, etc, we'll use an aggregated cloud for now (should be evaluating independently for correctness, but this requires propagating a few virtual methods from Registration)
IterativeClosestPoint<PointSource, PointTarget, Scalar>::setInputSource (sources_combined);
IterativeClosestPoint<PointSource, PointTarget, Scalar>::setInputTarget (targets_combined);
// If we automatically set the correspondence estimators, we should clear them now
if (!manual_correspondence_estimations_set)
{
correspondence_estimations_.clear ();
}
// By definition, this method will return an empty cloud (for compliance with the ICP API).
// We can figure out a better solution, if necessary.
output = PointCloudSource ();
}
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::JointIterativeClosestPoint<PointSource, PointTarget, Scalar>::determineRequiredBlobData ()
{
need_source_blob_ = false;
need_target_blob_ = false;
// Check estimators
for (size_t i = 0; i < correspondence_estimations_.size (); i++)
{
CorrespondenceEstimationPtr& ce = correspondence_estimations_[i];
need_source_blob_ |= ce->requiresSourceNormals ();
need_target_blob_ |= ce->requiresTargetNormals ();
// Add warnings if necessary
if (ce->requiresSourceNormals () && !source_has_normals_)
{
PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects source normals, but we can't provide them.\n", getClassName ().c_str ());
}
if (ce->requiresTargetNormals () && !target_has_normals_)
{
PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects target normals, but we can't provide them.\n", getClassName ().c_str ());
}
}
// Check rejectors
for (size_t i = 0; i < correspondence_rejectors_.size (); i++)
{
registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
need_source_blob_ |= rej->requiresSourcePoints ();
need_source_blob_ |= rej->requiresSourceNormals ();
need_target_blob_ |= rej->requiresTargetPoints ();
need_target_blob_ |= rej->requiresTargetNormals ();
if (rej->requiresSourceNormals () && !source_has_normals_)
{
PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects source normals, but we can't provide them.\n", getClassName ().c_str (), rej->getClassName ().c_str ());
}
if (rej->requiresTargetNormals () && !target_has_normals_)
{
PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects target normals, but we can't provide them.\n", getClassName ().c_str (), rej->getClassName ().c_str ());
}
}
}
#endif /* PCL_REGISTRATION_IMPL_JOINT_ICP_HPP_ */
pcl::search::KdTree
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62
pcl::Correspondence::index_match
int index_match
Index of the matching (target) point.
Definition: correspondence.h:63
pcl::IterativeClosestPoint::PointCloudTargetPtr
PointCloudTarget::Ptr PointCloudTargetPtr
Definition: icp.h:102
pcl::registration::DefaultConvergenceCriteria
DefaultConvergenceCriteria represents an instantiation of ConvergenceCriteria, and implements the fol...
Definition: default_convergence_criteria.h:65
pcl::JointIterativeClosestPoint::KdTreeReciprocalPtr
KdTree::Ptr KdTreeReciprocalPtr
Definition: joint_icp.h:70
pcl::Correspondence
Correspondence represents a match between two entities (e.g., points, descriptors, etc).
Definition: correspondence.h:58
pcl::JointIterativeClosestPoint::computeTransformation
virtual void computeTransformation(PointCloudSource &output, const Matrix4 &guess)
Rigid transformation computation method with initial guess.
Definition: joint_icp.hpp:49
pcl::PCLPointCloud2::Ptr
boost::shared_ptr< ::pcl::PCLPointCloud2 > Ptr
Definition: PCLPointCloud2.h:51
pcl::Correspondence::index_query
int index_query
Index of the query (source) point.
Definition: correspondence.h:61
pcl::JointIterativeClosestPoint::CorrespondenceEstimationPtr
CorrespondenceEstimation::Ptr CorrespondenceEstimationPtr
Definition: joint_icp.h:80
pcl::Correspondences
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Definition: correspondence.h:92
pcl::IterativeClosestPoint
IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm...
Definition: icp.h:94
pcl::CorrespondencesPtr
boost::shared_ptr< Correspondences > CorrespondencesPtr
Definition: correspondence.h:93
pcl::PointCloud< PointSource >
pcl::IterativeClosestPoint::Matrix4
Registration< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
Definition: icp.h:134
pcl::toPCLPointCloud2
void toPCLPointCloud2(const pcl::PointCloud< PointT > &cloud, pcl::PCLPointCloud2 &msg)
Convert a pcl::PointCloud<T> object to a PCLPointCloud2 binary data blob.
Definition: conversions.h:242
pcl::PCLPointCloud2
Definition: PCLPointCloud2.h:20
pcl::registration::CorrespondenceRejector::Ptr
boost::shared_ptr< CorrespondenceRejector > Ptr
Definition: correspondence_rejection.h:62
pcl::JointIterativeClosestPoint::determineRequiredBlobData
void determineRequiredBlobData()
Looks at the Estimators and Rejectors and determines whether their blob-setter methods need to be cal...
Definition: joint_icp.hpp:283
pcl::JointIterativeClosestPoint::KdTreePtr
pcl::search::KdTree< PointTarget >::Ptr KdTreePtr
Definition: joint_icp.h:67
pcl::IterativeClosestPoint::PointCloudSourcePtr
PointCloudSource::Ptr PointCloudSourcePtr
Definition: icp.h:98