MRPT  1.9.9
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-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "slam-precomp.h" // Precompiled headers
11 
12 // ----------------------------------------------------------------------------------------
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>
22 #include <mrpt/opengl/CEllipsoid.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  const size_t& idx_landmark_to_predict, KFMatrix_OxV& Hx,
491  KFMatrix_OxF& Hy) const
492 {
493  MRPT_START
494 
495  // Mean of the prior of the robot pose:
496  const CPose3DQuat robotPose = getCurrentRobotPoseMean();
497 
498  // Get the sensor pose relative to the robot:
500  m_SF->getObservationByClass<CObservationBearingRange>();
501  ASSERTMSG_(
502  obs,
503  "*ERROR*: This method requires an observation of type "
504  "CObservationBearingRange");
505  const CPose3DQuat sensorPoseOnRobot =
506  CPose3DQuat(obs->sensorLocationOnRobot);
507 
508  const size_t vehicle_size = get_vehicle_size();
509  // const size_t obs_size = get_observation_size();
510  const size_t feature_size = get_feature_size();
511 
512  // Compute the jacobians, needed below:
513  // const CPose3DQuat sensorPoseAbs= robotPose + sensorPoseOnRobot;
514  CPose3DQuat sensorPoseAbs(UNINITIALIZED_QUATERNION);
516  CMatrixFixed<kftype, 7, 7> H_senpose_senrelpose(
517  UNINITIALIZED_MATRIX); // Not actually used
518 
519  CPose3DQuatPDF::jacobiansPoseComposition(
520  robotPose, sensorPoseOnRobot, H_senpose_vehpose, H_senpose_senrelpose,
521  &sensorPoseAbs);
522 
523  const size_t row_in = feature_size * idx_landmark_to_predict;
524 
525  // Landmark absolute 3D position in the map:
526  const TPoint3D mapEst(
527  m_xkk[vehicle_size + row_in + 0], m_xkk[vehicle_size + row_in + 1],
528  m_xkk[vehicle_size + row_in + 2]);
529 
530  // The Jacobian wrt the sensor pose must be transformed later on:
531  KFMatrix_OxV Hx_sensor;
532  double obsData[3];
533  sensorPoseAbs.sphericalCoordinates(
534  mapEst,
535  obsData[0], // range
536  obsData[1], // yaw
537  obsData[2], // pitch
538  &Hy, &Hx_sensor);
539 
540  // Chain rule: Hx = d sensorpose / d vehiclepose * Hx_sensor
541  Hx = Hx_sensor * H_senpose_vehpose;
542 
543  MRPT_END
544 }
545 
546 /** This is called between the KF prediction step and the update step, and the
547  * application must return the observations and, when applicable, the data
548  * association between these observations and the current map.
549  *
550  * \param out_z N vectors, each for one "observation" of length OBS_SIZE, N
551  * being the number of "observations": how many observed landmarks for a map, or
552  * just one if not applicable.
553  * \param out_data_association An empty vector or, where applicable, a vector
554  * where the i'th element corresponds to the position of the observation in the
555  * i'th row of out_z within the system state vector (in the range
556  * [0,getNumberOfLandmarksInTheMap()-1]), or -1 if it is a new map element and
557  * we want to insert it at the end of this KF iteration.
558  * \param in_S The full covariance matrix of the observation predictions (i.e.
559  * the "innovation covariance matrix"). This is a M·O x M·O matrix with M=length
560  * of "in_lm_indices_in_S".
561  * \param in_lm_indices_in_S The indices of the map landmarks (range
562  * [0,getNumberOfLandmarksInTheMap()-1]) that can be found in the matrix in_S.
563  *
564  * This method will be called just once for each complete KF iteration.
565  * \note It is assumed that the observations are independent, i.e. there are NO
566  * cross-covariances between them.
567  */
569  vector_KFArray_OBS& Z, std::vector<int>& data_association,
570  const vector_KFArray_OBS& all_predictions, const KFMatrix& S,
571  const std::vector<size_t>& lm_indices_in_S, const KFMatrix_OxO& R)
572 {
573  MRPT_START
574 
575  // static const size_t vehicle_size = get_vehicle_size();
576  constexpr size_t obs_size = get_observation_size();
577 
578  // Z: Observations
579  CObservationBearingRange::TMeasurementList::const_iterator itObs;
580 
582  m_SF->getObservationByClass<CObservationBearingRange>();
583  ASSERTMSG_(
584  obs,
585  "*ERROR*: This method requires an observation of type "
586  "CObservationBearingRange");
587 
588  const size_t N = obs->sensedData.size();
589  Z.resize(N);
590 
591  size_t row;
592  for (row = 0, itObs = obs->sensedData.begin();
593  itObs != obs->sensedData.end(); ++itObs, ++row)
594  {
595  // Fill one row in Z:
596  Z[row][0] = itObs->range;
597  Z[row][1] = itObs->yaw;
598  Z[row][2] = itObs->pitch;
599  }
600 
601  // Data association:
602  // ---------------------
603  data_association.assign(N, -1); // Initially, all new landmarks
604 
605  // For each observed LM:
606  std::vector<size_t> obs_idxs_needing_data_assoc;
607  obs_idxs_needing_data_assoc.reserve(N);
608 
609  {
610  std::vector<int>::iterator itDA;
611  for (row = 0, itObs = obs->sensedData.begin(),
612  itDA = data_association.begin();
613  itObs != obs->sensedData.end(); ++itObs, ++itDA, ++row)
614  {
615  // Fill data asociation: Using IDs!
616  if (itObs->landmarkID < 0)
617  obs_idxs_needing_data_assoc.push_back(row);
618  else
619  {
621  CLandmark::TLandmarkID, unsigned int>::iterator itID;
622  if ((itID = m_IDs.find_key(itObs->landmarkID)) != m_IDs.end())
623  *itDA = itID->second; // This row in Z corresponds to the
624  // i'th map element in the state
625  // vector:
626  }
627  }
628  }
629 
630  // ---- Perform data association ----
631  // Only for observation indices in "obs_idxs_needing_data_assoc"
632  if (obs_idxs_needing_data_assoc.empty())
633  {
634  // We don't need to do DA:
636 
637  // Save them for the case the external user wants to access them:
638  for (size_t idxObs = 0; idxObs < data_association.size(); idxObs++)
639  {
640  int da = data_association[idxObs];
641  if (da >= 0)
643  data_association[idxObs];
644  }
645  }
646  else
647  {
648  // Build a Z matrix with the observations that need dat.assoc:
649  const size_t nObsDA = obs_idxs_needing_data_assoc.size();
650 
651  CMatrixDynamic<kftype> Z_obs_means(nObsDA, obs_size);
652  for (size_t i = 0; i < nObsDA; i++)
653  {
654  const size_t idx = obs_idxs_needing_data_assoc[i];
655  for (unsigned k = 0; k < obs_size; k++)
656  Z_obs_means(i, k) = Z[idx][k];
657  }
658 
659  // Vehicle uncertainty
660  // KFMatrix_VxV Pxx = m_pkk.extractMatrix<7, 7>(0, 0);
661 
662  // Build predictions:
663  // ---------------------------
664  const size_t nPredictions = lm_indices_in_S.size();
666 
667  // S is the covariance of the predictions:
669 
670  // The means:
671  m_last_data_association.Y_pred_means.setSize(nPredictions, obs_size);
672  for (size_t q = 0; q < nPredictions; q++)
673  {
674  const size_t i = lm_indices_in_S[q];
675  for (size_t w = 0; w < obs_size; w++)
677  all_predictions[i][w];
679  i); // for the conversion of indices...
680  }
681 
682  // Do Dat. Assoc :
683  // ---------------------------
684  if (nPredictions)
685  {
686  // CMatrixDouble Z_obs_cov = CMatrixDouble(R);
687 
693  true, // Use KD-tree
697 
698  // Return pairings to the main KF algorithm:
699  for (auto it = m_last_data_association.results.associations.begin();
700  it != m_last_data_association.results.associations.end(); ++it)
701  data_association[it->first] = it->second;
702  }
703  }
704  // ---- End of data association ----
705 
706  MRPT_END
707 }
708 
709 /** This virtual function musts normalize the state vector and covariance matrix
710  * (only if its necessary).
711  */
713 {
714  MRPT_START
715 
716  // m_xkk[3:6] must be a normalized quaternion:
717  const double T = std::sqrt(
718  square(m_xkk[3]) + square(m_xkk[4]) + square(m_xkk[5]) +
719  square(m_xkk[6]));
720  ASSERTMSG_(T > 0, "Vehicle pose quaternion norm is not >0!!");
721 
722  const double T_ = (m_xkk[3] < 0 ? -1.0 : 1.0) / T; // qr>=0
723  m_xkk[3] *= T_;
724  m_xkk[4] *= T_;
725  m_xkk[5] *= T_;
726  m_xkk[6] *= T_;
727 
728  MRPT_END
729 }
730 
731 /*---------------------------------------------------------------
732  loadOptions
733  ---------------------------------------------------------------*/
735 {
736  // Main
737  options.loadFromConfigFile(ini, "RangeBearingKFSLAM");
738  KF_options.loadFromConfigFile(ini, "RangeBearingKFSLAM_KalmanFilter");
739  // partition algorithm:
740  mapPartitioner.options.loadFromConfigFile(ini, "RangeBearingKFSLAM");
741 }
742 
743 /*---------------------------------------------------------------
744  loadOptions
745  ---------------------------------------------------------------*/
747  const mrpt::config::CConfigFileBase& source, const std::string& section)
748 {
749  source.read_vector(section, "stds_Q_no_odo", stds_Q_no_odo, stds_Q_no_odo);
750  ASSERT_(stds_Q_no_odo.size() == 7);
752  source.read_float(section, "std_sensor_range", std_sensor_range);
753  std_sensor_yaw = DEG2RAD(source.read_float(
754  section, "std_sensor_yaw_deg", RAD2DEG(std_sensor_yaw)));
755  std_sensor_pitch = DEG2RAD(source.read_float(
756  section, "std_sensor_pitch_deg", RAD2DEG(std_sensor_pitch)));
757 
758  std_odo_z_additional = source.read_float(
759  section, "std_odo_z_additional", std_odo_z_additional);
760 
763 
765 
767 
769  section, "data_assoc_method", data_assoc_method);
771  section, "data_assoc_metric", data_assoc_metric);
773  section, "data_assoc_IC_metric", data_assoc_IC_metric);
774 
777 
779 }
780 
781 /*---------------------------------------------------------------
782  Constructor
783  ---------------------------------------------------------------*/
785  : stds_Q_no_odo(get_vehicle_size(), 0),
786 
787  std_sensor_yaw(DEG2RAD(0.2f)),
788  std_sensor_pitch(DEG2RAD(0.2f))
789 
790 {
791  stds_Q_no_odo[0] = stds_Q_no_odo[1] = stds_Q_no_odo[2] = 0.10f;
793  0.05f;
794 }
795 
796 /*---------------------------------------------------------------
797  dumpToTextStream
798  ---------------------------------------------------------------*/
800 {
801  using namespace mrpt::typemeta;
802 
803  out << "\n----------- [CRangeBearingKFSLAM::TOptions] ------------ \n\n";
804 
805  out << mrpt::format(
806  "doPartitioningExperiment = %c\n",
807  doPartitioningExperiment ? 'Y' : 'N');
808  out << mrpt::format(
809  "partitioningMethod = %i\n", partitioningMethod);
810  out << mrpt::format(
811  "data_assoc_method = %s\n",
813  .c_str());
814  out << mrpt::format(
815  "data_assoc_metric = %s\n",
817  .c_str());
818  out << mrpt::format(
819  "data_assoc_IC_chi2_thres = %.06f\n",
820  data_assoc_IC_chi2_thres);
821  out << mrpt::format(
822  "data_assoc_IC_metric = %s\n",
824  .c_str());
825  out << mrpt::format(
826  "data_assoc_IC_ml_threshold = %.06f\n",
827  data_assoc_IC_ml_threshold);
828 
829  out << "\n";
830 }
831 
833  const KFArray_OBS& in_z, KFArray_FEAT& yn, KFMatrix_FxV& dyn_dxv,
834  KFMatrix_FxO& dyn_dhn) const
835 {
836  MRPT_START
837 
839  m_SF->getObservationByClass<CObservationBearingRange>();
840  ASSERTMSG_(
841  obs,
842  "*ERROR*: This method requires an observation of type "
843  "CObservationBearingRange");
844  const CPose3DQuat sensorPoseOnRobot =
845  CPose3DQuat(obs->sensorLocationOnRobot);
846 
847  // Mean of the prior of the robot pose:
848  const CPose3DQuat robotPose = getCurrentRobotPoseMean();
849 
850  // const CPose3DQuat sensorPoseAbs= robotPose + sensorPoseOnRobot;
851  CPose3DQuat sensorPoseAbs(UNINITIALIZED_QUATERNION);
853  CMatrixFixed<kftype, 7, 7> dsensorabs_dsenrelpose(
854  UNINITIALIZED_MATRIX); // Not actually used
855 
856  CPose3DQuatPDF::jacobiansPoseComposition(
857  robotPose, sensorPoseOnRobot, dsensorabs_dvehpose,
858  dsensorabs_dsenrelpose, &sensorPoseAbs);
859 
860  kftype hn_r = in_z[0];
861  kftype hn_y = in_z[1];
862  kftype hn_p = in_z[2];
863 
864  kftype chn_y = cos(hn_y);
865  kftype shn_y = sin(hn_y);
866  kftype chn_p = cos(hn_p);
867  kftype shn_p = sin(hn_p);
868 
869  /* -------------------------------------------
870  syms H h_range h_yaw h_pitch real;
871  H=[ h_range ; h_yaw ; h_pitch ];
872 
873  syms X xi_ yi_ zi_ real;
874  xi_ = h_range * cos(h_yaw) * cos(h_pitch);
875  yi_ = h_range * sin(h_yaw) * cos(h_pitch);
876  zi_ = -h_range * sin(h_pitch);
877 
878  X=[xi_ yi_ zi_];
879  jacob_inv_mod=jacobian(X,H)
880  ------------------------------------------- */
881 
882  // The new point, relative to the sensor:
883  const TPoint3D yn_rel_sensor(
884  hn_r * chn_y * chn_p, hn_r * shn_y * chn_p, -hn_r * shn_p);
885 
886  // The Jacobian of the 3D point in the coordinate system of the sensor:
887  /*
888  [ cos(h_pitch)*cos(h_yaw), -h_range*cos(h_pitch)*sin(h_yaw),
889  -h_range*cos(h_yaw)*sin(h_pitch)]
890  [ cos(h_pitch)*sin(h_yaw), h_range*cos(h_pitch)*cos(h_yaw),
891  -h_range*sin(h_pitch)*sin(h_yaw)]
892  [ -sin(h_pitch), 0,
893  -h_range*cos(h_pitch)]
894  */
895  const kftype values_dynlocal_dhn[] = {chn_p * chn_y,
896  -hn_r * chn_p * shn_y,
897  -hn_r * chn_y * shn_p,
898  chn_p * shn_y,
899  hn_r * chn_p * chn_y,
900  -hn_r * shn_p * shn_y,
901  -shn_p,
902  0,
903  -hn_r * chn_p};
904  const KFMatrix_FxO dynlocal_dhn(values_dynlocal_dhn);
905 
906  KFMatrix_FxF jacob_dyn_dynrelsensor(UNINITIALIZED_MATRIX);
907  KFMatrix_FxV jacob_dyn_dsensorabs(UNINITIALIZED_MATRIX);
908 
909  sensorPoseAbs.composePoint(
910  yn_rel_sensor.x, yn_rel_sensor.y,
911  yn_rel_sensor.z, // yn rel. to the sensor
912  yn[0], yn[1], yn[2], // yn in global coords
913  &jacob_dyn_dynrelsensor, &jacob_dyn_dsensorabs);
914 
915  dyn_dhn = jacob_dyn_dynrelsensor * dynlocal_dhn;
916 
917  // dyn_dxv =
918  dyn_dxv = jacob_dyn_dsensorabs * dsensorabs_dvehpose;
919 
920  MRPT_END
921 }
922 
923 /** If applicable to the given problem, do here any special handling of adding a
924  * new landmark to the map.
925  * \param in_obsIndex The index of the observation whose inverse sensor is to
926  * be computed. It corresponds to the row in in_z where the observation can be
927  * found.
928  * \param in_idxNewFeat The index that this new feature will have in the state
929  * vector (0:just after the vehicle state, 1: after that,...). Save this number
930  * so data association can be done according to these indices.
931  * \sa OnInverseObservationModel
932  */
934  const size_t in_obsIdx, const size_t in_idxNewFeat)
935 {
936  MRPT_START
937 
939  m_SF->getObservationByClass<CObservationBearingRange>();
940  ASSERTMSG_(
941  obs,
942  "*ERROR*: This method requires an observation of type "
943  "CObservationBearingRange");
944 
945  // ----------------------------------------------
946  // introduce in the lists of ID<->index in map:
947  // ----------------------------------------------
948  ASSERT_(in_obsIdx < obs->sensedData.size());
949  if (obs->sensedData[in_obsIdx].landmarkID >= 0)
950  {
951  // The sensor provides us a LM ID... use it:
952  m_IDs.insert(obs->sensedData[in_obsIdx].landmarkID, in_idxNewFeat);
953  }
954  else
955  {
956  // Features do not have IDs... use indices:
957  m_IDs.insert(in_idxNewFeat, in_idxNewFeat);
958  }
959 
960  MRPT_END
961 }
962 
963 /*---------------------------------------------------------------
964  getAs3DObject
965  ---------------------------------------------------------------*/
967  mrpt::opengl::CSetOfObjects::Ptr& outObj) const
968 {
969  outObj->clear();
970 
971  // ------------------------------------------------
972  // Add the XYZ corner for the current area:
973  // ------------------------------------------------
974  outObj->insert(opengl::stock_objects::CornerXYZ());
975 
976  // 3D ellipsoid for robot pose:
977  CPointPDFGaussian pointGauss;
978  pointGauss.mean.x(m_xkk[0]);
979  pointGauss.mean.y(m_xkk[1]);
980  pointGauss.mean.z(m_xkk[2]);
981  pointGauss.cov = m_pkk.blockCopy<3, 3>(0, 0);
982 
983  {
984  auto ellip = opengl::CEllipsoid::Create();
985 
986  ellip->setPose(pointGauss.mean);
987  ellip->setCovMatrix(pointGauss.cov);
988  ellip->enableDrawSolid3D(false);
989  ellip->setQuantiles(options.quantiles_3D_representation);
990  ellip->set3DsegmentsCount(10);
991  ellip->setColor(1, 0, 0);
992 
993  outObj->insert(ellip);
994  }
995 
996  // 3D ellipsoids for landmarks:
997  const size_t nLMs = this->getNumberOfLandmarksInTheMap();
998  for (size_t i = 0; i < nLMs; i++)
999  {
1000  pointGauss.mean.x(
1001  m_xkk[get_vehicle_size() + get_feature_size() * i + 0]);
1002  pointGauss.mean.y(
1003  m_xkk[get_vehicle_size() + get_feature_size() * i + 1]);
1004  pointGauss.mean.z(
1005  m_xkk[get_vehicle_size() + get_feature_size() * i + 2]);
1006 
1007  pointGauss.cov =
1011 
1012  auto ellip = opengl::CEllipsoid::Create();
1013 
1014  ellip->setName(format("%u", static_cast<unsigned int>(i)));
1015  ellip->enableShowName(true);
1016  ellip->setPose(pointGauss.mean);
1017  ellip->setCovMatrix(pointGauss.cov);
1018  ellip->enableDrawSolid3D(false);
1019  ellip->setQuantiles(options.quantiles_3D_representation);
1020  ellip->set3DsegmentsCount(10);
1021 
1022  ellip->setColor(0, 0, 1);
1023 
1024  // Set color depending on partitions?
1026  {
1027  // This is done for each landmark:
1028  map<int, bool> belongToPartition;
1029  const CSimpleMap* SFs =
1030  &m_SFs; // mapPartitioner.getSequenceOfFrames();
1031 
1032  for (size_t p = 0; p < m_lastPartitionSet.size(); p++)
1033  {
1034  for (size_t w = 0; w < m_lastPartitionSet[p].size(); w++)
1035  {
1036  // Check if landmark #i is in the SF of
1037  // m_lastPartitionSet[p][w]:
1038  CLandmark::TLandmarkID i_th_ID = m_IDs.inverse(i);
1039 
1040  // Look for the lm_ID in the SF:
1041  CPose3DPDF::Ptr pdf;
1042  CSensoryFrame::Ptr SF_i;
1043  SFs->get(m_lastPartitionSet[p][w], pdf, SF_i);
1044 
1046  SF_i->getObservationByClass<CObservationBearingRange>();
1047 
1048  for (auto& o : obs->sensedData)
1049  {
1050  if (o.landmarkID == i_th_ID)
1051  {
1052  belongToPartition[p] = true;
1053  break;
1054  }
1055  } // end for o
1056  } // end for w
1057  } // end for p
1058 
1059  // Build label:
1060  string strParts("[");
1061 
1062  for (auto it = belongToPartition.begin();
1063  it != belongToPartition.end(); ++it)
1064  {
1065  if (it != belongToPartition.begin()) strParts += string(",");
1066  strParts += format("%i", it->first);
1067  }
1068 
1069  ellip->setName(ellip->getName() + strParts + string("]"));
1070  }
1071 
1072  outObj->insert(ellip);
1073  }
1074 }
1075 
1076 /*---------------------------------------------------------------
1077  getLastPartitionLandmarks
1078  ---------------------------------------------------------------*/
1080  size_t K, std::vector<std::vector<uint32_t>>& landmarksMembership)
1081 {
1082  // temporary copy:
1083  std::vector<std::vector<uint32_t>> tmpParts = m_lastPartitionSet;
1084 
1085  // Fake "m_lastPartitionSet":
1086 
1087  // Fixed partitions every K observations:
1088  vector<std::vector<uint32_t>> partitions;
1089  std::vector<uint32_t> tmpCluster;
1090 
1091  for (size_t i = 0; i < m_SFs.size(); i++)
1092  {
1093  tmpCluster.push_back(i);
1094  if ((i % K) == 0)
1095  {
1096  partitions.push_back(tmpCluster);
1097  tmpCluster.clear();
1098  tmpCluster.push_back(
1099  i); // This observation "i" is shared between both clusters
1100  }
1101  }
1102  m_lastPartitionSet = partitions;
1103 
1104  // Call the actual method:
1105  getLastPartitionLandmarks(landmarksMembership);
1106 
1107  // Replace copy:
1108  m_lastPartitionSet = tmpParts; //-V519
1109 }
1110 
1111 /*---------------------------------------------------------------
1112  getLastPartitionLandmarks
1113  ---------------------------------------------------------------*/
1115  std::vector<std::vector<uint32_t>>& landmarksMembership) const
1116 {
1117  landmarksMembership.clear();
1118 
1119  // All the computation is made based on "m_lastPartitionSet"
1120 
1121  if (!options.doPartitioningExperiment) return;
1122 
1123  const size_t nLMs = this->getNumberOfLandmarksInTheMap();
1124  for (size_t i = 0; i < nLMs; i++)
1125  {
1126  map<int, bool> belongToPartition;
1127  const CSimpleMap* SFs =
1128  &m_SFs; // mapPartitioner.getSequenceOfFrames();
1129 
1130  for (size_t p = 0; p < m_lastPartitionSet.size(); p++)
1131  {
1132  for (size_t w = 0; w < m_lastPartitionSet[p].size(); w++)
1133  {
1134  // Check if landmark #i is in the SF of
1135  // m_lastPartitionSet[p][w]:
1136  CLandmark::TLandmarkID i_th_ID = m_IDs.inverse(i);
1137 
1138  // Look for the lm_ID in the SF:
1139  CPose3DPDF::Ptr pdf;
1140  CSensoryFrame::Ptr SF_i;
1141  SFs->get(m_lastPartitionSet[p][w], pdf, SF_i);
1142 
1144  SF_i->getObservationByClass<CObservationBearingRange>();
1145 
1146  for (auto& o : obs->sensedData)
1147  {
1148  if (o.landmarkID == i_th_ID)
1149  {
1150  belongToPartition[p] = true;
1151  break;
1152  }
1153  } // end for o
1154  } // end for w
1155  } // end for p
1156 
1157  // Build membership list:
1158  std::vector<uint32_t> membershipOfThisLM;
1159 
1160  for (auto& it : belongToPartition)
1161  membershipOfThisLM.push_back(it.first);
1162 
1163  landmarksMembership.push_back(membershipOfThisLM);
1164  } // end for i
1165 }
1166 
1167 /*---------------------------------------------------------------
1168  computeOffDiagonalBlocksApproximationError
1169  ---------------------------------------------------------------*/
1171  const std::vector<std::vector<uint32_t>>& landmarksMembership) const
1172 {
1173  MRPT_START
1174 
1175  // Compute the information matrix:
1176  CMatrixDynamic<kftype> fullCov(m_pkk);
1177  size_t i;
1178  for (i = 0; i < get_vehicle_size(); i++)
1179  fullCov(i, i) = max(fullCov(i, i), 1e-6);
1180 
1181  CMatrixDynamic<kftype> H(fullCov.inverse_LLt());
1182  H.array().abs(); // Replace by absolute values:
1183 
1184  double sumOffBlocks = 0;
1185  unsigned int nLMs = landmarksMembership.size();
1186 
1187  ASSERT_(
1188  int(get_vehicle_size() + nLMs * get_feature_size()) == fullCov.cols());
1189 
1190  for (i = 0; i < nLMs; i++)
1191  {
1192  for (size_t j = i + 1; j < nLMs; j++)
1193  {
1194  // Sum the cross cov. between LM(i) and LM(j)??
1195  // --> Only if there is no common cluster:
1196  if (0 == math::countCommonElements(
1197  landmarksMembership[i], landmarksMembership[j]))
1198  {
1199  size_t col = get_vehicle_size() + i * get_feature_size();
1200  size_t row = get_vehicle_size() + j * get_feature_size();
1201  sumOffBlocks += 2 * H.block<2, 2>(row, col).sum();
1202  }
1203  }
1204  }
1205 
1206  return sumOffBlocks / H.asEigen()
1207  .block(
1209  H.rows() - get_vehicle_size(),
1210  H.cols() - get_vehicle_size())
1211  .sum(); // Starting (7,7)-end
1212  MRPT_END
1213 }
1214 
1215 /*---------------------------------------------------------------
1216  reconsiderPartitionsNow
1217  ---------------------------------------------------------------*/
1219 {
1220  // A different buffer for making this
1221  // thread-safe some day...
1222  vector<std::vector<uint32_t>> partitions;
1223  mapPartitioner.updatePartitions(partitions);
1224  m_lastPartitionSet = partitions;
1225 }
1226 
1227 /*---------------------------------------------------------------
1228  saveMapAndPathRepresentationAsMATLABFile
1229  ---------------------------------------------------------------*/
1231  const string& fil, float stdCount, const string& styleLandmarks,
1232  const string& stylePath, const string& styleRobot) const
1233 {
1234  FILE* f = os::fopen(fil.c_str(), "wt");
1235  if (!f) return;
1236 
1237  CMatrixDouble cov(2, 2);
1238  CVectorDouble mean(2);
1239 
1240  // Header:
1241  os::fprintf(
1242  f,
1243  "%%--------------------------------------------------------------------"
1244  "\n");
1245  os::fprintf(f, "%% File automatically generated using the MRPT method:\n");
1246  os::fprintf(
1247  f,
1248  "%% "
1249  "'CRangeBearingKFSLAM::saveMapAndPath2DRepresentationAsMATLABFile'\n");
1250  os::fprintf(f, "%%\n");
1251  os::fprintf(f, "%% ~ MRPT ~\n");
1252  os::fprintf(
1253  f, "%% Jose Luis Blanco Claraco, University of Malaga @ 2008\n");
1254  os::fprintf(f, "%% https://www.mrpt.org/ \n");
1255  os::fprintf(
1256  f,
1257  "%%--------------------------------------------------------------------"
1258  "\n");
1259 
1260  // Main code:
1261  os::fprintf(f, "hold on;\n\n");
1262 
1263  const size_t nLMs = this->getNumberOfLandmarksInTheMap();
1264 
1265  for (size_t i = 0; i < nLMs; i++)
1266  {
1267  size_t idx = get_vehicle_size() + i * get_feature_size();
1268 
1269  cov(0, 0) = m_pkk(idx + 0, idx + 0);
1270  cov(1, 1) = m_pkk(idx + 1, idx + 1);
1271  cov(0, 1) = cov(1, 0) = m_pkk(idx + 0, idx + 1);
1272 
1273  mean[0] = m_xkk[idx + 0];
1274  mean[1] = m_xkk[idx + 1];
1275 
1276  // Command to draw the 2D ellipse:
1277  os::fprintf(
1278  f, "%s",
1279  math::MATLAB_plotCovariance2D(cov, mean, stdCount, styleLandmarks)
1280  .c_str());
1281  }
1282 
1283  // Now: the robot path:
1284  // ------------------------------
1285  if (m_SFs.size())
1286  {
1287  os::fprintf(f, "\nROB_PATH=[");
1288  for (size_t i = 0; i < m_SFs.size(); i++)
1289  {
1290  CSensoryFrame::Ptr dummySF;
1291  CPose3DPDF::Ptr pdf3D;
1292  m_SFs.get(i, pdf3D, dummySF);
1293 
1294  CPose3D p;
1295  pdf3D->getMean(p);
1296 
1297  os::fprintf(f, "%.04f %.04f", p.x(), p.y());
1298  if (i < (m_SFs.size() - 1)) os::fprintf(f, ";");
1299  }
1300  os::fprintf(f, "];\n");
1301 
1302  os::fprintf(
1303  f, "plot(ROB_PATH(:,1),ROB_PATH(:,2),'%s');\n", stylePath.c_str());
1304  }
1305 
1306  // The robot pose:
1307  cov(0, 0) = m_pkk(0, 0);
1308  cov(1, 1) = m_pkk(1, 1);
1309  cov(0, 1) = cov(1, 0) = m_pkk(0, 1);
1310 
1311  mean[0] = m_xkk[0];
1312  mean[1] = m_xkk[1];
1313 
1314  os::fprintf(
1315  f, "%s",
1316  math::MATLAB_plotCovariance2D(cov, mean, stdCount, styleRobot).c_str());
1317 
1318  os::fprintf(f, "\naxis equal;\n");
1319  os::fclose(f);
1320 }
1321 
1322 /** Computes A=A-B, which may need to be re-implemented depending on the
1323  * topology of the individual scalar components (eg, angles).
1324  */
1326  KFArray_OBS& A, const KFArray_OBS& B) const
1327 {
1328  A -= B;
1331 }
1332 
1333 /** Return the observation NOISE covariance matrix, that is, the model of the
1334  * Gaussian additive noise of the sensor.
1335  * \param out_R The noise covariance matrix. It might be non diagonal, but
1336  * it'll usually be.
1337  */
1339 {
1340  out_R(0, 0) = square(options.std_sensor_range);
1341  out_R(1, 1) = square(options.std_sensor_yaw);
1342  out_R(2, 2) = square(options.std_sensor_pitch);
1343 }
1344 
1345 /** This will be called before OnGetObservationsAndDataAssociation to allow the
1346  * application to reduce the number of covariance landmark predictions to be
1347  * made.
1348  * For example, features which are known to be "out of sight" shouldn't be
1349  * added to the output list to speed up the calculations.
1350  * \param in_all_prediction_means The mean of each landmark predictions; the
1351  * computation or not of the corresponding covariances is what we're trying to
1352  * determined with this method.
1353  * \param out_LM_indices_to_predict The list of landmark indices in the map
1354  * [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.
1355  * \note This is not a pure virtual method, so it should be implemented only if
1356  * desired. The default implementation returns a vector with all the landmarks
1357  * in the map.
1358  * \sa OnGetObservations, OnDataAssociation
1359  */
1361  const vector_KFArray_OBS& prediction_means,
1362  std::vector<size_t>& out_LM_indices_to_predict) const
1363 {
1365  m_SF->getObservationByClass<CObservationBearingRange>();
1366  ASSERTMSG_(
1367  obs,
1368  "*ERROR*: This method requires an observation of type "
1369  "CObservationBearingRange");
1370 
1371 #define USE_HEURISTIC_PREDICTION
1372 
1373 #ifdef USE_HEURISTIC_PREDICTION
1374  const double sensor_max_range = obs->maxSensorDistance;
1375  const double fov_yaw = obs->fieldOfView_yaw;
1376  const double fov_pitch = obs->fieldOfView_pitch;
1377 
1378  const double max_vehicle_loc_uncertainty =
1379  4 * std::sqrt(m_pkk(0, 0) + m_pkk(1, 1) + m_pkk(2, 2));
1380 #endif
1381 
1382  out_LM_indices_to_predict.clear();
1383  for (size_t i = 0; i < prediction_means.size(); i++)
1384  {
1385 #ifndef USE_HEURISTIC_PREDICTION
1386  out_LM_indices_to_predict.push_back(i);
1387 #else
1388  // Heuristic: faster but doesn't work always!
1389  if (prediction_means[i][0] <
1390  (15 + sensor_max_range + max_vehicle_loc_uncertainty +
1391  4 * options.std_sensor_range) &&
1392  fabs(prediction_means[i][1]) <
1393  (30.0_deg + 0.5 * fov_yaw + 4 * options.std_sensor_yaw) &&
1394  fabs(prediction_means[i][2]) <
1395  (30.0_deg + 0.5 * fov_pitch + 4 * options.std_sensor_pitch))
1396  {
1397  out_LM_indices_to_predict.push_back(i);
1398  }
1399 #endif
1400  }
1401 }
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
std::string MATLAB_plotCovariance2D(const CMatrixFloat &cov22, const CVectorFloat &mean, const float &stdCount, const std::string &style=std::string("b"), const size_t &nEllipsePoints=30)
Generates a string with the MATLAB commands required to plot an confidence interval (ellipse) for a 2...
Definition: math.cpp:356
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.
double x
X,Y,Z coordinates.
Definition: TPoint3D.h:83
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:275
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...
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3727
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.
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...
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
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4199
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
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.
T square(const T x)
Inline function for the square of a number.
A helper class that can convert an enum value into its textual representation, and viceversa...
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)
GLsizei const GLchar ** string
Definition: glext.h:4116
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:410
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 ...
#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...
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
static Ptr Create(Args &&... args)
Definition: CEllipsoid.h:46
void OnTransitionNoise(KFMatrix_VxV &out_Q) const override
Implements the transition noise covariance .
#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...
GLenum GLenum GLvoid * row
Definition: glext.h:3580
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.
GLsizei GLsizei GLchar * source
Definition: glext.h:4097
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:257
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
void OnObservationJacobians(const size_t &idx_landmark_to_predict, KFMatrix_OxV &Hx, KFMatrix_OxF &Hy) const override
Implements the observation Jacobians and (when applicable) .
void resize(std::size_t N, bool zeroNewElements=false)
mrpt::math::CVectorFixed< double, OBS_SIZE > KFArray_OBS
Lightweight 3D point.
Definition: TPoint3D.h:90
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
GLfloat GLfloat p
Definition: glext.h:6398
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 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 1.9.9 Git: 9b18308f3 Mon Nov 18 23:39:25 2019 +0100 at lun nov 18 23:45:12 CET 2019