Function ComputeMeanAndNormal

cpp/map_closures/VoxelMap.cpp:42–61

static constexpr unsigned int min_points_for_covariance_computation = 10;

std::tuple<Eigen::Vector3d, Eigen::Vector3d> ComputeMeanAndNormal(
    const map_closures::VoxelBlock &coordinates) {
    const double num_points = static_cast<double>(coordinates.size());
    Eigen::Vector3d mean =
        std::reduce(coordinates.cbegin(), coordinates.cend(), Eigen::Vector3d().setZero()) /
        num_points;

    const Eigen::Matrix3d covariance =
        std::transform_reduce(coordinates.cbegin(), coordinates.cend(), Eigen::Matrix3d().setZero(),
                              std::plus<Eigen::Matrix3d>(),
                              [&mean](const Eigen::Vector3d &point) {
                                  Eigen::Vector3d centered = point - mean;
                                  Eigen::Matrix3d S = centered * centered.transpose();
                                  return S;
                              }) /
        (num_points - 1);
    const Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver(covariance);
    Eigen::Vector3d normal = solver.eigenvectors().col(0);
    return {std::move(mean), std::move(normal)};
}

}  // namespace