class mrpt::slam::CRangeBearingKFSLAM

Overview

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

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

The state vector comprises: 3D robot position, a quaternion for its attitude, and the 3D landmarks in the map.

The front-end application kf-slam is based on this class.

For the theory behind this implementation, see the technical report: [3]

See also:

An implementation for 2D and SE(2) is in CRangeBearingKFSLAM2D

#include <mrpt/slam/CRangeBearingKFSLAM.h>

class CRangeBearingKFSLAM: public mrpt::bayes::CKalmanFilterCapable
{
public:
    // typedefs

    typedef mrpt::math::TPoint3D landmark_point_t;

    // structs

    struct TDataAssocInfo;
    struct TOptions;

    // fields

    mrpt::slam::CRangeBearingKFSLAM::TOptions options;

    // construction

    CRangeBearingKFSLAM();

    // methods

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

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

    void getCurrentState(
        mrpt::poses::CPose3DPDFGaussian& out_robotPose,
        std::vector<mrpt::math::TPoint3D>& 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::CPose3DQuatPDFGaussian& out_robotPose) const;
    mrpt::poses::CPose3DQuat getCurrentRobotPoseMean() const;
    void getCurrentRobotPose(mrpt::poses::CPose3DPDFGaussian& out_robotPose) const;
    void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr& outObj) const;
    void loadOptions(const mrpt::config::CConfigFileBase& ini);
    const TDataAssocInfo& getLastDataAssociation() const;
    void getLastPartition(std::vector<std::vector<uint32_t>>& parts);
    void getLastPartitionLandmarks(std::vector<std::vector<uint32_t>>& landmarksMembership) const;
    void getLastPartitionLandmarksAsIfFixedSubmaps(size_t K, std::vector<std::vector<uint32_t>>& landmarksMembership);
    double computeOffDiagonalBlocksApproximationError(const std::vector<std::vector<uint32_t>>& landmarksMembership) const;
    void reconsiderPartitionsNow();
    CIncrementalMapPartitioner::TOptions* mapPartitionOptions();

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

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(] size_t in_obsIdx, ] 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::TPoint3D landmark_point_t

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

Construction

CRangeBearingKFSLAM()

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::CPose3DQuatPDFGaussian& out_robotPose,
    std::vector<mrpt::math::TPoint3D>& 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 and the 7x7 covariance matrix of the robot 6D pose
out_landmarksPositions	One entry for each of the M landmark positions (3D).
out_landmarkIDs	Each element[index] (for indices of out_landmarksPositions) gives the corresponding landmark ID.
out_fullState	The complete state vector (7+3M).
out_fullCovariance	The full (7+3M)x(7+3M) covariance matrix of the filter.

See also:

getCurrentRobotPose

void getCurrentState(
    mrpt::poses::CPose3DPDFGaussian& out_robotPose,
    std::vector<mrpt::math::TPoint3D>& 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 and the 7x7 covariance matrix of the robot 6D pose
out_landmarksPositions	One entry for each of the M landmark positions (3D).
out_landmarkIDs	Each element[index] (for indices of out_landmarksPositions) gives the corresponding landmark ID.
out_fullState	The complete state vector (7+3M).
out_fullCovariance	The full (7+3M)x(7+3M) covariance matrix of the filter.

See also:

getCurrentRobotPose

void getCurrentRobotPose(mrpt::poses::CPose3DQuatPDFGaussian& out_robotPose) const

Returns the mean & the 7x7 covariance matrix of the robot 6D pose (with rotation as a quaternion).

See also:

getCurrentState, getCurrentRobotPoseMean

mrpt::poses::CPose3DQuat getCurrentRobotPoseMean() const

Get the current robot pose mean, as a 3D+quaternion pose.

See also:

getCurrentRobotPose

void getCurrentRobotPose(mrpt::poses::CPose3DPDFGaussian& out_robotPose) const

Returns the mean & the 6x6 covariance matrix of the robot 6D pose (with rotation as 3 angles).

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.

const TDataAssocInfo& getLastDataAssociation() const

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

void getLastPartition(std::vector<std::vector<uint32_t>>& parts)

Return the last partition of the sequence of sensoryframes (it is NOT a partition of the map!!) Only if options.doPartitioningExperiment = true.

See also:

getLastPartitionLandmarks

void getLastPartitionLandmarks(std::vector<std::vector<uint32_t>>& landmarksMembership) const

Return the partitioning of the landmarks in clusters accoring to the last partition.

Note that the same landmark may appear in different clusters (the partition is not in the space of landmarks) Only if options.doPartitioningExperiment = true

Parameters:

landmarksMembership

The i’th element of this vector is the set of clusters to which the i’th landmark in the map belongs to (landmark index != landmark ID !!).

See also:

getLastPartition

void getLastPartitionLandmarksAsIfFixedSubmaps(
    size_t K,
    std::vector<std::vector<uint32_t>>& landmarksMembership
    )

For testing only: returns the partitioning as “getLastPartitionLandmarks” but as if a fixed-size submaps (size K) were have been used.

double computeOffDiagonalBlocksApproximationError(const std::vector<std::vector<uint32_t>>& landmarksMembership) const

Computes the ratio of the missing information matrix elements which are ignored under a certain partitioning of the landmarks.

See also:

getLastPartitionLandmarks, getLastPartitionLandmarksAsIfFixedSubmaps

void reconsiderPartitionsNow()

The partitioning of the entire map is recomputed again.

Only when options.doPartitioningExperiment = true. This can be used after changing the parameters of the partitioning method. After this method, you can call getLastPartitionLandmarks.

See also:

getLastPartitionLandmarks

CIncrementalMapPartitioner::TOptions* mapPartitionOptions()

Provides access to the parameters of the map partitioning algorithm.

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.