From ef654bdb1a3fe0bae6f3247b4959e230e63ba429 Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Mon, 3 Jun 2024 08:58:07 +0900 Subject: [PATCH] feat: add score array (#55) * Added score_array Signed-off-by: Shintaro Sakoda * Fixed transform_probability Signed-off-by: Shintaro Sakoda * Added print gains Signed-off-by: Shintaro Sakoda --------- Signed-off-by: Shintaro Sakoda Signed-off-by: Shintaro Sakoda --- apps/regression_test.cpp | 5 ++++- include/multigrid_pclomp/multigrid_ndt_omp.h | 4 ++++ .../multigrid_pclomp/multigrid_ndt_omp_impl.hpp | 15 ++++++++++++++- include/pclomp/ndt_struct.hpp | 2 ++ 4 files changed, 24 insertions(+), 2 deletions(-) diff --git a/apps/regression_test.cpp b/apps/regression_test.cpp index 47f1a37b..3a813937 100644 --- a/apps/regression_test.cpp +++ b/apps/regression_test.cpp @@ -107,6 +107,9 @@ 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; @@ -114,7 +117,7 @@ int main(int argc, char** argv) { 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 index 7bbe7e60..29f7880e 100644 --- a/include/multigrid_pclomp/multigrid_ndt_omp.h +++ b/include/multigrid_pclomp/multigrid_ndt_omp.h @@ -278,7 +278,9 @@ class MultiGridNormalDistributionsTransform : public pcl::RegistrationgetFinalTransformation(); 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 hessian_; std::vector> transformation_array_; + std::vector transform_probability_array_; + std::vector nearest_voxel_transformation_likelihood_array_; double nearest_voxel_transformation_likelihood_; boost::optional regularization_pose_; diff --git a/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp b/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp index 1118a105..ac8691f1 100644 --- a/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp +++ b/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp @@ -71,6 +71,8 @@ MultiGridNormalDistributionsTransform::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::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 &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 &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::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 p, delta_p, score_gradient; Eigen::Vector3f init_translation = eig_transformation.translation(); @@ -409,7 +420,9 @@ double MultiGridNormalDistributionsTransform::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(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 index 8582ee26..69aa2bd6 100644 --- a/include/pclomp/ndt_struct.hpp +++ b/include/pclomp/ndt_struct.hpp @@ -69,6 +69,8 @@ struct NdtResult { int iteration_num; std::vector> transformation_array; Eigen::Matrix hessian; + std::vector transform_probability_array; + std::vector nearest_voxel_transformation_likelihood_array; EIGEN_MAKE_ALIGNED_OPERATOR_NEW friend std::ostream &operator<<(std::ostream &os, const NdtResult &val) {