Debian Codegraph @

libs/topography/src/path_from_rtk_gps.cpp

raw
1 /* +------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | https://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2026, 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>
13 #include <mrpt/obs/CObservationGPS.h>
14 #include <mrpt/tfest/se3.h>
15 #include <mrpt/topography/conversions.h>
16 #include <mrpt/topography/data_types.h>
17 #include <mrpt/topography/path_from_rtk_gps.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 ---------------------------------------------------------------*/
43 void mrpt::topography::path_from_rtk_gps(
44 mrpt::poses::CPose3DInterpolator& robot_path,
45 const mrpt::obs::CRawlog& rawlog,
46 size_t first,
47 size_t last,
48 bool isGUI,
49 bool disableGPSInterp,
50 int PATH_SMOOTH_FILTER,
51 TPathFromRTKInfo* outInfo)
52 {
53 MRPT_START
54
55 // Go: generate the map:
56 ASSERT_(first <= last);
57 ASSERT_(last <= rawlog.size() - 1);
58
59 set<string> lstGPSLabels;
60
61 robot_path.clear();
62 robot_path.setMaxTimeInterpolation(
63 std::chrono::seconds(3)); // Max. seconds of GPS blackout not to interpolate.
64 robot_path.setInterpolationMethod(mrpt::poses::imSSLSLL);
65
66 TPathFromRTKInfo outInfoTemp;
67 if (outInfo) *outInfo = outInfoTemp;
68
69 // label -> (time -> 3D local coords)
70 map<string, map<Clock::time_point, TPoint3D>> gps_paths;
71
72 bool abort = false;
73 bool ref_valid = false;
74
75 // Load configuration block:
76 CConfigFileMemory memFil;
77 rawlog.getCommentTextAsConfigFile(memFil);
78
79 TGeodeticCoords ref(
80 memFil.read_double("GPS_ORIGIN", "lat_deg", 0),
81 memFil.read_double("GPS_ORIGIN", "lon_deg", 0),
82 memFil.read_double("GPS_ORIGIN", "height", 0));
83
84 ref_valid = !ref.isClear();
85
86 // Do we have info for the consistency test?
87 const double std_0 = memFil.read_double("CONSISTENCY_TEST", "std_0", 0);
88 bool doConsistencyCheck = std_0 > 0;
89
90 // Do we have the "reference uncertainty" matrix W^\star ??
91 memFil.read_matrix("UNCERTAINTY", "W_star", outInfoTemp.W_star);
92 const bool doUncertaintyCovs = outInfoTemp.W_star.rows() != 0;
93 if (doUncertaintyCovs && (outInfoTemp.W_star.rows() != 6 || outInfoTemp.W_star.cols() != 6))
94 THROW_EXCEPTION(
95 "ERROR: W_star matrix for uncertainty estimation is provided but "
96 "it's not a 6x6 matrix.");
97
98 // ------------------------------------------
99 // Look for the 2 observations:
100 // ------------------------------------------
101
102 // The list with all time ordered gps's in valid RTK mode
103 using TListGPSs = std::map<mrpt::Clock::time_point, std::map<std::string, CObservationGPS::Ptr>>;
104 TListGPSs list_gps_obs;
105
106 map<string, size_t> GPS_RTK_reads; // label-># of RTK readings
107 map<string, TPoint3D> GPS_local_coords_on_vehicle; // label -> local pose on the vehicle
108
109 for (size_t i = first; !abort && i <= last; i++)
110 {
111 switch (rawlog.getType(i))
112 {
113 default:
114 break;
115
116 case CRawlog::etObservation:
117 {
118 CObservation::Ptr o = rawlog.getAsObservation(i);
119
120 if (o->GetRuntimeClass() == CLASS_ID(CObservationGPS))
121 {
122 CObservationGPS::Ptr obs = std::dynamic_pointer_cast<CObservationGPS>(o);
123
124 if (obs->has_GGA_datum() &&
125 obs->getMsgByClass<gnss::Message_NMEA_GGA>().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>().fields.fix_quality == 4 ||
136 obs->getMsgByClass<gnss::Message_NMEA_GGA>().fields.fix_quality == 5))
137 {
138 GPS_RTK_reads[obs->sensorLabel]++;
139
140 // map<string,TPoint3D> GPS_local_coords_on_vehicle; //
141 // label -> local pose on the vehicle
142 if (GPS_local_coords_on_vehicle.find(obs->sensorLabel) ==
143 GPS_local_coords_on_vehicle.end())
144 GPS_local_coords_on_vehicle[obs->sensorLabel] = TPoint3D(obs->sensorPose.asTPose());
145
146 // map<string, map<Clock::time_point,TPoint3D> >
147 // gps_paths;
148 // //
149 // label -> (time -> 3D local coords)
150 gps_paths[obs->sensorLabel][obs->timestamp] = TPoint3D(
151 obs->getMsgByClass<gnss::Message_NMEA_GGA>().fields.longitude_degrees,
152 obs->getMsgByClass<gnss::Message_NMEA_GGA>().fields.latitude_degrees,
153 obs->getMsgByClass<gnss::Message_NMEA_GGA>().fields.altitude_meters);
154 }
155 }
156 }
157 break;
158 } // end switch type
159
160 } // end for i
161
162 // -----------------------------------------------------------
163 // At this point we already have the sensor positions, thus
164 // we can estimate the covariance matrix D:
165 //
166 // TODO: Generalize equations for # of GPS > 3
167 // -----------------------------------------------------------
168 map<set<string>, double> Ad_ij; // InterGPS distances in 2D
169 map<set<string>, double> phi_ij; // Directions on XY of the lines between i-j
170 map<string, size_t> D_cov_indexes; // Sensor label-> index in the matrix (first=0, ...)
171 map<size_t, string> D_cov_rev_indexes; // Reverse of D_cov_indexes
172
173 CMatrixDouble D_cov; // square distances cov
174 CMatrixDouble D_cov_1; // square distances cov (inverse)
175 CVectorDouble D_mean; // square distances mean
176
177 if (doConsistencyCheck && GPS_local_coords_on_vehicle.size() == 3)
178 {
179 unsigned int cnt = 0;
180 for (auto i = GPS_local_coords_on_vehicle.begin(); i != GPS_local_coords_on_vehicle.end(); ++i)
181 {
182 // Index tables:
183 D_cov_indexes[i->first] = cnt;
184 D_cov_rev_indexes[cnt] = i->first;
185 cnt++;
186
187 for (auto j = i; j != GPS_local_coords_on_vehicle.end(); ++j)
188 {
189 if (i != j)
190 {
191 const TPoint3D& pi = i->second;
192 const TPoint3D& pj = j->second;
193 Ad_ij[make_set(i->first, j->first)] = pi.distanceTo(pj);
194 phi_ij[make_set(i->first, j->first)] = atan2(pj.y - pi.y, pj.x - pi.x);
195 }
196 }
197 }
198 ASSERT_(D_cov_indexes.size() == 3 && D_cov_rev_indexes.size() == 3);
199
200 D_cov.setSize(D_cov_indexes.size(), D_cov_indexes.size());
201 D_mean.resize(D_cov_indexes.size());
202
203 // See paper for the formulas!
204 // TODO: generalize for N>3
205
206 D_cov(0, 0) = 2 * square(Ad_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[1])]);
207 D_cov(1, 1) = 2 * square(Ad_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[2])]);
208 D_cov(2, 2) = 2 * square(Ad_ij[make_set(D_cov_rev_indexes[1], D_cov_rev_indexes[2])]);
209
210 D_cov(1, 0) = Ad_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[1])] *
211 Ad_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[2])] *
212 cos(phi_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[1])] -
213 phi_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[2])]);
214 D_cov(0, 1) = D_cov(1, 0);
215
216 D_cov(2, 0) = -Ad_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[1])] *
217 Ad_ij[make_set(D_cov_rev_indexes[1], D_cov_rev_indexes[2])] *
218 cos(phi_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[1])] -
219 phi_ij[make_set(D_cov_rev_indexes[1], D_cov_rev_indexes[2])]);
220 D_cov(0, 2) = D_cov(2, 0);
221
222 D_cov(2, 1) = Ad_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[2])] *
223 Ad_ij[make_set(D_cov_rev_indexes[1], D_cov_rev_indexes[2])] *
224 cos(phi_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[2])] -
225 phi_ij[make_set(D_cov_rev_indexes[1], D_cov_rev_indexes[2])]);
226 D_cov(1, 2) = D_cov(2, 1);
227
228 D_cov *= 4 * square(std_0);
229
230 D_cov_1 = D_cov.inverse_LLt();
231
232 // cout << D_cov.inMatlabFormat() << endl;
233
234 D_mean[0] = square(Ad_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[1])]);
235 D_mean[1] = square(Ad_ij[make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[2])]);
236 D_mean[2] = square(Ad_ij[make_set(D_cov_rev_indexes[1], D_cov_rev_indexes[2])]);
237 }
238 else
239 doConsistencyCheck = false;
240
241 // ------------------------------------------
242 // Look for the 2 observations:
243 // ------------------------------------------
244 int N_GPSs = list_gps_obs.size();
245
246 if (N_GPSs)
247 {
248 // loop interpolate 1-out-of-5: this solves the issue with JAVAD GPSs
249 // that skip some readings at some times .0 .2 .4 .6 .8
250 if (list_gps_obs.size() > 4)
251 {
252 auto F = list_gps_obs.begin();
253 ++F;
254 ++F;
255 auto E = list_gps_obs.end();
256 --E;
257 --E;
258
259 for (auto it = F; it != E; ++it)
260 {
261 // Now check if we have 3 gps with the same time stamp:
262 // const size_t N = i->second.size();
263 std::map<std::string, CObservationGPS::Ptr>& GPS = it->second;
264
265 // Check if any in "lstGPSLabels" is missing here:
266 for (const auto& lstGPSLabel : lstGPSLabels)
267 {
268 // For each GPS in the current timestamp:
269 bool fnd = (GPS.find(lstGPSLabel) != GPS.end());
270
271 if (fnd) continue; // this one is present.
272
273 // Ok, we have "*l" missing in the set "*i".
274 // Try to interpolate from neighbors:
275 auto i_b1 = it;
276 --i_b1;
277 auto i_a1 = it;
278 ++i_a1;
279
280 CObservationGPS::Ptr GPS_b1, GPS_a1;
281
282 if (i_b1->second.find(lstGPSLabel) != i_b1->second.end())
283 GPS_b1 = i_b1->second.find(lstGPSLabel)->second;
284
285 if (i_a1->second.find(lstGPSLabel) != i_a1->second.end())
286 GPS_a1 = i_a1->second.find(lstGPSLabel)->second;
287
288 if (!disableGPSInterp && GPS_a1 && GPS_b1)
289 {
290 if (mrpt::system::timeDifference(GPS_b1->timestamp, GPS_a1->timestamp) < 0.5)
291 {
292 auto new_gps = CObservationGPS::Create(*GPS_a1);
293 new_gps->sensorLabel = lstGPSLabel;
294
295 // cout <<
296 // mrpt::system::timeLocalToString(GPS_b1->timestamp)
297 // << " " <<
298 // mrpt::system::timeLocalToString(GPS_a1->timestamp)
299 // << " " << *l;
300 // cout << endl;
301
302 new_gps->getMsgByClass<gnss::Message_NMEA_GGA>().fields.longitude_degrees =
303 0.5 * (GPS_a1->getMsgByClass<gnss::Message_NMEA_GGA>().fields.longitude_degrees +
304 GPS_b1->getMsgByClass<gnss::Message_NMEA_GGA>().fields.longitude_degrees);
305 new_gps->getMsgByClass<gnss::Message_NMEA_GGA>().fields.latitude_degrees =
306 0.5 * (GPS_a1->getMsgByClass<gnss::Message_NMEA_GGA>().fields.latitude_degrees +
307 GPS_b1->getMsgByClass<gnss::Message_NMEA_GGA>().fields.latitude_degrees);
308 new_gps->getMsgByClass<gnss::Message_NMEA_GGA>().fields.altitude_meters =
309 0.5 * (GPS_a1->getMsgByClass<gnss::Message_NMEA_GGA>().fields.altitude_meters +
310 GPS_b1->getMsgByClass<gnss::Message_NMEA_GGA>().fields.altitude_meters);
311
312 new_gps->timestamp = mrpt::Clock::time_point(mrpt::Clock::duration(
313 (GPS_a1->timestamp.time_since_epoch().count() +
314 GPS_b1->timestamp.time_since_epoch().count()) /
315 2));
316
317 it->second[new_gps->sensorLabel] = new_gps;
318 }
319 }
320 }
321 } // end loop interpolate 1-out-of-5
322 }
323
324 int idx_in_GPSs = 0;
325
326 for (auto i = list_gps_obs.begin(); i != list_gps_obs.end(); ++i, idx_in_GPSs++)
327 {
328 // Now check if we have 3 gps with the same time stamp:
329 if (i->second.size() >= 3)
330 {
331 const size_t N = i->second.size();
332 std::map<std::string, CObservationGPS::Ptr>& GPS = i->second;
333 CVectorDouble X(N), Y(N), Z(N); // Global XYZ coordinates
334 std::map<string, size_t> XYZidxs; // Sensor label -> indices in X Y Z
335
336 if (!ref_valid) // get the reference lat/lon, if it's not set
337 // from rawlog configuration block.
338 {
339 ref_valid = true;
340 ref = GPS.begin()
341 ->second->getMsgByClass<gnss::Message_NMEA_GGA>()
342 .getAsStruct<TGeodeticCoords>();
343 }
344
345 // Compute the XYZ coordinates of all sensors:
346 TMatchingPairList corrs;
347 unsigned int k;
348 std::map<std::string, CObservationGPS::Ptr>::iterator g_it;
349
350 for (k = 0, g_it = GPS.begin(); g_it != GPS.end(); ++g_it, ++k)
351 {
352 TPoint3D P;
353 mrpt::topography::geodeticToENU_WGS84(
354 g_it->second->getMsgByClass<gnss::Message_NMEA_GGA>().getAsStruct<TGeodeticCoords>(),
355 P, ref);
356
357 // Correction of offsets:
358 const string sect = string("OFFSET_") + g_it->second->sensorLabel;
359 P.x += memFil.read_double(sect, "x", 0);
360 P.y += memFil.read_double(sect, "y", 0);
361 P.z += memFil.read_double(sect, "z", 0);
362
363 XYZidxs[g_it->second->sensorLabel] = k; // Save index correspondence
364
365 // Create the correspondence:
366 corrs.push_back(TMatchingPair(
367 k, k, // Indices
368 // "This"/Global coords
369 d2f(P.x), d2f(P.y), d2f(P.z),
370 // "other"/local coordinates
371 d2f(g_it->second->sensorPose.x()), d2f(g_it->second->sensorPose.y()),
372 d2f(g_it->second->sensorPose.z())));
373
374 X[k] = P.x;
375 Y[k] = P.y;
376 Z[k] = P.z;
377 }
378
379 if (doConsistencyCheck && GPS.size() == 3)
380 {
381 // XYZ[k] have the k'd final coordinates of each GPS
382 // GPS[k] are the CObservations:
383
384 // Compute the inter-GPS square distances:
385 CVectorDouble iGPSdist2(3);
386
387 // [0]: sq dist between:
388 // D_cov_rev_indexes[0],D_cov_rev_indexes[1]
389 TPoint3D P0(
390 X[XYZidxs[D_cov_rev_indexes[0]]], Y[XYZidxs[D_cov_rev_indexes[0]]],
391 Z[XYZidxs[D_cov_rev_indexes[0]]]);
392 TPoint3D P1(
393 X[XYZidxs[D_cov_rev_indexes[1]]], Y[XYZidxs[D_cov_rev_indexes[1]]],
394 Z[XYZidxs[D_cov_rev_indexes[1]]]);
395 TPoint3D P2(
396 X[XYZidxs[D_cov_rev_indexes[2]]], Y[XYZidxs[D_cov_rev_indexes[2]]],
397 Z[XYZidxs[D_cov_rev_indexes[2]]]);
398
399 iGPSdist2[0] = P0.sqrDistanceTo(P1);
400 iGPSdist2[1] = P0.sqrDistanceTo(P2);
401 iGPSdist2[2] = P1.sqrDistanceTo(P2);
402
403 double mahaD = mrpt::math::mahalanobisDistance(iGPSdist2, D_mean, D_cov_1);
404 outInfoTemp.mahalabis_quality_measure[i->first] = mahaD;
405
406 // cout << "x: " << iGPSdist2 << " MU: " << D_mean << " ->
407 // " << mahaD << endl;
408 } // end consistency
409
410 // Use a 6D matching method to estimate the location of the
411 // vehicle:
412 CPose3DQuat optimal_pose;
413 double optimal_scale;
414
415 // "this" (reference map) -> GPS global coordinates
416 // "other" -> GPS local coordinates on the vehicle
417 mrpt::tfest::se3_l2(corrs, optimal_pose, optimal_scale,
418 true); // Force scale=1
419 // cout << "optimal pose: " << optimal_pose << " " <<
420 // optimal_scale << endl;
421 MRPT_CHECK_NORMAL_NUMBER(optimal_pose.x());
422 MRPT_CHECK_NORMAL_NUMBER(optimal_pose.y());
423 MRPT_CHECK_NORMAL_NUMBER(optimal_pose.z());
424 MRPT_CHECK_NORMAL_NUMBER(optimal_pose.quat().x());
425 MRPT_CHECK_NORMAL_NUMBER(optimal_pose.quat().y());
426 MRPT_CHECK_NORMAL_NUMBER(optimal_pose.quat().z());
427 MRPT_CHECK_NORMAL_NUMBER(optimal_pose.quat().r());
428
429 // Final vehicle pose:
430 const CPose3D veh_pose = CPose3D(optimal_pose);
431
432 // Add to the interpolator:
433 robot_path.insert(i->first, veh_pose);
434
435 // If we have W_star, compute the pose uncertainty:
436 if (doUncertaintyCovs)
437 {
438 CPose3DPDFGaussian final_veh_uncert;
439 final_veh_uncert.mean.setFromValues(0, 0, 0, 0, 0, 0);
440 final_veh_uncert.cov = outInfoTemp.W_star;
441
442 // Rotate the covariance according to the real vehicle pose:
443 final_veh_uncert.changeCoordinatesReference(veh_pose);
444
445 outInfoTemp.vehicle_uncertainty[i->first] = final_veh_uncert.cov;
446 }
447 }
448
449 } // end for i
450
451 if (PATH_SMOOTH_FILTER > 0 && robot_path.size() > 1)
452 {
453 CPose3DInterpolator filtered_robot_path = robot_path;
454
455 // Do Angles smoother filter of the path:
456 // ---------------------------------------------
457 const double MAX_DIST_TO_FILTER = 4.0;
458
459 for (auto i = robot_path.begin(); i != robot_path.end(); ++i)
460 {
461 mrpt::math::TPose3D p = i->second;
462
463 CVectorDouble pitchs, rolls; // The elements to average
464
465 pitchs.push_back(p.pitch);
466 rolls.push_back(p.roll);
467
468 auto q = i;
469 for (int k = 0; k < PATH_SMOOTH_FILTER && q != robot_path.begin(); k++)
470 {
471 --q;
472 if (std::abs(mrpt::system::timeDifference(q->first, i->first)) < MAX_DIST_TO_FILTER)
473 {
474 pitchs.push_back(q->second.pitch);
475 rolls.push_back(q->second.roll);
476 }
477 }
478 q = i;
479 for (int k = 0; k < PATH_SMOOTH_FILTER && q != (--robot_path.end()); k++)
480 {
481 ++q;
482 if (std::abs(mrpt::system::timeDifference(q->first, i->first)) < MAX_DIST_TO_FILTER)
483 {
484 pitchs.push_back(q->second.pitch);
485 rolls.push_back(q->second.roll);
486 }
487 }
488
489 p.pitch = mrpt::math::averageWrap2Pi(pitchs);
490 p.roll = mrpt::math::averageWrap2Pi(rolls);
491
492 // save in filtered path:
493 filtered_robot_path.insert(i->first, p);
494 }
495 // Replace:
496 robot_path = filtered_robot_path;
497
498 } // end PATH_SMOOTH_FILTER
499
500 } // end step generate 6D path
501
502 // Here we can set best_gps_path (that with the max. number of RTK
503 // fixed/foat readings):
504 if (outInfo)
505 {
506 string bestLabel;
507 size_t bestNum = 0;
508 for (auto& GPS_RTK_read : GPS_RTK_reads)
509 {
510 if (GPS_RTK_read.second > bestNum)
511 {
512 bestNum = GPS_RTK_read.second;
513 bestLabel = GPS_RTK_read.first;
514 }
515 }
516 outInfoTemp.best_gps_path = gps_paths[bestLabel];
517
518 // and transform to XYZ:
519 // Correction of offsets:
520 const string sect = string("OFFSET_") + bestLabel;
521 const double off_X = memFil.read_double(sect, "x", 0);
522 const double off_Y = memFil.read_double(sect, "y", 0);
523 const double off_Z = memFil.read_double(sect, "z", 0);
524
525 // map<TTimeStamp,TPoint3D> best_gps_path; // time -> 3D local
526 // coords
527 for (auto& i : outInfoTemp.best_gps_path)
528 {
529 TPoint3D P;
530 TPoint3D& pl = i.second;
531 mrpt::topography::geodeticToENU_WGS84(
532 TGeodeticCoords(
533 pl.x, pl.y,
534 pl.z), // i->second.x,i->second.y,i->second.z,
535 // // lat, lon, heigh
536 P, // X Y Z
537 ref);
538
539 pl.x = P.x + off_X;
540 pl.y = P.y + off_Y;
541 pl.z = P.z + off_Z;
542 }
543 } // end best_gps_path
544
545 if (outInfo) *outInfo = outInfoTemp;
546
547 MRPT_END
548 }