MRPT  1.9.9
CRangeBearingKFSLAM.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "slam-precomp.h" // Precompiled headers
11 
12 // ----------------------------------------------------------------------------------------
13 // For the theory behind this implementation, see the technical report in:
14 //
15 // http://www.mrpt.org/6D-SLAM
16 // ----------------------------------------------------------------------------------------
17 
19 #include <mrpt/poses/CPosePDF.h>
23 #include <mrpt/math/utils.h>
25 #include <mrpt/math/wrap2pi.h>
26 #include <mrpt/system/CTicTac.h>
27 #include <mrpt/system/os.h>
28 
31 #include <mrpt/opengl/CEllipsoid.h>
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.zeros();
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  m_pkk.extractMatrix(0, 0, out_robotPose.cov);
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  m_pkk.extractMatrix(0, 0, out_robotPose.cov);
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  for (KFVector::Index i = 0; i < m_xkk.size(); i++)
177  out_fullState[i] = m_xkk[i];
178 
179  // Full cov:
180  out_fullCovariance = m_pkk;
181 
182  MRPT_END
183 }
184 
185 /*---------------------------------------------------------------
186  processActionObservation
187  ---------------------------------------------------------------*/
190 {
191  MRPT_START
192 
193  m_action = action;
194  m_SF = SF;
195 
196  // Sanity check:
197  ASSERT_(
198  m_IDs.size() ==
199  (m_pkk.cols() - get_vehicle_size()) / get_feature_size());
200 
201  // ===================================================================================================================
202  // Here's the meat!: Call the main method for the KF algorithm, which will
203  // call all the callback methods as required:
204  // ===================================================================================================================
206 
207  // =============================================================
208  // ADD TO SFs SEQUENCE
209  // =============================================================
211  this->getCurrentRobotPose(q);
212  CPose3DPDFGaussian::Ptr auxPosePDF =
214 
216  {
217  m_SFs.insert(CPose3DPDF::Ptr(auxPosePDF), SF);
218  }
219 
220  // =============================================================
221  // UPDATE THE PARTITION GRAPH EXPERIMENT
222  // =============================================================
224  {
225  if (options.partitioningMethod == 0)
226  {
227  // Use spectral-graph technique:
228  mapPartitioner.addMapFrame(*SF, *auxPosePDF);
229 
230  vector<std::vector<uint32_t>> partitions;
231  mapPartitioner.updatePartitions(partitions);
232  m_lastPartitionSet = partitions;
233  }
234  else
235  {
236  // Fixed partitions every K observations:
237  vector<std::vector<uint32_t>> partitions;
238  std::vector<uint32_t> tmpCluster;
239 
241  size_t K = options.partitioningMethod;
242 
243  for (size_t i = 0; i < m_SFs.size(); i++)
244  {
245  tmpCluster.push_back(i);
246  if ((i % K) == 0)
247  {
248  partitions.push_back(tmpCluster);
249  tmpCluster.clear();
250  tmpCluster.push_back(i); // This observation "i" is shared
251  // between both clusters
252  }
253  }
254  m_lastPartitionSet = partitions;
255  }
256 
257  printf(
258  "Partitions: %u\n",
259  static_cast<unsigned>(m_lastPartitionSet.size()));
260  }
261 
262  MRPT_END
263 }
264 
265 /** Return the last odometry, as a pose increment.
266  */
268 {
269  CActionRobotMovement2D::Ptr actMov2D =
270  m_action->getBestMovementEstimation();
271  CActionRobotMovement3D::Ptr actMov3D =
272  m_action->getActionByClass<CActionRobotMovement3D>();
273  if (actMov3D && !options.force_ignore_odometry)
274  {
275  return CPose3DQuat(actMov3D->poseChange.mean);
276  }
277  else if (actMov2D && !options.force_ignore_odometry)
278  {
279  CPose2D estMovMean;
280  actMov2D->poseChange->getMean(estMovMean);
281  return CPose3DQuat(CPose3D(estMovMean));
282  }
283  else
284  {
285  return CPose3DQuat();
286  }
287 }
288 
289 /** Must return the action vector u.
290  * \param out_u The action vector which will be passed to OnTransitionModel
291  */
292 void CRangeBearingKFSLAM::OnGetAction(KFArray_ACT& u) const
293 {
294  // Get odometry estimation:
295  const CPose3DQuat theIncr = getIncrementFromOdometry();
296 
297  for (KFArray_ACT::Index i = 0; i < u.size(); i++) u[i] = theIncr[i];
298 }
299 
300 /** This virtual function musts implement the prediction model of the Kalman
301  * filter.
302  */
304  const KFArray_ACT& u, KFArray_VEH& xv, bool& out_skipPrediction) const
305 {
306  MRPT_START
307 
308  // Do not update the vehicle pose & its covariance until we have some
309  // landmarks in the map,
310  // otherwise, we are imposing a lower bound to the best uncertainty from now
311  // on:
312  if (size_t(m_xkk.size()) == get_vehicle_size())
313  {
314  out_skipPrediction = true;
315  }
316 
317  // Current pose: copy xyz+quat
318  CPose3DQuat robotPose = getCurrentRobotPoseMean();
319 
320  // Increment pose: copy xyz+quat (explicitly unroll the loop)
322  odoIncrement.m_coords[0] = u[0];
323  odoIncrement.m_coords[1] = u[1];
324  odoIncrement.m_coords[2] = u[2];
325  odoIncrement.m_quat[0] = u[3];
326  odoIncrement.m_quat[1] = u[4];
327  odoIncrement.m_quat[2] = u[5];
328  odoIncrement.m_quat[3] = u[6];
329 
330  // Pose composition:
331  robotPose += odoIncrement;
332 
333  // Output:
334  for (size_t i = 0; i < xv.static_size; i++) xv[i] = robotPose[i];
335 
336  MRPT_END
337 }
338 
339 /** This virtual function musts calculate the Jacobian F of the prediction
340  * model.
341  */
342 void CRangeBearingKFSLAM::OnTransitionJacobian(KFMatrix_VxV& F) const
343 {
344  MRPT_START
345 
346  // Current pose: copy xyz+quat
347  CPose3DQuat robotPose = getCurrentRobotPoseMean();
348 
349  // Odometry:
350  const CPose3DQuat theIncr = getIncrementFromOdometry();
351 
352  // Compute jacobians:
354 
355  CPose3DQuatPDF::jacobiansPoseComposition(
356  robotPose, // x
357  theIncr, // u
358  F, // df_dx,
359  df_du);
360 
361  MRPT_END
362 }
363 
364 /** This virtual function musts calculate de noise matrix of the prediction
365  * model.
366  */
367 void CRangeBearingKFSLAM::OnTransitionNoise(KFMatrix_VxV& Q) const
368 {
369  MRPT_START
370 
371  // The uncertainty of the 2D odometry, projected from the current position:
372  CActionRobotMovement2D::Ptr act2D = m_action->getBestMovementEstimation();
374  m_action->getActionByClass<CActionRobotMovement3D>();
375 
376  if (act3D && act2D)
377  THROW_EXCEPTION("Both 2D & 3D odometry are present!?!?");
378 
379  CPose3DQuatPDFGaussian odoIncr;
380 
381  if ((!act3D && !act2D) || options.force_ignore_odometry)
382  {
383  // Use constant Q:
384  Q.zeros();
385  ASSERT_(size_t(options.stds_Q_no_odo.size()) == size_t(Q.cols()));
386  for (size_t i = 0; i < get_vehicle_size(); i++)
387  Q.set_unsafe(i, i, square(options.stds_Q_no_odo[i]));
388  return;
389  }
390  else
391  {
392  if (act2D)
393  {
394  // 2D odometry:
395  CPosePDFGaussian odoIncr2D;
396  odoIncr2D.copyFrom(*act2D->poseChange);
397  odoIncr = CPose3DQuatPDFGaussian(CPose3DPDFGaussian(odoIncr2D));
398  }
399  else
400  {
401  // 3D odometry:
402  odoIncr = CPose3DQuatPDFGaussian(act3D->poseChange);
403  }
404  }
405 
406  odoIncr.cov(2, 2) += square(options.std_odo_z_additional);
407 
408  // Current pose: copy xyz+quat
409  CPose3DQuat robotPose = getCurrentRobotPoseMean();
410 
411  // Transform from odometry increment to "relative to the robot":
412  odoIncr.changeCoordinatesReference(robotPose);
413 
414  Q = odoIncr.cov;
415 
416  MRPT_END
417 }
418 
420  const std::vector<size_t>& idx_landmarks_to_predict,
421  vector_KFArray_OBS& out_predictions) const
422 {
423  MRPT_START
424 
425  // Mean of the prior of the robot pose:
426  CPose3DQuat robotPose = getCurrentRobotPoseMean();
427 
428  // Get the sensor pose relative to the robot:
430  m_SF->getObservationByClass<CObservationBearingRange>();
431  ASSERTMSG_(
432  obs,
433  "*ERROR*: This method requires an observation of type "
434  "CObservationBearingRange");
435  const CPose3DQuat sensorPoseOnRobot =
436  CPose3DQuat(obs->sensorLocationOnRobot);
437 
438  /* -------------------------------------------
439  Equations, obtained using matlab, of the relative 3D position of a
440  landmark (xi,yi,zi), relative
441  to a robot 6D pose (x0,y0,z0,y,p,r)
442  Refer to technical report "6D EKF derivation...", 2008
443 
444  x0 y0 z0 y p r % Robot's 6D pose
445  x0s y0s z0s ys ps rs % Sensor's 6D pose relative to robot
446  xi yi zi % Absolute 3D landmark coordinates:
447 
448  Hx : dh_dxv -> Jacobian of the observation model wrt the robot pose
449  Hy : dh_dyi -> Jacobian of the observation model wrt each landmark
450  mean position
451 
452  Sizes:
453  h: Lx3
454  Hx: 3Lx6
455  Hy: 3Lx3
456 
457  L=# of landmarks in the map (ALL OF THEM)
458  ------------------------------------------- */
459 
460  const size_t vehicle_size = get_vehicle_size();
461  // const size_t obs_size = get_observation_size();
462  const size_t feature_size = get_feature_size();
463 
464  const CPose3DQuat sensorPoseAbs = robotPose + sensorPoseOnRobot;
465 
466  const size_t N = idx_landmarks_to_predict.size();
467  out_predictions.resize(N);
468  for (size_t i = 0; i < N; i++)
469  {
470  const size_t row_in = feature_size * idx_landmarks_to_predict[i];
471 
472  // Landmark absolute 3D position in the map:
473  const TPoint3D mapEst(
474  m_xkk[vehicle_size + row_in + 0], m_xkk[vehicle_size + row_in + 1],
475  m_xkk[vehicle_size + row_in + 2]);
476 
477  // Generate Range, yaw, pitch
478  // ---------------------------------------------------
479  sensorPoseAbs.sphericalCoordinates(
480  mapEst,
481  out_predictions[i][0], // range
482  out_predictions[i][1], // yaw
483  out_predictions[i][2] // pitch
484  );
485  }
486 
487  MRPT_END
488 }
489 
491  const size_t& idx_landmark_to_predict, KFMatrix_OxV& Hx,
492  KFMatrix_OxF& Hy) const
493 {
494  MRPT_START
495 
496  // Mean of the prior of the robot pose:
497  const CPose3DQuat robotPose = getCurrentRobotPoseMean();
498 
499  // Get the sensor pose relative to the robot:
501  m_SF->getObservationByClass<CObservationBearingRange>();
502  ASSERTMSG_(
503  obs,
504  "*ERROR*: This method requires an observation of type "
505  "CObservationBearingRange");
506  const CPose3DQuat sensorPoseOnRobot =
507  CPose3DQuat(obs->sensorLocationOnRobot);
508 
509  const size_t vehicle_size = get_vehicle_size();
510  // const size_t obs_size = get_observation_size();
511  const size_t feature_size = get_feature_size();
512 
513  // Compute the jacobians, needed below:
514  // const CPose3DQuat sensorPoseAbs= robotPose + sensorPoseOnRobot;
515  CPose3DQuat sensorPoseAbs(UNINITIALIZED_QUATERNION);
517  CMatrixFixedNumeric<kftype, 7, 7> H_senpose_senrelpose(
518  UNINITIALIZED_MATRIX); // Not actually used
519 
520  CPose3DQuatPDF::jacobiansPoseComposition(
521  robotPose, sensorPoseOnRobot, H_senpose_vehpose, H_senpose_senrelpose,
522  &sensorPoseAbs);
523 
524  const size_t row_in = feature_size * idx_landmark_to_predict;
525 
526  // Landmark absolute 3D position in the map:
527  const TPoint3D mapEst(
528  m_xkk[vehicle_size + row_in + 0], m_xkk[vehicle_size + row_in + 1],
529  m_xkk[vehicle_size + row_in + 2]);
530 
531  // The Jacobian wrt the sensor pose must be transformed later on:
532  KFMatrix_OxV Hx_sensor;
533  double obsData[3];
534  sensorPoseAbs.sphericalCoordinates(
535  mapEst,
536  obsData[0], // range
537  obsData[1], // yaw
538  obsData[2], // pitch
539  &Hy, &Hx_sensor);
540 
541  // Chain rule: Hx = d sensorpose / d vehiclepose * Hx_sensor
542  Hx.multiply(Hx_sensor, H_senpose_vehpose);
543 
544  MRPT_END
545 }
546 
547 /** This is called between the KF prediction step and the update step, and the
548  * application must return the observations and, when applicable, the data
549  * association between these observations and the current map.
550  *
551  * \param out_z N vectors, each for one "observation" of length OBS_SIZE, N
552  * being the number of "observations": how many observed landmarks for a map, or
553  * just one if not applicable.
554  * \param out_data_association An empty vector or, where applicable, a vector
555  * where the i'th element corresponds to the position of the observation in the
556  * i'th row of out_z within the system state vector (in the range
557  * [0,getNumberOfLandmarksInTheMap()-1]), or -1 if it is a new map element and
558  * we want to insert it at the end of this KF iteration.
559  * \param in_S The full covariance matrix of the observation predictions (i.e.
560  * the "innovation covariance matrix"). This is a M·O x M·O matrix with M=length
561  * of "in_lm_indices_in_S".
562  * \param in_lm_indices_in_S The indices of the map landmarks (range
563  * [0,getNumberOfLandmarksInTheMap()-1]) that can be found in the matrix in_S.
564  *
565  * This method will be called just once for each complete KF iteration.
566  * \note It is assumed that the observations are independent, i.e. there are NO
567  * cross-covariances between them.
568  */
570  vector_KFArray_OBS& Z, std::vector<int>& data_association,
571  const vector_KFArray_OBS& all_predictions, const KFMatrix& S,
572  const std::vector<size_t>& lm_indices_in_S, const KFMatrix_OxO& R)
573 {
574  MRPT_START
575 
576  // static const size_t vehicle_size = get_vehicle_size();
577  static const size_t obs_size = get_observation_size();
578 
579  // Z: Observations
581 
583  m_SF->getObservationByClass<CObservationBearingRange>();
584  ASSERTMSG_(
585  obs,
586  "*ERROR*: This method requires an observation of type "
587  "CObservationBearingRange");
588 
589  const size_t N = obs->sensedData.size();
590  Z.resize(N);
591 
592  size_t row;
593  for (row = 0, itObs = obs->sensedData.begin();
594  itObs != obs->sensedData.end(); ++itObs, ++row)
595  {
596  // Fill one row in Z:
597  Z[row][0] = itObs->range;
598  Z[row][1] = itObs->yaw;
599  Z[row][2] = itObs->pitch;
600  }
601 
602  // Data association:
603  // ---------------------
604  data_association.assign(N, -1); // Initially, all new landmarks
605 
606  // For each observed LM:
607  std::vector<size_t> obs_idxs_needing_data_assoc;
608  obs_idxs_needing_data_assoc.reserve(N);
609 
610  {
613  size_t row;
614  for (row = 0, itObs = obs->sensedData.begin(),
615  itDA = data_association.begin();
616  itObs != obs->sensedData.end(); ++itObs, ++itDA, ++row)
617  {
618  // Fill data asociation: Using IDs!
619  if (itObs->landmarkID < 0)
620  obs_idxs_needing_data_assoc.push_back(row);
621  else
622  {
624  unsigned int>::iterator itID;
625  if ((itID = m_IDs.find_key(itObs->landmarkID)) != m_IDs.end())
626  *itDA = itID->second; // This row in Z corresponds to the
627  // i'th map element in the state
628  // vector:
629  }
630  }
631  }
632 
633  // ---- Perform data association ----
634  // Only for observation indices in "obs_idxs_needing_data_assoc"
635  if (obs_idxs_needing_data_assoc.empty())
636  {
637  // We don't need to do DA:
639 
640  // Save them for the case the external user wants to access them:
641  for (size_t idxObs = 0; idxObs < data_association.size(); idxObs++)
642  {
643  int da = data_association[idxObs];
644  if (da >= 0)
646  data_association[idxObs];
647  }
648  }
649  else
650  {
651  // Build a Z matrix with the observations that need dat.assoc:
652  const size_t nObsDA = obs_idxs_needing_data_assoc.size();
653 
654  CMatrixTemplateNumeric<kftype> Z_obs_means(nObsDA, obs_size);
655  for (size_t i = 0; i < nObsDA; i++)
656  {
657  const size_t idx = obs_idxs_needing_data_assoc[i];
658  for (unsigned k = 0; k < obs_size; k++)
659  Z_obs_means.get_unsafe(i, k) = Z[idx][k];
660  }
661 
662  // Vehicle uncertainty
664  m_pkk.extractMatrix(0, 0, Pxx);
665 
666  // Build predictions:
667  // ---------------------------
668  const size_t nPredictions = lm_indices_in_S.size();
670 
671  // S is the covariance of the predictions:
673 
674  // The means:
675  m_last_data_association.Y_pred_means.setSize(nPredictions, obs_size);
676  for (size_t q = 0; q < nPredictions; q++)
677  {
678  const size_t i = lm_indices_in_S[q];
679  for (size_t w = 0; w < obs_size; w++)
680  m_last_data_association.Y_pred_means.get_unsafe(q, w) =
681  all_predictions[i][w];
683  i); // for the conversion of indices...
684  }
685 
686  // Do Dat. Assoc :
687  // ---------------------------
688  if (nPredictions)
689  {
690  CMatrixDouble Z_obs_cov = CMatrixDouble(R);
691 
693  Z_obs_means, // Z_obs_cov,
698  true, // Use KD-tree
702 
703  // Return pairings to the main KF algorithm:
706  it != m_last_data_association.results.associations.end(); ++it)
707  data_association[it->first] = it->second;
708  }
709  }
710  // ---- End of data association ----
711 
712  MRPT_END
713 }
714 
715 /** This virtual function musts normalize the state vector and covariance matrix
716  * (only if its necessary).
717  */
719 {
720  MRPT_START
721 
722  // m_xkk[3:6] must be a normalized quaternion:
723  const double T = std::sqrt(
724  square(m_xkk[3]) + square(m_xkk[4]) + square(m_xkk[5]) +
725  square(m_xkk[6]));
726  ASSERTMSG_(T > 0, "Vehicle pose quaternion norm is not >0!!");
727 
728  const double T_ = (m_xkk[3] < 0 ? -1.0 : 1.0) / T; // qr>=0
729  m_xkk[3] *= T_;
730  m_xkk[4] *= T_;
731  m_xkk[5] *= T_;
732  m_xkk[6] *= T_;
733 
734  MRPT_END
735 }
736 
737 /*---------------------------------------------------------------
738  loadOptions
739  ---------------------------------------------------------------*/
741 {
742  // Main
743  options.loadFromConfigFile(ini, "RangeBearingKFSLAM");
744  KF_options.loadFromConfigFile(ini, "RangeBearingKFSLAM_KalmanFilter");
745  // partition algorithm:
746  mapPartitioner.options.loadFromConfigFile(ini, "RangeBearingKFSLAM");
747 }
748 
749 /*---------------------------------------------------------------
750  loadOptions
751  ---------------------------------------------------------------*/
753  const mrpt::config::CConfigFileBase& source, const std::string& section)
754 {
755  source.read_vector(section, "stds_Q_no_odo", stds_Q_no_odo, stds_Q_no_odo);
756  ASSERT_(stds_Q_no_odo.size() == 7);
758  source.read_float(section, "std_sensor_range", std_sensor_range);
760  source.read_float(
761  section, "std_sensor_yaw_deg", RAD2DEG(std_sensor_yaw)));
763  source.read_float(
764  section, "std_sensor_pitch_deg", RAD2DEG(std_sensor_pitch)));
765 
766  std_odo_z_additional = source.read_float(
767  section, "std_odo_z_additional", std_odo_z_additional);
768 
771 
773 
775 
777  section, "data_assoc_method", data_assoc_method);
779  section, "data_assoc_metric", data_assoc_metric);
781  section, "data_assoc_IC_metric", data_assoc_IC_metric);
782 
785 
787 }
788 
789 /*---------------------------------------------------------------
790  Constructor
791  ---------------------------------------------------------------*/
793  : stds_Q_no_odo(get_vehicle_size(), 0),
794  std_sensor_range(0.01f),
795  std_sensor_yaw(DEG2RAD(0.2f)),
796  std_sensor_pitch(DEG2RAD(0.2f)),
797  std_odo_z_additional(0),
798  doPartitioningExperiment(false),
799  quantiles_3D_representation(3),
800  partitioningMethod(0),
801  data_assoc_method(assocNN),
802  data_assoc_metric(metricMaha),
803  data_assoc_IC_chi2_thres(0.99),
804  data_assoc_IC_metric(metricMaha),
805  data_assoc_IC_ml_threshold(0.0),
806  create_simplemap(false),
807  force_ignore_odometry(false)
808 {
809  stds_Q_no_odo[0] = stds_Q_no_odo[1] = stds_Q_no_odo[2] = 0.10f;
811  0.05f;
812 }
813 
814 /*---------------------------------------------------------------
815  dumpToTextStream
816  ---------------------------------------------------------------*/
818 {
819  using namespace mrpt::typemeta;
820 
821  out << mrpt::format(
822  "\n----------- [CRangeBearingKFSLAM::TOptions] ------------ \n\n");
823 
824  out << mrpt::format(
825  "doPartitioningExperiment = %c\n",
826  doPartitioningExperiment ? 'Y' : 'N');
827  out << mrpt::format(
828  "partitioningMethod = %i\n", partitioningMethod);
829  out << mrpt::format(
830  "data_assoc_method = %s\n",
832  .c_str());
833  out << mrpt::format(
834  "data_assoc_metric = %s\n",
836  .c_str());
837  out << mrpt::format(
838  "data_assoc_IC_chi2_thres = %.06f\n",
839  data_assoc_IC_chi2_thres);
840  out << mrpt::format(
841  "data_assoc_IC_metric = %s\n",
843  .c_str());
844  out << mrpt::format(
845  "data_assoc_IC_ml_threshold = %.06f\n",
846  data_assoc_IC_ml_threshold);
847 
848  out << mrpt::format("\n");
849 }
850 
852  const KFArray_OBS& in_z, KFArray_FEAT& yn, KFMatrix_FxV& dyn_dxv,
853  KFMatrix_FxO& dyn_dhn) const
854 {
855  MRPT_START
856 
858  m_SF->getObservationByClass<CObservationBearingRange>();
859  ASSERTMSG_(
860  obs,
861  "*ERROR*: This method requires an observation of type "
862  "CObservationBearingRange");
863  const CPose3DQuat sensorPoseOnRobot =
864  CPose3DQuat(obs->sensorLocationOnRobot);
865 
866  // Mean of the prior of the robot pose:
867  const CPose3DQuat robotPose = getCurrentRobotPoseMean();
868 
869  // const CPose3DQuat sensorPoseAbs= robotPose + sensorPoseOnRobot;
870  CPose3DQuat sensorPoseAbs(UNINITIALIZED_QUATERNION);
872  CMatrixFixedNumeric<kftype, 7, 7> dsensorabs_dsenrelpose(
873  UNINITIALIZED_MATRIX); // Not actually used
874 
875  CPose3DQuatPDF::jacobiansPoseComposition(
876  robotPose, sensorPoseOnRobot, dsensorabs_dvehpose,
877  dsensorabs_dsenrelpose, &sensorPoseAbs);
878 
879  kftype hn_r = in_z[0];
880  kftype hn_y = in_z[1];
881  kftype hn_p = in_z[2];
882 
883  kftype chn_y = cos(hn_y);
884  kftype shn_y = sin(hn_y);
885  kftype chn_p = cos(hn_p);
886  kftype shn_p = sin(hn_p);
887 
888  /* -------------------------------------------
889  syms H h_range h_yaw h_pitch real;
890  H=[ h_range ; h_yaw ; h_pitch ];
891 
892  syms X xi_ yi_ zi_ real;
893  xi_ = h_range * cos(h_yaw) * cos(h_pitch);
894  yi_ = h_range * sin(h_yaw) * cos(h_pitch);
895  zi_ = -h_range * sin(h_pitch);
896 
897  X=[xi_ yi_ zi_];
898  jacob_inv_mod=jacobian(X,H)
899  ------------------------------------------- */
900 
901  // The new point, relative to the sensor:
902  const TPoint3D yn_rel_sensor(
903  hn_r * chn_y * chn_p, hn_r * shn_y * chn_p, -hn_r * shn_p);
904 
905  // The Jacobian of the 3D point in the coordinate system of the sensor:
906  /*
907  [ cos(h_pitch)*cos(h_yaw), -h_range*cos(h_pitch)*sin(h_yaw),
908  -h_range*cos(h_yaw)*sin(h_pitch)]
909  [ cos(h_pitch)*sin(h_yaw), h_range*cos(h_pitch)*cos(h_yaw),
910  -h_range*sin(h_pitch)*sin(h_yaw)]
911  [ -sin(h_pitch), 0,
912  -h_range*cos(h_pitch)]
913  */
914  const kftype values_dynlocal_dhn[] = {chn_p * chn_y,
915  -hn_r * chn_p * shn_y,
916  -hn_r * chn_y * shn_p,
917  chn_p * shn_y,
918  hn_r * chn_p * chn_y,
919  -hn_r * shn_p * shn_y,
920  -shn_p,
921  0,
922  -hn_r * chn_p};
923  const KFMatrix_FxO dynlocal_dhn(values_dynlocal_dhn);
924 
925  KFMatrix_FxF jacob_dyn_dynrelsensor(UNINITIALIZED_MATRIX);
926  KFMatrix_FxV jacob_dyn_dsensorabs(UNINITIALIZED_MATRIX);
927 
928  sensorPoseAbs.composePoint(
929  yn_rel_sensor.x, yn_rel_sensor.y,
930  yn_rel_sensor.z, // yn rel. to the sensor
931  yn[0], yn[1], yn[2], // yn in global coords
932  &jacob_dyn_dynrelsensor, &jacob_dyn_dsensorabs);
933 
934  // dyn_dhn = jacob_dyn_dynrelsensor * dynlocal_dhn;
935  dyn_dhn.multiply_AB(jacob_dyn_dynrelsensor, dynlocal_dhn);
936 
937  // dyn_dxv =
938  dyn_dxv.multiply_AB(
939  jacob_dyn_dsensorabs, dsensorabs_dvehpose); // dsensorabs_dsenrelpose);
940 
941  MRPT_END
942 }
943 
944 /** If applicable to the given problem, do here any special handling of adding a
945  * new landmark to the map.
946  * \param in_obsIndex The index of the observation whose inverse sensor is to
947  * be computed. It corresponds to the row in in_z where the observation can be
948  * found.
949  * \param in_idxNewFeat The index that this new feature will have in the state
950  * vector (0:just after the vehicle state, 1: after that,...). Save this number
951  * so data association can be done according to these indices.
952  * \sa OnInverseObservationModel
953  */
955  const size_t in_obsIdx, const size_t in_idxNewFeat)
956 {
957  MRPT_START
958 
960  m_SF->getObservationByClass<CObservationBearingRange>();
961  ASSERTMSG_(
962  obs,
963  "*ERROR*: This method requires an observation of type "
964  "CObservationBearingRange");
965 
966  // ----------------------------------------------
967  // introduce in the lists of ID<->index in map:
968  // ----------------------------------------------
969  ASSERT_(in_obsIdx < obs->sensedData.size());
970  if (obs->sensedData[in_obsIdx].landmarkID >= 0)
971  {
972  // The sensor provides us a LM ID... use it:
973  m_IDs.insert(obs->sensedData[in_obsIdx].landmarkID, in_idxNewFeat);
974  }
975  else
976  {
977  // Features do not have IDs... use indices:
978  m_IDs.insert(in_idxNewFeat, in_idxNewFeat);
979  }
980 
981  MRPT_END
982 }
983 
984 /*---------------------------------------------------------------
985  getAs3DObject
986  ---------------------------------------------------------------*/
988  mrpt::opengl::CSetOfObjects::Ptr& outObj) const
989 {
990  outObj->clear();
991 
992  // ------------------------------------------------
993  // Add the XYZ corner for the current area:
994  // ------------------------------------------------
995  outObj->insert(opengl::stock_objects::CornerXYZ());
996 
997  // 3D ellipsoid for robot pose:
998  CPointPDFGaussian pointGauss;
999  pointGauss.mean.x(m_xkk[0]);
1000  pointGauss.mean.y(m_xkk[1]);
1001  pointGauss.mean.z(m_xkk[2]);
1003  m_pkk.extractMatrix(0, 0, 3, 3, COV);
1004  pointGauss.cov = COV;
1005 
1006  {
1007  opengl::CEllipsoid::Ptr ellip =
1008  mrpt::make_aligned_shared<opengl::CEllipsoid>();
1009 
1010  ellip->setPose(pointGauss.mean);
1011  ellip->setCovMatrix(pointGauss.cov);
1012  ellip->enableDrawSolid3D(false);
1013  ellip->setQuantiles(options.quantiles_3D_representation);
1014  ellip->set3DsegmentsCount(10);
1015  ellip->setColor(1, 0, 0);
1016 
1017  outObj->insert(ellip);
1018  }
1019 
1020  // 3D ellipsoids for landmarks:
1021  const size_t nLMs = this->getNumberOfLandmarksInTheMap();
1022  for (size_t i = 0; i < nLMs; i++)
1023  {
1024  pointGauss.mean.x(
1025  m_xkk[get_vehicle_size() + get_feature_size() * i + 0]);
1026  pointGauss.mean.y(
1027  m_xkk[get_vehicle_size() + get_feature_size() * i + 1]);
1028  pointGauss.mean.z(
1029  m_xkk[get_vehicle_size() + get_feature_size() * i + 2]);
1030  m_pkk.extractMatrix(
1033  get_feature_size(), COV);
1034  pointGauss.cov = COV;
1035 
1036  opengl::CEllipsoid::Ptr ellip =
1037  mrpt::make_aligned_shared<opengl::CEllipsoid>();
1038 
1039  ellip->setName(format("%u", static_cast<unsigned int>(i)));
1040  ellip->enableShowName(true);
1041  ellip->setPose(pointGauss.mean);
1042  ellip->setCovMatrix(pointGauss.cov);
1043  ellip->enableDrawSolid3D(false);
1044  ellip->setQuantiles(options.quantiles_3D_representation);
1045  ellip->set3DsegmentsCount(10);
1046 
1047  ellip->setColor(0, 0, 1);
1048 
1049  // Set color depending on partitions?
1051  {
1052  // This is done for each landmark:
1053  map<int, bool> belongToPartition;
1054  const CSimpleMap* SFs =
1055  &m_SFs; // mapPartitioner.getSequenceOfFrames();
1056 
1057  for (size_t p = 0; p < m_lastPartitionSet.size(); p++)
1058  {
1059  for (size_t w = 0; w < m_lastPartitionSet[p].size(); w++)
1060  {
1061  // Check if landmark #i is in the SF of
1062  // m_lastPartitionSet[p][w]:
1063  CLandmark::TLandmarkID i_th_ID = m_IDs.inverse(i);
1064 
1065  // Look for the lm_ID in the SF:
1066  CPose3DPDF::Ptr pdf;
1067  CSensoryFrame::Ptr SF_i;
1068  SFs->get(m_lastPartitionSet[p][w], pdf, SF_i);
1069 
1071  SF_i->getObservationByClass<CObservationBearingRange>();
1072 
1073  for (size_t o = 0; o < obs->sensedData.size(); o++)
1074  {
1075  if (obs->sensedData[o].landmarkID == i_th_ID)
1076  {
1077  belongToPartition[p] = true;
1078  break;
1079  }
1080  } // end for o
1081  } // end for w
1082  } // end for p
1083 
1084  // Build label:
1085  string strParts("[");
1086 
1087  for (map<int, bool>::iterator it = belongToPartition.begin();
1088  it != belongToPartition.end(); ++it)
1089  {
1090  if (it != belongToPartition.begin()) strParts += string(",");
1091  strParts += format("%i", it->first);
1092  }
1093 
1094  ellip->setName(ellip->getName() + strParts + string("]"));
1095  }
1096 
1097  outObj->insert(ellip);
1098  }
1099 }
1100 
1101 /*---------------------------------------------------------------
1102  getLastPartitionLandmarks
1103  ---------------------------------------------------------------*/
1105  size_t K, std::vector<std::vector<uint32_t>>& landmarksMembership)
1106 {
1107  // temporary copy:
1108  std::vector<std::vector<uint32_t>> tmpParts = m_lastPartitionSet;
1109 
1110  // Fake "m_lastPartitionSet":
1111 
1112  // Fixed partitions every K observations:
1113  vector<std::vector<uint32_t>> partitions;
1114  std::vector<uint32_t> tmpCluster;
1115 
1116  for (size_t i = 0; i < m_SFs.size(); i++)
1117  {
1118  tmpCluster.push_back(i);
1119  if ((i % K) == 0)
1120  {
1121  partitions.push_back(tmpCluster);
1122  tmpCluster.clear();
1123  tmpCluster.push_back(
1124  i); // This observation "i" is shared between both clusters
1125  }
1126  }
1127  m_lastPartitionSet = partitions;
1128 
1129  // Call the actual method:
1130  getLastPartitionLandmarks(landmarksMembership);
1131 
1132  // Replace copy:
1133  m_lastPartitionSet = tmpParts; //-V519
1134 }
1135 
1136 /*---------------------------------------------------------------
1137  getLastPartitionLandmarks
1138  ---------------------------------------------------------------*/
1140  std::vector<std::vector<uint32_t>>& landmarksMembership) const
1141 {
1142  landmarksMembership.clear();
1143 
1144  // All the computation is made based on "m_lastPartitionSet"
1145 
1146  if (!options.doPartitioningExperiment) return;
1147 
1148  const size_t nLMs = this->getNumberOfLandmarksInTheMap();
1149  for (size_t i = 0; i < nLMs; i++)
1150  {
1151  map<int, bool> belongToPartition;
1152  const CSimpleMap* SFs =
1153  &m_SFs; // mapPartitioner.getSequenceOfFrames();
1154 
1155  for (size_t p = 0; p < m_lastPartitionSet.size(); p++)
1156  {
1157  for (size_t w = 0; w < m_lastPartitionSet[p].size(); w++)
1158  {
1159  // Check if landmark #i is in the SF of
1160  // m_lastPartitionSet[p][w]:
1161  CLandmark::TLandmarkID i_th_ID = m_IDs.inverse(i);
1162 
1163  // Look for the lm_ID in the SF:
1164  CPose3DPDF::Ptr pdf;
1165  CSensoryFrame::Ptr SF_i;
1166  SFs->get(m_lastPartitionSet[p][w], pdf, SF_i);
1167 
1169  SF_i->getObservationByClass<CObservationBearingRange>();
1170 
1171  for (size_t o = 0; o < obs->sensedData.size(); o++)
1172  {
1173  if (obs->sensedData[o].landmarkID == i_th_ID)
1174  {
1175  belongToPartition[p] = true;
1176  break;
1177  }
1178  } // end for o
1179  } // end for w
1180  } // end for p
1181 
1182  // Build membership list:
1183  std::vector<uint32_t> membershipOfThisLM;
1184 
1185  for (map<int, bool>::iterator it = belongToPartition.begin();
1186  it != belongToPartition.end(); ++it)
1187  membershipOfThisLM.push_back(it->first);
1188 
1189  landmarksMembership.push_back(membershipOfThisLM);
1190  } // end for i
1191 }
1192 
1193 /*---------------------------------------------------------------
1194  computeOffDiagonalBlocksApproximationError
1195  ---------------------------------------------------------------*/
1197  const std::vector<std::vector<uint32_t>>& landmarksMembership) const
1198 {
1199  MRPT_START
1200 
1201  // Compute the information matrix:
1203  size_t i;
1204  for (i = 0; i < get_vehicle_size(); i++)
1205  fullCov(i, i) = max(fullCov(i, i), 1e-6);
1206 
1207  CMatrixTemplateNumeric<kftype> H(fullCov.inv());
1208  H.array().abs(); // Replace by absolute values:
1209 
1210  double sumOffBlocks = 0;
1211  unsigned int nLMs = landmarksMembership.size();
1212 
1213  ASSERT_(
1214  int(get_vehicle_size() + nLMs * get_feature_size()) == fullCov.cols());
1215 
1216  for (i = 0; i < nLMs; i++)
1217  {
1218  for (size_t j = i + 1; j < nLMs; j++)
1219  {
1220  // Sum the cross cov. between LM(i) and LM(j)??
1221  // --> Only if there is no common cluster:
1222  if (0 == math::countCommonElements(
1223  landmarksMembership[i], landmarksMembership[j]))
1224  {
1225  size_t col = get_vehicle_size() + i * get_feature_size();
1226  size_t row = get_vehicle_size() + j * get_feature_size();
1227  sumOffBlocks += 2 * H.block(row, col, 2, 2).sum();
1228  }
1229  }
1230  }
1231 
1232  return sumOffBlocks /
1233  H.block(
1235  H.rows() - get_vehicle_size(),
1236  H.cols() - get_vehicle_size())
1237  .sum(); // Starting (7,7)-end
1238  MRPT_END
1239 }
1240 
1241 /*---------------------------------------------------------------
1242  reconsiderPartitionsNow
1243  ---------------------------------------------------------------*/
1245 {
1246  // A different buffer for making this
1247  // thread-safe some day...
1248  vector<std::vector<uint32_t>> partitions;
1249  mapPartitioner.updatePartitions(partitions);
1250  m_lastPartitionSet = partitions;
1251 }
1252 
1253 /*---------------------------------------------------------------
1254  saveMapAndPathRepresentationAsMATLABFile
1255  ---------------------------------------------------------------*/
1257  const string& fil, float stdCount, const string& styleLandmarks,
1258  const string& stylePath, const string& styleRobot) const
1259 {
1260  FILE* f = os::fopen(fil.c_str(), "wt");
1261  if (!f) return;
1262 
1263  CMatrixDouble cov(2, 2);
1264  CVectorDouble mean(2);
1265 
1266  // Header:
1267  os::fprintf(
1268  f,
1269  "%%--------------------------------------------------------------------"
1270  "\n");
1271  os::fprintf(f, "%% File automatically generated using the MRPT method:\n");
1272  os::fprintf(
1273  f,
1274  "%% "
1275  "'CRangeBearingKFSLAM::saveMapAndPath2DRepresentationAsMATLABFile'\n");
1276  os::fprintf(f, "%%\n");
1277  os::fprintf(f, "%% ~ MRPT ~\n");
1278  os::fprintf(
1279  f, "%% Jose Luis Blanco Claraco, University of Malaga @ 2008\n");
1280  os::fprintf(f, "%% http://www.mrpt.org/ \n");
1281  os::fprintf(
1282  f,
1283  "%%--------------------------------------------------------------------"
1284  "\n");
1285 
1286  // Main code:
1287  os::fprintf(f, "hold on;\n\n");
1288 
1289  const size_t nLMs = this->getNumberOfLandmarksInTheMap();
1290 
1291  for (size_t i = 0; i < nLMs; i++)
1292  {
1293  size_t idx = get_vehicle_size() + i * get_feature_size();
1294 
1295  cov(0, 0) = m_pkk(idx + 0, idx + 0);
1296  cov(1, 1) = m_pkk(idx + 1, idx + 1);
1297  cov(0, 1) = cov(1, 0) = m_pkk(idx + 0, idx + 1);
1298 
1299  mean[0] = m_xkk[idx + 0];
1300  mean[1] = m_xkk[idx + 1];
1301 
1302  // Command to draw the 2D ellipse:
1303  os::fprintf(
1304  f, "%s",
1305  math::MATLAB_plotCovariance2D(cov, mean, stdCount, styleLandmarks)
1306  .c_str());
1307  }
1308 
1309  // Now: the robot path:
1310  // ------------------------------
1311  if (m_SFs.size())
1312  {
1313  os::fprintf(f, "\nROB_PATH=[");
1314  for (size_t i = 0; i < m_SFs.size(); i++)
1315  {
1316  CSensoryFrame::Ptr dummySF;
1317  CPose3DPDF::Ptr pdf3D;
1318  m_SFs.get(i, pdf3D, dummySF);
1319 
1320  CPose3D p;
1321  pdf3D->getMean(p);
1322 
1323  os::fprintf(f, "%.04f %.04f", p.x(), p.y());
1324  if (i < (m_SFs.size() - 1)) os::fprintf(f, ";");
1325  }
1326  os::fprintf(f, "];\n");
1327 
1328  os::fprintf(
1329  f, "plot(ROB_PATH(:,1),ROB_PATH(:,2),'%s');\n", stylePath.c_str());
1330  }
1331 
1332  // The robot pose:
1333  cov(0, 0) = m_pkk(0, 0);
1334  cov(1, 1) = m_pkk(1, 1);
1335  cov(0, 1) = cov(1, 0) = m_pkk(0, 1);
1336 
1337  mean[0] = m_xkk[0];
1338  mean[1] = m_xkk[1];
1339 
1340  os::fprintf(
1341  f, "%s",
1342  math::MATLAB_plotCovariance2D(cov, mean, stdCount, styleRobot).c_str());
1343 
1344  os::fprintf(f, "\naxis equal;\n");
1345  os::fclose(f);
1346 }
1347 
1348 /** Computes A=A-B, which may need to be re-implemented depending on the
1349  * topology of the individual scalar components (eg, angles).
1350  */
1352  KFArray_OBS& A, const KFArray_OBS& B) const
1353 {
1354  A -= B;
1357 }
1358 
1359 /** Return the observation NOISE covariance matrix, that is, the model of the
1360  * Gaussian additive noise of the sensor.
1361  * \param out_R The noise covariance matrix. It might be non diagonal, but
1362  * it'll usually be.
1363  */
1365 {
1366  out_R(0, 0) = square(options.std_sensor_range);
1367  out_R(1, 1) = square(options.std_sensor_yaw);
1368  out_R(2, 2) = square(options.std_sensor_pitch);
1369 }
1370 
1371 /** This will be called before OnGetObservationsAndDataAssociation to allow the
1372  * application to reduce the number of covariance landmark predictions to be
1373  * made.
1374  * For example, features which are known to be "out of sight" shouldn't be
1375  * added to the output list to speed up the calculations.
1376  * \param in_all_prediction_means The mean of each landmark predictions; the
1377  * computation or not of the corresponding covariances is what we're trying to
1378  * determined with this method.
1379  * \param out_LM_indices_to_predict The list of landmark indices in the map
1380  * [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.
1381  * \note This is not a pure virtual method, so it should be implemented only if
1382  * desired. The default implementation returns a vector with all the landmarks
1383  * in the map.
1384  * \sa OnGetObservations, OnDataAssociation
1385  */
1387  const vector_KFArray_OBS& prediction_means,
1388  std::vector<size_t>& out_LM_indices_to_predict) const
1389 {
1391  m_SF->getObservationByClass<CObservationBearingRange>();
1392  ASSERTMSG_(
1393  obs,
1394  "*ERROR*: This method requires an observation of type "
1395  "CObservationBearingRange");
1396 
1397 #define USE_HEURISTIC_PREDICTION
1398 
1399 #ifdef USE_HEURISTIC_PREDICTION
1400  const double sensor_max_range = obs->maxSensorDistance;
1401  const double fov_yaw = obs->fieldOfView_yaw;
1402  const double fov_pitch = obs->fieldOfView_pitch;
1403 
1404  const double max_vehicle_loc_uncertainty =
1405  4 * std::sqrt(
1406  m_pkk.get_unsafe(0, 0) + m_pkk.get_unsafe(1, 1) +
1407  m_pkk.get_unsafe(2, 2));
1408 #endif
1409 
1410  out_LM_indices_to_predict.clear();
1411  for (size_t i = 0; i < prediction_means.size(); i++)
1412  {
1413 #ifndef USE_HEURISTIC_PREDICTION
1414  out_LM_indices_to_predict.push_back(i);
1415 #else
1416  // Heuristic: faster but doesn't work always!
1417  if (prediction_means[i][0] <
1418  (15 + sensor_max_range + max_vehicle_loc_uncertainty +
1419  4 * options.std_sensor_range) &&
1420  fabs(prediction_means[i][1]) <
1421  (DEG2RAD(30) + 0.5 * fov_yaw + 4 * options.std_sensor_yaw) &&
1422  fabs(prediction_means[i][2]) <
1423  (DEG2RAD(30) + 0.5 * fov_pitch + 4 * options.std_sensor_pitch))
1424  {
1425  out_LM_indices_to_predict.push_back(i);
1426  }
1427 #endif
1428  }
1429 }
mrpt::math::CMatrixFixedNumeric< double, OBS_SIZE, VEH_SIZE > KFMatrix_OxV
Nearest-neighbor.
const_iterator end() const
Definition: bimap.h:47
void changeCoordinatesReference(const CPose3DQuat &newReferenceBase)
this = p (+) this.
void updatePartitions(std::vector< std::vector< uint32_t >> &partitions)
Recalculate the map/graph partitions.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
Scalar * iterator
Definition: eigen_plugins.h:26
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)...
std::string MATLAB_plotCovariance2D(const CMatrixFloat &cov22, const CVectorFloat &mean, const float &stdCount, const std::string &style=std::string("b"), const size_t &nEllipsePoints=30)
Generates a string with the MATLAB commands required to plot an confidence interval (ellipse) for a 2...
Definition: math.cpp:354
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:262
mrpt::poses::CPose3DQuat getIncrementFromOdometry() const
Return the last odometry, as a pose increment.
void OnObservationJacobians(const size_t &idx_landmark_to_predict, KFMatrix_OxV &Hx, KFMatrix_OxF &Hy) const
Implements the observation Jacobians and (when applicable) .
double RAD2DEG(const double x)
Radians to degrees.
Mahalanobis distance.
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:41
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:33
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:273
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:3721
const_iterator find_key(const KEY &k) const
Definition: bimap.h:139
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 OnNormalizeStateVector()
This method is called after the prediction and after the update, to give the user an opportunity to n...
void OnPreComputingPredictions(const vector_KFArray_OBS &in_all_prediction_means, std::vector< size_t > &out_LM_indices_to_predict) const
This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the...
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
Definition: bimap.h:70
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
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...
void OnGetAction(KFArray_ACT &out_u) const
Must return the action vector u.
void OnTransitionJacobian(KFMatrix_VxV &out_F) const
Implements the transition Jacobian .
void sphericalCoordinates(const mrpt::math::TPoint3D &point, double &out_range, double &out_yaw, double &out_pitch, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacob_dryp_dpoint=nullptr, mrpt::math::CMatrixFixedNumeric< 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...
STL namespace.
void runOneKalmanIteration()
The main entry point, executes one complete step: prediction + update.
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.
mrpt::poses::CPose3DQuat getCurrentRobotPoseMean() const
Get the current robot pose mean, as a 3D+quaternion pose.
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:63
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4178
void OnNewLandmarkAddedToMap(const size_t in_obsIdx, const size_t in_idxNewFeat)
If applicable to the given problem, do here any special handling of adding a new landmark to the map...
mrpt::math::CMatrixTemplateNumeric< kftype > Y_pred_means
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
size_t size() const
Definition: bimap.h:56
mrpt::math::CArrayNumeric< double, FEAT_SIZE > KFArray_FEAT
int64_t TLandmarkID
The type for the IDs of landmarks.
Definition: CLandmark.h:39
T square(const T x)
Inline function for the square of a number.
mrpt::math::CMatrixFixedNumeric< double, VEH_SIZE, VEH_SIZE > KFMatrix_VxV
const std::map< VALUE, KEY > & getInverseMap() const
Return a read-only reference to the internal map KEY->VALUES.
Definition: bimap.h:61
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
void composePoint(const double lx, const double ly, const double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 7 > *out_jacobian_df_dpose=nullptr) const
Computes the 3D point G such as .
Represents a probabilistic 3D (6D) movement.
void OnTransitionNoise(KFMatrix_VxV &out_Q) const
Implements the transition noise covariance .
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:62
A numeric matrix of compile-time fixed size.
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.
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
Definition: bimap.h:30
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
TKF_options KF_options
Generic options for the Kalman Filter algorithm itself.
mrpt::math::CArrayNumeric< double, OBS_SIZE > KFArray_OBS
A helper class that can convert an enum value into its textual representation, and viceversa...
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.
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
void OnSubstractObservationVectors(KFArray_OBS &A, const KFArray_OBS &B) const
Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scala...
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
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.
double x
X,Y,Z coordinates.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
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...
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:46
double kftype
The numeric type used in the Kalman Filter (default=double)
void get(size_t index, mrpt::poses::CPose3DPDF::Ptr &out_posePDF, mrpt::obs::CSensoryFrame::Ptr &out_SF) const
Access to the i&#39;th pair, first one is index &#39;0&#39;.
Definition: CSimpleMap.cpp:56
mrpt::math::CMatrixFixedNumeric< double, OBS_SIZE, OBS_SIZE > KFMatrix_OxO
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:4101
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::aligned_std_vector< KFArray_OBS > vector_KFArray_OBS
mrpt::math::CArrayDouble< 3 > m_coords
The translation vector [x,y,z].
Definition: CPose3DQuat.h:53
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:406
#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...
void OnGetObservationNoise(KFMatrix_OxO &out_R) const
Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of ...
void OnGetObservationsAndDataAssociation(vector_KFArray_OBS &out_z, std::vector< int > &out_data_association, const vector_KFArray_OBS &in_all_predictions, const KFMatrix &in_S, const std::vector< size_t > &in_lm_indices_in_S, const KFMatrix_OxO &in_R)
This is called between the KF prediction step and the update step, and the application must return th...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void OnObservationModel(const std::vector< size_t > &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const
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.
mrpt::math::CMatrixFixedNumeric< double, FEAT_SIZE, VEH_SIZE > KFMatrix_FxV
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:38
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
Definition: exceptions.h:205
CSetOfObjects::Ptr CornerXYZ(float scale=1.0)
Returns three arrows representing a X,Y,Z 3D corner.
std::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:3576
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...
#define MRPT_END
Definition: exceptions.h:266
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 OnTransitionModel(const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const
Implements the transition model .
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...
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
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:119
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
void OnInverseObservationModel(const KFArray_OBS &in_z, KFArray_FEAT &out_yn, KFMatrix_FxV &out_dyn_dxv, KFMatrix_FxO &out_dyn_dhn) const
If applicable to the given problem, this method implements the inverse observation model needed to ex...
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:255
Eigen::Matrix< typename MATRIX::Scalar, MATRIX::ColsAtCompileTime, MATRIX::ColsAtCompileTime > cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...
Definition: ops_matrices.h:148
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:55
Lightweight 3D point.
mrpt::math::CMatrixTemplateNumeric< kftype > Y_pred_covs
void insert(const mrpt::poses::CPose3DPDF *in_posePDF, const mrpt::obs::CSensoryFrame &in_SF)
Add a new pair to the sequence.
Definition: CSimpleMap.cpp:136
mrpt::obs::CActionCollection::Ptr m_action
Set up by processActionObservation.
TDataAssociationMethod
Different algorithms for data association, used in mrpt::slam::data_association.
virtual ~CRangeBearingKFSLAM()
Destructor:
GLfloat GLfloat p
Definition: glext.h:6305
const Scalar * const_iterator
Definition: eigen_plugins.h:27
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...
mrpt::math::CMatrixFixedNumeric< double, FEAT_SIZE, OBS_SIZE > KFMatrix_FxO
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
void clear()
Remove all stored pairs.
Definition: CSimpleMap.cpp:55
A gaussian distribution for 3D points.
mrpt::math::CVectorFloat stds_Q_no_odo
A 7-length vector with the std.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020