template <typename PointSource, typename PointTarget> void
pcl::IterativeClosestPoint<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess)
{
// Allocate enough space to hold the results
std::vector<int> nn_indices (1);
std::vector<float> nn_dists (1);
// Point cloud containing the correspondences of each point in <input, indices>
PointCloudTarget input_corresp;
input_corresp.points.resize (indices_->size ());
nr_iterations_ = 0;
converged_ = false;
double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_;
// If the guessed transformation is non identity
if (guess != Eigen::Matrix4f::Identity ())
{
// Initialise final transformation to the guessed one
final_transformation_ = guess;
// Apply guessed transformation prior to search for neighbours
transformPointCloud (output, output, guess);
}
// Resize the vector of distances between correspondences
std::vector<float> previous_correspondence_distances (indices_->size ());
correspondence_distances_.resize (indices_->size ());
while (!converged_) // repeat until convergence
{
// Save the previously estimated transformation
previous_transformation_ = transformation_;
// And the previous set of distances
previous_correspondence_distances = correspondence_distances_;
int cnt = 0;
std::vector<int> source_indices (indices_->size ());
std::vector<int> target_indices (indices_->size ());
// Iterating over the entire index vector and find all correspondences
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
if (!this->searchForNeighbors (output, (*indices_)[idx], nn_indices, nn_dists))
{
PCL_ERROR ("[pcl::%s::computeTransformation] Unable to find a nearest neighbor in the target dataset for point %d in the source!\n", getClassName ().c_str (), (*indices_)[idx]);
return;
}
// Check if the distance to the nearest neighbor is smaller than the user imposed threshold
if (nn_dists[0] < dist_threshold)
{
source_indices[cnt] = (*indices_)[idx];
target_indices[cnt] = nn_indices[0];
cnt++;
}
// Save the nn_dists[0] to a global vector of distances
correspondence_distances_[(*indices_)[idx]] = std::min (nn_dists[0], static_cast<float> (dist_threshold));
}
if (cnt < min_number_correspondences_)
{
PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
converged_ = false;
return;
}
// Resize to the actual number of valid correspondences
source_indices.resize (cnt); target_indices.resize (cnt);
std::vector<int> source_indices_good;
std::vector<int> target_indices_good;
{
// From the set of correspondences found, attempt to remove outliers
// Create the registration model
typedef typename SampleConsensusModelRegistration<PointSource>::Ptr SampleConsensusModelRegistrationPtr;
SampleConsensusModelRegistrationPtr model;
model.reset (new SampleConsensusModelRegistration<PointSource> (output.makeShared (), source_indices));
// Pass the target_indices
model->setInputTarget (target_, target_indices);
// Create a RANSAC model
RandomSampleConsensus<PointSource> sac (model, inlier_threshold_);
sac.setMaxIterations (ransac_iterations_);
// Compute the set of inliers
if (!sac.computeModel ())
{
source_indices_good = source_indices;
target_indices_good = target_indices;
}
else
{
std::vector<int> inliers;
// Get the inliers
sac.getInliers (inliers);
source_indices_good.resize (inliers.size ());
target_indices_good.resize (inliers.size ());
boost::unordered_map<int, int> source_to_target;
for (unsigned int i = 0; i < source_indices.size(); ++i)
source_to_target[source_indices[i]] = target_indices[i];
// Copy just the inliers
std::copy(inliers.begin(), inliers.end(), source_indices_good.begin());
for (size_t i = 0; i < inliers.size (); ++i)
target_indices_good[i] = source_to_target[inliers[i]];
}
}
// Check whether we have enough correspondences
cnt = static_cast<int> (source_indices_good.size ());
if (cnt < min_number_correspondences_)
{
PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
converged_ = false;
return;
}
PCL_DEBUG ("[pcl::%s::computeTransformation] Number of correspondences %d [%f%%] out of %zu points [100.0%%], RANSAC rejected: %zu [%f%%].\n",
getClassName ().c_str (),
cnt,
(static_cast<float> (cnt) * 100.0f) / static_cast<float> (indices_->size ()),
indices_->size (),
source_indices.size () - cnt,
static_cast<float> (source_indices.size () - cnt) * 100.0f / static_cast<float> (source_indices.size ()));
// Estimate the transform
//rigid_transformation_estimation_(output, source_indices_good, *target_, target_indices_good, transformation_);
transformation_estimation_->estimateRigidTransformation (output, source_indices_good, *target_, target_indices_good, transformation_);
// Tranform the data
transformPointCloud (output, output, 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 );
// Various/Different convergence termination criteria
// 1. Number of iterations has reached the maximum user imposed number of iterations (via
// setMaximumIterations)
// 2. The epsilon (difference) between the previous transformation and the current estimated transformation
// is smaller than an user imposed value (via setTransformationEpsilon)
// 3. The sum of Euclidean squared errors is smaller than a user defined threshold (via
// setEuclideanFitnessEpsilon)
if (nr_iterations_ >= max_iterations_ ||
(transformation_ - previous_transformation_).array ().abs ().sum () < transformation_epsilon_ ||
fabs (this->getFitnessScore (correspondence_distances_, previous_correspondence_distances)) <= euclidean_fitness_epsilon_
)
{
converged_ = true;
PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %f\n",
getClassName ().c_str (), nr_iterations_, max_iterations_, (transformation_ - previous_transformation_).array ().abs ().sum ());
PCL_DEBUG ("nr_iterations_ (%d) >= max_iterations_ (%d)\n", nr_iterations_, max_iterations_);
PCL_DEBUG ("(transformation_ - previous_transformation_).array ().abs ().sum () (%f) < transformation_epsilon_ (%f)\n",
(transformation_ - previous_transformation_).array ().abs ().sum (), transformation_epsilon_);
PCL_DEBUG ("fabs (getFitnessScore (correspondence_distances_, previous_correspondence_distances)) (%f) <= euclidean_fitness_epsilon_ (%f)\n",
fabs (this->getFitnessScore (correspondence_distances_, previous_correspondence_distances)),
euclidean_fitness_epsilon_);
}
}
}
template <typename PointT> void
pcl::registration::CorrespondenceRejectorSampleConsensus<PointT>::getRemainingCorrespondences (
const pcl::Correspondences& original_correspondences,
pcl::Correspondences& remaining_correspondences)
{
if (!input_)
{
PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No input cloud dataset was given!\n", getClassName ().c_str ());
return;
}
if (!target_)
{
PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No input target dataset was given!\n", getClassName ().c_str ());
return;
}
if (save_inliers_)
inlier_indices_.clear ();
int nr_correspondences = static_cast<int> (original_correspondences.size ());
std::vector<int> source_indices (nr_correspondences);
std::vector<int> target_indices (nr_correspondences);
// Copy the query-match indices
for (size_t i = 0; i < original_correspondences.size (); ++i)
{
source_indices[i] = original_correspondences[i].index_query;
target_indices[i] = original_correspondences[i].index_match;
}
// from pcl/registration/icp.hpp:
std::vector<int> source_indices_good;
std::vector<int> target_indices_good;
{
// From the set of correspondences found, attempt to remove outliers
// Create the registration model
typedef typename pcl::SampleConsensusModelRegistration<PointT>::Ptr SampleConsensusModelRegistrationPtr;
SampleConsensusModelRegistrationPtr model;
model.reset (new pcl::SampleConsensusModelRegistration<PointT> (input_, source_indices));
// Pass the target_indices
model->setInputTarget (target_, target_indices);
// Create a RANSAC model
pcl::RandomSampleConsensus<PointT> sac (model, inlier_threshold_);
sac.setMaxIterations (max_iterations_);
// Compute the set of inliers
if (!sac.computeModel ())
{
remaining_correspondences = original_correspondences;
best_transformation_.setIdentity ();
return;
}
else
{
if (refine_ && !sac.refineModel ())
{
PCL_ERROR ("[pcl::registration::CorrespondenceRejectorSampleConsensus::getRemainingCorrespondences] Could not refine the model! Returning an empty solution.\n");
return;
}
std::vector<int> inliers;
sac.getInliers (inliers);
if (inliers.size () < 3)
{
remaining_correspondences = original_correspondences;
best_transformation_.setIdentity ();
return;
}
boost::unordered_map<int, int> index_to_correspondence;
for (int i = 0; i < nr_correspondences; ++i)
index_to_correspondence[original_correspondences[i].index_query] = i;
remaining_correspondences.resize (inliers.size ());
for (size_t i = 0; i < inliers.size (); ++i)
remaining_correspondences[i] = original_correspondences[index_to_correspondence[inliers[i]]];
if (save_inliers_)
{
inlier_indices_.reserve (inliers.size ());
for (const int &inlier : inliers)
inlier_indices_.push_back (index_to_correspondence[inlier]);
}
// get best transformation
Eigen::VectorXf model_coefficients;
sac.getModelCoefficients (model_coefficients);
best_transformation_.row (0) = model_coefficients.segment<4>(0);
best_transformation_.row (1) = model_coefficients.segment<4>(4);
best_transformation_.row (2) = model_coefficients.segment<4>(8);
best_transformation_.row (3) = model_coefficients.segment<4>(12);
}
}
}
template <typename PointT> inline void
pcl::registration::CorrespondenceRejectorSampleConsensus<PointT>::applyRejection (
pcl::Correspondences &correspondences)
{
int nr_correspondences = input_correspondences_->size ();
std::vector<int> source_indices (nr_correspondences);
std::vector<int> target_indices (nr_correspondences);
// Copy the query-match indices
for (size_t i = 0; i < input_correspondences_->size (); ++i)
{
source_indices[i] = (*input_correspondences_)[i].index_query;
target_indices[i] = (*input_correspondences_)[i].index_match;
}
// from pcl/registration/icp.hpp:
std::vector<int> source_indices_good;
std::vector<int> target_indices_good;
{
// From the set of correspondences found, attempt to remove outliers
// Create the registration model
typedef typename pcl::SampleConsensusModelRegistration<PointT>::Ptr SampleConsensusModelRegistrationPtr;
SampleConsensusModelRegistrationPtr model;
model.reset (new pcl::SampleConsensusModelRegistration<PointT> (input_, source_indices));
// Pass the target_indices
model->setInputTarget (target_, target_indices);
// Create a RANSAC model
pcl::RandomSampleConsensus<PointT> sac (model, inlier_threshold_);
sac.setMaxIterations (max_iterations_);
// Compute the set of inliers
if (!sac.computeModel ())
{
correspondences = *input_correspondences_;
best_transformation_.setIdentity ();
return;
}
else
{
std::vector<int> inliers;
sac.getInliers (inliers);
if (inliers.size () < 3)
{
correspondences = *input_correspondences_;
best_transformation_.setIdentity ();
return;
}
boost::unordered_map<int, int> index_to_correspondence;
for (int i = 0; i < nr_correspondences; ++i)
index_to_correspondence[(*input_correspondences_)[i].index_query] = i;
correspondences.resize (inliers.size ());
for (size_t i = 0; i < inliers.size (); ++i)
correspondences[i] = (*input_correspondences_)[index_to_correspondence[inliers[i]]];
//correspondences[i] = (*input_correspondences_)[inliers[i]];
// get best transformation
Eigen::VectorXf model_coefficients;
sac.getModelCoefficients (model_coefficients);
best_transformation_.row (0) = model_coefficients.segment<4>(0);
best_transformation_.row (1) = model_coefficients.segment<4>(4);
best_transformation_.row (2) = model_coefficients.segment<4>(8);
best_transformation_.row (3) = model_coefficients.segment<4>(12);
}
}
}
