feat: score based covariance estimation (#45)

* Added score_array Signed-off-by: Shintaro Sakoda <[email protected]> * Added covariance_estimation Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed to shared_ptr Signed-off-by: Shintaro Sakoda <[email protected]> * Added score Signed-off-by: Shintaro Sakoda <[email protected]> * Added default Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed fill and aspect Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed scale Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed struct Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed color Signed-off-by: Shintaro Sakoda <[email protected]> * Applied formatter Signed-off-by: Shintaro Sakoda <[email protected]> * Added calculate_weighted_mean_and_cov Signed-off-by: Shintaro Sakoda <[email protected]> * Applied formatter Signed-off-by: Shintaro Sakoda <[email protected]> * Refactored score Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed temperature Signed-off-by: Shintaro Sakoda <[email protected]> * Added temperature arg Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed signature Signed-off-by: Shintaro Sakoda <[email protected]> * Added time Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed to use propose_search_points Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed plot Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed plot Signed-off-by: Shintaro Sakoda <[email protected]> * Renamed function Signed-off-by: Shintaro Sakoda <[email protected]> * Added make_flat_pcd.py Signed-off-by: Shintaro Sakoda <[email protected]> * Added awsim_nishishinjuku_flat Signed-off-by: Shintaro Sakoda <[email protected]> * Organized codes Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed return type Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed result Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed calc unbiased covariance Signed-off-by: Shintaro Sakoda <[email protected]> * Refactored Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed bug Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed args Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed propose_poses_to_search Signed-off-by: Shintaro Sakoda <[email protected]> * Removed output_pose_score_weight Signed-off-by: Shintaro Sakoda <[email protected]> * Refactored Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed execute script Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed plot script Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed plot script Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed plot script Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed rot Signed-off-by: Shintaro Sakoda <[email protected]> * Fix parameter Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed rot Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed to calculate only score Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed plot script Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed check_covariance.cpp Signed-off-by: Shintaro Sakoda <[email protected]> * Removed transform_probability_array and nearest_voxel_transformation_likelihood_array Signed-off-by: Shintaro Sakoda <[email protected]> * Added a line break Signed-off-by: Shintaro Sakoda <[email protected]> * Removed make_flat_pcd.py Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed output csv Signed-off-by: Shintaro Sakoda <[email protected]> * Added rotate_covariance_to_base_link Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed rotation Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed execute_script Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed regression_test.yml Signed-off-by: Shintaro Sakoda <[email protected]> * Fixed script name Signed-off-by: Shintaro Sakoda <[email protected]> * Added tqdm Signed-off-by: Shintaro Sakoda <[email protected]> * Added adjust_diagonal_covariance Signed-off-by: Shintaro Sakoda <[email protected]> --------- Signed-off-by: Shintaro Sakoda <[email protected]> Signed-off-by: Shintaro Sakoda <[email protected]>
tier4 · Apr 5, 2024 · c803c10 · c803c10
1 parent 609d9f6
commit c803c10
Show file tree

Hide file tree

Showing 8 changed files with 655 additions and 5 deletions.
diff --git a/.github/workflows/regression_test.yml b/.github/workflows/regression_test.yml
@@ -23,7 +23,7 @@ jobs:
         run: |
           apt-get update
           apt-get install -y wget unzip libpcl-dev python3-pip
-          pip3 install pandas matplotlib
+          pip3 install pandas matplotlib tqdm
 
       - name: Download data
         run: |
@@ -39,13 +39,19 @@ jobs:
           make -j
           cd ..
 
-      - name: Run
+      - name: Run regression test
         run: |
           ./script/execute_regression_test.sh
 
+      - name: Run check_covariance
+        run: |
+          ./script/execute_check_covariance.sh ./regression_test_data/input/ ./regression_test_data/output_check_covariance/
+
       - name: Upload output files
         uses: actions/upload-artifact@v4
         if: always()
         with:
           name: regression-test-output
-          path: ./regression_test_data/output/
+          path: |
+            ./regression_test_data/output/
+            ./regression_test_data/output_check_covariance/
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -59,6 +59,7 @@ endif()
 ament_auto_add_library(multigrid_ndt_omp SHARED
   src/multigrid_pclomp/multi_voxel_grid_covariance_omp.cpp
   src/multigrid_pclomp/multigrid_ndt_omp.cpp
+  src/estimate_covariance/estimate_covariance.cpp
 )
 target_link_libraries(multigrid_ndt_omp ${PCL_LIBRARIES})
 if(OpenMP_CXX_FOUND)
@@ -94,3 +95,18 @@ target_link_libraries(regression_test
   ${PCL_LIBRARIES}
   ndt_omp multigrid_ndt_omp
 )
+
+# check_covariance
+add_executable(check_covariance
+  apps/check_covariance.cpp
+)
+add_dependencies(check_covariance
+  ndt_omp multigrid_ndt_omp
+)
+include_directories(
+  ${PROJECT_SOURCE_DIR}/include
+)
+target_link_libraries(check_covariance
+  ${PCL_LIBRARIES}
+  ndt_omp multigrid_ndt_omp
+)
diff --git a/apps/check_covariance.cpp b/apps/check_covariance.cpp
@@ -0,0 +1,174 @@
+#include <iostream>
+#include <chrono>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/registration/ndt.h>
+#include <pcl/registration/gicp.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <omp.h>
+#include <glob.h>
+#include <filesystem>
+
+#include <pclomp/gicp_omp.h>
+#include <multigrid_pclomp/multigrid_ndt_omp.h>
+#include "estimate_covariance/estimate_covariance.hpp"
+
+#include "util.hpp"
+
+int main(int argc, char** argv) {
+  if(argc != 3) {
+    std::cout << "usage: ./regression_test <input_dir> <output_dir>" << std::endl;
+    return 0;
+  }
+
+  const std::string input_dir = argv[1];
+  const std::string output_dir = argv[2];
+
+  // load target pcd
+  const std::string target_pcd = input_dir + "/pointcloud_map.pcd";
+  const pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud = load_pcd(target_pcd);
+
+  // prepare sensor_pcd
+  const std::string source_pcd_dir = input_dir + "/sensor_pcd/";
+  const std::vector<std::string> source_pcd_list = glob(source_pcd_dir);
+
+  // load kinematic_state.csv
+  const std::vector<Eigen::Matrix4f> initial_pose_list = load_pose_list(input_dir + "/kinematic_state.csv");
+
+  if(initial_pose_list.size() != source_pcd_list.size()) {
+    std::cerr << "initial_pose_list.size() != source_pcd_list.size()" << std::endl;
+    return 1;
+  }
+  const int64_t n_data = initial_pose_list.size();
+
+  // prepare ndt
+  std::shared_ptr<pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>> mg_ndt_omp(new pclomp::MultiGridNormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
+  mg_ndt_omp->setInputTarget(target_cloud);
+  mg_ndt_omp->setResolution(2.0);
+  mg_ndt_omp->setNumThreads(4);
+  mg_ndt_omp->setMaximumIterations(30);
+  mg_ndt_omp->setTransformationEpsilon(0.01);
+  mg_ndt_omp->setStepSize(0.1);
+  mg_ndt_omp->createVoxelKdtree();
+
+  std::cout << std::fixed;
+
+  const std::vector<double> offset_x = {0.0, 0.0, 0.5, -0.5, 1.0, -1.0, 0.0, 0.0, 2.0, -2.0};
+  const std::vector<double> offset_y = {0.5, -0.5, 0.0, 0.0, 0.0, 0.0, 1.0, -1.0, 0.0, 0.0};
+
+  // output result
+  std::filesystem::create_directories(output_dir);
+  std::ofstream ofs(output_dir + "/result.csv");
+  ofs << std::fixed;
+  ofs << "index,score,";
+  ofs << "initial_x,initial_y,initial_yaw,";
+  ofs << "result_x,result_y,result_yaw,";
+  ofs << "elapsed_la,cov_by_la_00,cov_by_la_01,cov_by_la_10,cov_by_la_11,";
+  ofs << "elapsed_mndt,cov_by_mndt_00,cov_by_mndt_01,cov_by_mndt_10,cov_by_mndt_11,";
+  ofs << "elapsed_mndt_score,cov_by_mndt_score_00,cov_by_mndt_score_01,cov_by_mndt_score_10,cov_by_mndt_score_11,";
+  ofs << "cov_by_la_rotated_00,cov_by_la_rotated_01,cov_by_la_rotated_10,cov_by_la_rotated_11,";
+  ofs << "cov_by_mndt_rotated_00,cov_by_mndt_rotated_01,cov_by_mndt_rotated_10,cov_by_mndt_rotated_11,";
+  ofs << "cov_by_mndt_score_rotated_00,cov_by_mndt_score_rotated_01,cov_by_mndt_score_rotated_10,cov_by_mndt_score_rotated_11" << std::endl;
+
+  const std::string multi_ndt_dir = output_dir + "/multi_ndt";
+  std::filesystem::create_directories(multi_ndt_dir);
+  const std::string multi_ndt_score_dir = output_dir + "/multi_ndt_score";
+  std::filesystem::create_directories(multi_ndt_score_dir);
+
+  auto t1 = std::chrono::system_clock::now();
+  auto t2 = std::chrono::system_clock::now();
+
+  // execute align
+  for(int64_t i = 0; i < n_data; i++) {
+    const Eigen::Matrix4f initial_pose = initial_pose_list[i];
+    const std::string& source_pcd = source_pcd_list[i];
+    pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>());
+    if(pcl::io::loadPCDFile(source_pcd, *source_cloud)) {
+      std::cerr << "failed to load " << source_pcd << std::endl;
+      return 1;
+    }
+    mg_ndt_omp->setInputSource(source_cloud);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZ>());
+    mg_ndt_omp->align(*aligned, initial_pose);
+    const pclomp::NdtResult ndt_result = mg_ndt_omp->getResult();
+    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, offset_x, offset_y);
+
+    // estimate covariance
+    // (1) Laplace approximation
+    t1 = std::chrono::system_clock::now();
+    const Eigen::Matrix2d cov_by_la = pclomp::estimate_xy_covariance_by_Laplace_approximation(ndt_result.hessian);
+    t2 = std::chrono::system_clock::now();
+    const auto elapsed_la = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1).count() / 1000.0;
+
+    // (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, poses_to_search);
+    const Eigen::Matrix2d cov_by_mndt = pclomp::adjust_diagonal_covariance(result_of_mndt.covariance, ndt_result.pose, 0.0225, 0.0225);
+    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
+    const double temperature = 0.1;
+    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, poses_to_search, temperature);
+    const Eigen::Matrix2d cov_by_mndt_score = pclomp::adjust_diagonal_covariance(result_of_mndt_score.covariance, ndt_result.pose, 0.0225, 0.0225);
+    t2 = std::chrono::system_clock::now();
+    const auto elapsed_mndt_score = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1).count() / 1000.0;
+
+    // output result
+    const auto result_x = ndt_result.pose(0, 3);
+    const auto result_y = ndt_result.pose(1, 3);
+    const Eigen::Vector3f euler_initial = initial_pose.block<3, 3>(0, 0).eulerAngles(0, 1, 2);
+    const Eigen::Vector3f euler_result = ndt_result.pose.block<3, 3>(0, 0).eulerAngles(0, 1, 2);
+    const Eigen::Matrix2d cov_by_la_rotated = pclomp::rotate_covariance_to_base_link(cov_by_la, ndt_result.pose);
+    const Eigen::Matrix2d cov_by_mndt_rotated = pclomp::rotate_covariance_to_base_link(cov_by_mndt, ndt_result.pose);
+    const Eigen::Matrix2d cov_by_mndt_score_rotated = pclomp::rotate_covariance_to_base_link(cov_by_mndt_score, ndt_result.pose);
+    ofs << i << "," << score << ",";
+    ofs << initial_pose(0, 3) << "," << initial_pose(1, 3) << "," << euler_initial(2) << ",";
+    ofs << result_x << "," << result_y << "," << euler_result(2) << ",";
+    ofs << elapsed_la << "," << cov_by_la(0, 0) << "," << cov_by_la(0, 1) << "," << cov_by_la(1, 0) << "," << cov_by_la(1, 1) << ",";
+    ofs << elapsed_mndt << "," << cov_by_mndt(0, 0) << "," << cov_by_mndt(0, 1) << "," << cov_by_mndt(1, 0) << "," << cov_by_mndt(1, 1) << ",";
+    ofs << elapsed_mndt_score << "," << cov_by_mndt_score(0, 0) << "," << cov_by_mndt_score(0, 1) << "," << cov_by_mndt_score(1, 0) << "," << cov_by_mndt_score(1, 1) << ",";
+    ofs << cov_by_la_rotated(0, 0) << "," << cov_by_la_rotated(0, 1) << "," << cov_by_la_rotated(1, 0) << "," << cov_by_la_rotated(1, 1) << ",";
+    ofs << cov_by_mndt_rotated(0, 0) << "," << cov_by_mndt_rotated(0, 1) << "," << cov_by_mndt_rotated(1, 0) << "," << cov_by_mndt_rotated(1, 1) << ",";
+    ofs << cov_by_mndt_score_rotated(0, 0) << "," << cov_by_mndt_score_rotated(0, 1) << "," << cov_by_mndt_score_rotated(1, 0) << "," << cov_by_mndt_score_rotated(1, 1) << std::endl;
+
+    std::stringstream filename_ss;
+    filename_ss << std::setw(8) << std::setfill('0') << i << ".csv";
+
+    // output multi ndt result
+    std::ofstream ofs_mndt(multi_ndt_dir + "/" + filename_ss.str());
+    const int n_mndt = result_of_mndt.ndt_results.size();
+    ofs_mndt << "index,score,initial_x,initial_y,result_x,result_y" << std::endl;
+    ofs_mndt << std::fixed;
+    for(int j = 0; j < n_mndt; j++) {
+      const pclomp::NdtResult& ndt_result = result_of_mndt.ndt_results[j];
+      const auto nvtl = ndt_result.nearest_voxel_transformation_likelihood;
+      const auto initial_x = result_of_mndt.ndt_initial_poses[j](0, 3);
+      const auto initial_y = result_of_mndt.ndt_initial_poses[j](1, 3);
+      const auto result_x = ndt_result.pose(0, 3);
+      const auto result_y = ndt_result.pose(1, 3);
+      ofs_mndt << j << "," << nvtl << "," << initial_x << "," << initial_y << "," << result_x << "," << result_y << std::endl;
+    }
+
+    // output multi ndt score result
+    std::ofstream ofs_mndt_score(multi_ndt_score_dir + "/" + filename_ss.str());
+    const int n_mndt_score = result_of_mndt_score.ndt_results.size();
+    ofs_mndt_score << "index,score,initial_x,initial_y,result_x,result_y" << std::endl;
+    ofs_mndt_score << std::fixed;
+    for(int j = 0; j < n_mndt_score; j++) {
+      const pclomp::NdtResult& ndt_result = result_of_mndt_score.ndt_results[j];
+      const auto nvtl = ndt_result.nearest_voxel_transformation_likelihood;
+      const auto initial_x = result_of_mndt_score.ndt_initial_poses[j](0, 3);
+      const auto initial_y = result_of_mndt_score.ndt_initial_poses[j](1, 3);
+      const auto result_x = ndt_result.pose(0, 3);
+      const auto result_y = ndt_result.pose(1, 3);
+      ofs_mndt_score << j << "," << nvtl << "," << initial_x << "," << initial_y << "," << result_x << "," << result_y << std::endl;
+    }
+  }
+}
diff --git a/include/estimate_covariance/estimate_covariance.hpp b/include/estimate_covariance/estimate_covariance.hpp
@@ -0,0 +1,52 @@
+#ifndef NDT_OMP__ESTIMATE_COVARIANCE_HPP_
+#define NDT_OMP__ESTIMATE_COVARIANCE_HPP_
+
+#include <Eigen/Core>
+#include "multigrid_pclomp/multigrid_ndt_omp.h"
+
+namespace pclomp {
+
+struct ResultOfMultiNdtCovarianceEstimation {
+  Eigen::Vector2d mean;
+  Eigen::Matrix2d covariance;
+  std::vector<Eigen::Matrix4f> ndt_initial_poses;
+  std::vector<NdtResult> ndt_results;
+};
+
+/** \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 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, const double temperature);
+
+/** \brief Find rotation matrix aligning covariance to principal axes
+ * (1) Compute eigenvalues and eigenvectors
+ * (2) Compute angle for first eigenvector
+ * (3) Return rotation matrix
+ */
+Eigen::Matrix2d find_rotation_matrix_aligning_covariance_to_principal_axes(const Eigen::Matrix2d& matrix);
+
+/** \brief Propose poses to search.
+ * (1) Compute covariance by Laplace approximation
+ * (2) Find rotation matrix aligning covariance to principal axes
+ * (3) Propose search points by adding offset_x and offset_y to the center_pose
+ */
+std::vector<Eigen::Matrix4f> propose_poses_to_search(const NdtResult& ndt_result, const std::vector<double>& offset_x, const std::vector<double>& offset_y);
+
+/** \brief Calculate weights by exponential */
+std::vector<double> calc_weight_vec(const std::vector<double>& score_vec, double temperature);
+
+/** \brief Calculate weighted mean and covariance */
+std::pair<Eigen::Vector2d, Eigen::Matrix2d> calculate_weighted_mean_and_cov(const std::vector<Eigen::Vector2d>& pose_2d_vec, const std::vector<double>& weight_vec);
+
+/** \brief Rotate covariance to base_link coordinate */
+Eigen::Matrix2d rotate_covariance_to_base_link(const Eigen::Matrix2d& covariance, const Eigen::Matrix4f& pose);
+
+/** \brief Rotate covariance to map coordinate */
+Eigen::Matrix2d rotate_covariance_to_map(const Eigen::Matrix2d& covariance, const Eigen::Matrix4f& pose);
+
+/** \brief  Adjust diagonal covariance */
+Eigen::Matrix2d adjust_diagonal_covariance(const Eigen::Matrix2d& covariance, const Eigen::Matrix4f& pose, const double fixed_cov00, const double fixed_cov11);
+
+}  // namespace pclomp
+
+#endif  // NDT_OMP__ESTIMATE_COVARIANCE_HPP_
diff --git a/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp b/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp
@@ -1015,8 +1015,8 @@ double MultiGridNormalDistributionsTransform<PointSource, PointTarget>::calculat
 
   // Sum up point-wise scores
   for(size_t idx = 0; idx < params_.num_threads; ++idx) {
-    found_neighborhood_voxel_num += t_nvs[idx];
-    nearest_voxel_score += t_found_nnvn[idx];
+    found_neighborhood_voxel_num += t_found_nnvn[idx];
+    nearest_voxel_score += t_nvs[idx];
   }
 
   double output_score = 0;

diff --git a/script/execute_check_covariance.sh b/script/execute_check_covariance.sh
@@ -0,0 +1,13 @@
+#!/bin/bash
+set -eux
+
+INPUT_DIR=$(readlink -f $1)
+OUTPUT_DIR=$(readlink -f $2)
+
+cd $(dirname $0)/../build
+
+make -j8
+
+rm -rf ${OUTPUT_DIR}
+./check_covariance ${INPUT_DIR} ${OUTPUT_DIR}
+python3 ../script/plot_covariance.py ${OUTPUT_DIR}/result.csv