feat: add covariance estimation

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)
@@ -83,3 +84,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
+)

apps/check_covariance.cpp

@@ -0,0 +1,141 @@
+#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"
+
+std::vector<std::string> glob(const std::string& input_dir) {
+  glob_t buffer;
+  std::vector<std::string> files;
+  glob((input_dir + "/*").c_str(), 0, NULL, &buffer);
+  for(size_t i = 0; i < buffer.gl_pathc; i++) {
+    files.push_back(buffer.gl_pathv[i]);
+  }
+  globfree(&buffer);
+  std::sort(files.begin(), files.end());
+  return files;
+}
+
+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";
+  pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>());
+  if(pcl::io::loadPCDFile(target_pcd, *target_cloud)) {
+    std::cerr << "failed to load " << target_pcd << std::endl;
+    return 1;
+  }
+
+  // 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();
+
+  // prepare sensor_pcd
+  const std::string source_pcd_dir = input_dir + "/sensor_pcd/";
+  std::vector<std::string> source_pcd_list = glob(source_pcd_dir);
+
+  // prepare results
+  std::vector<double> elapsed_milliseconds;
+  std::vector<double> scores;
+
+  // load kinematic_state.csv
+  /*
+  timestamp,pose_x,pose_y,pose_z,quat_w,quat_x,quat_y,quat_z,twist_linear_x,twist_linear_y,twist_linear_z,twist_angular_x,twist_angular_y,twist_angular_z
+  63.100010,81377.359702,49916.899866,41.232589,0.953768,0.000494,-0.007336,0.300453,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+  63.133344,81377.359780,49916.899912,41.232735,0.953769,0.000491,-0.007332,0.300452,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
+  ...
+  */
+  std::ifstream ifs(input_dir + "/kinematic_state.csv");
+  std::string line;
+  std::getline(ifs, line);  // skip header
+  std::vector<Eigen::Matrix4f> initial_pose_list;
+  while(std::getline(ifs, line)) {
+    std::istringstream iss(line);
+    std::string token;
+    std::vector<std::string> tokens;
+    while(std::getline(iss, token, ',')) {
+      tokens.push_back(token);
+    }
+    const double timestamp = std::stod(tokens[0]);
+    const double pose_x = std::stod(tokens[1]);
+    const double pose_y = std::stod(tokens[2]);
+    const double pose_z = std::stod(tokens[3]);
+    const double quat_w = std::stod(tokens[4]);
+    const double quat_x = std::stod(tokens[5]);
+    const double quat_y = std::stod(tokens[6]);
+    const double quat_z = std::stod(tokens[7]);
+    Eigen::Matrix4f initial_pose = Eigen::Matrix4f::Identity();
+    initial_pose.block<3, 3>(0, 0) = Eigen::Quaternionf(quat_w, quat_x, quat_y, quat_z).toRotationMatrix();
+    initial_pose.block<3, 1>(0, 3) = Eigen::Vector3f(pose_x, pose_y, pose_z);
+    initial_pose_list.push_back(initial_pose);
+  }
+
+  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();
+
+  std::cout << std::fixed;
+
+  const std::vector<double> offset_x = {0.0, 0.0, 0.5, -0.5, 1.0, -1.0};
+  const std::vector<double> offset_y = {0.5, -0.5, 0.0, 0.0, 0.0, 0.0};
+
+  // output result
+  mkdir(output_dir.c_str(), 0777);
+  std::ofstream ofs(output_dir + "/result.csv");
+  ofs << std::fixed;
+  ofs << "index,elapsed_milliseconds,score,x,y,cov_by_la_00,cov_by_la_01,cov_by_la_10,cov_by_la_11,cov_by_mndt_00,cov_by_mndt_01,cov_by_mndt_10,cov_by_mndt_11" << std::endl;
+
+  // 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>());
+    auto t1 = std::chrono::system_clock::now();
+    mg_ndt_omp->align(*aligned, initial_pose);
+    const pclomp::NdtResult ndt_result = mg_ndt_omp->getResult();
+    auto t2 = std::chrono::system_clock::now();
+    const auto elapsed = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1).count() / 1000.0;
+    const double score = ndt_result.nearest_voxel_transformation_likelihood;
+    elapsed_milliseconds.push_back(elapsed);
+    scores.push_back(score);
+    std::cout << source_pcd << ", num=" << std::setw(4) << source_cloud->size() << " points, time=" << elapsed << " [msec], score=" << score << std::endl;
+
+    // estimate covariance
+    const Eigen::Matrix2d cov_by_la = pclomp::estimate_xy_covariance_by_Laplace_approximation(ndt_result);
+    const Eigen::Matrix2d cov_by_mndt = pclomp::estimate_xy_covariance_by_multi_ndt(ndt_result, mg_ndt_omp, initial_pose, offset_x, offset_y);
+
+    ofs << i << "," << elapsed << "," << score << "," << initial_pose(0, 3) << "," << initial_pose(1, 3) << "," << cov_by_la(0, 0) << "," << cov_by_la(0, 1) << "," << cov_by_la(1, 0) << "," << cov_by_la(1, 1) << "," << cov_by_mndt(0, 0) << "," << cov_by_mndt(0, 1) << "," << cov_by_mndt(1, 0) << "," << cov_by_mndt(1, 1) << std::endl;
+  }
+}

