Main MRPT website > C++ reference
MRPT logo
CRangeBearingKFSLAM2D.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef CRangeBearingKFSLAM2D_H
29 #define CRangeBearingKFSLAM2D_H
30 
31 #include <mrpt/utils/CDebugOutputCapable.h>
32 #include <mrpt/math/CMatrixTemplateNumeric.h>
33 #include <mrpt/math/CVectorTemplate.h>
34 #include <mrpt/utils/CConfigFileBase.h>
35 #include <mrpt/utils/CLoadableOptions.h>
36 #include <mrpt/opengl.h>
37 #include <mrpt/bayes/CKalmanFilterCapable.h>
38 
39 #include <mrpt/utils/safe_pointers.h>
40 #include <mrpt/utils/bimap.h>
41 
42 #include <mrpt/slam/CSensoryFrame.h>
43 #include <mrpt/slam/CActionCollection.h>
44 #include <mrpt/slam/CObservationBearingRange.h>
45 #include <mrpt/poses/CPoint2D.h>
46 #include <mrpt/poses/CPosePDFGaussian.h>
47 #include <mrpt/slam/CLandmark.h>
48 #include <mrpt/slam/CSimpleMap.h>
49 #include <mrpt/slam/CIncrementalMapPartitioner.h>
50 #include <mrpt/slam/data_association.h>
51 
52 #include <mrpt/slam/link_pragmas.h>
53 
54 namespace mrpt
55 {
56  namespace slam
57  {
58  using namespace mrpt::bayes;
59  using namespace mrpt::poses;
60 
61  /** An implementation of EKF-based SLAM with range-bearing sensors, odometry, and a 2D (+heading) robot pose, and 2D landmarks.
62  * The main method is "processActionObservation" which processes pairs of action/observation.
63  *
64  * The following pages describe front-end applications based on this class:
65  * - http://www.mrpt.org/Application:2d-slam-demo
66  * - http://www.mrpt.org/Application:kf-slam
67  *
68  * \sa CRangeBearingKFSLAM \ingroup metric_slam_grp
69  */
70  class SLAM_IMPEXP CRangeBearingKFSLAM2D :
71  public bayes::CKalmanFilterCapable<3 /* x y yaw */, 2 /* range yaw */, 2 /* x y */, 3 /* Ax Ay Ayaw */>
72  // <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, size typename kftype = double>
73  {
74  public:
75  CRangeBearingKFSLAM2D( ); //!< Default constructor
76  virtual ~CRangeBearingKFSLAM2D(); //!< Destructor
77  void reset(); //!< Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,0,0).
78 
79  /** 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.
80  * \param action May contain odometry
81  * \param SF The set of observations, must contain at least one CObservationBearingRange
82  */
83  void processActionObservation(
84  CActionCollectionPtr &action,
85  CSensoryFramePtr &SF );
86 
87  /** Returns the complete mean and cov.
88  * \param out_robotPose The mean & 3x3 covariance matrix of the robot 2D pose
89  * \param out_landmarksPositions One entry for each of the M landmark positions (2D).
90  * \param out_landmarkIDs Each element[index] (for indices of out_landmarksPositions) gives the corresponding landmark ID.
91  * \param out_fullState The complete state vector (3+2M).
92  * \param out_fullCovariance The full (3+2M)x(3+2M) covariance matrix of the filter.
93  * \sa getCurrentRobotPose
94  */
95  void getCurrentState(
96  CPosePDFGaussian &out_robotPose,
97  std::vector<TPoint2D> &out_landmarksPositions,
98  std::map<unsigned int,CLandmark::TLandmarkID> &out_landmarkIDs,
99  CVectorDouble &out_fullState,
100  CMatrixDouble &out_fullCovariance
101  ) const;
102 
103  /** Returns the mean & 3x3 covariance matrix of the robot 2D pose.
104  * \sa getCurrentState
105  */
106  void getCurrentRobotPose(
107  CPosePDFGaussian &out_robotPose ) const;
108 
109  /** Returns a 3D representation of the landmarks in the map and the robot 3D position according to the current filter state.
110  * \param out_objects
111  */
112  void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const;
113 
114  /** Load options from a ini-like file/text
115  */
116  void loadOptions( const mrpt::utils::CConfigFileBase &ini );
117 
118  /** The options for the algorithm
119  */
120  struct SLAM_IMPEXP TOptions : utils::CLoadableOptions
121  {
122  /** Default values
123  */
124  TOptions();
125 
126  /** Load from a config file/text
127  */
128  void loadFromConfigFile(
129  const mrpt::utils::CConfigFileBase &source,
130  const std::string &section);
131 
132  /** This method must display clearly all the contents of the structure in textual form, sending it to a CStream.
133  */
134  void dumpToTextStream(CStream &out) const;
135 
136 
137  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!)
138  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.
139  float quantiles_3D_representation; //!< Default = 3
140  bool create_simplemap; //!< Whether to fill m_SFs (default=false)
141 
142  // Data association:
143  TDataAssociationMethod data_assoc_method;
144  TDataAssociationMetric data_assoc_metric;
145  double data_assoc_IC_chi2_thres; //!< Threshold in [0,1] for the chi2square test for individual compatibility between predictions and observations (default: 0.99)
146  TDataAssociationMetric data_assoc_IC_metric; //!< Whether to use mahalanobis (->chi2 criterion) vs. Matching likelihood.
147  double data_assoc_IC_ml_threshold;//!< Only if data_assoc_IC_metric==ML, the log-ML threshold (Default=0.0)
148 
149  };
150 
151  TOptions options; //!< The options for the algorithm
152 
153 
154  /** Save the current state of the filter (robot pose & map) to a MATLAB script which displays all the elements in 2D
155  */
156  void saveMapAndPath2DRepresentationAsMATLABFile(
157  const std::string &fil,
158  float stdCount=3.0f,
159  const std::string &styleLandmarks = std::string("b"),
160  const std::string &stylePath = std::string("r"),
161  const std::string &styleRobot = std::string("r") ) const;
162 
163 
164  /** Information for data-association:
165  * \sa getLastDataAssociation
166  */
167  struct SLAM_IMPEXP TDataAssocInfo
168  {
169  TDataAssocInfo() :
170  Y_pred_means(0,0),
171  Y_pred_covs(0,0)
172  {
173  }
174 
175  void clear() {
176  results.clear();
177  predictions_IDs.clear();
178  newly_inserted_landmarks.clear();
179  }
180 
181  // Predictions from the map:
182  CMatrixTemplateNumeric<kftype> Y_pred_means,Y_pred_covs;
183  mrpt::vector_size_t predictions_IDs;
184 
185  /** Map from the 0-based index within the last observation and the landmark 0-based index in the map (the robot-map state vector)
186  Only used for stats and so. */
187  std::map<size_t,size_t> newly_inserted_landmarks;
188 
189  // DA results:
190  TDataAssociationResults results;
191  };
192 
193  /** Returns a read-only reference to the information on the last data-association */
194  const TDataAssocInfo & getLastDataAssociation() const {
195  return m_last_data_association;
196  }
197 
198  protected:
199 
200  /** @name Virtual methods for Kalman Filter implementation
201  @{
202  */
203 
204  /** Must return the action vector u.
205  * \param out_u The action vector which will be passed to OnTransitionModel
206  */
207  void OnGetAction( KFArray_ACT &out_u ) const;
208 
209  /** Implements the transition model \f$ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) \f$
210  * \param in_u The vector returned by OnGetAction.
211  * \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$ .
212  * \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
213  */
214  void OnTransitionModel(
215  const KFArray_ACT &in_u,
216  KFArray_VEH &inout_x,
217  bool &out_skipPrediction
218  ) const;
219 
220  /** Implements the transition Jacobian \f$ \frac{\partial f}{\partial x} \f$
221  * \param out_F Must return the Jacobian.
222  * 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).
223  */
224  void OnTransitionJacobian( KFMatrix_VxV &out_F ) const;
225 
226  /** 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.
227  */
228  void OnTransitionJacobianNumericGetIncrements(KFArray_VEH &out_increments) const;
229 
230 
231  /** Implements the transition noise covariance \f$ Q_k \f$
232  * \param out_Q Must return the covariance matrix.
233  * The returned matrix must be of the same size than the jacobian from OnTransitionJacobian
234  */
235  void OnTransitionNoise( KFMatrix_VxV &out_Q ) const;
236 
237  /** 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.
238  *
239  * \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.
240  * \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.
241  * \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".
242  * \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.
243  *
244  * This method will be called just once for each complete KF iteration.
245  * \note It is assumed that the observations are independent, i.e. there are NO cross-covariances between them.
246  */
247  void OnGetObservationsAndDataAssociation(
248  vector_KFArray_OBS &out_z,
249  vector_int &out_data_association,
250  const vector_KFArray_OBS &in_all_predictions,
251  const KFMatrix &in_S,
252  const vector_size_t &in_lm_indices_in_S,
253  const KFMatrix_OxO &in_R
254  );
255 
256  void OnObservationModel(
257  const vector_size_t &idx_landmarks_to_predict,
258  vector_KFArray_OBS &out_predictions
259  ) const;
260 
261  /** Implements the observation Jacobians \f$ \frac{\partial h_i}{\partial x} \f$ and (when applicable) \f$ \frac{\partial h_i}{\partial y_i} \f$.
262  * \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.
263  * \param Hx The output Jacobian \f$ \frac{\partial h_i}{\partial x} \f$.
264  * \param Hy The output Jacobian \f$ \frac{\partial h_i}{\partial y_i} \f$.
265  */
266  void OnObservationJacobians(
267  const size_t &idx_landmark_to_predict,
268  KFMatrix_OxV &Hx,
269  KFMatrix_OxF &Hy
270  ) const;
271 
272  /** 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.
273  */
274  void OnObservationJacobiansNumericGetIncrements(
275  KFArray_VEH &out_veh_increments,
276  KFArray_FEAT &out_feat_increments ) const;
277 
278 
279  /** Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scalar components (eg, angles).
280  */
281  void OnSubstractObservationVectors(KFArray_OBS &A, const KFArray_OBS &B) const;
282 
283  /** Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor.
284  * \param out_R The noise covariance matrix. It might be non diagonal, but it'll usually be.
285  */
286  void OnGetObservationNoise(KFMatrix_OxO &out_R) const;
287 
288  /** This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the number of covariance landmark predictions to be made.
289  * For example, features which are known to be "out of sight" shouldn't be added to the output list to speed up the calculations.
290  * \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.
291  * \param out_LM_indices_to_predict The list of landmark indices in the map [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.
292  * \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.
293  * \sa OnGetObservations, OnDataAssociation
294  */
295  void OnPreComputingPredictions(
296  const vector_KFArray_OBS &in_all_prediction_means,
297  vector_size_t &out_LM_indices_to_predict ) const;
298 
299  /** If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".
300  * \param in_z The observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservations().
301  * \param out_yn The F-length vector with the inverse observation model \f$ y_n=y(x,z_n) \f$.
302  * \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$.
303  * \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$.
304  *
305  * - O: OBS_SIZE
306  * - V: VEH_SIZE
307  * - F: FEAT_SIZE
308  *
309  * \note OnNewLandmarkAddedToMap will be also called after calling this method if a landmark is actually being added to the map.
310  */
311  void OnInverseObservationModel(
312  const KFArray_OBS & in_z,
313  KFArray_FEAT & out_yn,
314  KFMatrix_FxV & out_dyn_dxv,
315  KFMatrix_FxO & out_dyn_dhn ) const;
316 
317  /** If applicable to the given problem, do here any special handling of adding a new landmark to the map.
318  * \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.
319  * \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.
320  * \sa OnInverseObservationModel
321  */
322  void OnNewLandmarkAddedToMap(
323  const size_t in_obsIdx,
324  const size_t in_idxNewFeat );
325 
326 
327  /** 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.
328  */
329  void OnNormalizeStateVector();
330 
331  /** @}
332  */
333 
334 
335  void getLandmarkIDsFromIndexInStateVector(std::map<unsigned int,CLandmark::TLandmarkID> &out_id2index) const
336  {
337  out_id2index = m_IDs.getInverseMap();
338  }
339 
340  protected:
341 
342  /** Set up by processActionObservation
343  */
344  CActionCollectionPtr m_action;
345 
346  /** Set up by processActionObservation
347  */
348  CSensoryFramePtr m_SF;
349 
350  /** The mapping between landmark IDs and indexes in the Pkk cov. matrix:
351  */
352  mrpt::utils::bimap<CLandmark::TLandmarkID,unsigned int> m_IDs;
353 
354  /** The sequence of all the observations and the robot path (kept for debugging, statistics,etc)
355  */
356  CSimpleMap m_SFs;
357 
358  TDataAssocInfo m_last_data_association; //!< Last data association
359 
360 
361  }; // end class
362  } // End of namespace
363 } // End of namespace
364 
365 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013