9 #ifndef CRangeBearingKFSLAM2D_H 10 #define CRangeBearingKFSLAM2D_H 85 std::vector<mrpt::math::TPoint2D>& out_landmarksPositions,
86 std::map<unsigned int, mrpt::maps::CLandmark::TLandmarkID>&
118 std::ostream& out)
const override;
214 bool& out_skipPrediction)
const;
264 const std::vector<size_t>& in_lm_indices_in_S,
268 const std::vector<size_t>& idx_landmarks_to_predict,
281 const size_t& idx_landmark_to_predict,
KFMatrix_OxV& Hx,
322 std::vector<size_t>& out_LM_indices_to_predict)
const;
359 const size_t in_obsIdx,
const size_t in_idxNewFeat);
371 std::map<unsigned int, mrpt::maps::CLandmark::TLandmarkID>&
mrpt::math::CMatrixFixedNumeric< double, OBS_SIZE, VEH_SIZE > KFMatrix_OxV
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...
TOptions options
The options for the algorithm.
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
TDataAssociationMetric data_assoc_metric
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
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...
bool create_simplemap
Whether to fill m_SFs (default=false)
void OnTransitionJacobian(KFMatrix_VxV &out_F) const
Implements the transition Jacobian .
void OnTransitionNoise(KFMatrix_VxV &out_Q) const
Implements the transition noise covariance .
mrpt::math::CArrayNumeric< double, VEH_SIZE > KFArray_VEH
void reset()
Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,0,0).
mrpt::math::CMatrixTemplateNumeric< double > KFMatrix
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
mrpt::obs::CActionCollection::Ptr m_action
Set up by processActionObservation.
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::CMatrixTemplateNumeric< kftype > Y_pred_means
void OnNormalizeStateVector()
This method is called after the prediction and after the update, to give the user an opportunity to n...
void OnGetObservationNoise(KFMatrix_OxO &out_R) const
Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of ...
mrpt::math::CArrayNumeric< double, FEAT_SIZE > KFArray_FEAT
void OnTransitionJacobianNumericGetIncrements(KFArray_VEH &out_increments) const
Only called if using a numeric approximation of the transition Jacobian, this method must return the ...
mrpt::maps::CSimpleMap m_SFs
The sequence of all the observations and the robot path (kept for debugging, statistics,etc)
mrpt::math::CMatrixFixedNumeric< double, VEH_SIZE, VEH_SIZE > KFMatrix_VxV
const std::map< VALUE, KEY > & getInverseMap() const
Return a read-only reference to the internal map KEY->VALUES.
mrpt::math::CMatrixTemplateNumeric< kftype > Y_pred_covs
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 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...
This class allows loading and storing values and vectors of different types from a configuration text...
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.
void OnObservationJacobians(const size_t &idx_landmark_to_predict, KFMatrix_OxV &Hx, KFMatrix_OxF &Hy) const
Implements the observation Jacobians and (when applicable) .
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...
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
mrpt::math::CArrayNumeric< double, OBS_SIZE > KFArray_OBS
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:
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...
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...
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 OnTransitionModel(const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const
Implements the transition model .
std::vector< size_t > predictions_IDs
mrpt::math::CMatrixFixedNumeric< double, OBS_SIZE, OBS_SIZE > KFMatrix_OxO
GLsizei const GLchar ** string
mrpt::aligned_std_vector< KFArray_OBS > vector_KFArray_OBS
mrpt::math::CArrayNumeric< double, ACT_SIZE > KFArray_ACT
TDataAssocInfo m_last_data_association
Last data association.
CRangeBearingKFSLAM2D()
Default constructor.
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...
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...
mrpt::math::CMatrixFixedNumeric< double, FEAT_SIZE, VEH_SIZE > KFMatrix_FxV
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.
void OnObservationModel(const std::vector< size_t > &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const
TDataAssociationResults results
The options for the algorithm.
GLsizei GLsizei GLchar * source
mrpt::math::CMatrixFixedNumeric< double, OBS_SIZE, FEAT_SIZE > KFMatrix_OxF
void loadOptions(const mrpt::config::CConfigFileBase &ini)
Load options from a ini-like file/text.
TOptions()
Default values.
The results from mrpt::slam::data_association.
TDataAssociationMethod
Different algorithms for data association, used in mrpt::slam::data_association.
virtual ~CRangeBearingKFSLAM2D()
Destructor.
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.
mrpt::math::CMatrixFixedNumeric< double, FEAT_SIZE, OBS_SIZE > KFMatrix_FxO
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::obs::CSensoryFrame::Ptr m_SF
Set up by processActionObservation.
void OnGetAction(KFArray_ACT &out_u) const
Must return the action vector u.