Class CRangeBearingKFSLAM

Nested Relationships

Nested Types

Inheritance Relationships

Base Type

  • public bayes::CKalmanFilterCapable< 7, 3, 3, 7 >

Class Documentation

class CRangeBearingKFSLAM : public bayes::CKalmanFilterCapable<7, 3, 3, 7>

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: blanco2008ekf

See also

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

Virtual methods for Kalman Filter implementation

void OnGetAction(KFArray_ACT &out_u) const override

Must return the action vector u.

Parameters:

out_u – The action vector which will be passed to OnTransitionModel

void OnTransitionModel(const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const override

Implements the transition model \( \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) \)

Parameters:

  • in_u – The vector returned by OnGetAction.

  • inout_x – At input has

    \[ \hat{x}_{k-1|k-1} \]
    , at output must have \( \hat{x}_{k|k-1} \) .

  • out_skip – Set this to true if for some reason you want to skip the prediction step (to do not modify either the vector or the covariance). Default:false

void OnTransitionJacobian(KFMatrix_VxV &out_F) const override

Implements the transition Jacobian \( \frac{\partial f}{\partial x} \)

Parameters:

out_F – Must return the Jacobian. The returned matrix must be \(V \times V\) with V being either the size of the whole state vector (for non-SLAM problems) or VEH_SIZE (for SLAM problems).

void OnTransitionNoise(KFMatrix_VxV &out_Q) const override

Implements the transition noise covariance \( Q_k \)

Parameters:

out_Q – Must return the covariance matrix. The returned matrix must be of the same size than the jacobian from OnTransitionJacobian

void OnGetObservationsAndDataAssociation(vector_KFArray_OBS &out_z, std::vector<int> &out_data_association, const vector_KFArray_OBS &in_all_predictions, const KFMatrix &in_S, const std::vector<size_t> &in_lm_indices_in_S, const KFMatrix_OxO &in_R) override

This is called between the KF prediction step and the update step, and the application must return the observations and, when applicable, the data association between these observations and the current map.

This method will be called just once for each complete KF iteration.

Note

It is assumed that the observations are independent, i.e. there are NO cross-covariances between them.

Parameters:

  • out_z – N vectors, each for one “observation” of length OBS_SIZE, N being the number of “observations”: how many observed landmarks for a map, or just one if not applicable.

  • out_data_association – An empty vector or, where applicable, a vector where the i’th element corresponds to the position of the observation in the i’th row of out_z within the system state vector (in the range [0,getNumberOfLandmarksInTheMap()-1]), or -1 if it is a new map element and we want to insert it at the end of this KF iteration.

  • in_S – The full covariance matrix of the observation predictions (i.e. the “innovation covariance matrix”). This is a M*O x M*O matrix with M=length of “in_lm_indices_in_S”.

  • in_lm_indices_in_S – The indices of the map landmarks (range [0,getNumberOfLandmarksInTheMap()-1]) that can be found in the matrix in_S.