include/estimate_covariance/estimate_covariance.hpp

@@ -0,0 +1,15 @@
+#ifndef NDT_OMP__ESTIMATE_COVARIANCE_HPP_
+#define NDT_OMP__ESTIMATE_COVARIANCE_HPP_
+
+#include <Eigen/Core>
+#include "multigrid_pclomp/multigrid_ndt_omp.h"
+
+namespace pclomp {
+
+Eigen::Matrix2d find_rotation_matrix_aligning_covariance_to_principal_axes(const Eigen::Matrix2d& matrix);
+Eigen::Matrix2d estimate_xy_covariance_by_Laplace_approximation(const NdtResult& ndt_result);
+Eigen::Matrix2d 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);
+
+}  // namespace pclomp
+
+#endif  // NDT_OMP__ESTIMATE_COVARIANCE_HPP_

script/execute_check_covariance.sh

@@ -0,0 +1,8 @@
+#!/bin/bash
+set -eux
+
+cd $(dirname $0)/../build
+
+make -j8
+./check_covariance ../check_covariance_data/input/ ../check_covariance_data/output
+python3 ../script/plot_covariance.py ../check_covariance_data/output/result.csv

script/plot_covariance.py

@@ -0,0 +1,103 @@
+"""This script plots the covariance of ndt.
+"""
+
+import argparse
+import pathlib
+import pandas as pd
+import matplotlib.pyplot as plt
+from matplotlib.patches import Ellipse
+import numpy as np
+from tqdm import tqdm
+
+
+def parse_args():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("result_csv", type=pathlib.Path)
+    return parser.parse_args()
+
+
+def plot_ellipse(mean, cov, color, label, scale):
+    eigenvalues, eigenvectors = np.linalg.eig(cov)
+    eigenvalues = np.maximum(eigenvalues, 0)
+    angle = np.degrees(np.arctan2(eigenvectors[1, 0], eigenvectors[0, 0]))
+    width, height = 2 * np.sqrt(eigenvalues) * scale
+    ellipse = Ellipse(
+        xy=mean,
+        width=width,
+        height=height,
+        angle=angle,
+        color=color,
+        alpha=0.5,
+        fill=False,
+        label=f"{label}(scale={scale})",
+    )
+    plt.gca().add_patch(ellipse)
+
+
+if __name__ == "__main__":
+    args = parse_args()
+    result_csv = args.result_csv
+
+    df_result = pd.read_csv(result_csv)
+
+    """
+    df_result
+    index,elapsed_milliseconds,score,x,y,cov_by_la_00,cov_by_la_01,cov_by_la_10,cov_by_la_11,cov_by_mndt_00,cov_by_mndt_01,cov_by_mndt_10,cov_by_mndt_11
+    0,586.745000,1.649302,81377.359375,49916.902344,0.000205,0.000009,0.000009,0.000206,0.000015,0.000000,0.000000,0.000000
+    1,7.622000,1.645455,81377.359375,49916.902344,0.000206,0.000009,0.000009,0.000207,0.000017,0.000001,0.000001,0.000004
+    ...
+    """
+
+    cov_by_la = df_result[
+        ["cov_by_la_00", "cov_by_la_01", "cov_by_la_10", "cov_by_la_11"]
+    ].values
+    cov_by_mndt = df_result[
+        ["cov_by_mndt_00", "cov_by_mndt_01", "cov_by_mndt_10", "cov_by_mndt_11"]
+    ].values
+
+    for i in range(2):
+        for j in range(2):
+            plt.subplot(2, 2, i * 2 + j + 1)
+            plt.plot(cov_by_la[:, i * 2 + j], label="Laplace Approximation")
+            plt.plot(cov_by_mndt[:, i * 2 + j], label="Multi NDT")
+            plt.ylabel(f"cov_{i}{j}")
+            plt.legend()
+    plt.tight_layout()
+    save_path = result_csv.parent / "covariance.png"
+    plt.savefig(save_path, bbox_inches="tight", pad_inches=0.05)
+    print(f"Saved: {save_path}")
+    plt.close()
+
+    df_result["x"] -= df_result["x"].mean()
+    df_result["y"] -= df_result["y"].mean()
+
+    cov_default = 0.0225 * np.eye(2)
+
+    # plot each frame
+    output_dir = result_csv.parent / "covariance_each_frame"
+    output_dir.mkdir(exist_ok=True, parents=True)
+    progress = tqdm(total=len(df_result))
+    for i, row in df_result.iterrows():
+        progress.update(1)
+        # plot ellipse
+        plt.figure()
+        cov_by_la = row[
+            ["cov_by_la_00", "cov_by_la_01", "cov_by_la_10", "cov_by_la_11"]
+        ].values.reshape(2, 2)
+        cov_by_mndt = row[
+            ["cov_by_mndt_00", "cov_by_mndt_01", "cov_by_mndt_10", "cov_by_mndt_11"]
+        ].values.reshape(2, 2)
+        x, y = row["x"], row["y"]
+        plot_ellipse([x, y], cov_default, "green", "Default", 10)
+        plot_ellipse([x, y], cov_by_la, "blue", "Laplace Approximation", 100)
+        plot_ellipse([x, y], cov_by_mndt, "red", "Multi NDT", 100)
+        plt.scatter(df_result["x"][0:i], df_result["y"][0:i], color="black", s=1)
+        plt.legend(loc="upper center", bbox_to_anchor=(0.5, 1.15))
+        plt.grid()
+        plt.xlabel("x[m]")
+        plt.ylabel("y[m]")
+        plt.xlim(x - 10, x + 10)
+        plt.ylim(y - 10, y + 10)
+        plt.gca().set_aspect("equal", adjustable="box")
+        plt.savefig(output_dir / f"{i:08d}.png", bbox_inches="tight", pad_inches=0.05)
+        plt.close()

