map.hpp

Go to the documentation of this file.

/*
 * SPDX-FileCopyrightText: 2016-2019 Emanuele Vespa
 * SPDX-FileCopyrightText: 2019-2023 Smart Robotics Lab, Imperial College London, Technical University of Munich
 * SPDX-FileCopyrightText: 2019-2023 Nils Funk
 * SPDX-FileCopyrightText: 2019-2023 Sotiris Papatheodorou
 * SPDX-License-Identifier: BSD-3-Clause
 */
 
#ifndef SE_MAP_HPP
#define SE_MAP_HPP
 
#include <optional>
#include <se/common/eigen_utils.hpp>
#include <se/common/yaml.hpp>
#include <se/map/io/octree_io.hpp>
#include <se/map/octree/octree.hpp>
#include <se/map/raycaster.hpp>
 
 
 
namespace se {
 
// Forward Declaration
template<typename DataT = se::Data<Field::TSDF, Colour::Off, Id::Off>,
         Res ResT = Res::Single,
         int BlockSize = 8>
class Map;
 
 
 
template<Field FldT, Colour ColB, Id IdB, Res ResT, int BlockSize>
class Map<se::Data<FldT, ColB, IdB>, ResT, BlockSize> {
    public:
    typedef Octree<Data<FldT, ColB, IdB>, ResT, BlockSize> OctreeType;
    typedef typename OctreeType::DataType DataType;
    typedef typename OctreeType::DataConfigType DataConfigType;
    typedef typename OctreeType::BlockType BlockType;
    typedef typename OctreeType::NodeType NodeType;
    typedef typename OctreeType::SurfaceMesh SurfaceMesh;
    typedef typename OctreeType::StructureMesh StructureMesh;
 
    struct Config {
        Eigen::Vector3f dim = Eigen::Vector3f::Constant(10.0f);
 
        float res = 0.1f;
 
        Eigen::Isometry3f T_MW = Eigen::Isometry3f(Eigen::Translation3f(dim / 2));
 
        void readYaml(const std::string& yaml_file);
 
        // 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::volume_to_pretty_str(c.dim, "dim") << " m\n";
            os << str_utils::value_to_pretty_str(c.res, "res") << " m/voxel\n";
            os << str_utils::eigen_matrix_to_pretty_str(c.T_MW.matrix(), "T_MW") << "\n";
            return os;
        }
 
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };
 
    Map(const Eigen::Vector3f& dim,
        const float res,
        const typename Data<FldT, ColB, IdB>::Config& data_config =
            typename Data<FldT, ColB, IdB>::Config());
 
    Map(const Config& map_config,
        const typename Data<FldT, ColB, IdB>::Config& data_config =
            typename Data<FldT, ColB, IdB>::Config());
 
    Map(const Map&) = delete;
 
    Map& operator=(const Map&) = delete;
 
    bool contains(const Eigen::Vector3f& point_W) const;
 
    const Eigen::Isometry3f& getTMW() const
    {
        return T_MW_;
    };
 
    const Eigen::Isometry3f& getTWM() const
    {
        return T_WM_;
    };
 
    const Eigen::Vector3f& getDim() const
    {
        return dimension_;
    }
 
    float getRes() const
    {
        return resolution_;
    }
 
    const DataConfigType& getDataConfig() const
    {
        return data_config_;
    }
 
    template<Safe SafeB = Safe::Off>
    DataType getData(const Eigen::Vector3f& point_W) const;
 
    template<Safe SafeB = Safe::Off, Res ResTDummy = ResT>
    typename std::enable_if_t<ResTDummy == Res::Multi, DataType>
    getMinData(const Eigen::Vector3f& point_W, const int scale_desired) const;
 
    template<Safe SafeB = Safe::Off, Res ResTDummy = ResT>
    typename std::enable_if_t<ResTDummy == Res::Multi, DataType>
    getMaxData(const Eigen::Vector3f& point_W, const int scale_desired) const
    {
        Eigen::Vector3i voxel_coord;
 
        if constexpr (SafeB == Safe::Off) // Evaluate at compile time
        {
            pointToVoxel<Safe::Off>(point_W, voxel_coord);
        }
        else {
            if (!pointToVoxel<Safe::On>(point_W, voxel_coord)) {
                return DataType();
            }
        }
 
        return se::visitor::getMaxData(octree_, voxel_coord, scale_desired);
    }
 
