Main MRPT website > C++ reference for MRPT 1.5.6
CRangeBearingKFSLAM2D.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #include "slam-precomp.h" // Precompiled headers
11 
14 #include <mrpt/poses/CPosePDF.h>
17 #include <mrpt/math/utils.h>
18 #include <mrpt/math/wrap2pi.h>
19 #include <mrpt/utils/CTicTac.h>
20 #include <mrpt/system/os.h>
21 
24 #include <mrpt/opengl/CEllipsoid.h>
25 
26 using namespace mrpt::slam;
27 using namespace mrpt::maps;
28 using namespace mrpt::math;
29 using namespace mrpt::obs;
30 using namespace mrpt::poses;
31 using namespace mrpt::utils;
32 using namespace mrpt::system;
33 using namespace mrpt;
34 using namespace std;
35 
36 #define STATS_EXPERIMENT 0
37 #define STATS_EXPERIMENT_ALSO_NC 1
38 
39 /*---------------------------------------------------------------
40  Constructor
41  ---------------------------------------------------------------*/
43  options(),
44  m_action(),m_SF(),
45  m_IDs(),
46  m_SFs()
47 {
48  reset();
49 }
50 
51 /*---------------------------------------------------------------
52  reset
53  ---------------------------------------------------------------*/
55 {
56  m_action.clear_unique();
57  m_SF.clear_unique();
58  m_IDs.clear();
59  m_SFs.clear();
60 
61  // INIT KF STATE
62  m_xkk.assign(3,0); // State: 3D pose (x,y,phi)
63 
64  // Initial cov:
65  m_pkk.setSize(3,3);
66  m_pkk.zeros();
67 }
68 
69 /*---------------------------------------------------------------
70  Destructor
71  ---------------------------------------------------------------*/
73 {
74 }
75 
76 /*---------------------------------------------------------------
77  getCurrentRobotPose
78  ---------------------------------------------------------------*/
80  CPosePDFGaussian &out_robotPose ) const
81 {
83 
84  ASSERT_(m_xkk.size()>=3);
85 
86  // Set 6D pose mean:
87  out_robotPose.mean = CPose2D(m_xkk[0],m_xkk[1],m_xkk[2]);
88 
89  // and cov:
91  m_pkk.extractMatrix(0,0,COV);
92  out_robotPose.cov = COV;
93 
94  MRPT_END
95 }
96 
97 
98 /*---------------------------------------------------------------
99  getCurrentState
100  ---------------------------------------------------------------*/
102  CPosePDFGaussian &out_robotPose,
103  std::vector<TPoint2D> &out_landmarksPositions,
104  std::map<unsigned int,CLandmark::TLandmarkID> &out_landmarkIDs,
105  CVectorDouble &out_fullState,
106  CMatrixDouble &out_fullCovariance ) const
107 {
108  MRPT_START
109 
110  ASSERT_(m_xkk.size()>=3);
111 
112  // Set 6D pose mean:
113  out_robotPose.mean = CPose2D(m_xkk[0],m_xkk[1],m_xkk[2]);
114 
115  // and cov:
117  m_pkk.extractMatrix(0,0,COV);
118  out_robotPose.cov = COV;
119 
120 
121  // Landmarks:
122  ASSERT_( ((m_xkk.size() - 3) % 2)==0 );
123  size_t i,nLMs = (m_xkk.size() - 3) / 2;
124  out_landmarksPositions.resize(nLMs);
125  for (i=0;i<nLMs;i++)
126  {
127  out_landmarksPositions[i].x = m_xkk[3+i*2+0];
128  out_landmarksPositions[i].y = m_xkk[3+i*2+1];
129  } // end for i
130 
131  // IDs:
132  out_landmarkIDs = m_IDs.getInverseMap(); //m_IDs_inverse;
133 
134  // Full state:
135  out_fullState.resize( m_xkk.size() );
136  for (KFVector::Index i=0;i<m_xkk.size();i++)
137  out_fullState[i] = m_xkk[i];
138 
139  // Full cov:
140  out_fullCovariance = m_pkk;
141 
142  MRPT_END
143 }
144 
145 
146 /*---------------------------------------------------------------
147  processActionObservation
148  ---------------------------------------------------------------*/
150  CActionCollectionPtr &action,
151  CSensoryFramePtr &SF )
152 {
153  MRPT_START
154 
155  m_action = action;
156  m_SF = SF;
157 
158  // Sanity check:
159  ASSERT_( m_IDs.size() == this->getNumberOfLandmarksInTheMap() );
160 
161  // ===================================================================================================================
162  // Here's the meat!: Call the main method for the KF algorithm, which will call all the callback methods as required:
163  // ===================================================================================================================
165 
166  // =============================================================
167  // ADD TO SFs SEQUENCE
168  // =============================================================
170  {
171  CPosePDFGaussianPtr auxPosePDF = CPosePDFGaussian::Create();
172  getCurrentRobotPose( *auxPosePDF );
173  m_SFs.insert( auxPosePDF , SF );
174  }
175 
176  MRPT_END
177 }
178 
179 
180 /** Must return the action vector u.
181  * \param out_u The action vector which will be passed to OnTransitionModel
182  */
184 {
185  // Get odometry estimation:
186  CActionRobotMovement2DPtr actMov2D = m_action->getBestMovementEstimation();
187  CActionRobotMovement3DPtr actMov3D = m_action->getActionByClass<CActionRobotMovement3D>();
188  if (actMov3D)
189  {
190  u[0]=actMov3D->poseChange.mean.x();
191  u[1]=actMov3D->poseChange.mean.y();
192  u[2]=actMov3D->poseChange.mean.yaw();
193  }
194  else
195  if (actMov2D)
196  {
197  CPose2D estMovMean;
198  actMov2D->poseChange->getMean(estMovMean);
199  u[0]=estMovMean.x();
200  u[1]=estMovMean.y();
201  u[2]=estMovMean.phi();
202  }
203  else
204  {
205  // Left u as zeros
206  for (size_t i=0;i<3;i++) u[i]=0;
207  }
208 }
209 
210 
211 /** This virtual function musts implement the prediction model of the Kalman filter.
212  */
214  const KFArray_ACT &u,
215  KFArray_VEH &xv,
216  bool &out_skipPrediction ) const
217 {
218  MRPT_START
219 
220  // Do not update the vehicle pose & its covariance until we have some landmakrs in the map,
221  // otherwise, we are imposing a lower bound to the best uncertainty from now on:
222  if (size_t(m_xkk.size()) == get_vehicle_size() )
223  {
224  out_skipPrediction = true;
225  return;
226  }
227 
228  CPose2D robotPose(xv[0],xv[1],xv[2]);
229  CPose2D odoIncrement(u[0],u[1],u[2]);
230 
231  // Pose composition:
232  robotPose = robotPose + odoIncrement;
233 
234  xv[0]=robotPose.x();
235  xv[1]=robotPose.y();
236  xv[2]=robotPose.phi();
237 
238  MRPT_END
239 }
240 
241 /** This virtual function musts calculate the Jacobian F of the prediction model.
242  */
244 {
245  MRPT_START
246 
247  // The jacobian of the transition function: dfv_dxv
248  CActionRobotMovement2DPtr act2D = m_action->getBestMovementEstimation();
249  CActionRobotMovement3DPtr act3D = m_action->getActionByClass<CActionRobotMovement3D>();
250 
251  if (act3D && act2D) THROW_EXCEPTION("Both 2D & 3D odometry are present!?!?")
252 
253  TPoint2D Ap;
254 
255  if (act3D)
256  {
257  Ap = TPoint2D(CPose2D(act3D->poseChange.mean));
258  }
259  else if (act2D)
260  {
261  Ap = TPoint2D(act2D->poseChange->getMeanVal());
262  }
263  else
264  {
265  // No odometry at all:
266  F.setIdentity(); // Unit diagonal
267  return;
268  }
269 
270  const kftype cy = cos(m_xkk[2]);
271  const kftype sy = sin(m_xkk[2]);
272 
273  const kftype Ax = Ap.x;
274  const kftype Ay = Ap.y;
275 
276  F.setIdentity(); // Unit diagonal
277 
278  F.get_unsafe(0,2) = -Ax*sy-Ay*cy;
279  F.get_unsafe(1,2) = Ax*cy-Ay*sy;
280 
281  MRPT_END
282 }
283 
284 /** This virtual function musts calculate de noise matrix of the prediction model.
285  */
287 {
288  MRPT_START
289 
290  // The uncertainty of the 2D odometry, projected from the current position:
291  CActionRobotMovement2DPtr act2D = m_action->getBestMovementEstimation();
292  CActionRobotMovement3DPtr act3D = m_action->getActionByClass<CActionRobotMovement3D>();
293 
294  if (act3D && act2D) THROW_EXCEPTION("Both 2D & 3D odometry are present!?!?")
295 
296  CPosePDFGaussian odoIncr;
297 
298  if (!act3D && !act2D)
299  {
300  // Use constant Q:
301  Q.zeros();
302  ASSERT_(size_t(options.stds_Q_no_odo.size())==Q.getColCount())
303 
304  for (size_t i=0;i<3;i++)
305  Q.get_unsafe(i,i) = square( options.stds_Q_no_odo[i]);
306  return;
307  }
308  else
309  {
310  if (act2D)
311  {
312  // 2D odometry:
313  odoIncr = CPosePDFGaussian(*act2D->poseChange);
314  }
315  else
316  {
317  // 3D odometry:
318  odoIncr = CPosePDFGaussian(act3D->poseChange);
319  }
320  }
321 
322  odoIncr.rotateCov(m_xkk[2]);
323 
324  Q = odoIncr.cov;
325 
326  MRPT_END
327 }
328 
330  const vector_size_t &idx_landmarks_to_predict,
331  vector_KFArray_OBS &out_predictions ) const
332 {
333  MRPT_START
334 
335  // Get the sensor pose relative to the robot:
336  CObservationBearingRangePtr obs = m_SF->getObservationByClass<CObservationBearingRange>();
337  ASSERTMSG_(obs,"*ERROR*: This method requires an observation of type CObservationBearingRange")
338  const CPose2D sensorPoseOnRobot = CPose2D(obs->sensorLocationOnRobot);
339 
340  /* -------------------------------------------
341  Equations, obtained using matlab, of the relative 2D position of a landmark (xi,yi), relative
342  to a robot 2D pose (x0,y0,phi)
343  Refer to technical report "6D EKF derivation...", 2008
344 
345  x0 y0 phi0 % Robot's 2D pose
346  x0s y0s phis % Sensor's 2D pose relative to robot
347  xi yi % Absolute 2D landmark coordinates:
348 
349  Hx : dh_dxv -> Jacobian of the observation model wrt the robot pose
350  Hy : dh_dyi -> Jacobian of the observation model wrt each landmark mean position
351 
352  Sizes:
353  h: 1x2
354  Hx: 2x3
355  Hy: 2x2
356  ------------------------------------------- */
357  // Mean of the prior of the robot pose:
358  const CPose2D robotPose( m_xkk[0],m_xkk[1],m_xkk[2] );
359 
360  const size_t vehicle_size = get_vehicle_size();
361  const size_t feature_size = get_feature_size();
362 
363  const CPose2D sensorPoseAbs = robotPose + sensorPoseOnRobot;
364 
365  // ---------------------------------------------------
366  // Observation prediction
367  // ---------------------------------------------------
368  const size_t N = idx_landmarks_to_predict.size();
369  out_predictions.resize(N);
370  for (size_t i=0;i<N;i++)
371  {
372  const size_t idx_lm = idx_landmarks_to_predict[i];
374 
375  // Landmark absolute position in the map:
376  const kftype xi = m_xkk[ vehicle_size + feature_size*idx_lm + 0 ];
377  const kftype yi = m_xkk[ vehicle_size + feature_size*idx_lm + 1 ];
378 
379  const double Axi = xi-sensorPoseAbs.x();
380  const double Ayi = yi-sensorPoseAbs.y();
381 
382  out_predictions[i][0] = sqrt( square(Axi)+square(Ayi) ); // Range
383  out_predictions[i][1] = mrpt::math::wrapToPi( atan2(Ayi,Axi) - sensorPoseAbs.phi() ); // Yaw
384  }
385 
386  MRPT_END
387 }
388 
390  const size_t &idx_landmark_to_predict,
391  KFMatrix_OxV &Hx,
392  KFMatrix_OxF &Hy ) const
393 {
394  MRPT_START
395 
396  // Get the sensor pose relative to the robot:
397  CObservationBearingRangePtr obs = m_SF->getObservationByClass<CObservationBearingRange>();
398  ASSERTMSG_(obs,"*ERROR*: This method requires an observation of type CObservationBearingRange")
399  const CPose2D sensorPoseOnRobot = CPose2D(obs->sensorLocationOnRobot);
400 
401  /* -------------------------------------------
402  Equations, obtained using matlab, of the relative 2D position of a landmark (xi,yi), relative
403  to a robot 2D pose (x0,y0,phi)
404  Refer to technical report "6D EKF derivation...", 2008
405 
406  x0 y0 phi0 % Robot's 2D pose
407  x0s y0s phis % Sensor's 2D pose relative to robot
408  xi yi % Absolute 2D landmark coordinates:
409 
410  Hx : dh_dxv -> Jacobian of the observation model wrt the robot pose
411  Hy : dh_dyi -> Jacobian of the observation model wrt each landmark mean position
412 
413  Sizes:
414  h: 1x2
415  Hx: 2x3
416  Hy: 2x2
417  ------------------------------------------- */
418  // Mean of the prior of the robot pose:
419  const CPose2D robotPose( m_xkk[0],m_xkk[1],m_xkk[2] );
420 
421  const size_t vehicle_size = get_vehicle_size();
422  const size_t feature_size = get_feature_size();
423 
424  // Robot 6D pose:
425  const kftype x0 = m_xkk[0];
426  const kftype y0 = m_xkk[1];
427  const kftype phi0 = m_xkk[2];
428 
429  const kftype cphi0 = cos(phi0);
430  const kftype sphi0 = sin(phi0);
431 
432  // Sensor 2D pose on robot:
433  const kftype x0s = sensorPoseOnRobot.x();
434  const kftype y0s = sensorPoseOnRobot.y();
435  const kftype phis = sensorPoseOnRobot.phi();
436 
437  const kftype cphi0s = cos(phi0+phis);
438  const kftype sphi0s = sin(phi0+phis);
439 
440  const CPose2D sensorPoseAbs = robotPose + sensorPoseOnRobot;
441 
442  // Landmark absolute position in the map:
443  const kftype xi = m_xkk[ vehicle_size + feature_size*idx_landmark_to_predict + 0 ];
444  const kftype yi = m_xkk[ vehicle_size + feature_size*idx_landmark_to_predict + 1 ];
445 
446  // ---------------------------------------------------
447  // Generate dhi_dxv: A 2x3 block
448  // ---------------------------------------------------
449  const kftype EXP1 = -2*yi*y0s*cphi0-2*yi*y0+2*xi*y0s*sphi0-2*xi*x0-2*xi*x0s*cphi0-2*yi*x0s*sphi0+2*y0s*y0*cphi0-2*y0s*x0*sphi0+2*y0*x0s*sphi0+square(x0)+2*x0s*x0*cphi0+square(x0s)+square(y0s)+square(xi)+square(yi)+square(y0);
450  const kftype sqrtEXP1_1 = kftype(1)/sqrt(EXP1);
451 
452  const kftype EXP2 = cphi0s*xi+sphi0s*yi-sin(phis)*y0s-y0*sphi0s-x0s*cos(phis)-x0*cphi0s;
453  const kftype EXP2sq = square(EXP2);
454 
455  const kftype EXP3 = -sphi0s*xi+cphi0s*yi-cos(phis)*y0s-y0*cphi0s+x0s*sin(phis)+x0*sphi0s;
456  const kftype EXP3sq = square(EXP3);
457 
458  const kftype EXP4 = kftype(1)/(1+EXP3sq/EXP2sq);
459 
460  Hx.get_unsafe(0,0)= (-xi-sphi0*y0s+cphi0*x0s+x0)*sqrtEXP1_1;
461  Hx.get_unsafe(0,1)= (-yi+cphi0*y0s+y0+sphi0*x0s)*sqrtEXP1_1;
462  Hx.get_unsafe(0,2)= (y0s*xi*cphi0+y0s*yi*sphi0-y0*y0s*sphi0-x0*y0s*cphi0+x0s*xi*sphi0-x0s*yi*cphi0+y0*x0s*cphi0-x0s*x0*sphi0)*sqrtEXP1_1;
463 
464  Hx.get_unsafe(1,0)= (sphi0s/(EXP2)+(EXP3)/EXP2sq*cphi0s) * EXP4;
465  Hx.get_unsafe(1,1)= (-cphi0s/(EXP2)+(EXP3)/EXP2sq*sphi0s) * EXP4;
466  Hx.get_unsafe(1,2)= ((-cphi0s*xi-sphi0s*yi+y0*sphi0s+x0*cphi0s)/(EXP2)-(EXP3)/EXP2sq*(-sphi0s*xi+cphi0s*yi-y0*cphi0s+x0*sphi0s)) * EXP4;
467 
468  // ---------------------------------------------------
469  // Generate dhi_dyi: A 2x2 block
470  // ---------------------------------------------------
471  Hy.get_unsafe(0,0)= (xi+sphi0*y0s-cphi0*x0s-x0)*sqrtEXP1_1;
472  Hy.get_unsafe(0,1)= (yi-cphi0*y0s-y0-sphi0*x0s)*sqrtEXP1_1;
473 
474  Hy.get_unsafe(1,0)= (-sphi0s/(EXP2)-(EXP3)/EXP2sq*cphi0s)* EXP4;
475  Hy.get_unsafe(1,1)= (cphi0s/(EXP2)-(EXP3)/EXP2sq*sphi0s) * EXP4;
476 
477  MRPT_END
478 }
479 
480 
481 /** This is called between the KF prediction step and the update step, and the application must return the observations and, when applicable, the data association between these observations and the current map.
482  *
483  * \param out_z N vectors, each for one "observation" of length OBS_SIZE, N being the number of "observations": how many observed landmarks for a map, or just one if not applicable.
484  * \param out_data_association An empty vector or, where applicable, a vector where the i'th element corresponds to the position of the observation in the i'th row of out_z within the system state vector (in the range [0,getNumberOfLandmarksInTheMap()-1]), or -1 if it is a new map element and we want to insert it at the end of this KF iteration.
485  * \param in_S The full covariance matrix of the observation predictions (i.e. the "innovation covariance matrix"). This is a M·O x M·O matrix with M=length of "in_lm_indices_in_S".
486  * \param in_lm_indices_in_S The indices of the map landmarks (range [0,getNumberOfLandmarksInTheMap()-1]) that can be found in the matrix in_S.
487  *
488  * This method will be called just once for each complete KF iteration.
489  * \note It is assumed that the observations are independent, i.e. there are NO cross-covariances between them.
490  */
493  vector_int &data_association,
494  const vector_KFArray_OBS &all_predictions,
495  const KFMatrix &S,
496  const vector_size_t &lm_indices_in_S,
497  const KFMatrix_OxO &R
498  )
499 {
500  MRPT_START
501 
502  //static const size_t vehicle_size = get_vehicle_size();
503  static const size_t obs_size = get_observation_size();
504 
505  // Z: Observations
507 
508  CObservationBearingRangePtr obs = m_SF->getObservationByClass<CObservationBearingRange>();
509  ASSERTMSG_(obs,"*ERROR*: This method requires an observation of type CObservationBearingRange")
510 
511  const size_t N = obs->sensedData.size();
512  Z.resize(N);
513 
514  size_t row;
515  for (row=0,itObs = obs->sensedData.begin();itObs!=obs->sensedData.end();++itObs,++row)
516  {
517  // Fill one row in Z:
518  Z[row][0] =itObs->range;
519  Z[row][1] =itObs->yaw;
520  ASSERTMSG_(itObs->pitch==0,"ERROR: Observation contains pitch!=0 but this is 2D-SLAM!!!")
521  }
522 
523  // Data association:
524  // ---------------------
525  data_association.assign(N,-1); // Initially, all new landmarks
526 
527  // For each observed LM:
528  vector_size_t obs_idxs_needing_data_assoc;
529  obs_idxs_needing_data_assoc.reserve(N);
530 
531  {
534  size_t row;
535  for (row=0,itObs = obs->sensedData.begin(),itDA=data_association.begin();itObs!=obs->sensedData.end();++itObs,++itDA,++row)
536  {
537  // Fill data asociation: Using IDs!
538  if (itObs->landmarkID<0)
539  obs_idxs_needing_data_assoc.push_back(row);
540  else
541  {
543  if ( (itID = m_IDs.find_key( itObs->landmarkID )) != m_IDs.end() )
544  *itDA = itID->second; // This row in Z corresponds to the i'th map element in the state vector:
545  }
546  }
547  }
548 
549  // ---- Perform data association ----
550  // Only for observation indices in "obs_idxs_needing_data_assoc"
551  if (obs_idxs_needing_data_assoc.empty())
552  {
553  // We don't need to do DA:
555 
556  // Save them for the case the external user wants to access them:
557  for (size_t idxObs=0;idxObs<data_association.size();idxObs++)
558  {
559  int da = data_association[idxObs];
560  if (da>=0)
561  m_last_data_association.results.associations[idxObs] = data_association[idxObs];
562  }
563 
564 #if STATS_EXPERIMENT // DEBUG: Generate statistic info
565  {
566  static CFileOutputStream fC ("metric_stats_C.txt",true);
567  #if STATS_EXPERIMENT_ALSO_NC
568  static CFileOutputStream fNC("metric_stats_NC.txt",true);
569  #endif
570 
572  size_t idx_obs = 0;
573  for(itDA=data_association.begin();itDA!=data_association.end();++itDA,++idx_obs)
574  {
575  if (*itDA==-1) continue;
576  const size_t lm_idx_in_map = *itDA;
577  size_t valid_idx_pred = find_in_vector(lm_idx_in_map,lm_indices_in_S);
578 
579  const KFArray_OBS &obs_mu = Z[idx_obs];
580 
581  for (size_t idx_pred=0;idx_pred<lm_indices_in_S.size();idx_pred++)
582  {
583  const KFArray_OBS &lm_mu = all_predictions[lm_indices_in_S[idx_pred]];
584 
585  const size_t base_idx_in_S = obs_size*idx_pred;
586  KFMatrix_OxO lm_cov;
587  S.extractMatrix(base_idx_in_S,base_idx_in_S,lm_cov);
588 
589  double md,log_pdf;
590  mahalanobisDistance2AndLogPDF(lm_mu-obs_mu, lm_cov, md,log_pdf);
591 
592  if (valid_idx_pred==idx_pred)
593  fC.printf("%e %e\n",md,log_pdf);
594  else
595  {
596  #if STATS_EXPERIMENT_ALSO_NC
597  fNC.printf("%e %e\n",md,log_pdf);
598  #endif
599  }
600  }
601  }
602  }
603 #endif
604 
605  }
606  else
607  {
608  // Build a Z matrix with the observations that need dat.assoc:
609  const size_t nObsDA = obs_idxs_needing_data_assoc.size();
610 
611  CMatrixTemplateNumeric<kftype> Z_obs_means(nObsDA,obs_size);
612  for (size_t i=0;i<nObsDA;i++)
613  {
614  const size_t idx = obs_idxs_needing_data_assoc[i];
615  for (unsigned k=0;k<obs_size;k++)
616  Z_obs_means.get_unsafe(i,k) = Z[idx][k];
617  }
618 
619  // Vehicle uncertainty
621  m_pkk.extractMatrix(0,0, Pxx);
622 
623  // Build predictions:
624  // ---------------------------
625  const size_t nPredictions = lm_indices_in_S.size();
627 
628  // S is the covariance of the predictions:
630 
631  // The means:
632  m_last_data_association.Y_pred_means.setSize(nPredictions, obs_size);
633  for (size_t q=0;q<nPredictions;q++)
634  {
635  const size_t i = lm_indices_in_S[q];
636  for (size_t w=0;w<obs_size;w++)
637  m_last_data_association.Y_pred_means.get_unsafe(q,w) = all_predictions[i][w];
638  m_last_data_association.predictions_IDs.push_back( i ); // for the conversion of indices...
639  }
640 
641  // Do Dat. Assoc :
642  // ---------------------------
643  if (nPredictions)
644  {
645  CMatrixDouble Z_obs_cov = CMatrixDouble(R);
646 
648  Z_obs_means, //Z_obs_cov,
654  true, // Use KD-tree
658  );
659 
660  // Return pairings to the main KF algorithm:
662  data_association[ it->first ] = it->second;
663  }
664  }
665  // ---- End of data association ----
666 
667  MRPT_END
668 }
669 
670 
671 /** This virtual function musts normalize the state vector and covariance matrix (only if its necessary).
672  */
674 {
675  // Check angles:
677 
678 }
679 
680 /*---------------------------------------------------------------
681  loadOptions
682  ---------------------------------------------------------------*/
684 {
685  // Main
686  options.loadFromConfigFile( ini, "RangeBearingKFSLAM" );
687  KF_options.loadFromConfigFile( ini, "RangeBearingKFSLAM_KalmanFilter" );
688 }
689 
690 /*---------------------------------------------------------------
691  loadOptions
692  ---------------------------------------------------------------*/
695  const std::string &section)
696 {
698 
699  source.read_vector(section,"stds_Q_no_odo", stds_Q_no_odo, stds_Q_no_odo );
700  ASSERT_(stds_Q_no_odo.size()==3)
701 
703 
704  std_sensor_range = source.read_float(section,"std_sensor_range", std_sensor_range);
705  std_sensor_yaw = DEG2RAD( source.read_float(section,"std_sensor_yaw_deg", RAD2DEG(std_sensor_yaw)));
706 
707  MRPT_LOAD_CONFIG_VAR(quantiles_3D_representation, float, source,section);
708  MRPT_LOAD_CONFIG_VAR(create_simplemap, bool, source,section);
709 
710  data_assoc_method = source.read_enum<TDataAssociationMethod>(section,"data_assoc_method",data_assoc_method);
711  data_assoc_metric = source.read_enum<TDataAssociationMetric>(section,"data_assoc_metric",data_assoc_metric);
712  data_assoc_IC_metric = source.read_enum<TDataAssociationMetric>(section,"data_assoc_IC_metric",data_assoc_IC_metric);
713 
714  MRPT_LOAD_CONFIG_VAR(data_assoc_IC_chi2_thres,double, source, section);
715  MRPT_LOAD_CONFIG_VAR(data_assoc_IC_ml_threshold,double, source, section);
716 }
717 
718 /*---------------------------------------------------------------
719  Constructor
720  ---------------------------------------------------------------*/
722  stds_Q_no_odo(3,0),
723  std_sensor_range ( 0.1f),
724  std_sensor_yaw ( DEG2RAD( 0.5f )),
725  quantiles_3D_representation ( 3),
726  create_simplemap (false),
727  data_assoc_method (assocNN),
728  data_assoc_metric (metricMaha),
729  data_assoc_IC_chi2_thres (0.99),
730  data_assoc_IC_metric (metricMaha),
731  data_assoc_IC_ml_threshold (0.0)
732 
733 {
734  stds_Q_no_odo[0]=0.10f;
735  stds_Q_no_odo[1]=0.10f;
736  stds_Q_no_odo[2]=DEG2RAD(4.0f);
737 }
738 
739 /*---------------------------------------------------------------
740  dumpToTextStream
741  ---------------------------------------------------------------*/
743 {
744  out.printf("\n----------- [CRangeBearingKFSLAM2D::TOptions] ------------ \n\n");
745 
746  out.printf("data_assoc_method = %s\n", TEnumType<TDataAssociationMethod>::value2name(data_assoc_method).c_str() );
747  out.printf("data_assoc_metric = %s\n", TEnumType<TDataAssociationMetric>::value2name(data_assoc_metric).c_str() );
748  out.printf("data_assoc_IC_chi2_thres = %.06f\n", data_assoc_IC_chi2_thres );
749  out.printf("data_assoc_IC_metric = %s\n", TEnumType<TDataAssociationMetric>::value2name(data_assoc_IC_metric).c_str() );
750  out.printf("data_assoc_IC_ml_threshold = %.06f\n", data_assoc_IC_ml_threshold );
751 
752  out.printf("\n");
753 }
754 
756  const KFArray_OBS & in_z,
757  KFArray_FEAT & yn,
758  KFMatrix_FxV & dyn_dxv,
759  KFMatrix_FxO & dyn_dhn ) const
760 {
761  MRPT_START
762 
763  /* -------------------------------------------
764  Equations, obtained using matlab
765 
766  x0 y0 phi0 % Robot's 2D pose
767  x0s y0s phis % Sensor's 2D pose relative to robot
768  hr ha % Observation hn: range, yaw
769 
770  xi yi % Absolute 2D landmark coordinates:
771 
772  dyn_dxv -> Jacobian of the inv. observation model wrt the robot pose
773  dyn_dhn -> Jacobian of the inv. observation model wrt each landmark observation
774 
775  Sizes:
776  hn: 1x2 <--
777  yn: 1x2 -->
778  dyn_dxv: 2x3 -->
779  dyn_dhn: 2x2 -->
780  ------------------------------------------- */
781 
782  CObservationBearingRangePtr obs = m_SF->getObservationByClass<CObservationBearingRange>();
783  ASSERTMSG_(obs,"*ERROR*: This method requires an observation of type CObservationBearingRange")
784  const CPose2D sensorPoseOnRobot = CPose2D( obs->sensorLocationOnRobot );
785 
786  // Mean of the prior of the robot pose:
787  const TPose2D robotPose( m_xkk[0],m_xkk[1],m_xkk[2] );
788 
789  // Robot 2D pose:
790  const kftype x0 = m_xkk[0];
791  const kftype y0 = m_xkk[1];
792  const kftype phi0 = m_xkk[2];
793 
794  const kftype cphi0 = cos(phi0);
795  const kftype sphi0 = sin(phi0);
796 
797  // Sensor 6D pose on robot:
798  const kftype x0s = sensorPoseOnRobot.x();
799  const kftype y0s = sensorPoseOnRobot.y();
800  const kftype phis = sensorPoseOnRobot.phi();
801 
802  const kftype hr = in_z[0];
803  const kftype ha = in_z[1];
804 
805  const kftype cphi_0sa = cos(phi0+phis+ha);
806  const kftype sphi_0sa = sin(phi0+phis+ha);
807 
808  // Compute the mean 2D absolute coordinates:
809  yn[0] = hr*cphi_0sa+cphi0*x0s-sphi0*y0s+x0;
810  yn[1] = hr*sphi_0sa+sphi0*x0s+cphi0*y0s+y0;
811 
812  // Jacobian wrt xv:
813  dyn_dxv.get_unsafe(0,0) = 1;
814  dyn_dxv.get_unsafe(0,1) = 0;
815  dyn_dxv.get_unsafe(0,2) = -hr*sphi_0sa-sphi0*x0s-cphi0*y0s;
816 
817  dyn_dxv.get_unsafe(1,0) = 0;
818  dyn_dxv.get_unsafe(1,1) = 1;
819  dyn_dxv.get_unsafe(1,2) = hr*cphi_0sa+cphi0*x0s-sphi0*y0s;
820 
821  // Jacobian wrt hn:
822  dyn_dhn.get_unsafe(0,0) = cphi_0sa;
823  dyn_dhn.get_unsafe(0,1) = -hr*sphi_0sa;
824 
825  dyn_dhn.get_unsafe(1,0) = sphi_0sa;
826  dyn_dhn.get_unsafe(1,1) = hr*cphi_0sa;
827 
828  MRPT_END
829 }
830 
831 
832 /** If applicable to the given problem, do here any special handling of adding a new landmark to the map.
833  * \param in_obsIndex The index of the observation whose inverse sensor is to be computed. It corresponds to the row in in_z where the observation can be found.
834  * \param in_idxNewFeat The index that this new feature will have in the state vector (0:just after the vehicle state, 1: after that,...). Save this number so data association can be done according to these indices.
835  * \sa OnInverseObservationModel
836  */
838  const size_t in_obsIdx,
839  const size_t in_idxNewFeat )
840 {
841  MRPT_START
842 
843  CObservationBearingRangePtr obs = m_SF->getObservationByClass<CObservationBearingRange>();
844  ASSERTMSG_(obs,"*ERROR*: This method requires an observation of type CObservationBearingRange")
845 
846  // ----------------------------------------------
847  // introduce in the lists of ID<->index in map:
848  // ----------------------------------------------
849  ASSERT_( in_obsIdx < obs->sensedData.size() )
850 
851  if (obs->sensedData[in_obsIdx].landmarkID>=0)
852  {
853  // The sensor provides us a LM ID... use it:
854  m_IDs.insert(
855  obs->sensedData[in_obsIdx].landmarkID,
856  in_idxNewFeat );
857  }
858  else
859  {
860  // Features do not have IDs... use indices:
861  m_IDs.insert( in_idxNewFeat,in_idxNewFeat);
862  }
863 
864  m_last_data_association.newly_inserted_landmarks[in_obsIdx] = in_idxNewFeat; // Just for stats, etc...
865 
866  MRPT_END
867 }
868 
869 
870 /*---------------------------------------------------------------
871  getAs3DObject
872  ---------------------------------------------------------------*/
873 void CRangeBearingKFSLAM2D::getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const
874 {
875  outObj->clear();
876 
877  // ------------------------------------------------
878  // Add the XYZ corner for the current area:
879  // ------------------------------------------------
880  outObj->insert( opengl::stock_objects::CornerXYZ() );
881 
882 
883  // 2D ellipsoid for robot pose:
884  CPoint2DPDFGaussian pointGauss;
885  pointGauss.mean.x( m_xkk[0] );
886  pointGauss.mean.y( m_xkk[1] );
888  m_pkk.extractMatrix(0,0,2,2, COV);
889  pointGauss.cov = COV;
890 
891  {
892  opengl::CEllipsoidPtr ellip = opengl::CEllipsoid::Create();
893 
894  ellip->setPose(pointGauss.mean);
895  ellip->setCovMatrix(pointGauss.cov);
896  ellip->enableDrawSolid3D(false);
897  ellip->setQuantiles( options.quantiles_3D_representation );
898  ellip->set3DsegmentsCount(10);
899  ellip->setColor(1,0,0);
900 
901  outObj->insert( ellip );
902  }
903 
904 
905  // 2D ellipsoids for landmarks:
906  const size_t nLMs = (m_xkk.size()-3)/2;
907  for (size_t i=0;i<nLMs;i++)
908  {
909  pointGauss.mean.x( m_xkk[3+2*i+0] );
910  pointGauss.mean.y( m_xkk[3+2*i+1] );
911  m_pkk.extractMatrix(3+2*i,3+2*i,2,2, COV);
912  pointGauss.cov = COV;
913 
914  opengl::CEllipsoidPtr ellip = opengl::CEllipsoid::Create();
915 
916  ellip->setName( format( "%u",static_cast<unsigned int>(i) ) );
917  ellip->enableShowName(true);
918  ellip->setPose( pointGauss.mean );
919  ellip->setCovMatrix( pointGauss.cov );
920  ellip->enableDrawSolid3D(false);
921  ellip->setQuantiles( options.quantiles_3D_representation );
922  ellip->set3DsegmentsCount(10);
923 
924  ellip->setColor(0,0,1);
925 
926  outObj->insert( ellip );
927  }
928 }
929 
930 
931 
932 /*---------------------------------------------------------------
933  saveMapAndPathRepresentationAsMATLABFile
934  ---------------------------------------------------------------*/
936  const string &fil,
937  float stdCount,
938  const string &styleLandmarks,
939  const string &stylePath,
940  const string &styleRobot ) const
941 {
942  FILE *f= os::fopen(fil.c_str(),"wt");
943  if (!f) return;
944 
945  CMatrixDouble cov(2,2);
946  CVectorDouble mean(2);
947 
948  // Header:
949  os::fprintf(f,"%%--------------------------------------------------------------------\n");
950  os::fprintf(f,"%% File automatically generated using the MRPT method:\n");
951  os::fprintf(f,"%% 'CRangeBearingKFSLAM2D::saveMapAndPath2DRepresentationAsMATLABFile'\n");
952  os::fprintf(f,"%%\n");
953  os::fprintf(f,"%% ~ MRPT ~\n");
954  os::fprintf(f,"%% Jose Luis Blanco Claraco, University of Malaga @ 2008\n");
955  os::fprintf(f,"%% http://www.mrpt.org/ \n");
956  os::fprintf(f,"%%--------------------------------------------------------------------\n");
957 
958  // Main code:
959  os::fprintf(f,"hold on;\n\n");
960 
961  size_t i, nLMs = (m_xkk.size()-get_vehicle_size())/get_feature_size();
962 
963  for (i=0;i<nLMs;i++)
964  {
965  size_t idx = get_vehicle_size()+i*get_feature_size();
966 
967  cov(0,0) = m_pkk(idx+0,idx+0);
968  cov(1,1) = m_pkk(idx+1,idx+1);
969  cov(0,1) = cov(1,0) = m_pkk(idx+0,idx+1);
970 
971  mean[0] = m_xkk[idx+0];
972  mean[1] = m_xkk[idx+1];
973 
974  // Command to draw the 2D ellipse:
975  os::fprintf(f, "%s", math::MATLAB_plotCovariance2D( cov, mean, stdCount,styleLandmarks ).c_str() );
976  }
977 
978 
979  // Now: the robot path:
980  // ------------------------------
981  if (m_SFs.size())
982  {
983  os::fprintf(f,"\nROB_PATH=[");
984  for (i=0;i<m_SFs.size();i++)
985  {
986  CSensoryFramePtr dummySF;
987  CPose3DPDFPtr pdf3D;
988  m_SFs.get(i,pdf3D,dummySF);
989 
990  CPose3D p;
991  pdf3D->getMean(p);
992  CPoint3D pnt3D(p); // 6D -> 3D only
993 
994  os::fprintf(f,"%.04f %.04f", pnt3D.x(), pnt3D.y() );
995  if (i<(m_SFs.size()-1))
996  os::fprintf(f,";");
997  }
998  os::fprintf(f,"];\n");
999 
1000  os::fprintf(f,"plot(ROB_PATH(:,1),ROB_PATH(:,2),'%s');\n",stylePath.c_str());
1001  }
1002 
1003  // The robot pose:
1004  cov(0,0) = m_pkk(0,0);
1005  cov(1,1) = m_pkk(1,1);
1006  cov(0,1) = cov(1,0) = m_pkk(0,1);
1007 
1008  mean[0] = m_xkk[0];
1009  mean[1] = m_xkk[1];
1010 
1011  os::fprintf(f, "%s", math::MATLAB_plotCovariance2D( cov, mean, stdCount, styleRobot ).c_str() );
1012 
1013  os::fprintf(f,"\naxis equal;\n");
1014  os::fclose(f);
1015 }
1016 
1017 
1018 /** Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scalar components (eg, angles).
1019  */
1021 {
1022  A[0]-=B[0];
1023  A[1]-=B[1];
1025 }
1026 
1027 
1028 /** Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor.
1029  * \param out_R The noise covariance matrix. It might be non diagonal, but it'll usually be.
1030  */
1032 {
1033  out_R(0,0)= square( options.std_sensor_range );
1034  out_R(1,1)= square( options.std_sensor_yaw );
1035 }
1036 
1037 /** This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the number of covariance landmark predictions to be made.
1038  * For example, features which are known to be "out of sight" shouldn't be added to the output list to speed up the calculations.
1039  * \param in_all_prediction_means The mean of each landmark predictions; the computation or not of the corresponding covariances is what we're trying to determined with this method.
1040  * \param out_LM_indices_to_predict The list of landmark indices in the map [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.
1041  * \note This is not a pure virtual method, so it should be implemented only if desired. The default implementation returns a vector with all the landmarks in the map.
1042  * \sa OnGetObservations, OnDataAssociation
1043  */
1045  const vector_KFArray_OBS &prediction_means,
1046  vector_size_t &out_LM_indices_to_predict ) const
1047 {
1048  CObservationBearingRangePtr obs = m_SF->getObservationByClass<CObservationBearingRange>();
1049  ASSERTMSG_(obs,"*ERROR*: This method requires an observation of type CObservationBearingRange")
1050 
1051  const double sensor_max_range = obs->maxSensorDistance;
1052  const double fov_yaw = obs->fieldOfView_yaw;
1053 
1054  const double max_vehicle_loc_uncertainty = 4 * std::sqrt( m_pkk.get_unsafe(0,0) + m_pkk.get_unsafe(1,1) );
1055  const double max_vehicle_ang_uncertainty = 4 * std::sqrt( m_pkk.get_unsafe(2,2) );
1056 
1057  out_LM_indices_to_predict.clear();
1058  for (size_t i=0;i<prediction_means.size();i++)
1059 #if (!STATS_EXPERIMENT) // In the experiment we force errors too far and some predictions are out of range, so just generate all of them:
1060  if ( prediction_means[i][0] < ( 1.5 + sensor_max_range + max_vehicle_loc_uncertainty + 4*options.std_sensor_range) &&
1061  fabs(prediction_means[i][1]) < (DEG2RAD(20) + 0.5*fov_yaw + max_vehicle_ang_uncertainty + 4*options.std_sensor_yaw)
1062  )
1063 #endif
1064  {
1065  out_LM_indices_to_predict.push_back(i);
1066  }
1067 }
1068 
1069 
1070 /** Only called if using a numeric approximation of the transition Jacobian, this method must return the increments in each dimension of the vehicle state vector while estimating the Jacobian.
1071  */
1073 {
1074  for (size_t i=0;i<get_vehicle_size();i++) out_increments[i] = 1e-6;
1075 }
1076 
1077 
1078 /** Only called if using a numeric approximation of the observation Jacobians, this method must return the increments in each dimension of the vehicle state vector while estimating the Jacobian.
1079  */
1081  KFArray_VEH &out_veh_increments,
1082  KFArray_FEAT &out_feat_increments ) const
1083 {
1084  for (size_t i=0;i<get_vehicle_size();i++) out_veh_increments[i] = 1e-6;
1085  for (size_t i=0;i<get_feature_size();i++) out_feat_increments[i] = 1e-6;
1086 }
mrpt::math::CVectorFloat stds_Q_no_odo
A 3-length vector with the std. deviation of the transition model in (x,y,phi) used only when there i...
Nearest-neighbor.
void OnTransitionJacobian(KFMatrix_VxV &out_F) const
Implements the transition Jacobian .
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
FILE BASE_IMPEXP * fopen(const char *fileName, const char *mode) MRPT_NO_THROWS
An OS-independent version of fopen.
Definition: os.cpp:255
static CEllipsoidPtr Create()
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
std::string BASE_IMPEXP 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:1905
GLenum GLenum GLvoid * row
Definition: glew.h:2903
double y
X,Y coordinates.
CPose2D mean
The mean value.
void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const
Returns a 3D representation of the landmarks in the map and the robot 3D position according to the cu...
void loadOptions(const mrpt::utils::CConfigFileBase &ini)
Load options from a ini-like file/text.
TOptions options
The options for the algorithm.
Mahalanobis distance.
void OnObservationJacobiansNumericGetIncrements(KFArray_VEH &out_veh_increments, KFArray_FEAT &out_feat_increments) const
Only called if using a numeric approximation of the observation Jacobians, this method must return th...
ENUMTYPE read_enum(const std::string &section, const std::string &name, const ENUMTYPE &defaultValue, bool failIfNotFound=false) const
Reads an "enum" value, where the value in the config file can be either a numerical value or the symb...
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
A gaussian distribution for 2D points.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &iniFile, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
int BASE_IMPEXP void BASE_IMPEXP fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:272
std::map< size_t, size_t > newly_inserted_landmarks
Map from the 0-based index within the last observation and the landmark 0-based index in the map (the...
bool create_simplemap
Whether to fill m_SFs (default=false)
#define THROW_EXCEPTION(msg)
Scalar * iterator
Definition: eigen_plugins.h:23
GLubyte GLubyte GLubyte GLubyte w
Definition: glew.h:1797
void reset()
Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,0,0).
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
Definition: CArrayNumeric.h:25
void read_vector(const std::string &section, const std::string &name, const VECTOR_TYPE &defaultValue, VECTOR_TYPE &outValues, bool failIfNotFound=false) const
Reads a configuration parameter of type vector, stored in the file as a string: "[v1 v2 v3 ...
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
void get(size_t index, mrpt::poses::CPose3DPDFPtr &out_posePDF, mrpt::obs::CSensoryFramePtr &out_SF) const
Access to the i'th pair, first one is index '0'.
Definition: CSimpleMap.cpp:95
int BASE_IMPEXP fprintf(FILE *fil, const char *format,...) MRPT_NO_THROWS MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:412
CSetOfObjectsPtr OPENGL_IMPEXP CornerXYZ(float scale=1.0)
Returns three arrows representing a X,Y,Z 3D corner.
void runOneKalmanIteration()
The main entry point, executes one complete step: prediction + update.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
KFMatrix m_pkk
The system full covariance matrix.
mrpt::utils::bimap< mrpt::maps::CLandmark::TLandmarkID, unsigned int > m_IDs
The mapping between landmark IDs and indexes in the Pkk cov.
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...
void OnPreComputingPredictions(const vector_KFArray_OBS &in_all_prediction_means, vector_size_t &out_LM_indices_to_predict) const
This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the...
#define STATS_EXPERIMENT
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
mrpt::math::CMatrixTemplateNumeric< kftype > Y_pred_means
void OnNormalizeStateVector()
This method is called after the prediction and after the update, to give the user an opportunity to n...
This class allows loading and storing values and vectors of different types from a configuration text...
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
mrpt::maps::CSimpleMap m_SFs
The sequence of all the observations and the robot path (kept for debugging, statistics,etc)
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...
size_t size() const
Returns the count of pairs (pose,sensory data)
Definition: CSimpleMap.cpp:67
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...
Represents a probabilistic 3D (6D) movement.
mrpt::math::CMatrixTemplateNumeric< kftype > Y_pred_covs
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:61
A numeric matrix of compile-time fixed size.
void OnGetAction(KFArray_ACT &out_u) const
Must return the action vector u.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
void mahalanobisDistance2AndLogPDF(const VECLIKE &diff_mean, const MATRIXLIKE &cov, T &maha2_out, T &log_pdf_out)
Computes both, the logarithm of the PDF and the square Mahalanobis distance between a point (given by...
Definition: data_utils.h:205
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
#define MRPT_END
TKF_options KF_options
Generic options for the Kalman Filter algorithm itself.
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:135
This CStream derived class allow using a file as a write-only, binary stream.
void OnGetObservationNoise(KFMatrix_OxO &out_R) const
Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of ...
A helper class that can convert an enum value into its textual representation, and viceversa...
TDataAssociationMetric
Different metrics for data association, used in mrpt::slam::data_association For a comparison of both...
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
mrpt::aligned_containers< KFArray_OBS >::vector_t vector_KFArray_OBS
double data_assoc_IC_ml_threshold
Only if data_assoc_IC_metric==ML, the log-ML threshold (Default=0.0)
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
void OnObservationJacobians(const size_t &idx_landmark_to_predict, KFMatrix_OxV &Hx, KFMatrix_OxF &Hy) const
Implements the observation Jacobians and (when applicable) .
mrpt::obs::CActionCollectionPtr m_action
Set up by processActionObservation.
std::map< KEY, VALUE >::iterator iterator
Definition: bimap.h:36
GLfloat GLfloat p
Definition: glew.h:10113
#define DEG2RAD
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:51
A class used to store a 3D point.
Definition: CPoint3D.h:32
void getCurrentState(mrpt::poses::CPosePDFGaussian &out_robotPose, std::vector< mrpt::math::TPoint2D > &out_landmarksPositions, std::map< unsigned int, mrpt::maps::CLandmark::TLandmarkID > &out_landmarkIDs, mrpt::math::CVectorDouble &out_fullState, mrpt::math::CMatrixDouble &out_fullCovariance) const
Returns the complete mean and cov.
TDataAssocInfo m_last_data_association
Last data association.
KFTYPE kftype
The numeric type used in the Kalman Filter (default=double)
CRangeBearingKFSLAM2D()
Default constructor.
#define MRPT_START
#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 OnTransitionNoise(KFMatrix_VxV &out_Q) const
Implements the transition noise covariance .
#define RAD2DEG
void OnObservationModel(const vector_size_t &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const
Implements the observation prediction .
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...
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
double data_assoc_IC_chi2_thres
Threshold in [0,1] for the chi2square test for individual compatibility between predictions and obser...
GLsizei const GLcharARB ** string
Definition: glew.h:3293
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
float std_sensor_yaw
The std. deviation of the sensor (for the matrix R in the kalman filters), in meters and radians...
TDataAssociationMetric data_assoc_IC_metric
Whether to use mahalanobis (->chi2 criterion) vs. Matching likelihood.
This observation represents a number of range-bearing value pairs, each one for a detected landmark...
mrpt::obs::CSensoryFramePtr m_SF
Set up by processActionObservation.
void getCurrentRobotPose(mrpt::poses::CPosePDFGaussian &out_robotPose) const
Returns the mean & 3x3 covariance matrix of the robot 2D pose.
Lightweight 2D pose.
#define ASSERT_(f)
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
std::vector< size_t > vector_size_t
Definition: types_simple.h:25
std::vector< int32_t > vector_int
Definition: types_simple.h:23
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
GLdouble GLdouble GLdouble GLdouble q
Definition: glew.h:1319
size_t find_in_vector(const T &value, const CONTAINER &vect)
Returns the index of the value "T" in the container "vect" (std::vector,std::deque,etc), or string::npos if not found.
void insert(const mrpt::poses::CPose3DPDF *in_posePDF, const mrpt::obs::CSensoryFrame &in_SF)
Add a new pair to the sequence.
Definition: CSimpleMap.cpp:201
void rotateCov(const double ang)
Rotate the covariance matrix by replacing it by , where .
static size_type size()
Definition: CPose2D.h:251
TDataAssociationMethod
Different algorithms for data association, used in mrpt::slam::data_association.
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...
void OnGetObservationsAndDataAssociation(vector_KFArray_OBS &out_z, vector_int &out_data_association, const vector_KFArray_OBS &in_all_predictions, const KFMatrix &in_S, const 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...
Lightweight 2D point.
#define ASSERTMSG_(f, __ERROR_MSG)
mrpt::math::CMatrixDouble22 cov
The 2x2 covariance matrix.
void OnTransitionJacobianNumericGetIncrements(KFArray_VEH &out_increments) const
Only called if using a numeric approximation of the transition Jacobian, this method must return the ...
void SLAM_IMPEXP 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 processActionObservation(mrpt::obs::CActionCollectionPtr &action, mrpt::obs::CSensoryFramePtr &SF)
Process one new action and observations to update the map and robot pose estimate.
GLsizei GLsizei GLchar * source
Definition: glew.h:1739
void OnTransitionModel(const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const
Implements the transition model .
void clear()
Remove all stored pairs.
Definition: CSimpleMap.cpp:79
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
Definition: CStream.cpp:507
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018