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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7e629e01a Sat Dec 14 00:05:55 2019 +0100 at sáb dic 14 00:15:10 CET 2019