class mrpt::maps::CSimpleMap

A view-based map: a set of poses and what the robot saw from those poses.

A simplemap comprises a sequence of tuples, each containing:

  • The keyframe SE(3) pose of the robot, including (optionally) its uncertainty, as instances of mrpt::poses::CPose3DPDF

  • The raw observations from that keyframe, in a mrpt::obs::CSensoryFrame

  • Optionally, the twist (linear and angular velocity) of the robot in the local frame of reference, at that moment. It can be used to undistort data from a rotatory lidar, for example.

To generate an actual metric map (occupancy grid, point cloud, octomap, etc.) from a “simple map”, the user must instantiate the desired metric map class(es) and invoke its virtual method mrpt::maps::CMetricMap::loadFromSimpleMap().

Users can also use the new top-level library mp2p_icp_filters and its CLI application sm2mm (simple-map to metric-map) to generate metric maps including pre-processing of raw data in a flexible way.

To programatically change an existing simplemap, use the non-const get() method and modify the returned reference.

Copy constructor and copy operator makes shallow copies of all data. A makeDeepCopy() method is also provided which duplicates all internal data, if really needed.

Objects of this class are serialized into GZ-compressed files with the extension .simplemap. See Robotics file formats.

See also:

mrpt::obs::CSensoryFrame, mrpt::poses::CPose3DPDF, mrpt::maps::CMetricMap, https://github.com/MOLAorg/mp2p_icp/

#include <mrpt/maps/CSimpleMap.h>

class CSimpleMap: public mrpt::serialization::CSerializable
{
public:
    // typedefs

    typedef std::deque<Keyframe> KeyframeList;
    typedef KeyframeList::const_iterator const_iterator;
    typedef KeyframeList::iterator iterator;
    typedef KeyframeList::reverse_iterator reverse_iterator;
    typedef KeyframeList::const_reverse_iterator const_reverse_iterator;

    // structs

    struct Keyframe;

    // construction

    CSimpleMap();

    //
methods

    const_iterator begin() const;
    const_iterator end() const;
    const_iterator cbegin() const;
    const_iterator cend() const;
    iterator begin();
    iterator end();
    const_reverse_iterator rbegin() const;
    const_reverse_iterator rend() const;
    const_reverse_iterator crbegin() const;
    const_reverse_iterator crend() const;
    reverse_iterator rbegin();
    reverse_iterator rend();
    bool saveToFile(const std::string& filName) const;
    bool loadFromFile(const std::string& filName);
    size_t size() const;
    bool empty() const;
    const Keyframe& get(size_t index) const;
    Keyframe& get(size_t index);
    void remove(size_t index);
    void insert(const Keyframe& kf);

    void insert(
        const mrpt::poses::CPose3DPDF::Ptr& in_posePDF,
        const mrpt::obs::CSensoryFrame::Ptr& in_SF,
        const std::optional<mrpt::math::TTwist3D>& twist = std::nullopt
        );

    void clear();
    void changeCoordinatesOrigin(const mrpt::poses::CPose3D& newOrigin);
    CSimpleMap makeDeepCopy();
};

Construction

CSimpleMap()

Default ctor: empty map.

Methods

bool saveToFile(const std::string& filName) const

Save this object to a .simplemap binary file (compressed with gzip) See Robotics file formats.

Returns:

false on any error.

See also:

loadFromFile()

bool loadFromFile(const std::string& filName)

Load the contents of this object from a .simplemap binary file (possibly compressed with gzip) See Robotics file formats.

Returns:

false on any error.

See also:

saveToFile()

size_t size() const

Returns the number of keyframes in the map.

bool empty() const

Returns size()!=0.

const Keyframe& get(size_t index) const

const accessor

Keyframe& get(size_t index)

non-const accessor, returning a reference suitable for modification

void remove(size_t index)

Deletes the 0-based index i’th keyframe.

Parameters:

std::exception

On index out of bounds.

See also:

insert, get, set

void insert(const Keyframe& kf)

Adds a new keyframe (SE(3) pose) to the view-based map.

Both shared pointers are copied (shallow object copies).

void insert(
    const mrpt::poses::CPose3DPDF::Ptr& in_posePDF,
    const mrpt::obs::CSensoryFrame::Ptr& in_SF,
    const std::optional<mrpt::math::TTwist3D>& twist = std::nullopt
    )

Adds a new keyframe (SE(3) pose) to the view-based map.

Both shared pointers are copied (shallow object copies).

void clear()

Remove all stored keyframes.

See also:

remove

void changeCoordinatesOrigin(const mrpt::poses::CPose3D& newOrigin)

Change the coordinate origin of all stored poses, that is, translates and rotates the map such that the old SE(3) origin (identity transformation) becomes the new provided one.

CSimpleMap makeDeepCopy()

makes a deep copy of all data