Offloading VGICP_GPU factor

include/gtsam_points/factors/integrated_vgicp_derivatives.cuh

@@ -52,6 +52,12 @@ public:
   void issue_linearize(const Eigen::Isometry3f* d_x, LinearizedSystem6* d_output);
   void issue_compute_error(const Eigen::Isometry3f* d_xl, const Eigen::Isometry3f* d_xe, float* d_output);
 
+  // GPU memory offloading
+  size_t memory_usage_gpu() const;
+  bool loaded_on_gpu() const;
+  bool offload_gpu(CUstream_st* stream = 0);
+  bool reload_gpu(CUstream_st* stream = 0);
+
 private:
   bool enable_offloading;
 

include/gtsam_points/factors/integrated_vgicp_factor_gpu.hpp

@@ -3,6 +3,7 @@
 
 #pragma once
 
+#include <gtsam_points/types/offloadable.hpp>
 #include <gtsam_points/types/point_cloud.hpp>
 #include <gtsam_points/types/gaussian_voxelmap_gpu.hpp>
 #include <gtsam_points/factors/nonlinear_factor_gpu.hpp>
@@ -24,7 +25,7 @@ class TempBufferManager;
  *        Koide et al., "Voxelized GICP for Fast and Accurate 3D Point Cloud Registration", ICRA2021
  *        Koide et al., "Globally Consistent 3D LiDAR Mapping with GPU-accelerated GICP Matching Cost Factors", RA-L2021
  */
-class IntegratedVGICPFactorGPU : public gtsam_points::NonlinearFactorGPU {
+class IntegratedVGICPFactorGPU : public gtsam_points::NonlinearFactorGPU, public gtsam_points::OffloadableGPU {
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   using shared_ptr = std::shared_ptr<IntegratedVGICPFactorGPU>;
@@ -73,11 +74,6 @@ class IntegratedVGICPFactorGPU : public gtsam_points::NonlinearFactorGPU {
   /// @return Approximate CPU memory usage in bytes
   size_t memory_usage() const;
 
-  /// @brief  Calculate the GPU memory usage of this factor
-  /// @note   The result is approximate and does not account for objects not owned by this factor (e.g., point clouds)
-  /// @return Approximate GPU memory usage in bytes
-  size_t memory_usage_gpu() const;
-
   /// @brief Enable or disable GPU memory offloading.
   void set_enable_offloading(bool enable);
 
@@ -133,6 +129,12 @@ class IntegratedVGICPFactorGPU : public gtsam_points::NonlinearFactorGPU {
 
   virtual void sync() override;
 
+  // GPU memory offloading
+  size_t memory_usage_gpu() const override;
+  bool loaded_on_gpu() const override;
+  bool offload_gpu(CUstream_st* stream = 0) override;
+  bool reload_gpu(CUstream_st* stream = 0) override;
+
 private:
   Eigen::Isometry3f calc_delta(const gtsam::Values& values) const;
 

src/gtsam_points/factors/integrated_vgicp_derivatives.cu

@@ -108,4 +108,29 @@ double IntegratedVGICPDerivatives::compute_error(const Eigen::Isometry3f& d_xl,
   return error;
 }
 
+size_t IntegratedVGICPDerivatives::memory_usage_gpu() const {
+  return sizeof(int) + sizeof(int) * num_inliers;
+}
+
+bool IntegratedVGICPDerivatives::loaded_on_gpu() const {
+  return source_inliers;
+}
+
+bool IntegratedVGICPDerivatives::offload_gpu(CUstream_st* stream) {
+  if (!source_inliers) {
+    return false;
+  }
+
+  check_error << cudaFreeAsync(source_inliers, stream);
+  source_inliers = nullptr;
+  return true;
+}
+
+bool IntegratedVGICPDerivatives::reload_gpu(CUstream_st* stream) {
+  if (source_inliers) {
+    return false;
+  }
+  return false;
+}
+
 }  // namespace gtsam_points

src/gtsam_points/factors/integrated_vgicp_factor_gpu.cpp

@@ -95,10 +95,6 @@ size_t IntegratedVGICPFactorGPU::memory_usage() const {
   return sizeof(*this) + sizeof(IntegratedVGICPDerivatives);
 }
 
-size_t IntegratedVGICPFactorGPU::memory_usage_gpu() const {
-  return sizeof(Eigen::Isometry3f) + sizeof(int) + sizeof(int) * derivatives->get_num_inliers();
-}
-
 void IntegratedVGICPFactorGPU::set_enable_offloading(bool enable) {
   derivatives->set_enable_offloading(enable);
 }
@@ -228,6 +224,7 @@ void IntegratedVGICPFactorGPU::set_evaluation_point(const gtsam::Values& values,
 }
 
 void IntegratedVGICPFactorGPU::issue_linearize(const void* lin_input_cpu, const void* lin_input_gpu, void* lin_output_gpu) {
+  touch();
   auto linearization_point = reinterpret_cast<const Eigen::Isometry3f*>(lin_input_cpu);
   auto linearization_point_gpu = reinterpret_cast<const Eigen::Isometry3f*>(lin_input_gpu);
   auto linearized_gpu = reinterpret_cast<LinearizedSystem6*>(lin_output_gpu);
@@ -251,14 +248,15 @@ void IntegratedVGICPFactorGPU::issue_compute_error(
   const void* eval_input_gpu,
   void* eval_output_gpu) {
   //
+  touch();
   auto linearization_point = reinterpret_cast<const Eigen::Isometry3f*>(lin_input_cpu);
   auto evaluation_point = reinterpret_cast<const Eigen::Isometry3f*>(eval_input_cpu);
 
   auto linearization_point_gpu = reinterpret_cast<const Eigen::Isometry3f*>(lin_input_gpu);
   auto evaluation_point_gpu = reinterpret_cast<const Eigen::Isometry3f*>(eval_input_gpu);
 
   auto error_gpu = reinterpret_cast<float*>(eval_output_gpu);
-
+  derivatives->reset_inliers(*linearization_point, linearization_point_gpu);
   derivatives->issue_compute_error(linearization_point_gpu, evaluation_point_gpu, error_gpu);
 }
 
@@ -270,4 +268,20 @@ void IntegratedVGICPFactorGPU::store_computed_error(const void* eval_output_cpu)
 void IntegratedVGICPFactorGPU::sync() {
   derivatives->sync_stream();
 }
+size_t IntegratedVGICPFactorGPU::memory_usage_gpu() const {
+  return derivatives->memory_usage_gpu();
+}
+
+bool IntegratedVGICPFactorGPU::loaded_on_gpu() const {
+  return derivatives->loaded_on_gpu();
+}
+
+bool IntegratedVGICPFactorGPU::offload_gpu(CUstream_st* stream) {
+  return derivatives->offload_gpu(stream);
+}
+
+bool IntegratedVGICPFactorGPU::reload_gpu(CUstream_st* stream) {
+  return derivatives->reload_gpu(stream);
+}
+
 }  // namespace gtsam_points