9 #ifndef CRangeBearingKFSLAM2D_H
10 #define CRangeBearingKFSLAM2D_H
61 void processActionObservation(
62 mrpt::obs::CActionCollectionPtr &action,
63 mrpt::obs::CSensoryFramePtr &SF );
75 std::vector<mrpt::math::TPoint2D> &out_landmarksPositions,
76 std::map<unsigned int,mrpt::maps::CLandmark::TLandmarkID> &out_landmarkIDs,
84 void getCurrentRobotPose(
90 void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj )
const;
106 void loadFromConfigFile(
108 const std::string §ion);
134 void saveMapAndPath2DRepresentationAsMATLABFile(
135 const std::string &fil,
137 const std::string &styleLandmarks = std::string(
"b"),
138 const std::string &stylePath = std::string(
"r"),
139 const std::string &styleRobot = std::string(
"r") )
const;
155 predictions_IDs.clear();
156 newly_inserted_landmarks.clear();
173 return m_last_data_association;
185 void OnGetAction( KFArray_ACT &out_u )
const;
192 void OnTransitionModel(
193 const KFArray_ACT &in_u,
194 KFArray_VEH &inout_x,
195 bool &out_skipPrediction
202 void OnTransitionJacobian( KFMatrix_VxV &out_F )
const;
206 void OnTransitionJacobianNumericGetIncrements(KFArray_VEH &out_increments)
const;
213 void OnTransitionNoise( KFMatrix_VxV &out_Q )
const;
225 void OnGetObservationsAndDataAssociation(
226 vector_KFArray_OBS &out_z,
228 const vector_KFArray_OBS &in_all_predictions,
229 const KFMatrix &in_S,
231 const KFMatrix_OxO &in_R
234 void OnObservationModel(
236 vector_KFArray_OBS &out_predictions
244 void OnObservationJacobians(
245 const size_t &idx_landmark_to_predict,
252 void OnObservationJacobiansNumericGetIncrements(
253 KFArray_VEH &out_veh_increments,
254 KFArray_FEAT &out_feat_increments )
const;
259 void OnSubstractObservationVectors(KFArray_OBS &A,
const KFArray_OBS &B)
const;
264 void OnGetObservationNoise(KFMatrix_OxO &out_R)
const;
273 void OnPreComputingPredictions(
274 const vector_KFArray_OBS &in_all_prediction_means,
289 void OnInverseObservationModel(
290 const KFArray_OBS & in_z,
291 KFArray_FEAT & out_yn,
292 KFMatrix_FxV & out_dyn_dxv,
293 KFMatrix_FxO & out_dyn_dhn )
const;
300 void OnNewLandmarkAddedToMap(
301 const size_t in_obsIdx,
302 const size_t in_idxNewFeat );
307 void OnNormalizeStateVector();
313 void getLandmarkIDsFromIndexInStateVector(std::map<unsigned int,mrpt::maps::CLandmark::TLandmarkID> &out_id2index)
const
315 out_id2index = m_IDs.getInverseMap();
321 mrpt::obs::CActionCollectionPtr m_action;
324 mrpt::obs::CSensoryFramePtr m_SF;
mrpt::math::CVectorFloat stds_Q_no_odo
A 3-length vector with the std. deviation of the transition model in (x,y,phi) used only when there i...
An implementation of EKF-based SLAM with range-bearing sensors, odometry, and a 2D (+heading) robot p...
TOptions options
The options for the algorithm.
TDataAssociationMetric data_assoc_metric
This class stores a sequence of
pairs, thus a "metric map" can be t...
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...
bool create_simplemap
Whether to fill m_SFs (default=false)
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
mrpt::math::CMatrixTemplateNumeric< kftype > Y_pred_means
This class allows loading and storing values and vectors of different types from a configuration text...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
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:
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.
A bidirectional version of std::map, declared as bimap and which actually contains two std...
mrpt::math::TPoint2D landmark_point_t
Either mrpt::math::TPoint2D or mrpt::math::TPoint3D.
const TDataAssocInfo & getLastDataAssociation() const
Returns a read-only reference to the information on the last data-association.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
double data_assoc_IC_chi2_thres
Threshold in [0,1] for the chi2square test for individual compatibility between predictions and obser...
float std_sensor_yaw
The std. deviation of the sensor (for the matrix R in the kalman filters), in meters and radians...
TDataAssociationMetric data_assoc_IC_metric
Whether to use mahalanobis (->chi2 criterion) vs. Matching likelihood.
TDataAssociationResults results
The options for the algorithm.
std::vector< size_t > vector_size_t
std::vector< int32_t > vector_int
The results from mrpt::slam::data_association.
TDataAssociationMethod
Different algorithms for data association, used in mrpt::slam::data_association.
mrpt::vector_size_t predictions_IDs
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
std::vector< size_t > vector_size_t