tests

CMakeLists.txt

@@ -113,7 +113,6 @@ target_link_libraries(downsampling_benchmark
 )
 
 
-
 ##########
 ## Test ##
 ##########
@@ -143,6 +142,6 @@ if(BUILD_TESTS)
       ${TBB_LIBRARIES}
     )
 
-    gtest_discover_tests(${TEST_NAME})
+    gtest_discover_tests(${TEST_NAME} WORKING_DIRECTORY ${CMAKE_SOURCE_DIR})
   endforeach()
 endif()

include/small_gicp/ann/kdtree.hpp

@@ -8,9 +8,6 @@
 
 namespace small_gicp {
 
-template <typename Distance, class DatasetAdaptor, int DIM = -1, typename IndexType = size_t>
-class KDTreeSingleIndexAdaptor;
-
 /// @brief Unsafe KdTree with arbitrary nanoflann Adaptor.
 /// @note  This class does not hold the ownership of the input points.
 ///        You must keep the input points along with this class.

include/small_gicp/benchmark/read_points.hpp

@@ -6,7 +6,7 @@
 
 namespace small_gicp {
 
-/// @brief Read points from file (simple float4 array)
+/// @brief Read points from file (simple float4 array).
 /// @param filename  Filename
 /// @return          Points
 static std::vector<Eigen::Vector4f> read_points(const std::string& filename) {
@@ -29,4 +29,80 @@ static std::vector<Eigen::Vector4f> read_points(const std::string& filename) {
   return points;
 }
 
+/// @brief Write points to file (simple float4 array).
+/// @param filename  Filename
+/// @param points    Points
+static void write_points(const std::string& filename, const std::vector<Eigen::Vector4f>& points) {
+  std::ofstream ofs(filename, std::ios::binary);
+  if (!ofs) {
+    std::cerr << "error: failed to open " << filename << std::endl;
+    return;
+  }
+
+  ofs.write(reinterpret_cast<const char*>(points.data()), sizeof(Eigen::Vector4f) * points.size());
+}
+
+/// @brief Read point cloud from a PLY file.
+/// @note  This function can only handle simple PLY files with float properties (Property names must be "x", "y", "z"). Do not use this for general PLY IO.
+/// @param filename  Filename
+/// @return          Points
+static std::vector<Eigen::Vector4f> read_ply(const std::string& filename) {
+  std::ifstream ifs(filename);
+  if (!ifs) {
+    std::cerr << "error: failed to open " << filename << std::endl;
+    return {};
+  }
+
+  std::vector<std::string> properties;
+  std::vector<Eigen::Vector4f> points;
+
+  std::string line;
+  while (!ifs.eof() && std::getline(ifs, line) && !line.empty()) {
+    if (line == "end_header") {
+      break;
+    }
+
+    if (line.starts_with("element")) {
+      std::stringstream sst(line);
+      std::string token, vertex, num_points;
+      sst >> token >> vertex >> num_points;
+      if (token != "element" || vertex != "vertex") {
+        std::cerr << "error: invalid ply format (line=" << line << ")" << std::endl;
+        return {};
+      }
+
+      points.resize(std::stol(num_points));
+    } else if (line.starts_with("property")) {
+      std::stringstream sst(line);
+      std::string token, type, name;
+      sst >> token >> type >> name;
+      if (type != "float") {
+        std::cerr << "error: only float properties are supported!! (line=" << line << ")" << std::endl;
+        return {};
+      }
+
+      properties.emplace_back(name);
+    }
+  }
+
+  if (
+    line.size() < 3 || properties[0].size() != 1 || properties[1].size() != 1 || properties[2].size() != 1 || std::tolower(properties[0][0]) != 'x' ||
+    std::tolower(properties[1][0]) != 'y' || std::tolower(properties[2][0]) != 'z') {
+    std::cerr << "warning: invalid properties!!" << std::endl;
+    for (const auto& prop : properties) {
+      std::cerr << " - " << prop << std::endl;
+    }
+  }
+
+  std::vector<float> buffer(properties.size() * points.size());
+  ifs.read(reinterpret_cast<char*>(buffer.data()), sizeof(Eigen::Vector4f) * points.size());
+
+  for (size_t i = 0; i < points.size(); i++) {
+    const int stride = properties.size();
+    points[i] = Eigen::Vector4f(buffer[i * stride + 0], buffer[i * stride + 1], buffer[i * stride + 2], 1.0);
+  }
+
+  return points;
+}
+
 }  // namespace small_gicp

include/small_gicp/points/point_cloud.hpp

@@ -33,6 +33,9 @@ public:
   /// @brief Number of points
   size_t size() const { return points.size(); }
 
+  /// @brief Check if the point cloud is empty
+  bool empty() const { return points.empty(); }
+
   /// @brief Resize point/normal/cov buffers
   /// @param n  Number of points
   void resize(size_t n) {

include/small_gicp/util/downsampling.hpp

@@ -18,7 +18,6 @@ namespace small_gicp {
 template <typename InputPointCloud, typename OutputPointCloud = InputPointCloud>
 std::shared_ptr<OutputPointCloud> voxelgrid_sampling(const InputPointCloud& points, double leaf_size) {
   if (traits::size(points) == 0) {
-    std::cerr << "warning: empty input points!!" << std::endl;
     return std::make_shared<OutputPointCloud>();
   }
 

include/small_gicp/util/downsampling_omp.hpp

@@ -19,7 +19,6 @@ namespace small_gicp {
 template <typename InputPointCloud, typename OutputPointCloud = InputPointCloud>
 std::shared_ptr<OutputPointCloud> voxelgrid_sampling_omp(const InputPointCloud& points, double leaf_size, int num_threads = 4) {
   if (traits::size(points) == 0) {
-    std::cerr << "warning: empty input points!!" << std::endl;
     return std::make_shared<OutputPointCloud>();
   }
 

include/small_gicp/util/downsampling_tbb.hpp

@@ -19,7 +19,6 @@ namespace small_gicp {
 template <typename InputPointCloud, typename OutputPointCloud = InputPointCloud>
 std::shared_ptr<OutputPointCloud> voxelgrid_sampling_tbb(const InputPointCloud& points, double leaf_size) {
   if (traits::size(points) == 0) {
-    std::cerr << "warning: empty input points!!" << std::endl;
     return std::make_shared<OutputPointCloud>();
   }
 
@@ -50,7 +49,7 @@ std::shared_ptr<OutputPointCloud> voxelgrid_sampling_tbb(const InputPointCloud&
   traits::resize(*downsampled, traits::size(points));
 
   // Take block-wise sum
-  const int block_size = 1024;
+  const int block_size = 2048;
   std::atomic_uint64_t num_points = 0;
   tbb::parallel_for(tbb::blocked_range<size_t>(0, traits::size(points), block_size), [&](const tbb::blocked_range<size_t>& range) {
     std::vector<Eigen::Vector4d> sub_points;

src/test/downsampling_test.cpp

@@ -1,23 +1,94 @@
 #include <gtest/gtest.h>
 #include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
+#include <pcl/filters/voxel_grid.h>
 
 #include <small_gicp/points/point_cloud.hpp>
+#include <small_gicp/points/pcl_point.hpp>
 #include <small_gicp/points/pcl_point_traits.hpp>
 #include <small_gicp/util/downsampling.hpp>
 #include <small_gicp/util/downsampling_omp.hpp>
 #include <small_gicp/util/downsampling_tbb.hpp>
+#include <small_gicp/benchmark/read_points.hpp>
 
 using namespace small_gicp;
 
-TEST(DownsamplingTest, NullTest) {
-  auto points = std::make_shared<small_gicp::PointCloud>();
-  EXPECT_EQ(small_gicp::voxelgrid_sampling(*points, 0.1)->size(), 0);
-  EXPECT_EQ(small_gicp::voxelgrid_sampling_omp(*points, 0.1)->size(), 0);
-  EXPECT_EQ(small_gicp::voxelgrid_sampling_tbb(*points, 0.1)->size(), 0);
-
-  auto points_pcl = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
-  EXPECT_EQ(small_gicp::voxelgrid_sampling(*points_pcl, 0.1)->size(), 0);
-  EXPECT_EQ(small_gicp::voxelgrid_sampling_omp(*points_pcl, 0.1)->size(), 0);
-  EXPECT_EQ(small_gicp::voxelgrid_sampling_tbb(*points_pcl, 0.1)->size(), 0);
+class DownsamplingTest : public testing::Test, public testing::WithParamInterface<std::string> {
+public:
+  void SetUp() override {
+    // Load points
+    auto points_4f = read_ply("data/target.ply");
+    points = std::make_shared<PointCloud>(points_4f);
+    points_pcl = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+    points_pcl->resize(points_4f.size());
+    for (size_t i = 0; i < points_4f.size(); i++) {
+      points_pcl->at(i).getVector4fMap() = points_4f[i];
+    }
+
+    // Downsample points using pcl::VoxelGrid for reference
+    resolutions = {0.1, 0.5, 1.0};
+    for (double resolution : resolutions) {
+      pcl::VoxelGrid<pcl::PointXYZ> voxelgrid;
+      voxelgrid.setLeafSize(resolution, resolution, resolution);
+      voxelgrid.setInputCloud(points_pcl);
+
+      auto downsampled = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+      voxelgrid.filter(*downsampled);
+      downsampled_pcl.push_back(downsampled);
+    }
+  }
+
+  // Apply downsampling
+  template <typename PointCloud>
+  std::shared_ptr<PointCloud> downsample(const PointCloud& points, double resolution) {
+    const std::string method = GetParam();
+    if (method == "SMALL") {
+      return voxelgrid_sampling(points, resolution);
+    } else if (method == "TBB") {
+      return voxelgrid_sampling_tbb(points, resolution);
+    } else if (method == "OMP") {
+      return voxelgrid_sampling_omp(points, resolution);
+    } else {
+      throw std::runtime_error("Invalid method: " + method);
+    }
+  }
+
+protected:
+  std::vector<double> resolutions;                                   ///< Downsampling resolutions
+  PointCloud::Ptr points;                                            ///< Input points
+  pcl::PointCloud<pcl::PointXYZ>::Ptr points_pcl;                    ///< Input points (pcl)
+  std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> downsampled_pcl;  ///< Reference downsampling results (pcl)
+};
+
+// Load check
+TEST_F(DownsamplingTest, LoadCheck) {
+  EXPECT_FALSE(points->empty());
+  EXPECT_FALSE(points_pcl->empty());
+  for (auto downsampled : downsampled_pcl) {
+    EXPECT_FALSE(downsampled->empty());
+  }
+}
+
+INSTANTIATE_TEST_SUITE_P(DownsamplingTest, DownsamplingTest, testing::Values("SMALL", "TBB", "OMP"), [](const auto& info) { return info.param; });
+
+// Check if downsampling works correctly for empty points
+TEST_P(DownsamplingTest, EmptyTest) {
+  auto empty_points = std::make_shared<PointCloud>();
+  auto empty_downsampled = downsample(*empty_points, 0.1);
+  EXPECT_EQ(empty_downsampled && empty_downsampled->size(), 0) << "Empty test small: " + GetParam();
+
+  auto empty_points_pcl = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+  auto empty_downsampled_pcl = downsample(*empty_points_pcl, 0.1);
+  EXPECT_EQ(empty_downsampled_pcl && empty_downsampled_pcl->size(), 0) << "Empty test pcl: " + GetParam();
+}
+
+// Check if downsampling results are mostly identical to those of pcl::VoxelGrid
+TEST_P(DownsamplingTest, DownsampleTest) {
+  for (size_t i = 0; i < resolutions.size(); i++) {
+    auto result = downsample(*points, resolutions[i]);
+    EXPECT_LT(std::abs(1.0 - static_cast<double>(result->size()) / downsampled_pcl[i]->size()), 0.9) << "Downsampled size check (small): " + GetParam();
+    auto result_pcl = downsample(*points_pcl, resolutions[i]);
+    EXPECT_LT(std::abs(1.0 - static_cast<double>(result_pcl->size()) / downsampled_pcl[i]->size()), 0.9) << "Downsampled size check (pcl): " + GetParam();
+    EXPECT_EQ(result->size(), result_pcl->size()) << "Size check (small vs pcl): " + GetParam();
+  }
 }

src/test/kdtree_test.cpp

@@ -0,0 +1,162 @@
+#include <random>
+#include <ranges>
+
+#include <gtest/gtest.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+
+#include <small_gicp/ann/kdtree.hpp>
+#include <small_gicp/ann/kdtree_omp.hpp>
+#include <small_gicp/ann/kdtree_tbb.hpp>
+#include <small_gicp/util/downsampling.hpp>
+#include <small_gicp/points/point_cloud.hpp>
+#include <small_gicp/points/pcl_point_traits.hpp>
+#include <small_gicp/benchmark/read_points.hpp>
+
+using namespace small_gicp;
+
+class KdTreeTest : public testing::Test, public testing::WithParamInterface<std::string> {
+public:
+  void SetUp() override {
+    // Load points
+    auto points_4f = read_ply("data/target.ply");
+    points = voxelgrid_sampling(*std::make_shared<PointCloud>(points_4f), 0.5);
+
+    points_pcl = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+    points_pcl->resize(points->size());
+    for (size_t i = 0; i < points->size(); i++) {
+      points_pcl->at(i).getVector4fMap() = points->point(i).cast<float>();
+    }
+
+    // Generate queries
+    std::mt19937 mt;
+    std::uniform_real_distribution<> udist(0.0, 1.0);
+    for (size_t i = 0; i < 50; i++) {
+      queries.emplace_back(points->point(mt() % points->size()));
+      queries.emplace_back(points->point(mt() % points->size()) + Eigen::Vector4d(udist(mt), udist(mt), udist(mt), 0.0));
+      queries.emplace_back(Eigen::Vector4d(udist(mt) * 100.0, udist(mt) * 100.0, udist(mt) * 100.0, 1.0));
+    }
+
+    // Find k-nearest neighbors with brute force search
+    struct IndexDist {
+      bool operator<(const IndexDist& rhs) const { return dist < rhs.dist; }
+      size_t index;
+      double dist;
+    };
+
+    k = 20;
+    k_indices.resize(queries.size());
+    k_sq_dists.resize(queries.size());
+    for (size_t i = 0; i < queries.size(); i++) {
+      const auto& query = queries[i];
+
+      std::priority_queue<IndexDist> queue;
+      for (size_t pt_idx = 0; pt_idx < points->size(); pt_idx++) {
+        const double sq_dist = (points->point(pt_idx) - query).squaredNorm();
+        if (queue.size() < k) {
+          queue.push({pt_idx, sq_dist});
+        } else if (sq_dist < queue.top().dist) {
+          queue.pop();
+          queue.push({pt_idx, sq_dist});
+        }
+      }
+
+      std::vector<size_t> indices(queue.size(), 0);
+      std::vector<double> dists(queue.size(), 0.0);
+
+      while (!queue.empty()) {
+        indices[queue.size() - 1] = queue.top().index;
+        dists[queue.size() - 1] = queue.top().dist;
+        queue.pop();
+      }
+
+      k_indices[i] = std::move(indices);
+      k_sq_dists[i] = std::move(dists);
+    }
+  }
+
+  template <typename PointCloud, typename KdTree>
+  void test_knn_(const PointCloud& points, const KdTree& tree) {
+    for (size_t i = 0; i < queries.size(); i++) {
+      // k-nearest neighbors search
+      const auto& query = queries[i];
+      std::vector<size_t> indices(k);
+      std::vector<double> sq_dists(k);
+      const size_t num_results = traits::knn_search(tree, query, k, indices.data(), sq_dists.data());
+      EXPECT_EQ(num_results, k) << "num_neighbors must be k";
+      for (size_t j = 0; j < k; j++) {
+        EXPECT_EQ(indices[j], k_indices[i][j]);
+        EXPECT_NEAR(sq_dists[j], k_sq_dists[i][j], 1e-3);
+      }
+
+      // Nearest neighbor search
+      size_t k_index;
+      double k_sq_dist;
+      const size_t found = traits::nearest_neighbor_search(tree, query, &k_index, &k_sq_dist);
+      EXPECT_EQ(found, 1) << "num_neighbors must be 1";
+      EXPECT_EQ(k_index, k_indices[i][0]);
+      EXPECT_NEAR(k_sq_dist, k_sq_dists[i][0], 1e-3);
+    }
+  }
+
+protected:
+  PointCloud::Ptr points;                          ///< Input points
+  pcl::PointCloud<pcl::PointXYZ>::Ptr points_pcl;  ///< Input points (pcl)
+
+  int k;
+  std::vector<Eigen::Vector4d> queries;
+  std::vector<std::vector<size_t>> k_indices;
+  std::vector<std::vector<double>> k_sq_dists;
+};
+
+// Load check
+TEST_F(KdTreeTest, LoadCheck) {
+  EXPECT_NE(points->size(), 0) << "Load check";
+  EXPECT_NE(points_pcl->size(), 0) << "Load check";
+
+  EXPECT_NE(queries.size(), 0);
+  EXPECT_EQ(queries.size(), k_indices.size());
+  EXPECT_EQ(queries.size(), k_sq_dists.size());
+  for (size_t i = 0; i < queries.size(); i++) {
+    EXPECT_EQ(k_indices[i].size(), k);
+    EXPECT_EQ(k_sq_dists[i].size(), k);
+    EXPECT_TRUE(std::ranges::is_sorted(k_sq_dists[i])) << "Must be sorted by distance";
+  }
+}
+
+INSTANTIATE_TEST_SUITE_P(KdTreeTest, KdTreeTest, testing::Values("SMALL", "TBB", "OMP"), [](const auto& info) { return info.param; });
+
+// Check if kdtree works correctly for empty points
+TEST_P(KdTreeTest, EmptyTest) {
+  auto empty_points = std::make_shared<PointCloud>();
+  auto kdtree = std::make_shared<UnsafeKdTree<PointCloud>>(*empty_points);
+
+  auto empty_points_pcl = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+  auto kdtree_pcl = std::make_shared<UnsafeKdTree<pcl::PointCloud<pcl::PointXYZ>>>(*empty_points_pcl);
+}
+
+// Check if nearest neighbor search results are identical to those of brute force search
+TEST_P(KdTreeTest, KnnTest) {
+  const auto method = GetParam();
+  if (method == "SMALL") {
+    auto kdtree = std::make_shared<KdTree<PointCloud>>(points);
+    test_knn_(*points, *kdtree);
+
+    auto kdtree_pcl = std::make_shared<KdTree<pcl::PointCloud<pcl::PointXYZ>>>(points_pcl);
+    test_knn_(*points_pcl, *kdtree_pcl);
+  } else if (method == "TBB") {
+    auto kdtree = std::make_shared<KdTreeTBB<PointCloud>>(points);
+    test_knn_(*points, *kdtree);
+
+    auto kdtree_pcl = std::make_shared<KdTreeTBB<pcl::PointCloud<pcl::PointXYZ>>>(points_pcl);
+    test_knn_(*points_pcl, *kdtree_pcl);
+  } else if (method == "OMP") {
+    auto kdtree = std::make_shared<KdTreeOMP<PointCloud>>(points, 4);
+    test_knn_(*points, *kdtree);
+
+    auto kdtree_pcl = std::make_shared<KdTreeOMP<pcl::PointCloud<pcl::PointXYZ>>>(points_pcl, 4);
+    test_knn_(*points_pcl, *kdtree_pcl);
+  } else {
+    throw std::runtime_error("Invalid method: " + method);
+  }
+}

src/test/points_test.cpp

@@ -0,0 +1,61 @@
+#include <gtest/gtest.h>
+
+#include <random>
+#include <ranges>
+#include <small_gicp/points/point_cloud.hpp>
+#include <small_gicp/points/pcl_point.hpp>
+#include <small_gicp/points/pcl_point_traits.hpp>
+
+using namespace small_gicp;
+
+template <typename PointCloud>
+void test_points(const std::vector<Eigen::Vector4d>& src_points, PointCloud& points, std::mt19937& mt) {
+  EXPECT_EQ(traits::size(points), src_points.size());
+  EXPECT_TRUE(traits::has_points(points));
+  EXPECT_TRUE(traits::has_normals(points));
+  EXPECT_TRUE(traits::has_covs(points));
+
+  std::uniform_real_distribution<> dist(-100.0, 100.0);
+  std::vector<Eigen::Vector4d> normals(src_points.size());
+  std::vector<Eigen::Matrix4d> covs(src_points.size());
+  for (size_t i = 0; i < traits::size(points); i++) {
+    normals[i] = Eigen::Vector4d(dist(mt), dist(mt), dist(mt), dist(mt));
+    covs[i] = normals[i] * normals[i].transpose();
+  }
+
+  for (size_t i = 0; i < traits::size(points); i++) {
+    traits::set_normal(points, i, normals[i]);
+    traits::set_cov(points, i, covs[i]);
+
+    EXPECT_NEAR((traits::point(points, i) - src_points[i]).norm(), 0.0, 1e-3);
+    EXPECT_NEAR((traits::normal(points, i) - normals[i]).norm(), 0.0, 1e-3);
+    EXPECT_NEAR((traits::cov(points, i) - covs[i]).norm(), 0.0, 1e-3);
+  }
+
+  traits::resize(points, src_points.size() / 2);
+  EXPECT_EQ(traits::size(points), src_points.size() / 2);
+
+  for (size_t i = 0; i < traits::size(points); i++) {
+    EXPECT_NEAR((traits::point(points, i) - src_points[i]).norm(), 0.0, 1e-3);
+    EXPECT_NEAR((traits::normal(points, i) - normals[i]).norm(), 0.0, 1e-3);
+    EXPECT_NEAR((traits::cov(points, i) - covs[i]).norm(), 0.0, 1e-3);
+  }
+}
+
+TEST(PointsTest, PointsTest) {
+  std::mt19937 mt;
+  std::uniform_real_distribution<> dist(-100.0, 100.0);
+
+  std::vector<Eigen::Vector4d> src_points(100);
+  std::ranges::generate(src_points, [&] { return Eigen::Vector4d(dist(mt), dist(mt), dist(mt), 1.0); });
+
+  auto points = std::make_shared<PointCloud>(src_points);
+  test_points(src_points, *points, mt);
+
+  auto points_pcl = pcl::make_shared<pcl::PointCloud<pcl::PointNormalCovariance>>();
+  points_pcl->resize(src_points.size());
+  for (size_t i = 0; i < src_points.size(); i++) {
+    points_pcl->at(i).getVector4fMap() = src_points[i].cast<float>();
+  }
+  test_points(src_points, *points_pcl, mt);
+}

src/test/sort_omp_test.cpp

@@ -7,7 +7,9 @@
 
 using namespace small_gicp;
 
-bool identical(const std::vector<int>& arr1, const std::vector<int>& arr2) {
+// Check if two vectors are identical
+template <typename T>
+bool identical(const std::vector<T>& arr1, const std::vector<T>& arr2) {
   if (arr1.size() != arr2.size()) {
     return false;
   }
@@ -20,13 +22,15 @@ bool identical(const std::vector<int>& arr1, const std::vector<int>& arr2) {
   return true;
 }
 
+// Test merge_sort_omp
 TEST(SortOMP, MergeSortTest) {
   std::mt19937 mt;
 
-  std::uniform_int_distribution<> data_dist(-100, 100);
   std::uniform_int_distribution<> size_dist(0, 1024);
 
+  // int
   for (int i = 0; i < 100; i++) {
+    std::uniform_int_distribution<> data_dist(-100, 100);
     std::vector<int> data(size_dist(mt));
     std::ranges::generate(data, [&] { return data_dist(mt); });
 
@@ -38,15 +42,37 @@ TEST(SortOMP, MergeSortTest) {
 
     EXPECT_TRUE(identical(sorted, sorted_omp)) << fmt::format("i={} N={}", i, data.size());
   }
+
+  // double
+  for (int i = 0; i < 100; i++) {
+    std::uniform_real_distribution<> data_dist(-100, 100);
+    std::vector<double> data(size_dist(mt));
+    std::ranges::generate(data, [&] { return data_dist(mt); });
+
+    std::vector<double> sorted = data;
+    std::ranges::sort(sorted);
+
+    std::vector<double> sorted_omp = data;
+    merge_sort_omp(sorted_omp.begin(), sorted_omp.end(), std::less<double>(), 4);
+
+    EXPECT_TRUE(identical(sorted, sorted_omp)) << fmt::format("i={} N={}", i, data.size());
   }
 
+  // empty
+  std::vector<int> empty_vector;
+  merge_sort_omp(empty_vector.begin(), empty_vector.end(), std::less<int>(), 4);
+  EXPECT_TRUE(empty_vector.empty()) << "Empty vector check";
+}
+
+// Test quick_sort_omp
 TEST(SortOMP, QuickSortTest) {
   std::mt19937 mt;
 
-  std::uniform_int_distribution<> data_dist(-100, 100);
   std::uniform_int_distribution<> size_dist(0, 1024);
 
+  // int
   for (int i = 0; i < 100; i++) {
+    std::uniform_int_distribution<> data_dist(-100, 100);
     std::vector<int> data(size_dist(mt));
     std::ranges::generate(data, [&] { return data_dist(mt); });
 
@@ -58,4 +84,24 @@ TEST(SortOMP, QuickSortTest) {
 
     EXPECT_TRUE(identical(sorted, sorted_omp)) << fmt::format("i={} N={}", i, data.size());
   }
+
+  // double
+  for (int i = 0; i < 100; i++) {
+    std::uniform_real_distribution<> data_dist(-100, 100);
+    std::vector<double> data(size_dist(mt));
+    std::ranges::generate(data, [&] { return data_dist(mt); });
+
+    std::vector<double> sorted = data;
+    std::ranges::sort(sorted);
+
+    std::vector<double> sorted_omp = data;
+    quick_sort_omp(sorted_omp.begin(), sorted_omp.end(), std::less<double>(), 4);
+
+    EXPECT_TRUE(identical(sorted, sorted_omp)) << fmt::format("i={} N={}", i, data.size());
+  }
+
+  // empty
+  std::vector<int> empty_vector;
+  quick_sort_omp(empty_vector.begin(), empty_vector.end(), std::less<int>(), 4);
+  EXPECT_TRUE(empty_vector.empty()) << "Empty vector check";
 }

src/test/vector_test.cpp

@@ -0,0 +1,31 @@
+#include <random>
+#include <small_gicp/util/fast_floor.hpp>
+#include <small_gicp/util/vector3i_hash.hpp>
+
+#include <gtest/gtest.h>
+
+using namespace small_gicp;
+
+TEST(VectorTest, HashTest) {
+  std::mt19937 mt;
+
+  for (int i = 0; i < 1000; i++) {
+    std::uniform_int_distribution<> dist(-1000, 1000);
+    const Eigen::Vector3i v(dist(mt), dist(mt), dist(mt));
+    EXPECT_EQ(XORVector3iHash::hash(v), XORVector3iHash()(v));
+    EXPECT_TRUE(XORVector3iHash::equal(v, v));
+  }
+}
+
+TEST(VectorTest, FloorTest) {
+  std::mt19937 mt;
+  for (int i = 0; i < 1000; i++) {
+    std::uniform_real_distribution<> dist(-1000.0, 1000.0);
+    const Eigen::Vector4d v(dist(mt), dist(mt), dist(mt), 1.0);
+    const Eigen::Array4i floor1 = fast_floor(v);
+    EXPECT_EQ(floor1[0], std::floor(v[0]));
+    EXPECT_EQ(floor1[1], std::floor(v[1]));
+    EXPECT_EQ(floor1[2], std::floor(v[2]));
+    EXPECT_EQ(floor1[3], std::floor(v[3]));
+  }
+}