Main MRPT website > C++ reference
MRPT logo
CRangeBearingKFSLAM.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 CRangeBearingKFSLAM_H
29 #define CRangeBearingKFSLAM_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/CPoint3D.h>
46 #include <mrpt/poses/CPose3DPDFGaussian.h>
47 #include <mrpt/poses/CPose3DQuatPDFGaussian.h>
48 #include <mrpt/slam/CLandmark.h>
49 #include <mrpt/slam/CSimpleMap.h>
50 #include <mrpt/slam/CIncrementalMapPartitioner.h>
51 #include <mrpt/slam/data_association.h>
52 
53 #include <mrpt/slam/link_pragmas.h>
54 
55 namespace mrpt
56 {
57  namespace slam
58  {
59  using namespace mrpt::bayes;
60 
61  /** An implementation of EKF-based SLAM with range-bearing sensors, odometry, a full 6D robot pose, and 3D landmarks.
62  * The main method is "processActionObservation" which processes pairs of action/observation.
63  * The state vector comprises: 3D robot position, a quaternion for its attitude, and the 3D landmarks in the map.
64  *
65  * The following Wiki page describes an front-end application based on this class:
66  * http://www.mrpt.org/Application:kf-slam
67  *
68  * For the theory behind this implementation, see the technical report in:
69  * http://www.mrpt.org/6D-SLAM
70  *
71  * \sa An implementation for 2D only: CRangeBearingKFSLAM2D
72  * \ingroup metric_slam_grp
73  */
74  class SLAM_IMPEXP CRangeBearingKFSLAM :
75  public bayes::CKalmanFilterCapable<7 /* x y z qr qx qy qz*/,3 /* range yaw pitch */, 3 /* x y z */, 7 /* Ax Ay Az Aqr Aqx Aqy Aqz */ >
76  // <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, size typename kftype = double>
77  {
78  public:
79  /** Constructor.
80  */
81  CRangeBearingKFSLAM( );
82 
83  /** Destructor:
84  */
85  virtual ~CRangeBearingKFSLAM();
86 
87  void reset(); //!< Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,0,0).
88 
89  /** 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.
90  * \param action May contain odometry
91  * \param SF The set of observations, must contain at least one CObservationBearingRange
92  */
93  void processActionObservation(
94  CActionCollectionPtr &action,
95  CSensoryFramePtr &SF );
96 
97  /** Returns the complete mean and cov.
98  * \param out_robotPose The mean and the 7x7 covariance matrix of the robot 6D pose
99  * \param out_landmarksPositions One entry for each of the M landmark positions (3D).
100  * \param out_landmarkIDs Each element[index] (for indices of out_landmarksPositions) gives the corresponding landmark ID.
101  * \param out_fullState The complete state vector (7+3M).
102  * \param out_fullCovariance The full (7+3M)x(7+3M) covariance matrix of the filter.
103  * \sa getCurrentRobotPose
104  */
105  void getCurrentState(
106  CPose3DQuatPDFGaussian &out_robotPose,
107  std::vector<CPoint3D> &out_landmarksPositions,
108  std::map<unsigned int,CLandmark::TLandmarkID> &out_landmarkIDs,
109  CVectorDouble &out_fullState,
110  CMatrixDouble &out_fullCovariance
111  ) const;
112 
113  /** Returns the complete mean and cov.
114  * \param out_robotPose The mean and the 7x7 covariance matrix of the robot 6D pose
115  * \param out_landmarksPositions One entry for each of the M landmark positions (3D).
116  * \param out_landmarkIDs Each element[index] (for indices of out_landmarksPositions) gives the corresponding landmark ID.
117  * \param out_fullState The complete state vector (7+3M).
118  * \param out_fullCovariance The full (7+3M)x(7+3M) covariance matrix of the filter.
119  * \sa getCurrentRobotPose
120  */
121  inline void getCurrentState(
122  CPose3DPDFGaussian &out_robotPose,
123  std::vector<CPoint3D> &out_landmarksPositions,
124  std::map<unsigned int,CLandmark::TLandmarkID> &out_landmarkIDs,
125  CVectorDouble &out_fullState,
126  CMatrixDouble &out_fullCovariance
127  ) const
128  {
129  CPose3DQuatPDFGaussian q(UNINITIALIZED_QUATERNION);
130  this->getCurrentState(q,out_landmarksPositions,out_landmarkIDs,out_fullState,out_fullCovariance);
131  out_robotPose = CPose3DPDFGaussian(q);
132  }
133 
134  /** Returns the mean & the 7x7 covariance matrix of the robot 6D pose (with rotation as a quaternion).
135  * \sa getCurrentState, getCurrentRobotPoseMean
136  */
137  void getCurrentRobotPose( CPose3DQuatPDFGaussian &out_robotPose ) const;
138 
139  /** Get the current robot pose mean, as a 3D+quaternion pose.
140  * \sa getCurrentRobotPose
141  */
142  mrpt::poses::CPose3DQuat getCurrentRobotPoseMean() const;
143 
144  /** Returns the mean & the 6x6 covariance matrix of the robot 6D pose (with rotation as 3 angles).
145  * \sa getCurrentState
146  */
147  inline void getCurrentRobotPose( CPose3DPDFGaussian &out_robotPose ) const
148  {
149  CPose3DQuatPDFGaussian q(UNINITIALIZED_QUATERNION);
150  this->getCurrentRobotPose(q);
151  out_robotPose = CPose3DPDFGaussian(q);
152  }
153 
154  /** Returns a 3D representation of the landmarks in the map and the robot 3D position according to the current filter state.
155  * \param out_objects
156  */
157  void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const;
158 
159  /** Load options from a ini-like file/text
160  */
161  void loadOptions( const mrpt::utils::CConfigFileBase &ini );
162 
163  /** The options for the algorithm
164  */
165  struct SLAM_IMPEXP TOptions : utils::CLoadableOptions
166  {
167  /** Default values
168  */
169  TOptions();
170 
171  /** Load from a config file/text
172  */
173  void loadFromConfigFile(
174  const mrpt::utils::CConfigFileBase &source,
175  const std::string &section);
176 
177  /** This method must display clearly all the contents of the structure in textual form, sending it to a CStream.
178  */
179  void dumpToTextStream(CStream &out) const;
180 
181  /** A 7-length vector with the std. deviation of the transition model in (x,y,z, qr,qx,qy,qz) used only when there is no odometry (if there is odo, its uncertainty values will be used instead); x y z: In meters.
182  */
183  vector_float stds_Q_no_odo;
184 
185  /** The std. deviation of the sensor (for the matrix R in the kalman filters), in meters and radians.
186  */
187  float std_sensor_range, std_sensor_yaw, std_sensor_pitch;
188 
189  /** Additional std. dev. to sum to the motion model in the z axis (useful when there is only 2D odometry and we want to put things hard to the algorithm) (default=0)
190  */
191  float std_odo_z_additional;
192 
193  /** If set to true (default=false), map will be partitioned using the method stated by partitioningMethod
194  */
195  bool doPartitioningExperiment;
196 
197  /** Default = 3
198  */
199  float quantiles_3D_representation;
200 
201  /** Applicable only if "doPartitioningExperiment=true".
202  * 0: Automatically detect partition through graph-cut.
203  * N>=1: Cut every "N" observations.
204  */
205  int partitioningMethod;
206 
207  // Data association:
208  TDataAssociationMethod data_assoc_method;
209  TDataAssociationMetric data_assoc_metric;
210  double data_assoc_IC_chi2_thres; //!< Threshold in [0,1] for the chi2square test for individual compatibility between predictions and observations (default: 0.99)
211  TDataAssociationMetric data_assoc_IC_metric; //!< Whether to use mahalanobis (->chi2 criterion) vs. Matching likelihood.
212  double data_assoc_IC_ml_threshold;//!< Only if data_assoc_IC_metric==ML, the log-ML threshold (Default=0.0)
213 
214  bool create_simplemap; //!< Whether to fill m_SFs (default=false)
215 
216  bool force_ignore_odometry; //!< Whether to ignore the input odometry and behave as if there was no odometry at all (default: false)
217  } options;
218 
219  /** Information for data-association:
220  * \sa getLastDataAssociation
221  */
222  struct SLAM_IMPEXP TDataAssocInfo
223  {
224  TDataAssocInfo() :
225  Y_pred_means(0,0),
226  Y_pred_covs(0,0)
227  {
228  }
229 
230  void clear() {
231  results.clear();
232  predictions_IDs.clear();
233  newly_inserted_landmarks.clear();
234  }
235 
236  // Predictions from the map:
237  CMatrixTemplateNumeric<kftype> Y_pred_means,Y_pred_covs;
238  mrpt::vector_size_t predictions_IDs;
239 
240  /** Map from the 0-based index within the last observation and the landmark 0-based index in the map (the robot-map state vector)
241  Only used for stats and so. */
242  std::map<size_t,size_t> newly_inserted_landmarks;
243 
244  // DA results:
245  TDataAssociationResults results;
246  };
247 
248  /** Returns a read-only reference to the information on the last data-association */
249  const TDataAssocInfo & getLastDataAssociation() const {
250  return m_last_data_association;
251  }
252 
253 
254  /** Return the last partition of the sequence of sensoryframes (it is NOT a partition of the map!!)
255  * Only if options.doPartitioningExperiment = true
256  * \sa getLastPartitionLandmarks
257  */
258  void getLastPartition( std::vector<vector_uint> &parts )
259  {
260  parts = m_lastPartitionSet;
261  }
262 
263  /** Return the partitioning of the landmarks in clusters accoring to the last partition.
264  * Note that the same landmark may appear in different clusters (the partition is not in the space of landmarks)
265  * Only if options.doPartitioningExperiment = true
266  * \param landmarksMembership The i'th element of this vector is the set of clusters to which the i'th landmark in the map belongs to (landmark index != landmark ID !!).
267  * \sa getLastPartition
268  */
269  void getLastPartitionLandmarks( std::vector<vector_uint> &landmarksMembership ) const;
270 
271  /** For testing only: returns the partitioning as "getLastPartitionLandmarks" but as if a fixed-size submaps (size K) were have been used.
272  */
273  void getLastPartitionLandmarksAsIfFixedSubmaps( size_t K, std::vector<vector_uint> &landmarksMembership );
274 
275 
276  /** Computes the ratio of the missing information matrix elements which are ignored under a certain partitioning of the landmarks.
277  * \sa getLastPartitionLandmarks, getLastPartitionLandmarksAsIfFixedSubmaps
278  */
279  double computeOffDiagonalBlocksApproximationError( const std::vector<vector_uint> &landmarksMembership ) const;
280 
281 
282  /** The partitioning of the entire map is recomputed again.
283  * Only when options.doPartitioningExperiment = true.
284  * This can be used after changing the parameters of the partitioning method.
285  * After this method, you can call getLastPartitionLandmarks.
286  * \sa getLastPartitionLandmarks
287  */
288  void reconsiderPartitionsNow();
289 
290 
291  /** Provides access to the parameters of the map partitioning algorithm.
292  */
293  CIncrementalMapPartitioner::TOptions * mapPartitionOptions()
294  {
295  return &mapPartitioner.options;
296  }
297 
298  /** Save the current state of the filter (robot pose & map) to a MATLAB script which displays all the elements in 2D
299  */
300  void saveMapAndPath2DRepresentationAsMATLABFile(
301  const std::string &fil,
302  float stdCount=3.0f,
303  const std::string &styleLandmarks = std::string("b"),
304  const std::string &stylePath = std::string("r"),
305  const std::string &styleRobot = std::string("r") ) const;
306 
307 
308 
309  protected:
310 
311  /** @name Virtual methods for Kalman Filter implementation
312  @{
313  */
314 
315  /** Must return the action vector u.
316  * \param out_u The action vector which will be passed to OnTransitionModel
317  */
318  void OnGetAction( KFArray_ACT &out_u ) const;
319 
320  /** Implements the transition model \f$ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) \f$
321  * \param in_u The vector returned by OnGetAction.
322  * \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$ .
323  * \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
324  */
325  void OnTransitionModel(
326  const KFArray_ACT &in_u,
327  KFArray_VEH &inout_x,
328  bool &out_skipPrediction
329  ) const;
330 
331  /** Implements the transition Jacobian \f$ \frac{\partial f}{\partial x} \f$
332  * \param out_F Must return the Jacobian.
333  * 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).
334  */
335  void OnTransitionJacobian( KFMatrix_VxV &out_F ) const;
336 
337  /** Implements the transition noise covariance \f$ Q_k \f$
338  * \param out_Q Must return the covariance matrix.
339  * The returned matrix must be of the same size than the jacobian from OnTransitionJacobian
340  */
341  void OnTransitionNoise( KFMatrix_VxV &out_Q ) const;
342 
343  /** 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.
344  *
345  * \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.
346  * \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.
347  * \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".
348  * \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.
349  *
350  * This method will be called just once for each complete KF iteration.
351  * \note It is assumed that the observations are independent, i.e. there are NO cross-covariances between them.
352  */
353  void OnGetObservationsAndDataAssociation(
354  vector_KFArray_OBS &out_z,
355  vector_int &out_data_association,
356  const vector_KFArray_OBS &in_all_predictions,
357  const KFMatrix &in_S,
358  const vector_size_t &in_lm_indices_in_S,
359  const KFMatrix_OxO &in_R
360  );
361 
362  void OnObservationModel(
363  const vector_size_t &idx_landmarks_to_predict,
364  vector_KFArray_OBS &out_predictions
365  ) const;
366 
367  /** Implements the observation Jacobians \f$ \frac{\partial h_i}{\partial x} \f$ and (when applicable) \f$ \frac{\partial h_i}{\partial y_i} \f$.
368  * \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.
369  * \param Hx The output Jacobian \f$ \frac{\partial h_i}{\partial x} \f$.
370  * \param Hy The output Jacobian \f$ \frac{\partial h_i}{\partial y_i} \f$.
371  */
372  void OnObservationJacobians(
373  const size_t &idx_landmark_to_predict,
374  KFMatrix_OxV &Hx,
375  KFMatrix_OxF &Hy
376  ) const;
377 
378  /** Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scalar components (eg, angles).
379  */
380  void OnSubstractObservationVectors(KFArray_OBS &A, const KFArray_OBS &B) const;
381 
382  /** Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor.
383  * \param out_R The noise covariance matrix. It might be non diagonal, but it'll usually be.
384  */
385  void OnGetObservationNoise(KFMatrix_OxO &out_R) const;
386 
387  /** This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the number of covariance landmark predictions to be made.
388  * For example, features which are known to be "out of sight" shouldn't be added to the output list to speed up the calculations.
389  * \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.
390  * \param out_LM_indices_to_predict The list of landmark indices in the map [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.
391  * \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.
392  * \sa OnGetObservations, OnDataAssociation
393  */
394  void OnPreComputingPredictions(
395  const vector_KFArray_OBS &in_all_prediction_means,
396  vector_size_t &out_LM_indices_to_predict ) const;
397 
398  /** If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".
399  * \param in_z The observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservations().
400  * \param out_yn The F-length vector with the inverse observation model \f$ y_n=y(x,z_n) \f$.
401  * \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$.
402  * \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$.
403  *
404  * - O: OBS_SIZE
405  * - V: VEH_SIZE
406  * - F: FEAT_SIZE
407  *
408  * \note OnNewLandmarkAddedToMap will be also called after calling this method if a landmark is actually being added to the map.
409  */
410  void OnInverseObservationModel(
411  const KFArray_OBS & in_z,
412  KFArray_FEAT & out_yn,
413  KFMatrix_FxV & out_dyn_dxv,
414  KFMatrix_FxO & out_dyn_dhn ) const;
415 
416  /** If applicable to the given problem, do here any special handling of adding a new landmark to the map.
417  * \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.
418  * \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.
419  * \sa OnInverseObservationModel
420  */
421  void OnNewLandmarkAddedToMap(
422  const size_t in_obsIdx,
423  const size_t in_idxNewFeat );
424 
425 
426  /** 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.
427  */
428  void OnNormalizeStateVector();
429 
430  /** @}
431  */
432 
433  /** Set up by processActionObservation
434  */
435  CActionCollectionPtr m_action;
436 
437  /** Set up by processActionObservation
438  */
439  CSensoryFramePtr m_SF;
440 
441  /** The mapping between landmark IDs and indexes in the Pkk cov. matrix:
442  */
443  mrpt::utils::bimap<CLandmark::TLandmarkID,unsigned int> m_IDs;
444 
445 
446  /** Used for map partitioning experiments
447  */
448  CIncrementalMapPartitioner mapPartitioner;
449 
450  /** The sequence of all the observations and the robot path (kept for debugging, statistics,etc)
451  */
452  CSimpleMap m_SFs;
453 
454  std::vector<vector_uint> m_lastPartitionSet;
455 
456  TDataAssocInfo m_last_data_association; //!< Last data association
457 
458  /** Return the last odometry, as a pose increment. */
459  mrpt::poses::CPose3DQuat getIncrementFromOdometry() const;
460 
461 
462  }; // end class
463  } // End of namespace
464 } // End of namespace
465 
466 
467 
468 
469 #endif


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