Add transform-reduce extension points for CUDA VGICP derivatives

[noelex]

Summary

This PR adds an extension point to the CUDA VGICP derivative pipeline so downstream implementations can customize how individual correspondences contribute to the final reduction without duplicating correspondence lookup, VGICP derivative evaluation, CUDA stream handling, or CUB reduction logic.

Custom transforms are applied after the standard VGICP derivative/error computation and before reduction. They receive the source/target correspondence indices and can produce arbitrary reduction result types.

This enables use cases such as:

• Robust loss functions
• Per-correspondence weighting or filtering
• Semantic or application-specific correspondence weighting
• Custom reduction accumulators and diagnostics

The transforms are fused into the existing transform-reduce pipeline and do not require materializing per-correspondence derivative results.

What changed

• Make IntegratedVGICPDerivatives polymorphic:
  • Add a virtual destructor
  • Make issue_linearize() and issue_compute_error() virtual
• Add protected CUDA transform-reduce helpers for linearization and error evaluation:
  • Transforms receive the source/target correspondence indices
  • Custom result types, reduction operators, and identity values are supported
  • Invalid correspondences bypass the transform and reduce to the supplied identity value
• Expose the CUDA stream to derived derivative implementations
• Add IntegratedVGICPFactorGPU::replace_derivatives() for derived factors
• Preserve the existing VGICP implementation through identity transforms
• Require factors with custom derivatives to override clone() so cloning cannot silently restore the default implementation
• Bump the gtsam_points_cuda SOVERSION because making IntegratedVGICPDerivatives polymorphic changes its ABI
• Add CUDA tests covering:
  • Custom transform/reduction results
  • Synchronous and asynchronous factor paths
  • Correct cloning behavior
  • Rejection of inherited cloning with replaced derivatives
  • Graceful skipping when no CUDA device is available

Minimal usage example

The following example applies a Cauchy loss to each VGICP correspondence.

The derived implementation only defines how each already-computed VGICP correspondence contributes to the reduction. Correspondence lookup and the standard VGICP derivative/error evaluation remain in gtsam_points.

#include <cmath>
#include <memory>

#include <thrust/pair.h>

#include <gtsam/geometry/Pose3.h>

#include <gtsam_points/cuda/kernels/linearized_system.cuh>
#include <gtsam_points/factors/integrated_vgicp_derivatives.cuh>
#include <gtsam_points/factors/integrated_vgicp_factor_gpu.hpp>

namespace gp = gtsam_points;

struct CauchyLinearization {
  float scale_sq;

  __device__ gp::LinearizedSystem6 operator()(
    const thrust::pair<int, int>&,
    const gp::LinearizedSystem6& input) const {
    const float q = fmaxf(input.error, 0.0f);
    const float normalized = q / scale_sq;
    const float weight = 1.0f / (1.0f + normalized);

    // Copying input preserves num_inliers.
    gp::LinearizedSystem6 output = input;
    output.error = scale_sq * log1pf(normalized);
    output.H_target *= weight;
    output.H_source *= weight;
    output.H_target_source *= weight;
    output.b_target *= weight;
    output.b_source *= weight;
    return output;
  }
};

struct CauchyError {
  float scale_sq;

  __device__ float operator()(
    const thrust::pair<int, int>&,
    float error) const {
    const float q = fmaxf(error, 0.0f);
    return scale_sq * log1pf(q / scale_sq);
  }
};

class CauchyDerivatives final : public gp::IntegratedVGICPDerivatives {
public:
  CauchyDerivatives(
    const gp::GaussianVoxelMapGPU::ConstPtr& target,
    const gp::PointCloud::ConstPtr& source,
    float scale)
  : gp::IntegratedVGICPDerivatives(target, source, nullptr, nullptr),
    scale_sq_(scale * scale) {}

  void issue_linearize(
    const Eigen::Isometry3f* d_x,
    gp::LinearizedSystem6* d_output) override {
    issue_linearize_transform_reduce(
      d_x,
      d_output,
      CauchyLinearization{scale_sq_},
      gp::LinearizedSystem6::zero());
  }

  void issue_compute_error(
    const Eigen::Isometry3f* d_xl,
    const Eigen::Isometry3f* d_xe,
    float* d_output) override {
    issue_compute_error_transform_reduce(
      d_xl,
      d_xe,
      d_output,
      CauchyError{scale_sq_},
      0.0f);
  }

private:
  float scale_sq_;
};

class CauchyVGICPFactor final : public gp::IntegratedVGICPFactorGPU {
public:
  CauchyVGICPFactor(
    const gtsam::Pose3& fixed_target_pose,
    gtsam::Key source_key,
    const gp::GaussianVoxelMapGPU::ConstPtr& target,
    const gp::PointCloud::ConstPtr& source,
    float scale)
  : gp::IntegratedVGICPFactorGPU(
      fixed_target_pose, source_key, target, source),
    fixed_target_pose_(fixed_target_pose),
    source_(source),
    scale_(scale) {
    replace_derivatives(
      std::make_unique<CauchyDerivatives>(target, source, scale));
  }

  gtsam::NonlinearFactor::shared_ptr clone() const override {
    // Custom factors must reinstall their custom derivatives when cloned.
    return gtsam::make_shared<CauchyVGICPFactor>(
      fixed_target_pose_,
      keys()[0],
      get_target(),
      source_,
      scale_);
  }

private:
  gtsam::Pose3 fixed_target_pose_;
  gp::PointCloud::ConstPtr source_;
  float scale_;
};

The factor can then be used like a normal GPU VGICP factor:

auto factor = gtsam::make_shared<CauchyVGICPFactor>(
  fixed_target_pose,
  source_key,
  target_gpu,
  source_gpu,
  1.0f);

graph.add(factor);

Transforms used with the standard factor output should preserve LinearizedSystem6::num_inliers, since it is used to update the factor's inlier statistics.

For more advanced use cases, the transform-reduce helpers also support custom result types and custom associative reduction operators, allowing additional statistics or diagnostics to be accumulated in the same reduction pass.

Compatibility

This PR changes the ABI of gtsam_points_cuda because IntegratedVGICPDerivatives becomes polymorphic. The CUDA library SOVERSION is therefore bumped accordingly.

At the behavioral level, the standard IntegratedVGICPFactorGPU produces the same VGICP results as before because its implementation uses identity transforms.

Derived factors that call replace_derivatives() must override clone() and reinstall an equivalent derivative implementation. The inherited clone() throws instead of silently creating a factor with the default derivatives.

Testing

• VGICPDerivativesTransformReduceTest: 5/5 passed
• CUDA compute-sanitizer: 0 errors

[koide3]

Thank you for your contribution! This feature looks quite interesting. I'll take a closer look at it later.