sensor.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_SENSOR_HPP
#define SE_SENSOR_HPP

#include <limits>

#include "se/common/image_utils.hpp"
#include "se/common/math_util.hpp"
#include "se/common/projection.hpp"
#include "se/common/yaml.hpp"
#include "se/image/image.hpp"



namespace se {

struct SensorBaseConfig {
    int width = 0;

    int height = 0;

    float near_plane = 0.0f;

    float far_plane = std::numeric_limits<float>::infinity();

    Eigen::Matrix4f T_BS = Eigen::Matrix4f::Identity();

    void readYaml(const std::string& filename);

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};



template<typename DerivedT>
class SensorBase {
    public:
    template<typename ConfigT>
    SensorBase(const ConfigT& c);

    SensorBase(const DerivedT& d);

    template<typename ValidPredicate>
    bool projectToPixelValue(const Eigen::Vector3f& point_S,
                             const se::Image<float>& img,
                             float& img_value,
                             ValidPredicate valid_predicate) const;

    template<typename ValidPredicate>
    bool getPixelValue(const Eigen::Vector2f& pixel_f,
                       const se::Image<float>& img,
                       float& img_value,
                       ValidPredicate valid_predicate) const;
    int computeIntegrationScale(const Eigen::Vector3f& block_centre_S,
                                const float map_res,
                                const int last_scale,
                                const int min_scale,
                                const int max_block_scale) const;

    float nearDist(const Eigen::Vector3f& ray_S) const;

    float farDist(const Eigen::Vector3f& ray_S) const;

    float measurementFromPoint(const Eigen::Vector3f& point_S) const;

    bool pointInFrustum(const Eigen::Vector3f& point_S) const;

    bool pointInFrustumInf(const Eigen::Vector3f& point_S) const;

    bool sphereInFrustum(const Eigen::Vector3f& centre_S, const float radius) const;

    bool sphereInFrustumInf(const Eigen::Vector3f& centre_S, const float radius) const;

    static std::string type();

    bool left_hand_frame;
    float near_plane;
    float far_plane;
    Eigen::Matrix4f T_BS;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    private:
    // Make sure the derived class and the template parameter are the same (i.e. prevent class D1 : Base<D2>)
    SensorBase(){};
    friend DerivedT;

    // Simplify access to derived member functions
    DerivedT& underlying();
    const DerivedT& underlying() const;
};



} // namespace se

#include "impl/sensor_impl.hpp"
#include "ouster_lidar.hpp"
#include "pinhole_camera.hpp"

#endif // SE_SENSOR_HPP