gaussian_voxelmap_cpu.hpp

// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide
// SPDX-License-Identifier: MIT
#pragma once
 
#include <Eigen/Core>
#include <gtsam_points/types/point_cloud.hpp>
#include <gtsam_points/types/gaussian_voxelmap.hpp>
#include <gtsam_points/ann/incremental_voxelmap.hpp>
 
namespace gtsam_points {
 
struct GaussianVoxel {
public:
  using Ptr = std::shared_ptr<GaussianVoxel>;
  using ConstPtr = std::shared_ptr<const GaussianVoxel>;
 
  struct Setting {};
 
  GaussianVoxel() : finalized(false), num_points(0), mean(Eigen::Vector4d::Zero()), cov(Eigen::Matrix4d::Zero()), intensity(0.0) {}
 
  size_t size() const { return 1; }
 
  void add(const Setting& setting, const PointCloud& points, size_t i);
 
  void finalize();
 
  template <typename Result>
  void knn_search(const Eigen::Vector4d& pt, Result& result) const {
    const double sq_dist = (pt - mean).squaredNorm();
    result.push(0, sq_dist);
  }
 
public:
  bool finalized;        
  size_t num_points;     
  Eigen::Vector4d mean;  
  Eigen::Matrix4d cov;   
  double intensity;       
};
 
namespace frame {
 
template <>
struct traits<GaussianVoxel> {
  static int size(const GaussianVoxel& frame) { return frame.size(); }
 
  static bool has_points(const GaussianVoxel& frame) { return true; }
  static bool has_normals(const GaussianVoxel& frame) { return false; }
  static bool has_covs(const GaussianVoxel& frame) { return true; }
  static bool has_intensities(const GaussianVoxel& frame) { return true; }
 
  static const Eigen::Vector4d& point(const GaussianVoxel& frame, size_t i) { return frame.mean; }
  static const Eigen::Vector4d normal(const GaussianVoxel& frame, size_t i) { return Eigen::Vector4d::Zero(); }
  static const Eigen::Matrix4d& cov(const GaussianVoxel& frame, size_t i) { return frame.cov; }
  static double intensity(const GaussianVoxel& frame, size_t i) { return frame.intensity; }
};
 
}  // namespace frame
 
class GaussianVoxelMapCPU : public GaussianVoxelMap, public IncrementalVoxelMap<GaussianVoxel> {
public:
  using Ptr = std::shared_ptr<GaussianVoxelMapCPU>;
  using ConstPtr = std::shared_ptr<const GaussianVoxelMapCPU>;
 
  GaussianVoxelMapCPU(double resolution);
  virtual ~GaussianVoxelMapCPU();
 
  virtual double voxel_resolution() const override;
 
  Eigen::Vector3i voxel_coord(const Eigen::Vector4d& x) const;
 
  int lookup_voxel_index(const Eigen::Vector3i& coord) const;
 
  const GaussianVoxel& lookup_voxel(int voxel_id) const;
 
  virtual void insert(const PointCloud& frame) override;
 
  void save_compact(const std::string& path) const override;
 
  static GaussianVoxelMapCPU::Ptr load(const std::string& path);
};
 
namespace frame {
 
template <>
struct traits<GaussianVoxelMapCPU> {
  static bool has_points(const GaussianVoxelMapCPU& ivox) { return ivox.has_points(); }
  static bool has_normals(const GaussianVoxelMapCPU& ivox) { return ivox.has_normals(); }
  static bool has_covs(const GaussianVoxelMapCPU& ivox) { return ivox.has_covs(); }
  static bool has_intensities(const GaussianVoxelMapCPU& ivox) { return ivox.has_intensities(); }
 
  static decltype(auto) point(const GaussianVoxelMapCPU& ivox, size_t i) { return ivox.point(i); }
  static decltype(auto) normal(const GaussianVoxelMapCPU& ivox, size_t i) { return ivox.normal(i); }
  static decltype(auto) cov(const GaussianVoxelMapCPU& ivox, size_t i) { return ivox.cov(i); }
  static decltype(auto) intensity(const GaussianVoxelMapCPU& ivox, size_t i) { return ivox.intensity(i); }
};
 
}  // namespace frame
 
}  // namespace gtsam_points