From ae2f3cb2d10e6214cd88513278d04c232d3e261e Mon Sep 17 00:00:00 2001
From: koide3 <31344317+koide3@users.noreply.github.com>
Date: Fri, 5 Apr 2024 15:56:05 +0900
Subject: [PATCH] voxel point indices test (#12)

* add comments

* voxel point indices test
---
 .../small_gicp/ann/incremental_voxelmap.hpp   | 15 ++++++++++
 ...dometry_benchmark_small_gicp_model_tbb.cpp |  1 +
 ...ometry_benchmark_small_vgicp_model_tbb.cpp |  1 +
 src/test/kdtree_test.cpp                      | 29 +++++++++++++++++++
 4 files changed, 46 insertions(+)

diff --git a/include/small_gicp/ann/incremental_voxelmap.hpp b/include/small_gicp/ann/incremental_voxelmap.hpp
index 4c31151..180797b 100644
--- a/include/small_gicp/ann/incremental_voxelmap.hpp
+++ b/include/small_gicp/ann/incremental_voxelmap.hpp
@@ -192,6 +192,21 @@ struct Traits<IncrementalVoxelMap<VoxelContents>> {
   }
 };
 
+template <typename VoxelContents>
+std::vector<size_t> point_indices(const IncrementalVoxelMap<VoxelContents>& voxelmap) {
+  std::vector<size_t> indices;
+  indices.reserve(voxelmap.size() * 5);
+
+  for (size_t voxel_id = 0; voxel_id < voxelmap.flat_voxels.size(); voxel_id++) {
+    const auto& voxel = voxelmap.flat_voxels[voxel_id];
+    for (size_t point_id = 0; point_id < traits::size(voxel->second); point_id++) {
+      indices.emplace_back(voxelmap.calc_index(voxel_id, point_id));
+    }
+  }
+
+  return indices;
+}
+
 template <typename VoxelContents>
 std::vector<Eigen::Vector4d> voxel_points(const IncrementalVoxelMap<VoxelContents>& voxelmap) {
   std::vector<Eigen::Vector4d> points;
diff --git a/src/benchmark/odometry_benchmark_small_gicp_model_tbb.cpp b/src/benchmark/odometry_benchmark_small_gicp_model_tbb.cpp
index 1bf5764..7942624 100644
--- a/src/benchmark/odometry_benchmark_small_gicp_model_tbb.cpp
+++ b/src/benchmark/odometry_benchmark_small_gicp_model_tbb.cpp
@@ -23,6 +23,7 @@ class SmallGICPModelOnlineOdometryEstimationTBB : public OnlineOdometryEstimatio
     Stopwatch sw;
     sw.start();
 
+    // Note that input points here is already downsampled
     // Estimate per-point covariances
     estimate_covariances_tbb(*points, params.num_neighbors);
 
diff --git a/src/benchmark/odometry_benchmark_small_vgicp_model_tbb.cpp b/src/benchmark/odometry_benchmark_small_vgicp_model_tbb.cpp
index 5511fb7..07142bb 100644
--- a/src/benchmark/odometry_benchmark_small_vgicp_model_tbb.cpp
+++ b/src/benchmark/odometry_benchmark_small_vgicp_model_tbb.cpp
@@ -23,6 +23,7 @@ class SmallVGICPModelOnlineOdometryEstimationTBB : public OnlineOdometryEstimati
     Stopwatch sw;
     sw.start();
 
+    // Note that input points here is already downsampled
     // Estimate per-point covariances
     estimate_covariances_tbb(*points, params.num_neighbors);
 
diff --git a/src/test/kdtree_test.cpp b/src/test/kdtree_test.cpp
index 5de7d50..4b45b55 100644
--- a/src/test/kdtree_test.cpp
+++ b/src/test/kdtree_test.cpp
@@ -10,6 +10,7 @@
 #include <small_gicp/ann/gaussian_voxelmap.hpp>
 #include <small_gicp/ann/incremental_voxelmap.hpp>
 #include <small_gicp/util/downsampling.hpp>
+#include <small_gicp/util/normal_estimation.hpp>
 #include <small_gicp/points/point_cloud.hpp>
 #include <small_gicp/pcl/pcl_point_traits.hpp>
 #include <small_gicp/benchmark/read_points.hpp>
@@ -22,6 +23,7 @@ class KdTreeTest : public testing::Test, public testing::WithParamInterface<std:
     // Load points
     auto points_4f = read_ply("data/target.ply");
     points = voxelgrid_sampling(*std::make_shared<PointCloud>(points_4f), 0.5);
+    estimate_normals_covariances(*points);
 
     points_pcl = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
     points_pcl->resize(points->size());
@@ -199,6 +201,21 @@ TEST_P(KdTreeTest, KnnTest) {
     voxelmap->insert(*points);
     test_voxelmap(*points, *voxelmap);
 
+    auto indices = traits::point_indices(*voxelmap);
+    auto voxel_points = traits::voxel_points(*voxelmap);
+    auto voxel_normals = traits::voxel_normals(*voxelmap);
+    auto voxel_covs = traits::voxel_covs(*voxelmap);
+
+    EXPECT_EQ(indices.size(), voxel_points.size());
+    EXPECT_EQ(indices.size(), voxel_normals.size());
+    EXPECT_EQ(indices.size(), voxel_covs.size());
+
+    for (size_t i = 0; i < indices.size(); i++) {
+      EXPECT_NEAR((voxel_points[i] - traits::point(*voxelmap, indices[i])).squaredNorm(), 0.0, 1e-6);
+      EXPECT_NEAR((voxel_normals[i] - traits::normal(*voxelmap, indices[i])).squaredNorm(), 0.0, 1e-6);
+      EXPECT_NEAR((voxel_covs[i] - traits::cov(*voxelmap, indices[i])).squaredNorm(), 0.0, 1e-6);
+    }
+
     auto voxelmap_pcl = std::make_shared<IncrementalVoxelMap<FlatContainer<>>>(1.0);
     voxelmap_pcl->insert(*points_pcl);
     test_voxelmap(*points, *voxelmap_pcl);
@@ -206,6 +223,18 @@ TEST_P(KdTreeTest, KnnTest) {
     auto voxelmap = std::make_shared<GaussianVoxelMap>(1.0);
     voxelmap->insert(*points);
     test_voxelmap(*points, *voxelmap);
+
+    auto indices = traits::point_indices(*voxelmap);
+    auto voxel_points = traits::voxel_points(*voxelmap);
+    auto voxel_covs = traits::voxel_covs(*voxelmap);
+
+    EXPECT_EQ(indices.size(), voxel_points.size());
+    EXPECT_EQ(indices.size(), voxel_covs.size());
+
+    for (size_t i = 0; i < indices.size(); i++) {
+      EXPECT_NEAR((voxel_points[i] - traits::point(*voxelmap, indices[i])).squaredNorm(), 0.0, 1e-6);
+      EXPECT_NEAR((voxel_covs[i] - traits::cov(*voxelmap, indices[i])).squaredNorm(), 0.0, 1e-6);
+    }
   } else {
     throw std::runtime_error("Invalid method: " + method);
   }