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



Page generated by Doxygen 1.9.6 for MRPT 1.4.0 SVN: at Fri Jan 20 02:28:26 UTC 2023