Fix getFitnessScore to respect user-provided indices in Registration

Author: AlrIsmail

registration/include/pcl/registration/impl/registration.hpp

@@ -131,13 +131,16 @@ Registration<PointSource, PointTarget, Scalar>::getFitnessScore(
 
 template <typename PointSource, typename PointTarget, typename Scalar>
 inline double
-Registration<PointSource, PointTarget, Scalar>::getFitnessScore(double max_range)
+Registration<PointSource, PointTarget, Scalar>::getFitnessScore(double max_range, bool use_indices)
 {
   double fitness_score = 0.0;
 
   // Transform the input dataset using the final transformation
   PointCloudSource input_transformed;
-  transformPointCloud(*input_, input_transformed, final_transformation_);
+  if (use_indices && indices_ && indices_->size() != input_->size())
+    transformPointCloud(*input_, *indices_, input_transformed, final_transformation_);
+  else
+    transformPointCloud(*input_, input_transformed, final_transformation_);
 
   pcl::Indices nn_indices(1);
   std::vector<float> nn_dists(1);

registration/include/pcl/registration/registration.h

@@ -444,9 +444,10 @@ class Registration : public PCLBase<PointSource> {
   /** \brief Obtain the Euclidean fitness score (e.g., mean of squared distances from
    * the source to the target) \param[in] max_range maximum allowable distance between a
    * point and its correspondence in the target (default: double::max)
+   * \param[in] use_indices whether to use the registered indices or all points (default: false)
    */
   inline double
-  getFitnessScore(double max_range = std::numeric_limits<double>::max());
+  getFitnessScore(double max_range = std::numeric_limits<double>::max(), bool use_indices = false);
 
   /** \brief Obtain the Euclidean fitness score (e.g., mean of squared distances from
    * the source to the target) from two sets of correspondence distances (distances

test/registration/test_registration.cpp

@@ -194,6 +194,44 @@ TEST(PCL, ICP_translated)
   EXPECT_NEAR(icp.getFinalTransformation()(2, 3), 0.2, 2e-3);
 }
 
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+TEST (PCL, Registration_getFitnessScore_Indices)
+{
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);
+
+  cloud_in->push_back(pcl::PointXYZ(0, 0, 0));
+  cloud_in->push_back(pcl::PointXYZ(0, 1, 0));
+  cloud_in->push_back(pcl::PointXYZ(0, 0, 1));
+  cloud_in->push_back(pcl::PointXYZ(10, 0, 0));
+
+  cloud_out->push_back(pcl::PointXYZ(0, 0, 0));
+  cloud_out->push_back(pcl::PointXYZ(0, 1, 0));
+  cloud_out->push_back(pcl::PointXYZ(0, 0, 1));
+  cloud_out->push_back(pcl::PointXYZ(10, 0, 0.5)); // Dist squared = 0.25
+
+  RegistrationWrapper<pcl::PointXYZ, pcl::PointXYZ> reg;
+  reg.setInputSource(cloud_in);
+  reg.setInputTarget(cloud_out);
+  
+  pcl::IndicesPtr indices(new pcl::Indices());
+  indices->push_back(0);
+  indices->push_back(1);
+  indices->push_back(2);
+  reg.setIndices(indices);
+
+  pcl::PointCloud<pcl::PointXYZ> final_cloud;
+  reg.align(final_cloud);
+
+  // With use_indices = false (default), should calculate score using all points
+  // mean of squared distances: (0 + 0 + 0 + 0.25) / 4 = 0.0625
+  EXPECT_NEAR(reg.getFitnessScore(1.0, false), 0.0625, 1e-4);
+
+  // With use_indices = true, should calculate score using only indices (points 0, 1, 2)
+  // mean of squared distances: (0 + 0 + 0) / 3 = 0.0
+  EXPECT_NEAR(reg.getFitnessScore(1.0, true), 0.0, 1e-4);
+}
+
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 TEST (PCL, IterativeClosestPoint)
 {