MRPT  1.9.9
CRangeBearingKFSLAM2D.cpp
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 
10 #include "slam-precomp.h" // Precompiled headers
11 
12 #include <mrpt/math/TPose2D.h>
13 #include <mrpt/math/utils.h>
14 #include <mrpt/math/wrap2pi.h>
16 #include <mrpt/opengl/CEllipsoid.h>
19 #include <mrpt/poses/CPosePDF.h>
23 #include <mrpt/system/CTicTac.h>
24 #include <mrpt/system/os.h>
25 
26 using namespace mrpt::slam;
27 using namespace mrpt::maps;
28 using namespace mrpt::math;
29 using namespace mrpt::obs;
30 using namespace mrpt::poses;
31 using namespace mrpt::system;
32 using namespace mrpt;
33 using namespace std;
34 
35 #define STATS_EXPERIMENT 0
36 #define STATS_EXPERIMENT_ALSO_NC 1
37 
38 /*---------------------------------------------------------------
39  Constructor
40  ---------------------------------------------------------------*/
42  : options(), m_action(), m_SF(), m_IDs(), m_SFs()
43 {
44  reset();
45 }
46 
47 /*---------------------------------------------------------------
48  reset
49  ---------------------------------------------------------------*/
51 {
52  m_action.reset();
53  m_SF.reset();
54  m_IDs.clear();
55  m_SFs.clear();
56 
57  // INIT KF STATE
58  m_xkk.assign(3, 0); // State: 3D pose (x,y,phi)
59 
60  // Initial cov:
61  m_pkk.setSize(3, 3);
62  m_pkk.setZero();
63 }
64 
65 /*---------------------------------------------------------------
66  Destructor
67  ---------------------------------------------------------------*/
69 /*---------------------------------------------------------------
70  getCurrentRobotPose
71  ---------------------------------------------------------------*/
73  CPosePDFGaussian& out_robotPose) const
74 {
76 
77  ASSERT_(m_xkk.size() >= 3);
78 
79  // Set 6D pose mean:
80  out_robotPose.mean = CPose2D(m_xkk[0], m_xkk[1], m_xkk[2]);
81 
82  // and cov:
83  out_robotPose.cov = m_pkk.blockCopy<3, 3>(0, 0);
84 
85  MRPT_END
86 }
87 
88 /*---------------------------------------------------------------
89  getCurrentState
90  ---------------------------------------------------------------*/
92  CPosePDFGaussian& out_robotPose,
93  std::vector<TPoint2D>& out_landmarksPositions,
94  std::map<unsigned int, CLandmark::TLandmarkID>& out_landmarkIDs,
95  CVectorDouble& out_fullState, CMatrixDouble& out_fullCovariance) const
96 {
98 
99  ASSERT_(m_xkk.size() >= 3);
100 
101  // Set 6D pose mean:
102  out_robotPose.mean = CPose2D(m_xkk[0], m_xkk[1], m_xkk[2]);
103 
104  // and cov:
105  out_robotPose.cov = m_pkk.blockCopy<3, 3>(0, 0);
106 
107  // Landmarks:
108  ASSERT_(((m_xkk.size() - 3) % 2) == 0);
109  size_t i, nLMs = (m_xkk.size() - 3) / 2;
110  out_landmarksPositions.resize(nLMs);
111  for (i = 0; i < nLMs; i++)
112  {
113  out_landmarksPositions[i].x = m_xkk[3 + i * 2 + 0];
114  out_landmarksPositions[i].y = m_xkk[3 + i * 2 + 1];
115  } // end for i
116 
117  // IDs:
118  out_landmarkIDs = m_IDs.getInverseMap(); // m_IDs_inverse;
119 
120  // Full state:
121  out_fullState.resize(m_xkk.size());
122  std::copy(m_xkk.begin(), m_xkk.end(), out_fullState.begin());
123 
124  // Full cov:
125  out_fullCovariance = m_pkk;
126 
127  MRPT_END
128 }
129 
130 /*---------------------------------------------------------------
131  processActionObservation
132  ---------------------------------------------------------------*/
135 {
136  MRPT_START
137 
138  m_action = action;
139  m_SF = SF;
140 
141  // Sanity check:
143 
144  // ===================================================================================================================
145  // Here's the meat!: Call the main method for the KF algorithm, which will
146  // call all the callback methods as required:
147  // ===================================================================================================================
149 
150  // =============================================================
151  // ADD TO SFs SEQUENCE
152  // =============================================================
154  {
155  CPosePDFGaussian::Ptr auxPosePDF = std::make_shared<CPosePDFGaussian>();
156  getCurrentRobotPose(*auxPosePDF);
157  m_SFs.insert(auxPosePDF, SF);
158  }
159 
160  MRPT_END
161 }
162 
163 /** Must return the action vector u.
164  * \param out_u The action vector which will be passed to OnTransitionModel
165  */
166 void CRangeBearingKFSLAM2D::OnGetAction(KFArray_ACT& u) const
167 {
168  // Get odometry estimation:
169  CActionRobotMovement2D::Ptr actMov2D =
170  m_action->getBestMovementEstimation();
171  CActionRobotMovement3D::Ptr actMov3D =
172  m_action->getActionByClass<CActionRobotMovement3D>();
173  if (actMov3D)
174  {
175  u[0] = actMov3D->poseChange.mean.x();
176  u[1] = actMov3D->poseChange.mean.y();
177  u[2] = actMov3D->poseChange.mean.yaw();
178  }
179  else if (actMov2D)
180  {
181  CPose2D estMovMean;
182  actMov2D->poseChange->getMean(estMovMean);
183  u[0] = estMovMean.x();
184  u[1] = estMovMean.y();
185  u[2] = estMovMean.phi();
186  }
187  else
188  {
189  // Left u as zeros
190  for (size_t i = 0; i < 3; i++) u[i] = 0;
191  }
192 }
193 
194 /** This virtual function musts implement the prediction model of the Kalman
195  * filter.
196  */
198  const KFArray_ACT& u, KFArray_VEH& xv, bool& out_skipPrediction) const
199 {
200  MRPT_START
201 
202  // Do not update the vehicle pose & its covariance until we have some
203  // landmakrs in the map,
204  // otherwise, we are imposing a lower bound to the best uncertainty from now
205  // on:
206  if (size_t(m_xkk.size()) == get_vehicle_size())
207  {
208  out_skipPrediction = true;
209  return;
210  }
211 
212  CPose2D robotPose(xv[0], xv[1], xv[2]);
213  CPose2D odoIncrement(u[0], u[1], u[2]);
214 
215  // Pose composition:
216  robotPose = robotPose + odoIncrement;
217 
218  xv[0] = robotPose.x();
219  xv[1] = robotPose.y();
220  xv[2] = robotPose.phi();
221 
222  MRPT_END
223 }
224 
225 /** This virtual function musts calculate the Jacobian F of the prediction
226  * model.
227  */
229 {
230  MRPT_START
231 
232  // The jacobian of the transition function: dfv_dxv
233  CActionRobotMovement2D::Ptr act2D = m_action->getBestMovementEstimation();
235  m_action->getActionByClass<CActionRobotMovement3D>();
236 
237  if (act3D && act2D)
238  THROW_EXCEPTION("Both 2D & 3D odometry are present!?!?");
239 
240  TPoint2D Ap;
241 
242  if (act3D)
243  {
244  Ap = TPoint2D(CPose2D(act3D->poseChange.mean).asTPose());
245  }
246  else if (act2D)
247  {
248  Ap = TPoint2D(act2D->poseChange->getMeanVal().asTPose());
249  }
250  else
251  {
252  // No odometry at all:
253  F.setIdentity(); // Unit diagonal
254  return;
255  }
256 
257  const kftype cy = cos(m_xkk[2]);
258  const kftype sy = sin(m_xkk[2]);
259 
260  const kftype Ax = Ap.x;
261  const kftype Ay = Ap.y;
262 
263  F.setIdentity(); // Unit diagonal
264 
265  F(0, 2) = -Ax * sy - Ay * cy;
266  F(1, 2) = Ax * cy - Ay * sy;
267 
268  MRPT_END
269 }
270 
271 /** This virtual function musts calculate de noise matrix of the prediction
272  * model.
273  */
274 void CRangeBearingKFSLAM2D::OnTransitionNoise(KFMatrix_VxV& Q) const
275 {
276  MRPT_START
277 
278  // The uncertainty of the 2D odometry, projected from the current position:
279  CActionRobotMovement2D::Ptr act2D = m_action->getBestMovementEstimation();
281  m_action->getActionByClass<CActionRobotMovement3D>();
282 
283  if (act3D && act2D)
284  THROW_EXCEPTION("Both 2D & 3D odometry are present!?!?");
285 
286  CPosePDFGaussian odoIncr;
287 
288  if (!act3D && !act2D)
289  {
290  // Use constant Q:
291  Q.setZero();
292  ASSERT_(int(options.stds_Q_no_odo.size()) == Q.cols());
293  for (size_t i = 0; i < 3; i++)
294  Q(i, i) = square(options.stds_Q_no_odo[i]);
295  return;
296  }
297  else
298  {
299  if (act2D)
300  {
301  // 2D odometry:
302  odoIncr = CPosePDFGaussian(*act2D->poseChange);
303  }
304  else
305  {
306  // 3D odometry:
307  odoIncr = CPosePDFGaussian(act3D->poseChange);
308  }
309  }
310 
311  odoIncr.rotateCov(m_xkk[2]);
312 
313  Q = odoIncr.cov;
314 
315  MRPT_END
316 }
317 
319  const std::vector<size_t>& idx_landmarks_to_predict,
320  vector_KFArray_OBS& out_predictions) const
321 {
322  MRPT_START
323 
324  // Get the sensor pose relative to the robot:
326  m_SF->getObservationByClass<CObservationBearingRange>();
327  ASSERTMSG_(
328  obs,
329  "*ERROR*: This method requires an observation of type "
330  "CObservationBearingRange");
331  const CPose2D sensorPoseOnRobot = CPose2D(obs->sensorLocationOnRobot);
332 
333  /* -------------------------------------------
334  Equations, obtained using matlab, of the relative 2D position of a
335  landmark (xi,yi), relative
336  to a robot 2D pose (x0,y0,phi)
337  Refer to technical report "6D EKF derivation...", 2008
338 
339  x0 y0 phi0 % Robot's 2D pose
340  x0s y0s phis % Sensor's 2D pose relative to robot
341  xi yi % Absolute 2D landmark coordinates:
342 
343  Hx : dh_dxv -> Jacobian of the observation model wrt the robot pose
344  Hy : dh_dyi -> Jacobian of the observation model wrt each landmark
345  mean position
346 
347  Sizes:
348  h: 1x2
349  Hx: 2x3
350  Hy: 2x2
351  ------------------------------------------- */
352  // Mean of the prior of the robot pose:
353  const CPose2D robotPose(m_xkk[0], m_xkk[1], m_xkk[2]);
354 
355  const size_t vehicle_size = get_vehicle_size();
356  const size_t feature_size = get_feature_size();
357 
358  const CPose2D sensorPoseAbs = robotPose + sensorPoseOnRobot;
359 
360  // ---------------------------------------------------
361  // Observation prediction
362  // ---------------------------------------------------
363  const size_t N = idx_landmarks_to_predict.size();
364  out_predictions.resize(N);
365  for (size_t i = 0; i < N; i++)
366  {
367  const size_t idx_lm = idx_landmarks_to_predict[i];
368  ASSERTDEB_(idx_lm < this->getNumberOfLandmarksInTheMap());
369 
370  // Landmark absolute position in the map:
371  const kftype xi = m_xkk[vehicle_size + feature_size * idx_lm + 0];
372  const kftype yi = m_xkk[vehicle_size + feature_size * idx_lm + 1];
373 
374  const double Axi = xi - sensorPoseAbs.x();
375  const double Ayi = yi - sensorPoseAbs.y();
376 
377  out_predictions[i][0] = sqrt(square(Axi) + square(Ayi)); // Range
378  out_predictions[i][1] =
379  mrpt::math::wrapToPi(atan2(Ayi, Axi) - sensorPoseAbs.phi()); // Yaw
380  }
381 
382  MRPT_END
383 }
384 
386  size_t idx_landmark_to_predict, KFMatrix_OxV& Hx, KFMatrix_OxF& Hy) const
387 {
388  MRPT_START
389 
390  // Get the sensor pose relative to the robot:
392  m_SF->getObservationByClass<CObservationBearingRange>();
393  ASSERTMSG_(
394  obs,
395  "*ERROR*: This method requires an observation of type "
396  "CObservationBearingRange");
397  const CPose2D sensorPoseOnRobot = CPose2D(obs->sensorLocationOnRobot);
398 
399  /* -------------------------------------------
400  Equations, obtained using matlab, of the relative 2D position of a
401  landmark (xi,yi), relative
402  to a robot 2D pose (x0,y0,phi)
403  Refer to technical report "6D EKF derivation...", 2008
404 
405  x0 y0 phi0 % Robot's 2D pose
406  x0s y0s phis % Sensor's 2D pose relative to robot
407  xi yi % Absolute 2D landmark coordinates:
408 
409  Hx : dh_dxv -> Jacobian of the observation model wrt the robot pose
410  Hy : dh_dyi -> Jacobian of the observation model wrt each landmark
411  mean position
412 
413  Sizes:
414  h: 1x2
415  Hx: 2x3
416  Hy: 2x2
417  ------------------------------------------- */
418  // Mean of the prior of the robot pose:
419  const CPose2D robotPose(m_xkk[0], m_xkk[1], m_xkk[2]);
420 
421  const size_t vehicle_size = get_vehicle_size();
422  const size_t feature_size = get_feature_size();
423 
424  // Robot 6D pose:
425  const kftype x0 = m_xkk[0];
426  const kftype y0 = m_xkk[1];
427  const kftype phi0 = m_xkk[2];
428 
429  const kftype cphi0 = cos(phi0);
430  const kftype sphi0 = sin(phi0);
431 
432  // Sensor 2D pose on robot:
433  const kftype x0s = sensorPoseOnRobot.x();
434  const kftype y0s = sensorPoseOnRobot.y();
435  const kftype phis = sensorPoseOnRobot.phi();
436 
437  const kftype cphi0s = cos(phi0 + phis);
438  const kftype sphi0s = sin(phi0 + phis);
439 
440  const CPose2D sensorPoseAbs = robotPose + sensorPoseOnRobot;
441 
442  // Landmark absolute position in the map:
443  const kftype xi =
444  m_xkk[vehicle_size + feature_size * idx_landmark_to_predict + 0];
445  const kftype yi =
446  m_xkk[vehicle_size + feature_size * idx_landmark_to_predict + 1];
447 
448  // ---------------------------------------------------
449  // Generate dhi_dxv: A 2x3 block
450  // ---------------------------------------------------
451  const kftype EXP1 =
452  -2 * yi * y0s * cphi0 - 2 * yi * y0 + 2 * xi * y0s * sphi0 -
453  2 * xi * x0 - 2 * xi * x0s * cphi0 - 2 * yi * x0s * sphi0 +
454  2 * y0s * y0 * cphi0 - 2 * y0s * x0 * sphi0 + 2 * y0 * x0s * sphi0 +
455  square(x0) + 2 * x0s * x0 * cphi0 + square(x0s) + square(y0s) +
456  square(xi) + square(yi) + square(y0);
457  const kftype sqrtEXP1_1 = kftype(1) / sqrt(EXP1);
458 
459  const kftype EXP2 = cphi0s * xi + sphi0s * yi - sin(phis) * y0s -
460  y0 * sphi0s - x0s * cos(phis) - x0 * cphi0s;
461  const kftype EXP2sq = square(EXP2);
462 
463  const kftype EXP3 = -sphi0s * xi + cphi0s * yi - cos(phis) * y0s -
464  y0 * cphi0s + x0s * sin(phis) + x0 * sphi0s;
465  const kftype EXP3sq = square(EXP3);
466 
467  const kftype EXP4 = kftype(1) / (1 + EXP3sq / EXP2sq);
468 
469  Hx(0, 0) = (-xi - sphi0 * y0s + cphi0 * x0s + x0) * sqrtEXP1_1;
470  Hx(0, 1) = (-yi + cphi0 * y0s + y0 + sphi0 * x0s) * sqrtEXP1_1;
471  Hx(0, 2) = (y0s * xi * cphi0 + y0s * yi * sphi0 - y0 * y0s * sphi0 -
472  x0 * y0s * cphi0 + x0s * xi * sphi0 - x0s * yi * cphi0 +
473  y0 * x0s * cphi0 - x0s * x0 * sphi0) *
474  sqrtEXP1_1;
475 
476  Hx(1, 0) = (sphi0s / (EXP2) + (EXP3) / EXP2sq * cphi0s) * EXP4;
477  Hx(1, 1) = (-cphi0s / (EXP2) + (EXP3) / EXP2sq * sphi0s) * EXP4;
478  Hx(1, 2) =
479  ((-cphi0s * xi - sphi0s * yi + y0 * sphi0s + x0 * cphi0s) / (EXP2) -
480  (EXP3) / EXP2sq *
481  (-sphi0s * xi + cphi0s * yi - y0 * cphi0s + x0 * sphi0s)) *
482  EXP4;
483 
484  // ---------------------------------------------------
485  // Generate dhi_dyi: A 2x2 block
486  // ---------------------------------------------------
487  Hy(0, 0) = (xi + sphi0 * y0s - cphi0 * x0s - x0) * sqrtEXP1_1;
488  Hy(0, 1) = (yi - cphi0 * y0s - y0 - sphi0 * x0s) * sqrtEXP1_1;
489 
490  Hy(1, 0) = (-sphi0s / (EXP2) - (EXP3) / EXP2sq * cphi0s) * EXP4;
491  Hy(1, 1) = (cphi0s / (EXP2) - (EXP3) / EXP2sq * sphi0s) * EXP4;
492 
493  MRPT_END
494 }
495 
496 /** This is called between the KF prediction step and the update step, and the
497  * application must return the observations and, when applicable, the data
498  * association between these observations and the current map.
499  *
500  * \param out_z N vectors, each for one "observation" of length OBS_SIZE, N
501  * being the number of "observations": how many observed landmarks for a map, or
502  * just one if not applicable.
503  * \param out_data_association An empty vector or, where applicable, a vector
504  * where the i'th element corresponds to the position of the observation in the
505  * i'th row of out_z within the system state vector (in the range
506  * [0,getNumberOfLandmarksInTheMap()-1]), or -1 if it is a new map element and
507  * we want to insert it at the end of this KF iteration.
508  * \param in_S The full covariance matrix of the observation predictions (i.e.
509  * the "innovation covariance matrix"). This is a M·O x M·O matrix with M=length
510  * of "in_lm_indices_in_S".
511  * \param in_lm_indices_in_S The indices of the map landmarks (range
512  * [0,getNumberOfLandmarksInTheMap()-1]) that can be found in the matrix in_S.
513  *
514  * This method will be called just once for each complete KF iteration.
515  * \note It is assumed that the observations are independent, i.e. there are NO
516  * cross-covariances between them.
517  */
519  vector_KFArray_OBS& Z, std::vector<int>& data_association,
520  const vector_KFArray_OBS& all_predictions, const KFMatrix& S,
521  const std::vector<size_t>& lm_indices_in_S, const KFMatrix_OxO& R)
522 {
523  MRPT_START
524 
525  constexpr size_t obs_size = get_observation_size();
526 
527  // Z: Observations
528  CObservationBearingRange::TMeasurementList::const_iterator itObs;
529 
531  m_SF->getObservationByClass<CObservationBearingRange>();
532  ASSERTMSG_(
533  obs,
534  "*ERROR*: This method requires an observation of type "
535  "CObservationBearingRange");
536 
537  const size_t N = obs->sensedData.size();
538  Z.resize(N);
539 
540  size_t row;
541  for (row = 0, itObs = obs->sensedData.begin();
542  itObs != obs->sensedData.end(); ++itObs, ++row)
543  {
544  // Fill one row in Z:
545  Z[row][0] = itObs->range;
546  Z[row][1] = itObs->yaw;
547  ASSERTMSG_(
548  itObs->pitch == 0,
549  "ERROR: Observation contains pitch!=0 but this is 2D-SLAM!!!");
550  }
551 
552  // Data association:
553  // ---------------------
554  data_association.assign(N, -1); // Initially, all new landmarks
555 
556  // For each observed LM:
557  std::vector<size_t> obs_idxs_needing_data_assoc;
558  obs_idxs_needing_data_assoc.reserve(N);
559 
560  {
561  std::vector<int>::iterator itDA;
562  for (row = 0, itObs = obs->sensedData.begin(),
563  itDA = data_association.begin();
564  itObs != obs->sensedData.end(); ++itObs, ++itDA, ++row)
565  {
566  // Fill data asociation: Using IDs!
567  if (itObs->landmarkID < 0)
568  obs_idxs_needing_data_assoc.push_back(row);
569  else
570  {
572  CLandmark::TLandmarkID, unsigned int>::iterator itID;
573  if ((itID = m_IDs.find_key(itObs->landmarkID)) != m_IDs.end())
574  *itDA = itID->second; // This row in Z corresponds to the
575  // i'th map element in the state
576  // vector:
577  }
578  }
579  }
580 
581  // ---- Perform data association ----
582  // Only for observation indices in "obs_idxs_needing_data_assoc"
583  if (obs_idxs_needing_data_assoc.empty())
584  {
585  // We don't need to do DA:
587 
588  // Save them for the case the external user wants to access them:
589  for (size_t idxObs = 0; idxObs < data_association.size(); idxObs++)
590  {
591  int da = data_association[idxObs];
592  if (da >= 0)
594  data_association[idxObs];
595  }
596 
597 #if STATS_EXPERIMENT // DEBUG: Generate statistic info
598  {
599  static CFileOutputStream fC("metric_stats_C.txt", true);
600 #if STATS_EXPERIMENT_ALSO_NC
601  static CFileOutputStream fNC("metric_stats_NC.txt", true);
602 #endif
603 
604  std::vector<int>::iterator itDA;
605  size_t idx_obs = 0;
606  for (itDA = data_association.begin();
607  itDA != data_association.end(); ++itDA, ++idx_obs)
608  {
609  if (*itDA == -1) continue;
610  const size_t lm_idx_in_map = *itDA;
611  size_t valid_idx_pred =
612  find_in_vector(lm_idx_in_map, lm_indices_in_S);
613 
614  const KFArray_OBS& obs_mu = Z[idx_obs];
615 
616  for (size_t idx_pred = 0; idx_pred < lm_indices_in_S.size();
617  idx_pred++)
618  {
619  const KFArray_OBS& lm_mu =
620  all_predictions[lm_indices_in_S[idx_pred]];
621 
622  const size_t base_idx_in_S = obs_size * idx_pred;
623  KFMatrix_OxO lm_cov;
624  S.extractMatrix(base_idx_in_S, base_idx_in_S, lm_cov);
625 
626  double md, log_pdf;
628  lm_mu - obs_mu, lm_cov, md, log_pdf);
629 
630  if (valid_idx_pred == idx_pred)
631  fC.printf("%e %e\n", md, log_pdf);
632  else
633  {
634 #if STATS_EXPERIMENT_ALSO_NC
635  fNC.printf("%e %e\n", md, log_pdf);
636 #endif
637  }
638  }
639  }
640  }
641 #endif
642  }
643  else
644  {
645  // Build a Z matrix with the observations that need dat.assoc:
646  const size_t nObsDA = obs_idxs_needing_data_assoc.size();
647 
648  CMatrixDynamic<kftype> Z_obs_means(nObsDA, obs_size);
649  for (size_t i = 0; i < nObsDA; i++)
650  {
651  const size_t idx = obs_idxs_needing_data_assoc[i];
652  for (unsigned k = 0; k < obs_size; k++)
653  Z_obs_means(i, k) = Z[idx][k];
654  }
655 
656  // Vehicle uncertainty
657  /*const KFMatrix_VxV Pxx = m_pkk.extractMatrix<get_vehicle_size(),
658  * get_vehicle_size()>(0, 0);*/
659 
660  // Build predictions:
661  // ---------------------------
662  const size_t nPredictions = lm_indices_in_S.size();
664 
665  // S is the covariance of the predictions:
667 
668  // The means:
669  m_last_data_association.Y_pred_means.setSize(nPredictions, obs_size);
670  for (size_t q = 0; q < nPredictions; q++)
671  {
672  const size_t i = lm_indices_in_S[q];
673  for (size_t w = 0; w < obs_size; w++)
675  all_predictions[i][w];
677  i); // for the conversion of indices...
678  }
679 
680  // Do Dat. Assoc :
681  // ---------------------------
682  if (nPredictions)
683  {
684  // CMatrixDouble Z_obs_cov = CMatrixDouble(R);
685 
687  Z_obs_means, // Z_obs_cov,
692  true, // Use KD-tree
696 
697  // Return pairings to the main KF algorithm:
698  for (auto it = m_last_data_association.results.associations.begin();
699  it != m_last_data_association.results.associations.end(); ++it)
700  data_association[it->first] = it->second;
701  }
702  }
703  // ---- End of data association ----
704 
705  MRPT_END
706 }
707 
708 /** This virtual function musts normalize the state vector and covariance matrix
709  * (only if its necessary).
710  */
712 {
713  // Check angles:
715 }
716 
717 /*---------------------------------------------------------------
718  loadOptions
719  ---------------------------------------------------------------*/
722 {
723  // Main
724  options.loadFromConfigFile(ini, "RangeBearingKFSLAM");
725  KF_options.loadFromConfigFile(ini, "RangeBearingKFSLAM_KalmanFilter");
726 }
727 
728 /*---------------------------------------------------------------
729  loadOptions
730  ---------------------------------------------------------------*/
732  const mrpt::config::CConfigFileBase& source, const std::string& section)
733 {
735 
736  source.read_vector(section, "stds_Q_no_odo", stds_Q_no_odo, stds_Q_no_odo);
737  ASSERT_(stds_Q_no_odo.size() == 3);
739 
741  source.read_float(section, "std_sensor_range", std_sensor_range);
743  section, "std_sensor_yaw_deg", RAD2DEG(std_sensor_yaw)));
744 
745  MRPT_LOAD_CONFIG_VAR(quantiles_3D_representation, float, source, section);
746  MRPT_LOAD_CONFIG_VAR(create_simplemap, bool, source, section);
747 
749  section, "data_assoc_method", data_assoc_method);
751  section, "data_assoc_metric", data_assoc_metric);
753  section, "data_assoc_IC_metric", data_assoc_IC_metric);
754 
755  MRPT_LOAD_CONFIG_VAR(data_assoc_IC_chi2_thres, double, source, section);
756  MRPT_LOAD_CONFIG_VAR(data_assoc_IC_ml_threshold, double, source, section);
757 }
758 
759 /*---------------------------------------------------------------
760  Constructor
761  ---------------------------------------------------------------*/
763  : stds_Q_no_odo(3, 0),
764 
765  std_sensor_yaw(DEG2RAD(0.5f))
766 
767 {
768  stds_Q_no_odo[0] = 0.10f;
769  stds_Q_no_odo[1] = 0.10f;
770  stds_Q_no_odo[2] = DEG2RAD(4.0f);
771 }
772 
773 /*---------------------------------------------------------------
774  dumpToTextStream
775  ---------------------------------------------------------------*/
777 {
778  using namespace mrpt::typemeta;
779 
780  out << "\n----------- [CRangeBearingKFSLAM2D::TOptions] ------------ \n\n";
781 
782  out << mrpt::format(
783  "data_assoc_method = %s\n",
785  .c_str());
786  out << mrpt::format(
787  "data_assoc_metric = %s\n",
789  .c_str());
790  out << mrpt::format(
791  "data_assoc_IC_chi2_thres = %.06f\n",
792  data_assoc_IC_chi2_thres);
793  out << mrpt::format(
794  "data_assoc_IC_metric = %s\n",
796  .c_str());
797  out << mrpt::format(
798  "data_assoc_IC_ml_threshold = %.06f\n",
799  data_assoc_IC_ml_threshold);
800 
801  out << "\n";
802 }
803 
805  const KFArray_OBS& in_z, KFArray_FEAT& yn, KFMatrix_FxV& dyn_dxv,
806  KFMatrix_FxO& dyn_dhn) const
807 {
808  MRPT_START
809 
810  /* -------------------------------------------
811  Equations, obtained using matlab
812 
813  x0 y0 phi0 % Robot's 2D pose
814  x0s y0s phis % Sensor's 2D pose relative to robot
815  hr ha % Observation hn: range, yaw
816 
817  xi yi % Absolute 2D landmark coordinates:
818 
819  dyn_dxv -> Jacobian of the inv. observation model wrt the robot pose
820  dyn_dhn -> Jacobian of the inv. observation model wrt each landmark
821  observation
822 
823  Sizes:
824  hn: 1x2 <--
825  yn: 1x2 -->
826  dyn_dxv: 2x3 -->
827  dyn_dhn: 2x2 -->
828  ------------------------------------------- */
829 
831  m_SF->getObservationByClass<CObservationBearingRange>();
832  ASSERTMSG_(
833  obs,
834  "*ERROR*: This method requires an observation of type "
835  "CObservationBearingRange");
836  const CPose2D sensorPoseOnRobot = CPose2D(obs->sensorLocationOnRobot);
837 
838  // Mean of the prior of the robot pose:
839  const TPose2D robotPose(m_xkk[0], m_xkk[1], m_xkk[2]);
840 
841  // Robot 2D pose:
842  const kftype x0 = m_xkk[0];
843  const kftype y0 = m_xkk[1];
844  const kftype phi0 = m_xkk[2];
845 
846  const kftype cphi0 = cos(phi0);
847  const kftype sphi0 = sin(phi0);
848 
849  // Sensor 6D pose on robot:
850  const kftype x0s = sensorPoseOnRobot.x();
851  const kftype y0s = sensorPoseOnRobot.y();
852  const kftype phis = sensorPoseOnRobot.phi();
853 
854  const kftype hr = in_z[0];
855  const kftype ha = in_z[1];
856 
857  const kftype cphi_0sa = cos(phi0 + phis + ha);
858  const kftype sphi_0sa = sin(phi0 + phis + ha);
859 
860  // Compute the mean 2D absolute coordinates:
861  yn[0] = hr * cphi_0sa + cphi0 * x0s - sphi0 * y0s + x0;
862  yn[1] = hr * sphi_0sa + sphi0 * x0s + cphi0 * y0s + y0;
863 
864  // Jacobian wrt xv:
865  dyn_dxv(0, 0) = 1;
866  dyn_dxv(0, 1) = 0;
867  dyn_dxv(0, 2) = -hr * sphi_0sa - sphi0 * x0s - cphi0 * y0s;
868 
869  dyn_dxv(1, 0) = 0;
870  dyn_dxv(1, 1) = 1;
871  dyn_dxv(1, 2) = hr * cphi_0sa + cphi0 * x0s - sphi0 * y0s;
872 
873  // Jacobian wrt hn:
874  dyn_dhn(0, 0) = cphi_0sa;
875  dyn_dhn(0, 1) = -hr * sphi_0sa;
876 
877  dyn_dhn(1, 0) = sphi_0sa;
878  dyn_dhn(1, 1) = hr * cphi_0sa;
879 
880  MRPT_END
881 }
882 
883 /** If applicable to the given problem, do here any special handling of adding a
884  * new landmark to the map.
885  * \param in_obsIndex The index of the observation whose inverse sensor is to
886  * be computed. It corresponds to the row in in_z where the observation can be
887  * found.
888  * \param in_idxNewFeat The index that this new feature will have in the state
889  * vector (0:just after the vehicle state, 1: after that,...). Save this number
890  * so data association can be done according to these indices.
891  * \sa OnInverseObservationModel
892  */
894  const size_t in_obsIdx, const size_t in_idxNewFeat)
895 {
896  MRPT_START
897 
899  m_SF->getObservationByClass<CObservationBearingRange>();
900  ASSERTMSG_(
901  obs,
902  "*ERROR*: This method requires an observation of type "
903  "CObservationBearingRange");
904 
905  // ----------------------------------------------
906  // introduce in the lists of ID<->index in map:
907  // ----------------------------------------------
908  ASSERT_(in_obsIdx < obs->sensedData.size());
909  if (obs->sensedData[in_obsIdx].landmarkID >= 0)
910  {
911  // The sensor provides us a LM ID... use it:
912  m_IDs.insert(obs->sensedData[in_obsIdx].landmarkID, in_idxNewFeat);
913  }
914  else
915  {
916  // Features do not have IDs... use indices:
917  m_IDs.insert(in_idxNewFeat, in_idxNewFeat);
918  }
919 
921  in_idxNewFeat; // Just for stats, etc...
922 
923  MRPT_END
924 }
925 
926 /*---------------------------------------------------------------
927  getAs3DObject
928  ---------------------------------------------------------------*/
930  mrpt::opengl::CSetOfObjects::Ptr& outObj) const
931 {
932  outObj->clear();
933 
934  // ------------------------------------------------
935  // Add the XYZ corner for the current area:
936  // ------------------------------------------------
937  outObj->insert(opengl::stock_objects::CornerXYZ());
938 
939  // 2D ellipsoid for robot pose:
940  CPoint2DPDFGaussian pointGauss;
941  pointGauss.mean.x(m_xkk[0]);
942  pointGauss.mean.y(m_xkk[1]);
943  pointGauss.cov = m_pkk.blockCopy<2, 2>(0, 0);
944 
945  {
946  auto ellip = opengl::CEllipsoid::Create();
947 
948  ellip->setPose(pointGauss.mean);
949  ellip->setCovMatrix(pointGauss.cov);
950  ellip->enableDrawSolid3D(false);
951  ellip->setQuantiles(options.quantiles_3D_representation);
952  ellip->set3DsegmentsCount(10);
953  ellip->setColor(1, 0, 0);
954 
955  outObj->insert(ellip);
956  }
957 
958  // 2D ellipsoids for landmarks:
959  const size_t nLMs = (m_xkk.size() - 3) / 2;
960  for (size_t i = 0; i < nLMs; i++)
961  {
962  pointGauss.mean.x(m_xkk[3 + 2 * i + 0]);
963  pointGauss.mean.y(m_xkk[3 + 2 * i + 1]);
964  pointGauss.cov = m_pkk.blockCopy<2, 2>(3 + 2 * i, 3 + 2 * i);
965 
966  auto ellip = opengl::CEllipsoid::Create();
967 
968  ellip->setName(format("%u", static_cast<unsigned int>(i)));
969  ellip->enableShowName(true);
970  ellip->setPose(pointGauss.mean);
971  ellip->setCovMatrix(pointGauss.cov);
972  ellip->enableDrawSolid3D(false);
973  ellip->setQuantiles(options.quantiles_3D_representation);
974  ellip->set3DsegmentsCount(10);
975 
976  ellip->setColor(0, 0, 1);
977 
978  outObj->insert(ellip);
979  }
980 }
981 
982 /*---------------------------------------------------------------
983  saveMapAndPathRepresentationAsMATLABFile
984  ---------------------------------------------------------------*/
986  const string& fil, float stdCount, const string& styleLandmarks,
987  const string& stylePath, const string& styleRobot) const
988 {
989  FILE* f = os::fopen(fil.c_str(), "wt");
990  if (!f) return;
991 
992  CMatrixDouble cov(2, 2);
993  CVectorDouble mean(2);
994 
995  // Header:
996  os::fprintf(
997  f,
998  "%%--------------------------------------------------------------------"
999  "\n");
1000  os::fprintf(f, "%% File automatically generated using the MRPT method:\n");
1001  os::fprintf(
1002  f,
1003  "%% "
1004  "'CRangeBearingKFSLAM2D::saveMapAndPath2DRepresentationAsMATLABFile'"
1005  "\n");
1006  os::fprintf(f, "%%\n");
1007  os::fprintf(f, "%% ~ MRPT ~\n");
1008  os::fprintf(
1009  f, "%% Jose Luis Blanco Claraco, University of Malaga @ 2008\n");
1010  os::fprintf(f, "%% https://www.mrpt.org/ \n");
1011  os::fprintf(
1012  f,
1013  "%%--------------------------------------------------------------------"
1014  "\n");
1015 
1016  // Main code:
1017  os::fprintf(f, "hold on;\n\n");
1018 
1019  size_t i, nLMs = (m_xkk.size() - get_vehicle_size()) / get_feature_size();
1020 
1021  for (i = 0; i < nLMs; i++)
1022  {
1023  size_t idx = get_vehicle_size() + i * get_feature_size();
1024 
1025  cov(0, 0) = m_pkk(idx + 0, idx + 0);
1026  cov(1, 1) = m_pkk(idx + 1, idx + 1);
1027  cov(0, 1) = cov(1, 0) = m_pkk(idx + 0, idx + 1);
1028 
1029  mean[0] = m_xkk[idx + 0];
1030  mean[1] = m_xkk[idx + 1];
1031 
1032  // Command to draw the 2D ellipse:
1033  os::fprintf(
1034  f, "%s",
1035  math::MATLAB_plotCovariance2D(cov, mean, stdCount, styleLandmarks)
1036  .c_str());
1037  }
1038 
1039  // Now: the robot path:
1040  // ------------------------------
1041  if (m_SFs.size())
1042  {
1043  os::fprintf(f, "\nROB_PATH=[");
1044  for (i = 0; i < m_SFs.size(); i++)
1045  {
1046  CSensoryFrame::Ptr dummySF;
1047  CPose3DPDF::Ptr pdf3D;
1048  m_SFs.get(i, pdf3D, dummySF);
1049 
1050  CPose3D p;
1051  pdf3D->getMean(p);
1052  CPoint3D pnt3D(p); // 6D -> 3D only
1053 
1054  os::fprintf(f, "%.04f %.04f", pnt3D.x(), pnt3D.y());
1055  if (i < (m_SFs.size() - 1)) os::fprintf(f, ";");
1056  }
1057  os::fprintf(f, "];\n");
1058 
1059  os::fprintf(
1060  f, "plot(ROB_PATH(:,1),ROB_PATH(:,2),'%s');\n", stylePath.c_str());
1061  }
1062 
1063  // The robot pose:
1064  cov(0, 0) = m_pkk(0, 0);
1065  cov(1, 1) = m_pkk(1, 1);
1066  cov(0, 1) = cov(1, 0) = m_pkk(0, 1);
1067 
1068  mean[0] = m_xkk[0];
1069  mean[1] = m_xkk[1];
1070 
1071  os::fprintf(
1072  f, "%s",
1073  math::MATLAB_plotCovariance2D(cov, mean, stdCount, styleRobot).c_str());
1074 
1075  os::fprintf(f, "\naxis equal;\n");
1076  os::fclose(f);
1077 }
1078 
1079 /** Computes A=A-B, which may need to be re-implemented depending on the
1080  * topology of the individual scalar components (eg, angles).
1081  */
1083  KFArray_OBS& A, const KFArray_OBS& B) const
1084 {
1085  A[0] -= B[0];
1086  A[1] -= B[1];
1088 }
1089 
1090 /** Return the observation NOISE covariance matrix, that is, the model of the
1091  * Gaussian additive noise of the sensor.
1092  * \param out_R The noise covariance matrix. It might be non diagonal, but
1093  * it'll usually be.
1094  */
1096 {
1097  out_R(0, 0) = square(options.std_sensor_range);
1098  out_R(1, 1) = square(options.std_sensor_yaw);
1099 }
1100 
1101 /** This will be called before OnGetObservationsAndDataAssociation to allow the
1102  * application to reduce the number of covariance landmark predictions to be
1103  * made.
1104  * For example, features which are known to be "out of sight" shouldn't be
1105  * added to the output list to speed up the calculations.
1106  * \param in_all_prediction_means The mean of each landmark predictions; the
1107  * computation or not of the corresponding covariances is what we're trying to
1108  * determined with this method.
1109  * \param out_LM_indices_to_predict The list of landmark indices in the map
1110  * [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.
1111  * \note This is not a pure virtual method, so it should be implemented only if
1112  * desired. The default implementation returns a vector with all the landmarks
1113  * in the map.
1114  * \sa OnGetObservations, OnDataAssociation
1115  */
1117  const vector_KFArray_OBS& prediction_means,
1118  std::vector<size_t>& out_LM_indices_to_predict) const
1119 {
1121  m_SF->getObservationByClass<CObservationBearingRange>();
1122  ASSERTMSG_(
1123  obs,
1124  "*ERROR*: This method requires an observation of type "
1125  "CObservationBearingRange");
1126 
1127  const double sensor_max_range = obs->maxSensorDistance;
1128  const double fov_yaw = obs->fieldOfView_yaw;
1129 
1130  const double max_vehicle_loc_uncertainty =
1131  4 * std::sqrt(m_pkk(0, 0) + m_pkk(1, 1));
1132  const double max_vehicle_ang_uncertainty = 4 * std::sqrt(m_pkk(2, 2));
1133 
1134  out_LM_indices_to_predict.clear();
1135  for (size_t i = 0; i < prediction_means.size(); i++)
1136 #if (!STATS_EXPERIMENT) // In the experiment we force errors too far and some
1137  // predictions are out of range, so just generate all
1138  // of them:
1139  if (prediction_means[i][0] <
1140  (1.5 + sensor_max_range + max_vehicle_loc_uncertainty +
1141  4 * options.std_sensor_range) &&
1142  fabs(prediction_means[i][1]) <
1143  (20.0_deg + 0.5 * fov_yaw + max_vehicle_ang_uncertainty +
1144  4 * options.std_sensor_yaw))
1145 #endif
1146  {
1147  out_LM_indices_to_predict.push_back(i);
1148  }
1149 }
1150 
1151 /** Only called if using a numeric approximation of the transition Jacobian,
1152  * this method must return the increments in each dimension of the vehicle state
1153  * vector while estimating the Jacobian.
1154  */
1156  KFArray_VEH& out_increments) const
1157 {
1158  for (size_t i = 0; i < get_vehicle_size(); i++) out_increments[i] = 1e-6;
1159 }
1160 
1161 /** Only called if using a numeric approximation of the observation Jacobians,
1162  * this method must return the increments in each dimension of the vehicle state
1163  * vector while estimating the Jacobian.
1164  */
1166  KFArray_VEH& out_veh_increments, KFArray_FEAT& out_feat_increments) const
1167 {
1168  for (size_t i = 0; i < get_vehicle_size(); i++)
1169  out_veh_increments[i] = 1e-6;
1170  for (size_t i = 0; i < get_feature_size(); i++)
1171  out_feat_increments[i] = 1e-6;
1172 }
mrpt::math::CVectorFloat stds_Q_no_odo
A 3-length vector with the std.
const_iterator end() const
Definition: bimap.h:48
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
Definition: MatrixBase.h:233
#define MRPT_START
Definition: exceptions.h:241
CPose2D mean
The mean value.
void OnGetAction(KFArray_ACT &out_u) const override
Must return the action vector u.
void OnObservationJacobiansNumericGetIncrements(KFArray_VEH &out_veh_increments, KFArray_FEAT &out_feat_increments) const override
Only called if using a numeric approximation of the observation Jacobians, this method must return th...
TOptions options
The options for the algorithm.
TPoint2D_< double > TPoint2D
Lightweight 2D point.
Definition: TPoint2D.h:204
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
T x
X,Y coordinates.
Definition: TPoint2D.h:24
uint64_t TLandmarkID
Unique IDs for landmarks.
A gaussian distribution for 2D points.
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:275
void OnObservationJacobians(size_t idx_landmark_to_predict, KFMatrix_OxV &Hx, KFMatrix_OxF &Hy) const override
Implements the observation Jacobians and (when applicable) .
void loadFromConfigFile(const mrpt::config::CConfigFileBase &iniFile, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
bool create_simplemap
Whether to fill m_SFs (default=false)
const_iterator find_key(const KEY &k) const
Definition: bimap.h:140
void OnObservationModel(const std::vector< size_t > &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const override
void reset()
Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,0,0).
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
Definition: bimap.h:71
size_type size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
~CRangeBearingKFSLAM2D() override
Destructor.
STL namespace.
void runOneKalmanIteration()
The main entry point, executes one complete step: prediction + update.
mrpt::math::CVectorFixed< double, VEH_SIZE > KFArray_VEH
void OnNormalizeStateVector() override
This method is called after the prediction and after the update, to give the user an opportunity to n...
KFMatrix m_pkk
The system full covariance matrix.
mrpt::obs::CActionCollection::Ptr m_action
Set up by processActionObservation.
mrpt::math::CMatrixFixed< double, FEAT_SIZE, OBS_SIZE > KFMatrix_FxO
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
#define STATS_EXPERIMENT
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
void clear()
Clear the contents of the bi-map.
Definition: bimap.h:64
mrpt::math::CMatrixDynamic< kftype > Y_pred_means
size_t size() const
Definition: bimap.h:57
mrpt::math::TPose2D asTPose() const
Definition: CPose2D.cpp:468
ENUMTYPE read_enum(const std::string &section, const std::string &name, const ENUMTYPE &defaultValue, bool failIfNotFound=false) const
Reads an "enum" value, where the value in the config file can be either a numerical value or the symb...
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.
mrpt::maps::CSimpleMap m_SFs
The sequence of all the observations and the robot path (kept for debugging, statistics,etc)
const std::map< VALUE, KEY > & getInverseMap() const
Return a read-only reference to the internal map KEY->VALUES.
Definition: bimap.h:62
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
void OnTransitionJacobianNumericGetIncrements(KFArray_VEH &out_increments) const override
Only called if using a numeric approximation of the transition Jacobian, this method must return the ...
Represents a probabilistic 3D (6D) movement.
void OnTransitionNoise(KFMatrix_VxV &out_Q) const override
Implements the transition noise covariance .
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:61
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
void getCurrentState(mrpt::poses::CPosePDFGaussian &out_robotPose, std::vector< mrpt::math::TPoint2D > &out_landmarksPositions, std::map< unsigned int, mrpt::maps::CLandmark::TLandmarkID > &out_landmarkIDs, mrpt::math::CVectorDouble &out_fullState, mrpt::math::CMatrixDouble &out_fullCovariance) const
Returns the complete mean and cov.
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
Definition: bimap.h:31
void mahalanobisDistance2AndLogPDF(const VECLIKE &diff_mean, const MATRIXLIKE &cov, T &maha2_out, T &log_pdf_out)
Computes both, the logarithm of the PDF and the square Mahalanobis distance between a point (given by...
Definition: data_utils.h:209
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
void OnInverseObservationModel(const KFArray_OBS &in_z, KFArray_FEAT &out_yn, KFMatrix_FxV &out_dyn_dxv, KFMatrix_FxO &out_dyn_dhn) const override
If applicable to the given problem, this method implements the inverse observation model needed to ex...
TKF_options KF_options
Generic options for the Kalman Filter algorithm itself.
double phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
T square(const T x)
Inline function for the square of a number.
A helper class that can convert an enum value into its textual representation, and viceversa...
TDataAssociationMetric
Different metrics for data association, used in mrpt::slam::data_association For a comparison of both...
constexpr double DEG2RAD(const double x)
Degrees to radians.
void OnTransitionJacobian(KFMatrix_VxV &out_F) const override
Implements the transition Jacobian .
std::map< size_t, size_t > newly_inserted_landmarks
Map from the 0-based index within the last observation and the landmark 0-based index in the map (the...
double data_assoc_IC_ml_threshold
Only if data_assoc_IC_metric==ML, the log-ML threshold (Default=0.0)
void assign(const std::size_t N, const Scalar value)
This namespace contains representation of robot actions and observations.
void OnSubstractObservationVectors(KFArray_OBS &A, const KFArray_OBS &B) const override
Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scala...
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
mrpt::math::CMatrixDynamic< kftype > Y_pred_covs
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
size_t size() const
Returns the count of pairs (pose,sensory data)
Definition: CSimpleMap.cpp:53
double kftype
The numeric type used in the Kalman Filter (default=double)
void get(size_t index, mrpt::poses::CPose3DPDF::Ptr &out_posePDF, mrpt::obs::CSensoryFrame::Ptr &out_SF) const
Access to the i&#39;th pair, first one is index &#39;0&#39;.
Definition: CSimpleMap.cpp:56
CMatrixDouble cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...
Definition: ops_matrices.h:149
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
A class used to store a 3D point.
Definition: CPoint3D.h:31
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
mrpt::math::CMatrixFixed< double, FEAT_SIZE, VEH_SIZE > KFMatrix_FxV
mrpt::math::CVectorFixed< double, FEAT_SIZE > KFArray_FEAT
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:410
TDataAssocInfo m_last_data_association
Last data association.
#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...
CRangeBearingKFSLAM2D()
Default constructor.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void getCurrentRobotPose(mrpt::poses::CPosePDFGaussian &out_robotPose) const
Returns the mean & 3x3 covariance matrix of the robot 2D pose.
double data_assoc_IC_chi2_thres
Threshold in [0,1] for the chi2square test for individual compatibility between predictions and obser...
void OnGetObservationNoise(KFMatrix_OxO &out_R) const override
Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of ...
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
const float R
void OnGetObservationsAndDataAssociation(vector_KFArray_OBS &out_z, std::vector< int > &out_data_association, const vector_KFArray_OBS &in_all_predictions, const KFMatrix &in_S, const std::vector< size_t > &in_lm_indices_in_S, const KFMatrix_OxO &in_R) override
This is called between the KF prediction step and the update step, and the application must return th...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
static Ptr Create(Args &&... args)
Definition: CEllipsoid.h:46
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
Definition: exceptions.h:190
CSetOfObjects::Ptr CornerXYZ(float scale=1.0)
Returns three arrows representing a X,Y,Z 3D corner.
std::map< observation_index_t, prediction_index_t > associations
For each observation (with row index IDX_obs in the input "Z_observations"), its association in the p...
TDataAssociationMetric data_assoc_IC_metric
Whether to use mahalanobis (->chi2 criterion) vs.
constexpr double RAD2DEG(const double x)
Radians to degrees.
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
This observation represents a number of range-bearing value pairs, each one for a detected landmark...
Lightweight 2D pose.
Definition: TPose2D.h:22
double mean(const CONTAINER &v)
Computes the mean value of a vector.
mrpt::io::CFileOutputStream CFileOutputStream
void loadOptions(const mrpt::config::CConfigFileBase &ini)
Load options from a ini-like file/text.
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:257
void OnNewLandmarkAddedToMap(const size_t in_obsIdx, const size_t in_idxNewFeat) override
If applicable to the given problem, do here any special handling of adding a new landmark to the map...
std::string MATLAB_plotCovariance2D(const CMatrixFloat &cov22, const CVectorFloat &mean, float stdCount, const std::string &style=std::string("b"), size_t nEllipsePoints=30)
Generates a string with the MATLAB commands required to plot an confidence interval (ellipse) for a 2...
Definition: math.cpp:356
void resize(std::size_t N, bool zeroNewElements=false)
mrpt::math::CVectorFixed< double, OBS_SIZE > KFArray_OBS
void insert(const mrpt::poses::CPose3DPDF *in_posePDF, const mrpt::obs::CSensoryFrame &in_SF)
Add a new pair to the sequence.
Definition: CSimpleMap.cpp:138
void OnPreComputingPredictions(const vector_KFArray_OBS &in_all_prediction_means, std::vector< size_t > &out_LM_indices_to_predict) const override
This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the...
void rotateCov(const double ang)
Rotate the covariance matrix by replacing it by , where .
TDataAssociationMethod
Different algorithms for data association, used in mrpt::slam::data_association.
void OnTransitionModel(const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const override
Implements the transition model .
mrpt::math::CMatrixDouble22 cov
The 2x2 covariance matrix.
void saveMapAndPath2DRepresentationAsMATLABFile(const std::string &fil, float stdCount=3.0f, const std::string &styleLandmarks=std::string("b"), const std::string &stylePath=std::string("r"), const std::string &styleRobot=std::string("r")) const
Save the current state of the filter (robot pose & map) to a MATLAB script which displays all the ele...
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const
Returns a 3D representation of the landmarks in the map and the robot 3D position according to the cu...
void data_association_full_covariance(const mrpt::math::CMatrixDouble &Z_observations_mean, const mrpt::math::CMatrixDouble &Y_predictions_mean, const mrpt::math::CMatrixDouble &Y_predictions_cov, TDataAssociationResults &results, const TDataAssociationMethod method=assocJCBB, const TDataAssociationMetric metric=metricMaha, const double chi2quantile=0.99, const bool DAT_ASOC_USE_KDTREE=true, const std::vector< prediction_index_t > &predictions_IDs=std::vector< prediction_index_t >(), const TDataAssociationMetric compatibilityTestMetric=metricMaha, const double log_ML_compat_test_threshold=0.0)
Computes the data-association between the prediction of a set of landmarks and their observations...
void processActionObservation(mrpt::obs::CActionCollection::Ptr &action, mrpt::obs::CSensoryFrame::Ptr &SF)
Process one new action and observations to update the map and robot pose estimate.
mrpt::containers::bimap< mrpt::maps::CLandmark::TLandmarkID, unsigned int > m_IDs
The mapping between landmark IDs and indexes in the Pkk cov.
void clear()
Remove all stored pairs.
Definition: CSimpleMap.cpp:55
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
mrpt::math::CMatrixFixed< double, OBS_SIZE, OBS_SIZE > KFMatrix_OxO
void read_vector(const std::string &section, const std::string &name, const VECTOR_TYPE &defaultValue, VECTOR_TYPE &outValues, bool failIfNotFound=false) const
Reads a configuration parameter of type vector, stored in the file as a string: "[v1 v2 v3 ...
mrpt::obs::CSensoryFrame::Ptr m_SF
Set up by processActionObservation.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 338471620 Mon Feb 17 00:12:39 2020 +0100 at lun feb 17 00:15:10 CET 2020