Main MRPT website > C++ reference for MRPT 1.9.9
path_from_rtk_gps.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 "topography-precomp.h" // Precompiled headers
11 
12 #include <mrpt/tfest/se3.h>
13 #include <mrpt/obs/CObservationGPS.h>
14 #include <mrpt/math/data_utils.h>
15 #include <mrpt/topography/data_types.h>
16 #include <mrpt/topography/conversions.h>
17 #include <mrpt/topography/path_from_rtk_gps.h>
18 
19 #include <memory>
20 
21 #if MRPT_HAS_WXWIDGETS
22 #include <wx/app.h>
23 #include <wx/msgdlg.h>
24 #include <wx/string.h>
25 #include <wx/progdlg.h>
26 #include <wx/busyinfo.h>
27 #include <wx/log.h>
28 #endif // MRPT_HAS_WXWIDGETS
29 
30 using namespace std;
31 using namespace mrpt;
32 using namespace mrpt::obs;
33 using namespace mrpt::math;
34 using namespace mrpt::utils;
35 using namespace mrpt::poses;
36 using namespace mrpt::system;
37 using namespace mrpt::topography;
38 
39 template <class T>
40 std::set<T> make_set(const T& v0, const T& v1)
41 {
42  std::set<T> s;
43  s.insert(v0);
44  s.insert(v1);
45  return s;
46 }
47 
48 /*---------------------------------------------------------------
49  path_from_rtk_gps
50  ---------------------------------------------------------------*/
51 void mrpt::topography::path_from_rtk_gps(
52  mrpt::poses::CPose3DInterpolator& robot_path,
53  const mrpt::obs::CRawlog& rawlog, size_t first, size_t last, bool isGUI,
54  bool disableGPSInterp, int PATH_SMOOTH_FILTER, TPathFromRTKInfo* outInfo)
55 {
56  MRPT_START
57 
58 #if MRPT_HAS_WXWIDGETS
59  // Use a smart pointer so we are safe against exceptions:
60  std::unique_ptr<wxBusyCursor> waitCursorPtr;
61  if (isGUI)
62  waitCursorPtr = std::unique_ptr<wxBusyCursor>(new wxBusyCursor());
63 #else
64  MRPT_UNUSED_PARAM(isGUI);
65 #endif
66 
67  // Go: generate the map:
68  size_t i;
69 
70  ASSERT_(first <= last);
71  ASSERT_(last <= rawlog.size() - 1);
72 
73  set<string> lstGPSLabels;
74 
75  size_t count = 0;
76 
77  robot_path.clear();
78  robot_path.setMaxTimeInterpolation(
79  3.0); // Max. seconds of GPS blackout not to interpolate.
80  robot_path.setInterpolationMethod(mrpt::poses::imSSLSLL);
81 
82  TPathFromRTKInfo outInfoTemp;
83  if (outInfo) *outInfo = outInfoTemp;
84 
85  map<string, map<TTimeStamp, TPoint3D>>
86  gps_paths; // label -> (time -> 3D local coords)
87 
88  bool abort = false;
89  bool ref_valid = false;
90 
91  // Load configuration block:
92  CConfigFileMemory memFil;
93  rawlog.getCommentTextAsConfigFile(memFil);
94 
95  TGeodeticCoords ref(
96  memFil.read_double("GPS_ORIGIN", "lat_deg", 0),
97  memFil.read_double("GPS_ORIGIN", "lon_deg", 0),
98  memFil.read_double("GPS_ORIGIN", "height", 0));
99 
100  ref_valid = !ref.isClear();
101 
102  // Do we have info for the consistency test?
103  const double std_0 = memFil.read_double("CONSISTENCY_TEST", "std_0", 0);
104  bool doConsistencyCheck = std_0 > 0;
105 
106  // Do we have the "reference uncertainty" matrix W^\star ??
107  memFil.read_matrix("UNCERTAINTY", "W_star", outInfoTemp.W_star);
108  const bool doUncertaintyCovs = size(outInfoTemp.W_star, 1) != 0;
109  if (doUncertaintyCovs &&
110  (size(outInfoTemp.W_star, 1) != 6 || size(outInfoTemp.W_star, 2) != 6))
111  THROW_EXCEPTION(
112  "ERROR: W_star matrix for uncertainty estimation is provided but "
113  "it's not a 6x6 matrix.");
114 
115 // ------------------------------------------
116 // Look for the 2 observations:
117 // ------------------------------------------
118 #if MRPT_HAS_WXWIDGETS
119  wxProgressDialog* progDia = nullptr;
120  if (isGUI)
121  {
122  progDia = new wxProgressDialog(
123  wxT("Building map"), wxT("Getting GPS observations..."),
124  (int)(last - first + 1), // range
125  nullptr, // parent
126  wxPD_CAN_ABORT | wxPD_APP_MODAL | wxPD_SMOOTH | wxPD_AUTO_HIDE |
127  wxPD_ELAPSED_TIME | wxPD_ESTIMATED_TIME | wxPD_REMAINING_TIME);
128  }
129 #endif
130 
131  // The list with all time ordered gps's in valid RTK mode
132  typedef std::map<mrpt::system::TTimeStamp,
133  std::map<std::string, CObservationGPS::Ptr>>
134  TListGPSs;
135  TListGPSs list_gps_obs;
136 
137  map<string, size_t> GPS_RTK_reads; // label-># of RTK readings
138  map<string, TPoint3D>
139  GPS_local_coords_on_vehicle; // label -> local pose on the vehicle
140 
141  for (i = first; !abort && i <= last; i++)
142  {
143  switch (rawlog.getType(i))
144  {
145  default:
146  break;
147 
148  case CRawlog::etObservation:
149  {
150  CObservation::Ptr o = rawlog.getAsObservation(i);
151 
152  if (o->GetRuntimeClass() == CLASS_ID(CObservationGPS))
153  {
154  CObservationGPS::Ptr obs =
155  std::dynamic_pointer_cast<CObservationGPS>(o);
156 
157  if (obs->has_GGA_datum &&
158  obs->getMsgByClass<gnss::Message_NMEA_GGA>()
159  .fields.fix_quality == 4)
160  {
161  // Add to the list:
162  list_gps_obs[obs->timestamp][obs->sensorLabel] = obs;
163 
164  lstGPSLabels.insert(obs->sensorLabel);
165  }
166 
167  // Save to GPS paths:
168  if (obs->has_GGA_datum &&
169  (obs->getMsgByClass<gnss::Message_NMEA_GGA>()
170  .fields.fix_quality == 4 ||
171  obs->getMsgByClass<gnss::Message_NMEA_GGA>()
172  .fields.fix_quality == 5))
173  {
174  GPS_RTK_reads[obs->sensorLabel]++;
175 
176  // map<string,TPoint3D> GPS_local_coords_on_vehicle; //
177  // label -> local pose on the vehicle
178  if (GPS_local_coords_on_vehicle.find(
179  obs->sensorLabel) ==
180  GPS_local_coords_on_vehicle.end())
181  GPS_local_coords_on_vehicle[obs->sensorLabel] =
182  TPoint3D(obs->sensorPose);
183 
184  // map<string, map<TTimeStamp,TPoint3D> > gps_paths;
185  // //
186  // label -> (time -> 3D local coords)
187  gps_paths[obs->sensorLabel][obs->timestamp] = TPoint3D(
188  obs->getMsgByClass<gnss::Message_NMEA_GGA>()
189  .fields.longitude_degrees,
190  obs->getMsgByClass<gnss::Message_NMEA_GGA>()
191  .fields.latitude_degrees,
192  obs->getMsgByClass<gnss::Message_NMEA_GGA>()
193  .fields.altitude_meters);
194  }
195  }
196  }
197  break;
198  } // end switch type
199 
200  // Show progress:
201  if ((count++ % 100) == 0)
202  {
203 #if MRPT_HAS_WXWIDGETS
204  if (progDia)
205  {
206  if (!progDia->Update((int)(i - first))) abort = true;
207  wxTheApp->Yield();
208  }
209 #endif
210  }
211  } // end for i
212 
213 #if MRPT_HAS_WXWIDGETS
214  if (progDia)
215  {
216  delete progDia;
217  progDia = nullptr;
218  }
219 #endif
220 
221  // -----------------------------------------------------------
222  // At this point we already have the sensor positions, thus
223  // we can estimate the covariance matrix D:
224  //
225  // TODO: Generalize equations for # of GPS > 3
226  // -----------------------------------------------------------
227  map<set<string>, double> Ad_ij; // InterGPS distances in 2D
228  map<set<string>, double>
229  phi_ij; // Directions on XY of the lines between i-j
230  map<string, size_t>
231  D_cov_indexes; // Sensor label-> index in the matrix (first=0, ...)
232  map<size_t, string> D_cov_rev_indexes; // Reverse of D_cov_indexes
233 
234  CMatrixDouble D_cov; // square distances cov
235  CMatrixDouble D_cov_1; // square distances cov (inverse)
236  CVectorDouble D_mean; // square distances mean
237 
238  if (doConsistencyCheck && GPS_local_coords_on_vehicle.size() == 3)
239  {
240  unsigned int cnt = 0;
241  for (map<string, TPoint3D>::iterator i =
242  GPS_local_coords_on_vehicle.begin();
243  i != GPS_local_coords_on_vehicle.end(); ++i)
244  {
245  // Index tables:
246  D_cov_indexes[i->first] = cnt;
247  D_cov_rev_indexes[cnt] = i->first;
248  cnt++;
249 
250  for (map<string, TPoint3D>::iterator j = i;
251  j != GPS_local_coords_on_vehicle.end(); ++j)
252  {
253  if (i != j)
254  {
255  const TPoint3D& pi = i->second;
256  const TPoint3D& pj = j->second;
257  Ad_ij[make_set(i->first, j->first)] = pi.distanceTo(pj);
258  phi_ij[make_set(i->first, j->first)] =
259  atan2(pj.y - pi.y, pj.x - pi.x);
260  }
261  }
262  }
263  ASSERT_(D_cov_indexes.size() == 3 && D_cov_rev_indexes.size() == 3);
264 
265  D_cov.setSize(D_cov_indexes.size(), D_cov_indexes.size());
266  D_mean.resize(D_cov_indexes.size());
267 
268  // See paper for the formulas!
269  // TODO: generalize for N>3
270 
271  D_cov(0, 0) =
272  2 *
273  square(Ad_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[1])]);
274  D_cov(1, 1) =
275  2 *
276  square(Ad_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[2])]);
277  D_cov(2, 2) =
278  2 *
279  square(Ad_ij[make_set(D_cov_rev_indexes[1], D_cov_rev_indexes[2])]);
280 
281  D_cov(1, 0) =
282  Ad_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[1])] *
283  Ad_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[2])] *
284  cos(phi_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[1])] -
285  phi_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[2])]);
286  D_cov(0, 1) = D_cov(1, 0);
287 
288  D_cov(2, 0) =
289  -Ad_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[1])] *
290  Ad_ij[make_set(D_cov_rev_indexes[1], D_cov_rev_indexes[2])] *
291  cos(phi_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[1])] -
292  phi_ij[make_set(D_cov_rev_indexes[1], D_cov_rev_indexes[2])]);
293  D_cov(0, 2) = D_cov(2, 0);
294 
295  D_cov(2, 1) =
296  Ad_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[2])] *
297  Ad_ij[make_set(D_cov_rev_indexes[1], D_cov_rev_indexes[2])] *
298  cos(phi_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[2])] -
299  phi_ij[make_set(D_cov_rev_indexes[1], D_cov_rev_indexes[2])]);
300  D_cov(1, 2) = D_cov(2, 1);
301 
302  D_cov *= 4 * square(std_0);
303 
304  D_cov_1 = D_cov.inv();
305 
306  // cout << D_cov.inMatlabFormat() << endl;
307 
308  D_mean[0] =
309  square(Ad_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[1])]);
310  D_mean[1] =
311  square(Ad_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[2])]);
312  D_mean[2] =
313  square(Ad_ij[make_set(D_cov_rev_indexes[1], D_cov_rev_indexes[2])]);
314  }
315  else
316  doConsistencyCheck = false;
317 
318  // ------------------------------------------
319  // Look for the 2 observations:
320  // ------------------------------------------
321  int N_GPSs = list_gps_obs.size();
322 
323  if (N_GPSs)
324  {
325  // loop interpolate 1-out-of-5: this solves the issue with JAVAD GPSs
326  // that skip some readings at some times .0 .2 .4 .6 .8
327  if (list_gps_obs.size() > 4)
328  {
329  TListGPSs::iterator F = list_gps_obs.begin();
330  ++F;
331  ++F;
332  TListGPSs::iterator E = list_gps_obs.end();
333  --E;
334  --E;
335 
336  for (TListGPSs::iterator i = F; i != E; ++i)
337  {
338  // Now check if we have 3 gps with the same time stamp:
339  // const size_t N = i->second.size();
340  std::map<std::string, CObservationGPS::Ptr>& GPS = i->second;
341 
342  // Check if any in "lstGPSLabels" is missing here:
343  for (set<string>::iterator l = lstGPSLabels.begin();
344  l != lstGPSLabels.end(); ++l)
345  {
346  // For each GPS in the current timestamp:
347  bool fnd = (GPS.find(*l) != GPS.end());
348 
349  if (fnd) continue; // this one is present.
350 
351  // Ok, we have "*l" missing in the set "*i".
352  // Try to interpolate from neighbors:
353  TListGPSs::iterator i_b1 = i;
354  --i_b1;
355  TListGPSs::iterator i_a1 = i;
356  ++i_a1;
357 
358  CObservationGPS::Ptr GPS_b1, GPS_a1;
359 
360  if (i_b1->second.find(*l) != i_b1->second.end())
361  GPS_b1 = i_b1->second.find(*l)->second;
362 
363  if (i_a1->second.find(*l) != i_a1->second.end())
364  GPS_a1 = i_a1->second.find(*l)->second;
365 
366  if (!disableGPSInterp && GPS_a1 && GPS_b1)
367  {
368  if (mrpt::system::timeDifference(
369  GPS_b1->timestamp, GPS_a1->timestamp) < 0.5)
370  {
371  CObservationGPS::Ptr new_gps = CObservationGPS::Ptr(
372  new CObservationGPS(*GPS_a1));
373  new_gps->sensorLabel = *l;
374 
375  // cout <<
376  // mrpt::system::timeLocalToString(GPS_b1->timestamp)
377  // << " " <<
378  // mrpt::system::timeLocalToString(GPS_a1->timestamp)
379  // << " " << *l;
380  // cout << endl;
381 
382  new_gps->getMsgByClass<gnss::Message_NMEA_GGA>()
383  .fields.longitude_degrees =
384  0.5 *
385  (GPS_a1->getMsgByClass<gnss::Message_NMEA_GGA>()
386  .fields.longitude_degrees +
387  GPS_b1->getMsgByClass<gnss::Message_NMEA_GGA>()
388  .fields.longitude_degrees);
389  new_gps->getMsgByClass<gnss::Message_NMEA_GGA>()
390  .fields.latitude_degrees =
391  0.5 *
392  (GPS_a1->getMsgByClass<gnss::Message_NMEA_GGA>()
393  .fields.latitude_degrees +
394  GPS_b1->getMsgByClass<gnss::Message_NMEA_GGA>()
395  .fields.latitude_degrees);
396  new_gps->getMsgByClass<gnss::Message_NMEA_GGA>()
397  .fields.altitude_meters =
398  0.5 *
399  (GPS_a1->getMsgByClass<gnss::Message_NMEA_GGA>()
400  .fields.altitude_meters +
401  GPS_b1->getMsgByClass<gnss::Message_NMEA_GGA>()
402  .fields.altitude_meters);
403 
404  new_gps->timestamp =
405  (GPS_a1->timestamp + GPS_b1->timestamp) / 2;
406 
407  i->second[new_gps->sensorLabel] = new_gps;
408  }
409  }
410  }
411  } // end loop interpolate 1-out-of-5
412  }
413 
414 #if MRPT_HAS_WXWIDGETS
415  wxProgressDialog* progDia3 = nullptr;
416  if (isGUI)
417  {
418  progDia3 = new wxProgressDialog(
419  wxT("Building map"), wxT("Estimating 6D path..."),
420  N_GPSs, // range
421  nullptr, // parent
422  wxPD_CAN_ABORT | wxPD_APP_MODAL | wxPD_SMOOTH | wxPD_AUTO_HIDE |
423  wxPD_ELAPSED_TIME | wxPD_ESTIMATED_TIME |
424  wxPD_REMAINING_TIME);
425  }
426 #endif
427 
428  int idx_in_GPSs = 0;
429 
430  for (TListGPSs::iterator i = list_gps_obs.begin();
431  i != list_gps_obs.end(); ++i, idx_in_GPSs++)
432  {
433  // Now check if we have 3 gps with the same time stamp:
434  if (i->second.size() >= 3)
435  {
436  const size_t N = i->second.size();
437  std::map<std::string, CObservationGPS::Ptr>& GPS = i->second;
438  CVectorDouble X(N), Y(N), Z(N); // Global XYZ coordinates
439  std::map<string, size_t>
440  XYZidxs; // Sensor label -> indices in X Y Z
441 
442  if (!ref_valid) // get the reference lat/lon, if it's not set
443  // from rawlog configuration block.
444  {
445  ref_valid = true;
446  ref = GPS.begin()
447  ->second->getMsgByClass<gnss::Message_NMEA_GGA>()
448  .getAsStruct<TGeodeticCoords>();
449  }
450 
451  // Compute the XYZ coordinates of all sensors:
452  TMatchingPairList corrs;
453  unsigned int k;
454  std::map<std::string, CObservationGPS::Ptr>::iterator g_it;
455 
456  for (k = 0, g_it = GPS.begin(); g_it != GPS.end(); ++g_it, ++k)
457  {
458  TPoint3D P;
459  mrpt::topography::geodeticToENU_WGS84(
460  g_it->second->getMsgByClass<gnss::Message_NMEA_GGA>()
461  .getAsStruct<TGeodeticCoords>(),
462  P, ref);
463 
464  // Correction of offsets:
465  const string sect =
466  string("OFFSET_") + g_it->second->sensorLabel;
467  P.x += memFil.read_double(sect, "x", 0);
468  P.y += memFil.read_double(sect, "y", 0);
469  P.z += memFil.read_double(sect, "z", 0);
470 
471  XYZidxs[g_it->second->sensorLabel] =
472  k; // Save index correspondence
473 
474  // Create the correspondence:
475  corrs.push_back(
476  TMatchingPair(
477  k, k, // Indices
478  P.x, P.y, P.z, // "This"/Global coords
479  g_it->second->sensorPose.x(),
480  g_it->second->sensorPose.y(),
481  g_it->second->sensorPose
482  .z() // "other"/local coordinates
483  ));
484 
485  X[k] = P.x;
486  Y[k] = P.y;
487  Z[k] = P.z;
488  }
489 
490  if (doConsistencyCheck && GPS.size() == 3)
491  {
492  // XYZ[k] have the k'd final coordinates of each GPS
493  // GPS[k] are the CObservations:
494 
495  // Compute the inter-GPS square distances:
496  CVectorDouble iGPSdist2(3);
497 
498  // [0]: sq dist between:
499  // D_cov_rev_indexes[0],D_cov_rev_indexes[1]
500  TPoint3D P0(
501  X[XYZidxs[D_cov_rev_indexes[0]]],
502  Y[XYZidxs[D_cov_rev_indexes[0]]],
503  Z[XYZidxs[D_cov_rev_indexes[0]]]);
504  TPoint3D P1(
505  X[XYZidxs[D_cov_rev_indexes[1]]],
506  Y[XYZidxs[D_cov_rev_indexes[1]]],
507  Z[XYZidxs[D_cov_rev_indexes[1]]]);
508  TPoint3D P2(
509  X[XYZidxs[D_cov_rev_indexes[2]]],
510  Y[XYZidxs[D_cov_rev_indexes[2]]],
511  Z[XYZidxs[D_cov_rev_indexes[2]]]);
512 
513  iGPSdist2[0] = P0.sqrDistanceTo(P1);
514  iGPSdist2[1] = P0.sqrDistanceTo(P2);
515  iGPSdist2[2] = P1.sqrDistanceTo(P2);
516 
517  double mahaD = mrpt::math::mahalanobisDistance(
518  iGPSdist2, D_mean, D_cov_1);
519  outInfoTemp.mahalabis_quality_measure[i->first] = mahaD;
520 
521  // cout << "x: " << iGPSdist2 << " MU: " << D_mean << " ->
522  // " << mahaD << endl;
523  } // end consistency
524 
525  // Use a 6D matching method to estimate the location of the
526  // vehicle:
527  CPose3DQuat optimal_pose;
528  double optimal_scale;
529 
530  // "this" (reference map) -> GPS global coordinates
531  // "other" -> GPS local coordinates on the vehicle
532  mrpt::tfest::se3_l2(
533  corrs, optimal_pose, optimal_scale, true); // Force scale=1
534  // cout << "optimal pose: " << optimal_pose << " " <<
535  // optimal_scale << endl;
536  MRPT_CHECK_NORMAL_NUMBER(optimal_pose.x());
537  MRPT_CHECK_NORMAL_NUMBER(optimal_pose.y());
538  MRPT_CHECK_NORMAL_NUMBER(optimal_pose.z());
539  MRPT_CHECK_NORMAL_NUMBER(optimal_pose.quat().x());
540  MRPT_CHECK_NORMAL_NUMBER(optimal_pose.quat().y());
541  MRPT_CHECK_NORMAL_NUMBER(optimal_pose.quat().z());
542  MRPT_CHECK_NORMAL_NUMBER(optimal_pose.quat().r());
543 
544  // Final vehicle pose:
545  const CPose3D veh_pose = CPose3D(optimal_pose);
546 
547  // Add to the interpolator:
548  robot_path.insert(i->first, veh_pose);
549 
550  // If we have W_star, compute the pose uncertainty:
551  if (doUncertaintyCovs)
552  {
553  CPose3DPDFGaussian final_veh_uncert;
554  final_veh_uncert.mean.setFromValues(0, 0, 0, 0, 0, 0);
555  final_veh_uncert.cov = outInfoTemp.W_star;
556 
557  // Rotate the covariance according to the real vehicle pose:
558  final_veh_uncert.changeCoordinatesReference(veh_pose);
559 
560  outInfoTemp.vehicle_uncertainty[i->first] =
561  final_veh_uncert.cov;
562  }
563  }
564 
565  // Show progress:
566  if ((count++ % 100) == 0)
567  {
568 #if MRPT_HAS_WXWIDGETS
569  if (progDia3)
570  {
571  if (!progDia3->Update(idx_in_GPSs)) abort = true;
572  wxTheApp->Yield();
573  }
574 #endif
575  }
576  } // end for i
577 
578 #if MRPT_HAS_WXWIDGETS
579  if (progDia3)
580  {
581  delete progDia3;
582  progDia3 = nullptr;
583  }
584 #endif
585 
586  if (PATH_SMOOTH_FILTER > 0 && robot_path.size() > 1)
587  {
588  CPose3DInterpolator filtered_robot_path = robot_path;
589 
590  // Do Angles smoother filter of the path:
591  // ---------------------------------------------
592  const double MAX_DIST_TO_FILTER = 4.0;
593 
594  for (auto i = robot_path.begin(); i != robot_path.end(); ++i)
595  {
596  mrpt::math::TPose3D p = i->second;
597 
598  CVectorDouble pitchs, rolls; // The elements to average
599 
600  pitchs.push_back(p.pitch);
601  rolls.push_back(p.roll);
602 
603  CPose3DInterpolator::iterator q = i;
604  for (int k = 0;
605  k < PATH_SMOOTH_FILTER && q != robot_path.begin(); k++)
606  {
607  --q;
608  if (abs(mrpt::system::timeDifference(q->first, i->first)) <
609  MAX_DIST_TO_FILTER)
610  {
611  pitchs.push_back(q->second.pitch);
612  rolls.push_back(q->second.roll);
613  }
614  }
615  q = i;
616  for (int k = 0;
617  k < PATH_SMOOTH_FILTER && q != (--robot_path.end()); k++)
618  {
619  ++q;
620  if (abs(mrpt::system::timeDifference(q->first, i->first)) <
621  MAX_DIST_TO_FILTER)
622  {
623  pitchs.push_back(q->second.pitch);
624  rolls.push_back(q->second.roll);
625  }
626  }
627 
628  p.pitch = mrpt::math::averageWrap2Pi(pitchs);
629  p.roll = mrpt::math::averageWrap2Pi(rolls);
630 
631  // save in filtered path:
632  filtered_robot_path.insert(i->first, p);
633  }
634  // Replace:
635  robot_path = filtered_robot_path;
636 
637  } // end PATH_SMOOTH_FILTER
638 
639  } // end step generate 6D path
640 
641  // Here we can set best_gps_path (that with the max. number of RTK
642  // fixed/foat readings):
643  if (outInfo)
644  {
645  string bestLabel;
646  size_t bestNum = 0;
647  for (map<string, size_t>::iterator i = GPS_RTK_reads.begin();
648  i != GPS_RTK_reads.end(); ++i)
649  {
650  if (i->second > bestNum)
651  {
652  bestNum = i->second;
653  bestLabel = i->first;
654  }
655  }
656  outInfoTemp.best_gps_path = gps_paths[bestLabel];
657 
658  // and transform to XYZ:
659  // Correction of offsets:
660  const string sect = string("OFFSET_") + bestLabel;
661  const double off_X = memFil.read_double(sect, "x", 0);
662  const double off_Y = memFil.read_double(sect, "y", 0);
663  const double off_Z = memFil.read_double(sect, "z", 0);
664 
665  // map<TTimeStamp,TPoint3D> best_gps_path; // time -> 3D local
666  // coords
667  for (map<TTimeStamp, TPoint3D>::iterator i =
668  outInfoTemp.best_gps_path.begin();
669  i != outInfoTemp.best_gps_path.end(); ++i)
670  {
671  TPoint3D P;
672  TPoint3D& pl = i->second;
673  mrpt::topography::geodeticToENU_WGS84(
674  TGeodeticCoords(
675  pl.x, pl.y, pl.z), // i->second.x,i->second.y,i->second.z,
676  // // lat, lon, heigh
677  P, // X Y Z
678  ref);
679 
680  pl.x = P.x + off_X;
681  pl.y = P.y + off_Y;
682  pl.z = P.z + off_Z;
683  }
684  } // end best_gps_path
685 
686  if (outInfo) *outInfo = outInfoTemp;
687 
688  MRPT_END
689 }
CLASS_ID
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:85
mrpt::math::CQuaternion::z
T z() const
Return z coordinate of the quaternion.
Definition: CQuaternion.h:94
mrpt::math::CQuaternion::x
T x() const
Return x coordinate of the quaternion.
Definition: CQuaternion.h:90
mrpt::math::CQuaternion::r
T r() const
Return r coordinate of the quaternion.
Definition: CQuaternion.h:88
mrpt::math::CQuaternion::y
T y() const
Return y coordinate of the quaternion.
Definition: CQuaternion.h:92
mrpt::math::dynamic_vector
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
Definition: types_math.h:70
mrpt::obs::CObservationGPS
This class stores messages from GNSS or GNSS+IMU devices, from consumer-grade inexpensive GPS receive...
Definition: CObservationGPS.h:70
mrpt::obs::CObservationGPS::Ptr
std::shared_ptr< CObservationGPS > Ptr
Definition: CObservationGPS.h:71
mrpt::obs::CObservation::Ptr
std::shared_ptr< CObservation > Ptr
Definition: CObservation.h:43
mrpt::obs::CRawlog
This class stores a rawlog (robotic datasets) in one of two possible formats:
Definition: CRawlog.h:69
mrpt::obs::CRawlog::getAsObservation
CObservation::Ptr getAsObservation(size_t index) const
Returns the i'th element in the sequence, as being an observation, where index=0 is the first object.
Definition: CRawlog.cpp:105
mrpt::obs::CRawlog::getCommentTextAsConfigFile
void getCommentTextAsConfigFile(mrpt::utils::CConfigFileMemory &memCfg) const
Saves the block of comment text for the rawlog into the passed config file object.
Definition: CRawlog.cpp:579
mrpt::obs::CRawlog::size
size_t size() const
Returns the number of actions / observations object in the sequence.
Definition: CRawlog.cpp:88
mrpt::obs::CRawlog::getType
TEntryType getType(size_t index) const
Returns the type of a given element.
Definition: CRawlog.cpp:130
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:89
mrpt::poses::CPose3D::setFromValues
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:248
mrpt::poses::CPose3DInterpolator
This class stores a time-stamped trajectory in SE(3) (CPose3D poses).
Definition: CPose3DInterpolator.h:51
mrpt::poses::CPose3DPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
Definition: CPose3DPDFGaussian.h:41
mrpt::poses::CPose3DPDFGaussian::changeCoordinatesReference
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
Definition: CPose3DPDFGaussian.cpp:340
mrpt::poses::CPose3DPDFGaussian::cov
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
Definition: CPose3DPDFGaussian.h:83
mrpt::poses::CPose3DQuat
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,...
Definition: CPose3DQuat.h:49
mrpt::poses::CPose3DQuat::quat
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:60
mrpt::poses::CPoseInterpolatorBase::setMaxTimeInterpolation
void setMaxTimeInterpolation(double time)
Set value of the maximum time to consider interpolation.
Definition: CPoseInterpolatorBase.hpp:212
mrpt::poses::CPoseInterpolatorBase::insert
void insert(mrpt::system::TTimeStamp t, const pose_t &p)
Inserts a new pose in the sequence.
Definition: CPoseInterpolatorBase.hpp:41
mrpt::poses::CPoseInterpolatorBase::clear
void clear()
Clears the current sequence of poses.
Definition: CPoseInterpolatorBase.hpp:30
mrpt::poses::CPoseInterpolatorBase::setInterpolationMethod
void setInterpolationMethod(TInterpolatorMethod method)
Change the method used to interpolate the robot path.
Definition: CPoseInterpolatorBase.hpp:344
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:135
mrpt::utils::CConfigFileBase::read_matrix
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 ; ...
Definition: CConfigFileBase.h:203
mrpt::utils::CConfigFileBase::read_double
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:94
mrpt::utils::CConfigFileMemory
This class implements a config file-like interface over a memory-stored string list.
Definition: CConfigFileMemory.h:32
mrpt::utils::TMatchingPairList
A list of TMatchingPair.
Definition: TMatchingPair.h:94
data_types.h
data_utils.h
iterator
Scalar * iterator
Definition: eigen_plugins.h:26
v1
GLfloat GLfloat v1
Definition: glext.h:4105
ref
GLenum GLint ref
Definition: glext.h:4050
count
GLuint GLuint GLsizei count
Definition: glext.h:3528
v0
GLfloat v0
Definition: glext.h:4103
first
GLint * first
Definition: glext.h:3827
p
GLfloat GLfloat p
Definition: glext.h:6305
size
GLsizeiptr size
Definition: glext.h:3923
s
GLdouble s
Definition: glext.h:3676
string
GLsizei const GLchar ** string
Definition: glext.h:4101
q
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
mrpt::tfest::se3_l2
bool se3_l2(const mrpt::utils::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:223
mrpt::topography::geodeticToENU_WGS84
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...
Definition: conversions.cpp:127
mrpt::topography::path_from_rtk_gps
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.
Definition: path_from_rtk_gps.cpp:51
mrpt::poses::imSSLSLL
@ imSSLSLL
Definition: CPoseInterpolatorBase.h:43
mrpt::math::mahalanobisDistance
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:59
mrpt::math::averageWrap2Pi
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:1799
mrpt::system::TTimeStamp
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1,...
Definition: datetime.h:32
mrpt::system::timeDifference
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.cpp:208
MRPT_START
#define MRPT_START
Definition: mrpt_macros.h:425
ASSERT_
#define ASSERT_(f)
Definition: mrpt_macros.h:309
MRPT_END
#define MRPT_END
Definition: mrpt_macros.h:429
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: mrpt_macros.h:111
MRPT_CHECK_NORMAL_NUMBER
#define MRPT_CHECK_NORMAL_NUMBER(val)
Definition: mrpt_macros.h:312
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Definition: mrpt_macros.h:365
mrpt::math
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:20
mrpt::math::square
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:55
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:20
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:18
mrpt::system
This namespace provides a OS-independent interface to many useful functions: filenames manipulation,...
Definition: math_frwds.h:31
mrpt::topography
This namespace provides topography helper functions, coordinate transformations.
Definition: conversions.h:24
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values,...
Definition: CParticleFilter.h:224
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:18
make_set
std::set< T > make_set(const T &v0, const T &v1)
Definition: path_from_rtk_gps.cpp:40
path_from_rtk_gps.h
mrpt::math::TPoint3D
Lightweight 3D point.
Definition: lightweight_geom_data.h:382
mrpt::math::TPoint3D::distanceTo
double distanceTo(const TPoint3D &p) const
Point-to-point distance.
Definition: lightweight_geom_data.h:453
mrpt::math::TPoint3D::x
double x
X,Y,Z coordinates.
Definition: lightweight_geom_data.h:388
mrpt::math::TPoint3D::sqrDistanceTo
double sqrDistanceTo(const TPoint3D &p) const
Point-to-point distance, squared.
Definition: lightweight_geom_data.h:460
mrpt::math::TPose3D
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: lightweight_geom_data.h:617
mrpt::obs::gnss::Message_NMEA_GGA::content_t::latitude_degrees
double latitude_degrees
The measured latitude, in degrees (North:+ , South:-)
Definition: gnss_messages_ascii_nmea.h:37
mrpt::obs::gnss::Message_NMEA_GGA::content_t::fix_quality
uint8_t fix_quality
NMEA standard values: 0 = invalid, 1 = GPS fix (SPS), 2 = DGPS fix, 3 = PPS fix, 4 = Real Time Kinema...
Definition: gnss_messages_ascii_nmea.h:44
mrpt::obs::gnss::Message_NMEA_GGA::content_t::altitude_meters
double altitude_meters
The measured altitude, in meters (A).
Definition: gnss_messages_ascii_nmea.h:46
mrpt::obs::gnss::Message_NMEA_GGA::content_t::longitude_degrees
double longitude_degrees
The measured longitude, in degrees (East:+ , West:-)
Definition: gnss_messages_ascii_nmea.h:39
mrpt::obs::gnss::Message_NMEA_GGA
NMEA datum: GGA.
Definition: gnss_messages_ascii_nmea.h:24
mrpt::obs::gnss::Message_NMEA_GGA::getAsStruct
TGEODETICCOORDS getAsStruct() const
Return the geodetic coords as a mrpt::topography::TGeodeticCoords structure (requires linking against...
Definition: gnss_messages_ascii_nmea.h:97
mrpt::obs::gnss::Message_NMEA_GGA::fields
content_t fields
Message content, accesible by individual fields.
Definition: gnss_messages_ascii_nmea.h:67
mrpt::topography::TGeodeticCoords
A set of geodetic coordinates: latitude, longitude and height, defined over a given geoid (typically,...
Definition: data_types.h:197
mrpt::topography::TPathFromRTKInfo
Used to return optional information from mrpt::topography::path_from_rtk_gps.
Definition: path_from_rtk_gps.h:27
mrpt::topography::TPathFromRTKInfo::W_star
mrpt::math::CMatrixDouble W_star
The reference covariance matrix used to compute vehicle_uncertainty.
Definition: path_from_rtk_gps.h:39
mrpt::topography::TPathFromRTKInfo::best_gps_path
std::map< mrpt::system::TTimeStamp, mrpt::math::TPoint3D > best_gps_path
the path of the "best" GPS.
Definition: path_from_rtk_gps.h:29
mrpt::topography::TPathFromRTKInfo::mahalabis_quality_measure
std::map< mrpt::system::TTimeStamp, double > mahalabis_quality_measure
A measure of the quality at each point (may be empty if not there is no enough information).
Definition: path_from_rtk_gps.h:32
mrpt::topography::TPathFromRTKInfo::vehicle_uncertainty
mrpt::aligned_containers< mrpt::system::TTimeStamp, mrpt::math::CMatrixDouble66 >::map_t vehicle_uncertainty
The 6x6 covariance matrix for the uncertainty of each vehicle pose (may be empty if there is no W_sta...
Definition: path_from_rtk_gps.h:37
mrpt::utils::TMatchingPair
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
Definition: TMatchingPair.h:32


Page generated by Doxygen 1.9.1 for MRPT 1.9.9 Git: 63ea9d1f1 Thu Nov 23 00:06:53 2017 +0100 at mar 26 may 2026 12:19:29 CEST