Go to the documentation of this file.
9 #ifndef CRangeBearingKFSLAM_H
10 #define CRangeBearingKFSLAM_H
97 std::vector<mrpt::math::TPoint3D>& out_landmarksPositions,
98 std::map<unsigned int, mrpt::maps::CLandmark::TLandmarkID>&
117 std::vector<mrpt::math::TPoint3D>& out_landmarksPositions,
118 std::map<unsigned int, mrpt::maps::CLandmark::TLandmarkID>&
126 q, out_landmarksPositions, out_landmarkIDs, out_fullState,
177 std::ostream& out)
const override;
282 std::vector<std::vector<uint32_t>>& landmarksMembership)
const;
289 size_t K, std::vector<std::vector<uint32_t>>& landmarksMembership);
296 const std::vector<std::vector<uint32_t>>& landmarksMembership)
const;
344 bool& out_skipPrediction)
const;
387 const std::vector<size_t>& in_lm_indices_in_S,
391 const std::vector<size_t>& idx_landmarks_to_predict,
404 const size_t& idx_landmark_to_predict,
KFMatrix_OxV& Hx,
437 std::vector<size_t>& out_LM_indices_to_predict)
const;
474 const size_t in_obsIdx,
const size_t in_idxNewFeat);
virtual ~CRangeBearingKFSLAM()
Destructor:
Configuration parameters.
float std_sensor_range
The std.
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
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...
TDataAssociationMetric data_assoc_IC_metric
Whether to use mahalanobis (->chi2 criterion) vs.
TDataAssociationMethod
Different algorithms for data association, used in mrpt::slam::data_association.
The results from mrpt::slam::data_association.
bool create_simplemap
Whether to fill m_SFs (default=false)
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...
GLdouble GLdouble GLdouble GLdouble q
std::shared_ptr< CSensoryFrame > Ptr
TDataAssociationMethod data_assoc_method
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).
CIncrementalMapPartitioner mapPartitioner
Used for map partitioning experiments.
void OnGetObservationNoise(KFMatrix_OxO &out_R) const
Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of ...
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
TDataAssociationResults results
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ion) override
This method load the options from a ".ini"-like file or memory-stored string list.
mrpt::obs::CActionCollection::Ptr m_action
Set up by processActionObservation.
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 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::math::CMatrixFixedNumeric< double, OBS_SIZE, OBS_SIZE > KFMatrix_OxO
mrpt::obs::CSensoryFrame::Ptr m_SF
Set up by processActionObservation.
std::vector< std::vector< uint32_t > > m_lastPartitionSet
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.
TDataAssociationMetric
Different metrics for data association, used in mrpt::slam::data_association For a comparison of both...
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...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::containers::bimap< mrpt::maps::CLandmark::TLandmarkID, unsigned int > m_IDs
The mapping between landmark IDs and indexes in the Pkk cov.
void OnObservationModel(const std::vector< size_t > &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const
double data_assoc_IC_ml_threshold
Only if data_assoc_IC_metric==ML, the log-ML threshold (Default=0.0)
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,...
mrpt::math::CMatrixTemplateNumeric< kftype > Y_pred_covs
mrpt::poses::CPose3DQuat getIncrementFromOdometry() const
Return the last odometry, as a pose increment.
mrpt::poses::CPose3DQuat getCurrentRobotPoseMean() const
Get the current robot pose mean, as a 3D+quaternion pose.
const TDataAssocInfo & getLastDataAssociation() const
Returns a read-only reference to the information on the last data-association.
GLsizei GLsizei GLchar * source
TOptions options
Algorithm parameters.
mrpt::math::CMatrixFixedNumeric< double, FEAT_SIZE, VEH_SIZE > KFMatrix_FxV
mrpt::math::CArrayNumeric< double, OBS_SIZE > KFArray_OBS
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.
mrpt::math::CArrayNumeric< double, VEH_SIZE > KFArray_VEH
TDataAssocInfo m_last_data_association
Last data association.
double data_assoc_IC_chi2_thres
Threshold in [0,1] for the chi2square test for individual compatibility between predictions and obser...
void OnObservationJacobians(const size_t &idx_landmark_to_predict, KFMatrix_OxV &Hx, KFMatrix_OxF &Hy) const
Implements the observation Jacobians and (when applicable) .
This class allows loading and storing values and vectors of different types from a configuration text...
mrpt::math::CMatrixTemplateNumeric< double > KFMatrix
CRangeBearingKFSLAM()
Constructor.
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...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
mrpt::math::CMatrixFixedNumeric< double, OBS_SIZE, FEAT_SIZE > KFMatrix_OxF
TDataAssociationMetric data_assoc_metric
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...
float std_odo_z_additional
Additional std.
Information for data-association:
mrpt::aligned_std_vector< KFArray_OBS > vector_KFArray_OBS
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!...
void OnTransitionJacobian(KFMatrix_VxV &out_F) const
Implements the transition Jacobian .
void reconsiderPartitionsNow()
The partitioning of the entire map is recomputed again.
mrpt::math::CArrayNumeric< double, FEAT_SIZE > KFArray_FEAT
std::shared_ptr< CSetOfObjects > Ptr
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).
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...
mrpt::math::CMatrixFixedNumeric< double, FEAT_SIZE, OBS_SIZE > KFMatrix_FxO
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 reset()
Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,...
void loadOptions(const mrpt::config::CConfigFileBase &ini)
Load options from a ini-like file/text.
CIncrementalMapPartitioner::TOptions * mapPartitionOptions()
Provides access to the parameters of the map partitioning algorithm.
Finds partitions in metric maps based on N-cut graph partition theory.
mrpt::math::CArrayNumeric< double, ACT_SIZE > KFArray_ACT
The options for the algorithm.
An implementation of EKF-based SLAM with range-bearing sensors, odometry, a full 6D robot pose,...
mrpt::math::CMatrixFixedNumeric< double, OBS_SIZE, VEH_SIZE > KFMatrix_OxV
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
void OnGetAction(KFArray_ACT &out_u) const
Must return the action vector u.
bool force_ignore_odometry
Whether to ignore the input odometry and behave as if there was no odometry at all (default: false)
mrpt::math::CMatrixTemplateNumeric< kftype > Y_pred_means
mrpt::slam::CRangeBearingKFSLAM::TOptions options
mrpt::maps::CSimpleMap m_SFs
The sequence of all the observations and the robot path (kept for debugging, statistics,...
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
GLsizei const GLchar ** string
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::CVectorFloat stds_Q_no_odo
A 7-length vector with the std.
std::vector< size_t > predictions_IDs
mrpt::math::CMatrixFixedNumeric< double, VEH_SIZE, VEH_SIZE > KFMatrix_VxV
int partitioningMethod
Applicable only if "doPartitioningExperiment=true".
TOptions()
Default values.
std::shared_ptr< CActionCollection > Ptr
void OnTransitionModel(const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const
Implements the transition model .
void OnTransitionNoise(KFMatrix_VxV &out_Q) const
Implements the transition noise covariance .
Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.
@ UNINITIALIZED_QUATERNION
float quantiles_3D_representation
Default = 3.
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...
bool doPartitioningExperiment
If set to true (default=false), map will be partitioned using the method stated by partitioningMethod...
void getLastPartitionLandmarks(std::vector< std::vector< uint32_t >> &landmarksMembership) const
Return the partitioning of the landmarks in clusters accoring to the last partition.
void OnNormalizeStateVector()
This method is called after the prediction and after the update, to give the user an opportunity to n...
Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST | |