pinhole.h

Go to the documentation of this file.

 
#pragma once
 
#include <Eigen/Core>
#include <array>
 
#include "calib/models/camera_matrix.h"
#include "calib/models/cameramodel.h"
#include "calib/models/distortion.h"
 
namespace calib {
 
template <distortion_model DistortionT>
class PinholeCamera final {
  public:
    using Scalar = typename DistortionT::Scalar;  
    CameraMatrixT<Scalar> kmtx;                   
    DistortionT distortion;                       
 
    PinholeCamera() = default;
 
    PinholeCamera(const CameraMatrixT<Scalar>& matrix, const DistortionT& distortion_model)
        : kmtx(matrix), distortion(distortion_model) {}
 
    template <typename Derived>
    PinholeCamera(const CameraMatrixT<Scalar>& matrix, const Eigen::MatrixBase<Derived>& coeffs)
        : kmtx(matrix), distortion(coeffs) {}
 
    template <typename T>
    [[nodiscard]]
    auto apply_intrinsics(const Eigen::Matrix<T, 2, 1>& pixel) const -> Eigen::Matrix<T, 2, 1> {
        return normalize<T>(kmtx, pixel);
    }
 
    template <typename T>
    [[nodiscard]]
    auto remove_intrinsics(const Eigen::Matrix<T, 2, 1>& norm_xy) const -> Eigen::Matrix<T, 2, 1> {
        return denormalize<T>(kmtx, norm_xy);
    }
 
    template <typename T>
    [[nodiscard]]
    auto distort(const Eigen::Matrix<T, 2, 1>& norm_xy) const -> Eigen::Matrix<T, 2, 1> {
        return distortion.template distort<T>(norm_xy);
    }
 
    template <typename T>
    [[nodiscard]]
    auto undistort(const Eigen::Matrix<T, 2, 1>& distorted_xy) const -> Eigen::Matrix<T, 2, 1> {
        return distortion.template undistort<T>(distorted_xy);
    }
 
    template <typename T>
    [[nodiscard]]
    auto project(const Eigen::Matrix<T, 2, 1>& norm_xy) const -> Eigen::Matrix<T, 2, 1> {
        return remove_intrinsics(distort(norm_xy));
    }
 
    template <typename T>
    [[nodiscard]]
    auto project(const Eigen::Matrix<T, 3, 1>& xyz) const -> Eigen::Matrix<T, 2, 1> {
        Eigen::Matrix<T, 2, 1> norm_xy = xyz.hnormalized();
        return remove_intrinsics(distort(norm_xy));
    }
 
    template <typename T>
    [[nodiscard]]
    auto unproject(const Eigen::Matrix<T, 2, 1>& pixel) const -> Eigen::Matrix<T, 2, 1> {
        return undistort(apply_intrinsics(pixel));
    }
};
 
// Pinhole camera traits specialisation
template <distortion_model DistortionT>
struct CameraTraits<PinholeCamera<DistortionT>> {
    static constexpr size_t param_count = 10;
    static constexpr int idx_fx = 0;    
    static constexpr int idx_fy = 1;    
    static constexpr int idx_skew = 4;  
 
    static constexpr int k_num_dist_coeffs = 5;
    template <typename T>
    static auto from_array(const T* intr) -> PinholeCamera<BrownConrady<T>> {
        CameraMatrixT<T> k_matrix{intr[0], intr[1], intr[2], intr[3], intr[4]};
        Eigen::Matrix<T, Eigen::Dynamic, 1> dist(k_num_dist_coeffs);
        constexpr int k_intr_offset = 5;
        dist << intr[k_intr_offset], intr[k_intr_offset + 1], intr[k_intr_offset + 2],
            intr[k_intr_offset + 3], intr[k_intr_offset + 4];
        return PinholeCamera<BrownConrady<T>>(k_matrix, dist);
    }
 
    static void to_array(const PinholeCamera<DistortionT>& cam,
                         std::array<double, param_count>& arr) {
        arr[0] = cam.kmtx.fx;
        arr[1] = cam.kmtx.fy;
        arr[2] = cam.kmtx.cx;
        arr[3] = cam.kmtx.cy;
        arr[4] = cam.kmtx.skew;
        constexpr int k_intr_offset = 5;
        for (int i = 0; i < k_num_dist_coeffs; ++i) {
            arr[k_intr_offset + i] = cam.distortion.coeffs[i];
        }
    }
 
    template <typename T>
    static Eigen::Matrix<T, 2, 1> apply_linear_intrinsics(const PinholeCamera<DistortionT>& cam,
                                                          const Eigen::Matrix<T, 2, 1>& mx_my) {
        CameraMatrixT<T> kmtx{T(cam.kmtx.fx), T(cam.kmtx.fy), T(0), T(0), T(cam.kmtx.skew)};
        return denormalize<T>(kmtx, mx_my);
    }
 
    template <typename T>
    static Eigen::Matrix<T, 2, 1> remove_linear_intrinsics(const PinholeCamera<DistortionT>& cam,
                                                           const Eigen::Matrix<T, 2, 1>& px_py) {
        CameraMatrixT<T> kmtx{T(cam.kmtx.fx), T(cam.kmtx.fy), T(0), T(0), T(cam.kmtx.skew)};
        return normalize<T>(kmtx, px_py);
    }
};
 
// Backwards compatibility alias
template <distortion_model DistortionT>
using Camera = PinholeCamera<DistortionT>;
 
template <distortion_model DistortionT>
inline void to_json(nlohmann::json& j, const PinholeCamera<DistortionT>& cam) {
    j = {{"kmtx", cam.kmtx}, {"distortion", cam.distortion}};
}
 
template <distortion_model DistortionT>
inline void from_json(const nlohmann::json& j, PinholeCamera<DistortionT>& cam) {
    j.at("kmtx").get_to(cam.kmtx);
    if (j.contains("distortion")) j.at("distortion").get_to(cam.distortion);
}
 
static_assert(camera_model<PinholeCamera<BrownConrady<double>>>);
 
}  // namespace calib