Main MRPT website > C++ reference
MRPT logo
CRangeBearingKFSLAM2D.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (c) 2005-2013, Individual contributors, see AUTHORS file |
7  | Copyright (c) 2005-2013, MAPIR group, University of Malaga |
8  | Copyright (c) 2012-2013, University of Almeria |
9  | All rights reserved. |
10  | |
11  | Redistribution and use in source and binary forms, with or without |
12  | modification, are permitted provided that the following conditions are |
13  | met: |
14  | * Redistributions of source code must retain the above copyright |
15  | notice, this list of conditions and the following disclaimer. |
16  | * Redistributions in binary form must reproduce the above copyright |
17  | notice, this list of conditions and the following disclaimer in the |
18  | documentation and/or other materials provided with the distribution. |
19  | * Neither the name of the copyright holders nor the |
20  | names of its contributors may be used to endorse or promote products |
21  | derived from this software without specific prior written permission.|
22  | |
23  | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
24  | 'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED |
25  | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR|
26  | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS BE LIABLE |
27  | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL|
28  | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR|
29  | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) |
30  | HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, |
31  | STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
32  | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
33  | POSSIBILITY OF SUCH DAMAGE. |
34  +---------------------------------------------------------------------------+ */
35 #ifndef CRangeBearingKFSLAM2D_H
36 #define CRangeBearingKFSLAM2D_H
37 
43 #include <mrpt/opengl.h>
45 
47 #include <mrpt/utils/bimap.h>
48 
52 #include <mrpt/poses/CPoint2D.h>
54 #include <mrpt/slam/CLandmark.h>
55 #include <mrpt/slam/CSimpleMap.h>
58 
59 #include <mrpt/slam/link_pragmas.h>
60 
61 namespace mrpt
62 {
63  namespace slam
64  {
65  using namespace mrpt::bayes;
66  using namespace mrpt::poses;
67 
68  /** An implementation of EKF-based SLAM with range-bearing sensors, odometry, and a 2D (+heading) robot pose, and 2D landmarks.
69  * The main method is "processActionObservation" which processes pairs of action/observation.
70  *
71  * The following pages describe front-end applications based on this class:
72  * - http://www.mrpt.org/Application:2d-slam-demo
73  * - http://www.mrpt.org/Application:kf-slam
74  *
75  * \sa CRangeBearingKFSLAM \ingroup metric_slam_grp
76  */
78  public bayes::CKalmanFilterCapable<3 /* x y yaw */, 2 /* range yaw */, 2 /* x y */, 3 /* Ax Ay Ayaw */>
79  // <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, size typename kftype = double>
80  {
81  public:
82  CRangeBearingKFSLAM2D( ); //!< Default constructor
83  virtual ~CRangeBearingKFSLAM2D(); //!< Destructor
84  void reset(); //!< Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,0,0).
85 
86  /** Process one new action and observations to update the map and robot pose estimate. See the description of the class at the top of this page.
87  * \param action May contain odometry
88  * \param SF The set of observations, must contain at least one CObservationBearingRange
89  */
90  void processActionObservation(
91  CActionCollectionPtr &action,
92  CSensoryFramePtr &SF );
93 
94  /** Returns the complete mean and cov.
95  * \param out_robotPose The mean & 3x3 covariance matrix of the robot 2D pose
96  * \param out_landmarksPositions One entry for each of the M landmark positions (2D).
97  * \param out_landmarkIDs Each element[index] (for indices of out_landmarksPositions) gives the corresponding landmark ID.
98  * \param out_fullState The complete state vector (3+2M).
99  * \param out_fullCovariance The full (3+2M)x(3+2M) covariance matrix of the filter.
100  * \sa getCurrentRobotPose
101  */
102  void getCurrentState(
103  CPosePDFGaussian &out_robotPose,
104  std::vector<TPoint2D> &out_landmarksPositions,
105  std::map<unsigned int,CLandmark::TLandmarkID> &out_landmarkIDs,
106  CVectorDouble &out_fullState,
107  CMatrixDouble &out_fullCovariance
108  ) const;
109 
110  /** Returns the mean & 3x3 covariance matrix of the robot 2D pose.
111  * \sa getCurrentState
112  */
113  void getCurrentRobotPose(
114  CPosePDFGaussian &out_robotPose ) const;
115 
116  /** Returns a 3D representation of the landmarks in the map and the robot 3D position according to the current filter state.
117  * \param out_objects
118  */
119  void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const;
120 
121  /** Load options from a ini-like file/text
122  */
123  void loadOptions( const mrpt::utils::CConfigFileBase &ini );
124 
125  /** The options for the algorithm
126  */
128  {
129  /** Default values
130  */
131  TOptions();
132 
133  /** Load from a config file/text
134  */
135  void loadFromConfigFile(
136  const mrpt::utils::CConfigFileBase &source,
137  const std::string &section);
138 
139  /** This method must display clearly all the contents of the structure in textual form, sending it to a CStream.
140  */
141  void dumpToTextStream(CStream &out) const;
142 
143 
144  vector_float stds_Q_no_odo; //!< A 3-length vector with the std. deviation of the transition model in (x,y,phi) used only when there is no odometry (if there is odo, its uncertainty values will be used instead); x y: In meters, phi: radians (but in degrees when loading from a configuration ini-file!)
145  float std_sensor_range, std_sensor_yaw; //!< The std. deviation of the sensor (for the matrix R in the kalman filters), in meters and radians.
146  float quantiles_3D_representation; //!< Default = 3
147  bool create_simplemap; //!< Whether to fill m_SFs (default=false)
148 
149  // Data association:
152  double data_assoc_IC_chi2_thres; //!< Threshold in [0,1] for the chi2square test for individual compatibility between predictions and observations (default: 0.99)
153  TDataAssociationMetric data_assoc_IC_metric; //!< Whether to use mahalanobis (->chi2 criterion) vs. Matching likelihood.
154  double data_assoc_IC_ml_threshold;//!< Only if data_assoc_IC_metric==ML, the log-ML threshold (Default=0.0)
155 
156  };
157 
158  TOptions options; //!< The options for the algorithm
159 
160 
161  /** Save the current state of the filter (robot pose & map) to a MATLAB script which displays all the elements in 2D
162  */
163  void saveMapAndPath2DRepresentationAsMATLABFile(
164  const std::string &fil,
165  float stdCount=3.0f,
166  const std::string &styleLandmarks = std::string("b"),
167  const std::string &stylePath = std::string("r"),
168  const std::string &styleRobot = std::string("r") ) const;
169 
170 
171  /** Information for data-association:
172  * \sa getLastDataAssociation
173  */
175  {
177  Y_pred_means(0,0),
178  Y_pred_covs(0,0)
179  {
180  }
181 
182  void clear() {
183  results.clear();
184  predictions_IDs.clear();
185  newly_inserted_landmarks.clear();
186  }
187 
188  // Predictions from the map:
189  CMatrixTemplateNumeric<kftype> Y_pred_means,Y_pred_covs;
191 
192  /** Map from the 0-based index within the last observation and the landmark 0-based index in the map (the robot-map state vector)
193  Only used for stats and so. */
194  std::map<size_t,size_t> newly_inserted_landmarks;
195 
196  // DA results:
198  };
199 
200  /** Returns a read-only reference to the information on the last data-association */
202  return m_last_data_association;
203  }
204 
205  protected:
206 
207  /** @name Virtual methods for Kalman Filter implementation
208  @{
209  */
210 
211  /** Must return the action vector u.
212  * \param out_u The action vector which will be passed to OnTransitionModel
213  */
214  void OnGetAction( KFArray_ACT &out_u ) const;
215 
216  /** Implements the transition model \f$ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) \f$
217  * \param in_u The vector returned by OnGetAction.
218  * \param inout_x At input has \f[ \hat{x}_{k-1|k-1} \f] , at output must have \f$ \hat{x}_{k|k-1} \f$ .
219  * \param out_skip Set this to true if for some reason you want to skip the prediction step (to do not modify either the vector or the covariance). Default:false
220  */
221  void OnTransitionModel(
222  const KFArray_ACT &in_u,
223  KFArray_VEH &inout_x,
224  bool &out_skipPrediction
225  ) const;
226 
227  /** Implements the transition Jacobian \f$ \frac{\partial f}{\partial x} \f$
228  * \param out_F Must return the Jacobian.
229  * The returned matrix must be \f$V \times V\f$ with V being either the size of the whole state vector (for non-SLAM problems) or VEH_SIZE (for SLAM problems).
230  */
231  void OnTransitionJacobian( KFMatrix_VxV &out_F ) const;
232 
233  /** Only called if using a numeric approximation of the transition Jacobian, this method must return the increments in each dimension of the vehicle state vector while estimating the Jacobian.
234  */
235  void OnTransitionJacobianNumericGetIncrements(KFArray_VEH &out_increments) const;
236 
237 
238  /** Implements the transition noise covariance \f$ Q_k \f$
239  * \param out_Q Must return the covariance matrix.
240  * The returned matrix must be of the same size than the jacobian from OnTransitionJacobian
241  */
242  void OnTransitionNoise( KFMatrix_VxV &out_Q ) const;
243 
244  /** This is called between the KF prediction step and the update step, and the application must return the observations and, when applicable, the data association between these observations and the current map.
245  *
246  * \param out_z N vectors, each for one "observation" of length OBS_SIZE, N being the number of "observations": how many observed landmarks for a map, or just one if not applicable.
247  * \param out_data_association An empty vector or, where applicable, a vector where the i'th element corresponds to the position of the observation in the i'th row of out_z within the system state vector (in the range [0,getNumberOfLandmarksInTheMap()-1]), or -1 if it is a new map element and we want to insert it at the end of this KF iteration.
248  * \param in_S The full covariance matrix of the observation predictions (i.e. the "innovation covariance matrix"). This is a M·O x M·O matrix with M=length of "in_lm_indices_in_S".
249  * \param in_lm_indices_in_S The indices of the map landmarks (range [0,getNumberOfLandmarksInTheMap()-1]) that can be found in the matrix in_S.
250  *
251  * This method will be called just once for each complete KF iteration.
252  * \note It is assumed that the observations are independent, i.e. there are NO cross-covariances between them.
253  */
254  void OnGetObservationsAndDataAssociation(
255  vector_KFArray_OBS &out_z,
256  vector_int &out_data_association,
257  const vector_KFArray_OBS &in_all_predictions,
258  const KFMatrix &in_S,
259  const vector_size_t &in_lm_indices_in_S,
260  const KFMatrix_OxO &in_R
261  );
262 
263  void OnObservationModel(
264  const vector_size_t &idx_landmarks_to_predict,
265  vector_KFArray_OBS &out_predictions
266  ) const;
267 
268  /** Implements the observation Jacobians \f$ \frac{\partial h_i}{\partial x} \f$ and (when applicable) \f$ \frac{\partial h_i}{\partial y_i} \f$.
269  * \param idx_landmark_to_predict The index of the landmark in the map whose prediction is expected as output. For non SLAM-like problems, this will be zero and the expected output is for the whole state vector.
270  * \param Hx The output Jacobian \f$ \frac{\partial h_i}{\partial x} \f$.
271  * \param Hy The output Jacobian \f$ \frac{\partial h_i}{\partial y_i} \f$.
272  */
273  void OnObservationJacobians(
274  const size_t &idx_landmark_to_predict,
275  KFMatrix_OxV &Hx,
276  KFMatrix_OxF &Hy
277  ) const;
278 
279  /** Only called if using a numeric approximation of the observation Jacobians, this method must return the increments in each dimension of the vehicle state vector while estimating the Jacobian.
280  */
281  void OnObservationJacobiansNumericGetIncrements(
282  KFArray_VEH &out_veh_increments,
283  KFArray_FEAT &out_feat_increments ) const;
284 
285 
286  /** Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scalar components (eg, angles).
287  */
288  void OnSubstractObservationVectors(KFArray_OBS &A, const KFArray_OBS &B) const;
289 
290  /** Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor.
291  * \param out_R The noise covariance matrix. It might be non diagonal, but it'll usually be.
292  */
293  void OnGetObservationNoise(KFMatrix_OxO &out_R) const;
294 
295  /** This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the number of covariance landmark predictions to be made.
296  * For example, features which are known to be "out of sight" shouldn't be added to the output list to speed up the calculations.
297  * \param in_all_prediction_means The mean of each landmark predictions; the computation or not of the corresponding covariances is what we're trying to determined with this method.
298  * \param out_LM_indices_to_predict The list of landmark indices in the map [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.
299  * \note This is not a pure virtual method, so it should be implemented only if desired. The default implementation returns a vector with all the landmarks in the map.
300  * \sa OnGetObservations, OnDataAssociation
301  */
302  void OnPreComputingPredictions(
303  const vector_KFArray_OBS &in_all_prediction_means,
304  vector_size_t &out_LM_indices_to_predict ) const;
305 
306  /** If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".
307  * \param in_z The observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservations().
308  * \param out_yn The F-length vector with the inverse observation model \f$ y_n=y(x,z_n) \f$.
309  * \param out_dyn_dxv The \f$F \times V\f$ Jacobian of the inv. sensor model wrt the robot pose \f$ \frac{\partial y_n}{\partial x_v} \f$.
310  * \param out_dyn_dhn The \f$F \times O\f$ Jacobian of the inv. sensor model wrt the observation vector \f$ \frac{\partial y_n}{\partial h_n} \f$.
311  *
312  * - O: OBS_SIZE
313  * - V: VEH_SIZE
314  * - F: FEAT_SIZE
315  *
316  * \note OnNewLandmarkAddedToMap will be also called after calling this method if a landmark is actually being added to the map.
317  */
318  void OnInverseObservationModel(
319  const KFArray_OBS & in_z,
320  KFArray_FEAT & out_yn,
321  KFMatrix_FxV & out_dyn_dxv,
322  KFMatrix_FxO & out_dyn_dhn ) const;
323 
324  /** If applicable to the given problem, do here any special handling of adding a new landmark to the map.
325  * \param in_obsIndex The index of the observation whose inverse sensor is to be computed. It corresponds to the row in in_z where the observation can be found.
326  * \param in_idxNewFeat The index that this new feature will have in the state vector (0:just after the vehicle state, 1: after that,...). Save this number so data association can be done according to these indices.
327  * \sa OnInverseObservationModel
328  */
329  void OnNewLandmarkAddedToMap(
330  const size_t in_obsIdx,
331  const size_t in_idxNewFeat );
332 
333 
334  /** This method is called after the prediction and after the update, to give the user an opportunity to normalize the state vector (eg, keep angles within -pi,pi range) if the application requires it.
335  */
336  void OnNormalizeStateVector();
337 
338  /** @}
339  */
340 
342  void getLandmarkIDsFromIndexInStateVector(std::map<unsigned int,CLandmark::TLandmarkID> &out_id2index) const
343  {
344  out_id2index = m_IDs.getInverseMap();
345  }
346 
347  protected:
348 
349  /** Set up by processActionObservation
350  */
351  CActionCollectionPtr m_action;
352 
353  /** Set up by processActionObservation
354  */
355  CSensoryFramePtr m_SF;
356 
357  /** The mapping between landmark IDs and indexes in the Pkk cov. matrix:
358  */
360 
361  /** The sequence of all the observations and the robot path (kept for debugging, statistics,etc)
362  */
363  CSimpleMap m_SFs;
365  TDataAssocInfo m_last_data_association; //!< Last data association
366 
367 
368  }; // end class
369  } // End of namespace
370 } // End of namespace
371 
372 #endif
An implementation of EKF-based SLAM with range-bearing sensors, odometry, and a 2D (+heading) robot p...
TOptions options
The options for the algorithm.
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)
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
vector_float 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...
mrpt::vector_double CVectorDouble
This is just another name for mrpt::vector_double (Backward compatibility with MRPT <=0...
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...
Definition: CStream.h:62
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
std::vector< size_t > vector_size_t
TDataAssociationMetric
Different metrics for data association, used in mrpt::slam::data_association For a comparison of both...
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.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:59
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...
std::vector< int32_t > vector_int
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.
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
The results from mrpt::slam::data_association.
TDataAssociationMethod
Different algorithms for data association, used in mrpt::slam::data_association.
const TDataAssocInfo & getLastDataAssociation() const
Returns a read-only reference to the information on the last data-association.
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...



Page generated by Doxygen 1.8.14 for MRPT 1.0.2 SVN: at lun oct 28 00:52:41 CET 2019 Hosted on:
SourceForge.net Logo