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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019