From ddf5f533b2892c8772c867a49021720a289a28d2 Mon Sep 17 00:00:00 2001 From: "k.koide" Date: Thu, 28 Mar 2024 18:04:21 +0900 Subject: [PATCH] tests --- CMakeLists.txt | 3 +- include/small_gicp/ann/kdtree.hpp | 3 - include/small_gicp/benchmark/read_points.hpp | 78 ++++++++- include/small_gicp/points/point_cloud.hpp | 3 + include/small_gicp/util/downsampling.hpp | 1 - include/small_gicp/util/downsampling_omp.hpp | 1 - include/small_gicp/util/downsampling_tbb.hpp | 3 +- src/test/downsampling_test.cpp | 89 ++++++++-- src/test/kdtree_test.cpp | 162 +++++++++++++++++++ src/test/points_test.cpp | 61 +++++++ src/test/sort_omp_test.cpp | 52 +++++- src/test/vector_test.cpp | 31 ++++ 12 files changed, 465 insertions(+), 22 deletions(-) create mode 100644 src/test/kdtree_test.cpp create mode 100644 src/test/points_test.cpp create mode 100644 src/test/vector_test.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index b5aa98c..50d9929 100644 --- a/CMakeLists.txt +++ b/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() diff --git a/include/small_gicp/ann/kdtree.hpp b/include/small_gicp/ann/kdtree.hpp index 4ed25a8..3dc5391 100644 --- a/include/small_gicp/ann/kdtree.hpp +++ b/include/small_gicp/ann/kdtree.hpp @@ -8,9 +8,6 @@ namespace small_gicp { -template -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. diff --git a/include/small_gicp/benchmark/read_points.hpp b/include/small_gicp/benchmark/read_points.hpp index 1e50dc2..e323430 100644 --- a/include/small_gicp/benchmark/read_points.hpp +++ b/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 read_points(const std::string& filename) { @@ -29,4 +29,80 @@ static std::vector 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& points) { + std::ofstream ofs(filename, std::ios::binary); + if (!ofs) { + std::cerr << "error: failed to open " << filename << std::endl; + return; + } + + ofs.write(reinterpret_cast(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 read_ply(const std::string& filename) { + std::ifstream ifs(filename); + if (!ifs) { + std::cerr << "error: failed to open " << filename << std::endl; + return {}; + } + + std::vector properties; + std::vector 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 buffer(properties.size() * points.size()); + ifs.read(reinterpret_cast(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 diff --git a/include/small_gicp/points/point_cloud.hpp b/include/small_gicp/points/point_cloud.hpp index b800129..e72f049 100644 --- a/include/small_gicp/points/point_cloud.hpp +++ b/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) { diff --git a/include/small_gicp/util/downsampling.hpp b/include/small_gicp/util/downsampling.hpp index 92a485c..2bf88b7 100644 --- a/include/small_gicp/util/downsampling.hpp +++ b/include/small_gicp/util/downsampling.hpp @@ -18,7 +18,6 @@ namespace small_gicp { template std::shared_ptr 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(); } diff --git a/include/small_gicp/util/downsampling_omp.hpp b/include/small_gicp/util/downsampling_omp.hpp index 4274f7b..d802b7f 100644 --- a/include/small_gicp/util/downsampling_omp.hpp +++ b/include/small_gicp/util/downsampling_omp.hpp @@ -19,7 +19,6 @@ namespace small_gicp { template std::shared_ptr 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(); } diff --git a/include/small_gicp/util/downsampling_tbb.hpp b/include/small_gicp/util/downsampling_tbb.hpp index 8f42dcd..8c1e6b0 100644 --- a/include/small_gicp/util/downsampling_tbb.hpp +++ b/include/small_gicp/util/downsampling_tbb.hpp @@ -19,7 +19,6 @@ namespace small_gicp { template std::shared_ptr 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(); } @@ -50,7 +49,7 @@ std::shared_ptr 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(0, traits::size(points), block_size), [&](const tbb::blocked_range& range) { std::vector sub_points; diff --git a/src/test/downsampling_test.cpp b/src/test/downsampling_test.cpp index 4193013..9457115 100644 --- a/src/test/downsampling_test.cpp +++ b/src/test/downsampling_test.cpp @@ -1,23 +1,94 @@ #include #include #include +#include #include +#include #include #include #include #include +#include using namespace small_gicp; -TEST(DownsamplingTest, NullTest) { - auto points = std::make_shared(); - 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); +class DownsamplingTest : public testing::Test, public testing::WithParamInterface { +public: + void SetUp() override { + // Load points + auto points_4f = read_ply("data/target.ply"); + points = std::make_shared(points_4f); + points_pcl = pcl::make_shared>(); + points_pcl->resize(points_4f.size()); + for (size_t i = 0; i < points_4f.size(); i++) { + points_pcl->at(i).getVector4fMap() = points_4f[i]; + } - auto points_pcl = std::make_shared>(); - 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); + // Downsample points using pcl::VoxelGrid for reference + resolutions = {0.1, 0.5, 1.0}; + for (double resolution : resolutions) { + pcl::VoxelGrid voxelgrid; + voxelgrid.setLeafSize(resolution, resolution, resolution); + voxelgrid.setInputCloud(points_pcl); + + auto downsampled = pcl::make_shared>(); + voxelgrid.filter(*downsampled); + downsampled_pcl.push_back(downsampled); + } + } + + // Apply downsampling + template + std::shared_ptr 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 resolutions; ///< Downsampling resolutions + PointCloud::Ptr points; ///< Input points + pcl::PointCloud::Ptr points_pcl; ///< Input points (pcl) + std::vector::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(); + 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>(); + 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(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(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(); + } } diff --git a/src/test/kdtree_test.cpp b/src/test/kdtree_test.cpp new file mode 100644 index 0000000..8aa81cc --- /dev/null +++ b/src/test/kdtree_test.cpp @@ -0,0 +1,162 @@ +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +using namespace small_gicp; + +class KdTreeTest : public testing::Test, public testing::WithParamInterface { +public: + void SetUp() override { + // Load points + auto points_4f = read_ply("data/target.ply"); + points = voxelgrid_sampling(*std::make_shared(points_4f), 0.5); + + points_pcl = pcl::make_shared>(); + points_pcl->resize(points->size()); + for (size_t i = 0; i < points->size(); i++) { + points_pcl->at(i).getVector4fMap() = points->point(i).cast(); + } + + // 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 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 indices(queue.size(), 0); + std::vector 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 + 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 indices(k); + std::vector 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::Ptr points_pcl; ///< Input points (pcl) + + int k; + std::vector queries; + std::vector> k_indices; + std::vector> 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(); + auto kdtree = std::make_shared>(*empty_points); + + auto empty_points_pcl = pcl::make_shared>(); + auto kdtree_pcl = std::make_shared>>(*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>(points); + test_knn_(*points, *kdtree); + + auto kdtree_pcl = std::make_shared>>(points_pcl); + test_knn_(*points_pcl, *kdtree_pcl); + } else if (method == "TBB") { + auto kdtree = std::make_shared>(points); + test_knn_(*points, *kdtree); + + auto kdtree_pcl = std::make_shared>>(points_pcl); + test_knn_(*points_pcl, *kdtree_pcl); + } else if (method == "OMP") { + auto kdtree = std::make_shared>(points, 4); + test_knn_(*points, *kdtree); + + auto kdtree_pcl = std::make_shared>>(points_pcl, 4); + test_knn_(*points_pcl, *kdtree_pcl); + } else { + throw std::runtime_error("Invalid method: " + method); + } +} diff --git a/src/test/points_test.cpp b/src/test/points_test.cpp new file mode 100644 index 0000000..f001df7 --- /dev/null +++ b/src/test/points_test.cpp @@ -0,0 +1,61 @@ +#include + +#include +#include +#include +#include +#include + +using namespace small_gicp; + +template +void test_points(const std::vector& 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 normals(src_points.size()); + std::vector 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 src_points(100); + std::ranges::generate(src_points, [&] { return Eigen::Vector4d(dist(mt), dist(mt), dist(mt), 1.0); }); + + auto points = std::make_shared(src_points); + test_points(src_points, *points, mt); + + auto points_pcl = pcl::make_shared>(); + 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(); + } + test_points(src_points, *points_pcl, mt); +} \ No newline at end of file diff --git a/src/test/sort_omp_test.cpp b/src/test/sort_omp_test.cpp index 4712eb1..7e8ab31 100644 --- a/src/test/sort_omp_test.cpp +++ b/src/test/sort_omp_test.cpp @@ -7,7 +7,9 @@ using namespace small_gicp; -bool identical(const std::vector& arr1, const std::vector& arr2) { +// Check if two vectors are identical +template +bool identical(const std::vector& arr1, const std::vector& arr2) { if (arr1.size() != arr2.size()) { return false; } @@ -20,13 +22,15 @@ bool identical(const std::vector& arr1, const std::vector& 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 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 data(size_dist(mt)); + std::ranges::generate(data, [&] { return data_dist(mt); }); + + std::vector sorted = data; + std::ranges::sort(sorted); + + std::vector sorted_omp = data; + merge_sort_omp(sorted_omp.begin(), sorted_omp.end(), std::less(), 4); + + EXPECT_TRUE(identical(sorted, sorted_omp)) << fmt::format("i={} N={}", i, data.size()); + } + + // empty + std::vector empty_vector; + merge_sort_omp(empty_vector.begin(), empty_vector.end(), std::less(), 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 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 data(size_dist(mt)); + std::ranges::generate(data, [&] { return data_dist(mt); }); + + std::vector sorted = data; + std::ranges::sort(sorted); + + std::vector sorted_omp = data; + quick_sort_omp(sorted_omp.begin(), sorted_omp.end(), std::less(), 4); + + EXPECT_TRUE(identical(sorted, sorted_omp)) << fmt::format("i={} N={}", i, data.size()); + } + + // empty + std::vector empty_vector; + quick_sort_omp(empty_vector.begin(), empty_vector.end(), std::less(), 4); + EXPECT_TRUE(empty_vector.empty()) << "Empty vector check"; } diff --git a/src/test/vector_test.cpp b/src/test/vector_test.cpp new file mode 100644 index 0000000..504f0c5 --- /dev/null +++ b/src/test/vector_test.cpp @@ -0,0 +1,31 @@ +#include +#include +#include + +#include + +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])); + } +} \ No newline at end of file