sensor.hpp

Go to the documentation of this file.

/*
 * SPDX-FileCopyrightText: 2020-2024 Smart Robotics Lab, Imperial College London, Technical University of Munich
 * SPDX-FileCopyrightText: 2020-2022 Nils Funk
 * SPDX-FileCopyrightText: 2020-2022 Sotiris Papatheodorou
 * SPDX-FileCopyrightText: 2022-2024 Simon Boche
 * 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/str_utils.hpp>
#include <se/common/yaml.hpp>
#include <se/image/image.hpp>
 
namespace se {
 
template<typename DerivedT>
class SensorBase {
    public:
    struct Config {
        int width = 0;
 
        int height = 0;
 
        float near_plane = 0.01f;
 
        float far_plane = 10.0f;
 
        Eigen::Isometry3f T_BS = Eigen::Isometry3f::Identity();
 
        std::vector<float> pixel_voxel_ratio_per_scale = {1.5f, 3.0f, 6.0f};
 
        void readYaml(const std::string& filename);
 
        Config operator/(const float downsampling_factor) const
        {
            return Config{static_cast<int>(width / downsampling_factor),
                          static_cast<int>(height / downsampling_factor),
                          near_plane,
                          far_plane,
                          T_BS,
                          pixel_voxel_ratio_per_scale};
        }
 
        // The definition of this function MUST be inside the definition of Config for template
        // argument deduction to work.
        friend std::ostream& operator<<(std::ostream& os, const Config& c)
        {
            os << str_utils::value_to_pretty_str(c.width, "width") << " px\n";
            os << str_utils::value_to_pretty_str(c.height, "height") << " px\n";
            os << str_utils::value_to_pretty_str(c.near_plane, "near_plane") << " m\n";
            os << str_utils::value_to_pretty_str(c.far_plane, "far_plane") << " m\n";
            os << str_utils::eigen_matrix_to_pretty_str(c.T_BS.matrix(), "T_BS") << "\n";
            return os;
        }
 
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };
 
    template<typename ConfigT>
    SensorBase(const ConfigT& c);
 
    SensorBase(const DerivedT& d);
 
    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::Isometry3f T_BS;
    std::vector<float> pixel_voxel_ratio_per_scale;
 
    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
    const DerivedT* underlying() const;
};
 
} // namespace se
 
#include "impl/sensor_impl.hpp"
#include "leica_lidar.hpp"
#include "ouster_lidar.hpp"
#include "pinhole_camera.hpp"
 
#endif // SE_SENSOR_HPP