| 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 |
} |