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/Geometry>
#include <se/integrator/allocator/dense_pooling_image.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 Eigen::Isometry3f& /* T_WS */,
                 const timestamp_t /* timestamp */);
};
 
 
 
template<se::Colour ColB, se::Id IdB, int BlockSize, typename SensorT>
class VolumeCarver<Map<Data<se::Field::Occupancy, ColB, IdB>, se::Res::Multi, BlockSize>, SensorT> {
    public:
    typedef Map<Data<se::Field::Occupancy, ColB, IdB>, se::Res::Multi, BlockSize> 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().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;
    };
 
 
    VolumeCarver(MapType& map,
                 const SensorT& sensor,
                 const se::Image<float>& depth_img,
                 const se::Image<float>& depth_sigma_img,
                 const Eigen::Isometry3f& T_WS,
                 const timestamp_t timestamp);
 
    VolumeCarverAllocation operator()();
 
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
 
 
    private:
    bool crossesFrustum(std::vector<srl::projection::ProjectionStatus>& proj_corner_stati);
 
    bool cameraInNode(const Eigen::Vector3i& node_coord,
                      const int node_size,
                      const Eigen::Isometry3f& 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,
                                      const float std_dev_max);
 
    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_;
    // XXX: the sigma pooling image isn't strictly necessary, we only need the sigma corresponding
    // to the maximum depth at each depth pooling image lookup. Find a way to avoid it or combine
    // both pooling images into one. One way to avoid it would be to also store the pixel
    // coordinates corresponding to se::Pixel::min and se::Pixel::max in se::Pixel. Then, getting
    // the sigma corresponding to the maximum depth would be just an image lookup.
    const se::DensePoolingImage<SensorT> sigma_pooling_img_;
    const Eigen::Isometry3f T_SW_;
    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