volume_carver.hpp

Go to the documentation of this file.

/*
 * SPDX-FileCopyrightText: 2020-2021 Smart Robotics Lab, Imperial College London, Technical University of Munich
 * SPDX-FileCopyrightText: 2020-2021 Nils Funk
 * SPDX-FileCopyrightText: 2021 Sotiris Papatheodorou
 * SPDX-License-Identifier: BSD-3-Clause
 */

#ifndef SE_VOLUME_CARVER_HPP
#define SE_VOLUME_CARVER_HPP



#include <Eigen/Core>
#include <set>

#include "se/common/image_utils.hpp"
#include "se/common/str_utils.hpp"
#include "se/integrator/updater/multires_ofusion_core.hpp"
#include "se/map/map.hpp"



namespace se {



enum class VarianceState { Constant, Gradient, Undefined };

struct VolumeCarverAllocation {
    std::vector<se::OctantBase*> node_list;
    std::vector<se::OctantBase*> block_list;
    std::vector<se::VarianceState> variance_state_list;
    std::vector<bool> projects_inside_list;
};



template<typename MapT, typename SensorT>
class VolumeCarver {
    public:
    VolumeCarver(MapT& /* map */,
                 const SensorT& /* sensor */,
                 const se::Image<float>& /* depth_img */,
                 const se::DensePoolingImage<SensorT>& /* depth_pooling_img */,
                 const Eigen::Matrix4f& /* T_SW */,
                 const int /* frame */){};
};



template<se::Colour ColB, se::Semantics SemB, int BlockSize, typename SensorT>
class VolumeCarver<Map<Data<se::Field::Occupancy, ColB, SemB>, se::Res::Multi, BlockSize>,
                   SensorT> {
    public:
    typedef Map<Data<se::Field::Occupancy, ColB, SemB>, se::Res::Multi> MapType;
    typedef typename MapType::OctreeType OctreeType;
    typedef typename OctreeType::NodeType NodeType;
    typedef typename OctreeType::BlockType BlockType;

    struct VolumeCarverConfig {
        VolumeCarverConfig(const MapType& map) :
                sigma_min(map.getDataConfig().sigma_min_factor * map.getRes()),
                sigma_max(map.getDataConfig().sigma_max_factor * map.getRes()),
                tau_min(map.getDataConfig().tau_min_factor * map.getRes()),
                tau_max(map.getDataConfig().tau_max_factor * map.getRes())
        {
        }

        const float sigma_min;
        const float sigma_max;
        const float tau_min;
        const float tau_max;
    };


    VolumeCarver(MapType& map,
                 const SensorT& sensor,
                 const se::Image<float>& depth_img,
                 const Eigen::Matrix4f& T_WS,
                 const int frame);

    VolumeCarverAllocation operator()();



    private:
    bool crossesFrustum(std::vector<srl::projection::ProjectionStatus>& proj_corner_stati);

    bool cameraInNode(const Eigen::Vector3i& node_coord,
                      const int node_size,
                      const Eigen::Matrix4f& T_WS);

    se::VarianceState computeVariance(const float depth_value_min,
                                      const float depth_value_max,
                                      const float node_dist_min_m,
                                      const float node_dist_max_m);

    template<class SensorTDummy = SensorT>
    typename std::enable_if_t<std::is_same<SensorTDummy, se::PinholeCamera>::value, void>
    operator()(const Eigen::Vector3i& octant_coord,
               const int octant_size,
               const int octant_depth,
               se::OctantBase* octant_ptr);

    template<class SensorTDummy = SensorT>
    typename std::enable_if_t<std::is_same<SensorTDummy, se::OusterLidar>::value, void>
    operator()(const Eigen::Vector3i& octant_coord,
               const int octant_size,
               const int octant_depth,
               se::OctantBase* octant_ptr);

    MapType& map_;
    OctreeType& octree_;
    const SensorT& sensor_;
    const se::DensePoolingImage<SensorT> depth_pooling_img_;
    const Eigen::Matrix4f T_SW_;
    const int frame_;
    const float map_res_;
    VolumeCarverConfig config_;
    const float max_depth_value_;
    const float zero_depth_band_;
    const float size_to_radius_;
    VolumeCarverAllocation allocation_list_;
};



} // namespace se

#include "impl/volume_carver_impl.hpp"

#endif // SE_VOLUME_CARVER_HPP