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



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020