multires_ofusion_updater.hpp

Go to the documentation of this file.

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

#ifndef SE_MULTIRES_OFUSION_UPDATER_HPP
#define SE_MULTIRES_OFUSION_UPDATER_HPP



#include "se/sensor/sensor.hpp"



namespace se {



// Multi-res Occupancy updater
template<se::Colour ColB, se::Semantics SemB, int BlockSize, typename SensorT>
class Updater<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 UpdaterConfig {
        UpdaterConfig(const MapType& map) :
                sigma_min(map.getRes() * map.getDataConfig().sigma_min_factor),
                sigma_max(map.getRes() * map.getDataConfig().sigma_max_factor),
                tau_min(map.getRes() * map.getDataConfig().tau_min_factor),
                tau_max(map.getRes() * map.getDataConfig().tau_max_factor)
        {
        }

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

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

    void operator()(se::VolumeCarverAllocation& allocation_list);

    private:
    void propagateToRoot(std::vector<se::OctantBase*>& block_list);

    void freeBlock(se::OctantBase* octant_ptr);

    void updateBlock(se::OctantBase* octant_ptr, bool low_variance, bool project_inside);


    void freeNodeRecurse(se::OctantBase* octant_ptr, int depth);

    private:
    MapType& map_;
    OctreeType& octree_;
    const SensorT& sensor_;
    const se::Image<float>& depth_img_;
    const Eigen::Matrix4f T_SW_;
    const int frame_;
    const float map_res_;
    const UpdaterConfig config_;
    std::vector<std::set<se::OctantBase*>> node_set_;
    std::vector<se::OctantBase*> freed_block_list_;
};



} // namespace se

#include "impl/multires_ofusion_updater_impl.hpp"

#endif // SE_MULTIRES_OFUSION_UPDATER_HPP