void OnObservationModel(const std::vector<size_t> &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const override
void OnObservationJacobians(size_t idx_landmark_to_predict, KFMatrix_OxV &Hx, KFMatrix_OxF &Hy) const override

Implements the observation Jacobians \( \frac{\partial h_i}{\partial x} \) and (when applicable) \( \frac{\partial h_i}{\partial y_i} \).

Parameters:

  • idx_landmark_to_predict – The index of the landmark in the map whose prediction is expected as output. For non SLAM-like problems, this will be zero and the expected output is for the whole state vector.

  • Hx – The output Jacobian \( \frac{\partial h_i}{\partial x} \).

  • Hy – The output Jacobian \( \frac{\partial h_i}{\partial y_i} \).

void OnSubstractObservationVectors(KFArray_OBS &A, const KFArray_OBS &B) const override

Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scalar components (eg, angles).

void OnGetObservationNoise(KFMatrix_OxO &out_R) const override

Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor.

Parameters:

out_R – The noise covariance matrix. It might be non diagonal, but it’ll usually be.

void OnPreComputingPredictions(const vector_KFArray_OBS &in_all_prediction_means, std::vector<size_t> &out_LM_indices_to_predict) const override

This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the number of covariance landmark predictions to be made. For example, features which are known to be “out of sight” shouldn’t be added to the output list to speed up the calculations.

See also

OnGetObservations, OnDataAssociation

Note

This is not a pure virtual method, so it should be implemented only if desired. The default implementation returns a vector with all the landmarks in the map.

Parameters:

  • in_all_prediction_means – The mean of each landmark predictions; the computation or not of the corresponding covariances is what we’re trying to determined with this method.

  • out_LM_indices_to_predict – The list of landmark indices in the map [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.

void OnInverseObservationModel(const KFArray_OBS &in_z, KFArray_FEAT &out_yn, KFMatrix_FxV &out_dyn_dxv, KFMatrix_FxO &out_dyn_dhn) const override

If applicable to the given problem, this method implements the inverse observation model needed to extend the “map” with a new “element”.

  • O: OBS_SIZE

  • V: VEH_SIZE

  • F: FEAT_SIZE

Note

OnNewLandmarkAddedToMap will be also called after calling this method if a landmark is actually being added to the map.

Parameters:

  • in_z – The observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservations().

  • out_yn – The F-length vector with the inverse observation model \( y_n=y(x,z_n) \).

  • out_dyn_dxv – The \(F \times V\) Jacobian of the inv. sensor model wrt the robot pose \( \frac{\partial y_n}{\partial x_v} \).

  • out_dyn_dhn – The \(F \times O\) Jacobian of the inv. sensor model wrt the observation vector \( \frac{\partial y_n}{\partial h_n} \).

void OnNewLandmarkAddedToMap(size_t in_obsIdx, size_t in_idxNewFeat) override

If applicable to the given problem, do here any special handling of adding a new landmark to the map.

See also

OnInverseObservationModel

Parameters:

  • in_obsIndex – The index of the observation whose inverse sensor is to be computed. It corresponds to the row in in_z where the observation can be found.

  • in_idxNewFeat – The index that this new feature will have in the state vector (0:just after the vehicle state, 1: after that,…). Save this number so data association can be done according to these indices.

void OnNormalizeStateVector() override

This method is called after the prediction and after the update, to give the user an opportunity to normalize the state vector (eg, keep angles within -pi,pi range) if the application requires it.

Public Types

using landmark_point_t = mrpt::math::TPoint3D

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

Public Functions

CRangeBearingKFSLAM()

Constructor.

~CRangeBearingKFSLAM() override

Destructor:

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.

See also

getCurrentRobotPose

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.

inline 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.

See also

getCurrentRobotPose

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.

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

inline 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::viz::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

inline const TDataAssocInfo &getLastDataAssociation() const

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

inline 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

See also

getLastPartition

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 !!).

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

inline 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

Public Members

mrpt::slam::CRangeBearingKFSLAM::TOptions options

Protected Functions

mrpt::poses::CPose3DQuat getIncrementFromOdometry() const

Return the last odometry, as a pose increment.

Protected Attributes

mrpt::obs::CActionCollection::Ptr m_action

Set up by processActionObservation

mrpt::obs::CSensoryFrame::Ptr m_SF

Set up by processActionObservation

mrpt::containers::bimap<mrpt::maps::CLandmark::TLandmarkID, unsigned int> m_IDs

The mapping between landmark IDs and indexes in the Pkk cov. matrix:

CIncrementalMapPartitioner mapPartitioner

Used for map partitioning experiments

mrpt::maps::CSimpleMap m_SFs

The sequence of all the observations and the robot path (kept for debugging, statistics,etc)

std::vector<std::vector<uint32_t>> m_lastPartitionSet
TDataAssocInfo m_last_data_association

Last data association

struct TDataAssocInfo

Information for data-association:

See also

getLastDataAssociation

Public Functions

inline TDataAssocInfo()
inline void clear()

Public Members

mrpt::math::CMatrixDynamic<kftype> Y_pred_means
mrpt::math::CMatrixDynamic<kftype> Y_pred_covs
std::vector<size_t> predictions_IDs
std::map<size_t, size_t> newly_inserted_landmarks

Map from the 0-based index within the last observation and the landmark 0-based index in the map (the robot-map state vector) Only used for stats and so.

TDataAssociationResults results
struct TOptions : public mrpt::config::CLoadableOptions

The options for the algorithm

Public Functions

TOptions()

Default values

void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
void dumpToTextStream(std::ostream &out) const override

Public Members

mrpt::math::CVectorFloat stds_Q_no_odo

A 7-length vector with the std. deviation of the transition model in (x,y,z, qr,qx,qy,qz) used only when there is no odometry (if there is odo, its uncertainty values will be used instead); x y z: In meters.

float std_sensor_range = {0.01f}

The std. deviation of the sensor (for the matrix R in the kalman filters), in meters and radians.

float std_sensor_yaw
float std_sensor_pitch
float std_odo_z_additional = {0}

Additional std. dev. to sum to the motion model in the z axis (useful when there is only 2D odometry and we want to put things hard to the algorithm) (default=0)

bool doPartitioningExperiment = {false}

If set to true (default=false), map will be partitioned using the method stated by partitioningMethod

float quantiles_3D_representation = {3}

Default = 3

int partitioningMethod = {0}

Applicable only if “doPartitioningExperiment=true”. 0: Automatically detect partition through graph-cut. N>=1: Cut every “N” observations.

TDataAssociationMethod data_assoc_method = {assocNN}
TDataAssociationMetric data_assoc_metric = {metricMaha}
double data_assoc_IC_chi2_thres = {0.99}

Threshold in [0,1] for the chi2square test for individual compatibility between predictions and observations (default: 0.99)

TDataAssociationMetric data_assoc_IC_metric = {metricMaha}

Whether to use mahalanobis (->chi2 criterion) vs. Matching likelihood.

double data_assoc_IC_ml_threshold = {0.0}

Only if data_assoc_IC_metric==ML, the log-ML threshold (Default=0.0)

bool create_simplemap = {false}

Whether to fill m_SFs (default=false)

bool force_ignore_odometry = {false}

Whether to ignore the input odometry and behave as if there was no odometry at all (default: false)