test on noble (#43)

* test on noble

* use latest python packages

* add tests

* remove unnecessary dockerfiles

.github/workflows/build-linux.yml

@@ -34,7 +34,7 @@ jobs:
       - name: Docker build
         uses: docker/build-push-action@v2
         with:
-          file: ${{github.workspace}}/docker/Dockerfile.build.${{ matrix.TOOLCHAIN }}
+          file: ${{github.workspace}}/docker/Dockerfile.pytest.${{ matrix.TOOLCHAIN }}
           build-args: BASE_IMAGE=ubuntu:${{ matrix.DISTRO }}
           context: .
           push: false

docker/Dockerfile.build.gcc

@@ -1,23 +0,0 @@
-ARG BASE_IMAGE
-
-FROM ${BASE_IMAGE}
-
-ENV DEBIAN_FRONTEND=noninteractive
-
-RUN apt-get update && apt-get install --no-install-recommends -y \
-  && apt-get install --no-install-recommends -y \
-  wget nano build-essential git cmake \
-  libeigen3-dev libfmt-dev libtbb-dev libomp-dev libgtest-dev  \
-  && apt-get clean \
-  && rm -rf /var/lib/apt/lists/*
-
-COPY . /root/small_gicp
-WORKDIR /root/small_gicp/build
-RUN rm -rf ./*
-
-RUN cmake .. -DBUILD_WITH_TBB=ON
-RUN cmake --build . -j$(nproc)
-
-WORKDIR /
-
-CMD ["bash"]

docker/Dockerfile.pytest.gcc

@@ -0,0 +1,28 @@
+ARG BASE_IMAGE
+
+FROM ${BASE_IMAGE}
+
+ENV DEBIAN_FRONTEND=noninteractive
+
+RUN apt-get update && apt-get install --no-install-recommends -y \
+  && apt-get install --no-install-recommends -y \
+  wget nano build-essential git cmake python3-dev python3-pip pybind11-dev \
+  libeigen3-dev libomp-dev
+
+RUN mkdir -p ~/.config/pip
+RUN echo "[global]\nbreak-system-packages=true" > ~/.config/pip/pip.conf
+
+RUN pip install pytest numpy scipy
+
+COPY . /root/small_gicp
+
+WORKDIR /root/small_gicp
+RUN rm -rf build
+
+RUN pip install .
+RUN python3 -m pytest src/example/basic_registration.py
+RUN python3 -m pytest src/test/python_test.py
+
+WORKDIR /
+
+CMD ["bash"]

docker/Dockerfile.pytest.llvm

@@ -6,27 +6,30 @@ ENV DEBIAN_FRONTEND=noninteractive
 
 RUN apt-get update && apt-get install --no-install-recommends -y \
   && apt-get install --no-install-recommends -y \
-  wget nano build-essential git cmake \
-  libeigen3-dev libfmt-dev libtbb-dev libomp-dev libgtest-dev \
-  && apt-get clean \
-  && rm -rf /var/lib/apt/lists/*
+  wget nano build-essential git cmake python3-dev python3-pip pybind11-dev \
+  libeigen3-dev libomp-dev
 
 RUN apt-get update && apt-get install --no-install-recommends -y \
   && apt-get install --no-install-recommends -y \
-  clang lld \
-  && apt-get clean \
-  && rm -rf /var/lib/apt/lists/*
+  clang lld
 
 RUN update-alternatives --install /usr/bin/ld ld /usr/bin/ld.lld 50
 ENV CC=clang
 ENV CXX=clang++
 
-COPY . /root/small_gicp
-WORKDIR /root/small_gicp/build
-RUN rm -rf ./*
+RUN mkdir -p ~/.config/pip
+RUN echo "[global]\nbreak-system-packages=true" > ~/.config/pip/pip.conf
 
-RUN cmake .. -DBUILD_WITH_TBB=ON
-RUN cmake --build . -j$(nproc)
+RUN pip install pytest numpy scipy
+
+COPY . /root/small_gicp
+
+WORKDIR /root/small_gicp
+RUN rm -rf build
+
+RUN pip install .
+RUN python3 -m pytest src/example/basic_registration.py
+RUN python3 -m pytest src/test/python_test.py
 
 WORKDIR /
 

docker/Dockerfile.test.gcc

@@ -1,31 +0,0 @@
-ARG BASE_IMAGE
-
-FROM ${BASE_IMAGE}
-
-ENV DEBIAN_FRONTEND=noninteractive
-
-RUN apt-get update && apt-get install --no-install-recommends -y \
-  && apt-get install --no-install-recommends -y \
-  wget nano build-essential git cmake python3-pip pybind11-dev \
-  libeigen3-dev libfmt-dev libtbb-dev libomp-dev libpcl-dev libgtest-dev  \
-  && apt-get clean \
-  && rm -rf /var/lib/apt/lists/*
-
-RUN pip install -U setuptools pytest numpy scipy
-
-COPY . /root/small_gicp
-WORKDIR /root/small_gicp/build
-RUN rm -rf ./*
-
-RUN cmake .. -DBUILD_TESTS=ON -DBUILD_EXAMPLES=ON -DBUILD_BENCHMARKS=ON -DBUILD_WITH_TBB=ON -DBUILD_WITH_PCL=ON
-RUN cmake --build . -j$(nproc)
-RUN ctest -j$(nproc)
-
-WORKDIR /root/small_gicp
-RUN pip install .
-RUN pytest src/example/basic_registration.py
-RUN pytest src/test/python_test.py
-
-WORKDIR /
-
-CMD ["bash"]

docker/Dockerfile.test.llvm

@@ -1,41 +0,0 @@
-ARG BASE_IMAGE
-
-FROM ${BASE_IMAGE}
-
-ENV DEBIAN_FRONTEND=noninteractive
-
-RUN apt-get update && apt-get install --no-install-recommends -y \
-  && apt-get install --no-install-recommends -y \
-  wget nano build-essential git cmake python3-pip pybind11-dev \
-  libeigen3-dev libfmt-dev libtbb-dev libomp-dev libpcl-dev libgtest-dev \
-  && apt-get clean \
-  && rm -rf /var/lib/apt/lists/*
-
-RUN apt-get update && apt-get install --no-install-recommends -y \
-  && apt-get install --no-install-recommends -y \
-  clang lld \
-  && apt-get clean \
-  && rm -rf /var/lib/apt/lists/*
-
-RUN pip install -U setuptools pytest numpy scipy
-
-RUN update-alternatives --install /usr/bin/ld ld /usr/bin/ld.lld 50
-ENV CC=clang
-ENV CXX=clang++
-
-COPY . /root/small_gicp
-WORKDIR /root/small_gicp/build
-RUN rm -rf ./*
-
-RUN cmake .. -DBUILD_TESTS=ON -DBUILD_EXAMPLES=ON -DBUILD_BENCHMARKS=ON -DBUILD_WITH_TBB=ON -DBUILD_WITH_PCL=ON
-RUN cmake --build . -j$(nproc)
-RUN ctest -j$(nproc)
-
-WORKDIR /root/small_gicp
-RUN pip install .
-RUN pytest src/example/basic_registration.py
-RUN pytest src/test/python_test.py
-
-WORKDIR /
-
-CMD ["bash"]

src/test/downsampling_test.cpp

@@ -1,3 +1,5 @@
+#include <unordered_set>
+
 #include <gtest/gtest.h>
 #include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
@@ -75,11 +77,13 @@ INSTANTIATE_TEST_SUITE_P(DownsamplingTest, DownsamplingTest, testing::Values("SM
 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();
+  EXPECT_TRUE(empty_downsampled);
+  EXPECT_EQ(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();
+  EXPECT_TRUE(empty_downsampled_pcl);
+  EXPECT_EQ(empty_downsampled_pcl->size(), 0) << "Empty test pcl: " + GetParam();
 }
 
 // Check if downsampling results are mostly identical to those of pcl::VoxelGrid
@@ -92,3 +96,42 @@ TEST_P(DownsamplingTest, DownsampleTest) {
     EXPECT_EQ(result->size(), result_pcl->size()) << "Size check (small vs pcl): " + GetParam();
   }
 }
+
+// Check if random sampling works correctly for empty points
+TEST_P(DownsamplingTest, EmptyRandamSamplingTest) {
+  std::mt19937 mt;
+
+  auto empty_points = std::make_shared<PointCloud>();
+  auto empty_downsampled = random_sampling(*empty_points, 1000, mt);
+  EXPECT_TRUE(empty_downsampled);
+  EXPECT_EQ(empty_downsampled->size(), 0) << "Empty test small: " + GetParam();
+
+  auto empty_points_pcl = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+  auto empty_downsampled_pcl = random_sampling(*empty_points_pcl, 1000, mt);
+  EXPECT_TRUE(empty_downsampled_pcl);
+  EXPECT_EQ(empty_downsampled_pcl->size(), 0) << "Empty test pcl: " + GetParam();
+}
+
+// Test random sampling
+TEST_P(DownsamplingTest, RandamSamplingTest) {
+  std::mt19937 mt;
+
+  auto downsampled = voxelgrid_sampling(*points, 0.1);
+
+  const std::vector<size_t> num_points = {0, 100, 1000};
+  for (size_t N : num_points) {
+    auto result = random_sampling(*downsampled, N, mt);
+    EXPECT_TRUE(result);
+    EXPECT_EQ(result->size(), N) << "Size check (small): " + GetParam();
+
+    std::unordered_set<size_t> indices;
+    for (size_t i = 0; i < N; i++) {
+      const auto found = std::find_if(downsampled->points.begin(), downsampled->points.end(), [&](const auto& p) { return (p - result->points[i]).norm() < 1.0e-6; });
+      EXPECT_NE(found, downsampled->points.end()) << "Existence check (small): " + GetParam();
+
+      const size_t index = std::distance(downsampled->points.begin(), found);
+      EXPECT_EQ(indices.count(index), 0) << "Uniqueness check (small): " + GetParam();
+      indices.insert(index);
+    }
+  }
+}

src/test/helper_test.cpp

@@ -0,0 +1,157 @@
+#include <gtest/gtest.h>
+#include <small_gicp/registration/registration_helper.hpp>
+
+#include <small_gicp/benchmark/read_points.hpp>
+#include <small_gicp/registration/registration_helper.hpp>
+
+using namespace small_gicp;
+
+class HelperTest : public testing::Test, public testing::WithParamInterface<const char*> {
+public:
+  void SetUp() override {
+    // Load points
+    target_raw = std::make_shared<PointCloud>(read_ply("data/target.ply"));
+    source_raw = std::make_shared<PointCloud>(read_ply("data/source.ply"));
+
+    std::ifstream ifs("data/T_target_source.txt");
+    if (!ifs) {
+      std::cerr << "error: failed to open T_target_source.txt" << std::endl;
+    }
+    for (int i = 0; i < 4; i++) {
+      for (int j = 0; j < 4; j++) {
+        ifs >> T_target_source(i, j);
+      }
+    }
+  }
+
+  bool compare_transformation(const Eigen::Isometry3d& T1, const Eigen::Isometry3d& T2) {
+    const Eigen::Isometry3d e = T1.inverse() * T2;
+    const double error_rot = Eigen::AngleAxisd(e.linear()).angle();
+    const double error_trans = e.translation().norm();
+
+    const double rot_tol = 2.5 * M_PI / 180.0;
+    const double trans_tol = 0.2;
+
+    EXPECT_NEAR(error_rot, 0.0, rot_tol);
+    EXPECT_NEAR(error_trans, 0.0, trans_tol);
+
+    return error_rot < rot_tol && error_trans < trans_tol;
+  }
+
+protected:
+  PointCloud::Ptr target_raw;         ///< Target points
+  PointCloud::Ptr source_raw;         ///< Source points
+  Eigen::Isometry3d T_target_source;  ///< Ground truth transformation
+};
+
+// Load check
+TEST_F(HelperTest, LoadCheck) {
+  EXPECT_FALSE(target_raw->empty());
+  EXPECT_FALSE(source_raw->empty());
+}
+
+TEST_F(HelperTest, EmptyPreprocess) {
+  auto empty_points = std::make_shared<PointCloud>();
+  auto [target, target_tree] = preprocess_points(*empty_points, 0.1, 10, 1);
+  EXPECT_TRUE(target);
+  EXPECT_TRUE(target_tree);
+  EXPECT_EQ(target->size(), 0);
+}
+
+TEST_F(HelperTest, Preprocess) {
+  const std::vector<int> num_threads = {1, 2};
+
+  for (int N : num_threads) {
+    auto [target, target_tree] = preprocess_points(*target_raw, 0.1, 10, N);
+    EXPECT_TRUE(target) << "N=" << N;
+    EXPECT_TRUE(target_tree) << "N=" << N;
+    EXPECT_GT(target->size(), 1000) << "N=" << N;
+    EXPECT_LT(target->size(), target_raw->size()) << "N=" << N;
+
+    auto [target2, target_tree2] = preprocess_points(target_raw->points, 0.1, 10, N);
+    EXPECT_TRUE(target2) << "N=" << N;
+    EXPECT_TRUE(target_tree2) << "N=" << N;
+    EXPECT_NEAR(target->size(), target2->size(), 100) << "N=" << N;
+  }
+}
+
+TEST_F(HelperTest, EmptyGaussianVoxelMap) {
+  auto empty_points = std::make_shared<PointCloud>();
+  auto voxelmap = create_gaussian_voxelmap(*empty_points, 0.1);
+  EXPECT_TRUE(voxelmap);
+  EXPECT_EQ(voxelmap->size(), 0);
+}
+
+TEST_F(HelperTest, GaussianVoxelMap) {
+  auto voxelmap = create_gaussian_voxelmap(*target_raw, 0.1);
+  EXPECT_TRUE(voxelmap);
+  EXPECT_GT(voxelmap->size(), 1000);
+  EXPECT_LT(voxelmap->size(), target_raw->size());
+}
+
+INSTANTIATE_TEST_SUITE_P(HelperTest, HelperTest, testing::Values("ICP", "PLANE_ICP", "GICP", "VGICP"), [](const auto& info) { return info.param; });
+
+TEST_P(HelperTest, AlignEigen) {
+  const std::string method = GetParam();
+
+  RegistrationSetting setting;
+  if (method == "ICP") {
+    setting.type = RegistrationSetting::ICP;
+  } else if (method == "PLANE_ICP") {
+    setting.type = RegistrationSetting::PLANE_ICP;
+  } else if (method == "GICP") {
+    setting.type = RegistrationSetting::GICP;
+  } else if (method == "VGICP") {
+    setting.type = RegistrationSetting::VGICP;
+  } else {
+    std::cerr << "error: unknown method " << method << std::endl;
+  }
+
+  // Forward test
+  auto result = align(target_raw->points, source_raw->points, Eigen::Isometry3d::Identity(), setting);
+  EXPECT_TRUE(compare_transformation(T_target_source, result.T_target_source));
+
+  // Backward test
+  auto result2 = align(source_raw->points, target_raw->points, Eigen::Isometry3d::Identity(), setting);
+  EXPECT_TRUE(compare_transformation(T_target_source, result2.T_target_source.inverse()));
+}
+
+TEST_P(HelperTest, AlignSmall) {
+  const std::string method = GetParam();
+
+  RegistrationSetting setting;
+  if (method == "ICP") {
+    setting.type = RegistrationSetting::ICP;
+  } else if (method == "PLANE_ICP") {
+    setting.type = RegistrationSetting::PLANE_ICP;
+  } else if (method == "GICP") {
+    setting.type = RegistrationSetting::GICP;
+  } else if (method == "VGICP") {
+    setting.type = RegistrationSetting::VGICP;
+  } else {
+    std::cerr << "error: unknown method " << method << std::endl;
+  }
+
+  RegistrationResult result;
+
+  auto [target, target_tree] = preprocess_points(*target_raw, 0.1, 10, 1);
+  auto [source, source_tree] = preprocess_points(*source_raw, 0.1, 10, 1);
+
+  // Forward test
+  if (method != "VGICP") {
+    result = align(*target, *source, *target_tree, Eigen::Isometry3d::Identity(), setting);
+  } else {
+    auto target_voxelmap = create_gaussian_voxelmap(*target, 1.0);
+    result = align(*target_voxelmap, *source, Eigen::Isometry3d::Identity(), setting);
+  }
+  EXPECT_TRUE(compare_transformation(T_target_source, result.T_target_source));
+
+  // Backward test
+  if (method != "VGICP") {
+    result = align(*source, *target, *source_tree, Eigen::Isometry3d::Identity(), setting);
+  } else {
+    auto source_voxelmap = create_gaussian_voxelmap(*source, 1.0);
+    result = align(*source_voxelmap, *target, Eigen::Isometry3d::Identity(), setting);
+  }
+  EXPECT_TRUE(compare_transformation(T_target_source, result.T_target_source.inverse()));
+}

src/test/normal_estimation_test.cpp

@@ -237,7 +237,7 @@ TEST_F(NormalEstimationTest, NormalEstimationTestPCL) {
   EXPECT_TRUE(check_normals(*point_normals));
 
   // Covariance estimation
-  auto point_covs = pcl::make_shared<pcl::PointCloud<pcl::PointNormalCovariance>>();
+  auto point_covs = pcl::make_shared<pcl::PointCloud<pcl::PointCovariance>>();
   copy_points(*point_covs);
 
   estimate_covariances(*point_covs, num_neighbors);