    template<typename ValidF, typename GetF, Safe SafeB = Safe::Off, Res ResTDummy = ResT>
    typename std::enable_if_t<ResTDummy == Res::Multi,
                              std::optional<std::invoke_result_t<GetF, DataType>>>
    getInterp(const Eigen::Vector3f& point_W, ValidF valid, GetF get, int& returned_scale) const;
 
    template<Safe SafeB = Safe::Off, typename ValidF, typename GetF>
    std::optional<std::invoke_result_t<GetF, DataType>>
    getInterp(const Eigen::Vector3f& point_W, ValidF valid, GetF get) const;
 
    template<Safe SafeB = Safe::Off>
    std::optional<se::field_t> getFieldInterp(const Eigen::Vector3f& point_W) const;
 
    template<Safe SafeB = Safe::Off, Res ResTDummy = ResT>
    typename std::enable_if_t<ResTDummy == Res::Multi, std::optional<se::field_t>>
    getFieldInterp(const Eigen::Vector3f& point_W, int& returned_scale) const;
 
    template<Safe SafeB = Safe::Off, Res ResTDummy = ResT, Colour ColourTDummy = ColB>
    typename std::enable_if_t<ResTDummy == Res::Multi && ColourTDummy == Colour::On,
                              std::optional<colour_t>>
    getColourInterp(const Eigen::Vector3f& point_W, int& returned_scale) const;
 
    template<Safe SafeB = Safe::Off, Colour ColourTDummy = ColB>
    typename std::enable_if_t<ColourTDummy == Colour::On, std::optional<colour_t>>
    getColourInterp(const Eigen::Vector3f& point_W) const;
 
    template<Safe SafeB = Safe::Off>
    std::optional<se::field_vec_t> getFieldGrad(const Eigen::Vector3f& point_W) const;
 
    int saveFieldSlices(const std::string& filename_x,
                        const std::string& filename_y,
                        const std::string& filename_z,
                        const Eigen::Vector3f& point_W) const;
 
    template<se::Field FldTDummy = FldT>
    typename std::enable_if_t<FldTDummy == se::Field::Occupancy, int>
    saveMinFieldSlices(const std::string& filename_x,
                       const std::string& filename_y,
                       const std::string& filename_z,
                       const Eigen::Vector3f& point_W,
                       const int scale) const;
 
    template<se::Field FldTDummy = FldT>
    typename std::enable_if_t<FldTDummy == se::Field::Occupancy, int>
    saveMaxFieldSlices(const std::string& filename_x,
                       const std::string& filename_y,
                       const std::string& filename_z,
                       const Eigen::Vector3f& point_W,
                       const int scale) const;
 
    template<Res ResTDummy = ResT>
    typename std::enable_if_t<ResTDummy == Res::Multi, int>
    saveScaleSlices(const std::string& filename_x,
                    const std::string& filename_y,
                    const std::string& filename_z,
                    const Eigen::Vector3f& point_W) const;
 
    SurfaceMesh mesh(const Eigen::Affine3f& T_OW = Eigen::Affine3f::Identity(),
                     const int min_desired_scale = 0) const;
 
    int saveMesh(const std::string& filename,
                 const Eigen::Affine3f& T_OW = Eigen::Affine3f::Identity(),
                 const int min_desired_scale = 0) const;
 
    StructureMesh structure(const Eigen::Affine3f& T_OW = Eigen::Affine3f::Identity(),
                            const bool only_leaves = true) const;
 
    int saveStructure(const std::string& filename,
                      const Eigen::Affine3f& T_OW = Eigen::Affine3f::Identity(),
                      const bool only_leaves = true) const;
 
    void voxelToPoint(const Eigen::Vector3i& voxel_coord, Eigen::Vector3f& point_W) const;
 
    void voxelToPoint(const Eigen::Vector3i& voxel_coord,
                      const int voxel_size,
                      Eigen::Vector3f& point_W) const;
 
    void voxelToCornerPoints(const Eigen::Vector3i& voxel_coord,
                             Eigen::Matrix<float, 3, 8>& corner_points_W) const;
 
    void voxelToCornerPoints(const Eigen::Vector3i& voxel_coord,
                             const int voxel_size,
                             Eigen::Matrix<float, 3, 8>& corner_points_W) const;
 
