84 std::vector<mrpt::math::TPoint2D>& out_landmarksPositions,
85 std::map<unsigned int, mrpt::maps::CLandmark::TLandmarkID>&
115 const std::string& section)
override;
117 std::ostream&
out)
const override;
154 const std::string& fil,
float stdCount = 3.0f,
155 const std::string& styleLandmarks = std::string(
"b"),
156 const std::string& stylePath = std::string(
"r"),
157 const std::string& styleRobot = std::string(
"r"))
const;
213 bool& out_skipPrediction)
const override;
263 const std::vector<size_t>& in_lm_indices_in_S,
267 const std::vector<size_t>& idx_landmarks_to_predict,
321 std::vector<size_t>& out_LM_indices_to_predict)
const override;
358 const size_t in_obsIdx,
const size_t in_idxNewFeat)
override;
370 std::map<unsigned int, mrpt::maps::CLandmark::TLandmarkID>&
mrpt::math::CVectorFloat stds_Q_no_odo
A 3-length vector with the std.
An implementation of EKF-based SLAM with range-bearing sensors, odometry, and a 2D (+heading) robot p...
void OnGetAction(KFArray_ACT &out_u) const override
Must return the action vector u.
void OnObservationJacobiansNumericGetIncrements(KFArray_VEH &out_veh_increments, KFArray_FEAT &out_feat_increments) const override
Only called if using a numeric approximation of the observation Jacobians, this method must return th...
TOptions options
The options for the algorithm.
TPoint2D_< double > TPoint2D
Lightweight 2D point.
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
mrpt::math::CVectorFixed< double, ACT_SIZE > KFArray_ACT
mrpt::math::CMatrixFixed< double, VEH_SIZE, VEH_SIZE > KFMatrix_VxV
TDataAssociationMetric data_assoc_metric
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
void OnObservationJacobians(size_t idx_landmark_to_predict, KFMatrix_OxV &Hx, KFMatrix_OxF &Hy) const override
Implements the observation Jacobians and (when applicable) .
bool create_simplemap
Whether to fill m_SFs (default=false)
void OnObservationModel(const std::vector< size_t > &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const override
void reset()
Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,0,0).
~CRangeBearingKFSLAM2D() override
Destructor.
mrpt::math::CVectorFixed< double, VEH_SIZE > KFArray_VEH
void OnNormalizeStateVector() override
This method is called after the prediction and after the update, to give the user an opportunity to n...
mrpt::obs::CActionCollection::Ptr m_action
Set up by processActionObservation.
mrpt::math::CMatrixFixed< double, FEAT_SIZE, OBS_SIZE > KFMatrix_FxO
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ion) override
This method load the options from a ".ini"-like file or memory-stored string list.
mrpt::math::CMatrixDynamic< kftype > Y_pred_means
mrpt::maps::CSimpleMap m_SFs
The sequence of all the observations and the robot path (kept for debugging, statistics,etc)
const std::map< VALUE, KEY > & getInverseMap() const
Return a read-only reference to the internal map KEY->VALUES.
void OnTransitionJacobianNumericGetIncrements(KFArray_VEH &out_increments) const override
Only called if using a numeric approximation of the transition Jacobian, this method must return the ...
void OnTransitionNoise(KFMatrix_VxV &out_Q) const override
Implements the transition noise covariance .
This class allows loading and storing values and vectors of different types from a configuration text...
mrpt::math::CMatrixDynamic< double > KFMatrix
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.
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
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 ex...
float quantiles_3D_representation
Default = 3.
TDataAssociationMethod data_assoc_method
TDataAssociationMetric
Different metrics for data association, used in mrpt::slam::data_association For a comparison of both...
Information for data-association:
void OnTransitionJacobian(KFMatrix_VxV &out_F) const override
Implements the transition Jacobian .
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...
double data_assoc_IC_ml_threshold
Only if data_assoc_IC_metric==ML, the log-ML threshold (Default=0.0)
Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.
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 scala...
std::vector< size_t > predictions_IDs
mrpt::math::CMatrixDynamic< kftype > Y_pred_covs
mrpt::math::CMatrixFixed< double, FEAT_SIZE, VEH_SIZE > KFMatrix_FxV
mrpt::math::CVectorFixed< double, FEAT_SIZE > KFArray_FEAT
TDataAssocInfo m_last_data_association
Last data association.
CRangeBearingKFSLAM2D()
Default constructor.
std::vector< KFArray_OBS > vector_KFArray_OBS
void getCurrentRobotPose(mrpt::poses::CPosePDFGaussian &out_robotPose) const
Returns the mean & 3x3 covariance matrix of the robot 2D pose.
double data_assoc_IC_chi2_thres
Threshold in [0,1] for the chi2square test for individual compatibility between predictions and obser...
void OnGetObservationNoise(KFMatrix_OxO &out_R) const override
Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of ...
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 th...
mrpt::vision::TStereoCalibResults out
float std_sensor_range
The std.
void getLandmarkIDsFromIndexInStateVector(std::map< unsigned int, mrpt::maps::CLandmark::TLandmarkID > &out_id2index) const
TDataAssociationMetric data_assoc_IC_metric
Whether to use mahalanobis (->chi2 criterion) vs.
TDataAssociationResults results
The options for the algorithm.
void loadOptions(const mrpt::config::CConfigFileBase &ini)
Load options from a ini-like file/text.
void OnNewLandmarkAddedToMap(const size_t in_obsIdx, const size_t in_idxNewFeat) override
If applicable to the given problem, do here any special handling of adding a new landmark to the map...
TOptions()
Default values.
The results from mrpt::slam::data_association.
mrpt::math::CVectorFixed< double, OBS_SIZE > KFArray_OBS
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...
TDataAssociationMethod
Different algorithms for data association, used in mrpt::slam::data_association.
mrpt::math::CMatrixFixed< double, OBS_SIZE, VEH_SIZE > KFMatrix_OxV
void OnTransitionModel(const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const override
Implements the transition model .
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 ele...
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 cu...
const TDataAssocInfo & getLastDataAssociation() const
Returns a read-only reference to the information on the last data-association.
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.
mrpt::containers::bimap< mrpt::maps::CLandmark::TLandmarkID, unsigned int > m_IDs
The mapping between landmark IDs and indexes in the Pkk cov.
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
mrpt::math::CMatrixFixed< double, OBS_SIZE, OBS_SIZE > KFMatrix_OxO
mrpt::math::CMatrixFixed< double, OBS_SIZE, FEAT_SIZE > KFMatrix_OxF
mrpt::obs::CSensoryFrame::Ptr m_SF
Set up by processActionObservation.