integrated_vgicp_factor_gpu.hpp

// SPDX-License-Identifier: MIT
// Copyright (c) 2021  Kenji Koide (k.koide@aist.go.jp)
 
#pragma once
 
#include <optional>
#include <gtsam_points/util/gtsam_migration.hpp>
#include <gtsam_points/types/point_cloud.hpp>
#include <gtsam_points/types/gaussian_voxelmap_gpu.hpp>
#include <gtsam_points/factors/nonlinear_factor_gpu.hpp>
 
struct CUstream_st;
 
namespace gtsam {
class Pose3;
}
 
namespace gtsam_points {
 
class LinearizedSystem6;
class IntegratedVGICPDerivatives;
class TempBufferManager;
 
class IntegratedVGICPFactorGPU : public gtsam_points::NonlinearFactorGPU {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  using shared_ptr = gtsam_points::shared_ptr<IntegratedVGICPFactorGPU>;
 
  IntegratedVGICPFactorGPU(
    gtsam::Key target_key,
    gtsam::Key source_key,
    const GaussianVoxelMap::ConstPtr& target,
    const PointCloud::ConstPtr& source,
    CUstream_st* stream = nullptr,
    std::shared_ptr<TempBufferManager> temp_buffer = nullptr);
 
  IntegratedVGICPFactorGPU(
    const gtsam::Pose3& fixed_target_pose,
    gtsam::Key source_key,
    const GaussianVoxelMap::ConstPtr& target,
    const PointCloud::ConstPtr& source,
    CUstream_st* stream = nullptr,
    std::shared_ptr<TempBufferManager> temp_buffer = nullptr);
 
  virtual ~IntegratedVGICPFactorGPU() override;
 
  virtual void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const override;
 
  size_t memory_usage() const;
 
  size_t memory_usage_gpu() const;
 
  void set_enable_offloading(bool enable);
 
  void set_enable_surface_validation(bool enable);
 
  void set_inlier_update_thresh(double trans, double angle);
 
  int num_inliers() const;
 
  double inlier_fraction() const;
 
  GaussianVoxelMapGPU::ConstPtr get_target() const { return target; }
 
  Eigen::Isometry3f get_fixed_target_pose() const { return fixed_target_pose; }
 
  // forbid copy
  IntegratedVGICPFactorGPU(const IntegratedVGICPFactorGPU&) = delete;
  IntegratedVGICPFactorGPU& operator=(const IntegratedVGICPFactorGPU&) = delete;
 
  virtual gtsam::NonlinearFactor::shared_ptr clone() const override;
 
  virtual size_t dim() const override { return 6; }
  virtual double error(const gtsam::Values& values) const override;
  virtual gtsam::GaussianFactor::shared_ptr linearize(const gtsam::Values& values) const override;
 
  virtual size_t linearization_input_size() const override;
  virtual size_t linearization_output_size() const override;
  virtual size_t evaluation_input_size() const override;
  virtual size_t evaluation_output_size() const override;
 
  virtual void set_linearization_point(const gtsam::Values& values, void* lin_input_cpu) override;
  virtual void issue_linearize(const void* lin_input_cpu, const void* lin_input_gpu, void* lin_output_gpu) override;
  virtual void store_linearized(const void* lin_output_cpu) override;
 
  virtual void set_evaluation_point(const gtsam::Values& values, void* eval_input_cpu) override;
  virtual void issue_compute_error(
    const void* lin_input_cpu,
    const void* eval_input_cpu,
    const void* lin_input_gpu,
    const void* eval_input_gpu,
    void* eval_output_gpu) override;
  virtual void store_computed_error(const void* eval_output_cpu) override;
 
  virtual void sync() override;
 
private:
  Eigen::Isometry3f calc_delta(const gtsam::Values& values) const;
 
private:
  bool is_binary;
  Eigen::Isometry3f fixed_target_pose;
 
  GaussianVoxelMapGPU::ConstPtr target;
  PointCloud::ConstPtr source;
 
  std::unique_ptr<IntegratedVGICPDerivatives> derivatives;
 
  mutable bool linearized;
  mutable Eigen::Isometry3f linearization_point;
 
  mutable std::optional<float> evaluation_result;
  mutable std::unique_ptr<LinearizedSystem6> linearization_result;
};
 
}  // namespace gtsam_points