Fixed args

Signed-off-by: Shintaro Sakoda <[email protected]>
tier4 · Mar 20, 2024 · 6063134 · 6063134
1 parent 73b364e
commit 6063134
Show file tree

Hide file tree

Showing 3 changed files with 8 additions and 10 deletions.
diff --git 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<Eigen::Matrix4f> 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<std::chrono::microseconds>(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<std::chrono::microseconds>(t2 - t1).count() / 1000.0;

diff --git 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<double, 6, 6>& hessian);
-ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt(const NdtResult& ndt_result, std::shared_ptr<pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>> ndt_ptr, const Eigen::Matrix4f& initial_pose, const std::vector<double>& offset_x, const std::vector<double>& offset_y);
-ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt_score(const NdtResult& ndt_result, std::shared_ptr<pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>> ndt_ptr, const Eigen::Matrix4f& initial_pose, const std::vector<double>& offset_x, const std::vector<double>& offset_y);
+ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt(const NdtResult& ndt_result, std::shared_ptr<pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>> ndt_ptr, const std::vector<Eigen::Matrix4f>& poses_to_search);
+ResultOfMultiNdtCovarianceEstimation estimate_xy_covariance_by_multi_ndt_score(const NdtResult& ndt_result, std::shared_ptr<pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>> ndt_ptr, const std::vector<Eigen::Matrix4f>& 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
@@ -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<pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>> ndt_ptr, const Eigen::Matrix4f& initial_pose, const std::vector<double>& offset_x, const std::vector<double>& offset_y) {
-  const std::vector<Eigen::Matrix4f> 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<pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>> ndt_ptr, const std::vector<Eigen::Matrix4f>& 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<Eigen::Vector2d> 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<pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>> ndt_ptr, const Eigen::Matrix4f& initial_pose, const std::vector<double>& offset_x, const std::vector<double>& offset_y) {
-  const std::vector<Eigen::Matrix4f> 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<pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>> ndt_ptr, const std::vector<Eigen::Matrix4f>& 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<Eigen::Vector2d> ndt_pose_2d_vec{ndt_pose_2d};