183 auto obs = std::make_shared<RtklibPosObs>();
189 line.erase(line.begin(), std::ranges::find_if(line, [](
int ch) { return std::isgraph(ch); }));
196 std::istringstream lineStream(line);
200 std::optional<uint16_t> year;
201 std::optional<uint16_t> month;
202 std::optional<uint16_t> day;
203 std::optional<int32_t> hour;
204 std::optional<uint16_t> minute;
205 std::optional<long double> second = 0L;
206 std::optional<uint16_t> gpsWeek;
207 std::optional<long double> gpsToW;
208 Eigen::Vector3d lla_position{ std::nan(
""), std::nan(
""), std::nan(
"") };
209 Eigen::Vector3d e_position{ std::nan(
""), std::nan(
""), std::nan(
"") };
210 Eigen::Vector3d n_velocity{ std::nan(
""), std::nan(
""), std::nan(
"") };
211 Eigen::Vector3d e_velocity{ std::nan(
""), std::nan(
""), std::nan(
"") };
213 Eigen::Matrix3d e_posVar = Eigen::Matrix3d::Zero() * std::nan(
"");
214 Eigen::Matrix3d e_velVar = Eigen::Matrix3d::Zero() * std::nan(
"");
215 Eigen::Matrix3d n_posVar = Eigen::Matrix3d::Zero() * std::nan(
"");
216 Eigen::Matrix3d n_velVar = Eigen::Matrix3d::Zero() * std::nan(
"");
222 if (lineStream >> cell)
225 cell.erase(std::ranges::find_if(cell, [](
int ch) {
return std::iscntrl(ch); }), cell.end());
233 if (column ==
"GpsWeek")
235 gpsWeek =
static_cast<uint16_t
>(std::stoul(cell));
237 else if (column ==
"GpsToW")
239 gpsToW = std::stold(cell);
245 else if (column.starts_with(
"Date"))
247 timeSystem = column.ends_with(
"-GPST") ?
GPST :
UTC;
252 year =
static_cast<uint16_t
>(std::stoi(ymd.at(0)));
253 month =
static_cast<uint16_t
>(std::stoi(ymd.at(1)));
254 day =
static_cast<uint16_t
>(std::stoi(ymd.at(2)));
257 else if (column.starts_with(
"Time"))
262 hour =
static_cast<uint16_t
>(std::stoi(hms.at(0)));
263 if (column.ends_with(
"-JST")) { *hour -= 9; }
264 minute =
static_cast<uint16_t
>(std::stoi(hms.at(1)));
265 second = std::stold(hms.at(2));
268 else if (column ==
"x-ecef(m)" || column ==
"x-ecef")
270 e_position.x() = std::stod(cell);
272 else if (column ==
"y-ecef(m)" || column ==
"y-ecef")
274 e_position.y() = std::stod(cell);
276 else if (column ==
"z-ecef(m)" || column ==
"z-ecef")
278 e_position.z() = std::stod(cell);
280 else if (column ==
"latitude(deg)" || column ==
"latitude")
282 lla_position(0) =
deg2rad(std::stod(cell));
284 else if (column ==
"longitude(deg)" || column ==
"longitude")
286 lla_position(1) =
deg2rad(std::stod(cell));
288 else if (column ==
"height(m)" || column ==
"height")
290 lla_position(2) = std::stod(cell);
292 else if (column ==
"Q")
294 obs->Q =
static_cast<uint8_t
>(std::stoul(cell));
296 else if (column ==
"ns")
298 obs->ns =
static_cast<uint8_t
>(std::stoul(cell));
300 else if (column ==
"sdx(m)" || column ==
"sdx")
302 e_posVar(0, 0) = std::pow(std::stod(cell), 2);
304 else if (column ==
"sdy(m)" || column ==
"sdy")
306 e_posVar(1, 1) = std::pow(std::stod(cell), 2);
308 else if (column ==
"sdz(m)" || column ==
"sdz")
310 e_posVar(2, 2) = std::pow(std::stod(cell), 2);
312 else if (column ==
"sdn(m)" || column ==
"sdn")
314 n_posVar(0, 0) = std::pow(std::stod(cell), 2);
316 else if (column ==
"sde(m)" || column ==
"sde")
318 n_posVar(1, 1) = std::pow(std::stod(cell), 2);
320 else if (column ==
"sdu(m)" || column ==
"sdu")
322 n_posVar(2, 2) = std::pow(std::stod(cell), 2);
324 else if (column ==
"sdxy(m)" || column ==
"sdxy")
326 e_posVar(0, 1) = std::stod(cell);
327 e_posVar(0, 1) = gcem::sgn(e_posVar(0, 1)) * std::pow(e_posVar(0, 1), 2);
328 e_posVar(1, 0) = -e_posVar(0, 1);
330 else if (column ==
"sdyz(m)" || column ==
"sdyz")
332 e_posVar(1, 2) = std::stod(cell);
333 e_posVar(1, 2) = gcem::sgn(e_posVar(1, 2)) * std::pow(e_posVar(1, 2), 2);
334 e_posVar(2, 1) = -e_posVar(1, 2);
336 else if (column ==
"sdzx(m)" || column ==
"sdzx")
338 e_posVar(2, 0) = std::stod(cell);
339 e_posVar(2, 0) = gcem::sgn(e_posVar(2, 0)) * std::pow(e_posVar(2, 0), 2);
340 e_posVar(0, 2) = -e_posVar(2, 0);
342 else if (column ==
"sdne(m)" || column ==
"sdne")
344 n_posVar(0, 1) = std::stod(cell);
345 n_posVar(0, 1) = gcem::sgn(n_posVar(0, 1)) * std::pow(n_posVar(0, 1), 2);
346 n_posVar(1, 0) = -n_posVar(0, 1);
348 else if (column ==
"sdeu(m)" || column ==
"sdeu")
350 n_posVar(1, 2) = std::stod(cell);
351 n_posVar(1, 2) = gcem::sgn(n_posVar(1, 2)) * std::pow(n_posVar(1, 2), 2);
352 n_posVar(2, 1) = -n_posVar(1, 2);
354 else if (column ==
"sdun(m)" || column ==
"sdun")
356 n_posVar(2, 0) = std::stod(cell);
357 n_posVar(2, 0) = gcem::sgn(n_posVar(2, 0)) * std::pow(n_posVar(2, 0), 2);
358 n_posVar(0, 2) = -n_posVar(2, 0);
360 else if (column ==
"age(s)" || column ==
"age")
362 obs->age = std::stod(cell);
364 else if (column ==
"ratio")
366 obs->ratio = std::stod(cell);
368 else if (column ==
"vn(m/s)" || column ==
"vn")
370 n_velocity(0) = std::stod(cell);
372 else if (column ==
"ve(m/s)" || column ==
"ve")
374 n_velocity(1) = std::stod(cell);
376 else if (column ==
"vu(m/s)" || column ==
"vu")
378 n_velocity(2) = -std::stod(cell);
380 else if (column ==
"vx(m/s)" || column ==
"vx")
382 e_velocity(0) = std::stod(cell);
384 else if (column ==
"vy(m/s)" || column ==
"vy")
386 e_velocity(1) = std::stod(cell);
388 else if (column ==
"vz(m/s)" || column ==
"vz")
390 e_velocity(2) = std::stod(cell);
392 else if (column ==
"sdvn(m/s)" || column ==
"sdvn")
394 n_velVar(0, 0) = std::pow(std::stod(cell), 2);
396 else if (column ==
"sdve(m/s)" || column ==
"sdve")
398 n_velVar(1, 1) = std::pow(std::stod(cell), 2);
400 else if (column ==
"sdvu(m/s)" || column ==
"sdvu")
402 n_velVar(2, 2) = std::pow(std::stod(cell), 2);
404 else if (column ==
"sdvne(m/s)" || column ==
"sdvne")
406 n_velVar(0, 1) = std::stod(cell);
407 n_velVar(0, 1) = gcem::sgn(n_velVar(0, 1)) * std::pow(n_velVar(0, 1), 2);
408 n_velVar(1, 0) = -n_velVar(0, 1);
410 else if (column ==
"sdveu(m/s)" || column ==
"sdveu")
412 n_velVar(1, 2) = std::stod(cell);
413 n_velVar(1, 2) = gcem::sgn(n_velVar(1, 2)) * std::pow(n_velVar(1, 2), 2);
414 n_velVar(2, 1) = -n_velVar(1, 2);
416 else if (column ==
"sdvun(m/s)" || column ==
"sdvun")
418 n_velVar(2, 0) = std::stod(cell);
419 n_velVar(2, 0) = gcem::sgn(n_velVar(2, 0)) * std::pow(n_velVar(2, 0), 2);
420 n_velVar(0, 2) = -n_velVar(2, 0);
422 else if (column ==
"sdvx(m/s)" || column ==
"sdvx")
424 e_velVar(0, 0) = std::pow(std::stod(cell), 2);
426 else if (column ==
"sdvy(m/s)" || column ==
"sdvy")
428 e_velVar(1, 1) = std::pow(std::stod(cell), 2);
430 else if (column ==
"sdvz(m/s)" || column ==
"sdvz")
432 e_velVar(2, 2) = std::pow(std::stod(cell), 2);
434 else if (column ==
"sdvxy(m/s)" || column ==
"sdvxy")
436 e_velVar(0, 1) = std::stod(cell);
437 e_velVar(0, 1) = gcem::sgn(e_velVar(0, 1)) * std::pow(e_velVar(0, 1), 2);
438 e_velVar(1, 0) = -e_velVar(0, 1);
440 else if (column ==
"sdvyz(m/s)" || column ==
"sdvyz")
442 e_velVar(1, 2) = std::stod(cell);
443 e_velVar(1, 2) = gcem::sgn(e_velVar(1, 2)) * std::pow(e_velVar(1, 2), 2);
444 e_velVar(2, 1) = -e_velVar(1, 2);
446 else if (column ==
"sdvzx(m/s)" || column ==
"sdvzx")
448 e_velVar(2, 0) = std::stod(cell);
449 e_velVar(2, 0) = gcem::sgn(e_velVar(2, 0)) * std::pow(e_velVar(2, 0), 2);
450 e_velVar(0, 2) = -e_velVar(2, 0);
460 if (gpsWeek.has_value() && gpsToW.has_value())
462 obs->insTime =
InsTime(0, gpsWeek.value(), gpsToW.value());
464 else if (year.has_value() && month.has_value() && day.has_value()
465 && hour.has_value() && minute.has_value() && second.has_value())
467 obs->insTime =
InsTime(year.value(), month.value(), day.value(),
468 hour.value(), minute.value(), second.value(),
472 if (!e_position.hasNaN()) { obs->setPosition_e(e_position); }
473 else if (!lla_position.hasNaN()) { obs->setPosition_lla(lla_position); }
475 if (!e_velocity.hasNaN()) { obs->setVelocity_e(e_velocity); }
476 else if (!n_velocity.hasNaN()) { obs->setVelocity_n(n_velocity); }
478 if (!e_velVar.hasNaN() && !e_posVar.hasNaN())
481 cov.block<3, 3>(0, 0) = e_posVar;
482 cov.block<3, 3>(3, 3) = e_velVar;
483 obs->setPosVelCovarianceMatrix_e(cov);
485 else if (!n_velVar.hasNaN() && !n_posVar.hasNaN())
488 cov.block<3, 3>(0, 0) = n_posVar;
489 cov.block<3, 3>(3, 3) = n_velVar;
490 obs->setPosVelCovarianceMatrix_n(cov);
492 else if (!e_posVar.hasNaN())
494 obs->setPosCovarianceMatrix_e(e_posVar);
496 else if (!n_posVar.hasNaN())
498 obs->setPosCovarianceMatrix_n(n_posVar);