gaussian_voxelmap.hpp

// SPDX-License-Identifier: MIT
// Copyright (c) 2021  Kenji Koide (k.koide@aist.go.jp)
 
#pragma once
 
#include <gtsam_points/types/point_cloud.hpp>
 
// forward declaration
struct CUstream_st;
 
namespace gtsam_points {
 
class GaussianVoxelMap {
public:
  using Ptr = std::shared_ptr<GaussianVoxelMap>;
  using ConstPtr = std::shared_ptr<const GaussianVoxelMap>;
 
  GaussianVoxelMap() {}
  virtual ~GaussianVoxelMap() {}
 
  virtual void save_compact(const std::string& path) const = 0;
 
  virtual double voxel_resolution() const = 0;
 
  virtual void insert(const PointCloud& frame) = 0;
};
 
double overlap(const GaussianVoxelMap::ConstPtr& target, const PointCloud::ConstPtr& source, const Eigen::Isometry3d& T_target_source);
 
double overlap(
  const std::vector<GaussianVoxelMap::ConstPtr>& targets,
  const PointCloud::ConstPtr& source,
  const std::vector<Eigen::Isometry3d>& Ts_target_source);
 
template <typename VoxelMapPtr>
std::enable_if_t<!std::is_same_v<VoxelMapPtr, GaussianVoxelMap::ConstPtr>, double>
overlap(const std::vector<VoxelMapPtr>& targets, const PointCloud::ConstPtr& source, const std::vector<Eigen::Isometry3d>& Ts_target_source) {
  const std::vector<GaussianVoxelMap::ConstPtr> targets_(targets.begin(), targets.end());
  return overlap(targets_, source, Ts_target_source);
}
 
double overlap_gpu(
  const GaussianVoxelMap::ConstPtr& target,
  const PointCloud::ConstPtr& source,
  const Eigen::Isometry3f* T_target_source_gpu,
  CUstream_st* stream = 0);
 
double overlap_gpu(
  const GaussianVoxelMap::ConstPtr& target,
  const PointCloud::ConstPtr& source,
  const Eigen::Isometry3d& T_target_source,
  CUstream_st* stream = 0);
 
double overlap_gpu(
  const PointCloud::ConstPtr& target,
  const PointCloud::ConstPtr& source,
  const Eigen::Isometry3d& T_target_source,
  CUstream_st* stream = 0);
 
double overlap_gpu(
  const std::vector<GaussianVoxelMap::ConstPtr>& targets,
  const PointCloud::ConstPtr& source,
  const std::vector<Eigen::Isometry3d>& Ts_target_source,
  CUstream_st* stream = 0);
 
std::vector<double> overlap_gpu(
  const std::vector<GaussianVoxelMap::ConstPtr>& targets,
  const std::vector<PointCloud::ConstPtr>& sources,
  const std::vector<Eigen::Isometry3d>& Ts_target_source,
  CUstream_st* stream = 0);
 
template <typename VoxelMapPtr>
std::enable_if_t<!std::is_same_v<VoxelMapPtr, GaussianVoxelMap::ConstPtr>, double> overlap_gpu(
  const std::vector<VoxelMapPtr>& targets,
  const PointCloud::ConstPtr& source,
  const std::vector<Eigen::Isometry3d>& Ts_target_source,
  CUstream_st* stream = 0) {
  const std::vector<GaussianVoxelMap::ConstPtr> targets_(targets.begin(), targets.end());
  return overlap_gpu(targets_, source, Ts_target_source, stream);
}
 
template <typename VoxelMapPtr, typename PointCloudPtr>
std::
  enable_if_t<!std::is_same_v<VoxelMapPtr, GaussianVoxelMap::ConstPtr> || !std::is_same_v<PointCloudPtr, PointCloud::ConstPtr>, std::vector<double>>
  overlap_gpu(
    const std::vector<VoxelMapPtr>& targets,
    const std::vector<PointCloudPtr>& sources,
    const std::vector<Eigen::Isometry3d>& Ts_target_source,
    CUstream_st* stream = 0) {
  const std::vector<GaussianVoxelMap::ConstPtr> targets_(targets.begin(), targets.end());
  const std::vector<PointCloud::ConstPtr> sources_(sources.begin(), sources.end());
  return overlap_gpu(targets_, sources_, Ts_target_source, stream);
}
 
// Automatically select CPU or GPU method
double overlap_auto(const GaussianVoxelMap::ConstPtr& target, const PointCloud::ConstPtr& source, const Eigen::Isometry3d& T_target_source);
 
double overlap_auto(
  const std::vector<GaussianVoxelMap::ConstPtr>& targets,
  const PointCloud::ConstPtr& source,
  const std::vector<Eigen::Isometry3d>& Ts_target_source);
 
template <typename VoxelMapPtr>
std::enable_if_t<!std::is_same_v<VoxelMapPtr, GaussianVoxelMap::ConstPtr>, double>
overlap_auto(const std::vector<VoxelMapPtr>& targets, const PointCloud::ConstPtr& source, const std::vector<Eigen::Isometry3d>& Ts_target_source) {
  const std::vector<GaussianVoxelMap::ConstPtr> targets_(targets.begin(), targets.end());
  return overlap_auto(targets_, source, Ts_target_source);
}
 
}  // namespace gtsam_points