src/estimate_covariance/estimate_covariance.cpp

@@ -0,0 +1,72 @@
+#include "estimate_covariance/estimate_covariance.hpp"
+
+namespace pclomp {
+
+Eigen::Matrix2d find_rotation_matrix_aligning_covariance_to_principal_axes(const Eigen::Matrix2d& matrix) {
+  const Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigensolver(matrix);
+  if(eigensolver.info() == Eigen::Success) {
+    const Eigen::Vector2d eigen_vec = eigensolver.eigenvectors().col(0);
+    const double th = std::atan2(eigen_vec.y(), eigen_vec.x());
+    return Eigen::Rotation2Dd(th).toRotationMatrix();
+  }
+  throw std::runtime_error("Eigen solver failed. Return output_pose_covariance value.");
+}
+
+Eigen::Matrix2d estimate_xy_covariance_by_Laplace_approximation(const NdtResult& ndt_result) {
+  const Eigen::Matrix<double, 6, 6>& hessian = ndt_result.hessian;
+  const Eigen::Matrix2d hessian_xy = hessian.block<2, 2>(0, 0);
+  const Eigen::Matrix2d covariance_xy = -hessian_xy.inverse();
+  return covariance_xy;
+}
+
+Eigen::Matrix2d 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 Eigen::Matrix2d cov_by_la = estimate_xy_covariance_by_Laplace_approximation(ndt_result);
+  const Eigen::Matrix2d rot = find_rotation_matrix_aligning_covariance_to_principal_axes(-cov_by_la);
+
+  assert(offset_x.size() == offset_y.size());
+
+  // first result is added to mean
+  const int n = static_cast<int>(offset_x.size()) + 1;
+  const Eigen::Vector2d ndt_pose_2d(ndt_result.pose(0, 3), ndt_result.pose(1, 3));
+  Eigen::Vector2d mean = ndt_pose_2d;
+  std::vector<Eigen::Vector2d> ndt_pose_2d_vec;
+  ndt_pose_2d_vec.reserve(n);
+  ndt_pose_2d_vec.emplace_back(ndt_pose_2d);
+
+  std::vector<Eigen::Matrix4f> initial_pose_vec = {initial_pose};
+  std::vector<Eigen::Matrix4f> ndt_result_pose_vec = {ndt_result.pose};
+
+  // multiple searches
+  for(int i = 0; i < n - 1; i++) {
+    const Eigen::Vector2d pose_offset(offset_x[i], offset_y[i]);
+    const Eigen::Vector2d rotated_pose_offset_2d = rot * pose_offset;
+
+    Eigen::Matrix4f sub_initial_pose_matrix(Eigen::Matrix4f::Identity());
+    sub_initial_pose_matrix = ndt_result.pose;
+    sub_initial_pose_matrix(0, 3) += static_cast<float>(rotated_pose_offset_2d.x());
+    sub_initial_pose_matrix(1, 3) += static_cast<float>(rotated_pose_offset_2d.y());
+
+    auto sub_output_cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+    ndt_ptr->align(*sub_output_cloud, sub_initial_pose_matrix);
+    const Eigen::Matrix4f sub_ndt_result = ndt_ptr->getResult().pose;
+
+    const Eigen::Vector2d sub_ndt_pose_2d = sub_ndt_result.topRightCorner<2, 1>().cast<double>();
+    mean += sub_ndt_pose_2d;
+    ndt_pose_2d_vec.emplace_back(sub_ndt_pose_2d);
+
+    initial_pose_vec.push_back(sub_initial_pose_matrix);
+    ndt_result_pose_vec.push_back(sub_ndt_result);
+  }
+
+  // calculate the covariance matrix
+  mean /= n;
+  Eigen::Matrix2d pca_covariance = Eigen::Matrix2d::Zero();
+  for(const auto& temp_ndt_pose_2d : ndt_pose_2d_vec) {
+    const Eigen::Vector2d diff_2d = temp_ndt_pose_2d - mean;
+    pca_covariance += diff_2d * diff_2d.transpose();
+  }
+  pca_covariance /= (n - 1);  // unbiased covariance
+  return pca_covariance;
+}
+
+}  // namespace pclomp