registration_helper

CMakeLists.txt

@@ -54,6 +54,7 @@ endif()
 
 add_library(small_gicp SHARED
   src/small_gicp/registration/registration.cpp
+  src/small_gicp/registration/registration_helper.cpp
 )
 target_include_directories(small_gicp PUBLIC
   include

include/small_gicp/points/point_cloud.hpp

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <memory>
 #include <Eigen/Core>
 #include <small_gicp/points/traits.hpp>
 

include/small_gicp/registration/registration_helper.hpp

@@ -0,0 +1,81 @@
+#pragma once
+
+#include <small_gicp/ann/kdtree.hpp>
+#include <small_gicp/ann/gaussian_voxelmap.hpp>
+#include <small_gicp/points/point_cloud.hpp>
+#include <small_gicp/registration/registration_result.hpp>
+
+namespace small_gicp {
+
+/// @brief Preprocess point cloud (downsampling, kdtree creation, and normal and covariance estimation)
+/// @param points                Input points
+/// @param downsample_resolution Downsample resolution
+/// @param num_neighbors         Number of neighbors for normal/covariance estimation
+/// @param num_threads           Number of threads
+std::pair<PointCloud::Ptr, std::shared_ptr<KdTree<PointCloud>>>
+preprocess_points(const PointCloud& points, double downsample_resolution, int num_neighbors = 10, int num_threads = 4);
+
+/// @brief Preprocess point cloud (downsampling, kdtree creation, and normal and covariance estimation)
+/// @note  This function only accepts Eigen::Vector(3|4)(f|d) as input
+template <typename T, int D>
+std::pair<PointCloud::Ptr, std::shared_ptr<KdTree<PointCloud>>>
+preprocess_points(const std::vector<Eigen::Matrix<T, D, 1>>& points, double downsample_resolution, int num_neighbors = 10, int num_threads = 4);
+
+/// @brief Create Gaussian voxel map
+/// @param points            Input points
+/// @param voxel_resolution  Voxel resolution
+GaussianVoxelMap::Ptr create_gaussian_voxelmap(const PointCloud& points, double voxel_resolution);
+
+/// @brief Registration setting
+struct RegistrationSetting {
+  enum RegistrationType { ICP, PLANE_ICP, GICP, VGICP };
+
+  RegistrationType type = GICP;              ///< Registration type
+  double voxel_resolution = 1.0;             ///< Voxel resolution for VGICP
+  double downsample_resolution = 0.25;       ///< Downsample resolution (this will be used only in the Eigen-based interface)
+  double max_correspondence_distance = 1.0;  ///< Maximum correspondence distance
+  int num_threads = 4;                       ///< Number of threads
+};
+
+/// @brief Align point clouds
+/// @note This function only accepts Eigen::Vector(3|4)(f|d) as input
+/// @param target     Target points
+/// @param source     Source points
+/// @param init_T     Initial guess of T_target_source
+/// @param setting    Registration setting
+/// @return           Registration result
+template <typename T, int D>
+RegistrationResult align(
+  const std::vector<Eigen::Matrix<T, D, 1>>& target,
+  const std::vector<Eigen::Matrix<T, D, 1>>& source,
+  const Eigen::Isometry3d& init_T = Eigen::Isometry3d::Identity(),
+  const RegistrationSetting& setting = RegistrationSetting());
+
+/// @brief Align preprocessed point clouds
+/// @param target       Target point cloud
+/// @param source       Source point cloud
+/// @param target_tree  Nearest neighbor search for the target point cloud
+/// @param init_T       Initial guess of T_target_source
+/// @param setting      Registration setting
+/// @return             Registration result
+RegistrationResult align(
+  const PointCloud& target,
+  const PointCloud& source,
+  const KdTree<PointCloud>& target_tree,
+  const Eigen::Isometry3d& init_T = Eigen::Isometry3d::Identity(),
+  const RegistrationSetting& setting = RegistrationSetting());
+
+/// @brief Align preprocessed point clouds with VGICP
+/// @param target       Target point cloud
+/// @param source       Source point cloud
+/// @param target_tree  Nearest neighbor search for the target point cloud
+/// @param init_T       Initial guess of T_target_source
+/// @param setting      Registration setting
+/// @return             Registration result
+RegistrationResult align(
+  const GaussianVoxelMap& target,
+  const PointCloud& source,
+  const Eigen::Isometry3d& init_T = Eigen::Isometry3d::Identity(),
+  const RegistrationSetting& setting = RegistrationSetting());
+
+}  // namespace small_gicp

include/small_gicp/util/downsampling.hpp

@@ -65,4 +65,36 @@ std::shared_ptr<OutputPointCloud> voxelgrid_sampling(const InputPointCloud& poin
   return downsampled;
 }
 
+/// @brief Random downsampling.
+/// @param points      Input points
+/// @param num_samples Number of samples to be drawn
+/// @return            Downsampled points
+template <typename InputPointCloud, typename OutputPointCloud = InputPointCloud, typename RNG = std::mt19937>
+std::shared_ptr<OutputPointCloud> random_sampling(const InputPointCloud& points, size_t num_samples, RNG& rng) {
+  if (traits::size(points) == 0) {
+    std::cerr << "warning: empty input points!!" << std::endl;
+    return std::make_shared<OutputPointCloud>();
+  }
+
+  std::vector<size_t> indices(traits::size(points));
+  std::iota(indices.begin(), indices.end(), 0);
+
+  if (num_samples >= indices.size()) {
+    std::cerr << "warning: num_samples >= points.size()!! (" << num_samples << " vs " << traits::size(points) << ")" << std::endl;
+    num_samples = indices.size();
+  }
+
+  std::vector<size_t> samples(num_samples);
+  std::sample(indices.begin(), indices.end(), samples.begin(), num_samples, rng);
+
+  auto downsampled = std::make_shared<OutputPointCloud>();
+  traits::resize(*downsampled, num_samples);
+
+  for (size_t i = 0; i < num_samples; i++) {
+    traits::set_point(*downsampled, i, traits::point(points, samples[i]));
+  }
+
+  return downsampled;
+}
+
 }  // namespace small_gicp

