class mrpt::slam::CRangeBearingKFSLAM2D

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 mrpt::bayes::CKalmanFilterCapable
{
public:
    // typedefs

    typedef mrpt::math::TPoint2D landmark_point_t;
    typedef mrpt::math::CVectorDynamic<double> KFVector;
    typedef mrpt::math::CMatrixDynamic<double> KFMatrix;
    typedef mrpt::math::CMatrixFixed<double, VEH_SIZE, VEH_SIZE> KFMatrix_VxV;
    typedef mrpt::math::CMatrixFixed<double, OBS_SIZE, OBS_SIZE> KFMatrix_OxO;
    typedef mrpt::math::CMatrixFixed<double, FEAT_SIZE, FEAT_SIZE> KFMatrix_FxF;
    typedef mrpt::math::CMatrixFixed<double, ACT_SIZE, ACT_SIZE> KFMatrix_AxA;
    typedef mrpt::math::CMatrixFixed<double, VEH_SIZE, OBS_SIZE> KFMatrix_VxO;
    typedef mrpt::math::CMatrixFixed<double, VEH_SIZE, FEAT_SIZE> KFMatrix_VxF;
    typedef mrpt::math::CMatrixFixed<double, FEAT_SIZE, VEH_SIZE> KFMatrix_FxV;
    typedef mrpt::math::CMatrixFixed<double, FEAT_SIZE, OBS_SIZE> KFMatrix_FxO;
    typedef mrpt::math::CMatrixFixed<double, OBS_SIZE, FEAT_SIZE> KFMatrix_OxF;
    typedef mrpt::math::CMatrixFixed<double, OBS_SIZE, VEH_SIZE> KFMatrix_OxV;
    typedef mrpt::math::CVectorFixed<double, VEH_SIZE> KFArray_VEH;
    typedef mrpt::math::CVectorFixed<double, ACT_SIZE> KFArray_ACT;
    typedef mrpt::math::CVectorFixed<double, OBS_SIZE> KFArray_OBS;
    typedef std::vector<KFArray_OBS> vector_KFArray_OBS;
    typedef mrpt::math::CVectorFixed<double, FEAT_SIZE> KFArray_FEAT;

    // 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;
    size_t getNumberOfLandmarksInTheMap() const;
    bool isMapEmpty() const;
    size_t getStateVectorLength() const;
    KFVector& internal_getXkk();
    KFMatrix& internal_getPkk();
    mrpt::system::CTimeLogger& getProfiler();
    static constexpr size_t get_vehicle_size();
    static constexpr size_t get_observation_size();
    static constexpr size_t get_feature_size();
    static constexpr size_t get_action_size();
};

Inherited Members

public:
    // typedefs

    typedef KFTYPE kftype;
    typedef CKalmanFilterCapable<VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE> KFCLASS;

    // structs

    struct TMsg;

    //
fields

    TKF_options KF_options;

    //
methods

    virtual void OnInverseObservationModel(] const KFArray_OBS& in_z, ] KFArray_FEAT& out_yn, ] KFMatrix_FxV& out_dyn_dxv, ] KFMatrix_FxO& out_dyn_dhn) const;

    virtual void OnInverseObservationModel(
        const KFArray_OBS& in_z,
        KFArray_FEAT& out_yn,
        KFMatrix_FxV& out_dyn_dxv,
        KFMatrix_FxO& out_dyn_dhn,
        ] KFMatrix_FxF& out_dyn_dhn_R_dyn_dhnT,
        bool& out_use_dyn_dhn_jacobian
        ) const;

    virtual void OnNewLandmarkAddedToMap(] const size_t in_obsIdx, ] const size_t in_idxNewFeat);
    virtual void OnNormalizeStateVector();
    virtual void OnPostIteration();
    void getLandmarkMean(size_t idx, KFArray_FEAT& feat) const;
    void getLandmarkCov(size_t idx, KFMatrix_FxF& feat_cov) 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.