Main MRPT website > C++ reference for MRPT 1.5.7
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-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 
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/utils/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::utils;
39 using namespace mrpt::system;
40 using namespace mrpt;
41 using namespace std;
42 
43 
44 /*---------------------------------------------------------------
45  Constructor
46  ---------------------------------------------------------------*/
48  options(),
49  m_action(),m_SF(),
50  m_IDs(),
51  mapPartitioner(),
52  m_SFs(),
53  m_lastPartitionSet()
54 {
55  reset();
56 }
57 
58 /*---------------------------------------------------------------
59  reset
60  ---------------------------------------------------------------*/
62 {
63  m_action.clear_unique();
64  m_SF.clear_unique();
65  m_IDs.clear();
66  m_SFs.clear();
68  m_lastPartitionSet.clear();
69 
70  // -----------------------
71  // INIT KF STATE
72  m_xkk.assign(get_vehicle_size(),0); // State: 6D pose (x,y,z)=(0,0,0)
73  m_xkk[3]=1.0; // (qr,qx,qy,qz)=(1,0,0,0)
74 
75  // Initial cov: NULL diagonal -> perfect knowledge.
77  m_pkk.zeros();
78  // -----------------------
79 
80  // Use SF-based matching (faster & easier for bearing-range observations with ID).
82 }
83 
84 /*---------------------------------------------------------------
85  Destructor
86  ---------------------------------------------------------------*/
88 {
89 
90 }
91 
92 /*---------------------------------------------------------------
93  getCurrentRobotPose
94  ---------------------------------------------------------------*/
96  CPose3DQuatPDFGaussian &out_robotPose ) const
97 {
99 
100  ASSERT_(m_xkk.size()>=7);
101 
102  // Copy xyz+quat: (explicitly unroll the loop)
103  out_robotPose.mean.m_coords[0] = m_xkk[0];
104  out_robotPose.mean.m_coords[1] = m_xkk[1];
105  out_robotPose.mean.m_coords[2] = m_xkk[2];
106  out_robotPose.mean.m_quat [0] = m_xkk[3];
107  out_robotPose.mean.m_quat [1] = m_xkk[4];
108  out_robotPose.mean.m_quat [2] = m_xkk[5];
109  out_robotPose.mean.m_quat [3] = m_xkk[6];
110 
111  // and cov:
112  m_pkk.extractMatrix(0,0,out_robotPose.cov);
113 
114  MRPT_END
115 }
116 
117 /*---------------------------------------------------------------
118  getCurrentRobotPoseMean
119  ---------------------------------------------------------------*/
121 {
123  ASSERTDEB_(m_xkk.size()>=7)
124  // Copy xyz+quat: (explicitly unroll the loop)
125  q.m_coords[0] = m_xkk[0];
126  q.m_coords[1] = m_xkk[1];
127  q.m_coords[2] = m_xkk[2];
128  q.m_quat [0] = m_xkk[3];
129  q.m_quat [1] = m_xkk[4];
130  q.m_quat [2] = m_xkk[5];
131  q.m_quat [3] = m_xkk[6];
132 
133  return q;
134 }
135 
136 
137 /*---------------------------------------------------------------
138  getCurrentState
139  ---------------------------------------------------------------*/
141  CPose3DQuatPDFGaussian &out_robotPose,
142  std::vector<TPoint3D> &out_landmarksPositions,
143  std::map<unsigned int,CLandmark::TLandmarkID> &out_landmarkIDs,
144  CVectorDouble &out_fullState,
145  CMatrixDouble &out_fullCovariance ) const
146 {
147  MRPT_START
148 
149  ASSERT_(size_t(m_xkk.size())>=get_vehicle_size());
150 
151  // Copy xyz+quat: (explicitly unroll the loop)
152  out_robotPose.mean.m_coords[0] = m_xkk[0];
153  out_robotPose.mean.m_coords[1] = m_xkk[1];
154  out_robotPose.mean.m_coords[2] = m_xkk[2];
155  out_robotPose.mean.m_quat [0] = m_xkk[3];
156  out_robotPose.mean.m_quat [1] = m_xkk[4];
157  out_robotPose.mean.m_quat [2] = m_xkk[5];
158  out_robotPose.mean.m_quat [3] = m_xkk[6];
159 
160  // and cov:
161  m_pkk.extractMatrix(0,0,out_robotPose.cov);
162 
163  // Landmarks:
164  ASSERT_( ((m_xkk.size() - get_vehicle_size()) % get_feature_size())==0 );
165  size_t i,nLMs = (m_xkk.size() - get_vehicle_size()) / get_feature_size();
166  out_landmarksPositions.resize(nLMs);
167  for (i=0;i<nLMs;i++)
168  {
169  out_landmarksPositions[i].x = m_xkk[get_vehicle_size()+i*get_feature_size()+0];
170  out_landmarksPositions[i].y = m_xkk[get_vehicle_size()+i*get_feature_size()+1];
171  out_landmarksPositions[i].z = m_xkk[get_vehicle_size()+i*get_feature_size()+2];
172  } // end for i
173 
174  // IDs:
175  out_landmarkIDs = m_IDs.getInverseMap(); //m_IDs_inverse;
176 
177  // Full state:
178  out_fullState.resize( m_xkk.size() );
179  for (KFVector::Index i=0;i<m_xkk.size();i++)
180  out_fullState[i] = m_xkk[i];
181 
182  // Full cov:
183  out_fullCovariance = m_pkk;
184 
185  MRPT_END
186 }
187 
188 
189 /*---------------------------------------------------------------
190  processActionObservation
191  ---------------------------------------------------------------*/
193  CActionCollectionPtr &action,
194  CSensoryFramePtr &SF )
195 {
196  MRPT_START
197 
198  m_action = action;
199  m_SF = SF;
200 
201  // Sanity check:
202  ASSERT_( m_IDs.size() == (m_pkk.getColCount() - get_vehicle_size() )/get_feature_size() );
203 
204  // ===================================================================================================================
205  // Here's the meat!: Call the main method for the KF algorithm, which will call all the callback methods as required:
206  // ===================================================================================================================
208 
209 
210  // =============================================================
211  // ADD TO SFs SEQUENCE
212  // =============================================================
214  this->getCurrentRobotPose(q);
215  CPose3DPDFGaussianPtr auxPosePDF = CPose3DPDFGaussianPtr(new CPose3DPDFGaussian(q));
216 
218  {
219  m_SFs.insert( CPose3DPDFPtr(auxPosePDF) , SF );
220  }
221 
222  // =============================================================
223  // UPDATE THE PARTITION GRAPH EXPERIMENT
224  // =============================================================
226  {
228  {
229  // Use spectral-graph technique:
230  mapPartitioner.addMapFrame( SF, auxPosePDF );
231 
232  vector<vector_uint> partitions;
233  mapPartitioner.updatePartitions( partitions );
234  m_lastPartitionSet = partitions;
235  }
236  else
237  {
238  // Fixed partitions every K observations:
239  vector<vector_uint> partitions;
240  vector_uint tmpCluster;
241 
243  size_t K = options.partitioningMethod;
244 
245  for (size_t i=0;i<m_SFs.size();i++)
246  {
247  tmpCluster.push_back(i);
248  if ( (i % K) == 0 )
249  {
250  partitions.push_back(tmpCluster);
251  tmpCluster.clear();
252  tmpCluster.push_back(i); // This observation "i" is shared between both clusters
253  }
254  }
255  m_lastPartitionSet = partitions;
256  }
257 
258  printf("Partitions: %u\n", static_cast<unsigned>(m_lastPartitionSet.size() ));
259  }
260 
261  MRPT_END
262 }
263 
264 /** Return the last odometry, as a pose increment.
265  */
267 {
268  CActionRobotMovement2DPtr actMov2D = m_action->getBestMovementEstimation();
269  CActionRobotMovement3DPtr actMov3D = m_action->getActionByClass<CActionRobotMovement3D>();
270  if (actMov3D && !options.force_ignore_odometry)
271  {
272  return CPose3DQuat( actMov3D->poseChange.mean );
273  }
274  else if (actMov2D && !options.force_ignore_odometry)
275  {
276  CPose2D estMovMean;
277  actMov2D->poseChange->getMean(estMovMean);
278  return CPose3DQuat( CPose3D(estMovMean) );
279  }
280  else
281  {
282  return CPose3DQuat();
283  }
284 }
285 
286 /** Must return the action vector u.
287  * \param out_u The action vector which will be passed to OnTransitionModel
288  */
289 void CRangeBearingKFSLAM::OnGetAction( KFArray_ACT &u ) const
290 {
291  // Get odometry estimation:
292  const CPose3DQuat theIncr = getIncrementFromOdometry();
293 
294  for (KFArray_ACT::Index i=0;i<u.size();i++)
295  u[i] = theIncr[i];
296 }
297 
298 /** This virtual function musts implement the prediction model of the Kalman filter.
299  */
301  const KFArray_ACT &u,
302  KFArray_VEH &xv,
303  bool &out_skipPrediction ) const
304 {
305  MRPT_START
306 
307  // Do not update the vehicle pose & its covariance until we have some landmarks in the map,
308  // otherwise, we are imposing a lower bound to the best uncertainty from now on:
309  if (size_t(m_xkk.size()) == get_vehicle_size() )
310  {
311  out_skipPrediction = true;
312  }
313 
314  // Current pose: copy xyz+quat
315  CPose3DQuat robotPose = getCurrentRobotPoseMean();
316 
317  // Increment pose: copy xyz+quat (explicitly unroll the loop)
319  odoIncrement.m_coords[0] = u[0];
320  odoIncrement.m_coords[1] = u[1];
321  odoIncrement.m_coords[2] = u[2];
322  odoIncrement.m_quat [0] = u[3];
323  odoIncrement.m_quat [1] = u[4];
324  odoIncrement.m_quat [2] = u[5];
325  odoIncrement.m_quat [3] = u[6];
326 
327  // Pose composition:
328  robotPose += odoIncrement;
329 
330  // Output:
331  for (size_t i=0;i<xv.static_size;i++)
332  xv[i] = robotPose[i];
333 
334  MRPT_END
335 }
336 
337 /** This virtual function musts calculate the Jacobian F of the prediction model.
338  */
339 void CRangeBearingKFSLAM::OnTransitionJacobian( KFMatrix_VxV &F ) const
340 {
341  MRPT_START
342 
343  // Current pose: copy xyz+quat
344  CPose3DQuat robotPose = getCurrentRobotPoseMean();
345 
346  // Odometry:
347  const CPose3DQuat theIncr = getIncrementFromOdometry();
348 
349  // Compute jacobians:
351 
352  CPose3DQuatPDF::jacobiansPoseComposition(
353  robotPose, // x
354  theIncr, // u
355  F, // df_dx,
356  df_du);
357 
358  MRPT_END
359 }
360 
361 /** This virtual function musts calculate de noise matrix of the prediction model.
362  */
363 void CRangeBearingKFSLAM::OnTransitionNoise( KFMatrix_VxV &Q ) const
364 {
365  MRPT_START
366 
367  // The uncertainty of the 2D odometry, projected from the current position:
368  CActionRobotMovement2DPtr act2D = m_action->getBestMovementEstimation();
369  CActionRobotMovement3DPtr act3D = m_action->getActionByClass<CActionRobotMovement3D>();
370 
371  if (act3D && act2D) THROW_EXCEPTION("Both 2D & 3D odometry are present!?!?")
372 
373  CPose3DQuatPDFGaussian odoIncr;
374 
375  if ((!act3D && !act2D) || options.force_ignore_odometry)
376  {
377  // Use constant Q:
378  Q.zeros();
379  ASSERT_(size_t(options.stds_Q_no_odo.size())==size_t(Q.getColCount()))
380 
381  for (size_t i=0;i<get_vehicle_size();i++)
382  Q.set_unsafe(i,i, square( options.stds_Q_no_odo[i]));
383  return;
384  }
385  else
386  {
387  if (act2D)
388  {
389  // 2D odometry:
390  CPosePDFGaussian odoIncr2D;
391  odoIncr2D.copyFrom( *act2D->poseChange );
392  odoIncr = CPose3DQuatPDFGaussian( CPose3DPDFGaussian(odoIncr2D) );
393  }
394  else
395  {
396  // 3D odometry:
397  odoIncr = CPose3DQuatPDFGaussian( act3D->poseChange );
398  }
399  }
400 
401  odoIncr.cov(2,2) += square(options.std_odo_z_additional);
402 
403  // Current pose: copy xyz+quat
404  CPose3DQuat robotPose = getCurrentRobotPoseMean();
405 
406  // Transform from odometry increment to "relative to the robot":
407  odoIncr.changeCoordinatesReference( robotPose );
408 
409  Q = odoIncr.cov;
410 
411  MRPT_END
412 }
413 
414 
416  const vector_size_t &idx_landmarks_to_predict,
417  vector_KFArray_OBS &out_predictions ) const
418 {
419  MRPT_START
420 
421  // Mean of the prior of the robot pose:
422  CPose3DQuat robotPose = getCurrentRobotPoseMean();
423 
424  // Get the sensor pose relative to the robot:
425  CObservationBearingRangePtr obs = m_SF->getObservationByClass<CObservationBearingRange>();
426  ASSERTMSG_(obs,"*ERROR*: This method requires an observation of type CObservationBearingRange")
427  const CPose3DQuat sensorPoseOnRobot = CPose3DQuat(obs->sensorLocationOnRobot);
428 
429  /* -------------------------------------------
430  Equations, obtained using matlab, of the relative 3D position of a landmark (xi,yi,zi), relative
431  to a robot 6D pose (x0,y0,z0,y,p,r)
432  Refer to technical report "6D EKF derivation...", 2008
433 
434  x0 y0 z0 y p r % Robot's 6D pose
435  x0s y0s z0s ys ps rs % Sensor's 6D pose relative to robot
436  xi yi zi % Absolute 3D landmark coordinates:
437 
438  Hx : dh_dxv -> Jacobian of the observation model wrt the robot pose
439  Hy : dh_dyi -> Jacobian of the observation model wrt each landmark mean position
440 
441  Sizes:
442  h: Lx3
443  Hx: 3Lx6
444  Hy: 3Lx3
445 
446  L=# of landmarks in the map (ALL OF THEM)
447  ------------------------------------------- */
448 
449  const size_t vehicle_size = get_vehicle_size();
450  //const size_t obs_size = get_observation_size();
451  const size_t feature_size = get_feature_size();
452 
453  const CPose3DQuat sensorPoseAbs= robotPose + sensorPoseOnRobot;
454 
455  const size_t N = idx_landmarks_to_predict.size();
456  out_predictions.resize(N);
457  for (size_t i=0;i<N;i++)
458  {
459  const size_t row_in = feature_size*idx_landmarks_to_predict[i];
460 
461  // Landmark absolute 3D position in the map:
462  const TPoint3D mapEst(
463  m_xkk[ vehicle_size + row_in + 0 ],
464  m_xkk[ vehicle_size + row_in + 1 ],
465  m_xkk[ vehicle_size + row_in + 2 ] );
466 
467  // Generate Range, yaw, pitch
468  // ---------------------------------------------------
469  sensorPoseAbs.sphericalCoordinates(
470  mapEst,
471  out_predictions[i][0], // range
472  out_predictions[i][1], // yaw
473  out_predictions[i][2] // pitch
474  );
475  }
476 
477  MRPT_END
478 }
479 
481  const size_t &idx_landmark_to_predict,
482  KFMatrix_OxV &Hx,
483  KFMatrix_OxF &Hy ) const
484 {
485  MRPT_START
486 
487  // Mean of the prior of the robot pose:
488  const CPose3DQuat robotPose = getCurrentRobotPoseMean();
489 
490  // Get the sensor pose relative to the robot:
491  CObservationBearingRangePtr obs = m_SF->getObservationByClass<CObservationBearingRange>();
492  ASSERTMSG_(obs,"*ERROR*: This method requires an observation of type CObservationBearingRange")
493  const CPose3DQuat sensorPoseOnRobot = CPose3DQuat(obs->sensorLocationOnRobot);
494 
495  const size_t vehicle_size = get_vehicle_size();
496  //const size_t obs_size = get_observation_size();
497  const size_t feature_size= get_feature_size();
498 
499  // Compute the jacobians, needed below:
500  //const CPose3DQuat sensorPoseAbs= robotPose + sensorPoseOnRobot;
501  CPose3DQuat sensorPoseAbs(UNINITIALIZED_QUATERNION);
503  CMatrixFixedNumeric<kftype,7,7> H_senpose_senrelpose(UNINITIALIZED_MATRIX); // Not actually used
504 
505  CPose3DQuatPDF::jacobiansPoseComposition(
506  robotPose,
507  sensorPoseOnRobot,
508  H_senpose_vehpose,
509  H_senpose_senrelpose,
510  &sensorPoseAbs);
511 
512  const size_t row_in = feature_size*idx_landmark_to_predict;
513 
514  // Landmark absolute 3D position in the map:
515  const TPoint3D mapEst(
516  m_xkk[ vehicle_size + row_in + 0 ],
517  m_xkk[ vehicle_size + row_in + 1 ],
518  m_xkk[ vehicle_size + row_in + 2 ] );
519 
520  // The Jacobian wrt the sensor pose must be transformed later on:
521  KFMatrix_OxV Hx_sensor;
522  double obsData[3];
523  sensorPoseAbs.sphericalCoordinates(
524  mapEst,
525  obsData[0], // range
526  obsData[1], // yaw
527  obsData[2], // pitch
528  &Hy,
529  &Hx_sensor
530  );
531 
532  // Chain rule: Hx = d sensorpose / d vehiclepose * Hx_sensor
533  Hx.multiply( Hx_sensor, H_senpose_vehpose );
534 
535  MRPT_END
536 }
537 
538 
539 /** 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.
540  *
541  * \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.
542  * \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.
543  * \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".
544  * \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.
545  *
546  * This method will be called just once for each complete KF iteration.
547  * \note It is assumed that the observations are independent, i.e. there are NO cross-covariances between them.
548  */
550  vector_KFArray_OBS &Z,
551  vector_int &data_association,
552  const vector_KFArray_OBS &all_predictions,
553  const KFMatrix &S,
554  const vector_size_t &lm_indices_in_S,
555  const KFMatrix_OxO &R
556  )
557 {
558  MRPT_START
559 
560  //static const size_t vehicle_size = get_vehicle_size();
561  static const size_t obs_size = get_observation_size();
562 
563  // Z: Observations
565 
566  CObservationBearingRangePtr obs = m_SF->getObservationByClass<CObservationBearingRange>();
567  ASSERTMSG_(obs,"*ERROR*: This method requires an observation of type CObservationBearingRange")
568 
569  const size_t N = obs->sensedData.size();
570  Z.resize(N);
571 
572  size_t row;
573  for (row=0,itObs = obs->sensedData.begin();itObs!=obs->sensedData.end();++itObs,++row)
574  {
575  // Fill one row in Z:
576  Z[row][0] =itObs->range;
577  Z[row][1] =itObs->yaw;
578  Z[row][2] =itObs->pitch;
579  }
580 
581  // Data association:
582  // ---------------------
583  data_association.assign(N,-1); // Initially, all new landmarks
584 
585  // For each observed LM:
586  vector_size_t obs_idxs_needing_data_assoc;
587  obs_idxs_needing_data_assoc.reserve(N);
588 
589  {
592  size_t row;
593  for (row=0,itObs = obs->sensedData.begin(),itDA=data_association.begin();itObs!=obs->sensedData.end();++itObs,++itDA,++row)
594  {
595  // Fill data asociation: Using IDs!
596  if (itObs->landmarkID<0)
597  obs_idxs_needing_data_assoc.push_back(row);
598  else
599  {
601  if ( (itID = m_IDs.find_key( itObs->landmarkID )) != m_IDs.end() )
602  *itDA = itID->second; // This row in Z corresponds to the i'th map element in the state vector:
603  }
604  }
605  }
606 
607  // ---- Perform data association ----
608  // Only for observation indices in "obs_idxs_needing_data_assoc"
609  if (obs_idxs_needing_data_assoc.empty())
610  {
611  // We don't need to do DA:
613 
614  // Save them for the case the external user wants to access them:
615  for (size_t idxObs=0;idxObs<data_association.size();idxObs++)
616  {
617  int da = data_association[idxObs];
618  if (da>=0)
619  m_last_data_association.results.associations[idxObs] = data_association[idxObs];
620  }
621  }
622  else
623  {
624  // Build a Z matrix with the observations that need dat.assoc:
625  const size_t nObsDA = obs_idxs_needing_data_assoc.size();
626 
627  CMatrixTemplateNumeric<kftype> Z_obs_means(nObsDA,obs_size);
628  for (size_t i=0;i<nObsDA;i++)
629  {
630  const size_t idx = obs_idxs_needing_data_assoc[i];
631  for (unsigned k=0;k<obs_size;k++)
632  Z_obs_means.get_unsafe(i,k) = Z[idx][k];
633  }
634 
635  // Vehicle uncertainty
637  m_pkk.extractMatrix(0,0, Pxx);
638 
639  // Build predictions:
640  // ---------------------------
641  const size_t nPredictions = lm_indices_in_S.size();
643 
644  // S is the covariance of the predictions:
646 
647  // The means:
648  m_last_data_association.Y_pred_means.setSize(nPredictions, obs_size);
649  for (size_t q=0;q<nPredictions;q++)
650  {
651  const size_t i = lm_indices_in_S[q];
652  for (size_t w=0;w<obs_size;w++)
654  m_last_data_association.predictions_IDs.push_back( i ); // for the conversion of indices...
655  }
656 
657  // Do Dat. Assoc :
658  // ---------------------------
659  if (nPredictions)
660  {
661  CMatrixDouble Z_obs_cov = CMatrixDouble(R);
662 
664  Z_obs_means, //Z_obs_cov,
670  true, // Use KD-tree
674  );
675 
676  // Return pairings to the main KF algorithm:
678  data_association[ it->first ] = it->second;
679  }
680  }
681  // ---- End of data association ----
682 
683  MRPT_END
684 }
685 
686 
687 /** This virtual function musts normalize the state vector and covariance matrix (only if its necessary).
688  */
690 {
691  MRPT_START
692 
693  // m_xkk[3:6] must be a normalized quaternion:
694  const double T = std::sqrt( square(m_xkk[3])+square(m_xkk[4])+square(m_xkk[5])+square(m_xkk[6]) );
695  ASSERTMSG_(T>0,"Vehicle pose quaternion norm is not >0!!")
696 
697  const double T_ = (m_xkk[3]<0 ? -1.0:1.0)/T; // qr>=0
698  m_xkk[3]*=T_;
699  m_xkk[4]*=T_;
700  m_xkk[5]*=T_;
701  m_xkk[6]*=T_;
702 
703  MRPT_END
704 }
705 
706 /*---------------------------------------------------------------
707  loadOptions
708  ---------------------------------------------------------------*/
710 {
711  // Main
712  options.loadFromConfigFile( ini, "RangeBearingKFSLAM" );
713  KF_options.loadFromConfigFile( ini, "RangeBearingKFSLAM_KalmanFilter" );
714  // partition algorithm:
715  mapPartitioner.options.loadFromConfigFile(ini,"RangeBearingKFSLAM");
716 }
717 
718 /*---------------------------------------------------------------
719  loadOptions
720  ---------------------------------------------------------------*/
723  const std::string &section)
724 {
725  source.read_vector(section,"stds_Q_no_odo", stds_Q_no_odo, stds_Q_no_odo );
726  ASSERT_(stds_Q_no_odo.size()==7)
727 
728  std_sensor_range = source.read_float(section,"std_sensor_range", std_sensor_range);
729  std_sensor_yaw = DEG2RAD( source.read_float(section,"std_sensor_yaw_deg", RAD2DEG(std_sensor_yaw)));
730  std_sensor_pitch = DEG2RAD( source.read_float(section,"std_sensor_pitch_deg", RAD2DEG(std_sensor_pitch) ));
731 
732  std_odo_z_additional = source.read_float(section,"std_odo_z_additional", std_odo_z_additional);
733 
736 
738 
740 
741  data_assoc_method = source.read_enum<TDataAssociationMethod>(section,"data_assoc_method",data_assoc_method);
742  data_assoc_metric = source.read_enum<TDataAssociationMetric>(section,"data_assoc_metric",data_assoc_metric);
743  data_assoc_IC_metric = source.read_enum<TDataAssociationMetric>(section,"data_assoc_IC_metric",data_assoc_IC_metric);
744 
747 
749 
750 }
751 
752 /*---------------------------------------------------------------
753  Constructor
754  ---------------------------------------------------------------*/
756  stds_Q_no_odo(get_vehicle_size(),0),
757  std_sensor_range ( 0.01f),
758  std_sensor_yaw ( DEG2RAD( 0.2f )),
759  std_sensor_pitch ( DEG2RAD( 0.2f )),
760  std_odo_z_additional ( 0),
761  doPartitioningExperiment(false),
762  quantiles_3D_representation ( 3),
763  partitioningMethod(0),
764  data_assoc_method (assocNN),
765  data_assoc_metric (metricMaha),
766  data_assoc_IC_chi2_thres (0.99),
767  data_assoc_IC_metric (metricMaha),
768  data_assoc_IC_ml_threshold (0.0),
769  create_simplemap (false),
770  force_ignore_odometry (false)
771 {
772  stds_Q_no_odo[0]=
773  stds_Q_no_odo[1]=
774  stds_Q_no_odo[2]=0.10f;
775  stds_Q_no_odo[3]=
776  stds_Q_no_odo[4]=
777  stds_Q_no_odo[5]=
778  stds_Q_no_odo[6]=0.05f;
779 }
780 
781 /*---------------------------------------------------------------
782  dumpToTextStream
783  ---------------------------------------------------------------*/
785 {
786  out.printf("\n----------- [CRangeBearingKFSLAM::TOptions] ------------ \n\n");
787 
788  out.printf("doPartitioningExperiment = %c\n", doPartitioningExperiment ? 'Y':'N' );
789  out.printf("partitioningMethod = %i\n", partitioningMethod );
790  out.printf("data_assoc_method = %s\n", TEnumType<TDataAssociationMethod>::value2name(data_assoc_method).c_str() );
791  out.printf("data_assoc_metric = %s\n", TEnumType<TDataAssociationMetric>::value2name(data_assoc_metric).c_str() );
792  out.printf("data_assoc_IC_chi2_thres = %.06f\n", data_assoc_IC_chi2_thres );
793  out.printf("data_assoc_IC_metric = %s\n", TEnumType<TDataAssociationMetric>::value2name(data_assoc_IC_metric).c_str() );
794  out.printf("data_assoc_IC_ml_threshold = %.06f\n", data_assoc_IC_ml_threshold );
795 
796  out.printf("\n");
797 }
798 
800  const KFArray_OBS & in_z,
801  KFArray_FEAT & yn,
802  KFMatrix_FxV & dyn_dxv,
803  KFMatrix_FxO & dyn_dhn ) const
804 {
805  MRPT_START
806 
807  CObservationBearingRangePtr obs = m_SF->getObservationByClass<CObservationBearingRange>();
808  ASSERTMSG_(obs,"*ERROR*: This method requires an observation of type CObservationBearingRange")
809  const CPose3DQuat sensorPoseOnRobot = CPose3DQuat( obs->sensorLocationOnRobot );
810 
811  // Mean of the prior of the robot pose:
812  const CPose3DQuat robotPose = getCurrentRobotPoseMean();
813 
814  //const CPose3DQuat sensorPoseAbs= robotPose + sensorPoseOnRobot;
815  CPose3DQuat sensorPoseAbs(UNINITIALIZED_QUATERNION);
817  CMatrixFixedNumeric<kftype,7,7> dsensorabs_dsenrelpose(UNINITIALIZED_MATRIX); // Not actually used
818 
819  CPose3DQuatPDF::jacobiansPoseComposition(
820  robotPose,
821  sensorPoseOnRobot,
822  dsensorabs_dvehpose,
823  dsensorabs_dsenrelpose,
824  &sensorPoseAbs);
825 
826  kftype hn_r = in_z[0];
827  kftype hn_y = in_z[1];
828  kftype hn_p = in_z[2];
829 
830  kftype chn_y = cos(hn_y); kftype shn_y = sin(hn_y);
831  kftype chn_p = cos(hn_p); kftype shn_p = sin(hn_p);
832 
833  /* -------------------------------------------
834  syms H h_range h_yaw h_pitch real;
835  H=[ h_range ; h_yaw ; h_pitch ];
836 
837  syms X xi_ yi_ zi_ real;
838  xi_ = h_range * cos(h_yaw) * cos(h_pitch);
839  yi_ = h_range * sin(h_yaw) * cos(h_pitch);
840  zi_ = -h_range * sin(h_pitch);
841 
842  X=[xi_ yi_ zi_];
843  jacob_inv_mod=jacobian(X,H)
844  ------------------------------------------- */
845 
846  // The new point, relative to the sensor:
847  const TPoint3D yn_rel_sensor(
848  hn_r * chn_y * chn_p,
849  hn_r * shn_y * chn_p,
850  -hn_r * shn_p );
851 
852  // The Jacobian of the 3D point in the coordinate system of the sensor:
853  /*
854  [ cos(h_pitch)*cos(h_yaw), -h_range*cos(h_pitch)*sin(h_yaw), -h_range*cos(h_yaw)*sin(h_pitch)]
855  [ cos(h_pitch)*sin(h_yaw), h_range*cos(h_pitch)*cos(h_yaw), -h_range*sin(h_pitch)*sin(h_yaw)]
856  [ -sin(h_pitch), 0, -h_range*cos(h_pitch)]
857  */
858  const kftype values_dynlocal_dhn[] = {
859  chn_p*chn_y , -hn_r*chn_p*shn_y , -hn_r*chn_y*shn_p,
860  chn_p*shn_y , hn_r*chn_p*chn_y , -hn_r*shn_p*shn_y,
861  -shn_p , 0 , -hn_r*chn_p
862  };
863  const KFMatrix_FxO dynlocal_dhn(values_dynlocal_dhn);
864 
865  KFMatrix_FxF jacob_dyn_dynrelsensor(UNINITIALIZED_MATRIX);
866  KFMatrix_FxV jacob_dyn_dsensorabs(UNINITIALIZED_MATRIX);
867 
868  sensorPoseAbs.composePoint(
869  yn_rel_sensor.x, yn_rel_sensor.y, yn_rel_sensor.z, // yn rel. to the sensor
870  yn[0], yn[1], yn[2], // yn in global coords
871  &jacob_dyn_dynrelsensor,
872  &jacob_dyn_dsensorabs
873  );
874 
875  //dyn_dhn = jacob_dyn_dynrelsensor * dynlocal_dhn;
876  dyn_dhn.multiply_AB(jacob_dyn_dynrelsensor, dynlocal_dhn);
877 
878  //dyn_dxv =
879  dyn_dxv.multiply_AB(jacob_dyn_dsensorabs, dsensorabs_dvehpose ); // dsensorabs_dsenrelpose);
880 
881  MRPT_END
882 }
883 
884 
885 /** If applicable to the given problem, do here any special handling of adding a new landmark to the map.
886  * \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.
887  * \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.
888  * \sa OnInverseObservationModel
889  */
891  const size_t in_obsIdx,
892  const size_t in_idxNewFeat )
893 {
894  MRPT_START
895 
896  CObservationBearingRangePtr obs = m_SF->getObservationByClass<CObservationBearingRange>();
897  ASSERTMSG_(obs,"*ERROR*: This method requires an observation of type CObservationBearingRange")
898 
899  // ----------------------------------------------
900  // introduce in the lists of ID<->index in map:
901  // ----------------------------------------------
902  ASSERT_( in_obsIdx < obs->sensedData.size() )
903 
904  if (obs->sensedData[in_obsIdx].landmarkID>=0)
905  {
906  // The sensor provides us a LM ID... use it:
907  m_IDs.insert(
908  obs->sensedData[in_obsIdx].landmarkID,
909  in_idxNewFeat );
910  }
911  else
912  {
913  // Features do not have IDs... use indices:
914  m_IDs.insert( in_idxNewFeat, in_idxNewFeat);
915  }
916 
917  MRPT_END
918 }
919 
920 
921 /*---------------------------------------------------------------
922  getAs3DObject
923  ---------------------------------------------------------------*/
924 void CRangeBearingKFSLAM::getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const
925 {
926  outObj->clear();
927 
928  // ------------------------------------------------
929  // Add the XYZ corner for the current area:
930  // ------------------------------------------------
931  outObj->insert( opengl::stock_objects::CornerXYZ() );
932 
933 
934  // 3D ellipsoid for robot pose:
935  CPointPDFGaussian pointGauss;
936  pointGauss.mean.x( m_xkk[0] );
937  pointGauss.mean.y( m_xkk[1] );
938  pointGauss.mean.z( m_xkk[2] );
940  m_pkk.extractMatrix(0,0,3,3, COV);
941  pointGauss.cov = COV;
942 
943  {
944  opengl::CEllipsoidPtr ellip = opengl::CEllipsoid::Create();
945 
946  ellip->setPose(pointGauss.mean);
947  ellip->setCovMatrix(pointGauss.cov);
948  ellip->enableDrawSolid3D(false);
949  ellip->setQuantiles( options.quantiles_3D_representation );
950  ellip->set3DsegmentsCount(10);
951  ellip->setColor(1,0,0);
952 
953  outObj->insert( ellip );
954  }
955 
956 
957  // 3D ellipsoids for landmarks:
958  const size_t nLMs = this->getNumberOfLandmarksInTheMap();
959  for (size_t i=0;i<nLMs;i++)
960  {
961  pointGauss.mean.x( m_xkk[get_vehicle_size()+get_feature_size()*i+0] );
962  pointGauss.mean.y( m_xkk[get_vehicle_size()+get_feature_size()*i+1] );
963  pointGauss.mean.z( m_xkk[get_vehicle_size()+get_feature_size()*i+2] );
965  pointGauss.cov = COV;
966 
967  opengl::CEllipsoidPtr ellip = opengl::CEllipsoid::Create();
968 
969  ellip->setName( format( "%u",static_cast<unsigned int>(i) ) );
970  ellip->enableShowName(true);
971  ellip->setPose( pointGauss.mean );
972  ellip->setCovMatrix( pointGauss.cov );
973  ellip->enableDrawSolid3D(false);
974  ellip->setQuantiles( options.quantiles_3D_representation );
975  ellip->set3DsegmentsCount(10);
976 
977  ellip->setColor(0,0,1);
978 
979  // Set color depending on partitions?
981  {
982  // This is done for each landmark:
983  map<int,bool> belongToPartition;
984  const CSimpleMap *SFs = &m_SFs; //mapPartitioner.getSequenceOfFrames();
985 
986  for (size_t p=0;p<m_lastPartitionSet.size();p++)
987  {
988  for (size_t w=0;w<m_lastPartitionSet[p].size();w++)
989  {
990  // Check if landmark #i is in the SF of m_lastPartitionSet[p][w]:
991  CLandmark::TLandmarkID i_th_ID = m_IDs.inverse(i);
992 
993  // Look for the lm_ID in the SF:
994  CPose3DPDFPtr pdf;
995  CSensoryFramePtr SF_i;
996  SFs->get( m_lastPartitionSet[p][w], pdf, SF_i);
997 
998  CObservationBearingRangePtr obs = SF_i->getObservationByClass<CObservationBearingRange>();
999 
1000  for (size_t o=0;o<obs->sensedData.size();o++)
1001  {
1002  if ( obs->sensedData[o].landmarkID == i_th_ID )
1003  {
1004  belongToPartition[p]=true;
1005  break;
1006  }
1007  } // end for o
1008  } // end for w
1009  } // end for p
1010 
1011  // Build label:
1012  string strParts("[");
1013 
1014  for (map<int,bool>::iterator it=belongToPartition.begin();it!=belongToPartition.end();++it)
1015  {
1016  if (it!=belongToPartition.begin()) strParts += string(",");
1017  strParts += format("%i",it->first);
1018  }
1019 
1020  ellip->setName( ellip->getName() + strParts + string("]") );
1021 
1022  }
1023 
1024  outObj->insert( ellip );
1025  }
1026 }
1027 
1028 
1029 /*---------------------------------------------------------------
1030  getLastPartitionLandmarks
1031  ---------------------------------------------------------------*/
1032 void CRangeBearingKFSLAM::getLastPartitionLandmarksAsIfFixedSubmaps( size_t K, std::vector<vector_uint> &landmarksMembership )
1033 {
1034  // temporary copy:
1035  std::vector<vector_uint> tmpParts = m_lastPartitionSet;
1036 
1037  // Fake "m_lastPartitionSet":
1038 
1039  // Fixed partitions every K observations:
1040  vector<vector_uint> partitions;
1041  vector_uint tmpCluster;
1042 
1043  for (size_t i=0;i<m_SFs.size();i++)
1044  {
1045  tmpCluster.push_back(i);
1046  if ( (i % K) == 0 )
1047  {
1048  partitions.push_back(tmpCluster);
1049  tmpCluster.clear();
1050  tmpCluster.push_back(i); // This observation "i" is shared between both clusters
1051  }
1052  }
1053  m_lastPartitionSet = partitions;
1054 
1055  // Call the actual method:
1056  getLastPartitionLandmarks(landmarksMembership);
1057 
1058  // Replace copy:
1059  m_lastPartitionSet = tmpParts; //-V519
1060 }
1061 
1062 /*---------------------------------------------------------------
1063  getLastPartitionLandmarks
1064  ---------------------------------------------------------------*/
1066  std::vector<vector_uint> &landmarksMembership ) const
1067 {
1068  landmarksMembership.clear();
1069 
1070  // All the computation is made based on "m_lastPartitionSet"
1071 
1072  if (!options.doPartitioningExperiment) return;
1073 
1074  const size_t nLMs = this->getNumberOfLandmarksInTheMap();
1075  for (size_t i=0;i<nLMs;i++)
1076  {
1077  map<int,bool> belongToPartition;
1078  const CSimpleMap *SFs = &m_SFs; //mapPartitioner.getSequenceOfFrames();
1079 
1080  for (size_t p=0;p<m_lastPartitionSet.size();p++)
1081  {
1082  for (size_t w=0;w<m_lastPartitionSet[p].size();w++)
1083  {
1084  // Check if landmark #i is in the SF of m_lastPartitionSet[p][w]:
1085  CLandmark::TLandmarkID i_th_ID = m_IDs.inverse(i);
1086 
1087  // Look for the lm_ID in the SF:
1088  CPose3DPDFPtr pdf;
1089  CSensoryFramePtr SF_i;
1090  SFs->get( m_lastPartitionSet[p][w], pdf, SF_i);
1091 
1092  CObservationBearingRangePtr obs = SF_i->getObservationByClass<CObservationBearingRange>();
1093 
1094  for (size_t o=0;o<obs->sensedData.size();o++)
1095  {
1096  if ( obs->sensedData[o].landmarkID == i_th_ID )
1097  {
1098  belongToPartition[p]=true;
1099  break;
1100  }
1101  } // end for o
1102  } // end for w
1103  } // end for p
1104 
1105  // Build membership list:
1106  vector_uint membershipOfThisLM;
1107 
1108  for (map<int,bool>::iterator it=belongToPartition.begin();it!=belongToPartition.end();++it)
1109  membershipOfThisLM.push_back(it->first);
1110 
1111  landmarksMembership.push_back( membershipOfThisLM );
1112  } // end for i
1113 }
1114 
1115 
1116 /*---------------------------------------------------------------
1117  computeOffDiagonalBlocksApproximationError
1118  ---------------------------------------------------------------*/
1120  const std::vector<vector_uint> &landmarksMembership ) const
1121 {
1122  MRPT_START
1123 
1124  // Compute the information matrix:
1126  size_t i;
1127  for (i=0;i<get_vehicle_size();i++)
1128  fullCov(i,i) = max(fullCov(i,i), 1e-6);
1129 
1130  CMatrixTemplateNumeric<kftype> H( fullCov.inv() );
1131  H.array().abs(); // Replace by absolute values:
1132 
1133  double sumOffBlocks = 0;
1134  unsigned int nLMs = landmarksMembership.size();
1135 
1136  ASSERT_((get_vehicle_size()+nLMs*get_feature_size())==fullCov.getColCount());
1137 
1138  for (i=0;i<nLMs;i++)
1139  {
1140  for (size_t j=i+1;j<nLMs;j++)
1141  {
1142  // Sum the cross cov. between LM(i) and LM(j)??
1143  // --> Only if there is no common cluster:
1144  if ( 0==math::countCommonElements( landmarksMembership[i],landmarksMembership[j] ) )
1145  {
1146  size_t col = get_vehicle_size()+i*get_feature_size();
1147  size_t row = get_vehicle_size()+j*get_feature_size();
1148  sumOffBlocks += 2 * H.block( row,col,2,2).sum();
1149  }
1150  }
1151  }
1152 
1153  return sumOffBlocks / H.block(get_vehicle_size(),get_vehicle_size(),H.rows()-get_vehicle_size(),H.cols()-get_vehicle_size()).sum(); // Starting (7,7)-end
1154  MRPT_END
1155 }
1156 
1157 /*---------------------------------------------------------------
1158  reconsiderPartitionsNow
1159  ---------------------------------------------------------------*/
1161 {
1163 
1164  vector<vector_uint> partitions; // A different buffer for making this thread-safe some day...
1165 
1166  mapPartitioner.updatePartitions( partitions );
1167 
1168  m_lastPartitionSet = partitions;
1169 }
1170 
1171 /*---------------------------------------------------------------
1172  saveMapAndPathRepresentationAsMATLABFile
1173  ---------------------------------------------------------------*/
1175  const string &fil,
1176  float stdCount,
1177  const string &styleLandmarks,
1178  const string &stylePath,
1179  const string &styleRobot ) const
1180 {
1181  FILE *f= os::fopen(fil.c_str(),"wt");
1182  if (!f) return;
1183 
1184  CMatrixDouble cov(2,2);
1185  CVectorDouble mean(2);
1186 
1187  // Header:
1188  os::fprintf(f,"%%--------------------------------------------------------------------\n");
1189  os::fprintf(f,"%% File automatically generated using the MRPT method:\n");
1190  os::fprintf(f,"%% 'CRangeBearingKFSLAM::saveMapAndPath2DRepresentationAsMATLABFile'\n");
1191  os::fprintf(f,"%%\n");
1192  os::fprintf(f,"%% ~ MRPT ~\n");
1193  os::fprintf(f,"%% Jose Luis Blanco Claraco, University of Malaga @ 2008\n");
1194  os::fprintf(f,"%% http://www.mrpt.org/ \n");
1195  os::fprintf(f,"%%--------------------------------------------------------------------\n");
1196 
1197  // Main code:
1198  os::fprintf(f,"hold on;\n\n");
1199 
1200  const size_t nLMs = this->getNumberOfLandmarksInTheMap();
1201 
1202  for (size_t i=0;i<nLMs;i++)
1203  {
1204  size_t idx = get_vehicle_size()+i*get_feature_size();
1205 
1206  cov(0,0) = m_pkk(idx+0,idx+0);
1207  cov(1,1) = m_pkk(idx+1,idx+1);
1208  cov(0,1) = cov(1,0) = m_pkk(idx+0,idx+1);
1209 
1210  mean[0] = m_xkk[idx+0];
1211  mean[1] = m_xkk[idx+1];
1212 
1213  // Command to draw the 2D ellipse:
1214  os::fprintf(f, "%s", math::MATLAB_plotCovariance2D( cov, mean, stdCount,styleLandmarks ).c_str() );
1215  }
1216 
1217 
1218  // Now: the robot path:
1219  // ------------------------------
1220  if (m_SFs.size())
1221  {
1222  os::fprintf(f,"\nROB_PATH=[");
1223  for (size_t i=0;i<m_SFs.size();i++)
1224  {
1225  CSensoryFramePtr dummySF;
1226  CPose3DPDFPtr pdf3D;
1227  m_SFs.get(i,pdf3D,dummySF);
1228 
1229  CPose3D p;
1230  pdf3D->getMean(p);
1231 
1232  os::fprintf(f,"%.04f %.04f", p.x(), p.y() );
1233  if (i<(m_SFs.size()-1))
1234  os::fprintf(f,";");
1235  }
1236  os::fprintf(f,"];\n");
1237 
1238  os::fprintf(f,"plot(ROB_PATH(:,1),ROB_PATH(:,2),'%s');\n",stylePath.c_str());
1239  }
1240 
1241  // The robot pose:
1242  cov(0,0) = m_pkk(0,0);
1243  cov(1,1) = m_pkk(1,1);
1244  cov(0,1) = cov(1,0) = m_pkk(0,1);
1245 
1246  mean[0] = m_xkk[0];
1247  mean[1] = m_xkk[1];
1248 
1249  os::fprintf(f, "%s", math::MATLAB_plotCovariance2D( cov, mean, stdCount, styleRobot ).c_str() );
1250 
1251  os::fprintf(f,"\naxis equal;\n");
1252  os::fclose(f);
1253 }
1254 
1255 
1256 
1257 /** Computes A=A-B, which may need to be re-implemented depending on the topology of the individual scalar components (eg, angles).
1258  */
1260 {
1261  A-=B;
1264 }
1265 
1266 
1267 /** Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor.
1268  * \param out_R The noise covariance matrix. It might be non diagonal, but it'll usually be.
1269  */
1271 {
1272  out_R(0,0)= square( options.std_sensor_range );
1273  out_R(1,1)= square( options.std_sensor_yaw );
1274  out_R(2,2)= square( options.std_sensor_pitch );
1275 }
1276 
1277 /** This will be called before OnGetObservationsAndDataAssociation to allow the application to reduce the number of covariance landmark predictions to be made.
1278  * For example, features which are known to be "out of sight" shouldn't be added to the output list to speed up the calculations.
1279  * \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.
1280  * \param out_LM_indices_to_predict The list of landmark indices in the map [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.
1281  * \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.
1282  * \sa OnGetObservations, OnDataAssociation
1283  */
1285  const vector_KFArray_OBS &prediction_means,
1286  vector_size_t &out_LM_indices_to_predict ) const
1287 {
1288  CObservationBearingRangePtr obs = m_SF->getObservationByClass<CObservationBearingRange>();
1289  ASSERTMSG_(obs,"*ERROR*: This method requires an observation of type CObservationBearingRange")
1290 
1291 #define USE_HEURISTIC_PREDICTION
1292 
1293 #ifdef USE_HEURISTIC_PREDICTION
1294  const double sensor_max_range = obs->maxSensorDistance;
1295  const double fov_yaw = obs->fieldOfView_yaw;
1296  const double fov_pitch = obs->fieldOfView_pitch;
1297 
1298  const double max_vehicle_loc_uncertainty = 4 * std::sqrt( m_pkk.get_unsafe(0,0) + m_pkk.get_unsafe(1,1)+m_pkk.get_unsafe(2,2) );
1299 #endif
1300 
1301  out_LM_indices_to_predict.clear();
1302  for (size_t i=0;i<prediction_means.size();i++)
1303  {
1304 #ifndef USE_HEURISTIC_PREDICTION
1305  out_LM_indices_to_predict.push_back(i);
1306 #else
1307  // Heuristic: faster but doesn't work always!
1308  if ( prediction_means[i][0] < ( 15 + sensor_max_range + max_vehicle_loc_uncertainty + 4*options.std_sensor_range) &&
1309  fabs(prediction_means[i][1]) < (DEG2RAD(30) + 0.5*fov_yaw + 4*options.std_sensor_yaw) &&
1310  fabs(prediction_means[i][2]) < (DEG2RAD(30) + 0.5*fov_pitch + 4*options.std_sensor_pitch)
1311  )
1312  {
1313  out_LM_indices_to_predict.push_back(i);
1314  }
1315 #endif
1316  }
1317 }
Nearest-neighbor.
void changeCoordinatesReference(const CPose3DQuat &newReferenceBase)
this = p (+) this.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
mrpt::math::CArrayNumeric< double, FEAT_SIZE > KFArray_FEAT
FILE BASE_IMPEXP * fopen(const char *fileName, const char *mode) MRPT_NO_THROWS
An OS-independent version of fopen.
Definition: os.cpp:255
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)...
static CEllipsoidPtr Create()
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
void reset()
Reset the state of the SLAM filter: The map is emptied and the robot put back to (0,0,0).
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
mrpt::poses::CPose3DQuat getIncrementFromOdometry() const
Return the last odometry, as a pose increment.
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=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 7 > *out_jacob_dryp_dpose=NULL) const
Computes the spherical coordinates of a 3D point as seen from the 6D pose specified by this object...
void OnObservationJacobians(const size_t &idx_landmark_to_predict, KFMatrix_OxV &Hx, KFMatrix_OxF &Hy) const
Implements the observation Jacobians and (when applicable) .
std::vector< uint32_t > vector_uint
Definition: types_simple.h:28
Mahalanobis distance.
mrpt::math::CArrayNumeric< double, OBS_SIZE > KFArray_OBS
const_iterator end() const
Definition: bimap.h:46
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:29
bool force_ignore_odometry
Whether to ignore the input odometry and behave as if there was no odometry at all (default: false) ...
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=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 7 > *out_jacobian_df_dpose=NULL) const
Computes the 3D point G such as .
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
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.
void markAllNodesForReconsideration()
Mark all nodes for reconsideration in the next call to "updatePartitions", instead of considering jus...
int BASE_IMPEXP void BASE_IMPEXP fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:272
void reconsiderPartitionsNow()
The partitioning of the entire map is recomputed again.
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...
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...
static size_type size()
Definition: CPose3DQuat.h:257
void OnObservationModel(const vector_size_t &idx_landmarks_to_predict, vector_KFArray_OBS &out_predictions) const
#define THROW_EXCEPTION(msg)
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3626
bool useMapMatching
If set to true (default), adjacency matrix is computed from maps matching; otherwise, the method CObservation::likelihoodWith will be called directly from the SFs.
void updatePartitions(std::vector< vector_uint > &partitions)
This method executed only the neccesary part of the partition to take into account the lastest added ...
mrpt::utils::bimap< mrpt::maps::CLandmark::TLandmarkID, unsigned int > m_IDs
The mapping between landmark IDs and indexes in the Pkk cov.
Scalar * iterator
Definition: eigen_plugins.h:23
This file implements several operations that operate element-wise on individual or pairs of container...
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.
void OnNormalizeStateVector()
This method is called after the prediction and after the update, to give the user an opportunity to n...
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
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.
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
void OnTransitionJacobian(KFMatrix_VxV &out_F) const
Implements the transition Jacobian .
CSetOfObjectsPtr OPENGL_IMPEXP CornerXYZ(float scale=1.0)
Returns three arrows representing a X,Y,Z 3D corner.
double computeOffDiagonalBlocksApproximationError(const std::vector< vector_uint > &landmarksMembership) const
Computes the ratio of the missing information matrix elements which are ignored under a certain parti...
STL namespace.
void runOneKalmanIteration()
The main entry point, executes one complete step: prediction + update.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
double z
X,Y,Z coordinates.
KFMatrix m_pkk
The system full covariance matrix.
mrpt::poses::CPose3DQuat getCurrentRobotPoseMean() const
Get the current robot pose mean, as a 3D+quaternion pose.
int64_t TLandmarkID
The type for the IDs of landmarks.
TDataAssocInfo m_last_data_association
Last data association.
void getLastPartitionLandmarks(std::vector< vector_uint > &landmarksMembership) const
Return the partitioning of the landmarks in clusters accoring to the last partition.
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:3962
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...
void processActionObservation(mrpt::obs::CActionCollectionPtr &action, mrpt::obs::CSensoryFramePtr &SF)
Process one new action and observations to update the map and robot pose estimate.
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
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...
Represents a probabilistic 3D (6D) movement.
void clear()
Definition: bimap.h:62
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
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:61
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
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
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...
mrpt::aligned_containers< KFArray_OBS >::vector_t vector_KFArray_OBS
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
const_iterator find_key(const KEY &k) const
Definition: bimap.h:131
mrpt::obs::CActionCollectionPtr m_action
Set up by processActionObservation.
This namespace contains representation of robot actions and observations.
void loadOptions(const mrpt::utils::CConfigFileBase &ini)
Load options from a ini-like file/text.
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:111
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
mrpt::slam::CIncrementalMapPartitioner::TOptions options
CIncrementalMapPartitioner mapPartitioner
Used for map partitioning experiments.
TDataAssociationMetric data_assoc_IC_metric
Whether to use mahalanobis (->chi2 criterion) vs. Matching likelihood.
std::map< KEY, VALUE >::iterator iterator
Definition: bimap.h:36
size_t size() const
Returns the count of pairs (pose,sensory data)
Definition: CSimpleMap.cpp:67
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:41
#define DEG2RAD
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:3919
unsigned int addMapFrame(const mrpt::obs::CSensoryFramePtr &frame, const mrpt::poses::CPosePDFPtr &robotPose2D)
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...
Definition: CPoint.h:17
mrpt::obs::CSensoryFramePtr m_SF
Set up by processActionObservation.
mrpt::math::CArrayDouble< 3 > m_coords
The translation vector [x,y,z].
Definition: CPose3DQuat.h:47
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.
double kftype
The numeric type used in the Kalman Filter (default=double)
void get(size_t index, mrpt::poses::CPose3DPDFPtr &out_posePDF, mrpt::obs::CSensoryFramePtr &out_SF) const
Access to the i&#39;th pair, first one is index &#39;0&#39;.
Definition: CSimpleMap.cpp:95
void OnGetObservationNoise(KFMatrix_OxO &out_R) const
Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of ...
#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...
mrpt::math::CMatrixFixedNumeric< double, OBS_SIZE, OBS_SIZE > KFMatrix_OxO
#define RAD2DEG
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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...
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
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
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
GLenum GLenum GLvoid * row
Definition: glext.h:3533
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 .
GLsizei GLsizei GLchar * source
Definition: glext.h:3908
#define ASSERT_(f)
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...
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
std::vector< size_t > vector_size_t
Definition: types_simple.h:25
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.
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)
std::vector< int32_t > vector_int
Definition: types_simple.h:23
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:48
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
Definition: bimap.h:69
const std::map< VALUE, KEY > & getInverseMap() const
Return a read-only reference to the internal map KEY->VALUES.
Definition: bimap.h:60
mrpt::math::CMatrixFixedNumeric< double, VEH_SIZE, VEH_SIZE > KFMatrix_VxV
Lightweight 3D point.
mrpt::math::CMatrixFixedNumeric< double, FEAT_SIZE, VEH_SIZE > KFMatrix_FxV
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:201
TDataAssociationMethod
Different algorithms for data association, used in mrpt::slam::data_association.
virtual ~CRangeBearingKFSLAM()
Destructor:
std::vector< vector_uint > m_lastPartitionSet
#define ASSERTMSG_(f, __ERROR_MSG)
GLfloat GLfloat p
Definition: glext.h:5587
double data_assoc_IC_chi2_thres
Threshold in [0,1] for the chi2square test for individual compatibility between predictions and obser...
mrpt::math::CMatrixFixedNumeric< double, FEAT_SIZE, OBS_SIZE > KFMatrix_FxO
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...
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
void clear()
Remove all stored pairs.
Definition: CSimpleMap.cpp:79
A gaussian distribution for 3D points.
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
void getLastPartitionLandmarksAsIfFixedSubmaps(size_t K, std::vector< vector_uint > &landmarksMembership)
For testing only: returns the partitioning as "getLastPartitionLandmarks" but as if a fixed-size subm...
mrpt::math::CMatrixFixedNumeric< double, OBS_SIZE, VEH_SIZE > KFMatrix_OxV
mrpt::math::CVectorFloat stds_Q_no_odo
A 7-length vector with the std.
void copyFrom(const CPosePDF &o) MRPT_OVERRIDE
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
size_t size() const
Definition: bimap.h:54



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019