MRPT
2.0.4
|
An implementation of EKF-based SLAM with range-bearing sensors, odometry, a full 6D robot pose, and 3D landmarks.
The main method is "processActionObservation" which processes pairs of action/observation. The state vector comprises: 3D robot position, a quaternion for its attitude, and the 3D landmarks in the map.
The following Wiki page describes an front-end application based on this class: https://www.mrpt.org/Application:kf-slam
For the theory behind this implementation, see the technical report in: https://www.mrpt.org/6D-SLAM
Definition at line 47 of file CRangeBearingKFSLAM.h.
#include <mrpt/slam/CRangeBearingKFSLAM.h>
Classes | |
struct | TDataAssocInfo |
Information for data-association: More... | |
struct | TOptions |
The options for the algorithm. More... | |
Public Types | |
using | landmark_point_t = mrpt::math::TPoint3D |
Either mrpt::math::TPoint2D or mrpt::math::TPoint3D. More... | |
using | kftype = double |
The numeric type used in the Kalman Filter (default=double) More... | |
using | KFCLASS = CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, double > |
My class, in a shorter name! More... | |
using | KFVector = mrpt::math::CVectorDynamic< double > |
using | KFMatrix = mrpt::math::CMatrixDynamic< double > |
using | KFMatrix_VxV = mrpt::math::CMatrixFixed< double, VEH_SIZE, VEH_SIZE > |
using | KFMatrix_OxO = mrpt::math::CMatrixFixed< double, OBS_SIZE, OBS_SIZE > |
using | KFMatrix_FxF = mrpt::math::CMatrixFixed< double, FEAT_SIZE, FEAT_SIZE > |
using | KFMatrix_AxA = mrpt::math::CMatrixFixed< double, ACT_SIZE, ACT_SIZE > |
using | KFMatrix_VxO = mrpt::math::CMatrixFixed< double, VEH_SIZE, OBS_SIZE > |
using | KFMatrix_VxF = mrpt::math::CMatrixFixed< double, VEH_SIZE, FEAT_SIZE > |
using | KFMatrix_FxV = mrpt::math::CMatrixFixed< double, FEAT_SIZE, VEH_SIZE > |
using | KFMatrix_FxO = mrpt::math::CMatrixFixed< double, FEAT_SIZE, OBS_SIZE > |
using | KFMatrix_OxF = mrpt::math::CMatrixFixed< double, OBS_SIZE, FEAT_SIZE > |
using | KFMatrix_OxV = mrpt::math::CMatrixFixed< double, OBS_SIZE, VEH_SIZE > |
using | KFArray_VEH = mrpt::math::CVectorFixed< double, VEH_SIZE > |
using | KFArray_ACT = mrpt::math::CVectorFixed< double, ACT_SIZE > |
using | KFArray_OBS = mrpt::math::CVectorFixed< double, OBS_SIZE > |
using | vector_KFArray_OBS = std::vector< KFArray_OBS > |
using | KFArray_FEAT = mrpt::math::CVectorFixed< double, FEAT_SIZE > |
Public Member Functions | |
CRangeBearingKFSLAM () | |
Constructor. More... | |
~CRangeBearingKFSLAM () override | |
Destructor: More... | |
void | reset () |
Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,0,0). More... | |
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. More... | |
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. More... | |
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. More... | |
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). More... | |
mrpt::poses::CPose3DQuat | getCurrentRobotPoseMean () const |
Get the current robot pose mean, as a 3D+quaternion pose. More... | |
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). More... | |
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 current filter state. More... | |
void | loadOptions (const mrpt::config::CConfigFileBase &ini) |
Load options from a ini-like file/text. More... | |
const TDataAssocInfo & | getLastDataAssociation () const |
Returns a read-only reference to the information on the last data-association. More... | |
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 if options.doPartitioningExperiment = true. More... | |
void | getLastPartitionLandmarks (std::vector< std::vector< uint32_t >> &landmarksMembership) const |
Return the partitioning of the landmarks in clusters accoring to the last partition. More... | |
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 submaps (size K) were have been used. More... | |
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 partitioning of the landmarks. More... | |
void | reconsiderPartitionsNow () |
The partitioning of the entire map is recomputed again. More... | |
CIncrementalMapPartitioner::TOptions * | mapPartitionOptions () |
Provides access to the parameters of the map partitioning algorithm. More... | |
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 elements in 2D. More... | |
size_t | getNumberOfLandmarksInTheMap () const |
bool | isMapEmpty () const |
size_t | getStateVectorLength () const |
KFVector & | internal_getXkk () |
KFMatrix & | internal_getPkk () |
void | getLandmarkMean (size_t idx, KFArray_FEAT &feat) const |
Returns the mean of the estimated value of the idx'th landmark (not applicable to non-SLAM problems). More... | |
void | getLandmarkCov (size_t idx, KFMatrix_FxF &feat_cov) const |
Returns the covariance of the idx'th landmark (not applicable to non-SLAM problems). More... | |
mrpt::system::CTimeLogger & | getProfiler () |
Static Public Member Functions | |
static constexpr size_t | get_vehicle_size () |
static constexpr size_t | get_observation_size () |
static constexpr size_t | get_feature_size () |
static constexpr size_t | get_action_size () |
static std::array< mrpt::system::TConsoleColor, NUMBER_OF_VERBOSITY_LEVELS > & | logging_levels_to_colors () |
Map from VerbosityLevels to their corresponding mrpt::system::TConsoleColor. More... | |
static std::array< std::string, NUMBER_OF_VERBOSITY_LEVELS > & | logging_levels_to_names () |
Map from VerbosityLevels to their corresponding names. More... | |
Public Attributes | |
mrpt::slam::CRangeBearingKFSLAM::TOptions | options |
TKF_options | KF_options |
Generic options for the Kalman Filter algorithm itself. More... | |
Protected Member Functions | |
mrpt::poses::CPose3DQuat | getIncrementFromOdometry () const |
Return the last odometry, as a pose increment. More... | |
void | runOneKalmanIteration () |
The main entry point, executes one complete step: prediction + update. More... | |
Virtual methods for Kalman Filter implementation | |
void | OnGetAction (KFArray_ACT &out_u) const override |
Must return the action vector u. More... | |
void | OnTransitionModel (const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const override |
Implements the transition model . More... | |
void | OnTransitionJacobian (KFMatrix_VxV &out_F) const override |
Implements the transition Jacobian . More... | |
void | OnTransitionNoise (KFMatrix_VxV &out_Q) const override |
Implements the transition noise covariance . More... | |
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 the observations and, when applicable, the data association between these observations and the current map. More... | |
void | OnObservationModel (const std::vector< size_t > &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const override |
void | OnObservationJacobians (size_t idx_landmark_to_predict, KFMatrix_OxV &Hx, KFMatrix_OxF &Hy) const override |
Implements the observation Jacobians and (when applicable) . More... | |
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 scalar components (eg, angles). More... | |
void | OnGetObservationNoise (KFMatrix_OxO &out_R) const override |
Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor. More... | |
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 number of covariance landmark predictions to be made. More... | |
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 extend the "map" with a new "element". More... | |
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. More... | |
void | OnNormalizeStateVector () override |
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. More... | |
Protected Attributes | |
mrpt::obs::CActionCollection::Ptr | m_action |
Set up by processActionObservation. More... | |
mrpt::obs::CSensoryFrame::Ptr | m_SF |
Set up by processActionObservation. More... | |
mrpt::containers::bimap< mrpt::maps::CLandmark::TLandmarkID, unsigned int > | m_IDs |
The mapping between landmark IDs and indexes in the Pkk cov. More... | |
CIncrementalMapPartitioner | mapPartitioner |
Used for map partitioning experiments. More... | |
mrpt::maps::CSimpleMap | m_SFs |
The sequence of all the observations and the robot path (kept for debugging, statistics,etc) More... | |
std::vector< std::vector< uint32_t > > | m_lastPartitionSet |
TDataAssocInfo | m_last_data_association |
Last data association. More... | |
mrpt::system::CTimeLogger | m_timLogger |
VerbosityLevel | m_min_verbosity_level {LVL_INFO} |
Provided messages with VerbosityLevel smaller than this value shall be ignored. More... | |
Kalman filter state | |
KFVector | m_xkk |
The system state vector. More... | |
KFMatrix | m_pkk |
The system full covariance matrix. More... | |
Virtual methods for Kalman Filter implementation | |
virtual void | OnInverseObservationModel ([[maybe_unused]] const KFArray_OBS &in_z, [[maybe_unused]] KFArray_FEAT &out_yn, [[maybe_unused]] KFMatrix_FxV &out_dyn_dxv, [[maybe_unused]] KFMatrix_FxO &out_dyn_dhn) const |
If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element". More... | |
virtual void | OnInverseObservationModel (const KFArray_OBS &in_z, KFArray_FEAT &out_yn, KFMatrix_FxV &out_dyn_dxv, KFMatrix_FxO &out_dyn_dhn, [[maybe_unused]] KFMatrix_FxF &out_dyn_dhn_R_dyn_dhnT, bool &out_use_dyn_dhn_jacobian) const |
If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element". More... | |
virtual void | OnNewLandmarkAddedToMap ([[maybe_unused]] const size_t in_obsIdx, [[maybe_unused]] const size_t in_idxNewFeat) |
If applicable to the given problem, do here any special handling of adding a new landmark to the map. More... | |
virtual void | OnPostIteration () |
This method is called after finishing one KF iteration and before returning from runOneKalmanIteration(). More... | |
virtual void | OnGetAction (KFArray_ACT &out_u) const=0 |
Must return the action vector u. More... | |
virtual void | OnTransitionModel (const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const=0 |
Implements the transition model . More... | |
virtual void | OnTransitionJacobian ([[maybe_unused]] KFMatrix_VxV &out_F) const |
Implements the transition Jacobian . More... | |
virtual void | OnTransitionJacobianNumericGetIncrements (KFArray_VEH &out_increments) const |
Only called if using a numeric approximation of the transition Jacobian, this method must return the increments in each dimension of the vehicle state vector while estimating the Jacobian. More... | |
virtual void | OnTransitionNoise (KFMatrix_VxV &out_Q) const=0 |
Implements the transition noise covariance . More... | |
virtual void | OnPreComputingPredictions ([[maybe_unused]] 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 number of covariance landmark predictions to be made. More... | |
virtual void | OnGetObservationNoise (KFMatrix_OxO &out_R) const=0 |
Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor. More... | |
virtual 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)=0 |
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. More... | |
virtual void | OnObservationModel (const std::vector< size_t > &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const=0 |
Implements the observation prediction . More... | |
virtual void | OnObservationJacobians ([[maybe_unused]] size_t idx_landmark_to_predict, [[maybe_unused]] KFMatrix_OxV &Hx, [[maybe_unused]] KFMatrix_OxF &Hy) const |
Implements the observation Jacobians and (when applicable) . More... | |
virtual void | OnObservationJacobiansNumericGetIncrements (KFArray_VEH &out_veh_increments, KFArray_FEAT &out_feat_increments) const |
Only called if using a numeric approximation of the observation Jacobians, this method must return the increments in each dimension of the vehicle state vector while estimating the Jacobian. More... | |
virtual 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 scalar components (eg, angles). More... | |
Logging methods | |
bool | logging_enable_console_output {true} |
[Default=true] Set it to false in case you don't want the logged messages to be dumped to the output automatically. More... | |
bool | logging_enable_keep_record {false} |
[Default=false] Enables storing all messages into an internal list. More... | |
void | logStr (const VerbosityLevel level, std::string_view msg_str) const |
Main method to add the specified message string to the logger. More... | |
void | logFmt (const VerbosityLevel level, const char *fmt,...) const MRPT_printf_format_check(3 |
Alternative logging method, which mimics the printf behavior. More... | |
void void | logCond (const VerbosityLevel level, bool cond, const std::string &msg_str) const |
Log the given message only if the condition is satisfied. More... | |
void | setLoggerName (const std::string &name) |
Set the name of the COutputLogger instance. More... | |
std::string | getLoggerName () const |
Return the name of the COutputLogger instance. More... | |
void | setMinLoggingLevel (const VerbosityLevel level) |
Set the minimum logging level for which the incoming logs are going to be taken into account. More... | |
void | setVerbosityLevel (const VerbosityLevel level) |
alias of setMinLoggingLevel() More... | |
VerbosityLevel | getMinLoggingLevel () const |
bool | isLoggingLevelVisible (VerbosityLevel level) const |
void | getLogAsString (std::string &log_contents) const |
Fill the provided string with the contents of the logger's history in std::string representation. More... | |
std::string | getLogAsString () const |
Get the history of COutputLogger instance in a string representation. More... | |
void | writeLogToFile (const std::string *fname_in=nullptr) const |
Write the contents of the COutputLogger instance to an external file. More... | |
void | dumpLogToConsole () const |
Dump the current contents of the COutputLogger instance in the terminal window. More... | |
std::string | getLoggerLastMsg () const |
Return the last Tmsg instance registered in the logger history. More... | |
void | getLoggerLastMsg (std::string &msg_str) const |
Fill inputtted string with the contents of the last message in history. More... | |
void | loggerReset () |
Reset the contents of the logger instance. More... | |
void | logRegisterCallback (output_logger_callback_t userFunc) |
bool | logDeregisterCallback (output_logger_callback_t userFunc) |
|
inherited |
Definition at line 256 of file CKalmanFilterCapable.h.
|
inherited |
Definition at line 259 of file CKalmanFilterCapable.h.
|
inherited |
Definition at line 257 of file CKalmanFilterCapable.h.
|
inherited |
Definition at line 255 of file CKalmanFilterCapable.h.
|
inherited |
My class, in a shorter name!
Definition at line 237 of file CKalmanFilterCapable.h.
|
inherited |
Definition at line 241 of file CKalmanFilterCapable.h.
|
inherited |
Definition at line 246 of file CKalmanFilterCapable.h.
|
inherited |
Definition at line 245 of file CKalmanFilterCapable.h.
|
inherited |
Definition at line 251 of file CKalmanFilterCapable.h.
|
inherited |
Definition at line 250 of file CKalmanFilterCapable.h.
|
inherited |
Definition at line 252 of file CKalmanFilterCapable.h.
|
inherited |
Definition at line 244 of file CKalmanFilterCapable.h.
|
inherited |
Definition at line 253 of file CKalmanFilterCapable.h.
|
inherited |
Definition at line 249 of file CKalmanFilterCapable.h.
|
inherited |
Definition at line 248 of file CKalmanFilterCapable.h.
|
inherited |
Definition at line 243 of file CKalmanFilterCapable.h.
|
inherited |
The numeric type used in the Kalman Filter (default=double)
Definition at line 234 of file CKalmanFilterCapable.h.
|
inherited |
Definition at line 240 of file CKalmanFilterCapable.h.
Either mrpt::math::TPoint2D or mrpt::math::TPoint3D.
Definition at line 56 of file CRangeBearingKFSLAM.h.
|
inherited |
Definition at line 258 of file CKalmanFilterCapable.h.
CRangeBearingKFSLAM::CRangeBearingKFSLAM | ( | ) |
Constructor.
Definition at line 45 of file CRangeBearingKFSLAM.cpp.
References reset().
|
overridedefault |
Destructor:
double CRangeBearingKFSLAM::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 partitioning of the landmarks.
Definition at line 1169 of file CRangeBearingKFSLAM.cpp.
References mrpt::math::MatrixVectorBase< Scalar, Derived >::array(), ASSERT_, mrpt::math::countCommonElements(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::get_feature_size(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::get_vehicle_size(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_pkk, MRPT_END, MRPT_START, and mrpt::math::sum().
Referenced by kfslam_traits< CRangeBearingKFSLAM >::doPartitioningExperiment().
|
inherited |
Dump the current contents of the COutputLogger instance in the terminal window.
Definition at line 190 of file COutputLogger.cpp.
|
inlinestaticinherited |
Definition at line 227 of file CKalmanFilterCapable.h.
|
inlinestaticinherited |
Definition at line 226 of file CKalmanFilterCapable.h.
Referenced by computeOffDiagonalBlocksApproximationError(), getAs3DObject(), getCurrentState(), OnObservationJacobians(), OnObservationModel(), processActionObservation(), and saveMapAndPath2DRepresentationAsMATLABFile().
|
inlinestaticinherited |
Definition at line 225 of file CKalmanFilterCapable.h.
Referenced by OnGetObservationsAndDataAssociation().
|
inlinestaticinherited |
Definition at line 224 of file CKalmanFilterCapable.h.
Referenced by computeOffDiagonalBlocksApproximationError(), getAs3DObject(), getCurrentState(), OnObservationJacobians(), OnObservationModel(), OnTransitionModel(), OnTransitionNoise(), processActionObservation(), reset(), and saveMapAndPath2DRepresentationAsMATLABFile().
void CRangeBearingKFSLAM::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 current filter state.
out_objects |
Definition at line 965 of file CRangeBearingKFSLAM.cpp.
References mrpt::math::MatrixBase< Scalar, Derived >::blockCopy(), mrpt::opengl::stock_objects::CornerXYZ(), mrpt::poses::CPointPDFGaussian::cov, mrpt::opengl::CEllipsoid3D::Create(), mrpt::slam::CRangeBearingKFSLAM::TOptions::doPartitioningExperiment, mrpt::format(), mrpt::maps::CSimpleMap::get(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::get_feature_size(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::get_vehicle_size(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::getNumberOfLandmarksInTheMap(), mrpt::containers::bimap< KEY, VALUE >::inverse(), m_IDs, m_lastPartitionSet, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_pkk, m_SFs, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_xkk, mrpt::poses::CPointPDFGaussian::mean, options, mrpt::slam::CRangeBearingKFSLAM::TOptions::quantiles_3D_representation, mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::x(), and mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::y().
void CRangeBearingKFSLAM::getCurrentRobotPose | ( | mrpt::poses::CPose3DQuatPDFGaussian & | out_robotPose | ) | const |
Returns the mean & the 7x7 covariance matrix of the robot 6D pose (with rotation as a quaternion).
Definition at line 91 of file CRangeBearingKFSLAM.cpp.
References ASSERT_, mrpt::math::MatrixBase< Scalar, Derived >::blockCopy(), mrpt::poses::CPose3DQuatPDFGaussian::cov, mrpt::poses::CPose3DQuat::m_coords, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_pkk, mrpt::poses::CPose3DQuat::m_quat, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_xkk, mrpt::poses::CPose3DQuatPDFGaussian::mean, MRPT_END, MRPT_START, and mrpt::math::CVectorDynamic< T >::size().
Referenced by getCurrentRobotPose(), and processActionObservation().
|
inline |
Returns the mean & the 6x6 covariance matrix of the robot 6D pose (with rotation as 3 angles).
Definition at line 142 of file CRangeBearingKFSLAM.h.
References getCurrentRobotPose(), and mrpt::math::UNINITIALIZED_QUATERNION.
mrpt::poses::CPose3DQuat CRangeBearingKFSLAM::getCurrentRobotPoseMean | ( | ) | const |
Get the current robot pose mean, as a 3D+quaternion pose.
Definition at line 116 of file CRangeBearingKFSLAM.cpp.
References ASSERTDEB_, mrpt::poses::CPose3DQuat::m_coords, mrpt::poses::CPose3DQuat::m_quat, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_xkk, mrpt::math::CVectorDynamic< T >::size(), and mrpt::math::UNINITIALIZED_QUATERNION.
Referenced by OnInverseObservationModel(), OnObservationJacobians(), OnObservationModel(), OnTransitionJacobian(), OnTransitionModel(), and OnTransitionNoise().
void CRangeBearingKFSLAM::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.
out_robotPose | The mean and the 7x7 covariance matrix of the robot 6D pose |
out_landmarksPositions | One entry for each of the M landmark positions (3D). |
out_landmarkIDs | Each element[index] (for indices of out_landmarksPositions) gives the corresponding landmark ID. |
out_fullState | The complete state vector (7+3M). |
out_fullCovariance | The full (7+3M)x(7+3M) covariance matrix of the filter. |
Definition at line 135 of file CRangeBearingKFSLAM.cpp.
References ASSERT_, mrpt::math::CVectorDynamic< T >::begin(), mrpt::math::MatrixBase< Scalar, Derived >::blockCopy(), mrpt::poses::CPose3DQuatPDFGaussian::cov, mrpt::math::CVectorDynamic< T >::end(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::get_feature_size(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::get_vehicle_size(), mrpt::containers::bimap< KEY, VALUE >::getInverseMap(), mrpt::poses::CPose3DQuat::m_coords, m_IDs, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_pkk, mrpt::poses::CPose3DQuat::m_quat, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_xkk, mrpt::poses::CPose3DQuatPDFGaussian::mean, MRPT_END, MRPT_START, mrpt::math::CVectorDynamic< T >::resize(), and mrpt::math::CVectorDynamic< T >::size().
Referenced by getCurrentState().
|
inline |
Returns the complete mean and cov.
out_robotPose | The mean and the 7x7 covariance matrix of the robot 6D pose |
out_landmarksPositions | One entry for each of the M landmark positions (3D). |
out_landmarkIDs | Each element[index] (for indices of out_landmarksPositions) gives the corresponding landmark ID. |
out_fullState | The complete state vector (7+3M). |
out_fullCovariance | The full (7+3M)x(7+3M) covariance matrix of the filter. |
Definition at line 110 of file CRangeBearingKFSLAM.h.
References getCurrentState(), and mrpt::math::UNINITIALIZED_QUATERNION.
|
protected |
Return the last odometry, as a pose increment.
Definition at line 264 of file CRangeBearingKFSLAM.cpp.
References mrpt::slam::CRangeBearingKFSLAM::TOptions::force_ignore_odometry, m_action, and options.
Referenced by OnGetAction(), and OnTransitionJacobian().
|
inlineinherited |
Returns the covariance of the idx'th landmark (not applicable to non-SLAM problems).
std::exception | On idx>= getNumberOfLandmarksInTheMap() |
Definition at line 279 of file CKalmanFilterCapable.h.
References mrpt::math::MatrixBase< Scalar, Derived >::blockCopy(), and mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::m_pkk.
|
inlineinherited |
Returns the mean of the estimated value of the idx'th landmark (not applicable to non-SLAM problems).
std::exception | On idx>= getNumberOfLandmarksInTheMap() |
Definition at line 268 of file CKalmanFilterCapable.h.
References ASSERT_, mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::getNumberOfLandmarksInTheMap(), mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::m_xkk, and mrpt::system::os::memcpy().
|
inline |
Returns a read-only reference to the information on the last data-association.
Definition at line 251 of file CRangeBearingKFSLAM.h.
References m_last_data_association.
|
inline |
Return the last partition of the sequence of sensoryframes (it is NOT a partition of the map!!) Only if options.doPartitioningExperiment = true.
Definition at line 261 of file CRangeBearingKFSLAM.h.
References m_lastPartitionSet.
Referenced by kfslam_traits< CRangeBearingKFSLAM >::doPartitioningExperiment().
void CRangeBearingKFSLAM::getLastPartitionLandmarks | ( | std::vector< std::vector< uint32_t >> & | landmarksMembership | ) | const |
Return the partitioning of the landmarks in clusters accoring to the last partition.
Note that the same landmark may appear in different clusters (the partition is not in the space of landmarks) Only if options.doPartitioningExperiment = true
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 !!). |
Definition at line 1113 of file CRangeBearingKFSLAM.cpp.
References mrpt::slam::CRangeBearingKFSLAM::TOptions::doPartitioningExperiment, mrpt::maps::CSimpleMap::get(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::getNumberOfLandmarksInTheMap(), mrpt::containers::bimap< KEY, VALUE >::inverse(), m_IDs, m_lastPartitionSet, m_SFs, and options.
Referenced by kfslam_traits< CRangeBearingKFSLAM >::doPartitioningExperiment(), and getLastPartitionLandmarksAsIfFixedSubmaps().
void CRangeBearingKFSLAM::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 submaps (size K) were have been used.
Definition at line 1078 of file CRangeBearingKFSLAM.cpp.
References getLastPartitionLandmarks(), m_lastPartitionSet, m_SFs, and mrpt::maps::CSimpleMap::size().
Referenced by kfslam_traits< CRangeBearingKFSLAM >::doPartitioningExperiment().
|
inherited |
Fill the provided string with the contents of the logger's history in std::string representation.
Definition at line 154 of file COutputLogger.cpp.
|
inherited |
Get the history of COutputLogger instance in a string representation.
Definition at line 159 of file COutputLogger.cpp.
Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::getDescriptiveReport().
|
inherited |
Return the last Tmsg instance registered in the logger history.
Definition at line 195 of file COutputLogger.cpp.
References mrpt::system::COutputLogger::TMsg::getAsString().
|
inherited |
Fill inputtted string with the contents of the last message in history.
Definition at line 201 of file COutputLogger.cpp.
|
inherited |
Return the name of the COutputLogger instance.
Definition at line 143 of file COutputLogger.cpp.
|
inlineinherited |
Definition at line 201 of file system/COutputLogger.h.
References mrpt::system::COutputLogger::m_min_verbosity_level.
Referenced by mrpt::apps::RawlogGrabberApp::dump_verbose_info(), mrpt::maps::CRandomFieldGridMap2D::isEnabledVerbose(), mrpt::slam::CMetricMapBuilderRBPF::processActionObservation(), mrpt::apps::CGridMapAlignerApp::run(), mrpt::apps::RBPF_SLAM_App_Base::run(), and mrpt::apps::ICP_SLAM_App_Base::run().
|
inlineinherited |
Definition at line 228 of file CKalmanFilterCapable.h.
References mrpt::bayes::detail::getNumberOfLandmarksInMap().
Referenced by getAs3DObject(), getLastPartitionLandmarks(), and saveMapAndPath2DRepresentationAsMATLABFile().
|
inlineinherited |
Definition at line 596 of file CKalmanFilterCapable.h.
References mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::m_timLogger.
|
inlineinherited |
Definition at line 261 of file CKalmanFilterCapable.h.
References mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::m_xkk, and mrpt::math::CVectorDynamic< T >::size().
|
inlineinherited |
Definition at line 263 of file CKalmanFilterCapable.h.
References mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::m_pkk.
|
inlineinherited |
Definition at line 262 of file CKalmanFilterCapable.h.
References mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::m_xkk.
|
inlineinherited |
Definition at line 202 of file system/COutputLogger.h.
References mrpt::system::COutputLogger::m_min_verbosity_level.
Referenced by mrpt::slam::CMetricMapBuilderRBPF::processActionObservation(), and mrpt::system::COutputLoggerStreamWrapper::~COutputLoggerStreamWrapper().
|
inlineinherited |
Definition at line 232 of file CKalmanFilterCapable.h.
References mrpt::bayes::detail::isMapEmpty().
void CRangeBearingKFSLAM::loadOptions | ( | const mrpt::config::CConfigFileBase & | ini | ) |
Load options from a ini-like file/text.
Definition at line 733 of file CRangeBearingKFSLAM.cpp.
References mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::KF_options, mrpt::bayes::TKF_options::loadFromConfigFile(), mrpt::slam::CRangeBearingKFSLAM::TOptions::loadFromConfigFile(), mapPartitioner, mrpt::slam::CIncrementalMapPartitioner::options, and options.
|
inherited |
Log the given message only if the condition is satisfied.
Definition at line 131 of file COutputLogger.cpp.
|
inherited |
Definition at line 291 of file COutputLogger.cpp.
References getAddress(), and mrpt::system::COutputLogger::m_listCallbacks.
|
inherited |
Alternative logging method, which mimics the printf behavior.
Handy for not having to first use mrpt::format to pass a std::string message to logStr
Definition at line 91 of file COutputLogger.cpp.
Referenced by mrpt::hmtslam::CHMTSLAM::areaAbstraction(), mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::CICPCriteriaNRD(), mrpt::hmtslam::CTopLCDetector_GridMatching::computeTopologicalObservationModel(), CGraphSlamHandler< GRAPH_T >::execute(), mrpt::math::CLevenbergMarquardtTempl< VECTORTYPE, USERPARAM >::execute(), CGraphSlamHandler< GRAPH_T >::initOutputDir(), CGraphSlamHandler< GRAPH_T >::initVisualization(), mrpt::nav::CNavigatorManualSequence::navigationStep(), mrpt::nav::CAbstractNavigator::performNavigationStepNavigating(), CGraphSlamHandler< GRAPH_T >::readConfigFname(), CGraphSlamHandler< GRAPH_T >::saveResults(), CGraphSlamHandler< GRAPH_T >::setResultsDirName(), mrpt::nav::CReactiveNavigationSystem::STEP1_InitPTGs(), mrpt::hmtslam::CHMTSLAM::thread_3D_viewer(), mrpt::hmtslam::CHMTSLAM::thread_LSLAM(), mrpt::hmtslam::CHMTSLAM::thread_TBI(), and CGraphSlamHandler< GRAPH_T >::~CGraphSlamHandler().
|
inherited |
Reset the contents of the logger instance.
Called upon construction.
Definition at line 206 of file COutputLogger.cpp.
References mrpt::system::LVL_INFO.
|
staticinherited |
Map from VerbosityLevels to their corresponding mrpt::system::TConsoleColor.
Handy for coloring the input based on the verbosity of the message
Definition at line 47 of file COutputLogger.cpp.
References logging_levels_to_colors.
Referenced by mrpt::system::COutputLogger::TMsg::dumpToConsole().
|
staticinherited |
Map from VerbosityLevels to their corresponding names.
Handy for printing the current message VerbosityLevel along with the actual content
Definition at line 60 of file COutputLogger.cpp.
References logging_levels_to_names.
Referenced by mrpt::system::COutputLogger::TMsg::getAsString().
|
inherited |
Definition at line 278 of file COutputLogger.cpp.
References mrpt::system::COutputLogger::m_listCallbacks.
|
inherited |
Main method to add the specified message string to the logger.
Definition at line 72 of file COutputLogger.cpp.
References mrpt::system::COutputLogger::TMsg::body, mrpt::system::COutputLogger::TMsg::dumpToConsole(), mrpt::system::COutputLogger::TMsg::level, mrpt::system::COutputLogger::TMsg::name, and mrpt::system::COutputLogger::TMsg::timestamp.
Referenced by mrpt::slam::PF_implementation< mrpt::math::TPose3D, CMonteCarloLocalization3D, mrpt::bayes::particle_storage_mode::VALUE >::PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal(), mrpt::nav::CReactiveNavigationSystem::STEP1_InitPTGs(), mrpt::system::COutputLoggerStreamWrapper::~COutputLoggerStreamWrapper(), and mrpt::system::CTimeLoggerSaveAtDtor::~CTimeLoggerSaveAtDtor().
|
inline |
Provides access to the parameters of the map partitioning algorithm.
Definition at line 304 of file CRangeBearingKFSLAM.h.
References mapPartitioner, and mrpt::slam::CIncrementalMapPartitioner::options.
Referenced by kfslam_traits< CRangeBearingKFSLAM >::doPartitioningExperiment().
|
protectedpure virtualinherited |
Must return the action vector u.
out_u | The action vector which will be passed to OnTransitionModel |
|
overrideprotected |
Must return the action vector u.
out_u | The action vector which will be passed to OnTransitionModel |
Definition at line 289 of file CRangeBearingKFSLAM.cpp.
References getIncrementFromOdometry().
|
protectedpure virtualinherited |
Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor.
out_R | The noise covariance matrix. It might be non diagonal, but it'll usually be. |
|
overrideprotected |
Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor.
out_R | The noise covariance matrix. It might be non diagonal, but it'll usually be. |
Definition at line 1337 of file CRangeBearingKFSLAM.cpp.
References options, mrpt::square(), mrpt::slam::CRangeBearingKFSLAM::TOptions::std_sensor_pitch, mrpt::slam::CRangeBearingKFSLAM::TOptions::std_sensor_range, and mrpt::slam::CRangeBearingKFSLAM::TOptions::std_sensor_yaw.
|
overrideprotected |
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.
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. |
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. |
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". |
in_lm_indices_in_S | The indices of the map landmarks (range [0,getNumberOfLandmarksInTheMap()-1]) that can be found in the matrix in_S. |
This method will be called just once for each complete KF iteration.
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. |
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. |
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". |
in_lm_indices_in_S | The indices of the map landmarks (range [0,getNumberOfLandmarksInTheMap()-1]) that can be found in the matrix in_S. |
This method will be called just once for each complete KF iteration.
Definition at line 567 of file CRangeBearingKFSLAM.cpp.
References ASSERTMSG_, mrpt::slam::TDataAssociationResults::associations, mrpt::slam::CRangeBearingKFSLAM::TDataAssocInfo::clear(), mrpt::slam::CRangeBearingKFSLAM::TOptions::data_assoc_IC_chi2_thres, mrpt::slam::CRangeBearingKFSLAM::TOptions::data_assoc_IC_metric, mrpt::slam::CRangeBearingKFSLAM::TOptions::data_assoc_IC_ml_threshold, mrpt::slam::CRangeBearingKFSLAM::TOptions::data_assoc_method, mrpt::slam::CRangeBearingKFSLAM::TOptions::data_assoc_metric, mrpt::slam::data_association_full_covariance(), mrpt::containers::bimap< KEY, VALUE >::end(), mrpt::containers::bimap< KEY, VALUE >::find_key(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::get_observation_size(), m_IDs, m_last_data_association, m_SF, MRPT_END, MRPT_START, options, mrpt::slam::CRangeBearingKFSLAM::TDataAssocInfo::predictions_IDs, mrpt::slam::CRangeBearingKFSLAM::TDataAssocInfo::results, mrpt::math::CMatrixDynamic< T >::setSize(), mrpt::slam::CRangeBearingKFSLAM::TDataAssocInfo::Y_pred_covs, and mrpt::slam::CRangeBearingKFSLAM::TDataAssocInfo::Y_pred_means.
|
protectedpure virtualinherited |
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.
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. |
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. |
in_all_predictions | A vector with the prediction of ALL the landmarks in the map. Note that, in contrast, in_S only comprises a subset of all the landmarks. |
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". |
in_lm_indices_in_S | The indices of the map landmarks (range [0,getNumberOfLandmarksInTheMap()-1]) that can be found in the matrix in_S. |
This method will be called just once for each complete KF iteration.
|
overrideprotected |
If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".
in_z | The observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservations(). |
out_yn | The F-length vector with the inverse observation model . |
out_dyn_dxv | The Jacobian of the inv. sensor model wrt the robot pose . |
out_dyn_dhn | The Jacobian of the inv. sensor model wrt the observation vector . |
Definition at line 831 of file CRangeBearingKFSLAM.cpp.
References ASSERTMSG_, mrpt::poses::CPose3DQuat::composePoint(), getCurrentRobotPoseMean(), m_SF, MRPT_END, MRPT_START, mrpt::math::UNINITIALIZED_MATRIX, mrpt::math::UNINITIALIZED_QUATERNION, mrpt::math::TPoint3D_data< T >::x, mrpt::math::TPoint3D_data< T >::y, and mrpt::math::TPoint3D_data< T >::z.
|
inlinevirtualinherited |
If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".
in_z | The observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservationsAndDataAssociation(). |
out_yn | The F-length vector with the inverse observation model . |
out_dyn_dxv | The Jacobian of the inv. sensor model wrt the robot pose . |
out_dyn_dhn | The Jacobian of the inv. sensor model wrt the observation vector . |
Definition at line 489 of file CKalmanFilterCapable.h.
References MRPT_END, MRPT_START, and THROW_EXCEPTION.
|
inlinevirtualinherited |
If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".
The uncertainty in the new map feature comes from two parts: one from the vehicle uncertainty (through the out_dyn_dxv Jacobian), and another from the uncertainty in the observation itself. By default, out_use_dyn_dhn_jacobian=true on call, and if it's left at "true", the base KalmanFilter class will compute the uncertainty of the landmark relative position from out_dyn_dhn. Only in some problems (e.g. MonoSLAM), it'll be needed for the application to directly return the covariance matrix out_dyn_dhn_R_dyn_dhnT, which is the equivalent to:
.
but may be computed from additional terms, or whatever needed by the user.
in_z | The observation vector whose inverse sensor model is to be computed. This is actually one of the vector<> returned by OnGetObservationsAndDataAssociation(). |
out_yn | The F-length vector with the inverse observation model . |
out_dyn_dxv | The Jacobian of the inv. sensor model wrt the robot pose . |
out_dyn_dhn | The Jacobian of the inv. sensor model wrt the observation vector . |
out_dyn_dhn_R_dyn_dhnT | See the discussion above. |
Definition at line 540 of file CKalmanFilterCapable.h.
References MRPT_END, MRPT_START, and mrpt::bayes::CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::OnInverseObservationModel().
|
overrideprotected |
If applicable to the given problem, do here any special handling of adding a new landmark to the map.
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. |
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. |
Definition at line 932 of file CRangeBearingKFSLAM.cpp.
References ASSERT_, ASSERTMSG_, mrpt::containers::bimap< KEY, VALUE >::insert(), m_IDs, m_SF, MRPT_END, and MRPT_START.
|
inlinevirtualinherited |
If applicable to the given problem, do here any special handling of adding a new landmark to the map.
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. |
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. |
Definition at line 562 of file CKalmanFilterCapable.h.
|
overrideprotectedvirtual |
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.
This virtual function musts normalize the state vector and covariance matrix (only if its necessary).
Reimplemented from mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >.
Definition at line 711 of file CRangeBearingKFSLAM.cpp.
References ASSERTMSG_, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_xkk, MRPT_END, MRPT_START, and mrpt::square().
|
overrideprotected |
Implements the observation Jacobians and (when applicable) .
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. |
Hx | The output Jacobian . |
Hy | The output Jacobian . |
Definition at line 489 of file CRangeBearingKFSLAM.cpp.
References ASSERTMSG_, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::get_feature_size(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::get_vehicle_size(), getCurrentRobotPoseMean(), m_SF, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_xkk, MRPT_END, MRPT_START, mrpt::poses::CPose3DQuat::sphericalCoordinates(), mrpt::math::UNINITIALIZED_MATRIX, and mrpt::math::UNINITIALIZED_QUATERNION.
|
inlineprotectedvirtualinherited |
Implements the observation Jacobians and (when applicable) .
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. |
Hx | The output Jacobian . |
Hy | The output Jacobian . |
Definition at line 437 of file CKalmanFilterCapable.h.
|
inlineprotectedvirtualinherited |
Only called if using a numeric approximation of the observation Jacobians, this method must return the increments in each dimension of the vehicle state vector while estimating the Jacobian.
Definition at line 449 of file CKalmanFilterCapable.h.
|
overrideprotected |
Definition at line 418 of file CRangeBearingKFSLAM.cpp.
References ASSERTMSG_, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::get_feature_size(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::get_vehicle_size(), getCurrentRobotPoseMean(), m_SF, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_xkk, MRPT_END, MRPT_START, and mrpt::poses::CPose3DQuat::sphericalCoordinates().
|
protectedpure virtualinherited |
Implements the observation prediction .
idx_landmark_to_predict | The indices of the landmarks in the map whose predictions are expected as output. For non SLAM-like problems, this input value is undefined and the application should just generate one observation for the given problem. |
out_predictions | The predicted observations. |
|
inlinevirtualinherited |
This method is called after finishing one KF iteration and before returning from runOneKalmanIteration().
Definition at line 581 of file CKalmanFilterCapable.h.
|
inlineprotectedvirtualinherited |
This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the number of covariance landmark predictions to be made.
For example, features which are known to be "out of sight" shouldn't be added to the output list to speed up the calculations.
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. |
out_LM_indices_to_predict | The list of landmark indices in the map [0,getNumberOfLandmarksInTheMap()-1] that should be predicted. |
Definition at line 366 of file CKalmanFilterCapable.h.
|
overrideprotected |
This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the number of covariance landmark predictions to be made.
For example, features which are known to be "out of sight" shouldn't be added to the output list to speed up the calculations.
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. |
out_LM_indices_to_predict | The list of landmark indices in the map [0,getNumberOfLandmarksInTheMap()-1] that should be predicted. |
Definition at line 1359 of file CRangeBearingKFSLAM.cpp.
References ASSERTMSG_, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_pkk, m_SF, options, mrpt::slam::CRangeBearingKFSLAM::TOptions::std_sensor_pitch, mrpt::slam::CRangeBearingKFSLAM::TOptions::std_sensor_range, and mrpt::slam::CRangeBearingKFSLAM::TOptions::std_sensor_yaw.
|
overrideprotected |
Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scalar components (eg, angles).
Definition at line 1324 of file CRangeBearingKFSLAM.cpp.
References mrpt::math::wrapToPiInPlace().
|
inlineprotectedvirtualinherited |
Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scalar components (eg, angles).
Definition at line 460 of file CKalmanFilterCapable.h.
|
inlineprotectedvirtualinherited |
Implements the transition Jacobian .
out_F | Must return the Jacobian. The returned matrix must be with V being either the size of the whole state vector (for non-SLAM problems) or VEH_SIZE (for SLAM problems). |
Definition at line 328 of file CKalmanFilterCapable.h.
|
overrideprotected |
Implements the transition Jacobian .
This virtual function musts calculate the Jacobian F of the prediction model.
out_F | Must return the Jacobian. The returned matrix must be with V being either the size of the whole state vector (for non-SLAM problems) or VEH_SIZE (for SLAM problems). |
Definition at line 341 of file CRangeBearingKFSLAM.cpp.
References getCurrentRobotPoseMean(), getIncrementFromOdometry(), MRPT_END, MRPT_START, and mrpt::math::UNINITIALIZED_MATRIX.
|
inlineprotectedvirtualinherited |
Only called if using a numeric approximation of the transition Jacobian, this method must return the increments in each dimension of the vehicle state vector while estimating the Jacobian.
Definition at line 338 of file CKalmanFilterCapable.h.
|
protectedpure virtualinherited |
Implements the transition model .
in_u | The vector returned by OnGetAction. |
inout_x | At input has , at output must have . |
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 |
|
overrideprotected |
Implements the transition model .
This virtual function musts implement the prediction model of the Kalman filter.
in_u | The vector returned by OnGetAction. |
inout_x | At input has , at output must have . |
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 |
Definition at line 302 of file CRangeBearingKFSLAM.cpp.
References mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::get_vehicle_size(), getCurrentRobotPoseMean(), mrpt::poses::CPose3DQuat::m_coords, mrpt::poses::CPose3DQuat::m_quat, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_xkk, MRPT_END, MRPT_START, mrpt::math::CVectorDynamic< T >::size(), and mrpt::math::UNINITIALIZED_QUATERNION.
|
protectedpure virtualinherited |
Implements the transition noise covariance .
out_Q | Must return the covariance matrix. The returned matrix must be of the same size than the jacobian from OnTransitionJacobian |
|
overrideprotected |
Implements the transition noise covariance .
This virtual function musts calculate de noise matrix of the prediction model.
out_Q | Must return the covariance matrix. The returned matrix must be of the same size than the jacobian from OnTransitionJacobian |
Definition at line 366 of file CRangeBearingKFSLAM.cpp.
References ASSERT_, mrpt::poses::CPose3DQuatPDFGaussian::changeCoordinatesReference(), mrpt::poses::CPosePDFGaussian::copyFrom(), mrpt::poses::CPose3DQuatPDFGaussian::cov, mrpt::slam::CRangeBearingKFSLAM::TOptions::force_ignore_odometry, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::get_vehicle_size(), getCurrentRobotPoseMean(), m_action, MRPT_END, MRPT_START, options, mrpt::math::CVectorDynamic< T >::size(), mrpt::square(), mrpt::slam::CRangeBearingKFSLAM::TOptions::std_odo_z_additional, mrpt::slam::CRangeBearingKFSLAM::TOptions::stds_Q_no_odo, and THROW_EXCEPTION.
void CRangeBearingKFSLAM::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.
See the description of the class at the top of this page.
action | May contain odometry |
SF | The set of observations, must contain at least one CObservationBearingRange |
Definition at line 186 of file CRangeBearingKFSLAM.cpp.
References mrpt::slam::CIncrementalMapPartitioner::addMapFrame(), ASSERT_, mrpt::math::CMatrixDynamic< T >::cols(), mrpt::slam::CRangeBearingKFSLAM::TOptions::create_simplemap, mrpt::slam::CRangeBearingKFSLAM::TOptions::doPartitioningExperiment, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::get_feature_size(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::get_vehicle_size(), getCurrentRobotPose(), mrpt::maps::CSimpleMap::insert(), m_action, m_IDs, m_lastPartitionSet, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_pkk, m_SF, m_SFs, mapPartitioner, MRPT_END, MRPT_START, options, mrpt::slam::CRangeBearingKFSLAM::TOptions::partitioningMethod, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::runOneKalmanIteration(), mrpt::containers::bimap< KEY, VALUE >::size(), mrpt::maps::CSimpleMap::size(), mrpt::math::UNINITIALIZED_QUATERNION, and mrpt::slam::CIncrementalMapPartitioner::updatePartitions().
void CRangeBearingKFSLAM::reconsiderPartitionsNow | ( | ) |
The partitioning of the entire map is recomputed again.
Only when options.doPartitioningExperiment = true. This can be used after changing the parameters of the partitioning method. After this method, you can call getLastPartitionLandmarks.
Definition at line 1217 of file CRangeBearingKFSLAM.cpp.
References m_lastPartitionSet, mapPartitioner, and mrpt::slam::CIncrementalMapPartitioner::updatePartitions().
Referenced by kfslam_traits< CRangeBearingKFSLAM >::doPartitioningExperiment().
void CRangeBearingKFSLAM::reset | ( | ) |
Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,0,0).
Definition at line 60 of file CRangeBearingKFSLAM.cpp.
References mrpt::math::MatrixVectorBase< Scalar, Derived >::assign(), mrpt::containers::bimap< KEY, VALUE >::clear(), mrpt::slam::CIncrementalMapPartitioner::clear(), mrpt::maps::CSimpleMap::clear(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::get_vehicle_size(), m_action, m_IDs, m_lastPartitionSet, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_pkk, m_SF, m_SFs, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_xkk, mapPartitioner, mrpt::slam::CIncrementalMapPartitioner::options, mrpt::math::CMatrixDynamic< T >::setSize(), mrpt::math::MatrixVectorBase< Scalar, Derived >::setZero(), and mrpt::slam::smOBSERVATION_OVERLAP.
Referenced by CRangeBearingKFSLAM().
|
protectedinherited |
The main entry point, executes one complete step: prediction + update.
It is protected since derived classes must provide a problem-specific entry point for users. The exact order in which this method calls the virtual method is explained in https://www.mrpt.org/Kalman_Filters
Definition at line 28 of file CKalmanFilterCapable_impl.h.
References mrpt::bayes::detail::addNewLandmarks(), mrpt::math::CMatrixFixed< T, ROWS, COLS >::asEigen(), ASSERT_, ASSERTDEB_, ASSERTMSG_, mrpt::math::CMatrixFixed< T, ROWS, COLS >::cols(), mrpt::math::CMatrixDynamic< T >::cols(), mrpt::math::distance(), mrpt::math::estimateJacobian(), mrpt::math::extractSubmatrixSymmetrical(), mrpt::containers::find_in_vector(), mrpt::format(), mrpt::math::MatrixBase< Scalar, Derived >::inverse_LLt(), mrpt::bayes::kfEKFAlaDavison, mrpt::bayes::kfEKFNaive, mrpt::bayes::kfIKF, mrpt::bayes::kfIKFFull, mrpt::system::os::memcpy(), MRPT_END, MRPT_LOG_DEBUG, MRPT_LOG_WARN_STREAM, MRPT_START, R, mrpt::math::CMatrixDynamic< T >::setSize(), mrpt::math::size(), THROW_EXCEPTION, mrpt::math::UNINITIALIZED_MATRIX, and mrpt::io::vectorToTextFile().
Referenced by processActionObservation().
void CRangeBearingKFSLAM::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 elements in 2D.
Definition at line 1229 of file CRangeBearingKFSLAM.cpp.
References mrpt::math::cov(), mrpt::system::os::fclose(), mrpt::system::os::fopen(), mrpt::system::os::fprintf(), mrpt::maps::CSimpleMap::get(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::get_feature_size(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::get_vehicle_size(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::getNumberOfLandmarksInTheMap(), mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_pkk, m_SFs, mrpt::bayes::CKalmanFilterCapable< 7, 3, 3, 7 >::m_xkk, mrpt::math::MATLAB_plotCovariance2D(), mrpt::math::mean(), mrpt::maps::CSimpleMap::size(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::x(), and mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::y().
|
inherited |
Set the name of the COutputLogger instance.
Definition at line 138 of file COutputLogger.cpp.
Referenced by mrpt::slam::CMetricMapBuilderICP::CMetricMapBuilderICP(), mrpt::slam::CMetricMapBuilderRBPF::CMetricMapBuilderRBPF(), mrpt::slam::CMonteCarloLocalization2D::CMonteCarloLocalization2D(), mrpt::slam::CMonteCarloLocalization3D::CMonteCarloLocalization3D(), mrpt::apps::ICP_SLAM_App_Base::ICP_SLAM_App_Base(), mrpt::apps::ICP_SLAM_App_Live::ICP_SLAM_App_Live(), mrpt::apps::ICP_SLAM_App_Rawlog::ICP_SLAM_App_Rawlog(), mrpt::graphslam::CWindowManager::initCWindowManager(), mrpt::apps::MonteCarloLocalization_Base::MonteCarloLocalization_Base(), mrpt::apps::RBPF_SLAM_App_Base::RBPF_SLAM_App_Base(), and mrpt::apps::RBPF_SLAM_App_Rawlog::RBPF_SLAM_App_Rawlog().
|
inherited |
Set the minimum logging level for which the incoming logs are going to be taken into account.
String messages with specified VerbosityLevel smaller than the min, will not be outputted to the screen and neither will a record of them be stored in by the COutputLogger instance
Definition at line 144 of file COutputLogger.cpp.
Referenced by mrpt::maps::CRandomFieldGridMap2D::enableVerbose(), mrpt::math::CLevenbergMarquardtTempl< VECTORTYPE, USERPARAM >::execute(), generic_kf_slam_test(), generic_pf_test(), generic_rbpf_slam_test(), mrpt::apps::RawlogGrabberApp::initialize(), mrpt::hwdrivers::CHokuyoURG::initialize(), mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::loadParams(), mrpt::apps::CGridMapAlignerApp::run(), mrpt::apps::RBPF_SLAM_App_Base::run(), and mrpt::apps::ICP_SLAM_App_Base::run().
|
inherited |
alias of setMinLoggingLevel()
Definition at line 149 of file COutputLogger.cpp.
Referenced by mrpt::nav::CAbstractNavigator::CAbstractNavigator(), mrpt::slam::CMetricMapBuilderRBPF::CMetricMapBuilderRBPF(), mrpt::comms::CServerTCPSocket::CServerTCPSocket(), mrpt::slam::CMetricMapBuilderRBPF::processActionObservation(), mrpt::math::ransac_detect_2D_lines(), and mrpt::apps::ICP_SLAM_App_Base::run().
|
inherited |
Write the contents of the COutputLogger instance to an external file.
Upon call to this method, COutputLogger dumps the contents of all the logged commands so far to the specified external file. By default the filename is set to ${LOGGERNAME}.log except if the fname parameter is provided
Definition at line 165 of file COutputLogger.cpp.
References ASSERTMSG_, and mrpt::format().
|
inherited |
Generic options for the Kalman Filter algorithm itself.
Definition at line 598 of file CKalmanFilterCapable.h.
Referenced by loadOptions().
|
inherited |
[Default=true] Set it to false in case you don't want the logged messages to be dumped to the output automatically.
Definition at line 240 of file system/COutputLogger.h.
|
inherited |
[Default=false] Enables storing all messages into an internal list.
Definition at line 243 of file system/COutputLogger.h.
|
protected |
Set up by processActionObservation.
Definition at line 480 of file CRangeBearingKFSLAM.h.
Referenced by getIncrementFromOdometry(), OnTransitionNoise(), processActionObservation(), and reset().
|
protected |
The mapping between landmark IDs and indexes in the Pkk cov.
matrix:
Definition at line 487 of file CRangeBearingKFSLAM.h.
Referenced by getAs3DObject(), getCurrentState(), getLastPartitionLandmarks(), OnGetObservationsAndDataAssociation(), OnNewLandmarkAddedToMap(), processActionObservation(), and reset().
|
protected |
Last data association.
Definition at line 500 of file CRangeBearingKFSLAM.h.
Referenced by getLastDataAssociation(), and OnGetObservationsAndDataAssociation().
|
protected |
Definition at line 497 of file CRangeBearingKFSLAM.h.
Referenced by getAs3DObject(), getLastPartition(), getLastPartitionLandmarks(), getLastPartitionLandmarksAsIfFixedSubmaps(), processActionObservation(), reconsiderPartitionsNow(), and reset().
|
protectedinherited |
Provided messages with VerbosityLevel smaller than this value shall be ignored.
Definition at line 253 of file system/COutputLogger.h.
Referenced by mrpt::system::COutputLogger::getMinLoggingLevel(), and mrpt::system::COutputLogger::isLoggingLevelVisible().
|
protectedinherited |
The system full covariance matrix.
Definition at line 292 of file CKalmanFilterCapable.h.
Referenced by computeOffDiagonalBlocksApproximationError(), getAs3DObject(), getCurrentRobotPose(), getCurrentState(), OnPreComputingPredictions(), processActionObservation(), reset(), and saveMapAndPath2DRepresentationAsMATLABFile().
|
protected |
Set up by processActionObservation.
Definition at line 483 of file CRangeBearingKFSLAM.h.
Referenced by OnGetObservationsAndDataAssociation(), OnInverseObservationModel(), OnNewLandmarkAddedToMap(), OnObservationJacobians(), OnObservationModel(), OnPreComputingPredictions(), processActionObservation(), and reset().
|
protected |
The sequence of all the observations and the robot path (kept for debugging, statistics,etc)
Definition at line 495 of file CRangeBearingKFSLAM.h.
Referenced by getAs3DObject(), getLastPartitionLandmarks(), getLastPartitionLandmarksAsIfFixedSubmaps(), processActionObservation(), reset(), and saveMapAndPath2DRepresentationAsMATLABFile().
|
protectedinherited |
Definition at line 296 of file CKalmanFilterCapable.h.
|
protectedinherited |
The system state vector.
Definition at line 290 of file CKalmanFilterCapable.h.
Referenced by getAs3DObject(), getCurrentRobotPose(), getCurrentRobotPoseMean(), getCurrentState(), OnNormalizeStateVector(), OnObservationJacobians(), OnObservationModel(), OnTransitionModel(), reset(), and saveMapAndPath2DRepresentationAsMATLABFile().
|
protected |
Used for map partitioning experiments.
Definition at line 490 of file CRangeBearingKFSLAM.h.
Referenced by loadOptions(), mapPartitionOptions(), processActionObservation(), reconsiderPartitionsNow(), and reset().
mrpt::slam::CRangeBearingKFSLAM::TOptions mrpt::slam::CRangeBearingKFSLAM::options |
Referenced by kfslam_traits< CRangeBearingKFSLAM >::doPartitioningExperiment(), getAs3DObject(), getIncrementFromOdometry(), getLastPartitionLandmarks(), loadOptions(), OnGetObservationNoise(), OnGetObservationsAndDataAssociation(), OnPreComputingPredictions(), OnTransitionNoise(), and processActionObservation().
Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020 |