kdtree_cuda

CMakeLists.txt

@@ -251,12 +251,20 @@ if(BUILD_WITH_CUDA)
     src/gtsam_points/cuda/nonlinear_factor_set_gpu_create.cpp
     src/gtsam_points/cuda/stream_roundrobin.cu
     src/gtsam_points/cuda/stream_temp_buffer_roundrobin.cu
+    # ann
+    src/gtsam_points/ann/kdtree_gpu.cpp
+    src/gtsam_points/ann/kdtree_gpu.cu
     # types
     src/gtsam_points/types/point_cloud.cu
     src/gtsam_points/types/point_cloud_gpu.cu
     src/gtsam_points/types/gaussian_voxelmap_gpu.cu
     src/gtsam_points/types/gaussian_voxelmap_gpu_funcs.cu
     # factors
+    src/gtsam_points/factors/integrated_gicp_derivatives.cu
+    src/gtsam_points/factors/integrated_gicp_derivatives_inliers.cu
+    src/gtsam_points/factors/integrated_gicp_derivatives_linearize.cu
+    src/gtsam_points/factors/integrated_gicp_derivatives_compute.cu
+    src/gtsam_points/factors/integrated_gicp_factor_gpu.cpp
     src/gtsam_points/factors/integrated_vgicp_derivatives.cu
     src/gtsam_points/factors/integrated_vgicp_derivatives_inliers.cu
     src/gtsam_points/factors/integrated_vgicp_derivatives_compute.cu
@@ -267,6 +275,7 @@ if(BUILD_WITH_CUDA)
   )
   target_include_directories(gtsam_points_cuda PUBLIC
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>
     $<INSTALL_INTERFACE:include>
   )
   target_link_libraries(gtsam_points_cuda

include/gtsam_points/ann/fast_occupancy_grid.hpp

@@ -85,6 +85,16 @@ class FastOccupancyGrid {
   template <typename PointCloud>
   std::vector<unsigned char> get_overlaps(const PointCloud& points, const Eigen::Isometry3d& pose = Eigen::Isometry3d::Identity()) const;
 
+  /// @brief Update the overlap status of each point in the point cloud. This function only updates the status of non-overlapping points (points with
+  /// overlap status 1 will be unchanged).
+  /// @param overlaps [in/out] Overlap status of each point (0=free, 1=occupied). Size must be 0 or equal to the number of points in the point cloud.
+  /// @param points   Point cloud.
+  /// @param pose     Pose of the points.
+  /// @return         Number of points that newly found to be overlapping.
+  template <typename PointCloud>
+  int update_overlaps(std::vector<unsigned char>& overlaps, const PointCloud& points, const Eigen::Isometry3d& pose = Eigen::Isometry3d::Identity())
+    const;
+
   /// @brief Get the number of occupied cells in the grid.
   int num_occupied_cells() const;
 

include/gtsam_points/ann/impl/fast_occupancy_grid_impl.hpp

@@ -54,8 +54,26 @@ double FastOccupancyGrid::calc_overlap_rate(const PointCloud& points, const Eige
 template <typename PointCloud>
 std::vector<unsigned char> FastOccupancyGrid::get_overlaps(const PointCloud& points, const Eigen::Isometry3d& pose) const {
   std::vector<unsigned char> overlaps(frame::size(points), 0);
+  update_overlaps(overlaps, points, pose);
+  return overlaps;
+}
+
+template <typename PointCloud>
+int FastOccupancyGrid::update_overlaps(std::vector<unsigned char>& overlaps, const PointCloud& points, const Eigen::Isometry3d& pose) const {
+  if (overlaps.empty()) {
+    overlaps.resize(frame::size(points), 0);
+  }
 
+  if (overlaps.size() != frame::size(points)) {
+    throw std::runtime_error("Overlap vector size must be 0 or equal to the number of points in the point cloud.");
+  }
+
+  int num_updated = 0;
   for (int i = 0; i < frame::size(points); i++) {
+    if (overlaps[i]) {
+      continue;
+    }
+
     const auto& pt = frame::point(points, i);
     const Eigen::Array4i global_coord = fast_floor((pose * pt) * inv_resolution) + coord_offset;
     const Eigen::Array4i block_coord = global_coord / FastOccupancyBlock::stride;
@@ -68,9 +86,10 @@ std::vector<unsigned char> FastOccupancyGrid::get_overlaps(const PointCloud& poi
 
     const Eigen::Array4i cell_coord = global_coord - block_coord * FastOccupancyBlock::stride;
     overlaps[i] = blocks[block_loc].second.occupied(cell_coord.head<3>());
+    num_updated += (overlaps[i] != 0);
   }
 
-  return overlaps;
+  return num_updated;
 }
 
 }  // namespace gtsam_points

include/gtsam_points/ann/kdtree_gpu.hpp

@@ -0,0 +1,66 @@
+// SPDX-License-Identifier: MIT
+// Copyright (c) 2025  Kenji Koide (k.koide@aist.go.jp)
+#pragma once
+
+#include <limits>
+#include <cstdint>
+#include <gtsam_points/types/point_cloud.hpp>
+#include <gtsam_points/ann/small_kdtree.hpp>
+
+struct CUstream_st;
+
+namespace gtsam_points {
+
+struct KdTreeNodeGPU {
+  NodeIndexType left = INVALID_NODE;   ///< Left child node index.
+  NodeIndexType right = INVALID_NODE;  ///< Right child node index.
+
+  union {
+    struct Leaf {
+      NodeIndexType first;  ///< First point index in the leaf node.
+      NodeIndexType last;   ///< Last point index in the leaf node.
+    } lr;                   ///< Leaf node.
+    struct NonLeaf {
+      NodeIndexType axis;  ///< Projection axis.
+      float thresh;        ///< Threshold value.
+    } sub;                 ///< Non-leaf node.
+  } node_type;
+};
+
+class KdTreeGPU {
+public:
+  using Ptr = std::shared_ptr<KdTreeGPU>;
+  using ConstPtr = std::shared_ptr<const KdTreeGPU>;
+
+  KdTreeGPU(const PointCloud::ConstPtr& points, CUstream_st* stream = nullptr);
+  ~KdTreeGPU();
+
+  void nearest_neighbor_search(
+    const Eigen::Vector3f* queries,
+    size_t num_queries,
+    std::uint32_t* nn_indices,
+    float* nn_sq_dists,
+    CUstream_st* stream = nullptr);
+
+  void nearest_neighbor_search_cpu(
+    const Eigen::Vector3f* h_queries,
+    size_t num_queries,
+    std::uint32_t* h_nn_indices,
+    float* h_nn_sq_dists,
+    CUstream_st* stream = nullptr);
+
+  /// @brief Get the GPU pointer to the point indices
+  const std::uint32_t* get_indices() const { return indices; }
+
+  /// @brief Get the GPU pointer to the KdTree nodes
+  const KdTreeNodeGPU* get_nodes() const { return nodes; }
+
+private:
+  PointCloud::ConstPtr points;
+  size_t num_indices;
+  size_t num_nodes;
+  std::uint32_t* indices;
+  KdTreeNodeGPU* nodes;
+};
+
+}  // namespace gtsam_points

include/gtsam_points/ann/small_kdtree.hpp

@@ -100,7 +100,7 @@ struct AxisAlignedProjection {
 };
 
 using NodeIndexType = std::uint32_t;
-static constexpr NodeIndexType INVALID_NODE = std::numeric_limits<NodeIndexType>::max();
+constexpr NodeIndexType INVALID_NODE = std::numeric_limits<NodeIndexType>::max();
 
 /// @brief KdTree node.
 template <typename Projection>

include/gtsam_points/cuda/kernels/correspondence.hpp

@@ -0,0 +1,23 @@
+// SPDX-License-Identifier: MIT
+// Copyright (c) 2025  Kenji Koide (k.koide@aist.go.jp)
+
+#pragma once
+
+#ifdef __CUDACC__
+#define GTSAM_POINTS_HOST_DEVICE __host__ __device__
+#else
+#define GTSAM_POINTS_HOST_DEVICE
+#endif
+
+namespace gtsam_points {
+
+/// @brief A pair of source and target point indices representing a correspondence.
+struct Correspondence {
+  GTSAM_POINTS_HOST_DEVICE Correspondence() : source_idx(-1), target_idx(-1) {}
+  GTSAM_POINTS_HOST_DEVICE Correspondence(int source_idx, int target_idx) : source_idx(source_idx), target_idx(target_idx) {}
+
+  int source_idx;
+  int target_idx;
+};
+
+}  // namespace gtsam_points

include/gtsam_points/cuda/kernels/gicp_derivatives.cuh

@@ -0,0 +1,155 @@
+// SPDX-License-Identifier: MIT
+// Copyright (c) 2025  Kenji Koide (k.koide@aist.go.jp)
+
+#pragma once
+
+#include <Eigen/Core>
+#include <thrust/device_vector.h>
+
+#include <gtsam_points/cuda/kernels/correspondence.hpp>
+#include <gtsam_points/cuda/kernels/pose.cuh>
+#include <gtsam_points/cuda/kernels/linearized_system.cuh>
+#include <gtsam_points/cuda/kernels/robust_kernels.cuh>
+
+namespace gtsam_points {
+
+struct gicp_derivatives_kernel {
+  gicp_derivatives_kernel(
+    const Eigen::Isometry3f* linearization_point_ptr,
+    const Eigen::Vector3f* target_means,
+    const Eigen::Matrix3f* target_covs,
+    const Eigen::Vector3f* source_means,
+    const Eigen::Matrix3f* source_covs,
+    float robust_kernel_width)
+  : linearization_point_ptr(linearization_point_ptr),
+    target_means_ptr(target_means),
+    target_covs_ptr(target_covs),
+    source_means_ptr(source_means),
+    source_covs_ptr(source_covs),
+    robust_kernel_width(robust_kernel_width) {}
+
+  __device__ LinearizedSystem6 operator()(const Correspondence& source_target_correspondence) const {
+    const int source_idx = source_target_correspondence.source_idx;
+    const int target_idx = source_target_correspondence.target_idx;
+    if (source_idx < 0 || target_idx < 0) {
+      return LinearizedSystem6::zero();
+    }
+
+    const Eigen::Isometry3f& x = *linearization_point_ptr;
+    const Eigen::Matrix3f R = x.linear();
+    const Eigen::Vector3f t = x.translation();
+
+    const Eigen::Vector3f mean_A = source_means_ptr[source_idx];
+    const Eigen::Matrix3f cov_A = source_covs_ptr[source_idx];
+    const Eigen::Vector3f transed_mean_A = R * mean_A + t;
+
+    const Eigen::Vector3f mean_B = target_means_ptr[target_idx];
+    const Eigen::Matrix3f cov_B = target_covs_ptr[target_idx];
+
+    const Eigen::Matrix3f RCR = (R * cov_A * R.transpose());
+    const Eigen::Matrix3f RCR_inv = (cov_B + RCR).inverse();
+    const Eigen::Vector3f error = mean_B - transed_mean_A;
+
+    const float error_sq = error.transpose() * RCR_inv * error;
+
+    // Attenuation factor driven by the Euclidean residual (i.e., robust_kernel_width is in the metric unit).
+    // It is multiplied to the cost, H, and b so that they remain consistent with each other (IRLS).
+    // The resulting cost (weight * error_sq) saturates at robust_kernel_width^2 * |RCR_inv| and is identical
+    // to twice the Geman-McClure loss when RCR_inv is the identity.
+    const float weight = robust_kernel_width <= 0.0f ? 1.0f : geman_mcclure_scale_sqdist(error.squaredNorm(), robust_kernel_width);
+
+    Eigen::Matrix<float, 3, 6> J_target;
+    J_target.block<3, 3>(0, 0) = -skew_symmetric(transed_mean_A);
+    J_target.block<3, 3>(0, 3) = Eigen::Matrix3f::Identity();
+
+    Eigen::Matrix<float, 3, 6> J_source;
+    J_source.block<3, 3>(0, 0) = R * skew_symmetric(mean_A);
+    J_source.block<3, 3>(0, 3) = -R;
+
+    Eigen::Matrix<float, 6, 3> J_target_RCR_inv = J_target.transpose() * RCR_inv;
+    Eigen::Matrix<float, 6, 3> J_source_RCR_inv = J_source.transpose() * RCR_inv;
+
+    LinearizedSystem6 linearized;
+    linearized.num_inliers = 1;
+    linearized.error = weight * error_sq;
+    linearized.H_target = weight * J_target_RCR_inv * J_target;
+    linearized.H_source = weight * J_source_RCR_inv * J_source;
+    linearized.H_target_source = weight * J_target_RCR_inv * J_source;
+    linearized.b_target = weight * J_target_RCR_inv * error;
+    linearized.b_source = weight * J_source_RCR_inv * error;
+
+    return linearized;
+  }
+
+  const Eigen::Isometry3f* linearization_point_ptr;
+
+  const Eigen::Vector3f* target_means_ptr;
+  const Eigen::Matrix3f* target_covs_ptr;
+
+  const Eigen::Vector3f* source_means_ptr;
+  const Eigen::Matrix3f* source_covs_ptr;
+  const float robust_kernel_width;
+};
+
+struct gicp_error_kernel {
+  gicp_error_kernel(
+    const Eigen::Isometry3f* linearization_point_ptr,
+    const Eigen::Isometry3f* evaluation_point_ptr,
+    const Eigen::Vector3f* target_means,
+    const Eigen::Matrix3f* target_covs,
+    const Eigen::Vector3f* source_means,
+    const Eigen::Matrix3f* source_covs,
+    float robust_kernel_width)
+  : linearization_point_ptr(linearization_point_ptr),
+    evaluation_point_ptr(evaluation_point_ptr),
+    target_means_ptr(target_means),
+    target_covs_ptr(target_covs),
+    source_means_ptr(source_means),
+    source_covs_ptr(source_covs),
+    robust_kernel_width(robust_kernel_width) {}
+
+  __device__ float operator()(const Correspondence& source_target_correspondence) const {
+    const int source_idx = source_target_correspondence.source_idx;
+    const int target_idx = source_target_correspondence.target_idx;
+    if (source_idx < 0 || target_idx < 0) {
+      return 0.0f;
+    }
+
+    const Eigen::Isometry3f& xl = *linearization_point_ptr;
+    const Eigen::Matrix3f Rl = xl.linear();
+
+    const Eigen::Isometry3f& xe = *evaluation_point_ptr;
+    const Eigen::Matrix3f Re = xe.linear();
+    const Eigen::Vector3f te = xe.translation();
+
+    const Eigen::Vector3f mean_A = source_means_ptr[source_idx];
+    const Eigen::Matrix3f cov_A = source_covs_ptr[source_idx];
+    const Eigen::Vector3f transed_mean_A = Re * mean_A + te;
+
+    const Eigen::Vector3f mean_B = target_means_ptr[target_idx];
+    const Eigen::Matrix3f cov_B = target_covs_ptr[target_idx];
+
+    const Eigen::Matrix3f RCR = (Rl * cov_A * Rl.transpose());
+    const Eigen::Matrix3f RCR_inv = (cov_B + RCR).inverse();
+    Eigen::Vector3f error = mean_B - transed_mean_A;
+
+    const float error_sq = error.transpose() * RCR_inv * error;
+
+    // The attenuation factor is a part of the cost function and thus must be evaluated at the evaluation point.
+    // Only H and b freeze it at the linearization point (Gauss-Newton approximation).
+    const float weight = robust_kernel_width <= 0.0f ? 1.0f : geman_mcclure_scale_sqdist(error.squaredNorm(), robust_kernel_width);
+
+    return weight * error_sq;
+  }
+
+  const Eigen::Isometry3f* linearization_point_ptr;
+  const Eigen::Isometry3f* evaluation_point_ptr;
+
+  const Eigen::Vector3f* target_means_ptr;
+  const Eigen::Matrix3f* target_covs_ptr;
+  const Eigen::Vector3f* source_means_ptr;
+  const Eigen::Matrix3f* source_covs_ptr;
+  const float robust_kernel_width;
+};
+
+}  // namespace gtsam_points