MRPT  2.0.4
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-2020, 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>
20 #include <mrpt/poses/CPosePDF.h>
24 #include <mrpt/system/CTicTac.h>
25 #include <mrpt/system/os.h>
26 
27 using namespace mrpt::slam;
28 using namespace mrpt::maps;
29 using namespace mrpt::math;
30 using namespace mrpt::obs;
31 using namespace mrpt::poses;
32 using namespace mrpt::system;
33 using namespace mrpt;
34 using namespace std;
35 
36 #define STATS_EXPERIMENT 0
37 #define STATS_EXPERIMENT_ALSO_NC 1
38 
39 /*---------------------------------------------------------------
40  Constructor
41  ---------------------------------------------------------------*/
43  : options(), m_action(), m_SF(), m_IDs(), m_SFs()
44 {
45  reset();
46 }
47 
48 /*---------------------------------------------------------------
49  reset
50  ---------------------------------------------------------------*/
52 {
53  m_action.reset();
54  m_SF.reset();
55  m_IDs.clear();
56  m_SFs.clear();
57 
58  // INIT KF STATE
59  m_xkk.assign(3, 0); // State: 3D pose (x,y,phi)
60 
61  // Initial cov:
62  m_pkk.setSize(3, 3);
63  m_pkk.setZero();
64 }
65 
66 /*---------------------------------------------------------------
67  Destructor
68  ---------------------------------------------------------------*/
70 /*---------------------------------------------------------------
71  getCurrentRobotPose
72  ---------------------------------------------------------------*/
74  CPosePDFGaussian& out_robotPose) const
75 {
77 
78  ASSERT_(m_xkk.size() >= 3);
79 
80  // Set 6D pose mean:
81  out_robotPose.mean = CPose2D(m_xkk[0], m_xkk[1], m_xkk[2]);
82 
83  // and cov:
84  out_robotPose.cov = m_pkk.blockCopy<3, 3>(0, 0);
85 
86  MRPT_END
87 }
88 
89 /*---------------------------------------------------------------
90  getCurrentState
91  ---------------------------------------------------------------*/
93  CPosePDFGaussian& out_robotPose,
94  std::vector<TPoint2D>& out_landmarksPositions,
95  std::map<unsigned int, CLandmark::TLandmarkID>& out_landmarkIDs,
96  CVectorDouble& out_fullState, CMatrixDouble& out_fullCovariance) const
97 {
99 
100  ASSERT_(m_xkk.size() >= 3);
101 
102  // Set 6D pose mean:
103  out_robotPose.mean = CPose2D(m_xkk[0], m_xkk[1], m_xkk[2]);
104 
105  // and cov:
106  out_robotPose.cov = m_pkk.blockCopy<3, 3>(0, 0);
107 
108  // Landmarks:
109  ASSERT_(((m_xkk.size() - 3) % 2) == 0);
110  size_t i, nLMs = (m_xkk.size() - 3) / 2;
111  out_landmarksPositions.resize(nLMs);
112  for (i = 0; i < nLMs; i++)
113  {
114  out_landmarksPositions[i].x = m_xkk[3 + i * 2 + 0];
115  out_landmarksPositions[i].y = m_xkk[3 + i * 2 + 1];
116  } // end for i
117 
118  // IDs:
119  out_landmarkIDs = m_IDs.getInverseMap(); // m_IDs_inverse;
120 
121  // Full state:
122  out_fullState.resize(m_xkk.size());
123  std::copy(m_xkk.begin(), m_xkk.end(), out_fullState.begin());
124 
125  // Full cov:
126  out_fullCovariance = m_pkk;
127 
128  MRPT_END
129 }
130 
131 /*---------------------------------------------------------------
132  processActionObservation
133  ---------------------------------------------------------------*/
136 {
137  MRPT_START
138 
139  m_action = action;
140  m_SF = SF;
141 
142  // Sanity check:
144 
145  // ===================================================================================================================
146  // Here's the meat!: Call the main method for the KF algorithm, which will
147  // call all the callback methods as required:
148  // ===================================================================================================================
150 
151  // =============================================================
152  // ADD TO SFs SEQUENCE
153  // =============================================================
155  {
156  CPosePDFGaussian::Ptr auxPosePDF = std::make_shared<CPosePDFGaussian>();
157  getCurrentRobotPose(*auxPosePDF);
158  m_SFs.insert(auxPosePDF, SF);
159  }
160 
161  MRPT_END
162 }
163 
164 /** Must return the action vector u.
165  * \param out_u The action vector which will be passed to OnTransitionModel
166  */
167 void CRangeBearingKFSLAM2D::OnGetAction(KFArray_ACT& u) const
168 {
169  // Get odometry estimation:
170  CActionRobotMovement2D::Ptr actMov2D =
171  m_action->getBestMovementEstimation();
172  CActionRobotMovement3D::Ptr actMov3D =
173  m_action->getActionByClass<CActionRobotMovement3D>();
174  if (actMov3D)
175  {
176  u[0] = actMov3D->poseChange.mean.x();
177  u[1] = actMov3D->poseChange.mean.y();
178  u[2] = actMov3D->poseChange.mean.yaw();
179  }
180  else if (actMov2D)
181  {
182  CPose2D estMovMean;
183  actMov2D->poseChange->getMean(estMovMean);
184  u[0] = estMovMean.x();
185  u[1] = estMovMean.y();
186  u[2] = estMovMean.phi();
187  }
188  else
189  {
190  // Left u as zeros
191  for (size_t i = 0; i < 3; i++) u[i] = 0;
192  }
193 }
194 
195 /** This virtual function musts implement the prediction model of the Kalman
196  * filter.
197  */
199  const KFArray_ACT& u, KFArray_VEH& xv, bool& out_skipPrediction) const
200 {
201  MRPT_START
202 
203  // Do not update the vehicle pose & its covariance until we have some
204  // landmakrs in the map,
205  // otherwise, we are imposing a lower bound to the best uncertainty from now
206  // on:
207  if (size_t(m_xkk.size()) == get_vehicle_size())
208  {
209  out_skipPrediction = true;
210  return;
211  }
212 
213  CPose2D robotPose(xv[0], xv[1], xv[2]);
214  CPose2D odoIncrement(u[0], u[1], u[2]);
215 
216  // Pose composition:
217  robotPose = robotPose + odoIncrement;
218 
219  xv[0] = robotPose.x();
220  xv[1] = robotPose.y();
221  xv[2] = robotPose.phi();
222 
223  MRPT_END
224 }
225 
226 /** This virtual function musts calculate the Jacobian F of the prediction
227  * model.
228  */
230 {
231  MRPT_START
232 
233  // The jacobian of the transition function: dfv_dxv
234  CActionRobotMovement2D::Ptr act2D = m_action->getBestMovementEstimation();
236  m_action->getActionByClass<CActionRobotMovement3D>();
237 
238  if (act3D && act2D)
239  THROW_EXCEPTION("Both 2D & 3D odometry are present!?!?");
240 
241  TPoint2D Ap;
242 
243  if (act3D)
244  {
245  Ap = TPoint2D(CPose2D(act3D->poseChange.mean).asTPose());
246  }
247  else if (act2D)
248  {
249  Ap = TPoint2D(act2D->poseChange->getMeanVal().asTPose());
250  }
251  else
252  {
253  // No odometry at all:
254  F.setIdentity(); // Unit diagonal
255  return;
256  }
257 
258  const kftype cy = cos(m_xkk[2]);
259  const kftype sy = sin(m_xkk[2]);
260 
261  const kftype Ax = Ap.x;
262  const kftype Ay = Ap.y;
263 
264  F.setIdentity(); // Unit diagonal
265 
266  F(0, 2) = -Ax * sy - Ay * cy;
267  F(1, 2) = Ax * cy - Ay * sy;
268 
269  MRPT_END
270 }
271 
272 /** This virtual function musts calculate de noise matrix of the prediction
273  * model.
274  */
275 void CRangeBearingKFSLAM2D::OnTransitionNoise(KFMatrix_VxV& Q) const
276 {
277  MRPT_START
278 
279  // The uncertainty of the 2D odometry, projected from the current position:
280  CActionRobotMovement2D::Ptr act2D = m_action->getBestMovementEstimation();
282  m_action->getActionByClass<CActionRobotMovement3D>();
283 
284  if (act3D && act2D)
285  THROW_EXCEPTION("Both 2D & 3D odometry are present!?!?");
286 
287  CPosePDFGaussian odoIncr;
288 
289  if (!act3D && !act2D)
290  {
291  // Use constant Q:
292  Q.setZero();
293  ASSERT_(int(options.stds_Q_no_odo.size()) == Q.cols());
294  for (size_t i = 0; i < 3; i++)
295  Q(i, i) = square(options.stds_Q_no_odo[i]);
296  return;
297  }
298  else
299  {
300  if (act2D)
301  {
302  // 2D odometry:
303  odoIncr = CPosePDFGaussian(*act2D->poseChange);
304  }
305  else
306  {
307  // 3D odometry:
308  odoIncr = CPosePDFGaussian(act3D->poseChange);
309  }
310  }
311 
312  odoIncr.rotateCov(m_xkk[2]);
313 
314  Q = odoIncr.cov;
315 
316  MRPT_END
317 }
318 
320  const std::vector<size_t>& idx_landmarks_to_predict,
321  vector_KFArray_OBS& out_predictions) const
322 {
323  MRPT_START
324 
325  // Get the sensor pose relative to the robot:
327  m_SF->getObservationByClass<CObservationBearingRange>();
328  ASSERTMSG_(
329  obs,
330  "*ERROR*: This method requires an observation of type "
331  "CObservationBearingRange");
332  const CPose2D sensorPoseOnRobot = CPose2D(obs->sensorLocationOnRobot);
333 
334  /* -------------------------------------------
335  Equations, obtained using matlab, of the relative 2D position of a
336  landmark (xi,yi), relative
337  to a robot 2D pose (x0,y0,phi)
338  Refer to technical report "6D EKF derivation...", 2008
339 
340  x0 y0 phi0 % Robot's 2D pose
341  x0s y0s phis % Sensor's 2D pose relative to robot
342  xi yi % Absolute 2D landmark coordinates:
343 
344  Hx : dh_dxv -> Jacobian of the observation model wrt the robot pose
345  Hy : dh_dyi -> Jacobian of the observation model wrt each landmark
346  mean position
347 
348  Sizes:
349  h: 1x2
350  Hx: 2x3
351  Hy: 2x2
352  ------------------------------------------- */
353  // Mean of the prior of the robot pose:
354  const CPose2D robotPose(m_xkk[0], m_xkk[1], m_xkk[2]);
355 
356  const size_t vehicle_size = get_vehicle_size();
357  const size_t feature_size = get_feature_size();
358 
359  const CPose2D sensorPoseAbs = robotPose + sensorPoseOnRobot;
360 
361  // ---------------------------------------------------
362  // Observation prediction
363  // ---------------------------------------------------
364  const size_t N = idx_landmarks_to_predict.size();
365  out_predictions.resize(N);
366  for (size_t i = 0; i < N; i++)
367  {
368  const size_t idx_lm = idx_landmarks_to_predict[i];
369  ASSERTDEB_(idx_lm < this->getNumberOfLandmarksInTheMap());
370 
371  // Landmark absolute position in the map:
372  const kftype xi = m_xkk[vehicle_size + feature_size * idx_lm + 0];
373  const kftype yi = m_xkk[vehicle_size + feature_size * idx_lm + 1];
374 
375  const double Axi = xi - sensorPoseAbs.x();
376  const double Ayi = yi - sensorPoseAbs.y();
377 
378  out_predictions[i][0] = sqrt(square(Axi) + square(Ayi)); // Range
379  out_predictions[i][1] =
380  mrpt::math::wrapToPi(atan2(Ayi, Axi) - sensorPoseAbs.phi()); // Yaw
381  }
382 
383  MRPT_END
384 }
385 
387  size_t idx_landmark_to_predict, KFMatrix_OxV& Hx, KFMatrix_OxF& Hy) const
388 {
389  MRPT_START
390 
391  // Get the sensor pose relative to the robot:
393  m_SF->getObservationByClass<CObservationBearingRange>();
394  ASSERTMSG_(
395  obs,
396  "*ERROR*: This method requires an observation of type "
397  "CObservationBearingRange");
398  const CPose2D sensorPoseOnRobot = CPose2D(obs->sensorLocationOnRobot);
399 
400  /* -------------------------------------------
401  Equations, obtained using matlab, of the relative 2D position of a
402  landmark (xi,yi), relative
403  to a robot 2D pose (x0,y0,phi)
404  Refer to technical report "6D EKF derivation...", 2008
405 
406  x0 y0 phi0 % Robot's 2D pose
407  x0s y0s phis % Sensor's 2D pose relative to robot
408  xi yi % Absolute 2D landmark coordinates:
409 
410  Hx : dh_dxv -> Jacobian of the observation model wrt the robot pose
411  Hy : dh_dyi -> Jacobian of the observation model wrt each landmark
412  mean position
413 
414  Sizes:
415  h: 1x2
416  Hx: 2x3
417  Hy: 2x2
418  ------------------------------------------- */
419  // Mean of the prior of the robot pose:
420  const CPose2D robotPose(m_xkk[0], m_xkk[1], m_xkk[2]);
421 
422  const size_t vehicle_size = get_vehicle_size();
423  const size_t feature_size = get_feature_size();
424 
425  // Robot 6D pose:
426  const kftype x0 = m_xkk[0];
427  const kftype y0 = m_xkk[1];
428  const kftype phi0 = m_xkk[2];
429 
430  const kftype cphi0 = cos(phi0);
431  const kftype sphi0 = sin(phi0);
432 
433  // Sensor 2D pose on robot:
434  const kftype x0s = sensorPoseOnRobot.x();
435  const kftype y0s = sensorPoseOnRobot.y();
436  const kftype phis = sensorPoseOnRobot.phi();
437 
438  const kftype cphi0s = cos(phi0 + phis);
439  const kftype sphi0s = sin(phi0 + phis);
440 
441  const CPose2D sensorPoseAbs = robotPose + sensorPoseOnRobot;
442 
443  // Landmark absolute position in the map:
444  const kftype xi =
445  m_xkk[vehicle_size + feature_size * idx_landmark_to_predict + 0];
446  const kftype yi =
447  m_xkk[vehicle_size + feature_size * idx_landmark_to_predict + 1];
448 
449  // ---------------------------------------------------
450  // Generate dhi_dxv: A 2x3 block
451  // ---------------------------------------------------
452  const kftype EXP1 =
453  -2 * yi * y0s * cphi0 - 2 * yi * y0 + 2 * xi * y0s * sphi0 -
454  2 * xi * x0 - 2 * xi * x0s * cphi0 - 2 * yi * x0s * sphi0 +
455  2 * y0s * y0 * cphi0 - 2 * y0s * x0 * sphi0 + 2 * y0 * x0s * sphi0 +
456  square(x0) + 2 * x0s * x0 * cphi0 + square(x0s) + square(y0s) +
457  square(xi) + square(yi) + square(y0);
458  const kftype sqrtEXP1_1 = kftype(1) / sqrt(EXP1);
459 
460  const kftype EXP2 = cphi0s * xi + sphi0s * yi - sin(phis) * y0s -
461  y0 * sphi0s - x0s * cos(phis) - x0 * cphi0s;
462  const kftype EXP2sq = square(EXP2);
463 
464  const kftype EXP3 = -sphi0s * xi + cphi0s * yi - cos(phis) * y0s -
465  y0 * cphi0s + x0s * sin(phis) + x0 * sphi0s;
466  const kftype EXP3sq = square(EXP3);
467 
468  const kftype EXP4 = kftype(1) / (1 + EXP3sq / EXP2sq);
469 
470  Hx(0, 0) = (-xi - sphi0 * y0s + cphi0 * x0s + x0) * sqrtEXP1_1;
471  Hx(0, 1) = (-yi + cphi0 * y0s + y0 + sphi0 * x0s) * sqrtEXP1_1;
472  Hx(0, 2) = (y0s * xi * cphi0 + y0s * yi * sphi0 - y0 * y0s * sphi0 -
473  x0 * y0s * cphi0 + x0s * xi * sphi0 - x0s * yi * cphi0 +
474  y0 * x0s * cphi0 - x0s * x0 * sphi0) *
475  sqrtEXP1_1;
476 
477  Hx(1, 0) = (sphi0s / (EXP2) + (EXP3) / EXP2sq * cphi0s) * EXP4;
478  Hx(1, 1) = (-cphi0s / (EXP2) + (EXP3) / EXP2sq * sphi0s) * EXP4;
479  Hx(1, 2) =
480  ((-cphi0s * xi - sphi0s * yi + y0 * sphi0s + x0 * cphi0s) / (EXP2) -
481  (EXP3) / EXP2sq *
482  (-sphi0s * xi + cphi0s * yi - y0 * cphi0s + x0 * sphi0s)) *
483  EXP4;
484 
485  // ---------------------------------------------------
486  // Generate dhi_dyi: A 2x2 block
487  // ---------------------------------------------------
488  Hy(0, 0) = (xi + sphi0 * y0s - cphi0 * x0s - x0) * sqrtEXP1_1;
489  Hy(0, 1) = (yi - cphi0 * y0s - y0 - sphi0 * x0s) * sqrtEXP1_1;
490 
491  Hy(1, 0) = (-sphi0s / (EXP2) - (EXP3) / EXP2sq * cphi0s) * EXP4;
492  Hy(1, 1) = (cphi0s / (EXP2) - (EXP3) / EXP2sq * sphi0s) * EXP4;
493 
494  MRPT_END
495 }
496 
497 /** This is called between the KF prediction step and the update step, and the
498  * application must return the observations and, when applicable, the data
499  * association between these observations and the current map.
500  *
501  * \param out_z N vectors, each for one "observation" of length OBS_SIZE, N
502  * being the number of "observations": how many observed landmarks for a map, or
503  * just one if not applicable.
504  * \param out_data_association An empty vector or, where applicable, a vector
505  * where the i'th element corresponds to the position of the observation in the
506  * i'th row of out_z within the system state vector (in the range
507  * [0,getNumberOfLandmarksInTheMap()-1]), or -1 if it is a new map element and
508  * we want to insert it at the end of this KF iteration.
509  * \param in_S The full covariance matrix of the observation predictions (i.e.
510  * the "innovation covariance matrix"). This is a M·O x M·O matrix with M=length
511  * of "in_lm_indices_in_S".
512  * \param in_lm_indices_in_S The indices of the map landmarks (range
513  * [0,getNumberOfLandmarksInTheMap()-1]) that can be found in the matrix in_S.
514  *
515  * This method will be called just once for each complete KF iteration.
516  * \note It is assumed that the observations are independent, i.e. there are NO
517  * cross-covariances between them.
518  */
520  vector_KFArray_OBS& Z, std::vector<int>& data_association,
521  const vector_KFArray_OBS& all_predictions, const KFMatrix& S,
522  const std::vector<size_t>& lm_indices_in_S, const KFMatrix_OxO& R)
523 {
524  MRPT_START
525 
526  constexpr size_t obs_size = get_observation_size();
527 
528  // Z: Observations
529  CObservationBearingRange::TMeasurementList::const_iterator itObs;
530 
532  m_SF->getObservationByClass<CObservationBearingRange>();
533  ASSERTMSG_(
534  obs,
535  "*ERROR*: This method requires an observation of type "
536  "CObservationBearingRange");
537 
538  const size_t N = obs->sensedData.size();
539  Z.resize(N);
540 
541  size_t row;
542  for (row = 0, itObs = obs->sensedData.begin();
543  itObs != obs->sensedData.end(); ++itObs, ++row)
544  {
545  // Fill one row in Z:
546  Z[row][0] = itObs->range;
547  Z[row][1] = itObs->yaw;
548  ASSERTMSG_(
549  itObs->pitch == 0,
550  "ERROR: Observation contains pitch!=0 but this is 2D-SLAM!!!");
551  }
552 
553  // Data association:
554  // ---------------------
555  data_association.assign(N, -1); // Initially, all new landmarks
556 
557  // For each observed LM:
558  std::vector<size_t> obs_idxs_needing_data_assoc;
559  obs_idxs_needing_data_assoc.reserve(N);
560 
561  {
562  std::vector<int>::iterator itDA;
563  for (row = 0, itObs = obs->sensedData.begin(),
564  itDA = data_association.begin();
565  itObs != obs->sensedData.end(); ++itObs, ++itDA, ++row)
566  {
567  // Fill data asociation: Using IDs!
568  if (itObs->landmarkID < 0)
569  obs_idxs_needing_data_assoc.push_back(row);
570  else
571  {
573  CLandmark::TLandmarkID, unsigned int>::iterator itID;
574  if ((itID = m_IDs.find_key(itObs->landmarkID)) != m_IDs.end())
575  *itDA = itID->second; // This row in Z corresponds to the
576  // i'th map element in the state
577  // vector:
578  }
579  }
580  }
581 
582  // ---- Perform data association ----
583  // Only for observation indices in "obs_idxs_needing_data_assoc"
584  if (obs_idxs_needing_data_assoc.empty())
585  {
586  // We don't need to do DA:
588 
589  // Save them for the case the external user wants to access them:
590  for (size_t idxObs = 0; idxObs < data_association.size(); idxObs++)
591  {
592  int da = data_association[idxObs];
593  if (da >= 0)
595  data_association[idxObs];
596  }
597 
598 #if STATS_EXPERIMENT // DEBUG: Generate statistic info
599  {
600  static CFileOutputStream fC("metric_stats_C.txt", true);
601 #if STATS_EXPERIMENT_ALSO_NC
602  static CFileOutputStream fNC("metric_stats_NC.txt", true);
603 #endif
604 
605  std::vector<int>::iterator itDA;
606  size_t idx_obs = 0;
607  for (itDA = data_association.begin();
608  itDA != data_association.end(); ++itDA, ++idx_obs)
609  {
610  if (*itDA == -1) continue;
611  const size_t lm_idx_in_map = *itDA;
612  size_t valid_idx_pred =
613  find_in_vector(lm_idx_in_map, lm_indices_in_S);
614 
615  const KFArray_OBS& obs_mu = Z[idx_obs];
616 
617  for (size_t idx_pred = 0; idx_pred < lm_indices_in_S.size();
618  idx_pred++)
619  {
620  const KFArray_OBS& lm_mu =
621  all_predictions[lm_indices_in_S[idx_pred]];
622 
623  const size_t base_idx_in_S = obs_size * idx_pred;
624  KFMatrix_OxO lm_cov;
625  S.extractMatrix(base_idx_in_S, base_idx_in_S, lm_cov);
626 
627  double md, log_pdf;
629  lm_mu - obs_mu, lm_cov, md, log_pdf);
630 
631  if (valid_idx_pred == idx_pred)
632  fC.printf("%e %e\n", md, log_pdf);
633  else
634  {
635 #if STATS_EXPERIMENT_ALSO_NC
636  fNC.printf("%e %e\n", md, log_pdf);
637 #endif
638  }
639  }
640  }
641  }
642 #endif
643  }
644  else
645  {
646  // Build a Z matrix with the observations that need dat.assoc:
647  const size_t nObsDA = obs_idxs_needing_data_assoc.size();
648 
649  CMatrixDynamic<kftype> Z_obs_means(nObsDA, obs_size);
650  for (size_t i = 0; i < nObsDA; i++)
651  {
652  const size_t idx = obs_idxs_needing_data_assoc[i];
653  for (unsigned k = 0; k < obs_size; k++)
654  Z_obs_means(i, k) = Z[idx][k];
655  }
656 
657  // Vehicle uncertainty
658  /*const KFMatrix_VxV Pxx = m_pkk.extractMatrix<get_vehicle_size(),
659  * get_vehicle_size()>(0, 0);*/
660 
661  // Build predictions:
662  // ---------------------------
663  const size_t nPredictions = lm_indices_in_S.size();
665 
666  // S is the covariance of the predictions:
668 
669  // The means:
670  m_last_data_association.Y_pred_means.setSize(nPredictions, obs_size);
671  for (size_t q = 0; q < nPredictions; q++)
672  {
673  const size_t i = lm_indices_in_S[q];
674  for (size_t w = 0; w < obs_size; w++)
676  all_predictions[i][w];
678  i); // for the conversion of indices...
679  }
680 
681  // Do Dat. Assoc :
682  // ---------------------------
683  if (nPredictions)
684  {
685  // CMatrixDouble Z_obs_cov = CMatrixDouble(R);
686 
688  Z_obs_means, // Z_obs_cov,
693  true, // Use KD-tree
697 
698  // Return pairings to the main KF algorithm:
699  for (auto it = m_last_data_association.results.associations.begin();
700  it != m_last_data_association.results.associations.end(); ++it)
701  data_association[it->first] = it->second;
702  }
703  }
704  // ---- End of data association ----
705 
706  MRPT_END
707 }
708 
709 /** This virtual function musts normalize the state vector and covariance matrix
710  * (only if its necessary).
711  */
713 {
714  // Check angles:
716 }
717 
718 /*---------------------------------------------------------------
719  loadOptions
720  ---------------------------------------------------------------*/
723 {
724  // Main
725  options.loadFromConfigFile(ini, "RangeBearingKFSLAM");
726  KF_options.loadFromConfigFile(ini, "RangeBearingKFSLAM_KalmanFilter");
727 }
728 
729 /*---------------------------------------------------------------
730  loadOptions
731  ---------------------------------------------------------------*/
733  const mrpt::config::CConfigFileBase& source, const std::string& section)
734 {
736 
737  source.read_vector(section, "stds_Q_no_odo", stds_Q_no_odo, stds_Q_no_odo);
738  ASSERT_(stds_Q_no_odo.size() == 3);
740 
742  source.read_float(section, "std_sensor_range", std_sensor_range);
744  section, "std_sensor_yaw_deg", RAD2DEG(std_sensor_yaw)));
745 
746  MRPT_LOAD_CONFIG_VAR(quantiles_3D_representation, float, source, section);
747  MRPT_LOAD_CONFIG_VAR(create_simplemap, bool, source, section);
748 
750  section, "data_assoc_method", data_assoc_method);
752  section, "data_assoc_metric", data_assoc_metric);
754  section, "data_assoc_IC_metric", data_assoc_IC_metric);
755 
756  MRPT_LOAD_CONFIG_VAR(data_assoc_IC_chi2_thres, double, source, section);
757  MRPT_LOAD_CONFIG_VAR(data_assoc_IC_ml_threshold, double, source, section);
758 }
759 
760 /*---------------------------------------------------------------
761  Constructor
762  ---------------------------------------------------------------*/
764  : stds_Q_no_odo(3, 0),
765 
766  std_sensor_yaw(DEG2RAD(0.5f))
767 
768 {
769  stds_Q_no_odo[0] = 0.10f;
770  stds_Q_no_odo[1] = 0.10f;
771  stds_Q_no_odo[2] = DEG2RAD(4.0f);
772 }
773 
774 /*---------------------------------------------------------------
775  dumpToTextStream
776  ---------------------------------------------------------------*/
778 {
779  using namespace mrpt::typemeta;
780 
781  out << "\n----------- [CRangeBearingKFSLAM2D::TOptions] ------------ \n\n";
782 
783  out << mrpt::format(
784  "data_assoc_method = %s\n",
786  .c_str());
787  out << mrpt::format(
788  "data_assoc_metric = %s\n",
790  .c_str());
791  out << mrpt::format(
792  "data_assoc_IC_chi2_thres = %.06f\n",
793  data_assoc_IC_chi2_thres);
794  out << mrpt::format(
795  "data_assoc_IC_metric = %s\n",
797  .c_str());
798  out << mrpt::format(
799  "data_assoc_IC_ml_threshold = %.06f\n",
800  data_assoc_IC_ml_threshold);
801 
802  out << "\n";
803 }
804 
806  const KFArray_OBS& in_z, KFArray_FEAT& yn, KFMatrix_FxV& dyn_dxv,
807  KFMatrix_FxO& dyn_dhn) const
808 {
809  MRPT_START
810 
811  /* -------------------------------------------
812  Equations, obtained using matlab
813 
814  x0 y0 phi0 % Robot's 2D pose
815  x0s y0s phis % Sensor's 2D pose relative to robot
816  hr ha % Observation hn: range, yaw
817 
818  xi yi % Absolute 2D landmark coordinates:
819 
820  dyn_dxv -> Jacobian of the inv. observation model wrt the robot pose
821  dyn_dhn -> Jacobian of the inv. observation model wrt each landmark
822  observation
823 
824  Sizes:
825  hn: 1x2 <--
826  yn: 1x2 -->
827  dyn_dxv: 2x3 -->
828  dyn_dhn: 2x2 -->
829  ------------------------------------------- */
830 
832  m_SF->getObservationByClass<CObservationBearingRange>();
833  ASSERTMSG_(
834  obs,
835  "*ERROR*: This method requires an observation of type "
836  "CObservationBearingRange");
837  const CPose2D sensorPoseOnRobot = CPose2D(obs->sensorLocationOnRobot);
838 
839  // Mean of the prior of the robot pose:
840  const TPose2D robotPose(m_xkk[0], m_xkk[1], m_xkk[2]);
841 
842  // Robot 2D pose:
843  const kftype x0 = m_xkk[0];
844  const kftype y0 = m_xkk[1];
845  const kftype phi0 = m_xkk[2];
846 
847  const kftype cphi0 = cos(phi0);
848  const kftype sphi0 = sin(phi0);
849 
850  // Sensor 6D pose on robot:
851  const kftype x0s = sensorPoseOnRobot.x();
852  const kftype y0s = sensorPoseOnRobot.y();
853  const kftype phis = sensorPoseOnRobot.phi();
854 
855  const kftype hr = in_z[0];
856  const kftype ha = in_z[1];
857 
858  const kftype cphi_0sa = cos(phi0 + phis + ha);
859  const kftype sphi_0sa = sin(phi0 + phis + ha);
860 
861  // Compute the mean 2D absolute coordinates:
862  yn[0] = hr * cphi_0sa + cphi0 * x0s - sphi0 * y0s + x0;
863  yn[1] = hr * sphi_0sa + sphi0 * x0s + cphi0 * y0s + y0;
864 
865  // Jacobian wrt xv:
866  dyn_dxv(0, 0) = 1;
867  dyn_dxv(0, 1) = 0;
868  dyn_dxv(0, 2) = -hr * sphi_0sa - sphi0 * x0s - cphi0 * y0s;
869 
870  dyn_dxv(1, 0) = 0;
871  dyn_dxv(1, 1) = 1;
872  dyn_dxv(1, 2) = hr * cphi_0sa + cphi0 * x0s - sphi0 * y0s;
873 
874  // Jacobian wrt hn:
875  dyn_dhn(0, 0) = cphi_0sa;
876  dyn_dhn(0, 1) = -hr * sphi_0sa;
877 
878  dyn_dhn(1, 0) = sphi_0sa;
879  dyn_dhn(1, 1) = hr * cphi_0sa;
880 
881  MRPT_END
882 }
883 
884 /** If applicable to the given problem, do here any special handling of adding a
885  * new landmark to the map.
886  * \param in_obsIndex The index of the observation whose inverse sensor is to
887  * be computed. It corresponds to the row in in_z where the observation can be
888  * found.
889  * \param in_idxNewFeat The index that this new feature will have in the state
890  * vector (0:just after the vehicle state, 1: after that,...). Save this number
891  * so data association can be done according to these indices.
892  * \sa OnInverseObservationModel
893  */
895  const size_t in_obsIdx, const size_t in_idxNewFeat)
896 {
897  MRPT_START
898 
900  m_SF->getObservationByClass<CObservationBearingRange>();
901  ASSERTMSG_(
902  obs,
903  "*ERROR*: This method requires an observation of type "
904  "CObservationBearingRange");
905 
906  // ----------------------------------------------
907  // introduce in the lists of ID<->index in map:
908  // ----------------------------------------------
909  ASSERT_(in_obsIdx < obs->sensedData.size());
910  if (obs->sensedData[in_obsIdx].landmarkID >= 0)
911  {
912  // The sensor provides us a LM ID... use it:
913  m_IDs.insert(obs->sensedData[in_obsIdx].landmarkID, in_idxNewFeat);
914  }
915  else
916  {
917  // Features do not have IDs... use indices:
918  m_IDs.insert(in_idxNewFeat, in_idxNewFeat);
919  }
920 
922  in_idxNewFeat; // Just for stats, etc...
923 
924  MRPT_END
925 }
926 
927 /*---------------------------------------------------------------
928  getAs3DObject
929  ---------------------------------------------------------------*/
931  mrpt::opengl::CSetOfObjects::Ptr& outObj) const
932 {
933  outObj->clear();
934 
935  // ------------------------------------------------
936  // Add the XYZ corner for the current area:
937  // ------------------------------------------------
938  outObj->insert(opengl::stock_objects::CornerXYZ());
939 
940  // 2D ellipsoid for robot pose:
941  CPoint2DPDFGaussian pointGauss;
942  pointGauss.mean.x(m_xkk[0]);
943  pointGauss.mean.y(m_xkk[1]);
944  pointGauss.cov = m_pkk.blockCopy<2, 2>(0, 0);
945 
946  {
947  auto ellip = opengl::CEllipsoid2D::Create();
948 
949  ellip->setPose(pointGauss.mean);
950  ellip->setCovMatrix(pointGauss.cov);
951  ellip->enableDrawSolid3D(false);
952  ellip->setQuantiles(options.quantiles_3D_representation);
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::CEllipsoid2D::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 
975  ellip->setColor(0, 0, 1);
976 
977  outObj->insert(ellip);
978  }
979 }
980 
981 /*---------------------------------------------------------------
982  saveMapAndPathRepresentationAsMATLABFile
983  ---------------------------------------------------------------*/
985  const string& fil, float stdCount, const string& styleLandmarks,
986  const string& stylePath, const string& styleRobot) const
987 {
988  FILE* f = os::fopen(fil.c_str(), "wt");
989  if (!f) return;
990 
991  CMatrixDouble cov(2, 2);
992  CVectorDouble mean(2);
993 
994  // Header:
995  os::fprintf(
996  f,
997  "%%--------------------------------------------------------------------"
998  "\n");
999  os::fprintf(f, "%% File automatically generated using the MRPT method:\n");
1000  os::fprintf(
1001  f,
1002  "%% "
1003  "'CRangeBearingKFSLAM2D::saveMapAndPath2DRepresentationAsMATLABFile'"
1004  "\n");
1005  os::fprintf(f, "%%\n");
1006  os::fprintf(f, "%% ~ MRPT ~\n");
1007  os::fprintf(
1008  f, "%% Jose Luis Blanco Claraco, University of Malaga @ 2008\n");
1009  os::fprintf(f, "%% https://www.mrpt.org/ \n");
1010  os::fprintf(
1011  f,
1012  "%%--------------------------------------------------------------------"
1013  "\n");
1014 
1015  // Main code:
1016  os::fprintf(f, "hold on;\n\n");
1017 
1018  size_t i, nLMs = (m_xkk.size() - get_vehicle_size()) / get_feature_size();
1019 
1020  for (i = 0; i < nLMs; i++)
1021  {
1022  size_t idx = get_vehicle_size() + i * get_feature_size();
1023 
1024  cov(0, 0) = m_pkk(idx + 0, idx + 0);
1025  cov(1, 1) = m_pkk(idx + 1, idx + 1);
1026  cov(0, 1) = cov(1, 0) = m_pkk(idx + 0, idx + 1);
1027 
1028  mean[0] = m_xkk[idx + 0];
1029  mean[1] = m_xkk[idx + 1];
1030 
1031  // Command to draw the 2D ellipse:
1032  os::fprintf(
1033  f, "%s",
1034  math::MATLAB_plotCovariance2D(cov, mean, stdCount, styleLandmarks)
1035  .c_str());
1036  }
1037 
1038  // Now: the robot path:
1039  // ------------------------------
1040  if (m_SFs.size())
1041  {
1042  os::fprintf(f, "\nROB_PATH=[");
1043  for (i = 0; i < m_SFs.size(); i++)
1044  {
1045  CSensoryFrame::Ptr dummySF;
1046  CPose3DPDF::Ptr pdf3D;
1047  m_SFs.get(i, pdf3D, dummySF);
1048 
1049  CPose3D p;
1050  pdf3D->getMean(p);
1051  CPoint3D pnt3D(p); // 6D -> 3D only
1052 
1053  os::fprintf(f, "%.04f %.04f", pnt3D.x(), pnt3D.y());
1054  if (i < (m_SFs.size() - 1)) os::fprintf(f, ";");
1055  }
1056  os::fprintf(f, "];\n");
1057 
1058  os::fprintf(
1059  f, "plot(ROB_PATH(:,1),ROB_PATH(:,2),'%s');\n", stylePath.c_str());
1060  }
1061 
1062  // The robot pose:
1063  cov(0, 0) = m_pkk(0, 0);
1064  cov(1, 1) = m_pkk(1, 1);
1065  cov(0, 1) = cov(1, 0) = m_pkk(0, 1);
1066 
1067  mean[0] = m_xkk[0];
1068  mean[1] = m_xkk[1];
1069 
1070  os::fprintf(
1071  f, "%s",
1072  math::MATLAB_plotCovariance2D(cov, mean, stdCount, styleRobot).c_str());
1073 
1074  os::fprintf(f, "\naxis equal;\n");
1075  os::fclose(f);
1076 }
1077 
1078 /** Computes A=A-B, which may need to be re-implemented depending on the
1079  * topology of the individual scalar components (eg, angles).
1080  */
1082  KFArray_OBS& A, const KFArray_OBS& B) const
1083 {
1084  A[0] -= B[0];
1085  A[1] -= B[1];
1087 }
1088 
1089 /** Return the observation NOISE covariance matrix, that is, the model of the
1090  * Gaussian additive noise of the sensor.
1091  * \param out_R The noise covariance matrix. It might be non diagonal, but
1092  * it'll usually be.
1093  */
1095 {
1096  out_R(0, 0) = square(options.std_sensor_range);
1097  out_R(1, 1) = square(options.std_sensor_yaw);
1098 }
1099 
1100 /** This will be called before OnGetObservationsAndDataAssociation to allow the
1101  * application to reduce the number of covariance landmark predictions to be
1102  * made.
1103  * For example, features which are known to be "out of sight" shouldn't be
1104  * added to the output list to speed up the calculations.
1105  * \param in_all_prediction_means The mean of each landmark predictions; the
1106  * computation or not of the corresponding covariances is what we're trying to
1107  * determined with this method.
1108  * \param out_LM_indices_to_predict The list of landmark indices in the map
1109  * [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.
1110  * \note This is not a pure virtual method, so it should be implemented only if
1111  * desired. The default implementation returns a vector with all the landmarks
1112  * in the map.
1113  * \sa OnGetObservations, OnDataAssociation
1114  */
1116  const vector_KFArray_OBS& prediction_means,
1117  std::vector<size_t>& out_LM_indices_to_predict) const
1118 {
1120  m_SF->getObservationByClass<CObservationBearingRange>();
1121  ASSERTMSG_(
1122  obs,
1123  "*ERROR*: This method requires an observation of type "
1124  "CObservationBearingRange");
1125 
1126  const double sensor_max_range = obs->maxSensorDistance;
1127  const double fov_yaw = obs->fieldOfView_yaw;
1128 
1129  const double max_vehicle_loc_uncertainty =
1130  4 * std::sqrt(m_pkk(0, 0) + m_pkk(1, 1));
1131  const double max_vehicle_ang_uncertainty = 4 * std::sqrt(m_pkk(2, 2));
1132 
1133  out_LM_indices_to_predict.clear();
1134  for (size_t i = 0; i < prediction_means.size(); i++)
1135 #if (!STATS_EXPERIMENT) // In the experiment we force errors too far and some
1136  // predictions are out of range, so just generate all
1137  // of them:
1138  if (prediction_means[i][0] <
1139  (1.5 + sensor_max_range + max_vehicle_loc_uncertainty +
1140  4 * options.std_sensor_range) &&
1141  fabs(prediction_means[i][1]) <
1142  (20.0_deg + 0.5 * fov_yaw + max_vehicle_ang_uncertainty +
1143  4 * options.std_sensor_yaw))
1144 #endif
1145  {
1146  out_LM_indices_to_predict.push_back(i);
1147  }
1148 }
1149 
1150 /** Only called if using a numeric approximation of the transition Jacobian,
1151  * this method must return the increments in each dimension of the vehicle state
1152  * vector while estimating the Jacobian.
1153  */
1155  KFArray_VEH& out_increments) const
1156 {
1157  for (size_t i = 0; i < get_vehicle_size(); i++) out_increments[i] = 1e-6;
1158 }
1159 
1160 /** Only called if using a numeric approximation of the observation Jacobians,
1161  * this method must return the increments in each dimension of the vehicle state
1162  * vector while estimating the Jacobian.
1163  */
1165  KFArray_VEH& out_veh_increments, KFArray_FEAT& out_feat_increments) const
1166 {
1167  for (size_t i = 0; i < get_vehicle_size(); i++)
1168  out_veh_increments[i] = 1e-6;
1169  for (size_t i = 0; i < get_feature_size(); i++)
1170  out_feat_increments[i] = 1e-6;
1171 }
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:213
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
T x
X,Y coordinates.
Definition: TPoint2D.h:25
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:282
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.
static Ptr Create(Args &&... args)
Definition: CEllipsoid2D.h:43
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
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:415
TDataAssocInfo m_last_data_association
Last data association.
return_t square(const num_t x)
Inline function for the square of a number.
#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
#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:264
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 2.0.4 Git: c4d914402 Wed Jun 3 12:47:17 2020 +0200 at mié jun 3 13:00:13 CEST 2020