pinhole_camera.hpp

Go to the documentation of this file.

/*
 * SPDX-FileCopyrightText: 2020-2022 Smart Robotics Lab, Imperial College London, Technical University of Munich
 * SPDX-FileCopyrightText: 2020-2022 Nils Funk
 * SPDX-FileCopyrightText: 2020-2022 Sotiris Papatheodorou
 * SPDX-License-Identifier: BSD-3-Clause
 */
 
#ifndef SE_PINHOLE_CAMERA_HPP
#define SE_PINHOLE_CAMERA_HPP
 
 
 
namespace se {
 
class PinholeCamera : public SensorBase<PinholeCamera> {
    public:
    struct Config : public SensorBase<PinholeCamera>::Config {
        float fx = std::numeric_limits<float>::quiet_NaN();
 
        float fy = std::numeric_limits<float>::quiet_NaN();
 
        float cx = std::numeric_limits<float>::quiet_NaN();
 
        float cy = std::numeric_limits<float>::quiet_NaN();
 
        void readYaml(const std::string& filename);
 
        Config operator/(const float downsampling_factor) const;
 
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };
 
    struct FrustumVertex {
        enum {
            TopLeftNear,
            TopRightNear,
            BottomRightNear,
            BottomLeftNear,
            TopLeftFar,
            TopRightFar,
            BottomRightFar,
            BottomLeftFar,
            Num,
        };
    };
 
    struct FrustumNormal {
        enum {
            Left,
            Right,
            Bottom,
            Top,
            Near,
            Far,
            Num,
        };
    };
 
 
 
    PinholeCamera(const Config& config);
 
    PinholeCamera(const Config& config, const float downsampling_factor);
 
    PinholeCamera(const PinholeCamera& pinhole_camera, const float downsampling_factor);
 
    int computeIntegrationScaleImpl(const Eigen::Vector3f& block_centre,
                                    const float map_res,
                                    const int last_scale,
                                    const int min_scale,
                                    const int max_block_scale) const;
 
    float nearDistImpl(const Eigen::Vector3f& ray_S) const;
 
    float farDistImpl(const Eigen::Vector3f& ray_S) const;
 
    float measurementFromPointImpl(const Eigen::Vector3f& point_S) const;
 
    bool pointInFrustumImpl(const Eigen::Vector3f& point_S) const;
 
    bool pointInFrustumInfImpl(const Eigen::Vector3f& point_S) const;
 
    bool sphereInFrustumImpl(const Eigen::Vector3f& centre_S, const float radius) const;
 
    bool sphereInFrustumInfImpl(const Eigen::Vector3f& centre_S, const float radius) const;
 
    static std::string typeImpl();
 
    static PinholeCamera testInstance();
 
    srl::projection::PinholeCamera<srl::projection::NoDistortion> model;
    float scaled_pixel;
 
    float horizontal_fov;
 
    float vertical_fov;
 
    Eigen::Matrix<float, 3, FrustumVertex::Num> frustum_vertices_S;
 
    Eigen::Matrix<float, 4, FrustumNormal::Num> frustum_normals_S;
 
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    private:
    void computeFrustumVertices();
    void computeFrustumNormals();
};
 
std::ostream& operator<<(std::ostream& os, const PinholeCamera::Config& c);
 
} // namespace se
 
#include "impl/pinhole_camera_impl.hpp"
 
#endif // SE_PINHOLE_CAMERA_HPP