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 <Eigen/StdVector>
#include <optional>

#include "se/common/math_util.hpp"
#include "se/common/str_utils.hpp"
#include "se/map/algorithms/marching_cube.hpp"
#include "se/map/algorithms/structure_meshing.hpp"
#include "se/map/data.hpp"
#include "se/map/io/mesh_io.hpp"
#include "se/map/io/octree_io.hpp"
#include "se/map/octree/visitor.hpp"
#include "se/map/raycaster.hpp"
#include "se/map/utils/octant_util.hpp"



namespace se {

struct MapConfig {
    Eigen::Vector3f dim;

    float res;

    Eigen::Matrix4f T_MW;

    MapConfig();

    MapConfig(const std::string& yaml_file);

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

std::ostream& operator<<(std::ostream& os, const MapConfig& c);



// Forward Declaration
template<typename DataT = se::Data<Field::TSDF, Colour::Off, Semantics::Off>,
         Res ResT = Res::Single,
         int BlockSize = 8>
class Map;



template<Field FldT, Colour ColB, Semantics SemB, Res ResT, int BlockSize>
class Map<se::Data<FldT, ColB, SemB>, ResT, BlockSize> {
    public:
    typedef Data<FldT, ColB, SemB> DataType;
    typedef DataConfig<FldT, ColB, SemB> DataConfigType;
    typedef se::Octree<DataType, ResT, BlockSize> OctreeType;

    Map(const Eigen::Vector3f& dim,
        const float res,
        const se::DataConfig<FldT, ColB, SemB> data_config = se::DataConfig<FldT, ColB, SemB>());

    Map(const MapConfig& map_config,
        const se::DataConfig<FldT, ColB, SemB> data_config = se::DataConfig<FldT, ColB, SemB>());

    inline bool contains(const Eigen::Vector3f& point_W) const;

    inline Eigen::Matrix4f getTMW() const
    {
        return T_MW_;
    };

    inline Eigen::Matrix4f getTWM() const
    {
        return T_WM_;
    };

    inline Eigen::Vector3f gettMW() const
    {
        return se::math::to_translation(T_MW_);
    }

    inline Eigen::Vector3f gettWM() const
    {
        return se::math::to_translation(T_WM_);
    }

    inline Eigen::Matrix3f getRMW() const
    {
        return se::math::to_rotation(T_MW_);
    }

    inline Eigen::Matrix3f getRWM() const
    {
        return se::math::to_rotation(T_WM_);
    }

    inline Eigen::Vector3f getDim() const
    {
        return dimension_;
    }

    inline float getRes() const
    {
        return resolution_;
    }

    inline DataConfigType getDataConfig() const
    {
        return data_config_;
    }

    template<Safe SafeB = Safe::Off>
    inline const DataType getData(const Eigen::Vector3f& point_W) const;

    template<Safe SafeB = Safe::Off, Res ResTDummy = ResT>
    inline 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_ptr_, voxel_coord, scale_desired);
    }


    template<Safe SafeB = Safe::Off>
    inline std::optional<se::field_t> getFieldInterp(const Eigen::Vector3f& point_W) const;

    template<Safe SafeB = Safe::Off, Res ResTDummy = ResT>
    inline 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>
    inline 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>
    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;

    int saveStructure(const std::string& filename,
                      const Eigen::Matrix4f& T_WM = Eigen::Matrix4f::Identity()) const;

    int saveMesh(const std::string& filename,
                 const Eigen::Matrix4f& T_OW = Eigen::Matrix4f::Identity()) const;

    int saveMeshVoxel(const std::string& filename) const;

    inline void voxelToPoint(const Eigen::Vector3i& voxel_coord, Eigen::Vector3f& point_W) const;

    inline void voxelToPoint(const Eigen::Vector3i& voxel_coord,
                             const int voxel_size,
                             Eigen::Vector3f& point_W) const;

    inline void voxelToCornerPoints(const Eigen::Vector3i& voxel_coord,
                                    Eigen::Matrix<float, 3, 8>& corner_points_W) const;

    inline 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>
    inline 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>
    inline 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>
    inline 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>
    inline 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>
    inline 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>
    inline 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;

    inline std::shared_ptr<OctreeType> getOctree()
    {
        return octree_ptr_;
    };

    inline std::shared_ptr<OctreeType> getOctree() const
    {
        return octree_ptr_;
    };

    void setOctree(std::shared_ptr<OctreeType> octree_ptr)
    {
        //    delete octree_ptr_; // TODO: Delete old octree when setting a new one.
        octree_ptr_ = octree_ptr;
    };

    static constexpr Field fld_ = FldT;
    static constexpr Colour col_ = ColB;
    static constexpr Semantics sem_ = SemB;

    static constexpr Res res_ = ResT;

    protected:
    bool initialiseOctree();

    const Eigen::Vector3f dimension_; 
    const float resolution_;          
    const Eigen::Matrix4f T_MW_;      
    const Eigen::Matrix4f T_WM_;      

    const Eigen::Vector3f lb_M_; 
    const Eigen::Vector3f ub_M_; 

    std::shared_ptr<OctreeType> octree_ptr_ = nullptr;

    DataConfigType data_config_; 

    Eigen::Matrix<float, 3, 8> corner_rel_steps_; 
};

template<se::Field FldT = se::Field::TSDF,
         se::Colour ColB = se::Colour::Off,
         se::Semantics SemB = se::Semantics::Off,
         se::Res ResT = se::Res::Single,
         int BlockSize = 8>
using MapD = Map<Data<FldT, ColB, SemB>, ResT, BlockSize>;


// Occupancy map setups
template<se::Res ResT = se::Res::Single, int BlockSize = 8>
using OccupancyMap = Map<OccupancyData, ResT, BlockSize>;

template<se::Res ResT = se::Res::Single, int BlockSize = 8>
using OccupancyColMap = Map<OccupancyColData, ResT, BlockSize>;

template<se::Res ResT = se::Res::Single, int BlockSize = 8>
using OccupancySemMap = Map<OccupancySemData, ResT, BlockSize>;

template<se::Res ResT = se::Res::Single, int BlockSize = 8>
using OccupancyColSemMap = Map<OccupancyColSemData, 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 TSDFSemMap = Map<TSDFSemData, ResT, BlockSize>;

template<se::Res ResT = se::Res::Single, int BlockSize = 8>
using TSDFColSemMap = Map<TSDFColSemData, ResT, BlockSize>;

} // namespace se

#include "impl/map_impl.hpp"

#endif // SE_MAP_HPP