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>&
216 bool& out_skipPrediction)
const;
282 const size_t& idx_landmark_to_predict,
KFMatrix_OxV& Hx,
360 const size_t in_obsIdx,
const size_t in_idxNewFeat);
372 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 loadOptions(const mrpt::utils::CConfigFileBase &ini)
Load options from a ini-like file/text.
TOptions options
The options for the algorithm.
TDataAssociationMetric data_assoc_metric
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
mrpt::math::CMatrixFixedNumeric< double, FEAT_SIZE, VEH_SIZE > KFMatrix_FxV
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 .
void reset()
Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,0,0).
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
mrpt::math::CArrayNumeric< double, VEH_SIZE > KFArray_VEH
void dumpToTextStream(mrpt::utils::CStream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
mrpt::obs::CActionCollection::Ptr m_action
Set up by processActionObservation.
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 ...
This class allows loading and storing values and vectors of different types from a configuration text...
void OnTransitionJacobianNumericGetIncrements(KFArray_VEH &out_increments) const
Only called if using a numeric approximation of the transition Jacobian, this method must return the ...
mrpt::math::CMatrixFixedNumeric< double, OBS_SIZE, OBS_SIZE > KFMatrix_OxO
mrpt::maps::CSimpleMap m_SFs
The sequence of all the observations and the robot path (kept for debugging, statistics,etc)
mrpt::utils::bimap< mrpt::maps::CLandmark::TLandmarkID, unsigned int > m_IDs
The mapping between landmark IDs and indexes in the Pkk cov.
mrpt::math::CMatrixTemplateNumeric< kftype > Y_pred_covs
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
const std::map< VALUE, KEY > & getInverseMap() const
Return a read-only reference to the internal map KEY->VALUES.
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 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 ...
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 loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string §ion) override
This method load the options from a ".ini"-like file or memory-stored string list.
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...
mrpt::aligned_containers< KFArray_OBS >::vector_t vector_KFArray_OBS
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.
std::shared_ptr< CSetOfObjects > Ptr
mrpt::math::TPoint2D landmark_point_t
Either mrpt::math::TPoint2D or mrpt::math::TPoint3D.
std::shared_ptr< CSensoryFrame > Ptr
void OnTransitionModel(const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const
Implements the transition model .
mrpt::math::CArrayNumeric< double, ACT_SIZE > KFArray_ACT
GLsizei const GLchar ** string
mrpt::math::CMatrixFixedNumeric< double, OBS_SIZE, FEAT_SIZE > KFMatrix_OxF
void OnPreComputingPredictions(const vector_KFArray_OBS &in_all_prediction_means, vector_size_t &out_LM_indices_to_predict) const
This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the...
TDataAssocInfo m_last_data_association
Last data association.
CRangeBearingKFSLAM2D()
Default constructor.
mrpt::math::CArrayNumeric< double, OBS_SIZE > KFArray_OBS
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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...
std::shared_ptr< CActionCollection > Ptr
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::CMatrixTemplateNumeric< double > KFMatrix
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.
mrpt::math::CMatrixFixedNumeric< double, FEAT_SIZE, OBS_SIZE > KFMatrix_FxO
TDataAssociationResults results
The options for the algorithm.
GLsizei GLsizei GLchar * source
void OnObservationModel(const vector_size_t &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const
mrpt::math::CMatrixFixedNumeric< double, OBS_SIZE, VEH_SIZE > KFMatrix_OxV
std::vector< size_t > vector_size_t
std::vector< int32_t > vector_int
mrpt::math::CMatrixFixedNumeric< double, VEH_SIZE, VEH_SIZE > KFMatrix_VxV
TOptions()
Default values.
The results from mrpt::slam::data_association.
TDataAssociationMethod
Different algorithms for data association, used in mrpt::slam::data_association.
void OnGetObservationsAndDataAssociation(vector_KFArray_OBS &out_z, vector_int &out_data_association, const vector_KFArray_OBS &in_all_predictions, const KFMatrix &in_S, const 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::CArrayNumeric< double, FEAT_SIZE > KFArray_FEAT
mrpt::vector_size_t predictions_IDs
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.
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.
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
mrpt::obs::CSensoryFrame::Ptr m_SF
Set up by processActionObservation.
void OnGetAction(KFArray_ACT &out_u) const
Must return the action vector u.
std::vector< size_t > vector_size_t