point_cloud.hpp

 
#ifndef UFO_CLOUD_POINT_CLOUD_HPP
#define UFO_CLOUD_POINT_CLOUD_HPP
 
// UFO
#include <ufo/cloud/cloud.hpp>
#include <ufo/execution/execution.hpp>
#include <ufo/numeric/transform.hpp>
#include <ufo/numeric/vec.hpp>
 
// STL
#include <algorithm>
#include <cstddef>
#include <limits>
 
namespace ufo
{
template <std::size_t Dim, class T, class... Rest>
using PointCloud = Cloud<Vec<Dim, T>, Rest...>;
 
using PointCloud2f = PointCloud<2, float>;
using PointCloud3f = PointCloud<3, float>;
using PointCloud4f = PointCloud<4, float>;
using PointCloud2d = PointCloud<2, double>;
using PointCloud3d = PointCloud<3, double>;
using PointCloud4d = PointCloud<4, double>;
 
//
// Transform
//
 
template <std::size_t Dim, class T, class... Rest>
[[nodiscard]] PointCloud<Dim, T, Rest...> transform(Transform<Dim, T> const&    t,
                                                    PointCloud<Dim, T, Rest...> pc)
{
    transformInPlace(t, pc);
    return pc;
}
 
template <class ExecutionPolicy, std::size_t Dim, class T, class... Rest>
  requires execution::is_execution_policy_v<ExecutionPolicy>
[[nodiscard]] PointCloud<Dim, T, Rest...> transform(ExecutionPolicy&&           policy,
                                                    Transform<Dim, T> const&    t,
                                                    PointCloud<Dim, T, Rest...> pc)
{
    transformInPlace(std::forward<ExecutionPolicy>(policy), t, pc);
    return pc;
}
 
template <std::size_t Dim, class T, class... Rest>
void transformInPlace(Transform<Dim, T> const& t, PointCloud<Dim, T, Rest...>& pc)
{
    auto v = view<0>(pc);
    transform(v.begin(), v.end(), v.begin(), t);
}
 
template <class ExecutionPolicy, std::size_t Dim, class T, class... Rest>
  requires execution::is_execution_policy_v<ExecutionPolicy>
void transformInPlace(ExecutionPolicy&& policy, Transform<Dim, T> const& t,
                      PointCloud<Dim, T, Rest...>& pc)
{
    auto v = view<0>(pc);
    transform(std::forward<ExecutionPolicy>(policy), v.begin(), v.end(), v.begin(), t);
}
 
//
// Filter
//
 
template <std::size_t Dim, class T, class... Rest>
[[nodiscard]] PointCloud<Dim, T, Rest...> filterDistance(PointCloud<Dim, T, Rest...> pc,
                                                         Vec<Dim, T> const& origin,
                                                         T const&           min_distance,
                                                         T const&           max_distance,
                                                         bool filter_nan = true)
{
    filterDistanceInPlace(pc, origin, min_distance, max_distance, filter_nan);
    return pc;
}
 
template <std::size_t Dim, class T, class... Rest>
void filterDistanceInPlace(PointCloud<Dim, T, Rest...>& pc, Vec<Dim, T> const& origin,
                           T const& min_distance, T const& max_distance,
                           bool filter_nan = true)
{
    if (T(0) >= min_distance && std::numeric_limits<T>::max() <= max_distance &&
        !filter_nan) {
        return;
    }
 
    auto const min_sq = min_distance * min_distance;
    auto const max_sq = max_distance * max_distance;
 
    auto to_erase = std::ranges::remove_if(
        pc, [&origin, min_sq, max_sq, filter_nan](Vec<Dim, T> const& x) {
            auto dist_sq = distanceSquared(origin, x);
            return min_sq > dist_sq || max_sq < dist_sq || (filter_nan && isnan(x));
        });
    pc.erase(to_erase.begin(), to_erase.end());
}
}  // namespace ufo
 
#endif  // UFO_CLOUD_POINT_CLOUD_HPP