Main MRPT website > C++ reference for MRPT 1.5.7
CRangeBearingKFSLAM2D.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 CRangeBearingKFSLAM2D_H
10 #define CRangeBearingKFSLAM2D_H
11 
17 
19 #include <mrpt/utils/bimap.h>
20 
21 #include <mrpt/obs/CSensoryFrame.h>
25 #include <mrpt/maps/CLandmark.h>
26 #include <mrpt/maps/CSimpleMap.h>
29 
30 #include <mrpt/slam/link_pragmas.h>
31 
32 namespace mrpt
33 {
34  namespace slam
35  {
36  /** An implementation of EKF-based SLAM with range-bearing sensors, odometry, and a 2D (+heading) robot pose, and 2D landmarks.
37  * The main method is "processActionObservation" which processes pairs of action/observation.
38  *
39  * The following pages describe front-end applications based on this class:
40  * - http://www.mrpt.org/Application:2d-slam-demo
41  * - http://www.mrpt.org/Application:kf-slam
42  *
43  * \sa CRangeBearingKFSLAM \ingroup metric_slam_grp
44  */
46  public bayes::CKalmanFilterCapable<3 /* x y yaw */, 2 /* range yaw */, 2 /* x y */, 3 /* Ax Ay Ayaw */>
47  // <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, size typename kftype = double>
48  {
49  public:
50  typedef mrpt::math::TPoint2D landmark_point_t; //!< Either mrpt::math::TPoint2D or mrpt::math::TPoint3D
51 
52  CRangeBearingKFSLAM2D( ); //!< Default constructor
53  virtual ~CRangeBearingKFSLAM2D(); //!< Destructor
54  void reset(); //!< Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,0,0).
55 
56  /** 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.
57  * \param action May contain odometry
58  * \param SF The set of observations, must contain at least one CObservationBearingRange
59  */
60  void processActionObservation(
61  mrpt::obs::CActionCollectionPtr &action,
62  mrpt::obs::CSensoryFramePtr &SF );
63 
64  /** Returns the complete mean and cov.
65  * \param out_robotPose The mean & 3x3 covariance matrix of the robot 2D pose
66  * \param out_landmarksPositions One entry for each of the M landmark positions (2D).
67  * \param out_landmarkIDs Each element[index] (for indices of out_landmarksPositions) gives the corresponding landmark ID.
68  * \param out_fullState The complete state vector (3+2M).
69  * \param out_fullCovariance The full (3+2M)x(3+2M) covariance matrix of the filter.
70  * \sa getCurrentRobotPose
71  */
72  void getCurrentState(
73  mrpt::poses::CPosePDFGaussian &out_robotPose,
74  std::vector<mrpt::math::TPoint2D> &out_landmarksPositions,
75  std::map<unsigned int,mrpt::maps::CLandmark::TLandmarkID> &out_landmarkIDs,
76  mrpt::math::CVectorDouble &out_fullState,
77  mrpt::math::CMatrixDouble &out_fullCovariance
78  ) const;
79 
80  /** Returns the mean & 3x3 covariance matrix of the robot 2D pose.
81  * \sa getCurrentState
82  */
83  void getCurrentRobotPose(
84  mrpt::poses::CPosePDFGaussian &out_robotPose ) const;
85 
86  /** Returns a 3D representation of the landmarks in the map and the robot 3D position according to the current filter state.
87  * \param out_objects
88  */
89  void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const;
90 
91  /** Load options from a ini-like file/text
92  */
93  void loadOptions( const mrpt::utils::CConfigFileBase &ini );
94 
95  /** The options for the algorithm
96  */
98  {
99  /** Default values */
100  TOptions();
101 
102  void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
103  void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs
104 
105  mrpt::math::CVectorFloat 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!)
106  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.
107  float quantiles_3D_representation; //!< Default = 3
108  bool create_simplemap; //!< Whether to fill m_SFs (default=false)
109 
110  // Data association:
113  double data_assoc_IC_chi2_thres; //!< Threshold in [0,1] for the chi2square test for individual compatibility between predictions and observations (default: 0.99)
114  TDataAssociationMetric data_assoc_IC_metric; //!< Whether to use mahalanobis (->chi2 criterion) vs. Matching likelihood.
115  double data_assoc_IC_ml_threshold;//!< Only if data_assoc_IC_metric==ML, the log-ML threshold (Default=0.0)
116 
117  };
118 
119  TOptions options; //!< The options for the algorithm
120 
121 
122  /** Save the current state of the filter (robot pose & map) to a MATLAB script which displays all the elements in 2D
123  */
124  void saveMapAndPath2DRepresentationAsMATLABFile(
125  const std::string &fil,
126  float stdCount=3.0f,
127  const std::string &styleLandmarks = std::string("b"),
128  const std::string &stylePath = std::string("r"),
129  const std::string &styleRobot = std::string("r") ) const;
130 
131 
132  /** Information for data-association:
133  * \sa getLastDataAssociation
134  */
136  {
138  Y_pred_means(0,0),
139  Y_pred_covs(0,0)
140  {
141  }
142 
143  void clear() {
144  results.clear();
145  predictions_IDs.clear();
146  newly_inserted_landmarks.clear();
147  }
148 
149  // Predictions from the map:
152 
153  /** Map from the 0-based index within the last observation and the landmark 0-based index in the map (the robot-map state vector)
154  Only used for stats and so. */
155  std::map<size_t,size_t> newly_inserted_landmarks;
156 
157  // DA results:
159  };
160 
161  /** Returns a read-only reference to the information on the last data-association */
163  return m_last_data_association;
164  }
165 
166  protected:
167 
168  /** @name Virtual methods for Kalman Filter implementation
169  @{
170  */
171 
172  /** Must return the action vector u.
173  * \param out_u The action vector which will be passed to OnTransitionModel
174  */
175  void OnGetAction( KFArray_ACT &out_u ) const;
176 
177  /** Implements the transition model \f$ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) \f$
178  * \param in_u The vector returned by OnGetAction.
179  * \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$ .
180  * \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
181  */
182  void OnTransitionModel(
183  const KFArray_ACT &in_u,
184  KFArray_VEH &inout_x,
185  bool &out_skipPrediction
186  ) const;
187 
188  /** Implements the transition Jacobian \f$ \frac{\partial f}{\partial x} \f$
189  * \param out_F Must return the Jacobian.
190  * 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).
191  */
192  void OnTransitionJacobian( KFMatrix_VxV &out_F ) const;
193 
194  /** 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.
195  */
196  void OnTransitionJacobianNumericGetIncrements(KFArray_VEH &out_increments) const;
197 
198 
199  /** Implements the transition noise covariance \f$ Q_k \f$
200  * \param out_Q Must return the covariance matrix.
201  * The returned matrix must be of the same size than the jacobian from OnTransitionJacobian
202  */
203  void OnTransitionNoise( KFMatrix_VxV &out_Q ) const;
204 
205  /** 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.
206  *
207  * \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.
208  * \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.
209  * \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".
210  * \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.
211  *
212  * This method will be called just once for each complete KF iteration.
213  * \note It is assumed that the observations are independent, i.e. there are NO cross-covariances between them.
214  */
215  void OnGetObservationsAndDataAssociation(
216  vector_KFArray_OBS &out_z,
217  vector_int &out_data_association,
218  const vector_KFArray_OBS &in_all_predictions,
219  const KFMatrix &in_S,
220  const vector_size_t &in_lm_indices_in_S,
221  const KFMatrix_OxO &in_R
222  );
223 
224  void OnObservationModel(
225  const vector_size_t &idx_landmarks_to_predict,
226  vector_KFArray_OBS &out_predictions
227  ) const;
228 
229  /** Implements the observation Jacobians \f$ \frac{\partial h_i}{\partial x} \f$ and (when applicable) \f$ \frac{\partial h_i}{\partial y_i} \f$.
230  * \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.
231  * \param Hx The output Jacobian \f$ \frac{\partial h_i}{\partial x} \f$.
232  * \param Hy The output Jacobian \f$ \frac{\partial h_i}{\partial y_i} \f$.
233  */
234  void OnObservationJacobians(
235  const size_t &idx_landmark_to_predict,
236  KFMatrix_OxV &Hx,
237  KFMatrix_OxF &Hy
238  ) const;
239 
240  /** 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.
241  */
242  void OnObservationJacobiansNumericGetIncrements(
243  KFArray_VEH &out_veh_increments,
244  KFArray_FEAT &out_feat_increments ) const;
245 
246 
247  /** Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scalar components (eg, angles).
248  */
249  void OnSubstractObservationVectors(KFArray_OBS &A, const KFArray_OBS &B) const;
250 
251  /** Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor.
252  * \param out_R The noise covariance matrix. It might be non diagonal, but it'll usually be.
253  */
254  void OnGetObservationNoise(KFMatrix_OxO &out_R) const;
255 
256  /** This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the number of covariance landmark predictions to be made.
257  * For example, features which are known to be "out of sight" shouldn't be added to the output list to speed up the calculations.
258  * \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.
259  * \param out_LM_indices_to_predict The list of landmark indices in the map [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.
260  * \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.
261  * \sa OnGetObservations, OnDataAssociation
262  */
263  void OnPreComputingPredictions(
264  const vector_KFArray_OBS &in_all_prediction_means,
265  vector_size_t &out_LM_indices_to_predict ) const;
266 
267  /** If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".
268  * \param in_z The observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservations().
269  * \param out_yn The F-length vector with the inverse observation model \f$ y_n=y(x,z_n) \f$.
270  * \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$.
271  * \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$.
272  *
273  * - O: OBS_SIZE
274  * - V: VEH_SIZE
275  * - F: FEAT_SIZE
276  *
277  * \note OnNewLandmarkAddedToMap will be also called after calling this method if a landmark is actually being added to the map.
278  */
279  void OnInverseObservationModel(
280  const KFArray_OBS & in_z,
281  KFArray_FEAT & out_yn,
282  KFMatrix_FxV & out_dyn_dxv,
283  KFMatrix_FxO & out_dyn_dhn ) const;
284 
285  /** If applicable to the given problem, do here any special handling of adding a new landmark to the map.
286  * \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.
287  * \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.
288  * \sa OnInverseObservationModel
289  */
290  void OnNewLandmarkAddedToMap(
291  const size_t in_obsIdx,
292  const size_t in_idxNewFeat );
293 
294 
295  /** 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.
296  */
297  void OnNormalizeStateVector();
298 
299  /** @}
300  */
301 
303  void getLandmarkIDsFromIndexInStateVector(std::map<unsigned int,mrpt::maps::CLandmark::TLandmarkID> &out_id2index) const
304  {
305  out_id2index = m_IDs.getInverseMap();
306  }
307 
308  protected:
309 
310  /** Set up by processActionObservation */
311  mrpt::obs::CActionCollectionPtr m_action;
312 
313  /** Set up by processActionObservation */
314  mrpt::obs::CSensoryFramePtr m_SF;
315 
316  /** The mapping between landmark IDs and indexes in the Pkk cov. matrix: */
318 
319  /** The sequence of all the observations and the robot path (kept for debugging, statistics,etc) */
322  TDataAssocInfo m_last_data_association; //!< Last data association
323  }; // end class
324  } // End of namespace
325 } // End of namespace
326 
327 #endif
mrpt::math::CVectorFloat 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...
An implementation of EKF-based SLAM with range-bearing sensors, odometry, and a 2D (+heading) robot p...
TOptions options
The options for the algorithm.
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:
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)
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
mrpt::math::CMatrixTemplateNumeric< kftype > Y_pred_means
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:38
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
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.
mrpt::math::TPoint2D landmark_point_t
Either mrpt::math::TPoint2D or mrpt::math::TPoint3D.
GLsizei const GLchar ** string
Definition: glext.h:3919
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...
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.
GLboolean reset
Definition: glext.h:3535
GLsizei GLsizei GLchar * source
Definition: glext.h:3908
std::vector< size_t > vector_size_t
Definition: types_simple.h:25
std::vector< int32_t > vector_int
Definition: types_simple.h:23
The results from mrpt::slam::data_association.
TDataAssociationMethod
Different algorithms for data association, used in mrpt::slam::data_association.
Lightweight 2D point.
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...
std::vector< size_t > vector_size_t



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 8277875f6 Mon Jun 11 02:47:32 2018 +0200 at lun oct 28 01:50:49 CET 2019