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



Page generated by Doxygen 1.8.20 for MRPT 1.4.0 SVN: at Thu Aug 27 02:40:23 UTC 2020