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); } } }