Main MRPT website > C++ reference for MRPT 1.9.9
CRangeBearingKFSLAM2D.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #ifndef CRangeBearingKFSLAM2D_H
10 #define CRangeBearingKFSLAM2D_H
11 
17 
19 #include <mrpt/containers/bimap.h>
20 
21 #include <mrpt/obs/CSensoryFrame.h>
25 #include <mrpt/maps/CLandmark.h>
26 #include <mrpt/maps/CSimpleMap.h>
29 
30 namespace mrpt
31 {
32 namespace slam
33 {
34 /** An implementation of EKF-based SLAM with range-bearing sensors, odometry,
35  *and a 2D (+heading) robot pose, and 2D landmarks.
36  * The main method is "processActionObservation" which processes pairs of
37  *action/observation.
38  *
39  * The following pages describe front-end applications based on this class:
40  * - http://www.mrpt.org/Application:2d-slam-demo
41  * - http://www.mrpt.org/Application:kf-slam
42  *
43  * \sa CRangeBearingKFSLAM \ingroup metric_slam_grp
44  */
46  : public bayes::CKalmanFilterCapable<3 /* x y yaw */, 2 /* range yaw */,
47  2 /* x y */, 3 /* Ax Ay Ayaw */>
48 // <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, size
49 // typename kftype = double>
50 {
51  public:
52  /** Either mrpt::math::TPoint2D or mrpt::math::TPoint3D */
54 
55  /** Default constructor */
57  /** Destructor */
58  virtual ~CRangeBearingKFSLAM2D();
59  /** Reset the state of the SLAM filter: The map is emptied and the robot put
60  * back to (0,0,0). */
61  void reset();
62 
63  /** Process one new action and observations to update the map and robot pose
64  *estimate. See the description of the class at the top of this page.
65  * \param action May contain odometry
66  * \param SF The set of observations, must contain at least one
67  *CObservationBearingRange
68  */
72 
73  /** Returns the complete mean and cov.
74  * \param out_robotPose The mean & 3x3 covariance matrix of the robot 2D
75  * pose
76  * \param out_landmarksPositions One entry for each of the M landmark
77  * positions (2D).
78  * \param out_landmarkIDs Each element[index] (for indices of
79  * out_landmarksPositions) gives the corresponding landmark ID.
80  * \param out_fullState The complete state vector (3+2M).
81  * \param out_fullCovariance The full (3+2M)x(3+2M) covariance matrix of
82  * the filter.
83  * \sa getCurrentRobotPose
84  */
85  void getCurrentState(
86  mrpt::poses::CPosePDFGaussian& out_robotPose,
87  std::vector<mrpt::math::TPoint2D>& out_landmarksPositions,
88  std::map<unsigned int, mrpt::maps::CLandmark::TLandmarkID>&
89  out_landmarkIDs,
90  mrpt::math::CVectorDouble& out_fullState,
91  mrpt::math::CMatrixDouble& out_fullCovariance) const;
92 
93  /** Returns the mean & 3x3 covariance matrix of the robot 2D pose.
94  * \sa getCurrentState
95  */
97  mrpt::poses::CPosePDFGaussian& out_robotPose) const;
98 
99  /** Returns a 3D representation of the landmarks in the map and the robot 3D
100  * position according to the current filter state.
101  * \param out_objects
102  */
104 
105  /** Load options from a ini-like file/text
106  */
108 
109  /** The options for the algorithm
110  */
112  {
113  /** Default values */
114  TOptions();
115 
116  void loadFromConfigFile(
118  const std::string& section) override; // See base docs
119  void dumpToTextStream(
120  std::ostream& out) const override; // See base docs
121 
122  /** A 3-length vector with the std. deviation of the transition model in
123  * (x,y,phi) used only when there is no odometry (if there is odo, its
124  * uncertainty values will be used instead); x y: In meters, phi:
125  * radians (but in degrees when loading from a configuration ini-file!)
126  */
128  /** The std. deviation of the sensor (for the matrix R in the kalman
129  * filters), in meters and radians. */
131  /** Default = 3 */
133  /** Whether to fill m_SFs (default=false) */
135 
136  // Data association:
139  /** Threshold in [0,1] for the chi2square test for individual
140  * compatibility between predictions and observations (default: 0.99) */
142  /** Whether to use mahalanobis (->chi2 criterion) vs. Matching
143  * likelihood. */
145  /** Only if data_assoc_IC_metric==ML, the log-ML threshold (Default=0.0)
146  */
148  };
149 
150  /** The options for the algorithm */
152 
153  /** Save the current state of the filter (robot pose & map) to a MATLAB
154  * script which displays all the elements in 2D
155  */
157  const std::string& fil, float stdCount = 3.0f,
158  const std::string& styleLandmarks = std::string("b"),
159  const std::string& stylePath = std::string("r"),
160  const std::string& styleRobot = std::string("r")) const;
161 
162  /** Information for data-association:
163  * \sa getLastDataAssociation
164  */
166  {
168  void clear()
169  {
170  results.clear();
171  predictions_IDs.clear();
172  newly_inserted_landmarks.clear();
173  }
174 
175  // Predictions from the map:
177  std::vector<size_t> predictions_IDs;
178 
179  /** Map from the 0-based index within the last observation and the
180  landmark 0-based index in the map (the robot-map state vector)
181  Only used for stats and so. */
182  std::map<size_t, size_t> newly_inserted_landmarks;
183 
184  // DA results:
186  };
187 
188  /** Returns a read-only reference to the information on the last
189  * data-association */
191  {
193  }
194 
195  protected:
196  /** @name Virtual methods for Kalman Filter implementation
197  @{
198  */
199 
200  /** Must return the action vector u.
201  * \param out_u The action vector which will be passed to OnTransitionModel
202  */
203  void OnGetAction(KFArray_ACT& out_u) const;
204 
205  /** Implements the transition model \f$ \hat{x}_{k|k-1} = f(
206  * \hat{x}_{k-1|k-1}, u_k ) \f$
207  * \param in_u The vector returned by OnGetAction.
208  * \param inout_x At input has \f[ \hat{x}_{k-1|k-1} \f] , at output must
209  * have \f$ \hat{x}_{k|k-1} \f$ .
210  * \param out_skip Set this to true if for some reason you want to skip the
211  * prediction step (to do not modify either the vector or the covariance).
212  * Default:false
213  */
214  void OnTransitionModel(
215  const KFArray_ACT& in_u, KFArray_VEH& inout_x,
216  bool& out_skipPrediction) const;
217 
218  /** Implements the transition Jacobian \f$ \frac{\partial f}{\partial x} \f$
219  * \param out_F Must return the Jacobian.
220  * The returned matrix must be \f$V \times V\f$ with V being either the
221  * size of the whole state vector (for non-SLAM problems) or VEH_SIZE (for
222  * SLAM problems).
223  */
224  void OnTransitionJacobian(KFMatrix_VxV& out_F) const;
225 
226  /** Only called if using a numeric approximation of the transition Jacobian,
227  * this method must return the increments in each dimension of the vehicle
228  * state vector while estimating the Jacobian.
229  */
231  KFArray_VEH& out_increments) const;
232 
233  /** Implements the transition noise covariance \f$ Q_k \f$
234  * \param out_Q Must return the covariance matrix.
235  * The returned matrix must be of the same size than the jacobian from
236  * OnTransitionJacobian
237  */
238  void OnTransitionNoise(KFMatrix_VxV& out_Q) const;
239 
240  /** This is called between the KF prediction step and the update step, and
241  * the application must return the observations and, when applicable, the
242  * data association between these observations and the current map.
243  *
244  * \param out_z N vectors, each for one "observation" of length OBS_SIZE, N
245  * being the number of "observations": how many observed landmarks for a
246  * map, or just one if not applicable.
247  * \param out_data_association An empty vector or, where applicable, a
248  * vector where the i'th element corresponds to the position of the
249  * observation in the i'th row of out_z within the system state vector (in
250  * the range [0,getNumberOfLandmarksInTheMap()-1]), or -1 if it is a new map
251  * element and we want to insert it at the end of this KF iteration.
252  * \param in_S The full covariance matrix of the observation predictions
253  * (i.e. the "innovation covariance matrix"). This is a M*O x M*O matrix
254  * with M=length of "in_lm_indices_in_S".
255  * \param in_lm_indices_in_S The indices of the map landmarks (range
256  * [0,getNumberOfLandmarksInTheMap()-1]) that can be found in the matrix
257  * in_S.
258  *
259  * This method will be called just once for each complete KF iteration.
260  * \note It is assumed that the observations are independent, i.e. there
261  * are NO cross-covariances between them.
262  */
264  vector_KFArray_OBS& out_z, std::vector<int>& out_data_association,
265  const vector_KFArray_OBS& in_all_predictions, const KFMatrix& in_S,
266  const std::vector<size_t>& in_lm_indices_in_S,
267  const KFMatrix_OxO& in_R);
268 
269  void OnObservationModel(
270  const std::vector<size_t>& idx_landmarks_to_predict,
271  vector_KFArray_OBS& out_predictions) const;
272 
273  /** Implements the observation Jacobians \f$ \frac{\partial h_i}{\partial x}
274  * \f$ and (when applicable) \f$ \frac{\partial h_i}{\partial y_i} \f$.
275  * \param idx_landmark_to_predict The index of the landmark in the map
276  * whose prediction is expected as output. For non SLAM-like problems, this
277  * will be zero and the expected output is for the whole state vector.
278  * \param Hx The output Jacobian \f$ \frac{\partial h_i}{\partial x} \f$.
279  * \param Hy The output Jacobian \f$ \frac{\partial h_i}{\partial y_i}
280  * \f$.
281  */
283  const size_t& idx_landmark_to_predict, KFMatrix_OxV& Hx,
284  KFMatrix_OxF& Hy) const;
285 
286  /** Only called if using a numeric approximation of the observation
287  * Jacobians, this method must return the increments in each dimension of
288  * the vehicle state vector while estimating the Jacobian.
289  */
291  KFArray_VEH& out_veh_increments,
292  KFArray_FEAT& out_feat_increments) const;
293 
294  /** Computes A=A-B, which may need to be re-implemented depending on the
295  * topology of the individual scalar components (eg, angles).
296  */
298  KFArray_OBS& A, const KFArray_OBS& B) const;
299 
300  /** Return the observation NOISE covariance matrix, that is, the model of
301  * the Gaussian additive noise of the sensor.
302  * \param out_R The noise covariance matrix. It might be non diagonal, but
303  * it'll usually be.
304  */
305  void OnGetObservationNoise(KFMatrix_OxO& out_R) const;
306 
307  /** This will be called before OnGetObservationsAndDataAssociation to allow
308  * the application to reduce the number of covariance landmark predictions
309  * to be made.
310  * For example, features which are known to be "out of sight" shouldn't be
311  * added to the output list to speed up the calculations.
312  * \param in_all_prediction_means The mean of each landmark predictions;
313  * the computation or not of the corresponding covariances is what we're
314  * trying to determined with this method.
315  * \param out_LM_indices_to_predict The list of landmark indices in the map
316  * [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.
317  * \note This is not a pure virtual method, so it should be implemented
318  * only if desired. The default implementation returns a vector with all the
319  * landmarks in the map.
320  * \sa OnGetObservations, OnDataAssociation
321  */
323  const vector_KFArray_OBS& in_all_prediction_means,
324  std::vector<size_t>& out_LM_indices_to_predict) const;
325 
326  /** If applicable to the given problem, this method implements the inverse
327  * observation model needed to extend the "map" with a new "element".
328  * \param in_z The observation vector whose inverse sensor model is to be
329  * computed. This is actually one of the vector<> returned by
330  * OnGetObservations().
331  * \param out_yn The F-length vector with the inverse observation model \f$
332  * y_n=y(x,z_n) \f$.
333  * \param out_dyn_dxv The \f$F \times V\f$ Jacobian of the inv. sensor
334  * model wrt the robot pose \f$ \frac{\partial y_n}{\partial x_v} \f$.
335  * \param out_dyn_dhn The \f$F \times O\f$ Jacobian of the inv. sensor
336  * model wrt the observation vector \f$ \frac{\partial y_n}{\partial h_n}
337  * \f$.
338  *
339  * - O: OBS_SIZE
340  * - V: VEH_SIZE
341  * - F: FEAT_SIZE
342  *
343  * \note OnNewLandmarkAddedToMap will be also called after calling this
344  * method if a landmark is actually being added to the map.
345  */
347  const KFArray_OBS& in_z, KFArray_FEAT& out_yn,
348  KFMatrix_FxV& out_dyn_dxv, KFMatrix_FxO& out_dyn_dhn) const;
349 
350  /** If applicable to the given problem, do here any special handling of
351  * adding a new landmark to the map.
352  * \param in_obsIndex The index of the observation whose inverse sensor is
353  * to be computed. It corresponds to the row in in_z where the observation
354  * can be found.
355  * \param in_idxNewFeat The index that this new feature will have in the
356  * state vector (0:just after the vehicle state, 1: after that,...). Save
357  * this number so data association can be done according to these indices.
358  * \sa OnInverseObservationModel
359  */
361  const size_t in_obsIdx, const size_t in_idxNewFeat);
362 
363  /** This method is called after the prediction and after the update, to give
364  * the user an opportunity to normalize the state vector (eg, keep angles
365  * within -pi,pi range) if the application requires it.
366  */
367  void OnNormalizeStateVector();
368 
369  /** @}
370  */
371 
373  std::map<unsigned int, mrpt::maps::CLandmark::TLandmarkID>&
374  out_id2index) const
375  {
376  out_id2index = m_IDs.getInverseMap();
377  }
378 
379  protected:
380  /** Set up by processActionObservation */
382 
383  /** Set up by processActionObservation */
385 
386  /** The mapping between landmark IDs and indexes in the Pkk cov. matrix: */
389 
390  /** The sequence of all the observations and the robot path (kept for
391  * debugging, statistics,etc) */
393 
394  /** Last data association */
396 }; // end class
397 } // End of namespace
398 } // End of namespace
399 
400 #endif
mrpt::slam::CRangeBearingKFSLAM2D::OnSubstractObservationVectors
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...
Definition: CRangeBearingKFSLAM2D.cpp:1108
mrpt::slam::CRangeBearingKFSLAM2D::TDataAssocInfo
Information for data-association:
Definition: CRangeBearingKFSLAM2D.h:165
mrpt::slam::CRangeBearingKFSLAM2D::getAs3DObject
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...
Definition: CRangeBearingKFSLAM2D.cpp:950
mrpt::slam::CRangeBearingKFSLAM2D::TOptions::TOptions
TOptions()
Default values.
Definition: CRangeBearingKFSLAM2D.cpp:775
mrpt::slam::CRangeBearingKFSLAM2D::TOptions::create_simplemap
bool create_simplemap
Whether to fill m_SFs (default=false)
Definition: CRangeBearingKFSLAM2D.h:134
mrpt::slam::CRangeBearingKFSLAM2D::processActionObservation
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.
Definition: CRangeBearingKFSLAM2D.cpp:138
mrpt::slam::CRangeBearingKFSLAM2D::getCurrentState
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.
Definition: CRangeBearingKFSLAM2D.cpp:93
mrpt::slam::TDataAssociationMethod
TDataAssociationMethod
Different algorithms for data association, used in mrpt::slam::data_association.
Definition: data_association.h:32
mrpt::slam::CRangeBearingKFSLAM2D::OnTransitionJacobianNumericGetIncrements
void OnTransitionJacobianNumericGetIncrements(KFArray_VEH &out_increments) const
Only called if using a numeric approximation of the transition Jacobian, this method must return the ...
Definition: CRangeBearingKFSLAM2D.cpp:1182
mrpt::slam::TDataAssociationResults
The results from mrpt::slam::data_association.
Definition: data_association.h:64
mrpt::obs::CSensoryFrame::Ptr
std::shared_ptr< CSensoryFrame > Ptr
Definition: CSensoryFrame.h:56
opengl_frwds.h
mrpt::slam::CRangeBearingKFSLAM2D::loadOptions
void loadOptions(const mrpt::config::CConfigFileBase &ini)
Load options from a ini-like file/text.
Definition: CRangeBearingKFSLAM2D.cpp:732
mrpt::slam::CRangeBearingKFSLAM2D::m_last_data_association
TDataAssocInfo m_last_data_association
Last data association.
Definition: CRangeBearingKFSLAM2D.h:395
mrpt::slam::CRangeBearingKFSLAM2D::reset
void reset()
Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,...
Definition: CRangeBearingKFSLAM2D.cpp:50
mrpt::math::dynamic_vector
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
Definition: eigen_frwds.h:44
mrpt::slam::CRangeBearingKFSLAM2D::TDataAssocInfo::newly_inserted_landmarks
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...
Definition: CRangeBearingKFSLAM2D.h:182
mrpt::slam::CRangeBearingKFSLAM2D::TOptions::quantiles_3D_representation
float quantiles_3D_representation
Default = 3.
Definition: CRangeBearingKFSLAM2D.h:132
mrpt::slam::CRangeBearingKFSLAM2D::TOptions::std_sensor_yaw
float std_sensor_yaw
Definition: CRangeBearingKFSLAM2D.h:130
mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::KFMatrix_OxO
mrpt::math::CMatrixFixedNumeric< double, OBS_SIZE, OBS_SIZE > KFMatrix_OxO
Definition: CKalmanFilterCapable.h:248
mrpt::slam::CRangeBearingKFSLAM2D::TOptions::dumpToTextStream
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
Definition: CRangeBearingKFSLAM2D.cpp:796
mrpt::slam::CRangeBearingKFSLAM2D::TDataAssocInfo::predictions_IDs
std::vector< size_t > predictions_IDs
Definition: CRangeBearingKFSLAM2D.h:177
mrpt::slam::TDataAssociationMetric
TDataAssociationMetric
Different metrics for data association, used in mrpt::slam::data_association For a comparison of both...
Definition: data_association.h:48
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
mrpt::slam::CRangeBearingKFSLAM2D::TOptions::data_assoc_IC_metric
TDataAssociationMetric data_assoc_IC_metric
Whether to use mahalanobis (->chi2 criterion) vs.
Definition: CRangeBearingKFSLAM2D.h:144
mrpt::slam::CRangeBearingKFSLAM2D::TOptions::data_assoc_method
TDataAssociationMethod data_assoc_method
Definition: CRangeBearingKFSLAM2D.h:137
mrpt::slam::CRangeBearingKFSLAM2D::CRangeBearingKFSLAM2D
CRangeBearingKFSLAM2D()
Default constructor.
Definition: CRangeBearingKFSLAM2D.cpp:41
mrpt::slam::CRangeBearingKFSLAM2D::OnObservationJacobiansNumericGetIncrements
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...
Definition: CRangeBearingKFSLAM2D.cpp:1192
CActionCollection.h
bimap.h
mrpt::slam::CRangeBearingKFSLAM2D::OnGetObservationNoise
void OnGetObservationNoise(KFMatrix_OxO &out_R) const
Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of ...
Definition: CRangeBearingKFSLAM2D.cpp:1121
mrpt::slam::CRangeBearingKFSLAM2D::TOptions::data_assoc_IC_chi2_thres
double data_assoc_IC_chi2_thres
Threshold in [0,1] for the chi2square test for individual compatibility between predictions and obser...
Definition: CRangeBearingKFSLAM2D.h:141
mrpt::slam::CRangeBearingKFSLAM2D::TDataAssocInfo::TDataAssocInfo
TDataAssocInfo()
Definition: CRangeBearingKFSLAM2D.h:167
source
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
mrpt::slam::CRangeBearingKFSLAM2D::TDataAssocInfo::clear
void clear()
Definition: CRangeBearingKFSLAM2D.h:168
mrpt::slam::CRangeBearingKFSLAM2D::OnNewLandmarkAddedToMap
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.
Definition: CRangeBearingKFSLAM2D.cpp:914
mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::KFMatrix_FxV
mrpt::math::CMatrixFixedNumeric< double, FEAT_SIZE, VEH_SIZE > KFMatrix_FxV
Definition: CKalmanFilterCapable.h:259
mrpt::math::CMatrixTemplateNumeric< double >
mrpt::slam::CRangeBearingKFSLAM2D::m_SF
mrpt::obs::CSensoryFrame::Ptr m_SF
Set up by processActionObservation.
Definition: CRangeBearingKFSLAM2D.h:384
mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::KFArray_OBS
mrpt::math::CArrayNumeric< double, OBS_SIZE > KFArray_OBS
Definition: CKalmanFilterCapable.h:269
mrpt::slam::CRangeBearingKFSLAM2D::OnTransitionJacobian
void OnTransitionJacobian(KFMatrix_VxV &out_F) const
Implements the transition Jacobian .
Definition: CRangeBearingKFSLAM2D.cpp:234
CConfigFileBase.h
mrpt::slam::CRangeBearingKFSLAM2D::OnNormalizeStateVector
void OnNormalizeStateVector()
This method is called after the prediction and after the update, to give the user an opportunity to n...
Definition: CRangeBearingKFSLAM2D.cpp:723
mrpt::slam::CRangeBearingKFSLAM2D::TOptions::stds_Q_no_odo
mrpt::math::CVectorFloat stds_Q_no_odo
A 3-length vector with the std.
Definition: CRangeBearingKFSLAM2D.h:127
mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::KFArray_VEH
mrpt::math::CArrayNumeric< double, VEH_SIZE > KFArray_VEH
Definition: CKalmanFilterCapable.h:267
mrpt::slam::CRangeBearingKFSLAM2D::OnTransitionModel
void OnTransitionModel(const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const
Implements the transition model .
Definition: CRangeBearingKFSLAM2D.cpp:203
CObservationBearingRange.h
mrpt::slam::CRangeBearingKFSLAM2D::m_SFs
mrpt::maps::CSimpleMap m_SFs
The sequence of all the observations and the robot path (kept for debugging, statistics,...
Definition: CRangeBearingKFSLAM2D.h:392
mrpt::slam::CRangeBearingKFSLAM2D::TOptions::loadFromConfigFile
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
Definition: CRangeBearingKFSLAM2D.cpp:743
CMatrixTemplateNumeric.h
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::slam::CRangeBearingKFSLAM2D::TOptions::data_assoc_metric
TDataAssociationMetric data_assoc_metric
Definition: CRangeBearingKFSLAM2D.h:138
mrpt::slam::CRangeBearingKFSLAM2D::getCurrentRobotPose
void getCurrentRobotPose(mrpt::poses::CPosePDFGaussian &out_robotPose) const
Returns the mean & 3x3 covariance matrix of the robot 2D pose.
Definition: CRangeBearingKFSLAM2D.cpp:72
mrpt::slam::CRangeBearingKFSLAM2D::options
TOptions options
The options for the algorithm.
Definition: CRangeBearingKFSLAM2D.h:151
mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::KFMatrix
mrpt::math::CMatrixTemplateNumeric< double > KFMatrix
Definition: CKalmanFilterCapable.h:243
mrpt::containers::bimap< mrpt::maps::CLandmark::TLandmarkID, unsigned int >
mrpt::slam::CRangeBearingKFSLAM2D
An implementation of EKF-based SLAM with range-bearing sensors, odometry, and a 2D (+heading) robot p...
Definition: CRangeBearingKFSLAM2D.h:45
mrpt::config::CLoadableOptions
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
Definition: config/CLoadableOptions.h:28
mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::KFMatrix_OxF
mrpt::math::CMatrixFixedNumeric< double, OBS_SIZE, FEAT_SIZE > KFMatrix_OxF
Definition: CKalmanFilterCapable.h:263
mrpt::slam::CRangeBearingKFSLAM2D::TOptions::data_assoc_IC_ml_threshold
double data_assoc_IC_ml_threshold
Only if data_assoc_IC_metric==ML, the log-ML threshold (Default=0.0)
Definition: CRangeBearingKFSLAM2D.h:147
mrpt::slam::CRangeBearingKFSLAM2D::OnObservationModel
void OnObservationModel(const std::vector< size_t > &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const
Definition: CRangeBearingKFSLAM2D.cpp:324
mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::vector_KFArray_OBS
mrpt::aligned_std_vector< KFArray_OBS > vector_KFArray_OBS
Definition: CKalmanFilterCapable.h:270
mrpt::math::TPoint2D
Lightweight 2D point.
Definition: lightweight_geom_data.h:42
mrpt::slam::CRangeBearingKFSLAM2D::~CRangeBearingKFSLAM2D
virtual ~CRangeBearingKFSLAM2D()
Destructor.
Definition: CRangeBearingKFSLAM2D.cpp:68
mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::KFArray_FEAT
mrpt::math::CArrayNumeric< double, FEAT_SIZE > KFArray_FEAT
Definition: CKalmanFilterCapable.h:271
mrpt::opengl::CSetOfObjects::Ptr
std::shared_ptr< CSetOfObjects > Ptr
Definition: CSetOfObjects.h:30
mrpt::slam::CRangeBearingKFSLAM2D::OnGetAction
void OnGetAction(KFArray_ACT &out_u) const
Must return the action vector u.
Definition: CRangeBearingKFSLAM2D.cpp:172
mrpt::slam::CRangeBearingKFSLAM2D::TDataAssocInfo::Y_pred_covs
mrpt::math::CMatrixTemplateNumeric< kftype > Y_pred_covs
Definition: CRangeBearingKFSLAM2D.h:176
mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::KFMatrix_FxO
mrpt::math::CMatrixFixedNumeric< double, FEAT_SIZE, OBS_SIZE > KFMatrix_FxO
Definition: CKalmanFilterCapable.h:261
mrpt::slam::CRangeBearingKFSLAM2D::saveMapAndPath2DRepresentationAsMATLABFile
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...
Definition: CRangeBearingKFSLAM2D.cpp:1011
mrpt::slam::CRangeBearingKFSLAM2D::getLastDataAssociation
const TDataAssocInfo & getLastDataAssociation() const
Returns a read-only reference to the information on the last data-association.
Definition: CRangeBearingKFSLAM2D.h:190
CSensoryFrame.h
safe_pointers.h
CLoadableOptions.h
data_association.h
mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::KFArray_ACT
mrpt::math::CArrayNumeric< double, ACT_SIZE > KFArray_ACT
Definition: CKalmanFilterCapable.h:268
CPosePDFGaussian.h
mrpt::slam::CRangeBearingKFSLAM2D::OnGetObservationsAndDataAssociation
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...
Definition: CRangeBearingKFSLAM2D.cpp:526
mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::KFMatrix_OxV
mrpt::math::CMatrixFixedNumeric< double, OBS_SIZE, VEH_SIZE > KFMatrix_OxV
Definition: CKalmanFilterCapable.h:265
CKalmanFilterCapable.h
mrpt::maps::CSimpleMap
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:35
mrpt::slam::CRangeBearingKFSLAM2D::TOptions
The options for the algorithm.
Definition: CRangeBearingKFSLAM2D.h:111
mrpt::slam::CRangeBearingKFSLAM2D::OnInverseObservationModel
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...
Definition: CRangeBearingKFSLAM2D.cpp:825
CIncrementalMapPartitioner.h
mrpt::slam::CRangeBearingKFSLAM2D::getLandmarkIDsFromIndexInStateVector
void getLandmarkIDsFromIndexInStateVector(std::map< unsigned int, mrpt::maps::CLandmark::TLandmarkID > &out_id2index) const
Definition: CRangeBearingKFSLAM2D.h:372
mrpt::slam::TDataAssociationResults::clear
void clear()
Definition: data_association.h:76
string
GLsizei const GLchar ** string
Definition: glext.h:4101
mrpt::slam::CRangeBearingKFSLAM2D::OnObservationJacobians
void OnObservationJacobians(const size_t &idx_landmark_to_predict, KFMatrix_OxV &Hx, KFMatrix_OxF &Hy) const
Implements the observation Jacobians and (when applicable) .
Definition: CRangeBearingKFSLAM2D.cpp:391
mrpt::slam::CRangeBearingKFSLAM2D::m_IDs
mrpt::containers::bimap< mrpt::maps::CLandmark::TLandmarkID, unsigned int > m_IDs
The mapping between landmark IDs and indexes in the Pkk cov.
Definition: CRangeBearingKFSLAM2D.h:388
mrpt::slam::CRangeBearingKFSLAM2D::m_action
mrpt::obs::CActionCollection::Ptr m_action
Set up by processActionObservation.
Definition: CRangeBearingKFSLAM2D.h:381
mrpt::containers::bimap::getInverseMap
const std::map< VALUE, KEY > & getInverseMap() const
Return a read-only reference to the internal map KEY->VALUES.
Definition: bimap.h:63
mrpt::bayes::CKalmanFilterCapable< 3, 2, 2, 3 >::KFMatrix_VxV
mrpt::math::CMatrixFixedNumeric< double, VEH_SIZE, VEH_SIZE > KFMatrix_VxV
Definition: CKalmanFilterCapable.h:246
mrpt::poses::CPosePDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
Definition: CPosePDFGaussian.h:31
CLandmark.h
mrpt::slam::CRangeBearingKFSLAM2D::TDataAssocInfo::results
TDataAssociationResults results
Definition: CRangeBearingKFSLAM2D.h:185
mrpt::slam::CRangeBearingKFSLAM2D::OnTransitionNoise
void OnTransitionNoise(KFMatrix_VxV &out_Q) const
Implements the transition noise covariance .
Definition: CRangeBearingKFSLAM2D.cpp:280
CSimpleMap.h
mrpt::slam::CRangeBearingKFSLAM2D::TOptions::std_sensor_range
float std_sensor_range
The std.
Definition: CRangeBearingKFSLAM2D.h:130
mrpt::obs::CActionCollection::Ptr
std::shared_ptr< CActionCollection > Ptr
Definition: CActionCollection.h:30
mrpt::slam::CRangeBearingKFSLAM2D::OnPreComputingPredictions
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...
Definition: CRangeBearingKFSLAM2D.cpp:1142
mrpt::slam::CRangeBearingKFSLAM2D::TDataAssocInfo::Y_pred_means
mrpt::math::CMatrixTemplateNumeric< kftype > Y_pred_means
Definition: CRangeBearingKFSLAM2D.h:176
mrpt::bayes::CKalmanFilterCapable
Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.
Definition: CKalmanFilterCapable.h:52



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