feat: add score array (#55)

* Added score_array Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed transform_probability Signed-off-by: Shintaro Sakoda <[email protected]> * Added print gains Signed-off-by: Shintaro Sakoda <[email protected]> --------- Signed-off-by: Shintaro Sakoda <[email protected]> Signed-off-by: Shintaro Sakoda <[email protected]>
tier4 · Jun 2, 2024 · ef654bd · ef654bd
1 parent 96b5b68
commit ef654bd
Show file tree

Hide file tree

Showing 4 changed files with 24 additions and 2 deletions.
diff --git a/apps/regression_test.cpp b/apps/regression_test.cpp
@@ -107,14 +107,17 @@ int main(int argc, char** argv) {
     const pclomp::NdtResult ndt_result = mg_ndt_omp->getResult();
     const double elapsed = timer.elapsed_milliseconds();
 
+    const float gain_tp = ndt_result.transform_probability - ndt_result.transform_probability_array.front();
+    const float gain_nvtl = ndt_result.nearest_voxel_transformation_likelihood - ndt_result.nearest_voxel_transformation_likelihood_array.front();
+
     // output result
     const double tp = ndt_result.transform_probability;
     const double nvtl = ndt_result.nearest_voxel_transformation_likelihood;
     elapsed_milliseconds.push_back(elapsed);
     nvtl_scores.push_back(nvtl);
     tp_scores.push_back(tp);
     if(i % update_interval == 0) {
-      std::cout << "source_cloud->size()=" << std::setw(4) << source_cloud->size() << ", time=" << elapsed << " [msec], nvtl=" << nvtl << ", tp = " << tp << std::endl;
+      std::cout << "source_cloud->size()=" << std::setw(4) << source_cloud->size() << ", time=" << elapsed << " [msec], nvtl=" << nvtl << ", tp = " << tp << ", gain_nvtl=" << gain_nvtl << ", gain_tp=" << gain_tp << std::endl;
     }
   }
 

diff --git a/include/multigrid_pclomp/multigrid_ndt_omp.h b/include/multigrid_pclomp/multigrid_ndt_omp.h
@@ -278,7 +278,9 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
     ndt_result.pose = this->getFinalTransformation();
     ndt_result.transformation_array = getFinalTransformationArray();
     ndt_result.transform_probability = getTransformationProbability();
+    ndt_result.transform_probability_array = transform_probability_array_;
     ndt_result.nearest_voxel_transformation_likelihood = getNearestVoxelTransformationLikelihood();
+    ndt_result.nearest_voxel_transformation_likelihood_array = nearest_voxel_transformation_likelihood_array_;
     ndt_result.iteration_num = getFinalNumIteration();
     ndt_result.hessian = getHessian();
     return ndt_result;
@@ -515,6 +517,8 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
 
   Eigen::Matrix<double, 6, 6> hessian_;
   std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> transformation_array_;
+  std::vector<float> transform_probability_array_;
+  std::vector<float> nearest_voxel_transformation_likelihood_array_;
   double nearest_voxel_transformation_likelihood_;
 
   boost::optional<Eigen::Matrix4f> regularization_pose_;

diff --git a/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp b/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp
@@ -71,6 +71,8 @@ MultiGridNormalDistributionsTransform<PointSource, PointTarget>::MultiGridNormal
 
   hessian_ = other.hessian_;
   transformation_array_ = other.transformation_array_;
+  transform_probability_array_ = other.transform_probability_array_;
+  nearest_voxel_transformation_likelihood_array_ = other.nearest_voxel_transformation_likelihood_array_;
   nearest_voxel_transformation_likelihood_ = other.nearest_voxel_transformation_likelihood_;
 
   regularization_pose_ = other.regularization_pose_;
@@ -88,6 +90,8 @@ MultiGridNormalDistributionsTransform<PointSource, PointTarget>::MultiGridNormal
 
   hessian_ = other.hessian_;
   transformation_array_ = other.transformation_array_;
+  transform_probability_array_ = other.transform_probability_array_;
+  nearest_voxel_transformation_likelihood_array_ = other.nearest_voxel_transformation_likelihood_array_;
   nearest_voxel_transformation_likelihood_ = other.nearest_voxel_transformation_likelihood_;
 
   regularization_pose_ = other.regularization_pose_;
@@ -109,6 +113,8 @@ MultiGridNormalDistributionsTransform<PointSource, PointTarget> &MultiGridNormal
 
   hessian_ = other.hessian_;
   transformation_array_ = other.transformation_array_;
+  transform_probability_array_ = other.transform_probability_array_;
+  nearest_voxel_transformation_likelihood_array_ = other.nearest_voxel_transformation_likelihood_array_;
   nearest_voxel_transformation_likelihood_ = other.nearest_voxel_transformation_likelihood_;
 
   regularization_pose_ = other.regularization_pose_;
@@ -132,6 +138,8 @@ MultiGridNormalDistributionsTransform<PointSource, PointTarget> &MultiGridNormal
 
   hessian_ = other.hessian_;
   transformation_array_ = other.transformation_array_;
+  transform_probability_array_ = other.transform_probability_array_;
+  nearest_voxel_transformation_likelihood_array_ = other.nearest_voxel_transformation_likelihood_array_;
   nearest_voxel_transformation_likelihood_ = other.nearest_voxel_transformation_likelihood_;
 
   regularization_pose_ = other.regularization_pose_;
@@ -192,6 +200,9 @@ void MultiGridNormalDistributionsTransform<PointSource, PointTarget>::computeTra
   transformation_array_.clear();
   transformation_array_.push_back(final_transformation_);
 
+  transform_probability_array_.clear();
+  nearest_voxel_transformation_likelihood_array_.clear();
+
   // Convert initial guess matrix to 6 element transformation vector
   Eigen::Matrix<double, 6, 1> p, delta_p, score_gradient;
   Eigen::Vector3f init_translation = eig_transformation.translation();
@@ -409,7 +420,9 @@ double MultiGridNormalDistributionsTransform<PointSource, PointTarget>::computeD
   } else {
     nearest_voxel_transformation_likelihood_ = 0.0;
   }
-
+  nearest_voxel_transformation_likelihood_array_.push_back(nearest_voxel_transformation_likelihood_);
+  const float transform_probability = (input_->points.empty() ? 0.0f : score / static_cast<double>(input_->points.size()));
+  transform_probability_array_.push_back(transform_probability);
   return (score);
 }
 

diff --git a/include/pclomp/ndt_struct.hpp b/include/pclomp/ndt_struct.hpp
@@ -69,6 +69,8 @@ struct NdtResult {
   int iteration_num;
   std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> transformation_array;
   Eigen::Matrix<double, 6, 6> hessian;
+  std::vector<float> transform_probability_array;
+  std::vector<float> nearest_voxel_transformation_likelihood_array;
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
   friend std::ostream &operator<<(std::ostream &os, const NdtResult &val) {