covariance_estimation.hpp

// SPDX-License-Identifier: MIT
// Copyright (c) 2021  Kenji Koide (k.koide@aist.go.jp)
 
#pragma once
 
#include <vector>
#include <Eigen/Core>
#include <gtsam_points/types/point_cloud.hpp>
 
namespace gtsam_points {
 
struct CovarianceEstimationParams {
public:
  enum RegularizationMethod { NONE, EIG };
 
  CovarianceEstimationParams() : num_threads(1), k_neighbors(10), regularization_method(EIG), eigen_values(1e-3, 1.0, 1.0) {}
 
public:
  int num_threads;                             
  int k_neighbors;                             
  RegularizationMethod regularization_method;  
  Eigen::Vector3d eigen_values;                
};
 
std::vector<Eigen::Matrix4d> estimate_covariances(const Eigen::Vector4d* points, int num_points, const CovarianceEstimationParams& params);
 
std::vector<Eigen::Matrix4d>
estimate_covariances(const Eigen::Vector4d* points, int num_points, int k_neighbors, const Eigen::Vector3d& eigen_values, int num_threads);
 
std::vector<Eigen::Matrix4d> estimate_covariances(const Eigen::Vector4d* points, int num_points, int k_neighbors = 10, int num_threads = 1);
 
template <typename Alloc>
std::vector<Eigen::Matrix4d> estimate_covariances(const std::vector<Eigen::Vector4d, Alloc>& points, int k_neighbors = 10, int num_threads = 1) {
  return estimate_covariances(points.data(), points.size(), k_neighbors, num_threads);
}
 
std::vector<Eigen::Matrix4d> estimate_covariances(const PointCloud& points, int k_neighbors = 10, int num_threads = 1);
 
}  // namespace gtsam_points