src/odometry_benchmark.cpp

@@ -37,7 +37,7 @@ int main(int argc, char** argv) {
   OdometryEstimationParams params;
   std::string engine = "small_gicp";
 
-  for (auto arg = argv + 3; arg != argv + argc; arg++) {
+  for (auto arg = argv + 1; arg != argv + argc; arg++) {
     if (std::string(*arg) == "--visualize") {
       params.visualize = true;
     } else if (std::string(*arg) == "--num_threads") {

src/small_gicp/registration/registration_helper.cpp

@@ -0,0 +1,118 @@
+#include <small_gicp/registration/registration_helper.hpp>
+
+#include <small_gicp/ann/kdtree.hpp>
+#include <small_gicp/ann/kdtree_tbb.hpp>
+#include <small_gicp/ann/gaussian_voxelmap.hpp>
+#include <small_gicp/util/downsampling.hpp>
+#include <small_gicp/util/downsampling_omp.hpp>
+#include <small_gicp/util/normal_estimation.hpp>
+#include <small_gicp/util/normal_estimation_omp.hpp>
+
+#include <small_gicp/factors/icp_factor.hpp>
+#include <small_gicp/factors/plane_icp_factor.hpp>
+#include <small_gicp/factors/gicp_factor.hpp>
+#include <small_gicp/registration/reduction_omp.hpp>
+#include <small_gicp/registration/registration.hpp>
+
+namespace small_gicp {
+
+// Preprocess points
+std::pair<PointCloud::Ptr, std::shared_ptr<KdTree<PointCloud>>> preprocess_points(const PointCloud& points, double downsample_resolution, int num_neighbors, int num_threads) {
+  if (num_threads == 1) {
+    auto downsampled = voxelgrid_sampling(points, downsample_resolution);
+    auto kdtree = std::make_shared<KdTree<PointCloud>>(downsampled);
+    estimate_normals_covariances(*downsampled, *kdtree, num_neighbors);
+    return {downsampled, kdtree};
+  } else {
+    auto downsampled = voxelgrid_sampling_omp(points, downsample_resolution);
+    auto kdtree = std::make_shared<KdTree<PointCloud>>(downsampled);
+    estimate_normals_covariances_omp(*downsampled, *kdtree, num_neighbors, num_threads);
+    return {downsampled, kdtree};
+  }
+}
+
+// Preprocess points with Eigen input
+template <typename T, int D>
+std::pair<PointCloud::Ptr, std::shared_ptr<KdTree<PointCloud>>>
+preprocess_points(const std::vector<Eigen::Matrix<T, D, 1>>& points, double downsample_resolution, int num_neighbors, int num_threads) {
+  return preprocess_points(*std::make_shared<PointCloud>(points), downsample_resolution, num_neighbors, num_threads);
+}
+
+// Explicit instantiation
+template std::pair<PointCloud::Ptr, std::shared_ptr<KdTree<PointCloud>>> preprocess_points(const std::vector<Eigen::Matrix<float, 3, 1>>&, double, int, int);
+template std::pair<PointCloud::Ptr, std::shared_ptr<KdTree<PointCloud>>> preprocess_points(const std::vector<Eigen::Matrix<float, 4, 1>>&, double, int, int);
+template std::pair<PointCloud::Ptr, std::shared_ptr<KdTree<PointCloud>>> preprocess_points(const std::vector<Eigen::Matrix<double, 3, 1>>&, double, int, int);
+template std::pair<PointCloud::Ptr, std::shared_ptr<KdTree<PointCloud>>> preprocess_points(const std::vector<Eigen::Matrix<double, 4, 1>>&, double, int, int);
+
+// Create Gaussian voxel map
+GaussianVoxelMap::Ptr create_gaussian_voxelmap(const PointCloud& points, double voxel_resolution) {
+  auto voxelmap = std::make_shared<GaussianVoxelMap>(voxel_resolution);
+  voxelmap->insert(points);
+  return voxelmap;
+}
+
+// Align point clouds with Eigen input
+template <typename T, int D>
+RegistrationResult
+align(const std::vector<Eigen::Matrix<T, D, 1>>& target, const std::vector<Eigen::Matrix<T, D, 1>>& source, const Eigen::Isometry3d& init_T, const RegistrationSetting& setting) {
+  auto [target_points, target_tree] = preprocess_points(*std::make_shared<PointCloud>(target), setting.downsample_resolution, 10, setting.num_threads);
+  auto [source_points, source_tree] = preprocess_points(*std::make_shared<PointCloud>(source), setting.downsample_resolution, 10, setting.num_threads);
+
+  if (setting.type == RegistrationSetting::VGICP) {
+    auto target_voxelmap = create_gaussian_voxelmap(*target_points, setting.voxel_resolution);
+    return align(*target_voxelmap, *source_points, init_T, setting);
+  } else {
+    return align(*target_points, *source_points, *target_tree, init_T, setting);
+  }
+}
+
+template RegistrationResult
+align(const std::vector<Eigen::Matrix<float, 3, 1>>&, const std::vector<Eigen::Matrix<float, 3, 1>>&, const Eigen::Isometry3d&, const RegistrationSetting&);
+template RegistrationResult
+align(const std::vector<Eigen::Matrix<float, 4, 1>>&, const std::vector<Eigen::Matrix<float, 4, 1>>&, const Eigen::Isometry3d&, const RegistrationSetting&);
+template RegistrationResult
+align(const std::vector<Eigen::Matrix<double, 3, 1>>&, const std::vector<Eigen::Matrix<double, 3, 1>>&, const Eigen::Isometry3d&, const RegistrationSetting&);
+template RegistrationResult
+align(const std::vector<Eigen::Matrix<double, 4, 1>>&, const std::vector<Eigen::Matrix<double, 4, 1>>&, const Eigen::Isometry3d&, const RegistrationSetting&);
+
+// Align point clouds
+RegistrationResult
+align(const PointCloud& target, const PointCloud& source, const KdTree<PointCloud>& target_tree, const Eigen::Isometry3d& init_T, const RegistrationSetting& setting) {
+  switch (setting.type) {
+    case RegistrationSetting::ICP: {
+      Registration<ICPFactor, ParallelReductionOMP> registration;
+      registration.reduction.num_threads = setting.num_threads;
+      registration.rejector.max_dist_sq = setting.max_correspondence_distance * setting.max_correspondence_distance;
+      return registration.align(target, source, target_tree, init_T);
+    }
+    case RegistrationSetting::PLANE_ICP: {
+      Registration<PointToPlaneICPFactor, ParallelReductionOMP> registration;
+      registration.reduction.num_threads = setting.num_threads;
+      registration.rejector.max_dist_sq = setting.max_correspondence_distance * setting.max_correspondence_distance;
+      return registration.align(target, source, target_tree, init_T);
+    }
+    case RegistrationSetting::GICP: {
+      Registration<GICPFactor, ParallelReductionOMP> registration;
+      registration.reduction.num_threads = setting.num_threads;
+      registration.rejector.max_dist_sq = setting.max_correspondence_distance * setting.max_correspondence_distance;
+      return registration.align(target, source, target_tree, init_T);
+    }
+    case RegistrationSetting::VGICP: {
+      std::cerr << "error: use align(const GaussianVoxelMap&, const GaussianVoxelMap&, const Eigen::Isometry3d&, const RegistrationSetting&) for VGICP" << std::endl;
+      return RegistrationResult(Eigen::Isometry3d::Identity());
+    }
+  }
+}
+
+// Align point clouds with VGICP
+RegistrationResult align(const GaussianVoxelMap& target, const PointCloud& source, const Eigen::Isometry3d& init_T, const RegistrationSetting& setting) {
+  if (setting.type != RegistrationSetting::VGICP) {
+    std::cerr << "invalid registration type for GaussianVoxelMap" << std::endl;
+  }
+
+  Registration<GICPFactor, ParallelReductionOMP> registration;
+  registration.reduction.num_threads = setting.num_threads;
+  return registration.align(target, source, target, init_T);
+}
+
+}  // namespace small_gicp