class mrpt::slam::CRangeBearingKFSLAM2D

Overview

An implementation of EKF-based SLAM with range-bearing sensors, odometry, SE(2) robot pose, and 2D landmarks.

The main method is processActionObservation() which processes pairs of actions/observations.

The following front-end applications are based on this class:

See also:

#include <mrpt/slam/CRangeBearingKFSLAM2D.h>

class CRangeBearingKFSLAM2D: public bayes:: CKalmanFilterCapable< 3, 2, 2, 3 >
{
public:
    // typedefs

    typedef mrpt::math::TPoint2D landmark_point_t;

    // structs

    struct TDataAssocInfo;
    struct TOptions;

    // fields

    TOptions options;

    // construction

    CRangeBearingKFSLAM2D();

    // methods

    void reset();
    void processActionObservation(mrpt::obs::CActionCollection::Ptr& action, mrpt::obs::CSensoryFrame::Ptr& SF);

    void getCurrentState(
        mrpt::poses::CPosePDFGaussian& out_robotPose,
        std::vector<mrpt::math::TPoint2D>& out_landmarksPositions,
        std::map<unsigned int, mrpt::maps::CLandmark::TLandmarkID>& out_landmarkIDs,
        mrpt::math::CVectorDouble& out_fullState,
        mrpt::math::CMatrixDouble& out_fullCovariance
        ) const;

    void getCurrentRobotPose(mrpt::poses::CPosePDFGaussian& out_robotPose) const;
    void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr& outObj) const;
    void loadOptions(const mrpt::config::CConfigFileBase& ini);

    void saveMapAndPath2DRepresentationAsMATLABFile(
        const std::string& fil,
        float stdCount = 3.0f,
        const std::string& styleLandmarks = std::string("b"),
        const std::string& stylePath = std::string("r"),
        const std::string& styleRobot = std::string("r")
        ) const;

    const TDataAssocInfo& getLastDataAssociation() const;
};

Typedefs

typedef mrpt::math::TPoint2D landmark_point_t

Either mrpt::math::TPoint2D or mrpt::math::TPoint3D.

Fields

TOptions options

The options for the algorithm.

Construction

CRangeBearingKFSLAM2D()

Default constructor.

Methods

void reset()

Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,0,0).

void processActionObservation(mrpt::obs::CActionCollection::Ptr& action, mrpt::obs::CSensoryFrame::Ptr& SF)

Process one new action and observations to update the map and robot pose estimate.

See the description of the class at the top of this page.

Parameters:

action	May contain odometry
SF	The set of observations, must contain at least one CObservationBearingRange

void getCurrentState(
    mrpt::poses::CPosePDFGaussian& out_robotPose,
    std::vector<mrpt::math::TPoint2D>& out_landmarksPositions,
    std::map<unsigned int, mrpt::maps::CLandmark::TLandmarkID>& out_landmarkIDs,
    mrpt::math::CVectorDouble& out_fullState,
    mrpt::math::CMatrixDouble& out_fullCovariance
    ) const

Returns the complete mean and cov.

Parameters:

out_robotPose	The mean & 3x3 covariance matrix of the robot 2D pose
out_landmarksPositions	One entry for each of the M landmark positions (2D).
out_landmarkIDs	Each element[index] (for indices of out_landmarksPositions) gives the corresponding landmark ID.
out_fullState	The complete state vector (3+2M).
out_fullCovariance	The full (3+2M)x(3+2M) covariance matrix of the filter.

See also:

getCurrentState

void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr& outObj) const

Returns a 3D representation of the landmarks in the map and the robot 3D position according to the current filter state.

Parameters:

out_objects

void loadOptions(const mrpt::config::CConfigFileBase& ini)

Load options from a ini-like file/text.

void saveMapAndPath2DRepresentationAsMATLABFile(
    const std::string& fil,
    float stdCount = 3.0f,
    const std::string& styleLandmarks = std::string("b"),
    const std::string& stylePath = std::string("r"),
    const std::string& styleRobot = std::string("r")
    ) const

Save the current state of the filter (robot pose & map) to a MATLAB script which displays all the elements in 2D.

const TDataAssocInfo& getLastDataAssociation() const

Returns a read-only reference to the information on the last data-association.