    template<se::Safe SafeB = se::Safe::On>
    typename std::enable_if_t<SafeB == se::Safe::On, bool>
    pointToVoxel(const Eigen::Vector3f& point_W, Eigen::Vector3i& voxel_coord) const;
 
    template<se::Safe SafeB>
    typename std::enable_if_t<SafeB == se::Safe::Off, bool>
    pointToVoxel(const Eigen::Vector3f& point_W, Eigen::Vector3i& voxel_coord) const;
 
    template<se::Safe SafeB = se::Safe::On>
    typename std::enable_if_t<SafeB == se::Safe::On, bool>
    pointToVoxel(const Eigen::Vector3f& point_W, Eigen::Vector3f& voxel_coord_f) const;
 
    template<se::Safe SafeB>
    typename std::enable_if_t<SafeB == se::Safe::Off, bool>
    pointToVoxel(const Eigen::Vector3f& point_W, Eigen::Vector3f& voxel_coord_f) const;
 
    template<se::Safe SafeB = se::Safe::On>
    typename std::enable_if_t<SafeB == se::Safe::On, bool> pointsToVoxels(
        const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>>& points_W,
        std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i>>& voxel_coords)
        const;
 
    template<se::Safe SafeB>
    typename std::enable_if_t<SafeB == se::Safe::Off, bool> pointsToVoxels(
        const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>>& points_W,
        std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i>>& voxel_coords)
        const;
 
    OctreeType& getOctree()
    {
        return octree_;
    };
 
    const OctreeType& getOctree() const
    {
        return octree_;
    };
 
    const Eigen::AlignedBox3f& aabb() const;
 
    static constexpr Field fld_ = FldT;
    static constexpr Colour col_ = ColB;
    static constexpr Id id_ = IdB;
 
    static constexpr Res res_ = ResT;
 
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    protected:
    OctreeType octree_;
    const float resolution_;          
    const Eigen::Vector3f dimension_; 
    const Eigen::Isometry3f T_MW_;    
    const Eigen::Isometry3f T_WM_;    
 
    const Eigen::Vector3f lb_M_; 
    const Eigen::Vector3f ub_M_; 
 
    const DataConfigType data_config_; 
 
    mutable Eigen::AlignedBox3f cached_aabb_;
    mutable Eigen::AlignedBox3i cached_octree_aabb_;
 
    static const Eigen::Matrix<float, 3, 8> corner_rel_steps_;
};
 
template<se::Field FldT = se::Field::TSDF,
         se::Colour ColB = se::Colour::Off,
         se::Id IdB = se::Id::Off,
         se::Res ResT = se::Res::Single,
         int BlockSize = 8>
using MapD = Map<Data<FldT, ColB, IdB>, ResT, BlockSize>;
 
 
// Occupancy map setups
template<se::Res ResT = se::Res::Multi, int BlockSize = 8>
using OccupancyMap = Map<OccupancyData, ResT, BlockSize>;
 
template<se::Res ResT = se::Res::Multi, int BlockSize = 8>
using OccupancyColMap = Map<OccupancyColData, ResT, BlockSize>;
 
template<se::Res ResT = se::Res::Multi, int BlockSize = 8>
using OccupancyIdMap = Map<OccupancyIdData, ResT, BlockSize>;
 
template<se::Res ResT = se::Res::Multi, int BlockSize = 8>
using OccupancyColIdMap = Map<OccupancyColIdData, ResT, BlockSize>;
 
 
// TSDF map setups
template<se::Res ResT = se::Res::Single, int BlockSize = 8>
using TSDFMap = Map<TSDFData, ResT, BlockSize>;
 
template<se::Res ResT = se::Res::Single, int BlockSize = 8>
using TSDFColMap = Map<TSDFColData, ResT, BlockSize>;
 
template<se::Res ResT = se::Res::Single, int BlockSize = 8>
using TSDFIdMap = Map<TSDFIdData, ResT, BlockSize>;
 
template<se::Res ResT = se::Res::Single, int BlockSize = 8>
using TSDFColIdMap = Map<TSDFColIdData, ResT, BlockSize>;
 
} // namespace se
 
#include "impl/map_impl.hpp"
 
#endif // SE_MAP_HPP