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  ::memcpy(&m_xkk[0], &x_vehicle[0], sizeof(m_xkk[0]) * VEH_SIZE);
307 
309  x_feat,
310  std::function<void(
311  const KFArray_FEAT& x,
312  const std::pair<KFCLASS*, size_t>& dat,
313  KFArray_OBS& out_x)>(&KF_aux_estimate_obs_Hy_jacobian),
314  feat_increments, std::pair<KFCLASS*, size_t>(this, lm_idx),
315  Hy);
316  // The state vector was temporarily modified by
317  // KF_aux_estimate_*, restore it:
318  ::memcpy(
319  &m_xkk[lm_idx_in_statevector], &x_feat[0],
320  sizeof(m_xkk[0]) * FEAT_SIZE);
321 
322  if (KF_options.debug_verify_analytic_jacobians)
323  {
326  OnObservationJacobians(lm_idx, Hx_gt, Hy_gt);
327  if (KFMatrix(Hx - Hx_gt).sum_abs() >
328  KF_options.debug_verify_analytic_jacobians_threshold)
329  {
330  std::cerr << "[KalmanFilter] ERROR: User analytical "
331  "observation Hx Jacobians are wrong: \n"
332  << " Real Hx: \n"
333  << Hx.asEigen() << "\n Analytical Hx:\n"
334  << Hx_gt.asEigen() << "Diff:\n"
335  << (Hx.asEigen() - Hx_gt.asEigen()) << "\n";
337  "ERROR: User analytical observation Hx Jacobians "
338  "are wrong (More details dumped to cerr)");
339  }
340  if (KFMatrix(Hy - Hy_gt).sum_abs() >
341  KF_options.debug_verify_analytic_jacobians_threshold)
342  {
343  std::cerr << "[KalmanFilter] ERROR: User analytical "
344  "observation Hy Jacobians are wrong: \n"
345  << " Real Hy: \n"
346  << Hy.asEigen() << "\n Analytical Hx:\n"
347  << Hy_gt.asEigen() << "Diff:\n"
348  << Hy.asEigen() - Hy_gt.asEigen() << "\n";
350  "ERROR: User analytical observation Hy Jacobians "
351  "are wrong (More details dumped to cerr)");
352  }
353  }
354  }
355  }
356  m_timLogger.leave("KF:5.build Jacobians");
357 
358  m_timLogger.enter("KF:6.build m_S");
359 
360  // Compute m_S: m_S = H P H' + R (R will be added below)
361  // exploiting the sparsity of H:
362  // Each block in m_S is:
363  // Sij =
364  // ------------------------------------------
365  m_S.setSize(N_pred * OBS_SIZE, N_pred * OBS_SIZE);
366 
367  if (FEAT_SIZE > 0)
368  { // SLAM-like problem:
369  // Covariance of the vehicle pose
370  const auto Px = m_pkk.template block<VEH_SIZE, VEH_SIZE>(0, 0);
371 
372  for (size_t i = 0; i < N_pred; ++i)
373  {
374  const size_t lm_idx_i = m_predictLMidxs[i];
375  // Pxyi^t
376  const auto Pxyi_t = m_pkk.template block<FEAT_SIZE, VEH_SIZE>(
377  VEH_SIZE + lm_idx_i * FEAT_SIZE, 0);
378 
379  // Only do j>=i (upper triangle), since m_S is symmetric:
380  for (size_t j = i; j < N_pred; ++j)
381  {
382  const size_t lm_idx_j = m_predictLMidxs[j];
383  // Sij block:
385 
386  const auto Pxyj = m_pkk.template block<VEH_SIZE, FEAT_SIZE>(
387  0, VEH_SIZE + lm_idx_j * FEAT_SIZE);
388  const auto Pyiyj =
389  m_pkk.template block<FEAT_SIZE, FEAT_SIZE>(
390  VEH_SIZE + lm_idx_i * FEAT_SIZE,
391  VEH_SIZE + lm_idx_j * FEAT_SIZE);
392 
393  // clang-format off
394  Sij = m_Hxs[i].asEigen() * Px * m_Hxs[j].asEigen().transpose() +
395  m_Hys[i].asEigen() * Pxyi_t * m_Hxs[j].asEigen().transpose() +
396  m_Hxs[i].asEigen() * Pxyj * m_Hys[j].asEigen().transpose() +
397  m_Hys[i].asEigen() * Pyiyj * m_Hys[j].asEigen().transpose();
398  // clang-format on
399 
400  m_S.insertMatrix(OBS_SIZE * i, OBS_SIZE * j, Sij);
401 
402  // Copy transposed to the symmetric lower-triangular part:
403  if (i != j)
404  m_S.insertMatrixTransposed(
405  OBS_SIZE * j, OBS_SIZE * i, Sij);
406  }
407 
408  // Sum the "R" term to the diagonal blocks:
409  const size_t obs_idx_off = i * OBS_SIZE;
410  m_S.asEigen().template block<OBS_SIZE, OBS_SIZE>(
411  obs_idx_off, obs_idx_off) += R.asEigen();
412  }
413  }
414  else
415  { // Not SLAM-like problem: simply m_S=H*Pkk*H^t + R
416  ASSERTDEB_(N_pred == 1);
417  ASSERTDEB_(m_S.cols() == OBS_SIZE);
418 
419  m_S = m_Hxs[0].asEigen() * m_pkk.asEigen() *
420  m_Hxs[0].asEigen().transpose() +
421  R.asEigen();
422  }
423 
424  m_timLogger.leave("KF:6.build m_S");
425 
426  m_Z.clear(); // Each entry is one observation:
427 
428  m_timLogger.enter("KF:7.get obs & DA");
429 
430  // Get observations and do data-association:
431  OnGetObservationsAndDataAssociation(
432  m_Z, data_association, // Out
433  m_all_predictions, m_S, m_predictLMidxs, R // In
434  );
435  ASSERTDEB_(
436  data_association.size() == m_Z.size() ||
437  (data_association.empty() && FEAT_SIZE == 0));
438 
439  // Check if an observation hasn't been predicted in
440  // OnPreComputingPredictions() but has been actually
441  // observed. This may imply an error in the heuristic of
442  // OnPreComputingPredictions(), and forces us
443  // to rebuild the matrices
444  missing_predictions_to_add.clear();
445  if (FEAT_SIZE != 0)
446  {
447  for (int i : data_association)
448  {
449  if (i < 0) continue;
450  const auto assoc_idx_in_map = static_cast<size_t>(i);
451  const size_t assoc_idx_in_pred =
453  assoc_idx_in_map, m_predictLMidxs);
454  if (assoc_idx_in_pred == std::string::npos)
455  missing_predictions_to_add.push_back(assoc_idx_in_map);
456  }
457  }
458 
459  first_new_pred = N_pred; // If we do another loop, start at the begin
460  // of new predictions
461 
462  } while (!missing_predictions_to_add.empty());
463 
464  const double tim_obs_DA = m_timLogger.leave("KF:7.get obs & DA");
465 
466  // =============================================================
467  // 7. UPDATE USING THE KALMAN GAIN
468  // =============================================================
469  // Update, only if there are observations!
470  if (!m_Z.empty())
471  {
472  m_timLogger.enter("KF:8.update stage");
473 
474  switch (KF_options.method)
475  {
476  // -----------------------
477  // FULL KF- METHOD
478  // -----------------------
479  case kfEKFNaive:
480  case kfIKFFull:
481  {
482  // Build the whole Jacobian dh_dx matrix
483  // ---------------------------------------------
484  // Keep only those whose DA is not -1
485  std::vector<int> mapIndicesForKFUpdate(data_association.size());
486  mapIndicesForKFUpdate.resize(std::distance(
487  mapIndicesForKFUpdate.begin(),
488  std::remove_copy_if(
489  data_association.begin(), data_association.end(),
490  mapIndicesForKFUpdate.begin(),
491  [](int i) { return i == -1; })));
492 
493  const size_t N_upd =
494  (FEAT_SIZE == 0) ? 1 : // Non-SLAM problems: Just one
495  // observation for the entire
496  // system.
497  mapIndicesForKFUpdate
498  .size(); // SLAM: # of observed known landmarks
499 
500  // Just one, or several update iterations??
501  const size_t nKF_iterations = (KF_options.method == kfEKFNaive)
502  ? 1
503  : KF_options.IKF_iterations;
504 
505  const KFVector xkk_0 = m_xkk;
506 
507  // For each IKF iteration (or 1 for EKF)
508  if (N_upd > 0) // Do not update if we have no observations!
509  {
510  for (size_t IKF_iteration = 0;
511  IKF_iteration < nKF_iterations; IKF_iteration++)
512  {
513  // Compute ytilde = OBS - PREDICTION
514  KFVector ytilde(OBS_SIZE * N_upd);
515  size_t ytilde_idx = 0;
516 
517  // TODO: Use a Matrix view of "dh_dx_full" instead of
518  // creating a copy into "m_dh_dx_full_obs"
519  m_dh_dx_full_obs.setZero(
520  N_upd * OBS_SIZE,
521  VEH_SIZE + FEAT_SIZE * N_map); // Init to zeros.
522  KFMatrix S_observed; // The KF "m_S" matrix: A
523  // re-ordered, subset, version of
524  // the prediction m_S:
525 
526  if (FEAT_SIZE != 0)
527  { // SLAM problems:
528  std::vector<size_t> S_idxs;
529  S_idxs.reserve(OBS_SIZE * N_upd);
530 
531  // const size_t row_len = VEH_SIZE + FEAT_SIZE *
532  // N_map;
533 
534  for (size_t i = 0; i < data_association.size(); ++i)
535  {
536  if (data_association[i] < 0) continue;
537 
538  const auto assoc_idx_in_map =
539  static_cast<size_t>(data_association[i]);
540  const size_t assoc_idx_in_pred =
542  assoc_idx_in_map, m_predictLMidxs);
543  ASSERTMSG_(
544  assoc_idx_in_pred != string::npos,
545  "OnPreComputingPredictions() didn't "
546  "recommend the prediction of a landmark "
547  "which has been actually observed!");
548  // TODO: In these cases, extend the prediction
549  // right now instead of launching an
550  // exception... or is this a bad idea??
551 
552  // Build the subset m_dh_dx_full_obs:
553  // m_dh_dx_full_obs.block(S_idxs.size()
554  //,0, OBS_SIZE, row_len)
555  // =
556  // dh_dx_full.block
557  //(assoc_idx_in_pred*OBS_SIZE,0, OBS_SIZE,
558  // row_len);
559 
560  m_dh_dx_full_obs
561  .template block<OBS_SIZE, VEH_SIZE>(
562  S_idxs.size(), 0) =
563  m_Hxs[assoc_idx_in_pred].asEigen();
564  m_dh_dx_full_obs
565  .template block<OBS_SIZE, FEAT_SIZE>(
566  S_idxs.size(),
567  VEH_SIZE +
568  assoc_idx_in_map * FEAT_SIZE) =
569  m_Hys[assoc_idx_in_pred].asEigen();
570 
571  // S_idxs.size() is used as counter for
572  // "m_dh_dx_full_obs".
573  for (size_t k = 0; k < OBS_SIZE; k++)
574  S_idxs.push_back(
575  assoc_idx_in_pred * OBS_SIZE + k);
576 
577  // ytilde_i = Z[i] - m_all_predictions[i]
578  KFArray_OBS ytilde_i = m_Z[i];
579  OnSubstractObservationVectors(
580  ytilde_i,
581  m_all_predictions
582  [m_predictLMidxs[assoc_idx_in_pred]]);
583  for (size_t k = 0; k < OBS_SIZE; k++)
584  ytilde[ytilde_idx++] = ytilde_i[k];
585  }
586  // Extract the subset that is involved in this
587  // observation:
589  m_S, S_idxs, S_observed);
590  }
591  else
592  { // Non-SLAM problems:
593  ASSERT_(
594  m_Z.size() == 1 &&
595  m_all_predictions.size() == 1);
596  ASSERT_(
597  m_dh_dx_full_obs.rows() == OBS_SIZE &&
598  m_dh_dx_full_obs.cols() == VEH_SIZE);
599  ASSERT_(m_Hxs.size() == 1);
600  m_dh_dx_full_obs = m_Hxs[0]; // Was: dh_dx_full
601  KFArray_OBS ytilde_i = m_Z[0];
602  OnSubstractObservationVectors(
603  ytilde_i, m_all_predictions[0]);
604  for (size_t k = 0; k < OBS_SIZE; k++)
605  ytilde[ytilde_idx++] = ytilde_i[k];
606  // Extract the subset that is involved in this
607  // observation:
608  S_observed = m_S;
609  }
610 
611  // Compute the full K matrix:
612  // ------------------------------
613  m_timLogger.enter("KF:8.update stage:1.FULLKF:build K");
614 
615  m_K.setSize(m_pkk.rows(), S_observed.cols());
616 
617  // K = m_pkk * (~dh_dx) * m_S.inverse_LLt() );
618  m_K.asEigen() = m_pkk.asEigen() *
619  m_dh_dx_full_obs.asEigen().transpose();
620 
621  // Inverse of S_observed -> m_S_1
622  m_S_1 = S_observed.inverse_LLt();
623  m_K.asEigen() *= m_S_1.asEigen();
624 
625  m_timLogger.leave("KF:8.update stage:1.FULLKF:build K");
626 
627  // Use the full K matrix to update the mean:
628  if (nKF_iterations == 1)
629  {
630  m_timLogger.enter(
631  "KF:8.update stage:2.FULLKF:update xkk");
632  m_xkk.asEigen() += (m_K * ytilde).eval();
633  m_timLogger.leave(
634  "KF:8.update stage:2.FULLKF:update xkk");
635  }
636  else
637  {
638  m_timLogger.enter(
639  "KF:8.update stage:2.FULLKF:iter.update xkk");
640 
641  auto HAx_column =
642  KFVector(m_dh_dx_full_obs * (m_xkk - xkk_0));
643 
644  m_xkk = xkk_0;
645  m_xkk.asEigen() += m_K * (ytilde - HAx_column);
646 
647  m_timLogger.leave(
648  "KF:8.update stage:2.FULLKF:iter.update xkk");
649  }
650 
651  // Update the covariance just at the end
652  // of iterations if we are in IKF, always in normal
653  // EKF.
654  if (IKF_iteration == (nKF_iterations - 1))
655  {
656  m_timLogger.enter(
657  "KF:8.update stage:3.FULLKF:update Pkk");
658 
659  // Use the full K matrix to update the covariance:
660  // m_pkk = (I - K*dh_dx ) * m_pkk;
661  // TODO: "Optimize this: sparsity!"
662 
663  // K * m_dh_dx_full_obs
664  m_aux_K_dh_dx.matProductOf_AB(
665  m_K, m_dh_dx_full_obs);
666 
667  // m_aux_K_dh_dx <-- I-m_aux_K_dh_dx
668  const size_t stat_len = m_aux_K_dh_dx.cols();
669  for (size_t r = 0; r < stat_len; r++)
670  {
671  for (size_t c = 0; c < stat_len; c++)
672  {
673  if (r == c)
674  m_aux_K_dh_dx(r, c) =
675  -m_aux_K_dh_dx(r, c) + kftype(1);
676  else
677  m_aux_K_dh_dx(r, c) =
678  -m_aux_K_dh_dx(r, c);
679  }
680  }
681 
682  m_pkk = m_aux_K_dh_dx * m_pkk;
683 
684  m_timLogger.leave(
685  "KF:8.update stage:3.FULLKF:update Pkk");
686  }
687  } // end for each IKF iteration
688  }
689  }
690  break;
691 
692  // --------------------------------------------------------------------
693  // - EKF 'a la' Davison: One observation element at once
694  // --------------------------------------------------------------------
695  case kfEKFAlaDavison:
696  {
697  // For each observed landmark/whole system state:
698  for (size_t obsIdx = 0; obsIdx < m_Z.size(); obsIdx++)
699  {
700  // Known & mapped landmark?
701  bool doit;
702  size_t idxInTheFilter = 0;
703 
704  if (data_association.empty())
705  {
706  doit = true;
707  }
708  else
709  {
710  doit = data_association[obsIdx] >= 0;
711  if (doit) idxInTheFilter = data_association[obsIdx];
712  }
713 
714  if (doit)
715  {
716  m_timLogger.enter(
717  "KF:8.update stage:1.ScalarAtOnce.prepare");
718 
719  // Already mapped: OK
720  const size_t idx_off =
721  VEH_SIZE +
722  idxInTheFilter *
723  FEAT_SIZE; // The offset in m_xkk & Pkk.
724 
725  // Compute just the part of the Jacobian that we need
726  // using the current updated m_xkk:
727  vector_KFArray_OBS pred_obs;
728  OnObservationModel(
729  std::vector<size_t>(1, idxInTheFilter), pred_obs);
730  ASSERTDEB_(pred_obs.size() == 1);
731 
732  // ytilde = observation - prediction
733  KFArray_OBS ytilde = m_Z[obsIdx];
734  OnSubstractObservationVectors(ytilde, pred_obs[0]);
735 
736  // Jacobians:
737  // dh_dx: already is (N_pred*OBS_SIZE) x (VEH_SIZE +
738  // FEAT_SIZE * N_pred )
739  // with N_pred = |m_predictLMidxs|
740 
741  const size_t i_idx_in_preds =
743  idxInTheFilter, m_predictLMidxs);
744  ASSERTMSG_(
745  i_idx_in_preds != string::npos,
746  "OnPreComputingPredictions() didn't recommend the "
747  "prediction of a landmark which has been actually "
748  "observed!");
749 
750  const KFMatrix_OxV& Hx = m_Hxs[i_idx_in_preds];
751  const KFMatrix_OxF& Hy = m_Hys[i_idx_in_preds];
752 
753  m_timLogger.leave(
754  "KF:8.update stage:1.ScalarAtOnce.prepare");
755 
756  // For each component of the observation:
757  for (size_t j = 0; j < OBS_SIZE; j++)
758  {
759  m_timLogger.enter(
760  "KF:8.update stage:2.ScalarAtOnce.update");
761 
762  // Compute the scalar S_i for each component j of
763  // the observation: Sij = dhij_dxv Pxx dhij_dxv^t +
764  // 2 * dhij_dyi Pyix dhij_dxv + dhij_dyi Pyiyi
765  // dhij_dyi^t + R
766  // ^^
767  // Hx:(O*LxSv)
768  // \----------------------/
769  // \--------------------------/
770  // \------------------------/ \-/
771  // TERM 1 TERM 2
772  // TERM 3 R
773  //
774  // O: Observation size (3)
775  // L: # landmarks
776  // Sv: Vehicle state size (6)
777  //
778 
779 #if defined(_DEBUG)
780  {
781  // This algorithm is applicable only if the
782  // scalar component of the sensor noise are
783  // INDEPENDENT:
784  for (size_t a = 0; a < OBS_SIZE; a++)
785  for (size_t b = 0; b < OBS_SIZE; b++)
786  if (a != b)
787  if (R(a, b) != 0)
789  "This KF algorithm assumes "
790  "independent noise "
791  "components in the "
792  "observation (matrix R). "
793  "Select another KF "
794  "algorithm.");
795  }
796 #endif
797  // R:
798  KFTYPE Sij = R(j, j);
799 
800  // TERM 1:
801  for (size_t k = 0; k < VEH_SIZE; k++)
802  {
803  KFTYPE accum = 0;
804  for (size_t q = 0; q < VEH_SIZE; q++)
805  accum += Hx(j, q) * m_pkk(q, k);
806  Sij += Hx(j, k) * accum;
807  }
808 
809  // TERM 2:
810  KFTYPE term2 = 0;
811  for (size_t k = 0; k < VEH_SIZE; k++)
812  {
813  KFTYPE accum = 0;
814  for (size_t q = 0; q < FEAT_SIZE; q++)
815  accum += Hy(j, q) * m_pkk(idx_off + q, k);
816  term2 += Hx(j, k) * accum;
817  }
818  Sij += 2 * term2;
819 
820  // TERM 3:
821  for (size_t k = 0; k < FEAT_SIZE; k++)
822  {
823  KFTYPE accum = 0;
824  for (size_t q = 0; q < FEAT_SIZE; q++)
825  accum += Hy(j, q) *
826  m_pkk(idx_off + q, idx_off + k);
827  Sij += Hy(j, k) * accum;
828  }
829 
830  // Compute the Kalman gain "Kij" for this
831  // observation element:
832  // --> K = m_pkk * (~dh_dx) * m_S.inverse_LLt() );
833  size_t N = m_pkk.cols();
834  vector<KFTYPE> Kij(N);
835 
836  for (size_t k = 0; k < N; k++)
837  {
838  KFTYPE K_tmp = 0;
839 
840  // dhi_dxv term
841  size_t q;
842  for (q = 0; q < VEH_SIZE; q++)
843  K_tmp += m_pkk(k, q) * Hx(j, q);
844 
845  // dhi_dyi term
846  for (q = 0; q < FEAT_SIZE; q++)
847  K_tmp += m_pkk(k, idx_off + q) * Hy(j, q);
848 
849  Kij[k] = K_tmp / Sij;
850  } // end for k
851 
852  // Update the state vector m_xkk:
853  // x' = x + Kij * ytilde(ij)
854  for (size_t k = 0; k < N; k++)
855  m_xkk[k] += Kij[k] * ytilde[j];
856 
857  // Update the covariance Pkk:
858  // P' = P - Kij * Sij * Kij^t
859  {
860  for (size_t k = 0; k < N; k++)
861  {
862  for (size_t q = k; q < N;
863  q++) // Half matrix
864  {
865  m_pkk(k, q) -= Sij * Kij[k] * Kij[q];
866  // It is symmetric
867  m_pkk(q, k) = m_pkk(k, q);
868  }
869 
870 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
871  if (m_pkk(k, k) < 0)
872  {
873  m_pkk.saveToTextFile("Pkk_err.txt");
875  Kij, "Kij.txt");
876  ASSERT_(m_pkk(k, k) > 0);
877  }
878 #endif
879  }
880  }
881 
882  m_timLogger.leave(
883  "KF:8.update stage:2.ScalarAtOnce.update");
884  } // end for j
885  } // end if mapped
886  } // end for each observed LM.
887  }
888  break;
889 
890  // --------------------------------------------------------------------
891  // - IKF method, processing each observation scalar secuentially:
892  // --------------------------------------------------------------------
893  case kfIKF: // TODO !!
894  {
895  THROW_EXCEPTION("IKF scalar by scalar not implemented yet.");
896  }
897  break;
898 
899  default:
900  THROW_EXCEPTION("Invalid value of options.KF_method");
901  } // end switch method
902  }
903 
904  const double tim_update = m_timLogger.leave("KF:8.update stage");
905 
906  m_timLogger.enter("KF:9.OnNormalizeStateVector");
907  OnNormalizeStateVector();
908  m_timLogger.leave("KF:9.OnNormalizeStateVector");
909 
910  // =============================================================
911  // 8. INTRODUCE NEW LANDMARKS
912  // =============================================================
913  if (!data_association.empty())
914  {
915  m_timLogger.enter("KF:A.add new landmarks");
916  detail::addNewLandmarks(*this, m_Z, data_association, R);
917  m_timLogger.leave("KF:A.add new landmarks");
918  } // end if data_association!=empty
919 
920  // Post iteration user code:
921  m_timLogger.enter("KF:B.OnPostIteration");
922  OnPostIteration();
923  m_timLogger.leave("KF:B.OnPostIteration");
924 
925  m_timLogger.leave("KF:complete_step");
926 
928  "[KF] %u LMs | Pr: %.2fms | Pr.Obs: %.2fms | Obs.DA: %.2fms | Upd: "
929  "%.2fms\n",
930  static_cast<unsigned int>(getNumberOfLandmarksInTheMap()),
931  1e3 * tim_pred, 1e3 * tim_pred_obs, 1e3 * tim_obs_DA,
932  1e3 * tim_update));
933  MRPT_END
934 } // End of "runOneKalmanIteration"
935 
936 template <
937  size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
938  typename KFTYPE>
941  const KFArray_VEH& x, const std::pair<KFCLASS*, KFArray_ACT>& dat,
942  KFArray_VEH& out_x)
943 {
944  bool dumm;
945  out_x = x;
946  dat.first->OnTransitionModel(dat.second, out_x, dumm);
947 }
948 
949 template <
950  size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
951  typename KFTYPE>
954  const KFArray_VEH& x, const std::pair<KFCLASS*, size_t>& dat,
955  KFArray_OBS& out_x)
956 {
957  std::vector<size_t> idxs_to_predict(1, dat.second);
958  vector_KFArray_OBS prediction;
959  // Overwrite (temporarily!) the affected part of the state vector:
960  ::memcpy(&dat.first->m_xkk[0], &x[0], sizeof(x[0]) * VEH_SIZE);
961  dat.first->OnObservationModel(idxs_to_predict, prediction);
962  ASSERTDEB_(prediction.size() == 1);
963  out_x = prediction[0];
964 }
965 
966 template <
967  size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
968  typename KFTYPE>
971  const KFArray_FEAT& x, const std::pair<KFCLASS*, size_t>& dat,
972  KFArray_OBS& out_x)
973 {
974  std::vector<size_t> idxs_to_predict(1, dat.second);
975  vector_KFArray_OBS prediction;
976  const size_t lm_idx_in_statevector = VEH_SIZE + FEAT_SIZE * dat.second;
977  // Overwrite (temporarily!) the affected part of the state vector:
978  ::memcpy(
979  &dat.first->m_xkk[lm_idx_in_statevector], &x[0],
980  sizeof(x[0]) * FEAT_SIZE);
981  dat.first->OnObservationModel(idxs_to_predict, prediction);
982  ASSERTDEB_(prediction.size() == 1);
983  out_x = prediction[0];
984 }
985 
986 namespace detail
987 {
988 // generic version for SLAM. There is a speciation below for NON-SLAM problems.
989 template <
990  size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
991  typename KFTYPE>
994  const typename CKalmanFilterCapable<
995  VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE>::vector_KFArray_OBS& Z,
996  const std::vector<int>& data_association,
997  const typename CKalmanFilterCapable<
998  VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE>::KFMatrix_OxO& R)
999 {
1000  using KF =
1002 
1003  for (size_t idxObs = 0; idxObs < Z.size(); idxObs++)
1004  {
1005  // Is already in the map?
1006  if (data_association[idxObs] < 0) // Not in the map yet!
1007  {
1008  obj.getProfiler().enter("KF:9.create new LMs");
1009  // Add it:
1010 
1011  // Append to map of IDs <-> position in the state vector:
1012  ASSERTDEB_(FEAT_SIZE > 0);
1013  ASSERTDEB_(
1014  0 == ((obj.internal_getXkk().size() - VEH_SIZE) %
1015  FEAT_SIZE)); // Sanity test
1016 
1017  const size_t newIndexInMap =
1018  (obj.internal_getXkk().size() - VEH_SIZE) / FEAT_SIZE;
1019 
1020  // Inverse sensor model:
1021  typename KF::KFArray_FEAT yn;
1022  typename KF::KFMatrix_FxV dyn_dxv;
1023  typename KF::KFMatrix_FxO dyn_dhn;
1024  typename KF::KFMatrix_FxF dyn_dhn_R_dyn_dhnT;
1025  bool use_dyn_dhn_jacobian = true;
1026 
1027  // Compute the inv. sensor model and its Jacobians:
1028  obj.OnInverseObservationModel(
1029  Z[idxObs], yn, dyn_dxv, dyn_dhn, dyn_dhn_R_dyn_dhnT,
1030  use_dyn_dhn_jacobian);
1031 
1032  // And let the application do any special handling of adding a new
1033  // feature to the map:
1034  obj.OnNewLandmarkAddedToMap(idxObs, newIndexInMap);
1035 
1036  ASSERTDEB_(yn.size() == FEAT_SIZE);
1037 
1038  // Append to m_xkk:
1039  size_t q;
1040  size_t idx = obj.internal_getXkk().size();
1041  obj.internal_getXkk().resize(
1042  obj.internal_getXkk().size() + FEAT_SIZE);
1043 
1044  for (q = 0; q < FEAT_SIZE; q++)
1045  obj.internal_getXkk()[idx + q] = yn[q];
1046 
1047  // --------------------
1048  // Append to Pkk:
1049  // --------------------
1050  ASSERTDEB_(
1051  obj.internal_getPkk().cols() == (int)idx &&
1052  obj.internal_getPkk().rows() == (int)idx);
1053 
1054  obj.internal_getPkk().setSize(idx + FEAT_SIZE, idx + FEAT_SIZE);
1055 
1056  // Fill the Pxyn term:
1057  // --------------------
1058  auto Pxx = typename KF::KFMatrix_VxV(
1059  obj.internal_getPkk().template block<VEH_SIZE, VEH_SIZE>(0, 0));
1060  auto Pxyn = typename KF::KFMatrix_FxV(dyn_dxv * Pxx);
1061 
1062  obj.internal_getPkk().insertMatrix(idx, 0, Pxyn);
1063  obj.internal_getPkk().insertMatrix(0, idx, Pxyn.transpose());
1064 
1065  // Fill the Pyiyn terms:
1066  // --------------------
1067  const size_t nLMs =
1068  (idx - VEH_SIZE) / FEAT_SIZE; // Number of previous landmarks:
1069  for (q = 0; q < nLMs; q++)
1070  {
1071  auto P_x_yq = typename KF::KFMatrix_VxF(
1072  obj.internal_getPkk().template block<VEH_SIZE, FEAT_SIZE>(
1073  0, VEH_SIZE + q * FEAT_SIZE));
1074 
1075  auto P_cross = typename KF::KFMatrix_FxF(dyn_dxv * P_x_yq);
1076 
1077  obj.internal_getPkk().insertMatrix(
1078  idx, VEH_SIZE + q * FEAT_SIZE, P_cross);
1079  obj.internal_getPkk().insertMatrix(
1080  VEH_SIZE + q * FEAT_SIZE, idx, P_cross.transpose());
1081  } // end each previous LM(q)
1082 
1083  // Fill the Pynyn term:
1084  // P_yn_yn = (dyn_dxv * Pxx * ~dyn_dxv) + (dyn_dhn * R *
1085  // ~dyn_dhn);
1086  // --------------------
1087  typename KF::KFMatrix_FxF P_yn_yn =
1088  mrpt::math::multiply_HCHt(dyn_dxv, Pxx);
1089  if (use_dyn_dhn_jacobian)
1090  // Accumulate in P_yn_yn
1091  P_yn_yn += mrpt::math::multiply_HCHt(dyn_dhn, R);
1092  else
1093  P_yn_yn += dyn_dhn_R_dyn_dhnT;
1094 
1095  obj.internal_getPkk().insertMatrix(idx, idx, P_yn_yn);
1096 
1097  obj.getProfiler().leave("KF:9.create new LMs");
1098  }
1099  }
1100 }
1101 
1102 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
1105  VEH_SIZE, OBS_SIZE, 0 /* FEAT_SIZE=0 */, ACT_SIZE, KFTYPE>& obj,
1106  const typename CKalmanFilterCapable<
1107  VEH_SIZE, OBS_SIZE, 0 /* FEAT_SIZE=0 */, ACT_SIZE,
1108  KFTYPE>::vector_KFArray_OBS& Z,
1109  const std::vector<int>& data_association,
1110  const typename CKalmanFilterCapable<
1111  VEH_SIZE, OBS_SIZE, 0 /* FEAT_SIZE=0 */, ACT_SIZE,
1112  KFTYPE>::KFMatrix_OxO& R)
1113 {
1114  // Do nothing: this is NOT a SLAM problem.
1115 }
1116 
1117 template <
1118  size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
1119  typename KFTYPE>
1122  obj)
1123 {
1124  return (obj.getStateVectorLength() - VEH_SIZE) / FEAT_SIZE;
1125 }
1126 // Specialization for FEAT_SIZE=0
1127 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
1129  const CKalmanFilterCapable<
1130  VEH_SIZE, OBS_SIZE, 0 /*FEAT_SIZE*/, ACT_SIZE, KFTYPE>& obj)
1131 {
1132  return 0;
1133 }
1134 
1135 template <
1136  size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
1137  typename KFTYPE>
1138 inline bool isMapEmpty(
1140  obj)
1141 {
1142  return (obj.getStateVectorLength() == VEH_SIZE);
1143 }
1144 // Specialization for FEAT_SIZE=0
1145 template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
1146 inline bool isMapEmpty(
1147  const CKalmanFilterCapable<
1148  VEH_SIZE, OBS_SIZE, 0 /*FEAT_SIZE*/, ACT_SIZE, KFTYPE>& obj)
1149 {
1150  return true;
1151 }
1152 } // namespace detail
1153 } // namespace bayes
1154 } // 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
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:309
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
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
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()
void enter(const std::string_view &func_name)
Start of a named section.
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1889
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: 3316834cb Wed Aug 21 11:42:56 2019 +0200 at miƩ ago 21 11:50:11 CEST 2019