class mola::HashedSetSE3

Overview

HashedSetSE3 : a sparse hashed lattice of SE(3) poses

#include <HashedSetSE3.h>

class HashedSetSE3
{
public:
    // typedefs

    typedef std::vector<mrpt::math::TPose3D> pose_vector_t;
    typedef std::unordered_map<global_index3d_t, VoxelData, index_se3_t_hash<int32_t>> grids_map_t;
    typedef index_se3_t<int32_t> global_index3d_t;

    // structs

    struct VoxelData;

    // construction

    HashedSetSE3(
        double voxel_xyz_size = 1.0,
        double voxel_yaw_size = mrpt::DEG2RAD(10.0),
        double voxel_pitch_size = mrpt::DEG2RAD(10.0),
        double voxel_roll_size = mrpt::DEG2RAD(10.0)
        );

    // methods

    global_index3d_t coordToGlobalIdx(const mrpt::math::TPose3D& p) const;
    mrpt::math::TPose3D globalIdxToCoord(const global_index3d_t idx) const;

    void setVoxelProperties(
        double voxel_xyz_size,
        double voxel_yaw_size,
        double voxel_pitch_size,
        double voxel_roll_size
        );

    void clear();
    bool empty() const;
    VoxelData* voxelByGlobalIdxs(const global_index3d_t& idx, bool createIfNew);
    const VoxelData* voxelByGlobalIdxs(const global_index3d_t& idx) const;
    VoxelData* voxelByCoords(const mrpt::math::TPose3D& pt, bool createIfNew);
    const VoxelData* voxelByCoords(const mrpt::math::TPose3D& pt) const;
    void insertPose(const mrpt::math::TPose3D& pt);
    const grids_map_t& voxels() const;
    void visitAllPoses(const std::function<void(const mrpt::math::TPose3D&)>& f) const;
    void visitAllVoxels(const std::function<void(const global_index3d_t&, const VoxelData&)>& f) const;
    bool saveToTextFile(const std::string& file) const;
};

Construction

HashedSetSE3(
    double voxel_xyz_size = 1.0,
    double voxel_yaw_size = mrpt::DEG2RAD(10.0),
    double voxel_pitch_size = mrpt::DEG2RAD(10.0),
    double voxel_roll_size = mrpt::DEG2RAD(10.0)
    )

Constructor / default ctor.

Parameters:

voxel_size

Voxel size [meters]

Methods

mrpt::math::TPose3D globalIdxToCoord(const global_index3d_t idx) const

returns the coordinate of the voxel “bottom” corner

void setVoxelProperties(
    double voxel_xyz_size,
    double voxel_yaw_size,
    double voxel_pitch_size,
    double voxel_roll_size
    )

Reset the main voxel parameters, and clears all current map contents

VoxelData* voxelByGlobalIdxs(const global_index3d_t& idx, bool createIfNew)

returns the voxeldata by global index coordinates, creating it or not if not found depending on createIfNew. Returns nullptr if not found and createIfNew is false

Function defined in the header file so compilers can optimize for literals “createIfNew”

VoxelData* voxelByCoords(const mrpt::math::TPose3D& pt, bool createIfNew)

Get a voxeldata by (x,y,z) coordinates, creating the voxel if needed.

void insertPose(const mrpt::math::TPose3D& pt)

Insert one pose into the lattice

bool saveToTextFile(const std::string& file) const

Save to a text file. Each line contains “X Y Z YAW PITCH ROLL”. Returns false if any error occured, true elsewere.