scheimpflug.h

Go to the documentation of this file.

#pragma once
 
// std
#include <cmath>
 
// eigen
#include <ceres/ceres.h>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <array>
 
#include "calib/models/camera_matrix.h"
#include "calib/models/cameramodel.h"
 
namespace calib {
 
struct ScheimpflugAngles final {
    double tau_x{0};  
    double tau_y{0};  
};
 
template <camera_model CameraT>
struct ScheimpflugCamera final {
    using Scalar = typename CameraT::Scalar;
    CameraT camera;   
    Scalar tau_x{0};  
    Scalar tau_y{0};  
 
    ScheimpflugCamera() : camera() {}
 
    ScheimpflugCamera(const CameraT& cam, ScheimpflugAngles angles)
        : camera(cam), tau_x(angles.tau_x), tau_y(angles.tau_y) {}
 
    template <camera_model OtherCamT, typename T>
    ScheimpflugCamera(OtherCamT cam, T tau_x_angle, T tau_y_angle)
        : camera(std::move(cam)), tau_x(Scalar(tau_x_angle)), tau_y(Scalar(tau_y_angle)) {}
 
    ScheimpflugCamera(const ScheimpflugCamera& other)
        : camera(other.camera), tau_x(other.tau_x), tau_y(other.tau_y) {}
 
    ScheimpflugCamera& operator=(const ScheimpflugCamera& other) {
        if (this != &other) {
            camera = other.camera;
            tau_x = other.tau_x;
            tau_y = other.tau_y;
        }
        return *this;
    }
 
    ScheimpflugCamera(ScheimpflugCamera&& other) noexcept
        : camera(std::move(other.camera)), tau_x(other.tau_x), tau_y(other.tau_y) {}
 
    ScheimpflugCamera& operator=(ScheimpflugCamera&& other) noexcept {
        if (this != &other) {
            camera = std::move(other.camera);
            tau_x = other.tau_x;
            tau_y = other.tau_y;
        }
        return *this;
    }
 
    template <typename T>
    [[nodiscard]]
    Eigen::Matrix<T, 2, 1> apply_intrinsics(const Eigen::Matrix<T, 2, 1>& plane_point) const {
        return camera.template apply_intrinsics<T>(plane_point);
    }
 
    template <typename T>
    [[nodiscard]]
    Eigen::Matrix<T, 2, 1> remove_intrinsics(const Eigen::Matrix<T, 2, 1>& pixel) const {
        return camera.template remove_intrinsics<T>(pixel);
    }
 
    template <typename T>
    [[nodiscard]]
    auto project(const Eigen::Matrix<T, 3, 1>& x_camera) const -> Eigen::Matrix<T, 2, 1> {
        // Build rotation that aligns the tilted sensor basis
        const T tau_x_val = T(tau_x);
        const T tau_y_val = T(tau_y);
        const T cos_tau_x = ceres::cos(tau_x_val);
        const T sin_tau_x = ceres::sin(tau_x_val);
        const T cos_tau_y = ceres::cos(tau_y_val);
        const T sin_tau_y = ceres::sin(tau_y_val);
 
        Eigen::Matrix<T, 3, 3> rot_sensor;
        rot_sensor << cos_tau_y, sin_tau_x * sin_tau_y, cos_tau_x * sin_tau_y, T(0), cos_tau_x,
            -sin_tau_x, -sin_tau_y, sin_tau_x * cos_tau_y, cos_tau_x * cos_tau_y;
 
        // Basis vectors of the tilted sensor plane
        Eigen::Matrix<T, 3, 1> axis_sensor = rot_sensor.col(0);
        Eigen::Matrix<T, 3, 1> base_sensor = rot_sensor.col(1);
        Eigen::Matrix<T, 3, 1> normal_sensor = rot_sensor.col(2);
 
        // Ray-plane intersection (d=1)
        T sden = normal_sensor.dot(x_camera);
        T mx = axis_sensor.dot(x_camera) / sden;
        T my = base_sensor.dot(x_camera) / sden;
 
        // Principal ray intersection with the plane
        const T s0 = normal_sensor.z();
        const T mx0 = axis_sensor.z() / s0;
        const T my0 = base_sensor.z() / s0;
 
        // Distort only local delta in plane coords (about principal intersection)
        Eigen::Matrix<T, 2, 1> dxy(mx - mx0, my - my0);
 
        // Distort+intrinsics of the local delta via the base model:
        Eigen::Matrix<T, 2, 1> px_delta =
            camera.template project<T>(Eigen::Matrix<T, 3, 1>(dxy.x(), dxy.y(), T(1)));
 
        // Linear shift from principal-ray intersection (no cx,cy inside):
        Eigen::Matrix<T, 2, 1> base_shift =
            CameraTraits<CameraT>::template apply_linear_intrinsics<T>(camera, {mx0, my0});
 
        return px_delta + base_shift;
    }
 
