Incremental registration of stationary point clouds into gaussian voxelmap creates invalid transforms and breaks after ~40 clouds

[themightyoarfish]

Describe the bug

Registering the linked pointclouds one by one into a voxel map completely breaks after a while. The LIDAR sensor is stationary throughtout this recording, so the only change is noise. After about 40 clouds, the estimated transforms start to break and the rotation matrix is not even orthogonal anymore. Before that, large translations are suddenly estimated, even though nothing has moved. At the end, the transformation becomes NaN.

This seems to relate to the initial transform for align(), but it should still not be possible to create rotation matrices that are not orthogonal.

Here is a cmake project to reproduce (requires PCL visualization):

CMakeLists.txt:

cmake_minimum_required(VERSION 3.15.0)
set(CMAKE_CXX_STANDARD 17)

project(icp_experiments)
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)

# Find required packages
find_package(PCL REQUIRED)

# Fetch small_gicp
include(FetchContent)
set(BUILD_WITH_MARCH_NATIVE ON)
# Add small_gicp dependency
FetchContent_Declare(
  small_gicp
  GIT_REPOSITORY https://github.com/koide3/small_gicp
  GIT_TAG 1d8cce8add74d28f16a9efab677dbe93490b28e4
)

FetchContent_MakeAvailable(small_gicp)

# Mark small_gicp includes as system includes
get_target_property(
  SMALL_GICP_INCLUDE_DIRS small_gicp INTERFACE_INCLUDE_DIRECTORIES
)
target_include_directories(small_gicp SYSTEM PRIVATE ${SMALL_GICP_INCLUDE_DIRS})

# Create executables
add_executable(incremental_gicp_registration main.cpp)

# Link libraries
target_link_libraries(incremental_gicp_registration
    ${PCL_LIBRARIES}
    small_gicp
)

# Include directories
target_include_directories(incremental_gicp_registration PRIVATE
    ${PCL_INCLUDE_DIRS}
)

main.cpp

#include <algorithm>
#include <filesystem>
#include <iostream>
#include <limits>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <small_gicp/ann/gaussian_voxelmap.hpp>
#include <small_gicp/ann/sequential_voxelmap_accessor.hpp>
#include <small_gicp/benchmark/benchmark.hpp>
#include <small_gicp/factors/gicp_factor.hpp>
#include <small_gicp/pcl/pcl_point.hpp>
#include <small_gicp/pcl/pcl_registration.hpp>
#include <small_gicp/points/point_cloud.hpp>
#include <small_gicp/registration/reduction_omp.hpp>
#include <small_gicp/registration/registration.hpp>
#include <small_gicp/util/downsampling_omp.hpp>
#include <small_gicp/util/normal_estimation_omp.hpp>

#include "utils.hpp"

namespace fs = std::filesystem;

using namespace small_gicp;
using CloudCov = pcl::PointCloud<pcl::PointCovariance>;

struct OdometryEstimationParams {
public:
  int num_threads                    = 4;
  int num_neighbors                  = 20;
  double downsampling_resolution     = 0.1;
  double voxel_resolution            = 0.025;
  double max_correspondence_distance = 0.5;
};

class OdometryEstimation {
public:
  using Ptr = std::shared_ptr<OdometryEstimation>;

  OdometryEstimation(const OdometryEstimationParams& params) : params(params) {
  }
  virtual ~OdometryEstimation() = default;

  virtual Eigen::Isometry3d estimate(const CloudCov::Ptr& points) = 0;

protected:
  const OdometryEstimationParams params;
};


class SmallVGICPModelOdometryEstimationOMP : public OdometryEstimation {
public:
  explicit SmallVGICPModelOdometryEstimationOMP(
      const OdometryEstimationParams& params) :
      OdometryEstimation(params),
      T_world_lidar(Eigen::Isometry3d::Identity()),
      T_last_current(Eigen::Isometry3d::Identity()) {
  }

  Eigen::Isometry3d estimate(const CloudCov::Ptr& points) override {
    small_gicp::Stopwatch sw;
    sw.start();

    // Note that input points here is already downsampled
    // Estimate per-point covariances
    small_gicp::estimate_covariances_omp(
        *points, params.num_neighbors, params.num_threads);

    std::cout << "estimated covars" << std::endl;

    if (voxelmap == nullptr) {
      std::cout << "voxelmap nullptr" << std::endl;
      // This is the very first frame
      voxelmap = std::make_shared<small_gicp::GaussianVoxelMap>(
          params.voxel_resolution);
      voxelmap->insert(*points);
      return T_world_lidar;
    }

    // Registration using GICP + OMP-based parallel reduction
    Registration<GICPFactor, ParallelReductionOMP> registration;
    registration.reduction.num_threads = params.num_threads;
    registration.rejector.max_dist_sq =
        params.max_correspondence_distance * params.max_correspondence_distance;

    std::cout << "target size=" << voxelmap->size() << std::endl;
    std::cout << "source size=" << points->size() << std::endl;
    auto result = registration.align(
        *voxelmap, *points, *voxelmap, T_world_lidar * T_last_current);
    std::cout << "Converged? " << result.converged << std::endl;
    std::cout << "Num iterations " << result.iterations << std::endl;
    std::cout << "Error " << result.error << std::endl;

    T_last_current = T_world_lidar.inverse() * result.T_target_source;

    // Update T_world_lidar with the estimated transformation
    T_world_lidar = result.T_target_source;

    // Insert points to the target voxel map
    voxelmap->insert(*points, T_world_lidar);

    sw.stop();
    reg_times.push(sw.msec());
    std::cout << "time=" << reg_times.last() << std::endl;

    return T_world_lidar;
  }

