Go to the documentation of this file.
48 size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
64 const std::string& section)
override
86 out <<
"\n----------- [TKF_options] ------------ \n\n";
91 "verbosity_level = %s\n",
98 "enable_profiler = %c\n",
133 size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
139 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
142 VEH_SIZE, OBS_SIZE, 0 , ACT_SIZE, KFTYPE>& obj);
145 size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
151 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
154 VEH_SIZE, OBS_SIZE, 0 , ACT_SIZE, KFTYPE>& obj);
157 size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
162 VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE>::vector_KFArray_OBS& Z,
163 const std::vector<int>& data_association,
165 VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE>::KFMatrix_OxO&
R);
166 template <
size_t VEH_SIZE,
size_t OBS_SIZE,
size_t ACT_SIZE,
typename KFTYPE>
169 VEH_SIZE, OBS_SIZE, 0 , ACT_SIZE, KFTYPE>& obj,
171 VEH_SIZE, OBS_SIZE, 0 , ACT_SIZE,
172 KFTYPE>::vector_KFArray_OBS& Z,
173 const std::vector<int>& data_association,
175 VEH_SIZE, OBS_SIZE, 0 , ACT_SIZE,
176 KFTYPE>::KFMatrix_OxO&
R);
219 size_t VEH_SIZE,
size_t OBS_SIZE,
size_t FEAT_SIZE,
size_t ACT_SIZE,
220 typename KFTYPE =
double>
272 &feat[0], &
m_xkk[VEH_SIZE + idx * FEAT_SIZE],
273 FEAT_SIZE *
sizeof(
m_xkk[0]));
282 VEH_SIZE + idx * FEAT_SIZE, VEH_SIZE + idx * FEAT_SIZE);
320 bool& out_skipPrediction)
const = 0;
341 for (
size_t i = 0; i < VEH_SIZE; i++) out_increments[i] = 1e-6;
368 std::vector<size_t>& out_LM_indices_to_predict)
const
372 out_LM_indices_to_predict.resize(N);
373 for (
size_t i = 0; i < N; i++) out_LM_indices_to_predict[i] = i;
414 const std::vector<size_t>& in_lm_indices_in_S,
425 const std::vector<size_t>& idx_landmarks_to_predict,
438 [[maybe_unused]]
size_t idx_landmark_to_predict,
453 for (
size_t i = 0; i < VEH_SIZE; i++) out_veh_increments[i] = 1e-6;
454 for (
size_t i = 0; i < FEAT_SIZE; i++) out_feat_increments[i] = 1e-6;
497 "Inverse sensor model required but not implemented in derived "
544 bool& out_use_dyn_dhn_jacobian)
const
548 out_use_dyn_dhn_jacobian =
true;
563 [[maybe_unused]]
const size_t in_obsIdx,
564 [[maybe_unused]]
const size_t in_idxNewFeat)
632 const KFArray_VEH& x,
const std::pair<KFCLASS*, KFArray_ACT>& dat,
635 const KFArray_VEH& x,
const std::pair<KFCLASS*, size_t>& dat,
638 const KFArray_FEAT& x,
const std::pair<KFCLASS*, size_t>& dat,
642 size_t VEH_SIZEb,
size_t OBS_SIZEb,
size_t FEAT_SIZEb,
size_t ACT_SIZEb,
646 VEH_SIZEb, OBS_SIZEb, FEAT_SIZEb, ACT_SIZEb, KFTYPEb>& obj,
648 VEH_SIZEb, OBS_SIZEb, FEAT_SIZEb, ACT_SIZEb,
650 const std::vector<int>& data_association,
652 VEH_SIZEb, OBS_SIZEb, FEAT_SIZEb, ACT_SIZEb, KFTYPEb>::
KFMatrix_OxO&
668 #define CKalmanFilterCapable_H
TKF_options KF_options
Generic options for the Kalman Filter algorithm itself.
virtual void OnPostIteration()
This method is called after finishing one KF iteration and before returning from runOneKalmanIteratio...
size_t getNumberOfLandmarksInTheMap() const
static constexpr size_t get_vehicle_size()
static void KF_aux_estimate_trans_jacobian(const KFArray_VEH &x, const std::pair< KFCLASS *, KFArray_ACT > &dat, KFArray_VEH &out_x)
Auxiliary functions for Jacobian numeric estimation.
#define MRPT_ENUM_TYPE_END()
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.
#define MRPT_LOAD_CONFIG_VAR( variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
TKFMethod method
The method to employ (default: kfEKFNaive)
mrpt::math::CVectorFixed< KFTYPE, FEAT_SIZE > KFArray_FEAT
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X),...
TKF_options(mrpt::system::VerbosityLevel &verb_level_ref)
KFVector m_xkk
The system state vector.
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 th...
bool use_analytic_transition_jacobian
(default=true) If true, OnTransitionJacobian will be called; otherwise, the Jacobian will be estimate...
std::vector< KFMatrix_OxF > m_Hys
The vector of all partial Jacobians dh[i]_dy[i] for each prediction.
bool enable_profiler
If enabled (default=false), detailed timing information will be dumped to the console thru a CTimerLo...
static constexpr size_t get_feature_size()
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 th...
mrpt::vision::TStereoCalibResults out
virtual void OnNormalizeStateVector()
This method is called after the prediction and after the update, to give the user an opportunity to n...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::system::COutputLogger COutputLogger
virtual void OnTransitionNoise(KFMatrix_VxV &out_Q) const =0
Implements the transition noise covariance .
#define THROW_EXCEPTION(msg)
#define ASSERT_(f)
Defines an assertion mechanism.
#define MRPT_ENUM_TYPE_BEGIN(_ENUM_TYPE_WITH_NS)
bool isMapEmpty(const CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > &obj)
MRPT_FILL_ENUM(kfEKFNaive)
mrpt::math::CMatrixFixed< KFTYPE, VEH_SIZE, VEH_SIZE > KFMatrix_VxV
virtual void OnTransitionJacobianNumericGetIncrements(KFArray_VEH &out_increments) const
Only called if using a numeric approximation of the transition Jacobian, this method must return the ...
void loadFromConfigFile(const mrpt::config::CConfigFileBase &iniFile, const std::string §ion) override
This method load the options from a ".ini"-like file or memory-stored string list.
std::vector< KFArray_OBS > vector_KFArray_OBS
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) .
Generic options for the Kalman Filter algorithm in itself.
bool debug_verify_analytic_jacobians
(default=false) If true, will compute all the Jacobians numerically and compare them to the analytica...
KFMatrix m_dh_dx_full_obs
KFMatrix m_pkk
The system full covariance matrix.
A compile-time fixed-size numeric matrix container.
VerbosityLevel
Enumeration of available verbosity levels.
size_t getStateVectorLength() const
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 ...
This class allows loading and storing values and vectors of different types from a configuration text...
mrpt::math::CVectorFixed< KFTYPE, OBS_SIZE > KFArray_OBS
bool m_user_didnt_implement_jacobian
TKFMethod
The Kalman Filter algorithm to employ in bayes::CKalmanFilterCapable For further details on each algo...
virtual void OnObservationModel(const std::vector< size_t > &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const =0
Implements the observation prediction .
string iniFile(myDataDir+string("benchmark-options.ini"))
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
vector_KFArray_OBS m_all_predictions
static void KF_aux_estimate_obs_Hx_jacobian(const KFArray_VEH &x, const std::pair< KFCLASS *, size_t > &dat, KFArray_OBS &out_x)
void dumpToTextStream(std::ostream &out) const override
This method must display clearly all the contents of the structure in textual form,...
double kftype
The numeric type used in the Kalman Filter (default=double)
mrpt::math::CMatrixFixed< KFTYPE, OBS_SIZE, OBS_SIZE > KFMatrix_OxO
KFVector & internal_getXkk()
Versatile class for consistent logging and management of output messages.
int IKF_iterations
Number of refinement iterations, only for the IKF method.
CMatrixFixed< Scalar, BLOCK_ROWS, BLOCK_COLS > blockCopy(int start_row=0, int start_col=0) const
const blockCopy(): Returns a copy of the given block
size_type size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
mrpt::math::CVectorFixed< KFTYPE, VEH_SIZE > KFArray_VEH
static constexpr size_t get_action_size()
std::vector< size_t > m_predictLMidxs
mrpt::system::VerbosityLevel & verbosity_level
KFMatrix & internal_getPkk()
double debug_verify_analytic_jacobians_threshold
(default-1e-2) Sets the threshold for the difference between the analytic and the numerical jacobians
size_t getNumberOfLandmarksInMap(const CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > &obj)
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...
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
std::vector< KFMatrix_OxV > m_Hxs
The vector of all partial Jacobians dh[i]_dx for each prediction.
mrpt::math::CMatrixDynamic< KFTYPE > KFMatrix
void addNewLandmarks(CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > &obj, const typename CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::vector_KFArray_OBS &Z, const std::vector< int > &data_association, const typename CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE >::KFMatrix_OxO &R)
static void KF_aux_estimate_obs_Hy_jacobian(const KFArray_FEAT &x, const std::pair< KFCLASS *, size_t > &dat, KFArray_OBS &out_x)
void runOneKalmanIteration()
The main entry point, executes one complete step: prediction + update.
virtual void OnGetAction(KFArray_ACT &out_u) const =0
Must return the action vector u.
virtual void OnTransitionModel(const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const =0
Implements the transition model .
mrpt::system::CTimeLogger m_timLogger
mrpt::system::CTimeLogger & getProfiler()
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).
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 ex...
~CKalmanFilterCapable() override=default
Destructor.
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 scala...
void getLandmarkCov(size_t idx, KFMatrix_FxF &feat_cov) const
Returns the covariance of the idx'th landmark (not applicable to non-SLAM problems).
Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.
virtual void OnTransitionJacobian([[maybe_unused]] KFMatrix_VxV &out_F) const
Implements the transition Jacobian .
static constexpr size_t get_observation_size()
std::string std::string format(std::string_view fmt, ARGS &&... args)
bool use_analytic_observation_jacobian
(default=true) If true, OnObservationJacobians will be called; otherwise, the Jacobian will be estima...
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 ex...
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".
Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 29 13:06:46 UTC 2020 | |