MRPT  2.0.1
path_from_rtk_gps.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "topography-precomp.h" // Precompiled headers
11 
12 #include <mrpt/math/data_utils.h>
14 #include <mrpt/tfest/se3.h>
18 
19 #include <memory>
20 
21 using namespace std;
22 using namespace mrpt;
23 using namespace mrpt::obs;
24 using namespace mrpt::math;
25 using namespace mrpt::poses;
26 using namespace mrpt::system;
27 using namespace mrpt::tfest;
28 using namespace mrpt::config;
29 using namespace mrpt::topography;
30 
31 template <class T>
32 std::set<T> make_set(const T& v0, const T& v1)
33 {
34  std::set<T> s;
35  s.insert(v0);
36  s.insert(v1);
37  return s;
38 }
39 
40 /*---------------------------------------------------------------
41  path_from_rtk_gps
42  ---------------------------------------------------------------*/
45  const mrpt::obs::CRawlog& rawlog, size_t first, size_t last, bool isGUI,
46  bool disableGPSInterp, int PATH_SMOOTH_FILTER, TPathFromRTKInfo* outInfo)
47 {
49 
50  // Go: generate the map:
51  ASSERT_(first <= last);
52  ASSERT_(last <= rawlog.size() - 1);
53 
54  set<string> lstGPSLabels;
55 
56  robot_path.clear();
57  robot_path.setMaxTimeInterpolation(std::chrono::seconds(
58  3)); // Max. seconds of GPS blackout not to interpolate.
60 
61  TPathFromRTKInfo outInfoTemp;
62  if (outInfo) *outInfo = outInfoTemp;
63 
64  map<string, map<Clock::time_point, TPoint3D>>
65  gps_paths; // label -> (time -> 3D local coords)
66 
67  bool abort = false;
68  bool ref_valid = false;
69 
70  // Load configuration block:
71  CConfigFileMemory memFil;
72  rawlog.getCommentTextAsConfigFile(memFil);
73 
74  TGeodeticCoords ref(
75  memFil.read_double("GPS_ORIGIN", "lat_deg", 0),
76  memFil.read_double("GPS_ORIGIN", "lon_deg", 0),
77  memFil.read_double("GPS_ORIGIN", "height", 0));
78 
79  ref_valid = !ref.isClear();
80 
81  // Do we have info for the consistency test?
82  const double std_0 = memFil.read_double("CONSISTENCY_TEST", "std_0", 0);
83  bool doConsistencyCheck = std_0 > 0;
84 
85  // Do we have the "reference uncertainty" matrix W^\star ??
86  memFil.read_matrix("UNCERTAINTY", "W_star", outInfoTemp.W_star);
87  const bool doUncertaintyCovs = outInfoTemp.W_star.rows() != 0;
88  if (doUncertaintyCovs &&
89  (outInfoTemp.W_star.rows() != 6 || outInfoTemp.W_star.cols() != 6))
91  "ERROR: W_star matrix for uncertainty estimation is provided but "
92  "it's not a 6x6 matrix.");
93 
94  // ------------------------------------------
95  // Look for the 2 observations:
96  // ------------------------------------------
97 
98  // The list with all time ordered gps's in valid RTK mode
99  using TListGPSs = std::map<
100  mrpt::Clock::time_point, std::map<std::string, CObservationGPS::Ptr>>;
101  TListGPSs list_gps_obs;
102 
103  map<string, size_t> GPS_RTK_reads; // label-># of RTK readings
104  map<string, TPoint3D>
105  GPS_local_coords_on_vehicle; // label -> local pose on the vehicle
106 
107  for (size_t i = first; !abort && i <= last; i++)
108  {
109  switch (rawlog.getType(i))
110  {
111  default:
112  break;
113 
114  case CRawlog::etObservation:
115  {
116  CObservation::Ptr o = rawlog.getAsObservation(i);
117 
118  if (o->GetRuntimeClass() == CLASS_ID(CObservationGPS))
119  {
121  std::dynamic_pointer_cast<CObservationGPS>(o);
122 
123  if (obs->has_GGA_datum() &&
124  obs->getMsgByClass<gnss::Message_NMEA_GGA>()
125  .fields.fix_quality == 4)
126  {
127  // Add to the list:
128  list_gps_obs[obs->timestamp][obs->sensorLabel] = obs;
129 
130  lstGPSLabels.insert(obs->sensorLabel);
131  }
132 
133  // Save to GPS paths:
134  if (obs->has_GGA_datum() &&
135  (obs->getMsgByClass<gnss::Message_NMEA_GGA>()
136  .fields.fix_quality == 4 ||
137  obs->getMsgByClass<gnss::Message_NMEA_GGA>()
138  .fields.fix_quality == 5))
139  {
140  GPS_RTK_reads[obs->sensorLabel]++;
141 
142  // map<string,TPoint3D> GPS_local_coords_on_vehicle; //
143  // label -> local pose on the vehicle
144  if (GPS_local_coords_on_vehicle.find(
145  obs->sensorLabel) ==
146  GPS_local_coords_on_vehicle.end())
147  GPS_local_coords_on_vehicle[obs->sensorLabel] =
148  TPoint3D(obs->sensorPose.asTPose());
149 
150  // map<string, map<Clock::time_point,TPoint3D> >
151  // gps_paths;
152  // //
153  // label -> (time -> 3D local coords)
154  gps_paths[obs->sensorLabel][obs->timestamp] = TPoint3D(
155  obs->getMsgByClass<gnss::Message_NMEA_GGA>()
157  obs->getMsgByClass<gnss::Message_NMEA_GGA>()
159  obs->getMsgByClass<gnss::Message_NMEA_GGA>()
161  }
162  }
163  }
164  break;
165  } // end switch type
166 
167  } // end for i
168 
169  // -----------------------------------------------------------
170  // At this point we already have the sensor positions, thus
171  // we can estimate the covariance matrix D:
172  //
173  // TODO: Generalize equations for # of GPS > 3
174  // -----------------------------------------------------------
175  map<set<string>, double> Ad_ij; // InterGPS distances in 2D
176  map<set<string>, double>
177  phi_ij; // Directions on XY of the lines between i-j
178  map<string, size_t>
179  D_cov_indexes; // Sensor label-> index in the matrix (first=0, ...)
180  map<size_t, string> D_cov_rev_indexes; // Reverse of D_cov_indexes
181 
182  CMatrixDouble D_cov; // square distances cov
183  CMatrixDouble D_cov_1; // square distances cov (inverse)
184  CVectorDouble D_mean; // square distances mean
185 
186  if (doConsistencyCheck && GPS_local_coords_on_vehicle.size() == 3)
187  {
188  unsigned int cnt = 0;
189  for (auto i = GPS_local_coords_on_vehicle.begin();
190  i != GPS_local_coords_on_vehicle.end(); ++i)
191  {
192  // Index tables:
193  D_cov_indexes[i->first] = cnt;
194  D_cov_rev_indexes[cnt] = i->first;
195  cnt++;
196 
197  for (auto j = i; j != GPS_local_coords_on_vehicle.end(); ++j)
198  {
199  if (i != j)
200  {
201  const TPoint3D& pi = i->second;
202  const TPoint3D& pj = j->second;
203  Ad_ij[make_set(i->first, j->first)] = pi.distanceTo(pj);
204  phi_ij[make_set(i->first, j->first)] =
205  atan2(pj.y - pi.y, pj.x - pi.x);
206  }
207  }
208  }
209  ASSERT_(D_cov_indexes.size() == 3 && D_cov_rev_indexes.size() == 3);
210 
211  D_cov.setSize(D_cov_indexes.size(), D_cov_indexes.size());
212  D_mean.resize(D_cov_indexes.size());
213 
214  // See paper for the formulas!
215  // TODO: generalize for N>3
216 
217  D_cov(0, 0) =
218  2 *
219  square(Ad_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[1])]);
220  D_cov(1, 1) =
221  2 *
222  square(Ad_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[2])]);
223  D_cov(2, 2) =
224  2 *
225  square(Ad_ij[make_set(D_cov_rev_indexes[1], D_cov_rev_indexes[2])]);
226 
227  D_cov(1, 0) =
228  Ad_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[1])] *
229  Ad_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[2])] *
230  cos(phi_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[1])] -
231  phi_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[2])]);
232  D_cov(0, 1) = D_cov(1, 0);
233 
234  D_cov(2, 0) =
235  -Ad_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[1])] *
236  Ad_ij[make_set(D_cov_rev_indexes[1], D_cov_rev_indexes[2])] *
237  cos(phi_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[1])] -
238  phi_ij[make_set(D_cov_rev_indexes[1], D_cov_rev_indexes[2])]);
239  D_cov(0, 2) = D_cov(2, 0);
240 
241  D_cov(2, 1) =
242  Ad_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[2])] *
243  Ad_ij[make_set(D_cov_rev_indexes[1], D_cov_rev_indexes[2])] *
244  cos(phi_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[2])] -
245  phi_ij[make_set(D_cov_rev_indexes[1], D_cov_rev_indexes[2])]);
246  D_cov(1, 2) = D_cov(2, 1);
247 
248  D_cov *= 4 * square(std_0);
249 
250  D_cov_1 = D_cov.inverse_LLt();
251 
252  // cout << D_cov.inMatlabFormat() << endl;
253 
254  D_mean[0] =
255  square(Ad_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[1])]);
256  D_mean[1] =
257  square(Ad_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[2])]);
258  D_mean[2] =
259  square(Ad_ij[make_set(D_cov_rev_indexes[1], D_cov_rev_indexes[2])]);
260  }
261  else
262  doConsistencyCheck = false;
263 
264  // ------------------------------------------
265  // Look for the 2 observations:
266  // ------------------------------------------
267  int N_GPSs = list_gps_obs.size();
268 
269  if (N_GPSs)
270  {
271  // loop interpolate 1-out-of-5: this solves the issue with JAVAD GPSs
272  // that skip some readings at some times .0 .2 .4 .6 .8
273  if (list_gps_obs.size() > 4)
274  {
275  auto F = list_gps_obs.begin();
276  ++F;
277  ++F;
278  auto E = list_gps_obs.end();
279  --E;
280  --E;
281 
282  for (auto it = F; it != E; ++it)
283  {
284  // Now check if we have 3 gps with the same time stamp:
285  // const size_t N = i->second.size();
286  std::map<std::string, CObservationGPS::Ptr>& GPS = it->second;
287 
288  // Check if any in "lstGPSLabels" is missing here:
289  for (const auto& lstGPSLabel : lstGPSLabels)
290  {
291  // For each GPS in the current timestamp:
292  bool fnd = (GPS.find(lstGPSLabel) != GPS.end());
293 
294  if (fnd) continue; // this one is present.
295 
296  // Ok, we have "*l" missing in the set "*i".
297  // Try to interpolate from neighbors:
298  auto i_b1 = it;
299  --i_b1;
300  auto i_a1 = it;
301  ++i_a1;
302 
303  CObservationGPS::Ptr GPS_b1, GPS_a1;
304 
305  if (i_b1->second.find(lstGPSLabel) != i_b1->second.end())
306  GPS_b1 = i_b1->second.find(lstGPSLabel)->second;
307 
308  if (i_a1->second.find(lstGPSLabel) != i_a1->second.end())
309  GPS_a1 = i_a1->second.find(lstGPSLabel)->second;
310 
311  if (!disableGPSInterp && GPS_a1 && GPS_b1)
312  {
314  GPS_b1->timestamp, GPS_a1->timestamp) < 0.5)
315  {
316  auto new_gps = CObservationGPS::Create(*GPS_a1);
317  new_gps->sensorLabel = lstGPSLabel;
318 
319  // cout <<
320  // mrpt::system::timeLocalToString(GPS_b1->timestamp)
321  // << " " <<
322  // mrpt::system::timeLocalToString(GPS_a1->timestamp)
323  // << " " << *l;
324  // cout << endl;
325 
326  new_gps->getMsgByClass<gnss::Message_NMEA_GGA>()
327  .fields.longitude_degrees =
328  0.5 *
329  (GPS_a1->getMsgByClass<gnss::Message_NMEA_GGA>()
331  GPS_b1->getMsgByClass<gnss::Message_NMEA_GGA>()
332  .fields.longitude_degrees);
333  new_gps->getMsgByClass<gnss::Message_NMEA_GGA>()
334  .fields.latitude_degrees =
335  0.5 *
336  (GPS_a1->getMsgByClass<gnss::Message_NMEA_GGA>()
338  GPS_b1->getMsgByClass<gnss::Message_NMEA_GGA>()
339  .fields.latitude_degrees);
340  new_gps->getMsgByClass<gnss::Message_NMEA_GGA>()
341  .fields.altitude_meters =
342  0.5 *
343  (GPS_a1->getMsgByClass<gnss::Message_NMEA_GGA>()
345  GPS_b1->getMsgByClass<gnss::Message_NMEA_GGA>()
346  .fields.altitude_meters);
347 
348  new_gps->timestamp =
350  (GPS_a1->timestamp.time_since_epoch()
351  .count() +
352  GPS_b1->timestamp.time_since_epoch()
353  .count()) /
354  2));
355 
356  it->second[new_gps->sensorLabel] = new_gps;
357  }
358  }
359  }
360  } // end loop interpolate 1-out-of-5
361  }
362 
363  int idx_in_GPSs = 0;
364 
365  for (auto i = list_gps_obs.begin(); i != list_gps_obs.end();
366  ++i, idx_in_GPSs++)
367  {
368  // Now check if we have 3 gps with the same time stamp:
369  if (i->second.size() >= 3)
370  {
371  const size_t N = i->second.size();
372  std::map<std::string, CObservationGPS::Ptr>& GPS = i->second;
373  CVectorDouble X(N), Y(N), Z(N); // Global XYZ coordinates
374  std::map<string, size_t>
375  XYZidxs; // Sensor label -> indices in X Y Z
376 
377  if (!ref_valid) // get the reference lat/lon, if it's not set
378  // from rawlog configuration block.
379  {
380  ref_valid = true;
381  ref = GPS.begin()
382  ->second->getMsgByClass<gnss::Message_NMEA_GGA>()
383  .getAsStruct<TGeodeticCoords>();
384  }
385 
386  // Compute the XYZ coordinates of all sensors:
387  TMatchingPairList corrs;
388  unsigned int k;
389  std::map<std::string, CObservationGPS::Ptr>::iterator g_it;
390 
391  for (k = 0, g_it = GPS.begin(); g_it != GPS.end(); ++g_it, ++k)
392  {
393  TPoint3D P;
395  g_it->second->getMsgByClass<gnss::Message_NMEA_GGA>()
397  P, ref);
398 
399  // Correction of offsets:
400  const string sect =
401  string("OFFSET_") + g_it->second->sensorLabel;
402  P.x += memFil.read_double(sect, "x", 0);
403  P.y += memFil.read_double(sect, "y", 0);
404  P.z += memFil.read_double(sect, "z", 0);
405 
406  XYZidxs[g_it->second->sensorLabel] =
407  k; // Save index correspondence
408 
409  // Create the correspondence:
410  corrs.push_back(TMatchingPair(
411  k, k, // Indices
412  // "This"/Global coords
413  d2f(P.x), d2f(P.y), d2f(P.z),
414  // "other"/local coordinates
415  d2f(g_it->second->sensorPose.x()),
416  d2f(g_it->second->sensorPose.y()),
417  d2f(g_it->second->sensorPose.z())));
418 
419  X[k] = P.x;
420  Y[k] = P.y;
421  Z[k] = P.z;
422  }
423 
424  if (doConsistencyCheck && GPS.size() == 3)
425  {
426  // XYZ[k] have the k'd final coordinates of each GPS
427  // GPS[k] are the CObservations:
428 
429  // Compute the inter-GPS square distances:
430  CVectorDouble iGPSdist2(3);
431 
432  // [0]: sq dist between:
433  // D_cov_rev_indexes[0],D_cov_rev_indexes[1]
434  TPoint3D P0(
435  X[XYZidxs[D_cov_rev_indexes[0]]],
436  Y[XYZidxs[D_cov_rev_indexes[0]]],
437  Z[XYZidxs[D_cov_rev_indexes[0]]]);
438  TPoint3D P1(
439  X[XYZidxs[D_cov_rev_indexes[1]]],
440  Y[XYZidxs[D_cov_rev_indexes[1]]],
441  Z[XYZidxs[D_cov_rev_indexes[1]]]);
442  TPoint3D P2(
443  X[XYZidxs[D_cov_rev_indexes[2]]],
444  Y[XYZidxs[D_cov_rev_indexes[2]]],
445  Z[XYZidxs[D_cov_rev_indexes[2]]]);
446 
447  iGPSdist2[0] = P0.sqrDistanceTo(P1);
448  iGPSdist2[1] = P0.sqrDistanceTo(P2);
449  iGPSdist2[2] = P1.sqrDistanceTo(P2);
450 
451  double mahaD = mrpt::math::mahalanobisDistance(
452  iGPSdist2, D_mean, D_cov_1);
453  outInfoTemp.mahalabis_quality_measure[i->first] = mahaD;
454 
455  // cout << "x: " << iGPSdist2 << " MU: " << D_mean << " ->
456  // " << mahaD << endl;
457  } // end consistency
458 
459  // Use a 6D matching method to estimate the location of the
460  // vehicle:
461  CPose3DQuat optimal_pose;
462  double optimal_scale;
463 
464  // "this" (reference map) -> GPS global coordinates
465  // "other" -> GPS local coordinates on the vehicle
467  corrs, optimal_pose, optimal_scale, true); // Force scale=1
468  // cout << "optimal pose: " << optimal_pose << " " <<
469  // optimal_scale << endl;
470  MRPT_CHECK_NORMAL_NUMBER(optimal_pose.x());
471  MRPT_CHECK_NORMAL_NUMBER(optimal_pose.y());
472  MRPT_CHECK_NORMAL_NUMBER(optimal_pose.z());
473  MRPT_CHECK_NORMAL_NUMBER(optimal_pose.quat().x());
474  MRPT_CHECK_NORMAL_NUMBER(optimal_pose.quat().y());
475  MRPT_CHECK_NORMAL_NUMBER(optimal_pose.quat().z());
476  MRPT_CHECK_NORMAL_NUMBER(optimal_pose.quat().r());
477 
478  // Final vehicle pose:
479  const CPose3D veh_pose = CPose3D(optimal_pose);
480 
481  // Add to the interpolator:
482  robot_path.insert(i->first, veh_pose);
483 
484  // If we have W_star, compute the pose uncertainty:
485  if (doUncertaintyCovs)
486  {
487  CPose3DPDFGaussian final_veh_uncert;
488  final_veh_uncert.mean.setFromValues(0, 0, 0, 0, 0, 0);
489  final_veh_uncert.cov = outInfoTemp.W_star;
490 
491  // Rotate the covariance according to the real vehicle pose:
492  final_veh_uncert.changeCoordinatesReference(veh_pose);
493 
494  outInfoTemp.vehicle_uncertainty[i->first] =
495  final_veh_uncert.cov;
496  }
497  }
498 
499  } // end for i
500 
501  if (PATH_SMOOTH_FILTER > 0 && robot_path.size() > 1)
502  {
503  CPose3DInterpolator filtered_robot_path = robot_path;
504 
505  // Do Angles smoother filter of the path:
506  // ---------------------------------------------
507  const double MAX_DIST_TO_FILTER = 4.0;
508 
509  for (auto i = robot_path.begin(); i != robot_path.end(); ++i)
510  {
511  mrpt::math::TPose3D p = i->second;
512 
513  CVectorDouble pitchs, rolls; // The elements to average
514 
515  pitchs.push_back(p.pitch);
516  rolls.push_back(p.roll);
517 
518  auto q = i;
519  for (int k = 0;
520  k < PATH_SMOOTH_FILTER && q != robot_path.begin(); k++)
521  {
522  --q;
523  if (std::abs(mrpt::system::timeDifference(
524  q->first, i->first)) < MAX_DIST_TO_FILTER)
525  {
526  pitchs.push_back(q->second.pitch);
527  rolls.push_back(q->second.roll);
528  }
529  }
530  q = i;
531  for (int k = 0;
532  k < PATH_SMOOTH_FILTER && q != (--robot_path.end()); k++)
533  {
534  ++q;
535  if (std::abs(mrpt::system::timeDifference(
536  q->first, i->first)) < MAX_DIST_TO_FILTER)
537  {
538  pitchs.push_back(q->second.pitch);
539  rolls.push_back(q->second.roll);
540  }
541  }
542 
543  p.pitch = mrpt::math::averageWrap2Pi(pitchs);
544  p.roll = mrpt::math::averageWrap2Pi(rolls);
545 
546  // save in filtered path:
547  filtered_robot_path.insert(i->first, p);
548  }
549  // Replace:
550  robot_path = filtered_robot_path;
551 
552  } // end PATH_SMOOTH_FILTER
553 
554  } // end step generate 6D path
555 
556  // Here we can set best_gps_path (that with the max. number of RTK
557  // fixed/foat readings):
558  if (outInfo)
559  {
560  string bestLabel;
561  size_t bestNum = 0;
562  for (auto& GPS_RTK_read : GPS_RTK_reads)
563  {
564  if (GPS_RTK_read.second > bestNum)
565  {
566  bestNum = GPS_RTK_read.second;
567  bestLabel = GPS_RTK_read.first;
568  }
569  }
570  outInfoTemp.best_gps_path = gps_paths[bestLabel];
571 
572  // and transform to XYZ:
573  // Correction of offsets:
574  const string sect = string("OFFSET_") + bestLabel;
575  const double off_X = memFil.read_double(sect, "x", 0);
576  const double off_Y = memFil.read_double(sect, "y", 0);
577  const double off_Z = memFil.read_double(sect, "z", 0);
578 
579  // map<TTimeStamp,TPoint3D> best_gps_path; // time -> 3D local
580  // coords
581  for (auto& i : outInfoTemp.best_gps_path)
582  {
583  TPoint3D P;
584  TPoint3D& pl = i.second;
587  pl.x, pl.y, pl.z), // i->second.x,i->second.y,i->second.z,
588  // // lat, lon, heigh
589  P, // X Y Z
590  ref);
591 
592  pl.x = P.x + off_X;
593  pl.y = P.y + off_Y;
594  pl.z = P.z + off_Z;
595  }
596  } // end best_gps_path
597 
598  if (outInfo) *outInfo = outInfoTemp;
599 
600  MRPT_END
601 }
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:58
std::chrono::duration< rep, period > duration
Definition: Clock.h:24
This class implements a config file-like interface over a memory-stored string list.
double averageWrap2Pi(const CVectorDouble &angles)
Computes the average of a sequence of angles in radians taking into account the correct wrapping in t...
Definition: math.cpp:315
uint8_t fix_quality
NMEA standard values: 0 = invalid, 1 = GPS fix (SPS), 2 = DGPS fix, 3 = PPS fix, 4 = Real Time Kinema...
#define MRPT_START
Definition: exceptions.h:241
CPose3D mean
The mean value.
double latitude_degrees
The measured latitude, in degrees (North:+ , South:-)
std::chrono::time_point< Clock > time_point
Definition: Clock.h:25
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
double roll
Roll coordinate (rotation angle over X coordinate).
Definition: TPose3D.h:38
T y() const
Return y coordinate of the quaternion.
Definition: CQuaternion.h:107
void read_matrix(const std::string &section, const std::string &name, MATRIX_TYPE &outMatrix, const MATRIX_TYPE &defaultMatrix=MATRIX_TYPE(), bool failIfNotFound=false) const
Reads a configuration parameter as a matrix written in a matlab-like format - for example: "[2 3 4 ; ...
A set of geodetic coordinates: latitude, longitude and height, defined over a given geoid (typically...
Definition: data_types.h:192
STL namespace.
void geodeticToENU_WGS84(const TGeodeticCoords &in_coords, mrpt::math::TPoint3D &out_ENU_point, const TGeodeticCoords &in_coords_origin)
Coordinates transformation from longitude/latitude/height to ENU (East-North-Up) X/Y/Z coordinates Th...
void setInterpolationMethod(TInterpolatorMethod method)
Change the method used to interpolate the robot path.
Used to return optional information from mrpt::topography::path_from_rtk_gps.
void push_back(const T &val)
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
float d2f(const double d)
shortcut for static_cast<float>(double)
double altitude_meters
The measured altitude, in meters (A).
This base provides a set of functions for maths stuff.
T r() const
Return r (real part) coordinate of the quaternion.
Definition: CQuaternion.h:101
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:102
#define MRPT_CHECK_NORMAL_NUMBER(v)
Throws an exception if the number is NaN, IND, or +/-INF, or return the same number otherwise...
Definition: exceptions.h:125
CObservation::Ptr getAsObservation(size_t index) const
Returns the i&#39;th element in the sequence, as being an observation, where index=0 is the first object...
Definition: CRawlog.cpp:81
std::map< mrpt::Clock::time_point, mrpt::math::CMatrixDouble66 > vehicle_uncertainty
The 6x6 covariance matrix for the uncertainty of each vehicle pose (may be empty if there is no W_sta...
A list of TMatchingPair.
Definition: TMatchingPair.h:70
double longitude_degrees
The measured longitude, in degrees (East:+ , West:-)
This class stores a rawlog (robotic datasets) in one of two possible formats:
Definition: CRawlog.h:65
This namespace contains representation of robot actions and observations.
constexpr auto sect
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:268
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:45
Derived inverse_LLt() const
Returns the inverse of a symmetric matrix using LLt.
T sqrDistanceTo(const TPoint3D_< T > &p) const
Point-to-point distance, squared.
Definition: TPoint3D.h:136
size_type rows() const
Number of rows in the matrix.
size_type cols() const
Number of columns in the matrix.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
Definition: TMatchingPair.h:31
double pitch
Pitch coordinate (rotation angle over Y axis).
Definition: TPose3D.h:36
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
return_t square(const num_t x)
Inline function for the square of a number.
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
void setMaxTimeInterpolation(const mrpt::Clock::duration &time)
Set value of the maximum time to consider interpolation.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
T x() const
Return x coordinate of the quaternion.
Definition: CQuaternion.h:105
void clear()
Clears the current sequence of poses.
void path_from_rtk_gps(mrpt::poses::CPose3DInterpolator &robot_path, const mrpt::obs::CRawlog &rawlog, size_t rawlog_first, size_t rawlog_last, bool isGUI=false, bool disableGPSInterp=false, int path_smooth_filter_size=2, TPathFromRTKInfo *outInfo=nullptr)
Reconstruct the path of a vehicle equipped with 3 RTK GPSs.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
std::set< T > make_set(const T &v0, const T &v1)
std::map< mrpt::Clock::time_point, mrpt::math::TPoint3D > best_gps_path
the path of the "best" GPS.
mrpt::math::CMatrixDouble W_star
The reference covariance matrix used to compute vehicle_uncertainty.
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
#define MRPT_END
Definition: exceptions.h:245
VECTORLIKE1::Scalar mahalanobisDistance(const VECTORLIKE1 &X, const VECTORLIKE2 &MU, const MAT &COV)
Computes the mahalanobis distance of a vector X given the mean MU and the covariance inverse COV_inv ...
Definition: data_utils.h:57
TGEODETICCOORDS getAsStruct() const
Return the geodetic coords as a mrpt::topography::TGeodeticCoords structure (requires linking against...
void setFromValues(const double x0, const double y0, const double z0, const double yaw=0, const double pitch=0, const double roll=0)
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes...
Definition: CPose3D.cpp:265
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
content_t fields
Message content, accesible by individual fields.
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
bool se3_l2(const mrpt::tfest::TMatchingPairList &in_correspondences, mrpt::poses::CPose3DQuat &out_transform, double &out_scale, bool forceScaleToUnity=false)
Least-squares (L2 norm) solution to finding the optimal SE(3) transform between two reference frames ...
Definition: se3_l2.cpp:219
T distanceTo(const TPoint3D_< T > &p) const
Point-to-point distance.
Definition: TPoint3D.h:126
This class stores a time-stamped trajectory in SE(3) (CPose3D poses).
std::map< mrpt::Clock::time_point, double > mahalabis_quality_measure
A measure of the quality at each point (may be empty if not there is no enough information).
TEntryType getType(size_t index) const
Returns the type of a given element.
Definition: CRawlog.cpp:106
void insert(const mrpt::Clock::time_point &t, const pose_t &p)
Inserts a new pose in the sequence.
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
double timeDifference(const mrpt::system::TTimeStamp t_first, const mrpt::system::TTimeStamp t_later)
Returns the time difference from t1 to t2 (positive if t2 is posterior to t1), in seconds...
Definition: datetime.h:123
void resize(std::size_t N, bool zeroNewElements=false)
This namespace provides topography helper functions, coordinate transformations.
Definition: conversions.h:21
This class stores messages from GNSS or GNSS+IMU devices, from consumer-grade inexpensive GPS receive...
T z() const
Return z coordinate of the quaternion.
Definition: CQuaternion.h:109
size_t size() const
Returns the number of actions / observations object in the sequence.
Definition: CRawlog.cpp:64
Functions for estimating the optimal transformation between two frames of references given measuremen...
void getCommentTextAsConfigFile(mrpt::config::CConfigFileMemory &memCfg) const
Saves the block of comment text for the rawlog into the passed config file object.
Definition: CRawlog.cpp:527



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020