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



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