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



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