MRPT  2.0.4
CRangeBearingKFSLAM.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "slam-precomp.h" // Precompiled headers
11 
12 // ----------------------------------------------------------------------------------------
13 // For the theory behind this implementation, see the technical report in:
14 //
15 // https://www.mrpt.org/6D-SLAM
16 // ----------------------------------------------------------------------------------------
17 
19 #include <mrpt/math/utils.h>
20 #include <mrpt/math/wrap2pi.h>
26 #include <mrpt/poses/CPosePDF.h>
29 #include <mrpt/system/CTicTac.h>
30 #include <mrpt/system/os.h>
31 #include <Eigen/Dense>
32 
33 using namespace mrpt::slam;
34 using namespace mrpt::obs;
35 using namespace mrpt::maps;
36 using namespace mrpt::math;
37 using namespace mrpt::poses;
38 using namespace mrpt::system;
39 using namespace mrpt;
40 using namespace std;
41 
42 /*---------------------------------------------------------------
43  Constructor
44  ---------------------------------------------------------------*/
46  : options(),
47  m_action(),
48  m_SF(),
49  m_IDs(),
50  mapPartitioner(),
51  m_SFs(),
52  m_lastPartitionSet()
53 {
54  reset();
55 }
56 
57 /*---------------------------------------------------------------
58  reset
59  ---------------------------------------------------------------*/
61 {
62  m_action.reset();
63  m_SF.reset();
64  m_IDs.clear();
65  m_SFs.clear();
67  m_lastPartitionSet.clear();
68 
69  // -----------------------
70  // INIT KF STATE
71  m_xkk.assign(get_vehicle_size(), 0); // State: 6D pose (x,y,z)=(0,0,0)
72  m_xkk[3] = 1.0; // (qr,qx,qy,qz)=(1,0,0,0)
73 
74  // Initial cov: nullptr diagonal -> perfect knowledge.
76  m_pkk.setZero();
77  // -----------------------
78 
79  // Use SF-based matching (faster & easier for bearing-range observations
80  // with ID).
82 }
83 
84 /*---------------------------------------------------------------
85  Destructor
86  ---------------------------------------------------------------*/
88 /*---------------------------------------------------------------
89  getCurrentRobotPose
90  ---------------------------------------------------------------*/
92  CPose3DQuatPDFGaussian& out_robotPose) const
93 {
95 
96  ASSERT_(m_xkk.size() >= 7);
97 
98  // Copy xyz+quat: (explicitly unroll the loop)
99  out_robotPose.mean.m_coords[0] = m_xkk[0];
100  out_robotPose.mean.m_coords[1] = m_xkk[1];
101  out_robotPose.mean.m_coords[2] = m_xkk[2];
102  out_robotPose.mean.m_quat[0] = m_xkk[3];
103  out_robotPose.mean.m_quat[1] = m_xkk[4];
104  out_robotPose.mean.m_quat[2] = m_xkk[5];
105  out_robotPose.mean.m_quat[3] = m_xkk[6];
106 
107  // and cov:
108  out_robotPose.cov = m_pkk.blockCopy<7, 7>(0, 0);
109 
110  MRPT_END
111 }
112 
113 /*---------------------------------------------------------------
114  getCurrentRobotPoseMean
115  ---------------------------------------------------------------*/
117 {
119  ASSERTDEB_(m_xkk.size() >= 7);
120  // Copy xyz+quat: (explicitly unroll the loop)
121  q.m_coords[0] = m_xkk[0];
122  q.m_coords[1] = m_xkk[1];
123  q.m_coords[2] = m_xkk[2];
124  q.m_quat[0] = m_xkk[3];
125  q.m_quat[1] = m_xkk[4];
126  q.m_quat[2] = m_xkk[5];
127  q.m_quat[3] = m_xkk[6];
128 
129  return q;
130 }
131 
132 /*---------------------------------------------------------------
133  getCurrentState
134  ---------------------------------------------------------------*/
136  CPose3DQuatPDFGaussian& out_robotPose,
137  std::vector<TPoint3D>& out_landmarksPositions,
138  std::map<unsigned int, CLandmark::TLandmarkID>& out_landmarkIDs,
139  CVectorDouble& out_fullState, CMatrixDouble& out_fullCovariance) const
140 {
141  MRPT_START
142 
143  ASSERT_(size_t(m_xkk.size()) >= get_vehicle_size());
144 
145  // Copy xyz+quat: (explicitly unroll the loop)
146  out_robotPose.mean.m_coords[0] = m_xkk[0];
147  out_robotPose.mean.m_coords[1] = m_xkk[1];
148  out_robotPose.mean.m_coords[2] = m_xkk[2];
149  out_robotPose.mean.m_quat[0] = m_xkk[3];
150  out_robotPose.mean.m_quat[1] = m_xkk[4];
151  out_robotPose.mean.m_quat[2] = m_xkk[5];
152  out_robotPose.mean.m_quat[3] = m_xkk[6];
153 
154  // and cov:
155  out_robotPose.cov = m_pkk.blockCopy<7, 7>(0, 0);
156 
157  // Landmarks:
158  ASSERT_(((m_xkk.size() - get_vehicle_size()) % get_feature_size()) == 0);
159  size_t i, nLMs = (m_xkk.size() - get_vehicle_size()) / get_feature_size();
160  out_landmarksPositions.resize(nLMs);
161  for (i = 0; i < nLMs; i++)
162  {
163  out_landmarksPositions[i].x =
164  m_xkk[get_vehicle_size() + i * get_feature_size() + 0];
165  out_landmarksPositions[i].y =
166  m_xkk[get_vehicle_size() + i * get_feature_size() + 1];
167  out_landmarksPositions[i].z =
168  m_xkk[get_vehicle_size() + i * get_feature_size() + 2];
169  } // end for i
170 
171  // IDs:
172  out_landmarkIDs = m_IDs.getInverseMap(); // m_IDs_inverse;
173 
174  // Full state:
175  out_fullState.resize(m_xkk.size());
176  std::copy(m_xkk.begin(), m_xkk.end(), out_fullState.begin());
177  // Full cov:
178  out_fullCovariance = m_pkk;
179 
180  MRPT_END
181 }
182 
183 /*---------------------------------------------------------------
184  processActionObservation
185  ---------------------------------------------------------------*/
188 {
189  MRPT_START
190 
191  m_action = action;
192  m_SF = SF;
193 
194  // Sanity check:
195  ASSERT_(
196  m_IDs.size() ==
198 
199  // ===================================================================================================================
200  // Here's the meat!: Call the main method for the KF algorithm, which will
201  // call all the callback methods as required:
202  // ===================================================================================================================
204 
205  // =============================================================
206  // ADD TO SFs SEQUENCE
207  // =============================================================
209  this->getCurrentRobotPose(q);
210  CPose3DPDFGaussian::Ptr auxPosePDF = CPose3DPDFGaussian::Create(q);
211 
213  {
214  m_SFs.insert(CPose3DPDF::Ptr(auxPosePDF), SF);
215  }
216 
217  // =============================================================
218  // UPDATE THE PARTITION GRAPH EXPERIMENT
219  // =============================================================
221  {
222  if (options.partitioningMethod == 0)
223  {
224  // Use spectral-graph technique:
225  mapPartitioner.addMapFrame(*SF, *auxPosePDF);
226 
227  vector<std::vector<uint32_t>> partitions;
228  mapPartitioner.updatePartitions(partitions);
229  m_lastPartitionSet = partitions;
230  }
231  else
232  {
233  // Fixed partitions every K observations:
234  vector<std::vector<uint32_t>> partitions;
235  std::vector<uint32_t> tmpCluster;
236 
238  size_t N = options.partitioningMethod;
239 
240  for (size_t i = 0; i < m_SFs.size(); i++)
241  {
242  tmpCluster.push_back(i);
243  if ((i % N) == 0)
244  {
245  partitions.push_back(tmpCluster);
246  tmpCluster.clear();
247  tmpCluster.push_back(i); // This observation "i" is shared
248  // between both clusters
249  }
250  }
251  m_lastPartitionSet = partitions;
252  }
253 
254  printf(
255  "Partitions: %u\n",
256  static_cast<unsigned>(m_lastPartitionSet.size()));
257  }
258 
259  MRPT_END
260 }
261 
262 /** Return the last odometry, as a pose increment.
263  */
265 {
266  CActionRobotMovement2D::Ptr actMov2D =
267  m_action->getBestMovementEstimation();
268  CActionRobotMovement3D::Ptr actMov3D =
269  m_action->getActionByClass<CActionRobotMovement3D>();
270  if (actMov3D && !options.force_ignore_odometry)
271  {
272  return CPose3DQuat(actMov3D->poseChange.mean);
273  }
274  else if (actMov2D && !options.force_ignore_odometry)
275  {
276  CPose2D estMovMean;
277  actMov2D->poseChange->getMean(estMovMean);
278  return CPose3DQuat(CPose3D(estMovMean));
279  }
280  else
281  {
282  return CPose3DQuat();
283  }
284 }
285 
286 /** Must return the action vector u.
287  * \param out_u The action vector which will be passed to OnTransitionModel
288  */
289 void CRangeBearingKFSLAM::OnGetAction(KFArray_ACT& u) const
290 {
291  // Get odometry estimation:
292  const CPose3DQuat theIncr = getIncrementFromOdometry();
293 
294  for (KFArray_ACT::Index i = 0;
295  i < static_cast<KFArray_ACT::Index>(u.size()); i++)
296  u[i] = theIncr[i];
297 }
298 
299 /** This virtual function musts implement the prediction model of the Kalman
300  * filter.
301  */
303  const KFArray_ACT& u, KFArray_VEH& xv, bool& out_skipPrediction) const
304 {
305  MRPT_START
306 
307  // Do not update the vehicle pose & its covariance until we have some
308  // landmarks in the map,
309  // otherwise, we are imposing a lower bound to the best uncertainty from now
310  // on:
311  if (size_t(m_xkk.size()) == get_vehicle_size())
312  {
313  out_skipPrediction = true;
314  }
315 
316  // Current pose: copy xyz+quat
317  CPose3DQuat robotPose = getCurrentRobotPoseMean();
318 
319  // Increment pose: copy xyz+quat (explicitly unroll the loop)
321  odoIncrement.m_coords[0] = u[0];
322  odoIncrement.m_coords[1] = u[1];
323  odoIncrement.m_coords[2] = u[2];
324  odoIncrement.m_quat[0] = u[3];
325  odoIncrement.m_quat[1] = u[4];
326  odoIncrement.m_quat[2] = u[5];
327  odoIncrement.m_quat[3] = u[6];
328 
329  // Pose composition:
330  robotPose += odoIncrement;
331 
332  // Output:
333  for (size_t i = 0; i < xv.SizeAtCompileTime; i++) xv[i] = robotPose[i];
334 
335  MRPT_END
336 }
337 
338 /** This virtual function musts calculate the Jacobian F of the prediction
339  * model.
340  */
341 void CRangeBearingKFSLAM::OnTransitionJacobian(KFMatrix_VxV& F) const
342 {
343  MRPT_START
344 
345  // Current pose: copy xyz+quat
346  CPose3DQuat robotPose = getCurrentRobotPoseMean();
347 
348  // Odometry:
349  const CPose3DQuat theIncr = getIncrementFromOdometry();
350 
351  // Compute jacobians:
353 
354  CPose3DQuatPDF::jacobiansPoseComposition(
355  robotPose, // x
356  theIncr, // u
357  F, // df_dx,
358  df_du);
359 
360  MRPT_END
361 }
362 
363 /** This virtual function musts calculate de noise matrix of the prediction
364  * model.
365  */
366 void CRangeBearingKFSLAM::OnTransitionNoise(KFMatrix_VxV& Q) const
367 {
368  MRPT_START
369 
370  // The uncertainty of the 2D odometry, projected from the current position:
371  CActionRobotMovement2D::Ptr act2D = m_action->getBestMovementEstimation();
373  m_action->getActionByClass<CActionRobotMovement3D>();
374 
375  if (act3D && act2D)
376  THROW_EXCEPTION("Both 2D & 3D odometry are present!?!?");
377 
378  CPose3DQuatPDFGaussian odoIncr;
379 
380  if ((!act3D && !act2D) || options.force_ignore_odometry)
381  {
382  // Use constant Q:
383  Q.setZero();
384  ASSERT_(size_t(options.stds_Q_no_odo.size()) == size_t(Q.cols()));
385  for (size_t i = 0; i < get_vehicle_size(); i++)
386  Q(i, i) = square(options.stds_Q_no_odo[i]);
387  return;
388  }
389  else
390  {
391  if (act2D)
392  {
393  // 2D odometry:
394  CPosePDFGaussian odoIncr2D;
395  odoIncr2D.copyFrom(*act2D->poseChange);
396  odoIncr = CPose3DQuatPDFGaussian(CPose3DPDFGaussian(odoIncr2D));
397  }
398  else
399  {
400  // 3D odometry:
401  odoIncr = CPose3DQuatPDFGaussian(act3D->poseChange);
402  }
403  }
404 
405  odoIncr.cov(2, 2) += square(options.std_odo_z_additional);
406 
407  // Current pose: copy xyz+quat
408  CPose3DQuat robotPose = getCurrentRobotPoseMean();
409 
410  // Transform from odometry increment to "relative to the robot":
411  odoIncr.changeCoordinatesReference(robotPose);
412 
413  Q = odoIncr.cov;
414 
415  MRPT_END
416 }
417 
419  const std::vector<size_t>& idx_landmarks_to_predict,
420  vector_KFArray_OBS& out_predictions) const
421 {
422  MRPT_START
423 
424  // Mean of the prior of the robot pose:
425  CPose3DQuat robotPose = getCurrentRobotPoseMean();
426 
427  // Get the sensor pose relative to the robot:
429  m_SF->getObservationByClass<CObservationBearingRange>();
430  ASSERTMSG_(
431  obs,
432  "*ERROR*: This method requires an observation of type "
433  "CObservationBearingRange");
434  const CPose3DQuat sensorPoseOnRobot =
435  CPose3DQuat(obs->sensorLocationOnRobot);
436 
437  /* -------------------------------------------
438  Equations, obtained using matlab, of the relative 3D position of a
439  landmark (xi,yi,zi), relative
440  to a robot 6D pose (x0,y0,z0,y,p,r)
441  Refer to technical report "6D EKF derivation...", 2008
442 
443  x0 y0 z0 y p r % Robot's 6D pose
444  x0s y0s z0s ys ps rs % Sensor's 6D pose relative to robot
445  xi yi zi % Absolute 3D landmark coordinates:
446 
447  Hx : dh_dxv -> Jacobian of the observation model wrt the robot pose
448  Hy : dh_dyi -> Jacobian of the observation model wrt each landmark
449  mean position
450 
451  Sizes:
452  h: Lx3
453  Hx: 3Lx6
454  Hy: 3Lx3
455 
456  L=# of landmarks in the map (ALL OF THEM)
457  ------------------------------------------- */
458 
459  const size_t vehicle_size = get_vehicle_size();
460  // const size_t obs_size = get_observation_size();
461  const size_t feature_size = get_feature_size();
462 
463  const CPose3DQuat sensorPoseAbs = robotPose + sensorPoseOnRobot;
464 
465  const size_t N = idx_landmarks_to_predict.size();
466  out_predictions.resize(N);
467  for (size_t i = 0; i < N; i++)
468  {
469  const size_t row_in = feature_size * idx_landmarks_to_predict[i];
470 
471  // Landmark absolute 3D position in the map:
472  const TPoint3D mapEst(
473  m_xkk[vehicle_size + row_in + 0], m_xkk[vehicle_size + row_in + 1],
474  m_xkk[vehicle_size + row_in + 2]);
475 
476  // Generate Range, yaw, pitch
477  // ---------------------------------------------------
478  sensorPoseAbs.sphericalCoordinates(
479  mapEst,
480  out_predictions[i][0], // range
481  out_predictions[i][1], // yaw
482  out_predictions[i][2] // pitch
483  );
484  }
485 
486  MRPT_END
487 }
488 
490  size_t idx_landmark_to_predict, KFMatrix_OxV& Hx, KFMatrix_OxF& Hy) const
491 {
492  MRPT_START
493 
494  // Mean of the prior of the robot pose:
495  const CPose3DQuat robotPose = getCurrentRobotPoseMean();
496 
497  // Get the sensor pose relative to the robot:
499  m_SF->getObservationByClass<CObservationBearingRange>();
500  ASSERTMSG_(
501  obs,
502  "*ERROR*: This method requires an observation of type "
503  "CObservationBearingRange");
504  const CPose3DQuat sensorPoseOnRobot =
505  CPose3DQuat(obs->sensorLocationOnRobot);
506 
507  const size_t vehicle_size = get_vehicle_size();
508  // const size_t obs_size = get_observation_size();
509  const size_t feature_size = get_feature_size();
510 
511  // Compute the jacobians, needed below:
512  // const CPose3DQuat sensorPoseAbs= robotPose + sensorPoseOnRobot;
513  CPose3DQuat sensorPoseAbs(UNINITIALIZED_QUATERNION);
515  CMatrixFixed<kftype, 7, 7> H_senpose_senrelpose(
516  UNINITIALIZED_MATRIX); // Not actually used
517 
518  CPose3DQuatPDF::jacobiansPoseComposition(
519  robotPose, sensorPoseOnRobot, H_senpose_vehpose, H_senpose_senrelpose,
520  &sensorPoseAbs);
521 
522  const size_t row_in = feature_size * idx_landmark_to_predict;
523 
524  // Landmark absolute 3D position in the map:
525  const TPoint3D mapEst(
526  m_xkk[vehicle_size + row_in + 0], m_xkk[vehicle_size + row_in + 1],
527  m_xkk[vehicle_size + row_in + 2]);
528 
529  // The Jacobian wrt the sensor pose must be transformed later on:
530  KFMatrix_OxV Hx_sensor;
531  double obsData[3];
532  sensorPoseAbs.sphericalCoordinates(
533  mapEst,
534  obsData[0], // range
535  obsData[1], // yaw
536  obsData[2], // pitch
537  &Hy, &Hx_sensor);
538 
539  // Chain rule: Hx = d sensorpose / d vehiclepose * Hx_sensor
540  Hx = Hx_sensor * H_senpose_vehpose;
541 
542  MRPT_END
543 }
544 
545 /** This is called between the KF prediction step and the update step, and the
546  * application must return the observations and, when applicable, the data
547  * association between these observations and the current map.
548  *
549  * \param out_z N vectors, each for one "observation" of length OBS_SIZE, N
550  * being the number of "observations": how many observed landmarks for a map, or
551  * just one if not applicable.
552  * \param out_data_association An empty vector or, where applicable, a vector
553  * where the i'th element corresponds to the position of the observation in the
554  * i'th row of out_z within the system state vector (in the range
555  * [0,getNumberOfLandmarksInTheMap()-1]), or -1 if it is a new map element and
556  * we want to insert it at the end of this KF iteration.
557  * \param in_S The full covariance matrix of the observation predictions (i.e.
558  * the "innovation covariance matrix"). This is a M·O x M·O matrix with M=length
559  * of "in_lm_indices_in_S".
560  * \param in_lm_indices_in_S The indices of the map landmarks (range
561  * [0,getNumberOfLandmarksInTheMap()-1]) that can be found in the matrix in_S.
562  *
563  * This method will be called just once for each complete KF iteration.
564  * \note It is assumed that the observations are independent, i.e. there are NO
565  * cross-covariances between them.
566  */
568  vector_KFArray_OBS& Z, std::vector<int>& data_association,
569  const vector_KFArray_OBS& all_predictions, const KFMatrix& S,
570  const std::vector<size_t>& lm_indices_in_S, const KFMatrix_OxO& R)
571 {
572  MRPT_START
573 
574  // static const size_t vehicle_size = get_vehicle_size();
575  constexpr size_t obs_size = get_observation_size();
576 
577  // Z: Observations
578  CObservationBearingRange::TMeasurementList::const_iterator itObs;
579 
581  m_SF->getObservationByClass<CObservationBearingRange>();
582  ASSERTMSG_(
583  obs,
584  "*ERROR*: This method requires an observation of type "
585  "CObservationBearingRange");
586 
587  const size_t N = obs->sensedData.size();
588  Z.resize(N);
589 
590  size_t row;
591  for (row = 0, itObs = obs->sensedData.begin();
592  itObs != obs->sensedData.end(); ++itObs, ++row)
593  {
594  // Fill one row in Z:
595  Z[row][0] = itObs->range;
596  Z[row][1] = itObs->yaw;
597  Z[row][2] = itObs->pitch;
598  }
599 
600  // Data association:
601  // ---------------------
602  data_association.assign(N, -1); // Initially, all new landmarks
603 
604  // For each observed LM:
605  std::vector<size_t> obs_idxs_needing_data_assoc;
606  obs_idxs_needing_data_assoc.reserve(N);
607 
608  {
609  std::vector<int>::iterator itDA;
610  for (row = 0, itObs = obs->sensedData.begin(),
611  itDA = data_association.begin();
612  itObs != obs->sensedData.end(); ++itObs, ++itDA, ++row)
613  {
614  // Fill data asociation: Using IDs!
615  if (itObs->landmarkID < 0)
616  obs_idxs_needing_data_assoc.push_back(row);
617  else
618  {
620  CLandmark::TLandmarkID, unsigned int>::iterator itID;
621  if ((itID = m_IDs.find_key(itObs->landmarkID)) != m_IDs.end())
622  *itDA = itID->second; // This row in Z corresponds to the
623  // i'th map element in the state
624  // vector:
625  }
626  }
627  }
628 
629  // ---- Perform data association ----
630  // Only for observation indices in "obs_idxs_needing_data_assoc"
631  if (obs_idxs_needing_data_assoc.empty())
632  {
633  // We don't need to do DA:
635 
636  // Save them for the case the external user wants to access them:
637  for (size_t idxObs = 0; idxObs < data_association.size(); idxObs++)
638  {
639  int da = data_association[idxObs];
640  if (da >= 0)
642  data_association[idxObs];
643  }
644  }
645  else
646  {
647  // Build a Z matrix with the observations that need dat.assoc:
648  const size_t nObsDA = obs_idxs_needing_data_assoc.size();
649 
650  CMatrixDynamic<kftype> Z_obs_means(nObsDA, obs_size);
651  for (size_t i = 0; i < nObsDA; i++)
652  {
653  const size_t idx = obs_idxs_needing_data_assoc[i];
654  for (unsigned k = 0; k < obs_size; k++)
655  Z_obs_means(i, k) = Z[idx][k];
656  }
657 
658  // Vehicle uncertainty
659  // KFMatrix_VxV Pxx = m_pkk.extractMatrix<7, 7>(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 
692  true, // Use KD-tree
696 
697  // Return pairings to the main KF algorithm:
698  for (auto it = m_last_data_association.results.associations.begin();
699  it != m_last_data_association.results.associations.end(); ++it)
700  data_association[it->first] = it->second;
701  }
702  }
703  // ---- End of data association ----
704 
705  MRPT_END
706 }
707 
708 /** This virtual function musts normalize the state vector and covariance matrix
709  * (only if its necessary).
710  */
712 {
713  MRPT_START
714 
715  // m_xkk[3:6] must be a normalized quaternion:
716  const double T = std::sqrt(
717  square(m_xkk[3]) + square(m_xkk[4]) + square(m_xkk[5]) +
718  square(m_xkk[6]));
719  ASSERTMSG_(T > 0, "Vehicle pose quaternion norm is not >0!!");
720 
721  const double T_ = (m_xkk[3] < 0 ? -1.0 : 1.0) / T; // qr>=0
722  m_xkk[3] *= T_;
723  m_xkk[4] *= T_;
724  m_xkk[5] *= T_;
725  m_xkk[6] *= T_;
726 
727  MRPT_END
728 }
729 
730 /*---------------------------------------------------------------
731  loadOptions
732  ---------------------------------------------------------------*/
734 {
735  // Main
736  options.loadFromConfigFile(ini, "RangeBearingKFSLAM");
737  KF_options.loadFromConfigFile(ini, "RangeBearingKFSLAM_KalmanFilter");
738  // partition algorithm:
739  mapPartitioner.options.loadFromConfigFile(ini, "RangeBearingKFSLAM");
740 }
741 
742 /*---------------------------------------------------------------
743  loadOptions
744  ---------------------------------------------------------------*/
746  const mrpt::config::CConfigFileBase& source, const std::string& section)
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() == 7);
751  source.read_float(section, "std_sensor_range", std_sensor_range);
753  section, "std_sensor_yaw_deg", RAD2DEG(std_sensor_yaw)));
755  section, "std_sensor_pitch_deg", RAD2DEG(std_sensor_pitch)));
756 
758  section, "std_odo_z_additional", std_odo_z_additional);
759 
760  MRPT_LOAD_CONFIG_VAR(doPartitioningExperiment, bool, source, section);
761  MRPT_LOAD_CONFIG_VAR(partitioningMethod, int, source, section);
762 
763  MRPT_LOAD_CONFIG_VAR(create_simplemap, bool, source, section);
764 
765  MRPT_LOAD_CONFIG_VAR(force_ignore_odometry, bool, source, section);
766 
768  section, "data_assoc_method", data_assoc_method);
770  section, "data_assoc_metric", data_assoc_metric);
772  section, "data_assoc_IC_metric", data_assoc_IC_metric);
773 
774  MRPT_LOAD_CONFIG_VAR(data_assoc_IC_chi2_thres, double, source, section);
775  MRPT_LOAD_CONFIG_VAR(data_assoc_IC_ml_threshold, double, source, section);
776 
777  MRPT_LOAD_CONFIG_VAR(quantiles_3D_representation, float, source, section);
778 }
779 
780 /*---------------------------------------------------------------
781  Constructor
782  ---------------------------------------------------------------*/
784  : stds_Q_no_odo(get_vehicle_size(), 0),
785 
786  std_sensor_yaw(DEG2RAD(0.2f)),
787  std_sensor_pitch(DEG2RAD(0.2f))
788 
789 {
790  stds_Q_no_odo[0] = stds_Q_no_odo[1] = stds_Q_no_odo[2] = 0.10f;
792  0.05f;
793 }
794 
795 /*---------------------------------------------------------------
796  dumpToTextStream
797  ---------------------------------------------------------------*/
799 {
800  using namespace mrpt::typemeta;
801 
802  out << "\n----------- [CRangeBearingKFSLAM::TOptions] ------------ \n\n";
803 
804  out << mrpt::format(
805  "doPartitioningExperiment = %c\n",
806  doPartitioningExperiment ? 'Y' : 'N');
807  out << mrpt::format(
808  "partitioningMethod = %i\n", partitioningMethod);
809  out << mrpt::format(
810  "data_assoc_method = %s\n",
812  .c_str());
813  out << mrpt::format(
814  "data_assoc_metric = %s\n",
816  .c_str());
817  out << mrpt::format(
818  "data_assoc_IC_chi2_thres = %.06f\n",
819  data_assoc_IC_chi2_thres);
820  out << mrpt::format(
821  "data_assoc_IC_metric = %s\n",
823  .c_str());
824  out << mrpt::format(
825  "data_assoc_IC_ml_threshold = %.06f\n",
826  data_assoc_IC_ml_threshold);
827 
828  out << "\n";
829 }
830 
832  const KFArray_OBS& in_z, KFArray_FEAT& yn, KFMatrix_FxV& dyn_dxv,
833  KFMatrix_FxO& dyn_dhn) const
834 {
835  MRPT_START
836 
838  m_SF->getObservationByClass<CObservationBearingRange>();
839  ASSERTMSG_(
840  obs,
841  "*ERROR*: This method requires an observation of type "
842  "CObservationBearingRange");
843  const CPose3DQuat sensorPoseOnRobot =
844  CPose3DQuat(obs->sensorLocationOnRobot);
845 
846  // Mean of the prior of the robot pose:
847  const CPose3DQuat robotPose = getCurrentRobotPoseMean();
848 
849  // const CPose3DQuat sensorPoseAbs= robotPose + sensorPoseOnRobot;
850  CPose3DQuat sensorPoseAbs(UNINITIALIZED_QUATERNION);
852  CMatrixFixed<kftype, 7, 7> dsensorabs_dsenrelpose(
853  UNINITIALIZED_MATRIX); // Not actually used
854 
855  CPose3DQuatPDF::jacobiansPoseComposition(
856  robotPose, sensorPoseOnRobot, dsensorabs_dvehpose,
857  dsensorabs_dsenrelpose, &sensorPoseAbs);
858 
859  kftype hn_r = in_z[0];
860  kftype hn_y = in_z[1];
861  kftype hn_p = in_z[2];
862 
863  kftype chn_y = cos(hn_y);
864  kftype shn_y = sin(hn_y);
865  kftype chn_p = cos(hn_p);
866  kftype shn_p = sin(hn_p);
867 
868  /* -------------------------------------------
869  syms H h_range h_yaw h_pitch real;
870  H=[ h_range ; h_yaw ; h_pitch ];
871 
872  syms X xi_ yi_ zi_ real;
873  xi_ = h_range * cos(h_yaw) * cos(h_pitch);
874  yi_ = h_range * sin(h_yaw) * cos(h_pitch);
875  zi_ = -h_range * sin(h_pitch);
876 
877  X=[xi_ yi_ zi_];
878  jacob_inv_mod=jacobian(X,H)
879  ------------------------------------------- */
880 
881  // The new point, relative to the sensor:
882  const TPoint3D yn_rel_sensor(
883  hn_r * chn_y * chn_p, hn_r * shn_y * chn_p, -hn_r * shn_p);
884 
885  // The Jacobian of the 3D point in the coordinate system of the sensor:
886  /*
887  [ cos(h_pitch)*cos(h_yaw), -h_range*cos(h_pitch)*sin(h_yaw),
888  -h_range*cos(h_yaw)*sin(h_pitch)]
889  [ cos(h_pitch)*sin(h_yaw), h_range*cos(h_pitch)*cos(h_yaw),
890  -h_range*sin(h_pitch)*sin(h_yaw)]
891  [ -sin(h_pitch), 0,
892  -h_range*cos(h_pitch)]
893  */
894  const kftype values_dynlocal_dhn[] = {chn_p * chn_y,
895  -hn_r * chn_p * shn_y,
896  -hn_r * chn_y * shn_p,
897  chn_p * shn_y,
898  hn_r * chn_p * chn_y,
899  -hn_r * shn_p * shn_y,
900  -shn_p,
901  0,
902  -hn_r * chn_p};
903  const KFMatrix_FxO dynlocal_dhn(values_dynlocal_dhn);
904 
905  KFMatrix_FxF jacob_dyn_dynrelsensor(UNINITIALIZED_MATRIX);
906  KFMatrix_FxV jacob_dyn_dsensorabs(UNINITIALIZED_MATRIX);
907 
908  sensorPoseAbs.composePoint(
909  yn_rel_sensor.x, yn_rel_sensor.y,
910  yn_rel_sensor.z, // yn rel. to the sensor
911  yn[0], yn[1], yn[2], // yn in global coords
912  &jacob_dyn_dynrelsensor, &jacob_dyn_dsensorabs);
913 
914  dyn_dhn = jacob_dyn_dynrelsensor * dynlocal_dhn;
915 
916  // dyn_dxv =
917  dyn_dxv = jacob_dyn_dsensorabs * dsensorabs_dvehpose;
918 
919  MRPT_END
920 }
921 
922 /** If applicable to the given problem, do here any special handling of adding a
923  * new landmark to the map.
924  * \param in_obsIndex The index of the observation whose inverse sensor is to
925  * be computed. It corresponds to the row in in_z where the observation can be
926  * found.
927  * \param in_idxNewFeat The index that this new feature will have in the state
928  * vector (0:just after the vehicle state, 1: after that,...). Save this number
929  * so data association can be done according to these indices.
930  * \sa OnInverseObservationModel
931  */
933  const size_t in_obsIdx, const size_t in_idxNewFeat)
934 {
935  MRPT_START
936 
938  m_SF->getObservationByClass<CObservationBearingRange>();
939  ASSERTMSG_(
940  obs,
941  "*ERROR*: This method requires an observation of type "
942  "CObservationBearingRange");
943 
944  // ----------------------------------------------
945  // introduce in the lists of ID<->index in map:
946  // ----------------------------------------------
947  ASSERT_(in_obsIdx < obs->sensedData.size());
948  if (obs->sensedData[in_obsIdx].landmarkID >= 0)
949  {
950  // The sensor provides us a LM ID... use it:
951  m_IDs.insert(obs->sensedData[in_obsIdx].landmarkID, in_idxNewFeat);
952  }
953  else
954  {
955  // Features do not have IDs... use indices:
956  m_IDs.insert(in_idxNewFeat, in_idxNewFeat);
957  }
958 
959  MRPT_END
960 }
961 
962 /*---------------------------------------------------------------
963  getAs3DObject
964  ---------------------------------------------------------------*/
966  mrpt::opengl::CSetOfObjects::Ptr& outObj) const
967 {
968  outObj->clear();
969 
970  // ------------------------------------------------
971  // Add the XYZ corner for the current area:
972  // ------------------------------------------------
973  outObj->insert(opengl::stock_objects::CornerXYZ());
974 
975  // 3D ellipsoid for robot pose:
976  CPointPDFGaussian pointGauss;
977  pointGauss.mean.x(m_xkk[0]);
978  pointGauss.mean.y(m_xkk[1]);
979  pointGauss.mean.z(m_xkk[2]);
980  pointGauss.cov = m_pkk.blockCopy<3, 3>(0, 0);
981 
982  {
983  auto ellip = opengl::CEllipsoid3D::Create();
984 
985  ellip->setPose(pointGauss.mean);
986  ellip->setCovMatrix(pointGauss.cov);
987  ellip->enableDrawSolid3D(false);
988  ellip->setQuantiles(options.quantiles_3D_representation);
989  ellip->set3DsegmentsCount(10);
990  ellip->setColor(1, 0, 0);
991 
992  outObj->insert(ellip);
993  }
994 
995  // 3D ellipsoids for landmarks:
996  const size_t nLMs = this->getNumberOfLandmarksInTheMap();
997  for (size_t i = 0; i < nLMs; i++)
998  {
999  pointGauss.mean.x(
1000  m_xkk[get_vehicle_size() + get_feature_size() * i + 0]);
1001  pointGauss.mean.y(
1002  m_xkk[get_vehicle_size() + get_feature_size() * i + 1]);
1003  pointGauss.mean.z(
1004  m_xkk[get_vehicle_size() + get_feature_size() * i + 2]);
1005 
1006  pointGauss.cov =
1010 
1011  auto ellip = opengl::CEllipsoid3D::Create();
1012 
1013  ellip->setName(format("%u", static_cast<unsigned int>(i)));
1014  ellip->enableShowName(true);
1015  ellip->setPose(pointGauss.mean);
1016  ellip->setCovMatrix(pointGauss.cov);
1017  ellip->enableDrawSolid3D(false);
1018  ellip->setQuantiles(options.quantiles_3D_representation);
1019  ellip->set3DsegmentsCount(10);
1020 
1021  ellip->setColor(0, 0, 1);
1022 
1023  // Set color depending on partitions?
1025  {
1026  // This is done for each landmark:
1027  map<int, bool> belongToPartition;
1028  const CSimpleMap* SFs =
1029  &m_SFs; // mapPartitioner.getSequenceOfFrames();
1030 
1031  for (size_t p = 0; p < m_lastPartitionSet.size(); p++)
1032  {
1033  for (size_t w = 0; w < m_lastPartitionSet[p].size(); w++)
1034  {
1035  // Check if landmark #i is in the SF of
1036  // m_lastPartitionSet[p][w]:
1037  CLandmark::TLandmarkID i_th_ID = m_IDs.inverse(i);
1038 
1039  // Look for the lm_ID in the SF:
1040  CPose3DPDF::Ptr pdf;
1041  CSensoryFrame::Ptr SF_i;
1042  SFs->get(m_lastPartitionSet[p][w], pdf, SF_i);
1043 
1045  SF_i->getObservationByClass<CObservationBearingRange>();
1046 
1047  for (auto& o : obs->sensedData)
1048  {
1049  if (o.landmarkID == i_th_ID)
1050  {
1051  belongToPartition[p] = true;
1052  break;
1053  }
1054  } // end for o
1055  } // end for w
1056  } // end for p
1057 
1058  // Build label:
1059  string strParts("[");
1060 
1061  for (auto it = belongToPartition.begin();
1062  it != belongToPartition.end(); ++it)
1063  {
1064  if (it != belongToPartition.begin()) strParts += string(",");
1065  strParts += format("%i", it->first);
1066  }
1067 
1068  ellip->setName(ellip->getName() + strParts + string("]"));
1069  }
1070 
1071  outObj->insert(ellip);
1072  }
1073 }
1074 
1075 /*---------------------------------------------------------------
1076  getLastPartitionLandmarks
1077  ---------------------------------------------------------------*/
1079  size_t K, std::vector<std::vector<uint32_t>>& landmarksMembership)
1080 {
1081  // temporary copy:
1082  std::vector<std::vector<uint32_t>> tmpParts = m_lastPartitionSet;
1083 
1084  // Fake "m_lastPartitionSet":
1085 
1086  // Fixed partitions every K observations:
1087  vector<std::vector<uint32_t>> partitions;
1088  std::vector<uint32_t> tmpCluster;
1089 
1090  for (size_t i = 0; i < m_SFs.size(); i++)
1091  {
1092  tmpCluster.push_back(i);
1093  if ((i % K) == 0)
1094  {
1095  partitions.push_back(tmpCluster);
1096  tmpCluster.clear();
1097  tmpCluster.push_back(
1098  i); // This observation "i" is shared between both clusters
1099  }
1100  }
1101  m_lastPartitionSet = partitions;
1102 
1103  // Call the actual method:
1104  getLastPartitionLandmarks(landmarksMembership);
1105 
1106  // Replace copy:
1107  m_lastPartitionSet = tmpParts; //-V519
1108 }
1109 
1110 /*---------------------------------------------------------------
1111  getLastPartitionLandmarks
1112  ---------------------------------------------------------------*/
1114  std::vector<std::vector<uint32_t>>& landmarksMembership) const
1115 {
1116  landmarksMembership.clear();
1117 
1118  // All the computation is made based on "m_lastPartitionSet"
1119 
1120  if (!options.doPartitioningExperiment) return;
1121 
1122  const size_t nLMs = this->getNumberOfLandmarksInTheMap();
1123  for (size_t i = 0; i < nLMs; i++)
1124  {
1125  map<int, bool> belongToPartition;
1126  const CSimpleMap* SFs =
1127  &m_SFs; // mapPartitioner.getSequenceOfFrames();
1128 
1129  for (size_t p = 0; p < m_lastPartitionSet.size(); p++)
1130  {
1131  for (size_t w = 0; w < m_lastPartitionSet[p].size(); w++)
1132  {
1133  // Check if landmark #i is in the SF of
1134  // m_lastPartitionSet[p][w]:
1135  CLandmark::TLandmarkID i_th_ID = m_IDs.inverse(i);
1136 
1137  // Look for the lm_ID in the SF:
1138  CPose3DPDF::Ptr pdf;
1139  CSensoryFrame::Ptr SF_i;
1140  SFs->get(m_lastPartitionSet[p][w], pdf, SF_i);
1141 
1143  SF_i->getObservationByClass<CObservationBearingRange>();
1144 
1145  for (auto& o : obs->sensedData)
1146  {
1147  if (o.landmarkID == i_th_ID)
1148  {
1149  belongToPartition[p] = true;
1150  break;
1151  }
1152  } // end for o
1153  } // end for w
1154  } // end for p
1155 
1156  // Build membership list:
1157  std::vector<uint32_t> membershipOfThisLM;
1158 
1159  for (auto& it : belongToPartition)
1160  membershipOfThisLM.push_back(it.first);
1161 
1162  landmarksMembership.push_back(membershipOfThisLM);
1163  } // end for i
1164 }
1165 
1166 /*---------------------------------------------------------------
1167  computeOffDiagonalBlocksApproximationError
1168  ---------------------------------------------------------------*/
1170  const std::vector<std::vector<uint32_t>>& landmarksMembership) const
1171 {
1172  MRPT_START
1173 
1174  // Compute the information matrix:
1175  CMatrixDynamic<kftype> fullCov(m_pkk);
1176  size_t i;
1177  for (i = 0; i < get_vehicle_size(); i++)
1178  fullCov(i, i) = max(fullCov(i, i), 1e-6);
1179 
1180  CMatrixDynamic<kftype> H(fullCov.inverse_LLt());
1181  H.array().abs(); // Replace by absolute values:
1182 
1183  double sumOffBlocks = 0;
1184  unsigned int nLMs = landmarksMembership.size();
1185 
1186  ASSERT_(
1187  int(get_vehicle_size() + nLMs * get_feature_size()) == fullCov.cols());
1188 
1189  for (i = 0; i < nLMs; i++)
1190  {
1191  for (size_t j = i + 1; j < nLMs; j++)
1192  {
1193  // Sum the cross cov. between LM(i) and LM(j)??
1194  // --> Only if there is no common cluster:
1195  if (0 == math::countCommonElements(
1196  landmarksMembership[i], landmarksMembership[j]))
1197  {
1198  size_t col = get_vehicle_size() + i * get_feature_size();
1199  size_t row = get_vehicle_size() + j * get_feature_size();
1200  sumOffBlocks += 2 * H.block<2, 2>(row, col).sum();
1201  }
1202  }
1203  }
1204 
1205  return sumOffBlocks / H.asEigen()
1206  .block(
1208  H.rows() - get_vehicle_size(),
1209  H.cols() - get_vehicle_size())
1210  .sum(); // Starting (7,7)-end
1211  MRPT_END
1212 }
1213 
1214 /*---------------------------------------------------------------
1215  reconsiderPartitionsNow
1216  ---------------------------------------------------------------*/
1218 {
1219  // A different buffer for making this
1220  // thread-safe some day...
1221  vector<std::vector<uint32_t>> partitions;
1222  mapPartitioner.updatePartitions(partitions);
1223  m_lastPartitionSet = partitions;
1224 }
1225 
1226 /*---------------------------------------------------------------
1227  saveMapAndPathRepresentationAsMATLABFile
1228  ---------------------------------------------------------------*/
1230  const string& fil, float stdCount, const string& styleLandmarks,
1231  const string& stylePath, const string& styleRobot) const
1232 {
1233  FILE* f = os::fopen(fil.c_str(), "wt");
1234  if (!f) return;
1235 
1236  CMatrixDouble cov(2, 2);
1237  CVectorDouble mean(2);
1238 
1239  // Header:
1240  os::fprintf(
1241  f,
1242  "%%--------------------------------------------------------------------"
1243  "\n");
1244  os::fprintf(f, "%% File automatically generated using the MRPT method:\n");
1245  os::fprintf(
1246  f,
1247  "%% "
1248  "'CRangeBearingKFSLAM::saveMapAndPath2DRepresentationAsMATLABFile'\n");
1249  os::fprintf(f, "%%\n");
1250  os::fprintf(f, "%% ~ MRPT ~\n");
1251  os::fprintf(
1252  f, "%% Jose Luis Blanco Claraco, University of Malaga @ 2008\n");
1253  os::fprintf(f, "%% https://www.mrpt.org/ \n");
1254  os::fprintf(
1255  f,
1256  "%%--------------------------------------------------------------------"
1257  "\n");
1258 
1259  // Main code:
1260  os::fprintf(f, "hold on;\n\n");
1261 
1262  const size_t nLMs = this->getNumberOfLandmarksInTheMap();
1263 
1264  for (size_t i = 0; i < nLMs; i++)
1265  {
1266  size_t idx = get_vehicle_size() + i * get_feature_size();
1267 
1268  cov(0, 0) = m_pkk(idx + 0, idx + 0);
1269  cov(1, 1) = m_pkk(idx + 1, idx + 1);
1270  cov(0, 1) = cov(1, 0) = m_pkk(idx + 0, idx + 1);
1271 
1272  mean[0] = m_xkk[idx + 0];
1273  mean[1] = m_xkk[idx + 1];
1274 
1275  // Command to draw the 2D ellipse:
1276  os::fprintf(
1277  f, "%s",
1278  math::MATLAB_plotCovariance2D(cov, mean, stdCount, styleLandmarks)
1279  .c_str());
1280  }
1281 
1282  // Now: the robot path:
1283  // ------------------------------
1284  if (m_SFs.size())
1285  {
1286  os::fprintf(f, "\nROB_PATH=[");
1287  for (size_t i = 0; i < m_SFs.size(); i++)
1288  {
1289  CSensoryFrame::Ptr dummySF;
1290  CPose3DPDF::Ptr pdf3D;
1291  m_SFs.get(i, pdf3D, dummySF);
1292 
1293  CPose3D p;
1294  pdf3D->getMean(p);
1295 
1296  os::fprintf(f, "%.04f %.04f", p.x(), p.y());
1297  if (i < (m_SFs.size() - 1)) os::fprintf(f, ";");
1298  }
1299  os::fprintf(f, "];\n");
1300 
1301  os::fprintf(
1302  f, "plot(ROB_PATH(:,1),ROB_PATH(:,2),'%s');\n", stylePath.c_str());
1303  }
1304 
1305  // The robot pose:
1306  cov(0, 0) = m_pkk(0, 0);
1307  cov(1, 1) = m_pkk(1, 1);
1308  cov(0, 1) = cov(1, 0) = m_pkk(0, 1);
1309 
1310  mean[0] = m_xkk[0];
1311  mean[1] = m_xkk[1];
1312 
1313  os::fprintf(
1314  f, "%s",
1315  math::MATLAB_plotCovariance2D(cov, mean, stdCount, styleRobot).c_str());
1316 
1317  os::fprintf(f, "\naxis equal;\n");
1318  os::fclose(f);
1319 }
1320 
1321 /** Computes A=A-B, which may need to be re-implemented depending on the
1322  * topology of the individual scalar components (eg, angles).
1323  */
1325  KFArray_OBS& A, const KFArray_OBS& B) const
1326 {
1327  A -= B;
1330 }
1331 
1332 /** Return the observation NOISE covariance matrix, that is, the model of the
1333  * Gaussian additive noise of the sensor.
1334  * \param out_R The noise covariance matrix. It might be non diagonal, but
1335  * it'll usually be.
1336  */
1338 {
1339  out_R(0, 0) = square(options.std_sensor_range);
1340  out_R(1, 1) = square(options.std_sensor_yaw);
1341  out_R(2, 2) = square(options.std_sensor_pitch);
1342 }
1343 
1344 /** This will be called before OnGetObservationsAndDataAssociation to allow the
1345  * application to reduce the number of covariance landmark predictions to be
1346  * made.
1347  * For example, features which are known to be "out of sight" shouldn't be
1348  * added to the output list to speed up the calculations.
1349  * \param in_all_prediction_means The mean of each landmark predictions; the
1350  * computation or not of the corresponding covariances is what we're trying to
1351  * determined with this method.
1352  * \param out_LM_indices_to_predict The list of landmark indices in the map
1353  * [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.
1354  * \note This is not a pure virtual method, so it should be implemented only if
1355  * desired. The default implementation returns a vector with all the landmarks
1356  * in the map.
1357  * \sa OnGetObservations, OnDataAssociation
1358  */
1360  const vector_KFArray_OBS& prediction_means,
1361  std::vector<size_t>& out_LM_indices_to_predict) const
1362 {
1364  m_SF->getObservationByClass<CObservationBearingRange>();
1365  ASSERTMSG_(
1366  obs,
1367  "*ERROR*: This method requires an observation of type "
1368  "CObservationBearingRange");
1369 
1370 #define USE_HEURISTIC_PREDICTION
1371 
1372 #ifdef USE_HEURISTIC_PREDICTION
1373  const double sensor_max_range = obs->maxSensorDistance;
1374  const double fov_yaw = obs->fieldOfView_yaw;
1375  const double fov_pitch = obs->fieldOfView_pitch;
1376 
1377  const double max_vehicle_loc_uncertainty =
1378  4 * std::sqrt(m_pkk(0, 0) + m_pkk(1, 1) + m_pkk(2, 2));
1379 #endif
1380 
1381  out_LM_indices_to_predict.clear();
1382  for (size_t i = 0; i < prediction_means.size(); i++)
1383  {
1384 #ifndef USE_HEURISTIC_PREDICTION
1385  out_LM_indices_to_predict.push_back(i);
1386 #else
1387  // Heuristic: faster but doesn't work always!
1388  if (prediction_means[i][0] <
1389  (15 + sensor_max_range + max_vehicle_loc_uncertainty +
1390  4 * options.std_sensor_range) &&
1391  fabs(prediction_means[i][1]) <
1392  (30.0_deg + 0.5 * fov_yaw + 4 * options.std_sensor_yaw) &&
1393  fabs(prediction_means[i][2]) <
1394  (30.0_deg + 0.5 * fov_pitch + 4 * options.std_sensor_pitch))
1395  {
1396  out_LM_indices_to_predict.push_back(i);
1397  }
1398 #endif
1399  }
1400 }
const_iterator end() const
Definition: bimap.h:48
void changeCoordinatesReference(const CPose3DQuat &newReferenceBase)
this = p (+) this.
void updatePartitions(std::vector< std::vector< uint32_t >> &partitions)
Recalculate the map/graph partitions.
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
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
void getCurrentRobotPose(mrpt::poses::CPose3DQuatPDFGaussian &out_robotPose) const
Returns the mean & the 7x7 covariance matrix of the robot 6D pose (with rotation as a quaternion)...
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
mrpt::math::CMatrixDynamic< kftype > Y_pred_means
void reset()
Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,0,0).
#define MRPT_START
Definition: exceptions.h:241
mrpt::poses::CPose3DQuat getIncrementFromOdometry() const
Return the last odometry, as a pose increment.
void getLastPartitionLandmarks(std::vector< std::vector< uint32_t >> &landmarksMembership) const
Return the partitioning of the landmarks in clusters accoring to the last partition.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
bool force_ignore_odometry
Whether to ignore the input odometry and behave as if there was no odometry at all (default: false) ...
uint64_t TLandmarkID
Unique IDs for landmarks.
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:32
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:286
void reconsiderPartitionsNow()
The partitioning of the entire map is recomputed again.
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.
CPoint3D mean
The mean value.
size_t countCommonElements(const CONTAINER1 &a, const CONTAINER2 &b)
Counts the number of elements that appear in both STL-like containers (comparison through the == oper...
const_iterator find_key(const KEY &k) const
Definition: bimap.h:140
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.
void OnObservationJacobians(size_t idx_landmark_to_predict, KFMatrix_OxV &Hx, KFMatrix_OxF &Hy) const override
Implements the observation Jacobians and (when applicable) .
This file implements several operations that operate element-wise on individual or pairs of container...
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))
void OnObservationModel(const std::vector< size_t > &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const override
mrpt::math::CMatrixDouble77 cov
The 7x7 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...
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
STL namespace.
void sphericalCoordinates(const mrpt::math::TPoint3D &point, double &out_range, double &out_yaw, double &out_pitch, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacob_dryp_dpoint=nullptr, mrpt::math::CMatrixFixed< double, 3, 7 > *out_jacob_dryp_dpose=nullptr) const
Computes the spherical coordinates of a 3D point as seen from the 6D pose specified by this object...
void runOneKalmanIteration()
The main entry point, executes one complete step: prediction + update.
void OnTransitionModel(const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const override
Implements the transition model .
KFMatrix m_pkk
The system full covariance matrix.
uint32_t addMapFrame(const mrpt::obs::CSensoryFrame &frame, const mrpt::poses::CPose3DPDF &robotPose3D)
Insert a new keyframe to the graph.
void OnNormalizeStateVector() override
This method is called after the prediction and after the update, to give the user an opportunity to n...
mrpt::poses::CPose3DQuat getCurrentRobotPoseMean() const
Get the current robot pose mean, as a 3D+quaternion pose.
mrpt::math::CMatrixFixed< double, FEAT_SIZE, OBS_SIZE > KFMatrix_FxO
TDataAssocInfo m_last_data_association
Last data association.
mrpt::containers::bimap< mrpt::maps::CLandmark::TLandmarkID, unsigned int > m_IDs
The mapping between landmark IDs and indexes in the Pkk cov.
void clear()
Clear the contents of the bi-map.
Definition: bimap.h:64
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
size_t size() const
Definition: bimap.h:57
int64_t TLandmarkID
The type for the IDs of landmarks.
Definition: CLandmark.h:41
ENUMTYPE read_enum(const std::string &section, const std::string &name, const ENUMTYPE &defaultValue, bool failIfNotFound=false) const
Reads an "enum" value, where the value in the config file can be either a numerical value or the symb...
void composePoint(const double lx, const double ly, const double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixed< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixed< double, 3, 7 > *out_jacobian_df_dpose=nullptr) const
Computes the 3D point G such as .
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
Represents a probabilistic 3D (6D) movement.
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 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 bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
Definition: bimap.h:31
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.
A helper class that can convert an enum value into its textual representation, and viceversa...
int partitioningMethod
Applicable only if "doPartitioningExperiment=true".
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.
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
void assign(const std::size_t N, const Scalar value)
This namespace contains representation of robot actions and observations.
CIncrementalMapPartitioner mapPartitioner
Used for map partitioning experiments.
TDataAssociationMetric data_assoc_IC_metric
Whether to use mahalanobis (->chi2 criterion) vs.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
double computeOffDiagonalBlocksApproximationError(const std::vector< std::vector< uint32_t >> &landmarksMembership) const
Computes the ratio of the missing information matrix elements which are ignored under a certain parti...
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
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:45
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
mrpt::maps::CSimpleMap m_SFs
The sequence of all the observations and the robot path (kept for debugging, statistics,etc)
size_type cols() const
Number of columns in the matrix.
double data_assoc_IC_ml_threshold
Only if data_assoc_IC_metric==ML, the log-ML threshold (Default=0.0)
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
std::vector< std::vector< uint32_t > > m_lastPartitionSet
mrpt::math::CMatrixFixed< double, FEAT_SIZE, VEH_SIZE > KFMatrix_FxV
mrpt::math::CVectorFixed< double, FEAT_SIZE > KFArray_FEAT
void OnGetAction(KFArray_ACT &out_u) const override
Must return the action vector u.
void loadOptions(const mrpt::config::CConfigFileBase &ini)
Load options from a ini-like file/text.
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:419
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...
void OnGetObservationNoise(KFMatrix_OxO &out_R) const override
Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of ...
return_t square(const num_t x)
Inline function for the square of a number.
#define MRPT_LOAD_CONFIG_VAR( variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::obs::CSensoryFrame::Ptr m_SF
Set up by processActionObservation.
void getCurrentState(mrpt::poses::CPose3DQuatPDFGaussian &out_robotPose, std::vector< mrpt::math::TPoint3D > &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.
mrpt::slam::CRangeBearingKFSLAM::TOptions options
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.
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
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
void OnTransitionNoise(KFMatrix_VxV &out_Q) const override
Implements the transition noise covariance .
static Ptr Create(Args &&... args)
Definition: CEllipsoid3D.h:42
#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...
void getLastPartitionLandmarksAsIfFixedSubmaps(size_t K, std::vector< std::vector< uint32_t >> &landmarksMembership)
For testing only: returns the partitioning as "getLastPartitionLandmarks" but as if a fixed-size subm...
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
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.
This observation represents a number of range-bearing value pairs, each one for a detected landmark...
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...
double mean(const CONTAINER &v)
Computes the mean value of a vector.
bool inverse(const VALUE &v, KEY &out_k) const
Get the key associated the given value, VALUE->KEY, returning false if not present.
Definition: bimap.h:120
mrpt::math::CVectorFixedDouble< 3 > m_coords
The translation vector [x,y,z].
Definition: CPose3DQuat.h:52
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
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...
mrpt::math::CMatrixDynamic< kftype > Y_pred_covs
bool create_simplemap
Whether to fill m_SFs (default=false)
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:268
bool doPartitioningExperiment
If set to true (default=false), map will be partitioned using the method stated by partitioningMethod...
mrpt::math::CQuaternionDouble m_quat
The quaternion.
Definition: CPose3DQuat.h:54
std::string MATLAB_plotCovariance2D(const CMatrixFloat &cov22, const CVectorFloat &mean, float stdCount, const std::string &style=std::string("b"), size_t nEllipsePoints=30)
Generates a string with the MATLAB commands required to plot an confidence interval (ellipse) for a 2...
Definition: math.cpp:356
void resize(std::size_t N, bool zeroNewElements=false)
mrpt::math::CVectorFixed< double, OBS_SIZE > KFArray_OBS
void OnTransitionJacobian(KFMatrix_VxV &out_F) const override
Implements the transition Jacobian .
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
mrpt::obs::CActionCollection::Ptr m_action
Set up by processActionObservation.
TDataAssociationMethod
Different algorithms for data association, used in mrpt::slam::data_association.
~CRangeBearingKFSLAM() override
Destructor:
mrpt::math::CMatrixFixed< double, OBS_SIZE, VEH_SIZE > KFMatrix_OxV
double data_assoc_IC_chi2_thres
Threshold in [0,1] for the chi2square test for individual compatibility between predictions and obser...
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 clear()
Remove all stored pairs.
Definition: CSimpleMap.cpp:55
A gaussian distribution for 3D points.
mrpt::math::CMatrixFixed< double, OBS_SIZE, OBS_SIZE > KFMatrix_OxO
void read_vector(const std::string &section, const std::string &name, const VECTOR_TYPE &defaultValue, VECTOR_TYPE &outValues, bool failIfNotFound=false) const
Reads a configuration parameter of type vector, stored in the file as a string: "[v1 v2 v3 ...
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...
mrpt::math::CVectorFloat stds_Q_no_odo
A 7-length vector with the std.
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...



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020