void
PosesFromMatches::estimatePosesUsing1Correspondence (const PointCorrespondences6DVector& correspondences,
        int max_no_of_results,
        PosesFromMatches::PoseEstimatesVector& pose_estimates) const
{
    //MEASURE_FUNCTION_TIME;
    //int initial_no_of_pose_estimates = pose_estimates.size ();

    max_no_of_results = std::min (max_no_of_results, (int)correspondences.size ());

    for (int correspondence_idx=0; correspondence_idx<max_no_of_results; ++correspondence_idx)
    {
        const PointCorrespondence6D& correspondence = correspondences[correspondence_idx];
        PoseEstimate pose_estimate;
        pose_estimate.transformation = correspondence.transformation;
        pose_estimate.score = correspondence.score;
        pose_estimate.correspondence_indices.push_back (correspondence_idx);
        pose_estimates.push_back (pose_estimate);
    }

    // std::cout << "Extracted "<<pose_estimates.size ()-initial_no_of_pose_estimates<<" poses.
    // Set of pose estimates now has size "<<pose_estimates.size ()<<"\n";
}
void
pcl::PosesFromMatches::estimatePosesUsing3Correspondences (const PointCorrespondences6DVector& correspondences,
                                                           int max_no_of_tested_combinations, int max_no_of_results,
                                                           PosesFromMatches::PoseEstimatesVector& pose_estimates) const
{
  const Eigen::Vector3f x_direction (1.0f, 0.0f, 0.0f),
                        y_direction (0.0f, 1.0f, 0.0f),
                        z_direction (0.0f, 0.0f, 1.0f);
  
  int max_correspondence_idx = static_cast<int> (correspondences.size ());
  int counter_for_tested_combinations = 0,
      counter_for_added_pose_estimates = 0;
  float max_distance_quotient = 1.0f+parameters_.max_correspondence_distance_error,
        max_distance_quotient_squared = std::pow (max_distance_quotient, 2),
        min_distance_quotient = 1.0f / (max_distance_quotient),
        min_distance_quotient_squared = std::pow (min_distance_quotient, 2);

  pcl::TransformationFromCorrespondences transformation_from_correspondeces;
  
  // The following loop structure goes through the triples in the order 123, 124, 134, 234, 125, 135, 235, ...,
  // testing the best correspondences triples first, without beeing stuck too long with one specific
  // (possibly wrong) correspondence.
  bool done = false;
  for (int correspondence3_idx = 0; correspondence3_idx < max_correspondence_idx && !done; ++correspondence3_idx)
  {
    const pcl::PointCorrespondence6D& correspondence3 = correspondences[correspondence3_idx];
    const Eigen::Vector3f& point3 = correspondence3.point1,
                  & corr3  = correspondence3.point2;
    for (int correspondence2_idx = 0; correspondence2_idx < correspondence3_idx && !done; ++correspondence2_idx)
    {
      const pcl::PointCorrespondence6D& correspondence2 = correspondences[correspondence2_idx];
      const Eigen::Vector3f& point2 = correspondence2.point1,
                    & corr2  = correspondence2.point2;
      
      float distance23_squared = (point3-point2).squaredNorm (),
            distance23_corr_squared = (corr3-corr2).squaredNorm (),
            distance23_quotient_squared = distance23_squared/distance23_corr_squared;
      if (   distance23_quotient_squared < min_distance_quotient_squared 
          || distance23_quotient_squared > max_distance_quotient_squared)
        continue;
      
      for (int correspondence1_idx = 0; correspondence1_idx < correspondence2_idx; ++correspondence1_idx)
      {
        if (counter_for_tested_combinations >= max_no_of_tested_combinations)
        {
          done = true;
          break;
        }
        ++counter_for_tested_combinations;
        const pcl::PointCorrespondence6D& correspondence1 = correspondences[correspondence1_idx];
        const Eigen::Vector3f& point1 = correspondence1.point1,
                             & corr1  = correspondence1.point2;
        float distance12_squared = (point2-point1).squaredNorm (),
              distance12_corr_squared = (corr2-corr1).squaredNorm (),
              distance12_quotient_squared = distance12_squared/distance12_corr_squared;
        if (   distance12_quotient_squared < min_distance_quotient_squared
            || distance12_quotient_squared > max_distance_quotient_squared)
          continue;
        float distance13_squared = (point3-point1).squaredNorm (),
              distance13_corr_squared = (corr3-corr1).squaredNorm (),
              distance13_quotient_squared = distance13_squared/distance13_corr_squared;
        if (   distance13_quotient_squared < min_distance_quotient_squared
            || distance13_quotient_squared > max_distance_quotient_squared)
          continue;
        
        transformation_from_correspondeces.reset ();
        transformation_from_correspondeces.add (corr1, point1);
        transformation_from_correspondeces.add (corr2, point2);
        transformation_from_correspondeces.add (corr3, point3);
        
        ++counter_for_added_pose_estimates;
        PoseEstimate pose_estimate;
        pose_estimate.transformation = transformation_from_correspondeces.getTransformation ();
        pose_estimate.score = (correspondence1.distance + correspondence2.distance + correspondence3.distance) / 3.0f; // TODO: based
                                                                                  // on the measured distance_errors?
        pose_estimate.correspondence_indices.push_back (correspondence1_idx);
        pose_estimate.correspondence_indices.push_back (correspondence2_idx);
        pose_estimate.correspondence_indices.push_back (correspondence3_idx);
        pose_estimates.push_back (pose_estimate);
        if (counter_for_added_pose_estimates >= max_no_of_results)
        {
          done = true;
          break;
        }
      }
    }
  }
}
void
PosesFromMatches::estimatePosesUsing2Correspondences (const PointCorrespondences6DVector& correspondences,
        int max_no_of_tested_combinations, int max_no_of_results,
        PosesFromMatches::PoseEstimatesVector& pose_estimates) const
{
    const Eigen::Vector3f x_direction (1.0f, 0.0f, 0.0f),
          y_direction (0.0f, 1.0f, 0.0f),
          z_direction (0.0f, 0.0f, 1.0f);

    int max_correspondence_idx = correspondences.size ();
    int counter_for_tested_combinations = 0,
        counter_for_added_pose_estimates = 0;
    float max_distance_quotient = 1.0f+parameters_.max_correspondence_distance_error,
          max_distance_quotient_squared=powf (max_distance_quotient, 2),
          min_distance_quotient = 1.0f / (max_distance_quotient),
          min_distance_quotient_squared=powf (min_distance_quotient, 2);

    TransformationFromCorrespondences transformation_from_correspondeces;

    // The following loop structure goes through the pairs in the order 12, 13, 23, 14, 24, 34, ...,
    // testing the best correspondences pairs first, without beeing stuck too long with one specific
    // (possibly wrong) correspondence.
    bool done = false;
    for (int correspondence2_idx=0; correspondence2_idx<max_correspondence_idx && !done; ++correspondence2_idx)
    {
        const PointCorrespondence6D& correspondence2 = correspondences[correspondence2_idx];
        for (int correspondence1_idx=0; correspondence1_idx<correspondence2_idx; ++correspondence1_idx)
        {
            if (counter_for_tested_combinations >= max_no_of_tested_combinations)
            {
                done = true;
                break;
            }

            const PointCorrespondence6D& correspondence1 = correspondences[correspondence1_idx];
            ++counter_for_tested_combinations;

            const Eigen::Vector3f& point1 = correspondence1.point1, & point2 = correspondence2.point1,
                                   & corr1  = correspondence1.point2, & corr2  = correspondence2.point2;

            float distance_squared = (point2-point1).squaredNorm (),
                  distance_corr_squared = (corr2-corr1).squaredNorm (),
                  distance_quotient_squared = distance_squared/distance_corr_squared;
            if (   distance_quotient_squared < min_distance_quotient_squared
                    || distance_quotient_squared > max_distance_quotient_squared)
            {
                //std::cout << "Skipping because of mismatching distances "<<sqrtf (distance1_squared)
                //          << " and "<<sqrtf (distance1_corr_squared)<<".\n";
                continue;
            }

            float distance = sqrtf (distance_squared);

            Eigen::Vector3f corr3=corr1, corr4=corr2;
            corr3[0]+=distance;
            corr4[0]+=distance;
            Eigen::Vector3f point3=correspondence1.transformation*corr3, point4=correspondence2.transformation*corr4;

            distance_squared = (point4-point3).squaredNorm (),
            distance_corr_squared = (corr4-corr3).squaredNorm (),
            distance_quotient_squared = distance_squared/distance_corr_squared;
            if (   distance_quotient_squared < min_distance_quotient_squared
                    || distance_quotient_squared > max_distance_quotient_squared)
                continue;

            Eigen::Vector3f corr5=corr1, corr6=corr2;
            corr5[1]+=distance;
            corr6[1]+=distance;
            Eigen::Vector3f point5=correspondence1.transformation*corr5, point6=correspondence2.transformation*corr6;

            distance_squared = (point6-point5).squaredNorm (),
            distance_corr_squared = (corr6-corr5).squaredNorm (),
            distance_quotient_squared = distance_squared/distance_corr_squared;
            if (   distance_quotient_squared < min_distance_quotient_squared
                    || distance_quotient_squared > max_distance_quotient_squared)
                continue;

            Eigen::Vector3f corr7=corr1, corr8=corr2;
            corr7[2]+=distance;
            corr8[2]+=distance;
            Eigen::Vector3f point7=correspondence1.transformation*corr7, point8=correspondence2.transformation*corr8;

            distance_squared = (point8-point7).squaredNorm (),
            distance_corr_squared = (corr8-corr7).squaredNorm (),
            distance_quotient_squared = distance_squared/distance_corr_squared;
            if (   distance_quotient_squared < min_distance_quotient_squared
                    || distance_quotient_squared > max_distance_quotient_squared)
                continue;

            transformation_from_correspondeces.reset ();
            transformation_from_correspondeces.add (corr1, point1);
            transformation_from_correspondeces.add (corr2, point2);
            transformation_from_correspondeces.add (corr3, point3);
            transformation_from_correspondeces.add (corr4, point4);
            transformation_from_correspondeces.add (corr5, point5);
            transformation_from_correspondeces.add (corr6, point6);
            transformation_from_correspondeces.add (corr7, point7);
            transformation_from_correspondeces.add (corr8, point8);

            ++counter_for_added_pose_estimates;
            PoseEstimate pose_estimate;
            pose_estimate.transformation = transformation_from_correspondeces.getTransformation ();
            pose_estimate.score = 0.5f*(correspondence1.score+correspondence2.score); // TODO: based
            // on the measured distance_errors?
            pose_estimate.correspondence_indices.push_back (correspondence1_idx);
            pose_estimate.correspondence_indices.push_back (correspondence2_idx);
            pose_estimates.push_back (pose_estimate);
            if (counter_for_added_pose_estimates >= max_no_of_results)
            {
                done = true;
                break;
            }
        }
    }
    //std::cout << "Tested "<<counter_for_tested_combinations<<" combinations. Extracted "
    //          << counter_for_added_pose_estimates
    //<< " poses. Set of pose estimates now has size "<<pose_estimates.size ()<<"\n";
}