Beispiel #1
0
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);
     }
   }
}