9 #ifndef CRangeBearingKFSLAM_H 10 #define CRangeBearingKFSLAM_H 70 void processActionObservation(
71 mrpt::obs::CActionCollectionPtr &action,
72 mrpt::obs::CSensoryFramePtr &SF );
84 std::vector<mrpt::math::TPoint3D> &out_landmarksPositions,
85 std::map<unsigned int,mrpt::maps::CLandmark::TLandmarkID> &out_landmarkIDs,
100 std::vector<mrpt::math::TPoint3D> &out_landmarksPositions,
101 std::map<unsigned int,mrpt::maps::CLandmark::TLandmarkID> &out_landmarkIDs,
107 this->getCurrentState(q,out_landmarksPositions,out_landmarkIDs,out_fullState,out_fullCovariance);
127 this->getCurrentRobotPose(q);
134 void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj )
const;
150 void loadFromConfigFile(
152 const std::string §ion);
209 predictions_IDs.clear();
210 newly_inserted_landmarks.clear();
227 return m_last_data_association;
237 parts = m_lastPartitionSet;
246 void getLastPartitionLandmarks( std::vector<vector_uint> &landmarksMembership )
const;
250 void getLastPartitionLandmarksAsIfFixedSubmaps(
size_t K, std::vector<vector_uint> &landmarksMembership );
256 double computeOffDiagonalBlocksApproximationError(
const std::vector<vector_uint> &landmarksMembership )
const;
265 void reconsiderPartitionsNow();
272 return &mapPartitioner.options;
277 void saveMapAndPath2DRepresentationAsMATLABFile(
278 const std::string &fil,
280 const std::string &styleLandmarks = std::string(
"b"),
281 const std::string &stylePath = std::string(
"r"),
282 const std::string &styleRobot = std::string(
"r") )
const;
302 void OnTransitionModel(
305 bool &out_skipPrediction
330 void OnGetObservationsAndDataAssociation(
339 void OnObservationModel(
349 void OnObservationJacobians(
350 const size_t &idx_landmark_to_predict,
371 void OnPreComputingPredictions(
387 void OnInverseObservationModel(
398 void OnNewLandmarkAddedToMap(
399 const size_t in_obsIdx,
400 const size_t in_idxNewFeat );
405 void OnNormalizeStateVector();
411 mrpt::obs::CActionCollectionPtr m_action;
414 mrpt::obs::CSensoryFramePtr m_SF;
427 std::vector<vector_uint> m_lastPartitionSet;
mrpt::math::TPoint3D landmark_point_t
Either mrpt::math::TPoint2D or mrpt::math::TPoint3D.
An implementation of EKF-based SLAM with range-bearing sensors, odometry, a full 6D robot pose...
const TDataAssocInfo & getLastDataAssociation() const
Returns a read-only reference to the information on the last data-association.
bool force_ignore_odometry
Whether to ignore the input odometry and behave as if there was no odometry at all (default: false) ...
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 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...
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
mrpt::vector_size_t predictions_IDs
float std_odo_z_additional
Additional std.
void getLastPartition(std::vector< vector_uint > &parts)
Return the last partition of the sequence of sensoryframes (it is NOT a partition of the map!!) Only ...
mrpt::math::CMatrixTemplateNumeric< kftype > Y_pred_means
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
Configuration of the algorithm:
This class allows loading and storing values and vectors of different types from a configuration text...
CIncrementalMapPartitioner::TOptions * mapPartitionOptions()
Provides access to the parameters of the map partitioning algorithm.
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)...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
A numeric matrix of compile-time fixed size.
TDataAssociationMetric data_assoc_metric
int partitioningMethod
Applicable only if "doPartitioningExperiment=true".
TDataAssociationMetric
Different metrics for data association, used in mrpt::slam::data_association For a comparison of both...
mrpt::aligned_containers< KFArray_OBS >::vector_t vector_KFArray_OBS
Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
TDataAssociationMetric data_assoc_IC_metric
Whether to use mahalanobis (->chi2 criterion) vs. Matching likelihood.
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
double data_assoc_IC_ml_threshold
Only if data_assoc_IC_metric==ML, the log-ML threshold (Default=0.0)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
This class can be used to make partitions on a map/graph build from observations taken at some poses/...
float quantiles_3D_representation
Default = 3.
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
Information for data-association:
std::vector< size_t > vector_size_t
bool create_simplemap
Whether to fill m_SFs (default=false)
std::vector< int32_t > vector_int
TDataAssociationMethod data_assoc_method
bool doPartitioningExperiment
If set to true (default=false), map will be partitioned using the method stated by partitioningMethod...
TDataAssociationResults results
The results from mrpt::slam::data_association.
The options for the algorithm.
TDataAssociationMethod
Different algorithms for data association, used in mrpt::slam::data_association.
double data_assoc_IC_chi2_thres
Threshold in [0,1] for the chi2square test for individual compatibility between predictions and obser...
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...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
mrpt::math::CVectorFloat stds_Q_no_odo
A 7-length vector with the std.
std::vector< size_t > vector_size_t