  CloudI::Ptr get_points() {
    CloudI::Ptr cloud(new CloudI);
    if (!voxelmap) {
      return cloud;
    }
    auto accessor = small_gicp::create_sequential_accessor(*voxelmap);

    for (int i = 0; i < traits::size(accessor); ++i) {
      Eigen::Vector4d point = traits::point(accessor, i);
      cloud->push_back(pcl::PointXYZI(point.x(), point.y(), point.z()));
    }
    return cloud;
  }

private:
  small_gicp::Summarizer reg_times;

  small_gicp::GaussianVoxelMap::Ptr voxelmap =
      nullptr;                      // Target voxel map that is an accumulation
                                    // of past point clouds
  Eigen::Isometry3d T_world_lidar;  // Current world-to-lidar transformation
  Eigen::Isometry3d T_last_current;
};


int main(int argc, char* argv[]) {
  if (argc != 2) {
    std::cerr << "Usage: " << argv[0] << " <pcd_directory>" << std::endl;
    return 1;
  }

  std::string directory_path = argv[1];

  if (!fs::exists(directory_path) || !fs::is_directory(directory_path)) {
    std::cerr << "Error: " << directory_path << " is not a valid directory"
              << std::endl;
    return 1;
  }

  std::vector<std::string> pcd_files;

  for (const auto& entry : fs::directory_iterator(directory_path)) {
    if (entry.is_regular_file() && entry.path().extension() == ".pcd") {
      pcd_files.push_back(entry.path().string());
    }
  }

  std::sort(pcd_files.begin(), pcd_files.end());

  if (pcd_files.empty()) {
    std::cerr << "No PCD files found in " << directory_path << std::endl;
    return 1;
  }

  std::vector<CloudI::Ptr> pointclouds(pcd_files.size(), nullptr);
  std::vector<CloudCov::Ptr> pointclouds_with_cov(pcd_files.size(), nullptr);

  OdometryEstimationParams params;
  params.voxel_resolution        = 0.1;
  params.downsampling_resolution = 0.1;

#pragma omp parallel for
  for (int i = 0; i < pcd_files.size(); ++i) {
    const auto& pcd_file = pcd_files[i];

    CloudI::Ptr cloud(new CloudI);

    if (pcl::io::loadPCDFile<CloudI::PointType>(pcd_file, *cloud) == -1) {
      std::cerr << "Failed to load PCD file: " << pcd_file << std::endl;
      continue;
    }
    const auto max = std::numeric_limits<double>::max();
    pointclouds[i] =
        remove_points_outside_range(cloud, -max, max, -max, -1, -max, max, 25);

    // std::cout << "Loaded " << pcd_file << " with " << cloud->size() << "
    // points"
    //           << std::endl;
    CloudCov::Ptr cloud_cov =
        small_gicp::voxelgrid_sampling_omp<CloudI, CloudCov>(
            *pointclouds[i], params.downsampling_resolution);
    pointclouds_with_cov[i] = cloud_cov;
  }

  std::cout << "Successfully loaded " << pointclouds_with_cov.size()
            << " pointclouds" << std::endl;

  auto odometry_estimation =
      std::make_shared<SmallVGICPModelOdometryEstimationOMP>(params);

  auto visualizer =
      std::make_shared<pcl::visualization::PCLVisualizer>("Visualizer");
  visualizer->setBackgroundColor(0.5, 0.5, 0.5);
  visualizer->addCoordinateSystem(1.0);

  for (int i = 0; i < pointclouds_with_cov.size(); ++i) {
    const auto& cloud_cov = pointclouds_with_cov[i];
    auto T                = odometry_estimation->estimate(cloud_cov);
    std::cout << "Transformation:\n" << T.matrix() << std::endl;

    const auto& cloud = pointclouds[i];
    visualizer->removeAllPointClouds();
    visualizer->addPointCloud<CloudI::PointType>(
        odometry_estimation->get_points());
    // visualizer->addPointCloud<CloudI::PointType>(
    //     cloud, "cloud" + std::to_string(i));
    // visualizer->updatePointCloudPose(
    //     "cloud" + std::to_string(i), T.cast<float>().inverse());
    visualizer->setPointCloudRenderingProperties(
        pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud");
    visualizer->spin();
  }

  return 0;
}

To Reproduce
Steps to reproduce the behavior:

1. Download the data: https://drive.google.com/file/d/19H3UHx9TwWBBZuTjBcgk3FY9ukc1DRWt/view?usp=sharing
2. Compile the cmake project
3. run ./incremental_gicp_registration <folder>
4. viewer opens, press/hold q key until the registration diverges (each keypress registers the next cloud into the voxelmap)

Expected behavior
Error messages or successful registrations

Screenshots
n/a

Environment (please complete the following information):

• OS: macos
• Version 15
• Language C++

Additional context
n/a

[themightyoarfish]

If one uses only a single cloud (the first one) and registers it again and again, the same problem happens.

[themightyoarfish]

I assume this is related to numerical stability somehow, and repeated multiplications of transforms accumulate error after a while. if i do not use T_last_current, but simply use the current T_world_lidar transform as initial guess, no such problem.

[themightyoarfish]

Another way to work around this is to represent past transforms as quaternion and translation vector instead of rotation matrix, and convert between the two, but maybe this should be done at the level of the registration?