utils.hpp

Go to the documentation of this file.

// SPDX-FileCopyrightText: 2025 Povl Filip Sonne-Frederiksen
//
// SPDX-License-Identifier: GPL-3.0-or-later
 
#pragma once
#include "reusex/core/logging.hpp"
#include "reusex/types.hpp"
 
#include <Eigen/Core>
#include <boost/functional/hash.hpp>
#include <pcl/ModelCoefficients.h>
#include <pcl/PolygonMesh.h>
#include <pcl/common/pca.h>
#include <range/v3/view/iota.hpp>
#include <range/v3/view/zip.hpp>
 
#include <stdexcept>
 
namespace ReUseX::geometry {

auto dist_plane_point(const Eigen::Vector4d &plane,
                      const Eigen::Vector3d &point) -> double;

auto make_pairs(EigenVectorContainer<double, 4> &planes,
                std::vector<IndicesPtr> &inliers,
                EigenVectorContainer<double, 3> &centroids,
                const double threshold = 0.6,
                const double new_plane_offset = 0.5)
    -> std::vector<std::pair<size_t, size_t>>;

auto force_orthogonal_planes(EigenVectorContainer<double, 4> &planes,
                             const double threshold = 0.1,
                             const Eigen::Matrix<double, 3, 1> &up =
                                 Eigen::Matrix<double, 3, 1>(0, 0, 1))
    -> EigenVectorContainer<double, 4>;

auto compute_number_of_inliers(CloudConstPtr cloud,
                               Eigen::Vector4d const &plane,
                               IndicesConstPtr indices,
                               const float threshold = 0.2) -> size_t;

auto merge_planes(EigenVectorContainer<double, 4> const &planes_,
                  std::vector<IndicesPtr> const &inliers_,
                  EigenVectorContainer<double, 3> const &centroids_,
                  CloudConstPtr cloud, const double angle_threshold = 0.1,
                  const double distance_threshold = 0.5,
                  const double min_overlap = 0.8)
    -> std::tuple<EigenVectorContainer<double, 4>, std::vector<IndicesPtr>,
                  EigenVectorContainer<double, 3>>;

auto separate_planes(const EigenVectorContainer<double, 4> &planes,
                     const Eigen::Vector3d &up = Eigen::Vector3d(0, 0, 1),
                     const double epsilon = 0.1)
    -> std::tuple<std::vector<size_t>, std::vector<size_t>>;

template <typename CloudPtr>
auto compute_polygon_normal(const pcl::Vertices &poly, const CloudPtr &cloud)
    -> Eigen::Vector3f {
  if (poly.vertices.size() < 3) {
    throw std::invalid_argument(
        "Polygon must have at least 3 vertices to compute a normal");
  }
 
  Eigen::Vector3f normal = Eigen::Vector3f::Zero();
  for (size_t i = 0; i < poly.vertices.size(); ++i) {
    const auto idx_c = poly.vertices[i];
    const auto idx_n = poly.vertices[(i + 1) % poly.vertices.size()];
    const auto &v_c = cloud->points[idx_c].getVector3fMap();
    const auto &v_n = cloud->points[idx_n].getVector3fMap();
    normal += v_c.cross(v_n);
  }
 
  const float magnitude = normal.norm();
  if (magnitude < 1e-10f) {
    throw std::runtime_error(
        "Cannot compute normal for degenerate polygon (zero magnitude)");
  }
 
  normal.normalize();
  return normal;
}
 
} // namespace ReUseX::geometry