perspective.hpp

 
#ifndef UFO_VISION_CAMERA_PERSPECTIVE_HPP
#define UFO_VISION_CAMERA_PERSPECTIVE_HPP
 
// UFO
#include <ufo/execution/algorithm.hpp>
#include <ufo/execution/execution.hpp>
#include <ufo/geometry/ray.hpp>
#include <ufo/numeric/mat.hpp>
#include <ufo/numeric/transform.hpp>
#include <ufo/numeric/vec.hpp>
#include <ufo/vision/camera/perspective_intrinsics.hpp>
#include <ufo/vision/image.hpp>
 
// STL
#include <cmath>
#include <cstddef>
#include <format>
#include <numbers>
#include <ostream>
#include <random>
#include <utility>
 
namespace ufo
{
struct PerspectiveCamera {
    Transform3f pose;
    PerspectiveIntrinsics intrinsics;
    float near_clip;
    float far_clip;
 
    void lookAt(Vec3f center, Vec3f const& target, Vec3f const& up)
    {
        pose = Transform3f{inverse(ufo::lookAt(center, target, up))};
    }
 
    [[nodiscard]] Mat4f projection() const
    {
        Mat4f m = perspective(fx(), fy(), intrinsics.cx, intrinsics.cy,
                              static_cast<float>(intrinsics.cols),
                              static_cast<float>(intrinsics.rows), near_clip, far_clip);
        m[0][0] *= intrinsics.zoom;
        m[1][1] *= intrinsics.zoom;
        return m;
    }
 
    [[nodiscard]] Mat4f view() const { return Mat4f(pose); }
 
    [[nodiscard]] Image<Ray3f> rays() const { return rays(ufo::execution::seq); }
 
    template <class ExecutionPolicy>
    [[nodiscard]] Image<Ray3f> rays(ExecutionPolicy&& policy) const
    {
        auto const   view_inv = view();
        Image<Ray3f> rays(intrinsics.rows, intrinsics.cols);
 
        ufo::for_each(
            std::forward<ExecutionPolicy>(policy), std::size_t{}, intrinsics.rows,
            [this, &rays, &view_inv, fx = fx(), fy = fy()](std::size_t y) {
                if (0.0f < intrinsics.lens_radius && 0.0f < intrinsics.focus_distance) {
                    bool const   deterministic = intrinsics.deterministic;
                    std::mt19937 gen_local;
                    // Use a static thread_local generator for performance in non-deterministic
                    // mode
                    static thread_local std::mt19937 gen_tl(std::random_device{}());
 
                    std::mt19937& gen = deterministic ? gen_local : gen_tl;
                    std::uniform_real_distribution<float> dist(-1.0f, 1.0f);
 
                    if (deterministic) {
                        gen.seed(static_cast<unsigned>(y * intrinsics.cols));
                    }
 
                    for (std::size_t x{}; rays.cols() > x; ++x) {
                        float const dx = (x + 0.5f - intrinsics.cx) / fx;
                        float const dy = (y + 0.5f - intrinsics.cy) / fy;
 
                        Vec3f dir_cam = normalize(Vec3f(dx, dy, 1.0f));
 
                        float const r     = intrinsics.lens_radius * std::sqrt(std::abs(dist(gen)));
                        float const theta = 2.0f * std::numbers::pi_v<float> * dist(gen);
                        Vec3f const origin_cam(r * std::cos(theta), r * std::sin(theta), 0.0f);
 
                        float const ft      = intrinsics.focus_distance / dir_cam.z();
                        Vec3f const p_focus = dir_cam * ft;
                        dir_cam             = normalize(p_focus - origin_cam);
 
                        rays[y, x].origin = ufo::convert<Vec3f>(
                            view_inv * Vec4f(origin_cam.x(), origin_cam.y(), origin_cam.z(), 1.0f));
                        rays[y, x].direction = ufo::convert<Vec3f>(
                            view_inv * Vec4f(dir_cam.x(), dir_cam.y(), dir_cam.z(), 0.0f));
                    }
                } else {
                    for (std::size_t x{}; rays.cols() > x; ++x) {
                        float const dx = (x + 0.5f - intrinsics.cx) / fx;
                        float const dy = (y + 0.5f - intrinsics.cy) / fy;
 
                        Vec3f const dir_cam = normalize(Vec3f(dx, dy, 1.0f));
 
                        rays[y, x].origin =
                            ufo::convert<Vec3f>(view_inv * Vec4f(0.0f, 0.0f, 0.0f, 1.0f));
                        rays[y, x].direction = ufo::convert<Vec3f>(
                            view_inv * Vec4f(dir_cam.x(), dir_cam.y(), dir_cam.z(), 0.0f));
                    }
                }
            });
        return rays;
    }
 
    [[nodiscard]] float fx() const noexcept
    {
        return 0.0f < intrinsics.fx
                   ? intrinsics.fx
                   : intrinsics.rows / (2.0f * std::tan(intrinsics.vertical_fov / 2.0f));
    }
 
    [[nodiscard]] float fy() const noexcept
    {
        return 0.0f < intrinsics.fy
                   ? intrinsics.fy
                   : intrinsics.rows / (2.0f * std::tan(intrinsics.vertical_fov / 2.0f));
    }
 
    [[nodiscard]] friend bool operator==(PerspectiveCamera const&,
                                         PerspectiveCamera const&) noexcept = default;
};
 
inline std::ostream& operator<<(std::ostream& os, PerspectiveCamera const& camera)
{
    os << "Pose: " << camera.pose << '\n'
       << camera.intrinsics << "\nNear clip: " << camera.near_clip
       << ", Far clip: " << camera.far_clip << '\n';
    return os;
}
 
}  // namespace ufo
 
template <>
struct std::formatter<ufo::PerspectiveCamera> {
    constexpr auto parse(std::format_parse_context& ctx) const { return ctx.begin(); }
 
    auto format(ufo::PerspectiveCamera const& camera, std::format_context& ctx) const
    {
        return std::format_to(ctx.out(), "Pose: {}\n{}\nNear clip: {}, Far clip: {}",
                              camera.pose, camera.intrinsics, camera.near_clip,
                              camera.far_clip);
    }
};
 
#endif  // UFO_VISION_CAMERA_PERSPECTIVE_HPP