    template <typename T>
    [[nodiscard]]
    auto unproject(const Eigen::Matrix<T, 2, 1>& pixel) const -> Eigen::Matrix<T, 2, 1> {
        const T tau_x_val = T(tau_x);
        const T tau_y_val = T(tau_y);
        const T cos_tau_x = ceres::cos(tau_x_val);
        const T sin_tau_x = ceres::sin(tau_x_val);
        const T cos_tau_y = ceres::cos(tau_y_val);
        const T sin_tau_y = ceres::sin(tau_y_val);
 
        Eigen::Matrix<T, 3, 3> rot_x;
        rot_x << T(1), T(0), T(0), T(0), cos_tau_x, -sin_tau_x, T(0), sin_tau_x, cos_tau_x;
        Eigen::Matrix<T, 3, 3> rot_y;
        rot_y << cos_tau_y, T(0), sin_tau_y, T(0), T(1), T(0), -sin_tau_y, T(0), cos_tau_y;
        Eigen::Matrix<T, 3, 3> rot_sensor = rot_y * rot_x;
 
        Eigen::Matrix<T, 3, 1> axis_sensor = rot_sensor.col(0);
        Eigen::Matrix<T, 3, 1> base_sensor = rot_sensor.col(1);
        Eigen::Matrix<T, 3, 1> normal_sensor = rot_sensor.col(2);
 
        const T s0 = normal_sensor.z();
        const T mx0 = axis_sensor.z() / s0;
        const T my0 = base_sensor.z() / s0;
 
        // Remove principal-offset *linear* shift via traits
        Eigen::Matrix<T, 2, 1> base_shift =
            CameraTraits<CameraT>::template apply_intrinsics_linear<T>(camera, {mx0, my0});
 
        // Invert base camera mapping for the delta (distortion + intrinsics)
        // Your base unproject returns the *canonical* [dx, dy] on z=1
        Eigen::Matrix<T, 2, 1> dxy = camera.template unproject<T>(pixel - base_shift);
 
        // Return plane coords around the actual plane origin
        return {dxy.x() + mx0, dxy.y() + my0};
    }
};
 
// Traits specialisation for Scheimpflug camera
template <camera_model CameraT>
struct CameraTraits<ScheimpflugCamera<CameraT>> {
    static constexpr size_t param_count = CameraTraits<CameraT>::param_count + 2;
    static constexpr int idx_fx = CameraTraits<CameraT>::idx_fx;
    static constexpr int idx_fy = CameraTraits<CameraT>::idx_fy;
    static constexpr int idx_skew = CameraTraits<CameraT>::idx_skew;
 
    static constexpr int k_tau_x_idx = CameraTraits<CameraT>::param_count;
    static constexpr int k_tau_y_idx = CameraTraits<CameraT>::param_count + 1;
 
    template <typename T>
    static auto from_array(const T* intr)
        -> ScheimpflugCamera<decltype(CameraTraits<CameraT>::from_array(intr))> {
        auto cam = CameraTraits<CameraT>::from_array(intr);
        return ScheimpflugCamera<decltype(cam)>(cam, intr[k_tau_x_idx], intr[k_tau_y_idx]);
    }
 
    static void to_array(const ScheimpflugCamera<CameraT>& cam,
                         std::array<double, param_count>& arr) {
        std::array<double, CameraTraits<CameraT>::param_count> inner{};
        CameraTraits<CameraT>::to_array(cam.camera, inner);
        for (size_t i = 0; i < CameraTraits<CameraT>::param_count; ++i) {
            arr[i] = inner[i];
        }
        arr[k_tau_x_idx] = cam.tau_x;
        arr[k_tau_y_idx] = cam.tau_y;
    }
};
 
}  // namespace calib