ray_integrator.hpp

Go to the documentation of this file.

/*
 * SPDX-FileCopyrightText: 2020-2022 Smart Robotics Lab, Imperial College London, Technical University of Munich
 * SPDX-FileCopyrightText: 2022-2024 Simon Boche
 * SPDX-License-Identifier: BSD-3-Clause
 */
 
#ifndef SE_RAY_INTEGRATOR_HPP
#define SE_RAY_INTEGRATOR_HPP
 
#include <se/integrator/ray_integrator_core.hpp>
#include <set>
#include <unordered_set>
 
namespace se {
 
enum class RayState { FreeSpace, Transition, Occupied, Undefined };
 
template<typename MapT, typename SensorT>
class RayIntegrator {
    public:
    RayIntegrator(MapT& /* map */,
                  const SensorT& /* sensor */,
                  const Eigen::Vector3f& /*ray*/,
                  const Eigen::Isometry3f& /* T_SW need Lidar frame?*/,
                  const timestamp_t /* timestamp */,
                  std::set<const OctantBase*>* const /*updated_octants = nullptr*/){};
 
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
 
template<se::Colour ColB, se::Semantics SemB, int BlockSize, typename SensorT>
class RayIntegrator<Map<Data<se::Field::Occupancy, ColB, SemB>, se::Res::Multi, BlockSize>,
                    SensorT> {
    public:
    typedef Map<Data<se::Field::Occupancy, ColB, SemB>, se::Res::Multi, BlockSize> MapType;
    typedef typename MapType::DataType DataType;
    typedef typename MapType::OctreeType OctreeType;
    typedef typename MapType::OctreeType::NodeType NodeType;
    typedef typename MapType::OctreeType::BlockType BlockType;
 
    struct RayIntegratorConfig {
        RayIntegratorConfig(const MapType& map) :
                sigma_min(map.getDataConfig().field.sigma_min_factor * map.getRes()),
                sigma_max(map.getDataConfig().field.sigma_max_factor * map.getRes()),
                tau_min(map.getDataConfig().field.tau_min_factor * map.getRes()),
                tau_max(map.getDataConfig().field.tau_max_factor * map.getRes())
        {
        }
 
        const float sigma_min;
        const float sigma_max;
        const float tau_min;
        const float tau_max;
    };
 
 
    RayIntegrator(MapType& map,
                  const SensorT& sensor,
                  const Eigen::Vector3f& ray,
                  const Eigen::Isometry3f& T_WS,
                  const timestamp_t timestamp,
                  std::set<const OctantBase*>* const updated_octants = nullptr);
 
    bool resetIntegrator(const Eigen::Vector3f& ray,
                         const Eigen::Isometry3f& T_WS,
                         const timestamp_t timestamp,
                         bool skip_check = false);
 
    void operator()();
 
    void propagateToRoot();
 
    void propagateBlocksToCoarsestScale();
 
    se::RayState computeVariance(const float ray_step_depth);
 
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    private:
    template<class SensorTDummy = SensorT>
    typename std::enable_if_t<std::is_same<SensorTDummy, se::LeicaLidar>::value, void>
    operator()(const Eigen::Vector3f& ray_sample,
               const Eigen::Vector3i& voxel_coord,
               se::RayState rayState,
               se::OctantBase* octant_ptr);
 
 
    void updateBlock(se::OctantBase* octant_ptr,
                     Eigen::Vector3i& voxel_coords,
                     int desired_scale,
                     float sample_dist);
 
    MapType& map_;
    OctreeType& octree_;
    const SensorT& sensor_;
 
    std::vector<std::set<se::OctantBase*>> node_set_;
    std::vector<se::OctantBase*> updated_blocks_vector_;
    std::set<se::OctantBase*> updated_blocks_set_; // ToDo rename: updated_octants_;?
    std::set<const se::OctantBase*>* updated_octants_ = nullptr;
    RayIntegratorConfig config_;
 
    Eigen::Isometry3f T_SW_;
    Eigen::Vector3f ray_;
    Eigen::Vector3i last_visited_voxel_;
 
    const float map_res_;
 
    int free_space_scale_ = 0;
    int computed_integration_scale_ = 0;
    timestamp_t timestamp_;
 
    // Sensor Model - Tau and Sigma
    float ray_dist_ = 0.;
    float tau_ = 0.;
    float three_sigma_ = 0.;
};
 
} // namespace se
#include "impl/ray_integrator_impl.hpp"
 
#endif // SE_RAY_INTEGRATOR_HPP