54 set<string> lstGPSLabels;
62 if (outInfo) *outInfo = outInfoTemp;
64 map<string, map<Clock::time_point, TPoint3D>>
68 bool ref_valid =
false;
82 const double std_0 = memFil.
read_double(
"CONSISTENCY_TEST",
"std_0", 0);
83 bool doConsistencyCheck = std_0 > 0;
87 const bool doUncertaintyCovs = outInfoTemp.
W_star.
rows() != 0;
88 if (doUncertaintyCovs &&
91 "ERROR: W_star matrix for uncertainty estimation is provided but "
92 "it's not a 6x6 matrix.");
99 using TListGPSs = std::map<
101 TListGPSs list_gps_obs;
103 map<string, size_t> GPS_RTK_reads;
104 map<string, TPoint3D>
105 GPS_local_coords_on_vehicle;
107 for (
size_t i = first; !abort && i <= last; i++)
114 case CRawlog::etObservation:
121 std::dynamic_pointer_cast<CObservationGPS>(o);
123 if (obs->has_GGA_datum() &&
128 list_gps_obs[obs->timestamp][obs->sensorLabel] = obs;
130 lstGPSLabels.insert(obs->sensorLabel);
134 if (obs->has_GGA_datum() &&
140 GPS_RTK_reads[obs->sensorLabel]++;
144 if (GPS_local_coords_on_vehicle.find(
146 GPS_local_coords_on_vehicle.end())
147 GPS_local_coords_on_vehicle[obs->sensorLabel] =
148 TPoint3D(obs->sensorPose.asTPose());
154 gps_paths[obs->sensorLabel][obs->timestamp] =
TPoint3D(
175 map<set<string>,
double> Ad_ij;
176 map<set<string>,
double>
180 map<size_t, string> D_cov_rev_indexes;
186 if (doConsistencyCheck && GPS_local_coords_on_vehicle.size() == 3)
188 unsigned int cnt = 0;
189 for (
auto i = GPS_local_coords_on_vehicle.begin();
190 i != GPS_local_coords_on_vehicle.end(); ++i)
193 D_cov_indexes[i->first] = cnt;
194 D_cov_rev_indexes[cnt] = i->first;
197 for (
auto j = i; j != GPS_local_coords_on_vehicle.end(); ++j)
204 phi_ij[
make_set(i->first, j->first)] =
205 atan2(pj.
y - pi.
y, pj.
x - pi.
x);
209 ASSERT_(D_cov_indexes.size() == 3 && D_cov_rev_indexes.size() == 3);
211 D_cov.
setSize(D_cov_indexes.size(), D_cov_indexes.size());
212 D_mean.
resize(D_cov_indexes.size());
219 square(Ad_ij[
make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[1])]);
222 square(Ad_ij[
make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[2])]);
225 square(Ad_ij[
make_set(D_cov_rev_indexes[1], D_cov_rev_indexes[2])]);
228 Ad_ij[
make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[1])] *
229 Ad_ij[
make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[2])] *
230 cos(phi_ij[
make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[1])] -
231 phi_ij[
make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[2])]);
232 D_cov(0, 1) = D_cov(1, 0);
235 -Ad_ij[
make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[1])] *
236 Ad_ij[
make_set(D_cov_rev_indexes[1], D_cov_rev_indexes[2])] *
237 cos(phi_ij[
make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[1])] -
238 phi_ij[
make_set(D_cov_rev_indexes[1], D_cov_rev_indexes[2])]);
239 D_cov(0, 2) = D_cov(2, 0);
242 Ad_ij[
make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[2])] *
243 Ad_ij[
make_set(D_cov_rev_indexes[1], D_cov_rev_indexes[2])] *
244 cos(phi_ij[
make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[2])] -
245 phi_ij[
make_set(D_cov_rev_indexes[1], D_cov_rev_indexes[2])]);
246 D_cov(1, 2) = D_cov(2, 1);
248 D_cov *= 4 *
square(std_0);
255 square(Ad_ij[
make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[1])]);
257 square(Ad_ij[
make_set(D_cov_rev_indexes[0], D_cov_rev_indexes[2])]);
259 square(Ad_ij[
make_set(D_cov_rev_indexes[1], D_cov_rev_indexes[2])]);
262 doConsistencyCheck =
false;
267 int N_GPSs = list_gps_obs.size();
273 if (list_gps_obs.size() > 4)
275 auto F = list_gps_obs.begin();
278 auto E = list_gps_obs.end();
282 for (
auto it = F; it != E; ++it)
286 std::map<std::string, CObservationGPS::Ptr>& GPS = it->second;
289 for (
const auto& lstGPSLabel : lstGPSLabels)
292 bool fnd = (GPS.find(lstGPSLabel) != GPS.end());
305 if (i_b1->second.find(lstGPSLabel) != i_b1->second.end())
306 GPS_b1 = i_b1->second.find(lstGPSLabel)->second;
308 if (i_a1->second.find(lstGPSLabel) != i_a1->second.end())
309 GPS_a1 = i_a1->second.find(lstGPSLabel)->second;
311 if (!disableGPSInterp && GPS_a1 && GPS_b1)
314 GPS_b1->timestamp, GPS_a1->timestamp) < 0.5)
316 auto new_gps = CObservationGPS::Create(*GPS_a1);
317 new_gps->sensorLabel = lstGPSLabel;
327 .fields.longitude_degrees =
332 .fields.longitude_degrees);
334 .fields.latitude_degrees =
339 .fields.latitude_degrees);
341 .fields.altitude_meters =
346 .fields.altitude_meters);
350 (GPS_a1->timestamp.time_since_epoch()
352 GPS_b1->timestamp.time_since_epoch()
356 it->second[new_gps->sensorLabel] = new_gps;
365 for (
auto i = list_gps_obs.begin(); i != list_gps_obs.end();
369 if (i->second.size() >= 3)
371 const size_t N = i->second.size();
372 std::map<std::string, CObservationGPS::Ptr>& GPS = i->second;
374 std::map<string, size_t>
383 .getAsStruct<TGeodeticCoords>();
389 std::map<std::string, CObservationGPS::Ptr>::iterator g_it;
391 for (k = 0, g_it = GPS.begin(); g_it != GPS.end(); ++g_it, ++k)
401 string(
"OFFSET_") + g_it->second->sensorLabel;
406 XYZidxs[g_it->second->sensorLabel] =
415 d2f(g_it->second->sensorPose.x()),
416 d2f(g_it->second->sensorPose.y()),
417 d2f(g_it->second->sensorPose.z())));
424 if (doConsistencyCheck && GPS.size() == 3)
435 X[XYZidxs[D_cov_rev_indexes[0]]],
436 Y[XYZidxs[D_cov_rev_indexes[0]]],
437 Z[XYZidxs[D_cov_rev_indexes[0]]]);
439 X[XYZidxs[D_cov_rev_indexes[1]]],
440 Y[XYZidxs[D_cov_rev_indexes[1]]],
441 Z[XYZidxs[D_cov_rev_indexes[1]]]);
443 X[XYZidxs[D_cov_rev_indexes[2]]],
444 Y[XYZidxs[D_cov_rev_indexes[2]]],
445 Z[XYZidxs[D_cov_rev_indexes[2]]]);
452 iGPSdist2, D_mean, D_cov_1);
462 double optimal_scale;
467 corrs, optimal_pose, optimal_scale,
true);
482 robot_path.
insert(i->first, veh_pose);
485 if (doUncertaintyCovs)
489 final_veh_uncert.
cov = outInfoTemp.
W_star;
495 final_veh_uncert.
cov;
501 if (PATH_SMOOTH_FILTER > 0 && robot_path.
size() > 1)
507 const double MAX_DIST_TO_FILTER = 4.0;
509 for (
auto i = robot_path.
begin(); i != robot_path.
end(); ++i)
520 k < PATH_SMOOTH_FILTER && q != robot_path.
begin(); k++)
524 q->first, i->first)) < MAX_DIST_TO_FILTER)
532 k < PATH_SMOOTH_FILTER && q != (--robot_path.
end()); k++)
536 q->first, i->first)) < MAX_DIST_TO_FILTER)
547 filtered_robot_path.
insert(i->first, p);
550 robot_path = filtered_robot_path;
562 for (
auto& GPS_RTK_read : GPS_RTK_reads)
564 if (GPS_RTK_read.second > bestNum)
566 bestNum = GPS_RTK_read.second;
567 bestLabel = GPS_RTK_read.first;
574 const string sect = string(
"OFFSET_") + bestLabel;
598 if (outInfo) *outInfo = outInfoTemp;