Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, 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/containers/bimap.h>
19 
20 #include <mrpt/obs/CSensoryFrame.h>
26 #include <mrpt/maps/CLandmark.h>
27 #include <mrpt/maps/CSimpleMap.h>
30 
31 namespace mrpt
32 {
33 namespace slam
34 {
35 /** An implementation of EKF-based SLAM with range-bearing sensors, odometry, a
36  * full 6D robot pose, and 3D landmarks.
37  * The main method is "processActionObservation" which processes pairs of
38  * action/observation.
39  * The state vector comprises: 3D robot position, a quaternion for its
40  * attitude, and the 3D landmarks in the map.
41  *
42  * The following Wiki page describes an front-end application based on this
43  * class:
44  * http://www.mrpt.org/Application:kf-slam
45  *
46  * For the theory behind this implementation, see the technical report in:
47  * http://www.mrpt.org/6D-SLAM
48  *
49  * \sa An implementation for 2D only: CRangeBearingKFSLAM2D
50  * \ingroup metric_slam_grp
51  */
53  : public bayes::CKalmanFilterCapable<7 /* x y z qr qx qy qz*/,
54  3 /* range yaw pitch */, 3 /* x y z */,
55  7 /* Ax Ay Az Aqr Aqx Aqy Aqz */>
56 // <size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t
57 // ACT_SIZE, size typename kftype = double>
58 {
59  public:
60  /** Either mrpt::math::TPoint2D or mrpt::math::TPoint3D */
62 
63  /** Constructor. */
65 
66  /** Destructor: */
67  virtual ~CRangeBearingKFSLAM();
68 
69  /** Reset the state of the SLAM filter: The map is emptied and the robot put
70  * back to (0,0,0). */
71  void reset();
72 
73  /** Process one new action and observations to update the map and robot pose
74  *estimate. See the description of the class at the top of this page.
75  * \param action May contain odometry
76  * \param SF The set of observations, must contain at least one
77  *CObservationBearingRange
78  */
82 
83  /** Returns the complete mean and cov.
84  * \param out_robotPose The mean and the 7x7 covariance matrix of the
85  * robot 6D pose
86  * \param out_landmarksPositions One entry for each of the M landmark
87  * positions (3D).
88  * \param out_landmarkIDs Each element[index] (for indices of
89  * out_landmarksPositions) gives the corresponding landmark ID.
90  * \param out_fullState The complete state vector (7+3M).
91  * \param out_fullCovariance The full (7+3M)x(7+3M) covariance matrix of
92  * the filter.
93  * \sa getCurrentRobotPose
94  */
95  void getCurrentState(
97  std::vector<mrpt::math::TPoint3D>& out_landmarksPositions,
98  std::map<unsigned int, mrpt::maps::CLandmark::TLandmarkID>&
99  out_landmarkIDs,
100  mrpt::math::CVectorDouble& out_fullState,
101  mrpt::math::CMatrixDouble& out_fullCovariance) const;
102 
103  /** Returns the complete mean and cov.
104  * \param out_robotPose The mean and the 7x7 covariance matrix of the
105  * robot 6D pose
106  * \param out_landmarksPositions One entry for each of the M landmark
107  * positions (3D).
108  * \param out_landmarkIDs Each element[index] (for indices of
109  * out_landmarksPositions) gives the corresponding landmark ID.
110  * \param out_fullState The complete state vector (7+3M).
111  * \param out_fullCovariance The full (7+3M)x(7+3M) covariance matrix of
112  * the filter.
113  * \sa getCurrentRobotPose
114  */
115  inline void getCurrentState(
116  mrpt::poses::CPose3DPDFGaussian& out_robotPose,
117  std::vector<mrpt::math::TPoint3D>& out_landmarksPositions,
118  std::map<unsigned int, mrpt::maps::CLandmark::TLandmarkID>&
119  out_landmarkIDs,
120  mrpt::math::CVectorDouble& out_fullState,
121  mrpt::math::CMatrixDouble& out_fullCovariance) const
122  {
125  this->getCurrentState(
126  q, out_landmarksPositions, out_landmarkIDs, out_fullState,
127  out_fullCovariance);
128  out_robotPose = mrpt::poses::CPose3DPDFGaussian(q);
129  }
130 
131  /** Returns the mean & the 7x7 covariance matrix of the robot 6D pose (with
132  * rotation as a quaternion).
133  * \sa getCurrentState, getCurrentRobotPoseMean
134  */
135  void getCurrentRobotPose(
136  mrpt::poses::CPose3DQuatPDFGaussian& out_robotPose) const;
137 
138  /** Get the current robot pose mean, as a 3D+quaternion pose.
139  * \sa getCurrentRobotPose
140  */
142 
143  /** Returns the mean & the 6x6 covariance matrix of the robot 6D pose (with
144  * rotation as 3 angles).
145  * \sa getCurrentState
146  */
147  inline void getCurrentRobotPose(
148  mrpt::poses::CPose3DPDFGaussian& out_robotPose) const
149  {
152  this->getCurrentRobotPose(q);
153  out_robotPose = mrpt::poses::CPose3DPDFGaussian(q);
154  }
155 
156  /** Returns a 3D representation of the landmarks in the map and the robot 3D
157  * position according to the current filter state.
158  * \param out_objects
159  */
161 
162  /** Load options from a ini-like file/text
163  */
165 
166  /** The options for the algorithm
167  */
169  {
170  /** Default values */
171  TOptions();
172 
173  void loadFromConfigFile(
175  const std::string& section) override; // See base docs
176  void dumpToTextStream(
177  std::ostream& out) const override; // See base docs
178 
179  /** A 7-length vector with the std. deviation of the transition model in
180  * (x,y,z, qr,qx,qy,qz) used only when there is no odometry (if there is
181  * odo, its uncertainty values will be used instead); x y z: In meters.
182  */
184 
185  /** The std. deviation of the sensor (for the matrix R in the kalman
186  * filters), in meters and radians. */
188 
189  /** Additional std. dev. to sum to the motion model in the z axis
190  * (useful when there is only 2D odometry and we want to put things hard
191  * to the algorithm) (default=0) */
193 
194  /** If set to true (default=false), map will be partitioned using the
195  * method stated by partitioningMethod */
197 
198  /** Default = 3 */
200 
201  /** Applicable only if "doPartitioningExperiment=true".
202  * 0: Automatically detect partition through graph-cut.
203  * N>=1: Cut every "N" observations.
204  */
206 
207  // Data association:
210  /** Threshold in [0,1] for the chi2square test for individual
211  * compatibility between predictions and observations (default: 0.99) */
213  /** Whether to use mahalanobis (->chi2 criterion) vs. Matching
214  * likelihood. */
216  /** Only if data_assoc_IC_metric==ML, the log-ML threshold (Default=0.0)
217  */
219 
220  /** Whether to fill m_SFs (default=false) */
222 
223  /** Whether to ignore the input odometry and behave as if there was no
224  * odometry at all (default: false) */
226  } options;
227 
228  /** Information for data-association:
229  * \sa getLastDataAssociation
230  */
232  {
234  void clear()
235  {
236  results.clear();
237  predictions_IDs.clear();
238  newly_inserted_landmarks.clear();
239  }
240 
241  // Predictions from the map:
243  std::vector<size_t> predictions_IDs;
244 
245  /** Map from the 0-based index within the last observation and the
246  landmark 0-based index in the map (the robot-map state vector)
247  Only used for stats and so. */
248  std::map<size_t, size_t> newly_inserted_landmarks;
249 
250  // DA results:
252  };
253 
254  /** Returns a read-only reference to the information on the last
255  * data-association */
257  {
259  }
260 
261  /** Return the last partition of the sequence of sensoryframes (it is NOT a
262  * partition of the map!!)
263  * Only if options.doPartitioningExperiment = true
264  * \sa getLastPartitionLandmarks
265  */
266  void getLastPartition(std::vector<std::vector<uint32_t>>& parts)
267  {
268  parts = m_lastPartitionSet;
269  }
270 
271  /** Return the partitioning of the landmarks in clusters accoring to the
272  * last partition.
273  * Note that the same landmark may appear in different clusters (the
274  * partition is not in the space of landmarks)
275  * Only if options.doPartitioningExperiment = true
276  * \param landmarksMembership The i'th element of this vector is the set
277  * of clusters to which the i'th landmark in the map belongs to (landmark
278  * index != landmark ID !!).
279  * \sa getLastPartition
280  */
282  std::vector<std::vector<uint32_t>>& landmarksMembership) const;
283 
284  /** For testing only: returns the partitioning as
285  * "getLastPartitionLandmarks" but as if a fixed-size submaps (size K) were
286  * have been used.
287  */
289  size_t K, std::vector<std::vector<uint32_t>>& landmarksMembership);
290 
291  /** Computes the ratio of the missing information matrix elements which are
292  * ignored under a certain partitioning of the landmarks.
293  * \sa getLastPartitionLandmarks, getLastPartitionLandmarksAsIfFixedSubmaps
294  */
296  const std::vector<std::vector<uint32_t>>& landmarksMembership) const;
297 
298  /** The partitioning of the entire map is recomputed again.
299  * Only when options.doPartitioningExperiment = true.
300  * This can be used after changing the parameters of the partitioning
301  * method.
302  * After this method, you can call getLastPartitionLandmarks.
303  * \sa getLastPartitionLandmarks
304  */
306 
307  /** Provides access to the parameters of the map partitioning algorithm.
308  */
310  {
311  return &mapPartitioner.options;
312  }
313 
314  /** Save the current state of the filter (robot pose & map) to a MATLAB
315  * script which displays all the elements in 2D
316  */
318  const std::string& fil, float stdCount = 3.0f,
319  const std::string& styleLandmarks = std::string("b"),
320  const std::string& stylePath = std::string("r"),
321  const std::string& styleRobot = std::string("r")) const;
322 
323  protected:
324  /** @name Virtual methods for Kalman Filter implementation
325  @{
326  */
327 
328  /** Must return the action vector u.
329  * \param out_u The action vector which will be passed to OnTransitionModel
330  */
331  void OnGetAction(KFArray_ACT& out_u) const;
332 
333  /** Implements the transition model \f$ \hat{x}_{k|k-1} = f(
334  * \hat{x}_{k-1|k-1}, u_k ) \f$
335  * \param in_u The vector returned by OnGetAction.
336  * \param inout_x At input has \f[ \hat{x}_{k-1|k-1} \f] , at output must
337  * have \f$ \hat{x}_{k|k-1} \f$ .
338  * \param out_skip Set this to true if for some reason you want to skip the
339  * prediction step (to do not modify either the vector or the covariance).
340  * Default:false
341  */
342  void OnTransitionModel(
343  const KFArray_ACT& in_u, KFArray_VEH& inout_x,
344  bool& out_skipPrediction) const;
345 
346  /** Implements the transition Jacobian \f$ \frac{\partial f}{\partial x} \f$
347  * \param out_F Must return the Jacobian.
348  * The returned matrix must be \f$V \times V\f$ with V being either the
349  * size of the whole state vector (for non-SLAM problems) or VEH_SIZE (for
350  * SLAM problems).
351  */
352  void OnTransitionJacobian(KFMatrix_VxV& out_F) const;
353 
354  /** Implements the transition noise covariance \f$ Q_k \f$
355  * \param out_Q Must return the covariance matrix.
356  * The returned matrix must be of the same size than the jacobian from
357  * OnTransitionJacobian
358  */
359  void OnTransitionNoise(KFMatrix_VxV& out_Q) const;
360 
361  /** This is called between the KF prediction step and the update step, and
362  * the application must return the observations and, when applicable, the
363  * data association between these observations and the current map.
364  *
365  * \param out_z N vectors, each for one "observation" of length OBS_SIZE, N
366  * being the number of "observations": how many observed landmarks for a
367  * map, or just one if not applicable.
368  * \param out_data_association An empty vector or, where applicable, a
369  * vector where the i'th element corresponds to the position of the
370  * observation in the i'th row of out_z within the system state vector (in
371  * the range [0,getNumberOfLandmarksInTheMap()-1]), or -1 if it is a new map
372  * element and we want to insert it at the end of this KF iteration.
373  * \param in_S The full covariance matrix of the observation predictions
374  * (i.e. the "innovation covariance matrix"). This is a M*O x M*O matrix
375  * with M=length of "in_lm_indices_in_S".
376  * \param in_lm_indices_in_S The indices of the map landmarks (range
377  * [0,getNumberOfLandmarksInTheMap()-1]) that can be found in the matrix
378  * in_S.
379  *
380  * This method will be called just once for each complete KF iteration.
381  * \note It is assumed that the observations are independent, i.e. there
382  * are NO cross-covariances between them.
383  */
385  vector_KFArray_OBS& out_z, std::vector<int>& out_data_association,
386  const vector_KFArray_OBS& in_all_predictions, const KFMatrix& in_S,
387  const std::vector<size_t>& in_lm_indices_in_S,
388  const KFMatrix_OxO& in_R);
389 
390  void OnObservationModel(
391  const std::vector<size_t>& idx_landmarks_to_predict,
392  vector_KFArray_OBS& out_predictions) const;
393 
394  /** Implements the observation Jacobians \f$ \frac{\partial h_i}{\partial x}
395  * \f$ and (when applicable) \f$ \frac{\partial h_i}{\partial y_i} \f$.
396  * \param idx_landmark_to_predict The index of the landmark in the map
397  * whose prediction is expected as output. For non SLAM-like problems, this
398  * will be zero and the expected output is for the whole state vector.
399  * \param Hx The output Jacobian \f$ \frac{\partial h_i}{\partial x} \f$.
400  * \param Hy The output Jacobian \f$ \frac{\partial h_i}{\partial y_i}
401  * \f$.
402  */
404  const size_t& idx_landmark_to_predict, KFMatrix_OxV& Hx,
405  KFMatrix_OxF& Hy) const;
406 
407  /** Computes A=A-B, which may need to be re-implemented depending on the
408  * topology of the individual scalar components (eg, angles).
409  */
411  KFArray_OBS& A, const KFArray_OBS& B) const;
412 
413  /** Return the observation NOISE covariance matrix, that is, the model of
414  * the Gaussian additive noise of the sensor.
415  * \param out_R The noise covariance matrix. It might be non diagonal, but
416  * it'll usually be.
417  */
418  void OnGetObservationNoise(KFMatrix_OxO& out_R) const;
419 
420  /** This will be called before OnGetObservationsAndDataAssociation to allow
421  * the application to reduce the number of covariance landmark predictions
422  * to be made.
423  * For example, features which are known to be "out of sight" shouldn't be
424  * added to the output list to speed up the calculations.
425  * \param in_all_prediction_means The mean of each landmark predictions;
426  * the computation or not of the corresponding covariances is what we're
427  * trying to determined with this method.
428  * \param out_LM_indices_to_predict The list of landmark indices in the map
429  * [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.
430  * \note This is not a pure virtual method, so it should be implemented
431  * only if desired. The default implementation returns a vector with all the
432  * landmarks in the map.
433  * \sa OnGetObservations, OnDataAssociation
434  */
436  const vector_KFArray_OBS& in_all_prediction_means,
437  std::vector<size_t>& out_LM_indices_to_predict) const;
438 
439  /** If applicable to the given problem, this method implements the inverse
440  * observation model needed to extend the "map" with a new "element".
441  * \param in_z The observation vector whose inverse sensor model is to be
442  * computed. This is actually one of the vector<> returned by
443  * OnGetObservations().
444  * \param out_yn The F-length vector with the inverse observation model \f$
445  * y_n=y(x,z_n) \f$.
446  * \param out_dyn_dxv The \f$F \times V\f$ Jacobian of the inv. sensor
447  * model wrt the robot pose \f$ \frac{\partial y_n}{\partial x_v} \f$.
448  * \param out_dyn_dhn The \f$F \times O\f$ Jacobian of the inv. sensor
449  * model wrt the observation vector \f$ \frac{\partial y_n}{\partial h_n}
450  * \f$.
451  *
452  * - O: OBS_SIZE
453  * - V: VEH_SIZE
454  * - F: FEAT_SIZE
455  *
456  * \note OnNewLandmarkAddedToMap will be also called after calling this
457  * method if a landmark is actually being added to the map.
458  */
460  const KFArray_OBS& in_z, KFArray_FEAT& out_yn,
461  KFMatrix_FxV& out_dyn_dxv, KFMatrix_FxO& out_dyn_dhn) const;
462 
463  /** If applicable to the given problem, do here any special handling of
464  * adding a new landmark to the map.
465  * \param in_obsIndex The index of the observation whose inverse sensor is
466  * to be computed. It corresponds to the row in in_z where the observation
467  * can be found.
468  * \param in_idxNewFeat The index that this new feature will have in the
469  * state vector (0:just after the vehicle state, 1: after that,...). Save
470  * this number so data association can be done according to these indices.
471  * \sa OnInverseObservationModel
472  */
474  const size_t in_obsIdx, const size_t in_idxNewFeat);
475 
476  /** This method is called after the prediction and after the update, to give
477  * the user an opportunity to normalize the state vector (eg, keep angles
478  * within -pi,pi range) if the application requires it.
479  */
480  void OnNormalizeStateVector();
481 
482  /** @}
483  */
484 
485  /** Set up by processActionObservation */
487 
488  /** Set up by processActionObservation */
490 
491  /** The mapping between landmark IDs and indexes in the Pkk cov. matrix: */
493  m_IDs;
494 
495  /** Used for map partitioning experiments */
497 
498  /** The sequence of all the observations and the robot path (kept for
499  * debugging, statistics,etc)
500  */
503  std::vector<std::vector<uint32_t>> m_lastPartitionSet;
504 
505  /** Last data association */
507 
508  /** Return the last odometry, as a pose increment. */
510 
511 }; // end class
512 } // End of namespace
513 } // End of namespace
514 
515 #endif
mrpt::math::CMatrixFixedNumeric< double, OBS_SIZE, VEH_SIZE > KFMatrix_OxV
void getCurrentRobotPose(mrpt::poses::CPose3DQuatPDFGaussian &out_robotPose) const
Returns the mean & the 7x7 covariance matrix of the robot 6D pose (with rotation as a quaternion)...
An implementation of EKF-based SLAM with range-bearing sensors, odometry, a full 6D robot pose...
void reset()
Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,0,0).
mrpt::poses::CPose3DQuat getIncrementFromOdometry() const
Return the last odometry, as a pose increment.
void OnObservationJacobians(const size_t &idx_landmark_to_predict, KFMatrix_OxV &Hx, KFMatrix_OxF &Hy) const
Implements the observation Jacobians and (when applicable) .
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
void getLastPartitionLandmarks(std::vector< std::vector< uint32_t >> &landmarksMembership) const
Return the partitioning of the landmarks in clusters accoring to the last partition.
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...
Definition: CSimpleMap.h:35
void reconsiderPartitionsNow()
The partitioning of the entire map is recomputed again.
mrpt::math::CArrayNumeric< double, VEH_SIZE > KFArray_VEH
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
void processActionObservation(mrpt::obs::CActionCollection::Ptr &action, mrpt::obs::CSensoryFrame::Ptr &SF)
Process one new action and observations to update the map and robot pose estimate.
void OnNormalizeStateVector()
This method is called after the prediction and after the update, to give the user an opportunity to n...
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...
void OnPreComputingPredictions(const vector_KFArray_OBS &in_all_prediction_means, std::vector< size_t > &out_LM_indices_to_predict) const
This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the...
mrpt::math::CMatrixTemplateNumeric< double > KFMatrix
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
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...
void OnGetAction(KFArray_ACT &out_u) const
Must return the action vector u.
void OnTransitionJacobian(KFMatrix_VxV &out_F) const
Implements the transition Jacobian .
mrpt::poses::CPose3DQuat getCurrentRobotPoseMean() const
Get the current robot pose mean, as a 3D+quaternion pose.
TDataAssocInfo m_last_data_association
Last data association.
mrpt::containers::bimap< mrpt::maps::CLandmark::TLandmarkID, unsigned int > m_IDs
The mapping between landmark IDs and indexes in the Pkk cov.
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::math::CMatrixTemplateNumeric< kftype > Y_pred_means
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
mrpt::math::CArrayNumeric< double, FEAT_SIZE > KFArray_FEAT
CIncrementalMapPartitioner::TOptions * mapPartitionOptions()
Provides access to the parameters of the map partitioning algorithm.
mrpt::math::CMatrixFixedNumeric< double, VEH_SIZE, VEH_SIZE > KFMatrix_VxV
void OnTransitionNoise(KFMatrix_VxV &out_Q) const
Implements the transition noise covariance .
This class allows loading and storing values and vectors of different types from a configuration text...
mrpt::math::CArrayNumeric< double, OBS_SIZE > KFArray_OBS
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 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...
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.
CIncrementalMapPartitioner mapPartitioner
Used for map partitioning experiments.
TDataAssociationMetric data_assoc_IC_metric
Whether to use mahalanobis (->chi2 criterion) vs.
double computeOffDiagonalBlocksApproximationError(const std::vector< std::vector< uint32_t >> &landmarksMembership) const
Computes the ratio of the missing information matrix elements which are ignored under a certain parti...
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:48
mrpt::math::CMatrixFixedNumeric< double, OBS_SIZE, OBS_SIZE > KFMatrix_OxO
mrpt::maps::CSimpleMap m_SFs
The sequence of all the observations and the robot path (kept for debugging, statistics,etc)
GLsizei const GLchar ** string
Definition: glext.h:4101
double data_assoc_IC_ml_threshold
Only if data_assoc_IC_metric==ML, the log-ML threshold (Default=0.0)
std::vector< std::vector< uint32_t > > m_lastPartitionSet
mrpt::aligned_std_vector< KFArray_OBS > vector_KFArray_OBS
void loadOptions(const mrpt::config::CConfigFileBase &ini)
Load options from a ini-like file/text.
mrpt::math::CArrayNumeric< double, ACT_SIZE > KFArray_ACT
void OnGetObservationNoise(KFMatrix_OxO &out_R) const
Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of ...
void OnGetObservationsAndDataAssociation(vector_KFArray_OBS &out_z, std::vector< int > &out_data_association, const vector_KFArray_OBS &in_all_predictions, const KFMatrix &in_S, const std::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...
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.
void OnObservationModel(const std::vector< size_t > &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const
Finds partitions in metric maps based on N-cut graph partition theory.
mrpt::obs::CSensoryFrame::Ptr m_SF
Set up by processActionObservation.
void getCurrentState(mrpt::poses::CPose3DQuatPDFGaussian &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.
mrpt::slam::CRangeBearingKFSLAM::TOptions options
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
mrpt::math::CMatrixFixedNumeric< double, FEAT_SIZE, VEH_SIZE > KFMatrix_FxV
void getLastPartitionLandmarksAsIfFixedSubmaps(size_t K, std::vector< std::vector< uint32_t >> &landmarksMembership)
For testing only: returns the partitioning as "getLastPartitionLandmarks" but as if a fixed-size subm...
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
void OnTransitionModel(const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const
Implements the transition model .
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const
Returns a 3D representation of the landmarks in the map and the robot 3D position according to the cu...
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
mrpt::math::CMatrixFixedNumeric< double, OBS_SIZE, FEAT_SIZE > KFMatrix_OxF
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 ...
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...
bool create_simplemap
Whether to fill m_SFs (default=false)
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.
mrpt::math::CMatrixTemplateNumeric< kftype > Y_pred_covs
mrpt::obs::CActionCollection::Ptr m_action
Set up by processActionObservation.
TDataAssociationMethod
Different algorithms for data association, used in mrpt::slam::data_association.
virtual ~CRangeBearingKFSLAM()
Destructor:
double data_assoc_IC_chi2_thres
Threshold in [0,1] for the chi2square test for individual compatibility between predictions and obser...
mrpt::math::CMatrixFixedNumeric< double, FEAT_SIZE, OBS_SIZE > KFMatrix_FxO
void getLastPartition(std::vector< std::vector< uint32_t >> &parts)
Return the last partition of the sequence of sensoryframes (it is NOT a partition of the map!!) Only ...
mrpt::math::CVectorFloat stds_Q_no_odo
A 7-length vector with the std.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019