MRPT  1.9.9
CKalmanFilterCapable_impl.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
11 #ifndef CKalmanFilterCapable_H
12 #error Include this file only from 'CKalmanFilterCapable.h'
13 #endif
14 
16 #include <mrpt/math/ops_matrices.h> // extractSubmatrixSymmetrical()
17 #include <Eigen/Dense>
18 
19 namespace mrpt
20 {
21 namespace bayes
22 {
23 // The main entry point in the Kalman Filter class:
24 template <
25  size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
26  typename KFTYPE>
27 void CKalmanFilterCapable<
28  VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE>::runOneKalmanIteration()
29 {
30  using namespace std;
32 
33  m_timLogger.enable(KF_options.enable_profiler);
34  m_timLogger.enter("KF:complete_step");
35 
36  ASSERT_(int(m_xkk.size()) == m_pkk.cols());
37  ASSERT_(size_t(m_xkk.size()) >= VEH_SIZE);
38  // =============================================================
39  // 1. CREATE ACTION MATRIX u FROM ODOMETRY
40  // =============================================================
41  KFArray_ACT u;
42 
43  m_timLogger.enter("KF:1.OnGetAction");
44  OnGetAction(u);
45  m_timLogger.leave("KF:1.OnGetAction");
46 
47  // Sanity check:
48  if (FEAT_SIZE)
49  {
50  ASSERTDEB_(
51  (((m_xkk.size() - VEH_SIZE) / FEAT_SIZE) * FEAT_SIZE) ==
52  (m_xkk.size() - VEH_SIZE));
53  }
54 
55  // =============================================================
56  // 2. PREDICTION OF NEW POSE xv_{k+1|k}
57  // =============================================================
58  m_timLogger.enter("KF:2.prediction stage");
59 
60  const size_t N_map = getNumberOfLandmarksInTheMap();
61 
62  // Vehicle pose
63  KFArray_VEH xv(&m_xkk[0]);
64 
65  bool skipPrediction = false; // Whether to skip the prediction step (in
66  // SLAM this is desired for the first
67  // iteration...)
68 
69  // Update mean: xv will have the updated pose until we update it in the
70  // filterState later.
71  // This is to maintain a copy of the last robot pose in the state vector,
72  // required for the Jacobian computation.
73  OnTransitionModel(u, xv, skipPrediction);
74 
75  if (!skipPrediction)
76  {
77  // =============================================================
78  // 3. PREDICTION OF COVARIANCE P_{k+1|k}
79  // =============================================================
80  // First, we compute de Jacobian fv_by_xv (derivative of f_vehicle wrt
81  // x_vehicle):
82  KFMatrix_VxV dfv_dxv;
83 
84  // Try closed-form Jacobian first:
85  m_user_didnt_implement_jacobian = false; // Set to true by the default
86  // method if not reimplemented
87  // in base class.
88  if (KF_options.use_analytic_transition_jacobian)
89  OnTransitionJacobian(dfv_dxv);
90 
91  if (m_user_didnt_implement_jacobian ||
92  !KF_options.use_analytic_transition_jacobian ||
93  KF_options.debug_verify_analytic_jacobians)
94  { // Numeric approximation:
95  KFArray_VEH xkk_vehicle(
96  &m_xkk[0]); // A copy of the vehicle part of the state vector.
97  KFArray_VEH xkk_veh_increments;
98  OnTransitionJacobianNumericGetIncrements(xkk_veh_increments);
99 
101  xkk_vehicle,
102  std::function<void(
103  const KFArray_VEH& x,
104  const std::pair<KFCLASS*, KFArray_ACT>& dat,
105  KFArray_VEH& out_x)>(&KF_aux_estimate_trans_jacobian),
106  xkk_veh_increments, std::pair<KFCLASS*, KFArray_ACT>(this, u),
107  dfv_dxv);
108 
109  if (KF_options.debug_verify_analytic_jacobians)
110  {
112  OnTransitionJacobian(dfv_dxv_gt);
113  if ((dfv_dxv - dfv_dxv_gt).sum_abs() >
114  KF_options.debug_verify_analytic_jacobians_threshold)
115  {
116  std::cerr << "[KalmanFilter] ERROR: User analytical "
117  "transition Jacobians are wrong: \n"
118  << " Real dfv_dxv: \n"
119  << dfv_dxv << "\n Analytical dfv_dxv:\n"
120  << dfv_dxv_gt << "Diff:\n"
121  << (dfv_dxv - dfv_dxv_gt) << "\n";
123  "ERROR: User analytical transition Jacobians are wrong "
124  "(More details dumped to cerr)");
125  }
126  }
127  }
128 
129  // Q is the process noise covariance matrix, is associated to the robot
130  // movement and is necesary to calculate the prediction P(k+1|k)
131  KFMatrix_VxV Q;
132  OnTransitionNoise(Q);
133 
134  // ====================================
135  // 3.1: Pxx submatrix
136  // ====================================
137  // Replace old covariance:
138  m_pkk.asEigen().template block<VEH_SIZE, VEH_SIZE>(0, 0) =
139  Q.asEigen() + dfv_dxv.asEigen() *
140  m_pkk.template block<VEH_SIZE, VEH_SIZE>(0, 0) *
141  dfv_dxv.asEigen().transpose();
142 
143  // ====================================
144  // 3.2: All Pxy_i
145  // ====================================
146  // Now, update the cov. of landmarks, if any:
147  KFMatrix_VxF aux;
148  for (size_t i = 0; i < N_map; i++)
149  {
150  aux = dfv_dxv.asEigen() * m_pkk.template block<VEH_SIZE, FEAT_SIZE>(
151  0, VEH_SIZE + i * FEAT_SIZE);
152 
153  m_pkk.asEigen().template block<VEH_SIZE, FEAT_SIZE>(
154  0, VEH_SIZE + i * FEAT_SIZE) = aux.asEigen();
155  m_pkk.asEigen().template block<FEAT_SIZE, VEH_SIZE>(
156  VEH_SIZE + i * FEAT_SIZE, 0) = aux.asEigen().transpose();
157  }
158 
159  // =============================================================
160  // 4. NOW WE CAN OVERWRITE THE NEW STATE VECTOR
161  // =============================================================
162  for (size_t i = 0; i < VEH_SIZE; i++) m_xkk[i] = xv[i];
163 
164  // Normalize, if neccesary.
165  OnNormalizeStateVector();
166 
167  } // end if (!skipPrediction)
168 
169  const double tim_pred = m_timLogger.leave("KF:2.prediction stage");
170 
171  // =============================================================
172  // 5. PREDICTION OF OBSERVATIONS AND COMPUTE JACOBIANS
173  // =============================================================
174  m_timLogger.enter("KF:3.predict all obs");
175 
176  KFMatrix_OxO R; // Sensor uncertainty (covariance matrix): R
177  OnGetObservationNoise(R);
178 
179  // Predict the observations for all the map LMs, so the user
180  // can decide if their covariances (more costly) must be computed as well:
181  m_all_predictions.resize(N_map);
182  OnObservationModel(
183  mrpt::math::sequenceStdVec<size_t, 1>(0, N_map), m_all_predictions);
184 
185  const double tim_pred_obs = m_timLogger.leave("KF:3.predict all obs");
186 
187  m_timLogger.enter("KF:4.decide pred obs");
188 
189  // Decide if some of the covariances shouldn't be predicted.
190  m_predictLMidxs.clear();
191  if (FEAT_SIZE == 0)
192  // In non-SLAM problems, just do one prediction, for the entire system
193  // state:
194  m_predictLMidxs.assign(1, 0);
195  else
196  // On normal SLAM problems:
197  OnPreComputingPredictions(m_all_predictions, m_predictLMidxs);
198 
199  m_timLogger.leave("KF:4.decide pred obs");
200 
201  // =============================================================
202  // 6. COMPUTE INNOVATION MATRIX "m_S"
203  // =============================================================
204  // Do the prediction of the observation covariances:
205  // Compute m_S: m_S = H P H' + R
206  //
207  // Build a big dh_dx Jacobian composed of the small block Jacobians.
208  // , but: it's actually a subset of the full Jacobian, since the
209  // non-predicted
210  // features do not appear.
211  //
212  // dh_dx: O*M x V+M*F
213  // m_S: O*M x O*M
214  // M = |m_predictLMidxs|
215  // O=size of each observation.
216  // F=size of features in the map
217  //
218  // Updated: Now we only keep the non-zero blocks of that Jacobian,
219  // in the vectors m_Hxs[] and m_Hys[].
220  //
221 
222  m_timLogger.enter("KF:5.build Jacobians");
223 
224  size_t N_pred =
225  FEAT_SIZE == 0
226  ? 1 /* In non-SLAM problems, there'll be only 1 fixed observation */
227  : m_predictLMidxs.size();
228 
229  std::vector<int>
230  data_association; // -1: New map feature.>=0: Indexes in the
231  // state vector
232 
233  // The next loop will only do more than one iteration if the heuristic in
234  // OnPreComputingPredictions() fails,
235  // which will be detected by the addition of extra landmarks to predict
236  // into "missing_predictions_to_add"
237  std::vector<size_t> missing_predictions_to_add;
238 
239  m_Hxs.resize(N_pred); // Lists of partial Jacobians
240  m_Hys.resize(N_pred);
241 
242  size_t first_new_pred = 0; // This will be >0 only if we perform multiple
243  // loops due to failures in the prediction
244  // heuristic.
245 
246  do
247  {
248  if (!missing_predictions_to_add.empty())
249  {
250  const size_t nNew = missing_predictions_to_add.size();
252  "[KF] *Performance Warning*: "
253  << nNew
254  << " LMs were not correctly predicted by "
255  "OnPreComputingPredictions().");
256 
257  ASSERTDEB_(FEAT_SIZE != 0);
258  for (size_t j = 0; j < nNew; j++)
259  m_predictLMidxs.push_back(missing_predictions_to_add[j]);
260 
261  N_pred = m_predictLMidxs.size();
262  missing_predictions_to_add.clear();
263  }
264 
265  m_Hxs.resize(N_pred); // Append new entries, if needed.
266  m_Hys.resize(N_pred);
267 
268  for (size_t i = first_new_pred; i < N_pred; ++i)
269  {
270  const size_t lm_idx = FEAT_SIZE == 0 ? 0 : m_predictLMidxs[i];
271  KFMatrix_OxV& Hx = m_Hxs[i];
272  KFMatrix_OxF& Hy = m_Hys[i];
273 
274  // Try the analitic Jacobian first:
275  m_user_didnt_implement_jacobian =
276  false; // Set to true by the default method if not
277  // reimplemented in base class.
278  if (KF_options.use_analytic_observation_jacobian)
279  OnObservationJacobians(lm_idx, Hx, Hy);
280 
281  if (m_user_didnt_implement_jacobian ||
282  !KF_options.use_analytic_observation_jacobian ||
283  KF_options.debug_verify_analytic_jacobians)
284  { // Numeric approximation:
285  const size_t lm_idx_in_statevector =
286  VEH_SIZE + lm_idx * FEAT_SIZE;
287 
288  const KFArray_VEH x_vehicle(&m_xkk[0]);
289  const KFArray_FEAT x_feat(&m_xkk[lm_idx_in_statevector]);
290 
291  KFArray_VEH xkk_veh_increments;
292  KFArray_FEAT feat_increments;
293  OnObservationJacobiansNumericGetIncrements(
294  xkk_veh_increments, feat_increments);
295 
297  x_vehicle,
298  std::function<void(
299  const KFArray_VEH& x,
300  const std::pair<KFCLASS*, size_t>& dat,
301  KFArray_OBS& out_x)>(&KF_aux_estimate_obs_Hx_jacobian),
302  xkk_veh_increments,
303  std::pair<KFCLASS*, size_t>(this, lm_idx), Hx);
304  // The state vector was temporarily modified by
305  // KF_aux_estimate_*, restore it:
306  std::memcpy(
307  &m_xkk[0], &x_vehicle[0], sizeof(m_xkk[0]) * VEH_SIZE);
308 
310  x_feat,
311  std::function<void(
312  const KFArray_FEAT& x,
313  const std::pair<KFCLASS*, size_t>& dat,
314  KFArray_OBS& out_x)>(&KF_aux_estimate_obs_Hy_jacobian),
315  feat_increments, std::pair<KFCLASS*, size_t>(this, lm_idx),
316  Hy);
317  // The state vector was temporarily modified by
318  // KF_aux_estimate_*, restore it:
319  std::memcpy(
320  &m_xkk[lm_idx_in_statevector], &x_feat[0],
321  sizeof(m_xkk[0]) * FEAT_SIZE);
322 
323  if (KF_options.debug_verify_analytic_jacobians)
324  {
327  OnObservationJacobians(lm_idx, Hx_gt, Hy_gt);
328  if (KFMatrix(Hx - Hx_gt).sum_abs() >
329  KF_options.debug_verify_analytic_jacobians_threshold)
330  {
331  std::cerr << "[KalmanFilter] ERROR: User analytical "
332  "observation Hx Jacobians are wrong: \n"
333  << " Real Hx: \n"
334  << Hx.asEigen() << "\n Analytical Hx:\n"
335  << Hx_gt.asEigen() << "Diff:\n"
336  << (Hx.asEigen() - Hx_gt.asEigen()) << "\n";
338  "ERROR: User analytical observation Hx Jacobians "
339  "are wrong (More details dumped to cerr)");
340  }
341  if (KFMatrix(Hy - Hy_gt).sum_abs() >
342  KF_options.debug_verify_analytic_jacobians_threshold)
343  {
344  std::cerr << "[KalmanFilter] ERROR: User analytical "
345  "observation Hy Jacobians are wrong: \n"
346  << " Real Hy: \n"
347  << Hy.asEigen() << "\n Analytical Hx:\n"
348  << Hy_gt.asEigen() << "Diff:\n"
349  << Hy.asEigen() - Hy_gt.asEigen() << "\n";
351  "ERROR: User analytical observation Hy Jacobians "
352  "are wrong (More details dumped to cerr)");
353  }
354  }
355  }
356  }
357  m_timLogger.leave("KF:5.build Jacobians");
358 
359  m_timLogger.enter("KF:6.build m_S");
360 
361  // Compute m_S: m_S = H P H' + R (R will be added below)
362  // exploiting the sparsity of H:
363  // Each block in m_S is:
364  // Sij =
365  // ------------------------------------------
366  m_S.setSize(N_pred * OBS_SIZE, N_pred * OBS_SIZE);
367 
368  if (FEAT_SIZE > 0)
369  { // SLAM-like problem:
370  // Covariance of the vehicle pose
371  const auto Px = m_pkk.template block<VEH_SIZE, VEH_SIZE>(0, 0);
372 
373  for (size_t i = 0; i < N_pred; ++i)
374  {
375  const size_t lm_idx_i = m_predictLMidxs[i];
376  // Pxyi^t
377  const auto Pxyi_t = m_pkk.template block<FEAT_SIZE, VEH_SIZE>(
378  VEH_SIZE + lm_idx_i * FEAT_SIZE, 0);
379 
380  // Only do j>=i (upper triangle), since m_S is symmetric:
381  for (size_t j = i; j < N_pred; ++j)
382  {
383  const size_t lm_idx_j = m_predictLMidxs[j];
384  // Sij block:
386 
387  const auto Pxyj = m_pkk.template block<VEH_SIZE, FEAT_SIZE>(
388  0, VEH_SIZE + lm_idx_j * FEAT_SIZE);
389  const auto Pyiyj =
390  m_pkk.template block<FEAT_SIZE, FEAT_SIZE>(
391  VEH_SIZE + lm_idx_i * FEAT_SIZE,
392  VEH_SIZE + lm_idx_j * FEAT_SIZE);
393 
394  // clang-format off
395  Sij = m_Hxs[i].asEigen() * Px * m_Hxs[j].asEigen().transpose() +
396  m_Hys[i].asEigen() * Pxyi_t * m_Hxs[j].asEigen().transpose() +
397  m_Hxs[i].asEigen() * Pxyj * m_Hys[j].asEigen().transpose() +
398  m_Hys[i].asEigen() * Pyiyj * m_Hys[j].asEigen().transpose();
399  // clang-format on
400 
401  m_S.insertMatrix(OBS_SIZE * i, OBS_SIZE * j, Sij);
402 
403  // Copy transposed to the symmetric lower-triangular part:
404  if (i != j)
405  m_S.insertMatrixTransposed(
406  OBS_SIZE * j, OBS_SIZE * i, Sij);
407  }
408 
409  // Sum the "R" term to the diagonal blocks:
410  const size_t obs_idx_off = i * OBS_SIZE;
411  m_S.asEigen().template block<OBS_SIZE, OBS_SIZE>(
412  obs_idx_off, obs_idx_off) += R.asEigen();
413  }
414  }
415  else
416  { // Not SLAM-like problem: simply m_S=H*Pkk*H^t + R
417  ASSERTDEB_(N_pred == 1);
418  ASSERTDEB_(m_S.cols() == OBS_SIZE);
419 
420  m_S = m_Hxs[0].asEigen() * m_pkk.asEigen() *
421  m_Hxs[0].asEigen().transpose() +
422  R.asEigen();
423  }
424 
425  m_timLogger.leave("KF:6.build m_S");
426 
427  m_Z.clear(); // Each entry is one observation:
428 
429  m_timLogger.enter("KF:7.get obs & DA");
430 
431  // Get observations and do data-association:
432  OnGetObservationsAndDataAssociation(
433  m_Z, data_association, // Out
434  m_all_predictions, m_S, m_predictLMidxs, R // In
435  );
436  ASSERTDEB_(
437  data_association.size() == m_Z.size() ||
438  (data_association.empty() && FEAT_SIZE == 0));
439 
440  // Check if an observation hasn't been predicted in
441  // OnPreComputingPredictions() but has been actually
442  // observed. This may imply an error in the heuristic of
443  // OnPreComputingPredictions(), and forces us
444  // to rebuild the matrices
445  missing_predictions_to_add.clear();
446  if (FEAT_SIZE != 0)
447  {
448  for (int i : data_association)
449  {
450  if (i < 0) continue;
451  const auto assoc_idx_in_map = static_cast<size_t>(i);
452  const size_t assoc_idx_in_pred =
454  assoc_idx_in_map, m_predictLMidxs);
455  if (assoc_idx_in_pred == std::string::npos)
456  missing_predictions_to_add.push_back(assoc_idx_in_map);
457  }
458  }
459 
460  first_new_pred = N_pred; // If we do another loop, start at the begin
461  // of new predictions
462 
463  } while (!missing_predictions_to_add.empty());
464 
465  const double tim_obs_DA = m_timLogger.leave("KF:7.get obs & DA");
466 
467  // =============================================================
468  // 7. UPDATE USING THE KALMAN GAIN
469  // =============================================================
470  // Update, only if there are observations!
471  if (!m_Z.empty())
472  {
473  m_timLogger.enter("KF:8.update stage");
474 
475  switch (KF_options.method)
476  {
477  // -----------------------
478  // FULL KF- METHOD
479  // -----------------------
480  case kfEKFNaive:
481  case kfIKFFull:
482  {
483  // Build the whole Jacobian dh_dx matrix
484  // ---------------------------------------------
485  // Keep only those whose DA is not -1
486  std::vector<int> mapIndicesForKFUpdate(data_association.size());
487  mapIndicesForKFUpdate.resize(std::distance(
488  mapIndicesForKFUpdate.begin(),
489  std::remove_copy_if(
490  data_association.begin(), data_association.end(),
491  mapIndicesForKFUpdate.begin(),
492  [](int i) { return i == -1; })));
493 
494  const size_t N_upd =
495  (FEAT_SIZE == 0) ? 1 : // Non-SLAM problems: Just one
496  // observation for the entire
497  // system.
498  mapIndicesForKFUpdate
499  .size(); // SLAM: # of observed known landmarks
500 
501  // Just one, or several update iterations??
502  const size_t nKF_iterations = (KF_options.method == kfEKFNaive)
503  ? 1
504  : KF_options.IKF_iterations;
505 
506  const KFVector xkk_0 = m_xkk;
507 
508  // For each IKF iteration (or 1 for EKF)
509  if (N_upd > 0) // Do not update if we have no observations!
510  {
511  for (size_t IKF_iteration = 0;
512  IKF_iteration < nKF_iterations; IKF_iteration++)
513  {
514  // Compute ytilde = OBS - PREDICTION
515  KFVector ytilde(OBS_SIZE * N_upd);
516  size_t ytilde_idx = 0;
517 
518  // TODO: Use a Matrix view of "dh_dx_full" instead of
519  // creating a copy into "m_dh_dx_full_obs"
520  m_dh_dx_full_obs.setZero(
521  N_upd * OBS_SIZE,
522  VEH_SIZE + FEAT_SIZE * N_map); // Init to zeros.
523  KFMatrix S_observed; // The KF "m_S" matrix: A
524  // re-ordered, subset, version of
525  // the prediction m_S:
526 
527  if (FEAT_SIZE != 0)
528  { // SLAM problems:
529  std::vector<size_t> S_idxs;
530  S_idxs.reserve(OBS_SIZE * N_upd);
531 
532  // const size_t row_len = VEH_SIZE + FEAT_SIZE *
533  // N_map;
534 
535  for (size_t i = 0; i < data_association.size(); ++i)
536  {
537  if (data_association[i] < 0) continue;
538 
539  const auto assoc_idx_in_map =
540  static_cast<size_t>(data_association[i]);
541  const size_t assoc_idx_in_pred =
543  assoc_idx_in_map, m_predictLMidxs);
544  ASSERTMSG_(
545  assoc_idx_in_pred != string::npos,
546  "OnPreComputingPredictions() didn't "
547  "recommend the prediction of a landmark "
548  "which has been actually observed!");
549  // TODO: In these cases, extend the prediction
550  // right now instead of launching an
551  // exception... or is this a bad idea??
552 
553  // Build the subset m_dh_dx_full_obs:
554  // m_dh_dx_full_obs.block(S_idxs.size()
555  //,0, OBS_SIZE, row_len)
556  // =
557  // dh_dx_full.block
558  //(assoc_idx_in_pred*OBS_SIZE,0, OBS_SIZE,
559  // row_len);
560 
561  m_dh_dx_full_obs
562  .template block<OBS_SIZE, VEH_SIZE>(
563  S_idxs.size(), 0) =
564  m_Hxs[assoc_idx_in_pred].asEigen();
565  m_dh_dx_full_obs
566  .template block<OBS_SIZE, FEAT_SIZE>(
567  S_idxs.size(),
568  VEH_SIZE +
569  assoc_idx_in_map * FEAT_SIZE) =
570  m_Hys[assoc_idx_in_pred].asEigen();
571 
572  // S_idxs.size() is used as counter for
573  // "m_dh_dx_full_obs".
574  for (size_t k = 0; k < OBS_SIZE; k++)
575  S_idxs.push_back(
576  assoc_idx_in_pred * OBS_SIZE + k);
577 
578  // ytilde_i = Z[i] - m_all_predictions[i]
579  KFArray_OBS ytilde_i = m_Z[i];
580  OnSubstractObservationVectors(
581  ytilde_i,
582  m_all_predictions
583  [m_predictLMidxs[assoc_idx_in_pred]]);
584  for (size_t k = 0; k < OBS_SIZE; k++)
585  ytilde[ytilde_idx++] = ytilde_i[k];
586  }
587  // Extract the subset that is involved in this
588  // observation:
590  m_S, S_idxs, S_observed);
591  }
592  else
593  { // Non-SLAM problems:
594  ASSERT_(
595  m_Z.size() == 1 &&
596  m_all_predictions.size() == 1);
597  ASSERT_(
598  m_dh_dx_full_obs.rows() == OBS_SIZE &&
599  m_dh_dx_full_obs.cols() == VEH_SIZE);
600  ASSERT_(m_Hxs.size() == 1);
601  m_dh_dx_full_obs = m_Hxs[0]; // Was: dh_dx_full
602  KFArray_OBS ytilde_i = m_Z[0];
603  OnSubstractObservationVectors(
604  ytilde_i, m_all_predictions[0]);
605  for (size_t k = 0; k < OBS_SIZE; k++)
606  ytilde[ytilde_idx++] = ytilde_i[k];
607  // Extract the subset that is involved in this
608  // observation:
609  S_observed = m_S;
610  }
611 
612  // Compute the full K matrix:
613  // ------------------------------
614  m_timLogger.enter("KF:8.update stage:1.FULLKF:build K");
615 
616  m_K.setSize(m_pkk.rows(), S_observed.cols());
617 
618  // K = m_pkk * (~dh_dx) * m_S.inverse_LLt() );
619  m_K.asEigen() = m_pkk.asEigen() *
620  m_dh_dx_full_obs.asEigen().transpose();
621 
622  // Inverse of S_observed -> m_S_1
623  m_S_1 = S_observed.inverse_LLt();
624  m_K.asEigen() *= m_S_1.asEigen();
625 
626  m_timLogger.leave("KF:8.update stage:1.FULLKF:build K");
627 
628  // Use the full K matrix to update the mean:
629  if (nKF_iterations == 1)
630  {
631  m_timLogger.enter(
632  "KF:8.update stage:2.FULLKF:update xkk");
633  m_xkk.asEigen() += (m_K * ytilde).eval();
634  m_timLogger.leave(
635  "KF:8.update stage:2.FULLKF:update xkk");
636  }
637  else
638  {
639  m_timLogger.enter(
640  "KF:8.update stage:2.FULLKF:iter.update xkk");
641 
642  auto HAx_column =
643  KFVector(m_dh_dx_full_obs * (m_xkk - xkk_0));
644 
645  m_xkk = xkk_0;
646  m_xkk.asEigen() += m_K * (ytilde - HAx_column);
647 
648  m_timLogger.leave(
649  "KF:8.update stage:2.FULLKF:iter.update xkk");
650  }
651 
652  // Update the covariance just at the end
653  // of iterations if we are in IKF, always in normal
654  // EKF.
655  if (IKF_iteration == (nKF_iterations - 1))
656  {
657  m_timLogger.enter(
658  "KF:8.update stage:3.FULLKF:update Pkk");
659 
660  // Use the full K matrix to update the covariance:
661  // m_pkk = (I - K*dh_dx ) * m_pkk;
662  // TODO: "Optimize this: sparsity!"
663 
664  // K * m_dh_dx_full_obs
665  m_aux_K_dh_dx.matProductOf_AB(
666  m_K, m_dh_dx_full_obs);
667 
668  // m_aux_K_dh_dx <-- I-m_aux_K_dh_dx
669  const size_t stat_len = m_aux_K_dh_dx.cols();
670  for (size_t r = 0; r < stat_len; r++)
671  {
672  for (size_t c = 0; c < stat_len; c++)
673  {
674  if (r == c)
675  m_aux_K_dh_dx(r, c) =
676  -m_aux_K_dh_dx(r, c) + kftype(1);
677  else
678  m_aux_K_dh_dx(r, c) =
679  -m_aux_K_dh_dx(r, c);
680  }
681  }
682 
683  m_pkk = m_aux_K_dh_dx * m_pkk;
684 
685  m_timLogger.leave(
686  "KF:8.update stage:3.FULLKF:update Pkk");
687  }
688  } // end for each IKF iteration
689  }
690  }
691  break;
692 
693  // --------------------------------------------------------------------
694  // - EKF 'a la' Davison: One observation element at once
695  // --------------------------------------------------------------------
696  case kfEKFAlaDavison:
697  {
698  // For each observed landmark/whole system state:
699  for (size_t obsIdx = 0; obsIdx < m_Z.size(); obsIdx++)
700  {
701  // Known & mapped landmark?
702  bool doit;
703  size_t idxInTheFilter = 0;
704 
705  if (data_association.empty())
706  {
707  doit = true;
708  }
709  else
710  {
711  doit = data_association[obsIdx] >= 0;
712  if (doit) idxInTheFilter = data_association[obsIdx];
713  }
714 
715  if (doit)
716  {
717  m_timLogger.enter(
718  "KF:8.update stage:1.ScalarAtOnce.prepare");
719 
720  // Already mapped: OK
721  const size_t idx_off =
722  VEH_SIZE +
723  idxInTheFilter *
724  FEAT_SIZE; // The offset in m_xkk & Pkk.
725 
726  // Compute just the part of the Jacobian that we need
727  // using the current updated m_xkk:
728  vector_KFArray_OBS pred_obs;
729  OnObservationModel(
730  std::vector<size_t>(1, idxInTheFilter), pred_obs);
731  ASSERTDEB_(pred_obs.size() == 1);
732 
733  // ytilde = observation - prediction
734  KFArray_OBS ytilde = m_Z[obsIdx];
735  OnSubstractObservationVectors(ytilde, pred_obs[0]);
736 
737  // Jacobians:
738  // dh_dx: already is (N_pred*OBS_SIZE) x (VEH_SIZE +
739  // FEAT_SIZE * N_pred )
740  // with N_pred = |m_predictLMidxs|
741 
742  const size_t i_idx_in_preds =
744  idxInTheFilter, m_predictLMidxs);
745  ASSERTMSG_(
746  i_idx_in_preds != string::npos,
747  "OnPreComputingPredictions() didn't recommend the "
748  "prediction of a landmark which has been actually "
749  "observed!");
750 
751  const KFMatrix_OxV& Hx = m_Hxs[i_idx_in_preds];
752  const KFMatrix_OxF& Hy = m_Hys[i_idx_in_preds];
753 
754  m_timLogger.leave(
755  "KF:8.update stage:1.ScalarAtOnce.prepare");
756 
757  // For each component of the observation:
758  for (size_t j = 0; j < OBS_SIZE; j++)
759  {
760  m_timLogger.enter(
761  "KF:8.update stage:2.ScalarAtOnce.update");
762 
763  // Compute the scalar S_i for each component j of
764  // the observation: Sij = dhij_dxv Pxx dhij_dxv^t +
765  // 2 * dhij_dyi Pyix dhij_dxv + dhij_dyi Pyiyi
766  // dhij_dyi^t + R
767  // ^^
768  // Hx:(O*LxSv)
769  // \----------------------/
770  // \--------------------------/
771  // \------------------------/ \-/
772  // TERM 1 TERM 2
773  // TERM 3 R
774  //
775  // O: Observation size (3)
776  // L: # landmarks
777  // Sv: Vehicle state size (6)
778  //
779 
780 #if defined(_DEBUG)
781  {
782  // This algorithm is applicable only if the
783  // scalar component of the sensor noise are
784  // INDEPENDENT:
785  for (size_t a = 0; a < OBS_SIZE; a++)
786  for (size_t b = 0; b < OBS_SIZE; b++)
787  if (a != b)
788  if (R(a, b) != 0)
790  "This KF algorithm assumes "
791  "independent noise "
792  "components in the "
793  "observation (matrix R). "
794  "Select another KF "
795  "algorithm.");
796  }
797 #endif
798  // R:
799  KFTYPE Sij = R(j, j);
800 
801  // TERM 1:
802  for (size_t k = 0; k < VEH_SIZE; k++)
803  {
804  KFTYPE accum = 0;
805  for (size_t q = 0; q < VEH_SIZE; q++)
806  accum += Hx(j, q) * m_pkk(q, k);
807  Sij += Hx(j, k) * accum;
808  }
809 
810  // TERM 2:
811  KFTYPE term2 = 0;
812  for (size_t k = 0; k < VEH_SIZE; k++)
813  {
814  KFTYPE accum = 0;
815  for (size_t q = 0; q < FEAT_SIZE; q++)
816  accum += Hy(j, q) * m_pkk(idx_off + q, k);
817  term2 += Hx(j, k) * accum;
818  }
819  Sij += 2 * term2;
820 
821  // TERM 3:
822  for (size_t k = 0; k < FEAT_SIZE; k++)
823  {
824  KFTYPE accum = 0;
825  for (size_t q = 0; q < FEAT_SIZE; q++)
826  accum += Hy(j, q) *
827  m_pkk(idx_off + q, idx_off + k);
828  Sij += Hy(j, k) * accum;
829  }
830 
831  // Compute the Kalman gain "Kij" for this
832  // observation element:
833  // --> K = m_pkk * (~dh_dx) * m_S.inverse_LLt() );
834  size_t N = m_pkk.cols();
835  vector<KFTYPE> Kij(N);
836 
837  for (size_t k = 0; k < N; k++)
838  {
839  KFTYPE K_tmp = 0;
840 
841  // dhi_dxv term
842  size_t q;
843  for (q = 0; q < VEH_SIZE; q++)
844  K_tmp += m_pkk(k, q) * Hx(j, q);
845 
846  // dhi_dyi term
847  for (q = 0; q < FEAT_SIZE; q++)
848  K_tmp += m_pkk(k, idx_off + q) * Hy(j, q);
849 
850  Kij[k] = K_tmp / Sij;
851  } // end for k
852 
853  // Update the state vector m_xkk:
854  // x' = x + Kij * ytilde(ij)
855  for (size_t k = 0; k < N; k++)
856  m_xkk[k] += Kij[k] * ytilde[j];
857 
858  // Update the covariance Pkk:
859  // P' = P - Kij * Sij * Kij^t
860  {
861  for (size_t k = 0; k < N; k++)
862  {
863  for (size_t q = k; q < N;
864  q++) // Half matrix
865  {
866  m_pkk(k, q) -= Sij * Kij[k] * Kij[q];
867  // It is symmetric
868  m_pkk(q, k) = m_pkk(k, q);
869  }
870 
871 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
872  if (m_pkk(k, k) < 0)
873  {
874  m_pkk.saveToTextFile("Pkk_err.txt");
876  Kij, "Kij.txt");
877  ASSERT_(m_pkk(k, k) > 0);
878  }
879 #endif
880  }
881  }
882 
883  m_timLogger.leave(
884  "KF:8.update stage:2.ScalarAtOnce.update");
885  } // end for j
886  } // end if mapped
887  } // end for each observed LM.
888  }
889  break;
890 
891  // --------------------------------------------------------------------
892  // - IKF method, processing each observation scalar secuentially:
893  // --------------------------------------------------------------------
894  case kfIKF: // TODO !!
895  {
896  THROW_EXCEPTION("IKF scalar by scalar not implemented yet.");
897  }
898  break;
899 
900  default:
901  THROW_EXCEPTION("Invalid value of options.KF_method");
902  } // end switch method
903  }
904 
905  const double tim_update = m_timLogger.leave("KF:8.update stage");
906 
907  m_timLogger.enter("KF:9.OnNormalizeStateVector");
908  OnNormalizeStateVector();
909  m_timLogger.leave("KF:9.OnNormalizeStateVector");
910 
911  // =============================================================
912  // 8. INTRODUCE NEW LANDMARKS
913  // =============================================================
914  if (!data_association.empty())
915  {
916  m_timLogger.enter("KF:A.add new landmarks");
917  detail::addNewLandmarks(*this, m_Z, data_association, R);
918  m_timLogger.leave("KF:A.add new landmarks");
919  } // end if data_association!=empty
920 
921  // Post iteration user code:
922  m_timLogger.enter("KF:B.OnPostIteration");
923  OnPostIteration();
924  m_timLogger.leave("KF:B.OnPostIteration");
925 
926  m_timLogger.leave("KF:complete_step");
927 
929  "[KF] %u LMs | Pr: %.2fms | Pr.Obs: %.2fms | Obs.DA: %.2fms | Upd: "
930  "%.2fms\n",
931  static_cast<unsigned int>(getNumberOfLandmarksInTheMap()),
932  1e3 * tim_pred, 1e3 * tim_pred_obs, 1e3 * tim_obs_DA,
933  1e3 * tim_update));
934  MRPT_END
935 } // End of "runOneKalmanIteration"
936 
937 template <
938  size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
939  typename KFTYPE>
942  const KFArray_VEH& x, const std::pair<KFCLASS*, KFArray_ACT>& dat,
943  KFArray_VEH& out_x)
944 {
945  bool dumm;
946  out_x = x;
947  dat.first->OnTransitionModel(dat.second, out_x, dumm);
948 }
949 
950 template <
951  size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
952  typename KFTYPE>
955  const KFArray_VEH& x, const std::pair<KFCLASS*, size_t>& dat,
956  KFArray_OBS& out_x)
957 {
958  std::vector<size_t> idxs_to_predict(1, dat.second);
959  vector_KFArray_OBS prediction;
960  // Overwrite (temporarily!) the affected part of the state vector:
961  std::memcpy(&dat.first->m_xkk[0], &x[0], sizeof(x[0]) * VEH_SIZE);
962  dat.first->OnObservationModel(idxs_to_predict, prediction);
963  ASSERTDEB_(prediction.size() == 1);
964  out_x = prediction[0];
965 }
966 
967 template <
968  size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
969  typename KFTYPE>
972  const KFArray_FEAT& x, const std::pair<KFCLASS*, size_t>& dat,
973  KFArray_OBS& out_x)
974 {
975  std::vector<size_t> idxs_to_predict(1, dat.second);
976  vector_KFArray_OBS prediction;
977  const size_t lm_idx_in_statevector = VEH_SIZE + FEAT_SIZE * dat.second;
978  // Overwrite (temporarily!) the affected part of the state vector:
979  std::memcpy(
980  &dat.first->m_xkk[lm_idx_in_statevector], &x[0],
981  sizeof(x[0]) * FEAT_SIZE);
982  dat.first->OnObservationModel(idxs_to_predict, prediction);
983  ASSERTDEB_(prediction.size() == 1);
984  out_x = prediction[0];
985 }
986 
987 namespace detail
988 {
989 // generic version for SLAM. There is a speciation below for NON-SLAM problems.
990 template <
991  size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
992  typename KFTYPE>
995  const typename CKalmanFilterCapable<
996  VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE>::vector_KFArray_OBS& Z,
997  const std::vector<int>& data_association,
998  const typename CKalmanFilterCapable<
999  VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE>::KFMatrix_OxO& R)
1000 {
1001  using KF =
1003 
1004  for (size_t idxObs = 0; idxObs < Z.size(); idxObs++)
1005  {
1006  // Is already in the map?
1007  if (data_association[idxObs] < 0) // Not in the map yet!
1008  {
1009  obj.getProfiler().enter("KF:9.create new LMs");
1010  // Add it:
1011 
1012  // Append to map of IDs <-> position in the state vector:
1013  ASSERTDEB_(FEAT_SIZE > 0);
1014  ASSERTDEB_(
1015  0 == ((obj.internal_getXkk().size() - VEH_SIZE) %
1016  FEAT_SIZE)); // Sanity test
1017 
1018  const size_t newIndexInMap =
1019  (obj.internal_getXkk().size() - VEH_SIZE) / FEAT_SIZE;
1020 
1021  // Inverse sensor model:
1022  typename KF::KFArray_FEAT yn;
1023  typename KF::KFMatrix_FxV dyn_dxv;
1024  typename KF::KFMatrix_FxO dyn_dhn;
1025  typename KF::KFMatrix_FxF dyn_dhn_R_dyn_dhnT;
1026  bool use_dyn_dhn_jacobian = true;
1027 
1028  // Compute the inv. sensor model and its Jacobians:
1029  obj.OnInverseObservationModel(
1030  Z[idxObs], yn, dyn_dxv, dyn_dhn, dyn_dhn_R_dyn_dhnT,
1031  use_dyn_dhn_jacobian);
1032 
1033  // And let the application do any special handling of adding a new
1034  // feature to the map:
1035  obj.OnNewLandmarkAddedToMap(idxObs, newIndexInMap);
1036 
1037  ASSERTDEB_(yn.size() == FEAT_SIZE);
1038 
1039  // Append to m_xkk:
1040  size_t q;
1041  size_t idx = obj.internal_getXkk().size();
1042  obj.internal_getXkk().resize(
1043  obj.internal_getXkk().size() + FEAT_SIZE);
1044 
1045  for (q = 0; q < FEAT_SIZE; q++)
1046  obj.internal_getXkk()[idx + q] = yn[q];
1047 
1048  // --------------------
1049  // Append to Pkk:
1050  // --------------------
1051  ASSERTDEB_(
1052  obj.internal_getPkk().cols() == (int)idx &&
1053  obj.internal_getPkk().rows() == (int)idx);
1054 
1055  obj.internal_getPkk().setSize(idx + FEAT_SIZE, idx + FEAT_SIZE);
1056 
1057  // Fill the Pxyn term:
1058  // --------------------
1059  auto Pxx = typename KF::KFMatrix_VxV(
1060  obj.internal_getPkk().template block<VEH_SIZE, VEH_SIZE>(0, 0));
1061  auto Pxyn = typename KF::KFMatrix_FxV(dyn_dxv * Pxx);
1062 
1063  obj.internal_getPkk().insertMatrix(idx, 0, Pxyn);
1064  obj.internal_getPkk().insertMatrix(0, idx, Pxyn.transpose());
1065 
1066  // Fill the Pyiyn terms:
1067  // --------------------
1068  const size_t nLMs =
1069  (idx - VEH_SIZE) / FEAT_SIZE; // Number of previous landmarks:
1070  for (q = 0; q < nLMs; q++)
1071  {
1072  auto P_x_yq = typename KF::KFMatrix_VxF(
1073  obj.internal_getPkk().template block<VEH_SIZE, FEAT_SIZE>(
1074  0, VEH_SIZE + q * FEAT_SIZE));
1075 
1076  auto P_cross = typename KF::KFMatrix_FxF(dyn_dxv * P_x_yq);
1077 
1078  obj.internal_getPkk().insertMatrix(
1079  idx, VEH_SIZE + q * FEAT_SIZE, P_cross);
1080  obj.internal_getPkk().insertMatrix(
1081  VEH_SIZE + q * FEAT_SIZE, idx, P_cross.transpose());
1082  } // end each previous LM(q)
1083 
1084  // Fill the Pynyn term:
1085  // P_yn_yn = (dyn_dxv * Pxx * ~dyn_dxv) + (dyn_dhn * R *
1086  // ~dyn_dhn);
1087  // --------------------
1088  typename KF::KFMatrix_FxF P_yn_yn =
1089  mrpt::math::multiply_HCHt(dyn_dxv, Pxx);
1090  if (use_dyn_dhn_jacobian)
1091  // Accumulate in P_yn_yn
1092  P_yn_yn += mrpt::math::multiply_HCHt(dyn_dhn, R);
1093  else
1094  P_yn_yn += dyn_dhn_R_dyn_dhnT;
1095 
1096  obj.internal_getPkk().insertMatrix(idx, idx, P_yn_yn);
1097 
1098  obj.getProfiler().leave("KF:9.create new LMs");
1099  }
1100  }
1101 }
1102 
1103 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
1106  VEH_SIZE, OBS_SIZE, 0 /* FEAT_SIZE=0 */, ACT_SIZE, KFTYPE>& obj,
1107  const typename CKalmanFilterCapable<
1108  VEH_SIZE, OBS_SIZE, 0 /* FEAT_SIZE=0 */, ACT_SIZE,
1109  KFTYPE>::vector_KFArray_OBS& Z,
1110  const std::vector<int>& data_association,
1111  const typename CKalmanFilterCapable<
1112  VEH_SIZE, OBS_SIZE, 0 /* FEAT_SIZE=0 */, ACT_SIZE,
1113  KFTYPE>::KFMatrix_OxO& R)
1114 {
1115  // Do nothing: this is NOT a SLAM problem.
1116 }
1117 
1118 template <
1119  size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
1120  typename KFTYPE>
1123  obj)
1124 {
1125  return (obj.getStateVectorLength() - VEH_SIZE) / FEAT_SIZE;
1126 }
1127 // Specialization for FEAT_SIZE=0
1128 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
1130  const CKalmanFilterCapable<
1131  VEH_SIZE, OBS_SIZE, 0 /*FEAT_SIZE*/, ACT_SIZE, KFTYPE>& obj)
1132 {
1133  return 0;
1134 }
1135 
1136 template <
1137  size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
1138  typename KFTYPE>
1139 inline bool isMapEmpty(
1141  obj)
1142 {
1143  return (obj.getStateVectorLength() == VEH_SIZE);
1144 }
1145 // Specialization for FEAT_SIZE=0
1146 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
1147 inline bool isMapEmpty(
1148  const CKalmanFilterCapable<
1149  VEH_SIZE, OBS_SIZE, 0 /*FEAT_SIZE*/, ACT_SIZE, KFTYPE>& obj)
1150 {
1151  return true;
1152 }
1153 } // namespace detail
1154 } // namespace bayes
1155 } // namespace mrpt
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
#define MRPT_START
Definition: exceptions.h:241
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3727
static void KF_aux_estimate_obs_Hx_jacobian(const KFArray_VEH &x, const std::pair< KFCLASS *, size_t > &dat, KFArray_OBS &out_x)
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
STL namespace.
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
#define MRPT_LOG_WARN_STREAM(__CONTENTS)
size_t find_in_vector(const T &value, const CONTAINER &vect)
Returns the index of the value "T" in the container "vect" (std::vector,std::deque,etc), or string::npos if not found.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
const GLubyte * c
Definition: glext.h:6406
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)
Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.
size_t getNumberOfLandmarksInMap(const CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > &obj)
GLubyte GLubyte b
Definition: glext.h:6372
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 ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
Derived inverse_LLt() const
Returns the inverse of a symmetric matrix using LLt.
double kftype
The numeric type used in the Kalman Filter (default=double)
size_type cols() const
Number of columns in the matrix.
void estimateJacobian(const VECTORLIKE &x, const std::function< void(const VECTORLIKE &x, const USERPARAM &y, VECTORLIKE3 &out)> &functor, const VECTORLIKE2 &increments, const USERPARAM &userParam, MATRIXLIKE &out_Jacobian)
Estimate the Jacobian of a multi-dimensional function around a point "x", using finite differences of...
Definition: num_jacobian.h:31
void extractSubmatrixSymmetrical(const MAT &m, const std::vector< size_t > &indices, MATRIX &out)
Get a submatrix from a square matrix, by collecting the elements M(idxs,idxs), where idxs is the sequ...
Definition: ops_matrices.h:310
void enter(const std::string_view &func_name) noexcept
Start of a named section.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
GLdouble GLdouble GLdouble r
Definition: glext.h:3711
const float R
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
Definition: exceptions.h:190
bool isMapEmpty(const CKalmanFilterCapable< VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE > &obj)
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
#define MRPT_END
Definition: exceptions.h:245
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
Definition: CMatrixFixed.h:251
constexpr size_type cols() const
Number of columns in the matrix.
Definition: CMatrixFixed.h:230
static void KF_aux_estimate_obs_Hy_jacobian(const KFArray_FEAT &x, const std::pair< KFCLASS *, size_t > &dat, KFArray_OBS &out_x)
GLsizeiptr size
Definition: glext.h:3934
GLenum GLint x
Definition: glext.h:3542
bool vectorToTextFile(const std::vector< float > &vec, const std::string &fileName, bool append=false, bool byRows=false)
A useful function for debugging, which saves a std::vector into a text file (compat.
GLubyte GLubyte GLubyte a
Definition: glext.h:6372
mrpt::system::CTimeLogger & getProfiler()
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1891
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".
Definition: os.cpp:358
void multiply_HCHt(const MAT_H &H, const MAT_C &C, MAT_R &R, bool accumResultInOutput=false)
R = H * C * H^t.
Definition: ops_matrices.h:28



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 9b18308f3 Mon Nov 18 23:39:25 2019 +0100 at lun nov 18 23:45:12 CET 2019