projection.hpp

Go to the documentation of this file.

// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide
// SPDX-License-Identifier: MIT
#pragma once
 
#include <array>
#include <Eigen/Core>
#include <Eigen/Eigen>
 
namespace small_gicp {
 
struct ProjectionSetting {
  int max_scan_count = 128;  
};
 
struct AxisAlignedProjection {
public:
  double operator()(const Eigen::Vector4d& pt) const { return pt[axis]; }
 
  template <typename PointCloud, typename IndexConstIterator>
  static AxisAlignedProjection find_axis(const PointCloud& points, IndexConstIterator first, IndexConstIterator last, const ProjectionSetting& setting) {
    const size_t N = std::distance(first, last);
    Eigen::Vector4d sum_pt = Eigen::Vector4d::Zero();
    Eigen::Vector4d sum_sq = Eigen::Vector4d::Zero();
 
    const size_t step = N < setting.max_scan_count ? 1 : N / setting.max_scan_count;
    const size_t num_steps = N / step;
    for (int i = 0; i < num_steps; i++) {
      const auto itr = first + step * i;
      const Eigen::Vector4d pt = traits::point(points, *itr);
      sum_pt += pt;
      sum_sq += pt.cwiseProduct(pt);
    }
 
    const Eigen::Vector4d mean = sum_pt / sum_pt.w();
    const Eigen::Vector4d var = (sum_sq - mean.cwiseProduct(sum_pt));
 
    return AxisAlignedProjection{var[0] > var[1] ? (var[0] > var[2] ? 0 : 2) : (var[1] > var[2] ? 1 : 2)};
  }
 
public:
  int axis;  
};
 
struct NormalProjection {
public:
  double operator()(const Eigen::Vector4d& pt) const { return Eigen::Map<const Eigen::Vector3d>(normal.data()).dot(pt.head<3>()); }
 
  template <typename PointCloud, typename IndexConstIterator>
  static NormalProjection find_axis(const PointCloud& points, IndexConstIterator first, IndexConstIterator last, const ProjectionSetting& setting) {
    const size_t N = std::distance(first, last);
    Eigen::Vector4d sum_pt = Eigen::Vector4d::Zero();
    Eigen::Matrix4d sum_sq = Eigen::Matrix4d::Zero();
 
    const size_t step = N < setting.max_scan_count ? 1 : N / setting.max_scan_count;
    const size_t num_steps = N / step;
    for (int i = 0; i < num_steps; i++) {
      const auto itr = first + step * i;
      const Eigen::Vector4d pt = traits::point(points, *itr);
      sum_pt += pt;
      sum_sq += pt * pt.transpose();
    }
 
    const Eigen::Vector4d mean = sum_pt / sum_pt.w();
    const Eigen::Matrix4d cov = (sum_sq - mean * sum_pt.transpose()) / sum_pt.w();
 
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eig;
    eig.computeDirect(cov.block<3, 3>(0, 0));
 
    NormalProjection p;
    Eigen::Map<Eigen::Vector3d>(p.normal.data()) = eig.eigenvectors().col(2);
    return p;
  }
 
public:
  std::array<double, 3> normal;  
};
 
}  // namespace small_gicp