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  std::cout << "Z_obs_cov:\n" << Z_obs_cov << "\n";
688 
690  Z_obs_means, // Z_obs_cov,
695  true, // Use KD-tree
699 
700  // Return pairings to the main KF algorithm:
701  for (auto it = m_last_data_association.results.associations.begin();
702  it != m_last_data_association.results.associations.end(); ++it)
703  data_association[it->first] = it->second;
704  }
705  }
706  // ---- End of data association ----
707 
708  MRPT_END
709 }
710 
711 /** This virtual function musts normalize the state vector and covariance matrix
712  * (only if its necessary).
713  */
715 {
716  MRPT_START
717 
718  // m_xkk[3:6] must be a normalized quaternion:
719  const double T = std::sqrt(
720  square(m_xkk[3]) + square(m_xkk[4]) + square(m_xkk[5]) +
721  square(m_xkk[6]));
722  ASSERTMSG_(T > 0, "Vehicle pose quaternion norm is not >0!!");
723 
724  const double T_ = (m_xkk[3] < 0 ? -1.0 : 1.0) / T; // qr>=0
725  m_xkk[3] *= T_;
726  m_xkk[4] *= T_;
727  m_xkk[5] *= T_;
728  m_xkk[6] *= T_;
729 
730  MRPT_END
731 }
732 
733 /*---------------------------------------------------------------
734  loadOptions
735  ---------------------------------------------------------------*/
737 {
738  // Main
739  options.loadFromConfigFile(ini, "RangeBearingKFSLAM");
740  KF_options.loadFromConfigFile(ini, "RangeBearingKFSLAM_KalmanFilter");
741  // partition algorithm:
742  mapPartitioner.options.loadFromConfigFile(ini, "RangeBearingKFSLAM");
743 }
744 
745 /*---------------------------------------------------------------
746  loadOptions
747  ---------------------------------------------------------------*/
749  const mrpt::config::CConfigFileBase& source, const std::string& section)
750 {
751  source.read_vector(section, "stds_Q_no_odo", stds_Q_no_odo, stds_Q_no_odo);
752  ASSERT_(stds_Q_no_odo.size() == 7);
754  source.read_float(section, "std_sensor_range", std_sensor_range);
755  std_sensor_yaw = DEG2RAD(source.read_float(
756  section, "std_sensor_yaw_deg", RAD2DEG(std_sensor_yaw)));
757  std_sensor_pitch = DEG2RAD(source.read_float(
758  section, "std_sensor_pitch_deg", RAD2DEG(std_sensor_pitch)));
759 
760  std_odo_z_additional = source.read_float(
761  section, "std_odo_z_additional", std_odo_z_additional);
762 
765 
767 
769 
771  section, "data_assoc_method", data_assoc_method);
773  section, "data_assoc_metric", data_assoc_metric);
775  section, "data_assoc_IC_metric", data_assoc_IC_metric);
776 
779 
781 }
782 
783 /*---------------------------------------------------------------
784  Constructor
785  ---------------------------------------------------------------*/
787  : stds_Q_no_odo(get_vehicle_size(), 0),
788 
789  std_sensor_yaw(DEG2RAD(0.2f)),
790  std_sensor_pitch(DEG2RAD(0.2f))
791 
792 {
793  stds_Q_no_odo[0] = stds_Q_no_odo[1] = stds_Q_no_odo[2] = 0.10f;
795  0.05f;
796 }
797 
798 /*---------------------------------------------------------------
799  dumpToTextStream
800  ---------------------------------------------------------------*/
802 {
803  using namespace mrpt::typemeta;
804 
805  out << mrpt::format(
806  "\n----------- [CRangeBearingKFSLAM::TOptions] ------------ \n\n");
807 
808  out << mrpt::format(
809  "doPartitioningExperiment = %c\n",
810  doPartitioningExperiment ? 'Y' : 'N');
811  out << mrpt::format(
812  "partitioningMethod = %i\n", partitioningMethod);
813  out << mrpt::format(
814  "data_assoc_method = %s\n",
816  .c_str());
817  out << mrpt::format(
818  "data_assoc_metric = %s\n",
820  .c_str());
821  out << mrpt::format(
822  "data_assoc_IC_chi2_thres = %.06f\n",
823  data_assoc_IC_chi2_thres);
824  out << mrpt::format(
825  "data_assoc_IC_metric = %s\n",
827  .c_str());
828  out << mrpt::format(
829  "data_assoc_IC_ml_threshold = %.06f\n",
830  data_assoc_IC_ml_threshold);
831 
832  out << mrpt::format("\n");
833 }
834 
836  const KFArray_OBS& in_z, KFArray_FEAT& yn, KFMatrix_FxV& dyn_dxv,
837  KFMatrix_FxO& dyn_dhn) const
838 {
839  MRPT_START
840 
842  m_SF->getObservationByClass<CObservationBearingRange>();
843  ASSERTMSG_(
844  obs,
845  "*ERROR*: This method requires an observation of type "
846  "CObservationBearingRange");
847  const CPose3DQuat sensorPoseOnRobot =
848  CPose3DQuat(obs->sensorLocationOnRobot);
849 
850  // Mean of the prior of the robot pose:
851  const CPose3DQuat robotPose = getCurrentRobotPoseMean();
852 
853  // const CPose3DQuat sensorPoseAbs= robotPose + sensorPoseOnRobot;
854  CPose3DQuat sensorPoseAbs(UNINITIALIZED_QUATERNION);
856  CMatrixFixed<kftype, 7, 7> dsensorabs_dsenrelpose(
857  UNINITIALIZED_MATRIX); // Not actually used
858 
859  CPose3DQuatPDF::jacobiansPoseComposition(
860  robotPose, sensorPoseOnRobot, dsensorabs_dvehpose,
861  dsensorabs_dsenrelpose, &sensorPoseAbs);
862 
863  kftype hn_r = in_z[0];
864  kftype hn_y = in_z[1];
865  kftype hn_p = in_z[2];
866 
867  kftype chn_y = cos(hn_y);
868  kftype shn_y = sin(hn_y);
869  kftype chn_p = cos(hn_p);
870  kftype shn_p = sin(hn_p);
871 
872  /* -------------------------------------------
873  syms H h_range h_yaw h_pitch real;
874  H=[ h_range ; h_yaw ; h_pitch ];
875 
876  syms X xi_ yi_ zi_ real;
877  xi_ = h_range * cos(h_yaw) * cos(h_pitch);
878  yi_ = h_range * sin(h_yaw) * cos(h_pitch);
879  zi_ = -h_range * sin(h_pitch);
880 
881  X=[xi_ yi_ zi_];
882  jacob_inv_mod=jacobian(X,H)
883  ------------------------------------------- */
884 
885  // The new point, relative to the sensor:
886  const TPoint3D yn_rel_sensor(
887  hn_r * chn_y * chn_p, hn_r * shn_y * chn_p, -hn_r * shn_p);
888 
889  // The Jacobian of the 3D point in the coordinate system of the sensor:
890  /*
891  [ cos(h_pitch)*cos(h_yaw), -h_range*cos(h_pitch)*sin(h_yaw),
892  -h_range*cos(h_yaw)*sin(h_pitch)]
893  [ cos(h_pitch)*sin(h_yaw), h_range*cos(h_pitch)*cos(h_yaw),
894  -h_range*sin(h_pitch)*sin(h_yaw)]
895  [ -sin(h_pitch), 0,
896  -h_range*cos(h_pitch)]
897  */
898  const kftype values_dynlocal_dhn[] = {chn_p * chn_y,
899  -hn_r * chn_p * shn_y,
900  -hn_r * chn_y * shn_p,
901  chn_p * shn_y,
902  hn_r * chn_p * chn_y,
903  -hn_r * shn_p * shn_y,
904  -shn_p,
905  0,
906  -hn_r * chn_p};
907  const KFMatrix_FxO dynlocal_dhn(values_dynlocal_dhn);
908 
909  KFMatrix_FxF jacob_dyn_dynrelsensor(UNINITIALIZED_MATRIX);
910  KFMatrix_FxV jacob_dyn_dsensorabs(UNINITIALIZED_MATRIX);
911 
912  sensorPoseAbs.composePoint(
913  yn_rel_sensor.x, yn_rel_sensor.y,
914  yn_rel_sensor.z, // yn rel. to the sensor
915  yn[0], yn[1], yn[2], // yn in global coords
916  &jacob_dyn_dynrelsensor, &jacob_dyn_dsensorabs);
917 
918  dyn_dhn = jacob_dyn_dynrelsensor * dynlocal_dhn;
919 
920  // dyn_dxv =
921  dyn_dxv = jacob_dyn_dsensorabs * dsensorabs_dvehpose;
922 
923  MRPT_END
924 }
925 
926 /** If applicable to the given problem, do here any special handling of adding a
927  * new landmark to the map.
928  * \param in_obsIndex The index of the observation whose inverse sensor is to
929  * be computed. It corresponds to the row in in_z where the observation can be
930  * found.
931  * \param in_idxNewFeat The index that this new feature will have in the state
932  * vector (0:just after the vehicle state, 1: after that,...). Save this number
933  * so data association can be done according to these indices.
934  * \sa OnInverseObservationModel
935  */
937  const size_t in_obsIdx, const size_t in_idxNewFeat)
938 {
939  MRPT_START
940 
942  m_SF->getObservationByClass<CObservationBearingRange>();
943  ASSERTMSG_(
944  obs,
945  "*ERROR*: This method requires an observation of type "
946  "CObservationBearingRange");
947 
948  // ----------------------------------------------
949  // introduce in the lists of ID<->index in map:
950  // ----------------------------------------------
951  ASSERT_(in_obsIdx < obs->sensedData.size());
952  if (obs->sensedData[in_obsIdx].landmarkID >= 0)
953  {
954  // The sensor provides us a LM ID... use it:
955  m_IDs.insert(obs->sensedData[in_obsIdx].landmarkID, in_idxNewFeat);
956  }
957  else
958  {
959  // Features do not have IDs... use indices:
960  m_IDs.insert(in_idxNewFeat, in_idxNewFeat);
961  }
962 
963  MRPT_END
964 }
965 
966 /*---------------------------------------------------------------
967  getAs3DObject
968  ---------------------------------------------------------------*/
970  mrpt::opengl::CSetOfObjects::Ptr& outObj) const
971 {
972  outObj->clear();
973 
974  // ------------------------------------------------
975  // Add the XYZ corner for the current area:
976  // ------------------------------------------------
977  outObj->insert(opengl::stock_objects::CornerXYZ());
978 
979  // 3D ellipsoid for robot pose:
980  CPointPDFGaussian pointGauss;
981  pointGauss.mean.x(m_xkk[0]);
982  pointGauss.mean.y(m_xkk[1]);
983  pointGauss.mean.z(m_xkk[2]);
984  pointGauss.cov = m_pkk.blockCopy<3, 3>(0, 0);
985 
986  {
987  auto ellip = opengl::CEllipsoid::Create();
988 
989  ellip->setPose(pointGauss.mean);
990  ellip->setCovMatrix(pointGauss.cov);
991  ellip->enableDrawSolid3D(false);
992  ellip->setQuantiles(options.quantiles_3D_representation);
993  ellip->set3DsegmentsCount(10);
994  ellip->setColor(1, 0, 0);
995 
996  outObj->insert(ellip);
997  }
998 
999  // 3D ellipsoids for landmarks:
1000  const size_t nLMs = this->getNumberOfLandmarksInTheMap();
1001  for (size_t i = 0; i < nLMs; i++)
1002  {
1003  pointGauss.mean.x(
1004  m_xkk[get_vehicle_size() + get_feature_size() * i + 0]);
1005  pointGauss.mean.y(
1006  m_xkk[get_vehicle_size() + get_feature_size() * i + 1]);
1007  pointGauss.mean.z(
1008  m_xkk[get_vehicle_size() + get_feature_size() * i + 2]);
1009 
1010  pointGauss.cov =
1014 
1015  auto ellip = opengl::CEllipsoid::Create();
1016 
1017  ellip->setName(format("%u", static_cast<unsigned int>(i)));
1018  ellip->enableShowName(true);
1019  ellip->setPose(pointGauss.mean);
1020  ellip->setCovMatrix(pointGauss.cov);
1021  ellip->enableDrawSolid3D(false);
1022  ellip->setQuantiles(options.quantiles_3D_representation);
1023  ellip->set3DsegmentsCount(10);
1024 
1025  ellip->setColor(0, 0, 1);
1026 
1027  // Set color depending on partitions?
1029  {
1030  // This is done for each landmark:
1031  map<int, bool> belongToPartition;
1032  const CSimpleMap* SFs =
1033  &m_SFs; // mapPartitioner.getSequenceOfFrames();
1034 
1035  for (size_t p = 0; p < m_lastPartitionSet.size(); p++)
1036  {
1037  for (size_t w = 0; w < m_lastPartitionSet[p].size(); w++)
1038  {
1039  // Check if landmark #i is in the SF of
1040  // m_lastPartitionSet[p][w]:
1041  CLandmark::TLandmarkID i_th_ID = m_IDs.inverse(i);
1042 
1043  // Look for the lm_ID in the SF:
1044  CPose3DPDF::Ptr pdf;
1045  CSensoryFrame::Ptr SF_i;
1046  SFs->get(m_lastPartitionSet[p][w], pdf, SF_i);
1047 
1049  SF_i->getObservationByClass<CObservationBearingRange>();
1050 
1051  for (auto& o : obs->sensedData)
1052  {
1053  if (o.landmarkID == i_th_ID)
1054  {
1055  belongToPartition[p] = true;
1056  break;
1057  }
1058  } // end for o
1059  } // end for w
1060  } // end for p
1061 
1062  // Build label:
1063  string strParts("[");
1064 
1065  for (auto it = belongToPartition.begin();
1066  it != belongToPartition.end(); ++it)
1067  {
1068  if (it != belongToPartition.begin()) strParts += string(",");
1069  strParts += format("%i", it->first);
1070  }
1071 
1072  ellip->setName(ellip->getName() + strParts + string("]"));
1073  }
1074 
1075  outObj->insert(ellip);
1076  }
1077 }
1078 
1079 /*---------------------------------------------------------------
1080  getLastPartitionLandmarks
1081  ---------------------------------------------------------------*/
1083  size_t K, std::vector<std::vector<uint32_t>>& landmarksMembership)
1084 {
1085  // temporary copy:
1086  std::vector<std::vector<uint32_t>> tmpParts = m_lastPartitionSet;
1087 
1088  // Fake "m_lastPartitionSet":
1089 
1090  // Fixed partitions every K observations:
1091  vector<std::vector<uint32_t>> partitions;
1092  std::vector<uint32_t> tmpCluster;
1093 
1094  for (size_t i = 0; i < m_SFs.size(); i++)
1095  {
1096  tmpCluster.push_back(i);
1097  if ((i % K) == 0)
1098  {
1099  partitions.push_back(tmpCluster);
1100  tmpCluster.clear();
1101  tmpCluster.push_back(
1102  i); // This observation "i" is shared between both clusters
1103  }
1104  }
1105  m_lastPartitionSet = partitions;
1106 
1107  // Call the actual method:
1108  getLastPartitionLandmarks(landmarksMembership);
1109 
1110  // Replace copy:
1111  m_lastPartitionSet = tmpParts; //-V519
1112 }
1113 
1114 /*---------------------------------------------------------------
1115  getLastPartitionLandmarks
1116  ---------------------------------------------------------------*/
1118  std::vector<std::vector<uint32_t>>& landmarksMembership) const
1119 {
1120  landmarksMembership.clear();
1121 
1122  // All the computation is made based on "m_lastPartitionSet"
1123 
1124  if (!options.doPartitioningExperiment) return;
1125 
1126  const size_t nLMs = this->getNumberOfLandmarksInTheMap();
1127  for (size_t i = 0; i < nLMs; i++)
1128  {
1129  map<int, bool> belongToPartition;
1130  const CSimpleMap* SFs =
1131  &m_SFs; // mapPartitioner.getSequenceOfFrames();
1132 
1133  for (size_t p = 0; p < m_lastPartitionSet.size(); p++)
1134  {
1135  for (size_t w = 0; w < m_lastPartitionSet[p].size(); w++)
1136  {
1137  // Check if landmark #i is in the SF of
1138  // m_lastPartitionSet[p][w]:
1139  CLandmark::TLandmarkID i_th_ID = m_IDs.inverse(i);
1140 
1141  // Look for the lm_ID in the SF:
1142  CPose3DPDF::Ptr pdf;
1143  CSensoryFrame::Ptr SF_i;
1144  SFs->get(m_lastPartitionSet[p][w], pdf, SF_i);
1145 
1147  SF_i->getObservationByClass<CObservationBearingRange>();
1148 
1149  for (auto& o : obs->sensedData)
1150  {
1151  if (o.landmarkID == i_th_ID)
1152  {
1153  belongToPartition[p] = true;
1154  break;
1155  }
1156  } // end for o
1157  } // end for w
1158  } // end for p
1159 
1160  // Build membership list:
1161  std::vector<uint32_t> membershipOfThisLM;
1162 
1163  for (auto& it : belongToPartition)
1164  membershipOfThisLM.push_back(it.first);
1165 
1166  landmarksMembership.push_back(membershipOfThisLM);
1167  } // end for i
1168 }
1169 
1170 /*---------------------------------------------------------------
1171  computeOffDiagonalBlocksApproximationError
1172  ---------------------------------------------------------------*/
1174  const std::vector<std::vector<uint32_t>>& landmarksMembership) const
1175 {
1176  MRPT_START
1177 
1178  // Compute the information matrix:
1179  CMatrixDynamic<kftype> fullCov(m_pkk);
1180  size_t i;
1181  for (i = 0; i < get_vehicle_size(); i++)
1182  fullCov(i, i) = max(fullCov(i, i), 1e-6);
1183 
1184  CMatrixDynamic<kftype> H(fullCov.inverse_LLt());
1185  H.array().abs(); // Replace by absolute values:
1186 
1187  double sumOffBlocks = 0;
1188  unsigned int nLMs = landmarksMembership.size();
1189 
1190  ASSERT_(
1191  int(get_vehicle_size() + nLMs * get_feature_size()) == fullCov.cols());
1192 
1193  for (i = 0; i < nLMs; i++)
1194  {
1195  for (size_t j = i + 1; j < nLMs; j++)
1196  {
1197  // Sum the cross cov. between LM(i) and LM(j)??
1198  // --> Only if there is no common cluster:
1199  if (0 == math::countCommonElements(
1200  landmarksMembership[i], landmarksMembership[j]))
1201  {
1202  size_t col = get_vehicle_size() + i * get_feature_size();
1203  size_t row = get_vehicle_size() + j * get_feature_size();
1204  sumOffBlocks += 2 * H.block<2, 2>(row, col).sum();
1205  }
1206  }
1207  }
1208 
1209  return sumOffBlocks / H.asEigen()
1210  .block(
1212  H.rows() - get_vehicle_size(),
1213  H.cols() - get_vehicle_size())
1214  .sum(); // Starting (7,7)-end
1215  MRPT_END
1216 }
1217 
1218 /*---------------------------------------------------------------
1219  reconsiderPartitionsNow
1220  ---------------------------------------------------------------*/
1222 {
1223  // A different buffer for making this
1224  // thread-safe some day...
1225  vector<std::vector<uint32_t>> partitions;
1226  mapPartitioner.updatePartitions(partitions);
1227  m_lastPartitionSet = partitions;
1228 }
1229 
1230 /*---------------------------------------------------------------
1231  saveMapAndPathRepresentationAsMATLABFile
1232  ---------------------------------------------------------------*/
1234  const string& fil, float stdCount, const string& styleLandmarks,
1235  const string& stylePath, const string& styleRobot) const
1236 {
1237  FILE* f = os::fopen(fil.c_str(), "wt");
1238  if (!f) return;
1239 
1240  CMatrixDouble cov(2, 2);
1241  CVectorDouble mean(2);
1242 
1243  // Header:
1244  os::fprintf(
1245  f,
1246  "%%--------------------------------------------------------------------"
1247  "\n");
1248  os::fprintf(f, "%% File automatically generated using the MRPT method:\n");
1249  os::fprintf(
1250  f,
1251  "%% "
1252  "'CRangeBearingKFSLAM::saveMapAndPath2DRepresentationAsMATLABFile'\n");
1253  os::fprintf(f, "%%\n");
1254  os::fprintf(f, "%% ~ MRPT ~\n");
1255  os::fprintf(
1256  f, "%% Jose Luis Blanco Claraco, University of Malaga @ 2008\n");
1257  os::fprintf(f, "%% https://www.mrpt.org/ \n");
1258  os::fprintf(
1259  f,
1260  "%%--------------------------------------------------------------------"
1261  "\n");
1262 
1263  // Main code:
1264  os::fprintf(f, "hold on;\n\n");
1265 
1266  const size_t nLMs = this->getNumberOfLandmarksInTheMap();
1267 
1268  for (size_t i = 0; i < nLMs; i++)
1269  {
1270  size_t idx = get_vehicle_size() + i * get_feature_size();
1271 
1272  cov(0, 0) = m_pkk(idx + 0, idx + 0);
1273  cov(1, 1) = m_pkk(idx + 1, idx + 1);
1274  cov(0, 1) = cov(1, 0) = m_pkk(idx + 0, idx + 1);
1275 
1276  mean[0] = m_xkk[idx + 0];
1277  mean[1] = m_xkk[idx + 1];
1278 
1279  // Command to draw the 2D ellipse:
1280  os::fprintf(
1281  f, "%s",
1282  math::MATLAB_plotCovariance2D(cov, mean, stdCount, styleLandmarks)
1283  .c_str());
1284  }
1285 
1286  // Now: the robot path:
1287  // ------------------------------
1288  if (m_SFs.size())
1289  {
1290  os::fprintf(f, "\nROB_PATH=[");
1291  for (size_t i = 0; i < m_SFs.size(); i++)
1292  {
1293  CSensoryFrame::Ptr dummySF;
1294  CPose3DPDF::Ptr pdf3D;
1295  m_SFs.get(i, pdf3D, dummySF);
1296 
1297  CPose3D p;
1298  pdf3D->getMean(p);
1299 
1300  os::fprintf(f, "%.04f %.04f", p.x(), p.y());
1301  if (i < (m_SFs.size() - 1)) os::fprintf(f, ";");
1302  }
1303  os::fprintf(f, "];\n");
1304 
1305  os::fprintf(
1306  f, "plot(ROB_PATH(:,1),ROB_PATH(:,2),'%s');\n", stylePath.c_str());
1307  }
1308 
1309  // The robot pose:
1310  cov(0, 0) = m_pkk(0, 0);
1311  cov(1, 1) = m_pkk(1, 1);
1312  cov(0, 1) = cov(1, 0) = m_pkk(0, 1);
1313 
1314  mean[0] = m_xkk[0];
1315  mean[1] = m_xkk[1];
1316 
1317  os::fprintf(
1318  f, "%s",
1319  math::MATLAB_plotCovariance2D(cov, mean, stdCount, styleRobot).c_str());
1320 
1321  os::fprintf(f, "\naxis equal;\n");
1322  os::fclose(f);
1323 }
1324 
1325 /** Computes A=A-B, which may need to be re-implemented depending on the
1326  * topology of the individual scalar components (eg, angles).
1327  */
1329  KFArray_OBS& A, const KFArray_OBS& B) const
1330 {
1331  A -= B;
1334 }
1335 
1336 /** Return the observation NOISE covariance matrix, that is, the model of the
1337  * Gaussian additive noise of the sensor.
1338  * \param out_R The noise covariance matrix. It might be non diagonal, but
1339  * it'll usually be.
1340  */
1342 {
1343  out_R(0, 0) = square(options.std_sensor_range);
1344  out_R(1, 1) = square(options.std_sensor_yaw);
1345  out_R(2, 2) = square(options.std_sensor_pitch);
1346 }
1347 
1348 /** This will be called before OnGetObservationsAndDataAssociation to allow the
1349  * application to reduce the number of covariance landmark predictions to be
1350  * made.
1351  * For example, features which are known to be "out of sight" shouldn't be
1352  * added to the output list to speed up the calculations.
1353  * \param in_all_prediction_means The mean of each landmark predictions; the
1354  * computation or not of the corresponding covariances is what we're trying to
1355  * determined with this method.
1356  * \param out_LM_indices_to_predict The list of landmark indices in the map
1357  * [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.
1358  * \note This is not a pure virtual method, so it should be implemented only if
1359  * desired. The default implementation returns a vector with all the landmarks
1360  * in the map.
1361  * \sa OnGetObservations, OnDataAssociation
1362  */
1364  const vector_KFArray_OBS& prediction_means,
1365  std::vector<size_t>& out_LM_indices_to_predict) const
1366 {
1368  m_SF->getObservationByClass<CObservationBearingRange>();
1369  ASSERTMSG_(
1370  obs,
1371  "*ERROR*: This method requires an observation of type "
1372  "CObservationBearingRange");
1373 
1374 #define USE_HEURISTIC_PREDICTION
1375 
1376 #ifdef USE_HEURISTIC_PREDICTION
1377  const double sensor_max_range = obs->maxSensorDistance;
1378  const double fov_yaw = obs->fieldOfView_yaw;
1379  const double fov_pitch = obs->fieldOfView_pitch;
1380 
1381  const double max_vehicle_loc_uncertainty =
1382  4 * std::sqrt(m_pkk(0, 0) + m_pkk(1, 1) + m_pkk(2, 2));
1383 #endif
1384 
1385  out_LM_indices_to_predict.clear();
1386  for (size_t i = 0; i < prediction_means.size(); i++)
1387  {
1388 #ifndef USE_HEURISTIC_PREDICTION
1389  out_LM_indices_to_predict.push_back(i);
1390 #else
1391  // Heuristic: faster but doesn't work always!
1392  if (prediction_means[i][0] <
1393  (15 + sensor_max_range + max_vehicle_loc_uncertainty +
1394  4 * options.std_sensor_range) &&
1395  fabs(prediction_means[i][1]) <
1396  (DEG2RAD(30) + 0.5 * fov_yaw + 4 * options.std_sensor_yaw) &&
1397  fabs(prediction_means[i][2]) <
1398  (DEG2RAD(30) + 0.5 * fov_pitch + 4 * options.std_sensor_pitch))
1399  {
1400  out_LM_indices_to_predict.push_back(i);
1401  }
1402 #endif
1403  }
1404 }
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:357
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
double RAD2DEG(const double x)
Radians to degrees.
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.
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...
double DEG2RAD(const double x)
Degrees to radians.
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 .
T square(const T x)
Inline function for the square of a number.
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...
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:148
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:84
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::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
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...
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...
CMatrixDynamic< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 3316834cb Wed Aug 21 11:42:56 2019 +0200 at mié ago 21 11:50:11 CEST 2019