Go to the documentation of this file.
9 #ifndef CRangeBearingKFSLAM2D_H
10 #define CRangeBearingKFSLAM2D_H
87 std::vector<mrpt::math::TPoint2D>& out_landmarksPositions,
88 std::map<unsigned int, mrpt::maps::CLandmark::TLandmarkID>&
120 std::ostream& out)
const override;
216 bool& out_skipPrediction)
const;
266 const std::vector<size_t>& in_lm_indices_in_S,
270 const std::vector<size_t>& idx_landmarks_to_predict,
283 const size_t& idx_landmark_to_predict,
KFMatrix_OxV& Hx,
324 std::vector<size_t>& out_LM_indices_to_predict)
const;
361 const size_t in_obsIdx,
const size_t in_idxNewFeat);
373 std::map<unsigned int, mrpt::maps::CLandmark::TLandmarkID>&
void OnSubstractObservationVectors(KFArray_OBS &A, const KFArray_OBS &B) const
Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scala...
Information for data-association:
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...
TOptions()
Default values.
bool create_simplemap
Whether to fill m_SFs (default=false)
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.
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.
TDataAssociationMethod
Different algorithms for data association, used in mrpt::slam::data_association.
void OnTransitionJacobianNumericGetIncrements(KFArray_VEH &out_increments) const
Only called if using a numeric approximation of the transition Jacobian, this method must return the ...
The results from mrpt::slam::data_association.
std::shared_ptr< CSensoryFrame > Ptr
void loadOptions(const mrpt::config::CConfigFileBase &ini)
Load options from a ini-like file/text.
TDataAssocInfo m_last_data_association
Last data association.
void reset()
Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,...
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
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...
float quantiles_3D_representation
Default = 3.
mrpt::math::CMatrixFixedNumeric< double, OBS_SIZE, OBS_SIZE > KFMatrix_OxO
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
std::vector< size_t > predictions_IDs
TDataAssociationMetric
Different metrics for data association, used in mrpt::slam::data_association For a comparison of both...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
TDataAssociationMetric data_assoc_IC_metric
Whether to use mahalanobis (->chi2 criterion) vs.
TDataAssociationMethod data_assoc_method
CRangeBearingKFSLAM2D()
Default constructor.
void OnObservationJacobiansNumericGetIncrements(KFArray_VEH &out_veh_increments, KFArray_FEAT &out_feat_increments) const
Only called if using a numeric approximation of the observation Jacobians, this method must return th...
void OnGetObservationNoise(KFMatrix_OxO &out_R) const
Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of ...
double data_assoc_IC_chi2_thres
Threshold in [0,1] for the chi2square test for individual compatibility between predictions and obser...
GLsizei GLsizei GLchar * source
void OnNewLandmarkAddedToMap(const size_t in_obsIdx, const size_t in_idxNewFeat)
If applicable to the given problem, do here any special handling of adding a new landmark to the map.
mrpt::math::CMatrixFixedNumeric< double, FEAT_SIZE, VEH_SIZE > KFMatrix_FxV
mrpt::obs::CSensoryFrame::Ptr m_SF
Set up by processActionObservation.
mrpt::math::CArrayNumeric< double, OBS_SIZE > KFArray_OBS
void OnTransitionJacobian(KFMatrix_VxV &out_F) const
Implements the transition Jacobian .
void OnNormalizeStateVector()
This method is called after the prediction and after the update, to give the user an opportunity to n...
mrpt::math::CVectorFloat stds_Q_no_odo
A 3-length vector with the std.
mrpt::math::CArrayNumeric< double, VEH_SIZE > KFArray_VEH
void OnTransitionModel(const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const
Implements the transition model .
mrpt::maps::CSimpleMap m_SFs
The sequence of all the observations and the robot path (kept for debugging, statistics,...
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.
This class allows loading and storing values and vectors of different types from a configuration text...
TDataAssociationMetric data_assoc_metric
void getCurrentRobotPose(mrpt::poses::CPosePDFGaussian &out_robotPose) const
Returns the mean & 3x3 covariance matrix of the robot 2D pose.
TOptions options
The options for the algorithm.
mrpt::math::CMatrixTemplateNumeric< double > KFMatrix
An implementation of EKF-based SLAM with range-bearing sensors, odometry, and a 2D (+heading) robot p...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
mrpt::math::CMatrixFixedNumeric< double, OBS_SIZE, FEAT_SIZE > KFMatrix_OxF
double data_assoc_IC_ml_threshold
Only if data_assoc_IC_metric==ML, the log-ML threshold (Default=0.0)
void OnObservationModel(const std::vector< size_t > &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const
mrpt::aligned_std_vector< KFArray_OBS > vector_KFArray_OBS
virtual ~CRangeBearingKFSLAM2D()
Destructor.
mrpt::math::CArrayNumeric< double, FEAT_SIZE > KFArray_FEAT
std::shared_ptr< CSetOfObjects > Ptr
void OnGetAction(KFArray_ACT &out_u) const
Must return the action vector u.
mrpt::math::CMatrixTemplateNumeric< kftype > Y_pred_covs
mrpt::math::CMatrixFixedNumeric< double, FEAT_SIZE, OBS_SIZE > KFMatrix_FxO
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...
const TDataAssocInfo & getLastDataAssociation() const
Returns a read-only reference to the information on the last data-association.
mrpt::math::CArrayNumeric< double, ACT_SIZE > KFArray_ACT
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)
This is called between the KF prediction step and the update step, and the application must return th...
mrpt::math::CMatrixFixedNumeric< double, OBS_SIZE, VEH_SIZE > KFMatrix_OxV
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
The options for the algorithm.
void OnInverseObservationModel(const KFArray_OBS &in_z, KFArray_FEAT &out_yn, KFMatrix_FxV &out_dyn_dxv, KFMatrix_FxO &out_dyn_dhn) const
If applicable to the given problem, this method implements the inverse observation model needed to ex...
void getLandmarkIDsFromIndexInStateVector(std::map< unsigned int, mrpt::maps::CLandmark::TLandmarkID > &out_id2index) const
GLsizei const GLchar ** string
void OnObservationJacobians(const size_t &idx_landmark_to_predict, KFMatrix_OxV &Hx, KFMatrix_OxF &Hy) const
Implements the observation Jacobians and (when applicable) .
mrpt::containers::bimap< mrpt::maps::CLandmark::TLandmarkID, unsigned int > m_IDs
The mapping between landmark IDs and indexes in the Pkk cov.
mrpt::obs::CActionCollection::Ptr m_action
Set up by processActionObservation.
const std::map< VALUE, KEY > & getInverseMap() const
Return a read-only reference to the internal map KEY->VALUES.
mrpt::math::CMatrixFixedNumeric< double, VEH_SIZE, VEH_SIZE > KFMatrix_VxV
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
TDataAssociationResults results
void OnTransitionNoise(KFMatrix_VxV &out_Q) const
Implements the transition noise covariance .
float std_sensor_range
The std.
std::shared_ptr< CActionCollection > Ptr
void OnPreComputingPredictions(const vector_KFArray_OBS &in_all_prediction_means, std::vector< size_t > &out_LM_indices_to_predict) const
This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the...
mrpt::math::CMatrixTemplateNumeric< kftype > Y_pred_means
Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.
Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST | |