Go to the documentation of this file.
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::CMatrixFixed< double, FEAT_SIZE, VEH_SIZE > KFMatrix_FxV
@ metricMaha
Mahalanobis distance.
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.
The results from mrpt::slam::data_association.
std::shared_ptr< mrpt::opengl ::CSetOfObjects > 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,...
mrpt::math::CVectorFixed< double, FEAT_SIZE > KFArray_FEAT
std::shared_ptr< mrpt::obs ::CSensoryFrame > Ptr
void OnObservationModel(const std::vector< size_t > &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const override
Implements the observation prediction .
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 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.
mrpt::math::CVectorFixed< double, ACT_SIZE > KFArray_ACT
void OnTransitionJacobian(KFMatrix_VxV &out_F) const override
Implements the transition Jacobian .
float quantiles_3D_representation
Default = 3.
mrpt::math::CMatrixFixed< double, OBS_SIZE, VEH_SIZE > KFMatrix_OxV
~CRangeBearingKFSLAM2D() override
Destructor.
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
void OnNormalizeStateVector() override
This method is called after the prediction and after the update, to give the user an opportunity to n...
std::shared_ptr< mrpt::obs ::CActionCollection > Ptr
mrpt::vision::TStereoCalibResults out
std::vector< size_t > predictions_IDs
TDataAssociationMetric
Different metrics for data association, used in mrpt::slam::data_association For a comparison of both...
TDataAssociationMetric data_assoc_IC_metric
Whether to use mahalanobis (->chi2 criterion) vs.
void OnTransitionNoise(KFMatrix_VxV &out_Q) const override
Implements the transition noise covariance .
TDataAssociationMethod data_assoc_method
CRangeBearingKFSLAM2D()
Default constructor.
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...
double data_assoc_IC_chi2_thres
Threshold in [0,1] for the chi2square test for individual compatibility between predictions and obser...
mrpt::math::CMatrixFixed< double, VEH_SIZE, VEH_SIZE > KFMatrix_VxV
std::vector< KFArray_OBS > vector_KFArray_OBS
mrpt::obs::CSensoryFrame::Ptr m_SF
Set up by processActionObservation.
mrpt::math::CMatrixDynamic< kftype > Y_pred_means
mrpt::math::CVectorFloat stds_Q_no_odo
A 3-length vector with the std.
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...
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...
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.
mrpt::math::CVectorFixed< double, OBS_SIZE > KFArray_OBS
TOptions options
The options for the algorithm.
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...
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...
double data_assoc_IC_ml_threshold
Only if data_assoc_IC_metric==ML, the log-ML threshold (Default=0.0)
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 OnGetAction(KFArray_ACT &out_u) const override
Must return the action vector u.
mrpt::math::CMatrixFixed< double, OBS_SIZE, OBS_SIZE > KFMatrix_OxO
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::CMatrixDynamic< kftype > Y_pred_covs
mrpt::math::CVectorFixed< double, VEH_SIZE > KFArray_VEH
mrpt::math::CMatrixFixed< double, FEAT_SIZE, OBS_SIZE > KFMatrix_FxO
mrpt::math::CMatrixDynamic< double > KFMatrix
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
void OnTransitionModel(const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const override
Implements the transition model .
TPoint2D_< double > TPoint2D
Lightweight 2D point.
The options for the algorithm.
@ assocNN
Nearest-neighbor.
void getLandmarkIDsFromIndexInStateVector(std::map< unsigned int, mrpt::maps::CLandmark::TLandmarkID > &out_id2index) const
mrpt::containers::bimap< mrpt::maps::CLandmark::TLandmarkID, unsigned int > m_IDs
The mapping between landmark IDs and indexes in the Pkk cov.
void OnGetObservationNoise(KFMatrix_OxO &out_R) const override
Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of ...
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.
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
TDataAssociationResults results
mrpt::math::CMatrixFixed< double, OBS_SIZE, FEAT_SIZE > KFMatrix_OxF
float std_sensor_range
The std.
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...
Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.
void OnObservationJacobians(size_t idx_landmark_to_predict, KFMatrix_OxV &Hx, KFMatrix_OxF &Hy) const override
Implements the observation Jacobians and (when applicable) .
Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sat Jun 27 14:00:59 UTC 2020 | |