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



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019