From 606313492f24800d6666342a79de471b14f5da8a Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Wed, 20 Mar 2024 10:12:27 +0900 Subject: [PATCH] Fixed args Signed-off-by: Shintaro Sakoda --- apps/check_covariance.cpp | 6 ++++-- include/estimate_covariance/estimate_covariance.hpp | 4 ++-- src/estimate_covariance/estimate_covariance.cpp | 8 ++------ 3 files changed, 8 insertions(+), 10 deletions(-) diff --git a/apps/check_covariance.cpp b/apps/check_covariance.cpp index 174590f1..313f0ded 100644 --- a/apps/check_covariance.cpp +++ b/apps/check_covariance.cpp @@ -131,6 +131,8 @@ int main(int argc, char** argv) { const double score = ndt_result.nearest_voxel_transformation_likelihood; std::cout << source_pcd << ", num=" << std::setw(4) << source_cloud->size() << " points, score=" << score << std::endl; + const std::vector poses_to_search = pclomp::propose_poses_to_search(ndt_result.hessian, ndt_result.pose, offset_x, offset_y); + // estimate covariance // (1) Laplace approximation t1 = std::chrono::system_clock::now(); @@ -140,14 +142,14 @@ int main(int argc, char** argv) { // (2) Multi NDT t1 = std::chrono::system_clock::now(); - const pclomp::ResultOfMultiNdtCovarianceEstimation result_of_mndt = pclomp::estimate_xy_covariance_by_multi_ndt(ndt_result, mg_ndt_omp, initial_pose, offset_x, offset_y); + const pclomp::ResultOfMultiNdtCovarianceEstimation result_of_mndt = pclomp::estimate_xy_covariance_by_multi_ndt(ndt_result, mg_ndt_omp, poses_to_search); const Eigen::Matrix2d cov_by_mndt = result_of_mndt.covariance; t2 = std::chrono::system_clock::now(); const auto elapsed_mndt = std::chrono::duration_cast(t2 - t1).count() / 1000.0; // (3) Multi NDT with score t1 = std::chrono::system_clock::now(); - const pclomp::ResultOfMultiNdtCovarianceEstimation result_of_mndt_score = pclomp::estimate_xy_covariance_by_multi_ndt_score(ndt_result, mg_ndt_omp, initial_pose, offset_x, offset_y); + const pclomp::ResultOfMultiNdtCovarianceEstimation result_of_mndt_score = pclomp::estimate_xy_covariance_by_multi_ndt_score(ndt_result, mg_ndt_omp, poses_to_search); const Eigen::Matrix2d cov_by_mndt_score = result_of_mndt_score.covariance; t2 = std::chrono::system_clock::now(); const auto elapsed_mndt_score = std::chrono::duration_cast(t2 - t1).count() / 1000.0; diff --git a/include/estimate_covariance/estimate_covariance.hpp b/include/estimate_covariance/estimate_covariance.hpp index 77d1107b..f1886492 100644 --- a/include/estimate_covariance/estimate_covariance.hpp +++ b/include/estimate_covariance/estimate_covariance.hpp @@ -15,8 +15,8 @@ struct ResultOfMultiNdtCovarianceEstimation { /** \brief Estimate functions */ Eigen::Matrix2d estimate_xy_covariance_by_Laplace_approximation(const Eigen::Matrix& hessian); -ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt(const NdtResult& ndt_result, std::shared_ptr> ndt_ptr, const Eigen::Matrix4f& initial_pose, const std::vector& offset_x, const std::vector& offset_y); -ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt_score(const NdtResult& ndt_result, std::shared_ptr> ndt_ptr, const Eigen::Matrix4f& initial_pose, const std::vector& offset_x, const std::vector& offset_y); +ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt(const NdtResult& ndt_result, std::shared_ptr> ndt_ptr, const std::vector& poses_to_search); +ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt_score(const NdtResult& ndt_result, std::shared_ptr> ndt_ptr, const std::vector& poses_to_search); /** \brief Find rotation matrix aligning covariance to principal axes * (1) Compute eigenvalues and eigenvectors diff --git a/src/estimate_covariance/estimate_covariance.cpp b/src/estimate_covariance/estimate_covariance.cpp index 6a9ab3b3..7ea0ef31 100644 --- a/src/estimate_covariance/estimate_covariance.cpp +++ b/src/estimate_covariance/estimate_covariance.cpp @@ -10,9 +10,7 @@ Eigen::Matrix2d estimate_xy_covariance_by_Laplace_approximation(const Eigen::Mat return covariance_xy; } -ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt(const NdtResult& ndt_result, std::shared_ptr> ndt_ptr, const Eigen::Matrix4f& initial_pose, const std::vector& offset_x, const std::vector& offset_y) { - const std::vector poses_to_search = propose_poses_to_search(ndt_result.hessian, ndt_result.pose, offset_x, offset_y); - +ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt(const NdtResult& ndt_result, std::shared_ptr> ndt_ptr, const std::vector& poses_to_search) { // initialize by the main result const Eigen::Vector2d ndt_pose_2d(ndt_result.pose(0, 3), ndt_result.pose(1, 3)); std::vector ndt_pose_2d_vec{ndt_pose_2d}; @@ -41,9 +39,7 @@ ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt(const N return {mean, covariance, poses_to_search, ndt_results}; } -ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt_score(const NdtResult& ndt_result, std::shared_ptr> ndt_ptr, const Eigen::Matrix4f& initial_pose, const std::vector& offset_x, const std::vector& offset_y) { - const std::vector poses_to_search = propose_poses_to_search(ndt_result.hessian, ndt_result.pose, offset_x, offset_y); - +ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt_score(const NdtResult& ndt_result, std::shared_ptr> ndt_ptr, const std::vector& poses_to_search) { // initialize by the main result const Eigen::Vector2d ndt_pose_2d(ndt_result.pose(0, 3), ndt_result.pose(1, 3)); std::vector ndt_pose_2d_vec{ndt_pose_2d};