incremental_covariance_voxelmap.hpp

// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide
// SPDX-License-Identifier: MIT
#pragma once
 
#include <deque>
#include <gtsam_points/util/runnning_statistics.hpp>
#include <gtsam_points/ann/incremental_voxelmap.hpp>
#include <gtsam_points/ann/incremental_covariance_container.hpp>
 
namespace gtsam_points {
 
struct IncrementalCovarianceVoxelMap : public IncrementalVoxelMap<IncrementalCovarianceContainer> {
public:
  IncrementalCovarianceVoxelMap(double voxel_resolution);
  virtual ~IncrementalCovarianceVoxelMap();
 
  void set_num_neighbors(int num_neighbors);
  void set_min_num_neighbors(int min_num_neighbors);
  void set_warmup_cycles(int warmup_cycles);
  void set_lowrate_cycles(int lowrate_cycles);
  void set_remove_invalid_age_thresh(int remove_invalid_age_thresh);
  void set_eig_stddev_thresh_scale(double eig_stddev_thresh_scale);
  void set_num_threads(int num_threads);
 
  virtual void clear() override;
 
  virtual void insert(const PointCloud& points) override;
 
  virtual size_t knn_search(
    const double* pt,
    size_t k,
    size_t* k_indices,
    double* k_sq_dists,
    double max_sq_dist = std::numeric_limits<double>::max()) const override;
 
  size_t knn_search_force(const double* pt, size_t k, size_t* k_indices, double* k_sq_dists, double max_sq_dist = std::numeric_limits<double>::max())
    const;
 
  std::vector<size_t> valid_indices(int num_threads = -1) const;
  std::vector<Eigen::Vector4d> voxel_points(const std::vector<size_t>& indices) const;
  std::vector<Eigen::Vector4d> voxel_normals(const std::vector<size_t>& indices) const;
  std::vector<Eigen::Matrix4d> voxel_covs(const std::vector<size_t>& indices) const;
 
  virtual std::vector<Eigen::Vector4d> voxel_points() const override;
  virtual std::vector<Eigen::Vector4d> voxel_normals() const override;
  virtual std::vector<Eigen::Matrix4d> voxel_covs() const override;
 
protected:
  int num_neighbors;               
  int min_num_neighbors;           
  int warmup_cycles;               
  int lowrate_cycles;              
  int remove_invalid_age_thresh;   
  double eig_stddev_thresh_scale;  
  int num_threads;                 
 
  std::deque<RunningStatistics<Eigen::Array3d>> eig_stats;  
};
 
namespace frame {
 
template <>
struct traits<IncrementalCovarianceVoxelMap> {
  static bool has_points(const IncrementalCovarianceVoxelMap& ivox) { return ivox.has_points(); }
  static bool has_normals(const IncrementalCovarianceVoxelMap& ivox) { return ivox.has_normals(); }
  static bool has_covs(const IncrementalCovarianceVoxelMap& ivox) { return ivox.has_covs(); }
  static bool has_intensities(const IncrementalCovarianceVoxelMap& ivox) { return ivox.has_intensities(); }
 
  static const Eigen::Vector4d& point(const IncrementalCovarianceVoxelMap& ivox, size_t i) { return ivox.point(i); }
  static const Eigen::Vector4d& normal(const IncrementalCovarianceVoxelMap& ivox, size_t i) { return ivox.normal(i); }
  static const Eigen::Matrix4d& cov(const IncrementalCovarianceVoxelMap& ivox, size_t i) { return ivox.cov(i); }
  static double intensity(const IncrementalCovarianceVoxelMap& ivox, size_t i) { return ivox.intensity(i); }
};
 
}  // namespace frame
 
}  // namespace gtsam_points