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  const size_t& idx_landmark_to_predict, KFMatrix_OxV& Hx,
387  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);
743  std_sensor_yaw = DEG2RAD(source.read_float(
744  section, "std_sensor_yaw_deg", RAD2DEG(std_sensor_yaw)));
745 
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 
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::CEllipsoid::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->set3DsegmentsCount(10);
954  ellip->setColor(1, 0, 0);
955 
956  outObj->insert(ellip);
957  }
958 
959  // 2D ellipsoids for landmarks:
960  const size_t nLMs = (m_xkk.size() - 3) / 2;
961  for (size_t i = 0; i < nLMs; i++)
962  {
963  pointGauss.mean.x(m_xkk[3 + 2 * i + 0]);
964  pointGauss.mean.y(m_xkk[3 + 2 * i + 1]);
965  pointGauss.cov = m_pkk.blockCopy<2, 2>(3 + 2 * i, 3 + 2 * i);
966 
967  auto ellip = opengl::CEllipsoid::Create();
968 
969  ellip->setName(format("%u", static_cast<unsigned int>(i)));
970  ellip->enableShowName(true);
971  ellip->setPose(pointGauss.mean);
972  ellip->setCovMatrix(pointGauss.cov);
973  ellip->enableDrawSolid3D(false);
974  ellip->setQuantiles(options.quantiles_3D_representation);
975  ellip->set3DsegmentsCount(10);
976 
977  ellip->setColor(0, 0, 1);
978 
979  outObj->insert(ellip);
980  }
981 }
982 
983 /*---------------------------------------------------------------
984  saveMapAndPathRepresentationAsMATLABFile
985  ---------------------------------------------------------------*/
987  const string& fil, float stdCount, const string& styleLandmarks,
988  const string& stylePath, const string& styleRobot) const
989 {
990  FILE* f = os::fopen(fil.c_str(), "wt");
991  if (!f) return;
992 
993  CMatrixDouble cov(2, 2);
994  CVectorDouble mean(2);
995 
996  // Header:
997  os::fprintf(
998  f,
999  "%%--------------------------------------------------------------------"
1000  "\n");
1001  os::fprintf(f, "%% File automatically generated using the MRPT method:\n");
1002  os::fprintf(
1003  f,
1004  "%% "
1005  "'CRangeBearingKFSLAM2D::saveMapAndPath2DRepresentationAsMATLABFile'"
1006  "\n");
1007  os::fprintf(f, "%%\n");
1008  os::fprintf(f, "%% ~ MRPT ~\n");
1009  os::fprintf(
1010  f, "%% Jose Luis Blanco Claraco, University of Malaga @ 2008\n");
1011  os::fprintf(f, "%% https://www.mrpt.org/ \n");
1012  os::fprintf(
1013  f,
1014  "%%--------------------------------------------------------------------"
1015  "\n");
1016 
1017  // Main code:
1018  os::fprintf(f, "hold on;\n\n");
1019 
1020  size_t i, nLMs = (m_xkk.size() - get_vehicle_size()) / get_feature_size();
1021 
1022  for (i = 0; i < nLMs; i++)
1023  {
1024  size_t idx = get_vehicle_size() + i * get_feature_size();
1025 
1026  cov(0, 0) = m_pkk(idx + 0, idx + 0);
1027  cov(1, 1) = m_pkk(idx + 1, idx + 1);
1028  cov(0, 1) = cov(1, 0) = m_pkk(idx + 0, idx + 1);
1029 
1030  mean[0] = m_xkk[idx + 0];
1031  mean[1] = m_xkk[idx + 1];
1032 
1033  // Command to draw the 2D ellipse:
1034  os::fprintf(
1035  f, "%s",
1036  math::MATLAB_plotCovariance2D(cov, mean, stdCount, styleLandmarks)
1037  .c_str());
1038  }
1039 
1040  // Now: the robot path:
1041  // ------------------------------
1042  if (m_SFs.size())
1043  {
1044  os::fprintf(f, "\nROB_PATH=[");
1045  for (i = 0; i < m_SFs.size(); i++)
1046  {
1047  CSensoryFrame::Ptr dummySF;
1048  CPose3DPDF::Ptr pdf3D;
1049  m_SFs.get(i, pdf3D, dummySF);
1050 
1051  CPose3D p;
1052  pdf3D->getMean(p);
1053  CPoint3D pnt3D(p); // 6D -> 3D only
1054 
1055  os::fprintf(f, "%.04f %.04f", pnt3D.x(), pnt3D.y());
1056  if (i < (m_SFs.size() - 1)) os::fprintf(f, ";");
1057  }
1058  os::fprintf(f, "];\n");
1059 
1060  os::fprintf(
1061  f, "plot(ROB_PATH(:,1),ROB_PATH(:,2),'%s');\n", stylePath.c_str());
1062  }
1063 
1064  // The robot pose:
1065  cov(0, 0) = m_pkk(0, 0);
1066  cov(1, 1) = m_pkk(1, 1);
1067  cov(0, 1) = cov(1, 0) = m_pkk(0, 1);
1068 
1069  mean[0] = m_xkk[0];
1070  mean[1] = m_xkk[1];
1071 
1072  os::fprintf(
1073  f, "%s",
1074  math::MATLAB_plotCovariance2D(cov, mean, stdCount, styleRobot).c_str());
1075 
1076  os::fprintf(f, "\naxis equal;\n");
1077  os::fclose(f);
1078 }
1079 
1080 /** Computes A=A-B, which may need to be re-implemented depending on the
1081  * topology of the individual scalar components (eg, angles).
1082  */
1084  KFArray_OBS& A, const KFArray_OBS& B) const
1085 {
1086  A[0] -= B[0];
1087  A[1] -= B[1];
1089 }
1090 
1091 /** Return the observation NOISE covariance matrix, that is, the model of the
1092  * Gaussian additive noise of the sensor.
1093  * \param out_R The noise covariance matrix. It might be non diagonal, but
1094  * it'll usually be.
1095  */
1097 {
1098  out_R(0, 0) = square(options.std_sensor_range);
1099  out_R(1, 1) = square(options.std_sensor_yaw);
1100 }
1101 
1102 /** This will be called before OnGetObservationsAndDataAssociation to allow the
1103  * application to reduce the number of covariance landmark predictions to be
1104  * made.
1105  * For example, features which are known to be "out of sight" shouldn't be
1106  * added to the output list to speed up the calculations.
1107  * \param in_all_prediction_means The mean of each landmark predictions; the
1108  * computation or not of the corresponding covariances is what we're trying to
1109  * determined with this method.
1110  * \param out_LM_indices_to_predict The list of landmark indices in the map
1111  * [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.
1112  * \note This is not a pure virtual method, so it should be implemented only if
1113  * desired. The default implementation returns a vector with all the landmarks
1114  * in the map.
1115  * \sa OnGetObservations, OnDataAssociation
1116  */
1118  const vector_KFArray_OBS& prediction_means,
1119  std::vector<size_t>& out_LM_indices_to_predict) const
1120 {
1122  m_SF->getObservationByClass<CObservationBearingRange>();
1123  ASSERTMSG_(
1124  obs,
1125  "*ERROR*: This method requires an observation of type "
1126  "CObservationBearingRange");
1127 
1128  const double sensor_max_range = obs->maxSensorDistance;
1129  const double fov_yaw = obs->fieldOfView_yaw;
1130 
1131  const double max_vehicle_loc_uncertainty =
1132  4 * std::sqrt(m_pkk(0, 0) + m_pkk(1, 1));
1133  const double max_vehicle_ang_uncertainty = 4 * std::sqrt(m_pkk(2, 2));
1134 
1135  out_LM_indices_to_predict.clear();
1136  for (size_t i = 0; i < prediction_means.size(); i++)
1137 #if (!STATS_EXPERIMENT) // In the experiment we force errors too far and some
1138  // predictions are out of range, so just generate all
1139  // of them:
1140  if (prediction_means[i][0] <
1141  (1.5 + sensor_max_range + max_vehicle_loc_uncertainty +
1142  4 * options.std_sensor_range) &&
1143  fabs(prediction_means[i][1]) <
1144  (20.0_deg + 0.5 * fov_yaw + max_vehicle_ang_uncertainty +
1145  4 * options.std_sensor_yaw))
1146 #endif
1147  {
1148  out_LM_indices_to_predict.push_back(i);
1149  }
1150 }
1151 
1152 /** Only called if using a numeric approximation of the transition Jacobian,
1153  * this method must return the increments in each dimension of the vehicle state
1154  * vector while estimating the Jacobian.
1155  */
1157  KFArray_VEH& out_increments) const
1158 {
1159  for (size_t i = 0; i < get_vehicle_size(); i++) out_increments[i] = 1e-6;
1160 }
1161 
1162 /** Only called if using a numeric approximation of the observation Jacobians,
1163  * this method must return the increments in each dimension of the vehicle state
1164  * vector while estimating the Jacobian.
1165  */
1167  KFArray_VEH& out_veh_increments, KFArray_FEAT& out_feat_increments) const
1168 {
1169  for (size_t i = 0; i < get_vehicle_size(); i++)
1170  out_veh_increments[i] = 1e-6;
1171  for (size_t i = 0; i < get_feature_size(); i++)
1172  out_feat_increments[i] = 1e-6;
1173 }
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
double x
X,Y coordinates.
Definition: TPoint2D.h:23
std::string MATLAB_plotCovariance2D(const CMatrixFloat &cov22, const CVectorFloat &mean, const float &stdCount, const std::string &style=std::string("b"), const 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
#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.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
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 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)
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3727
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))
~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
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4199
mrpt::math::CMatrixDynamic< kftype > Y_pred_means
size_t size() const
Definition: bimap.h:57
mrpt::math::TPose2D asTPose() const
Definition: CPose2D.cpp:468
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.
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
GLsizei const GLchar ** string
Definition: glext.h:4116
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.
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
GLenum GLenum GLvoid * row
Definition: glext.h:3580
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.
GLsizei GLsizei GLchar * source
Definition: glext.h:4097
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...
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.
Lightweight 2D point.
Definition: TPoint2D.h:31
void OnTransitionModel(const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const override
Implements the transition model .
GLfloat GLfloat p
Definition: glext.h:6398
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
mrpt::obs::CSensoryFrame::Ptr m_SF
Set up by processActionObservation.
void OnObservationJacobians(const size_t &idx_landmark_to_predict, KFMatrix_OxV &Hx, KFMatrix_OxF &Hy) const override
Implements the observation Jacobians and (when applicable) .



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 4363012a5 Tue Nov 19 10:55:26 2019 +0100 at mar nov 19 11:00:13 CET 2019