181 auto obs = std::make_shared<RtklibPosObs>();
187 line.erase(line.begin(), std::ranges::find_if(line, [](
int ch) { return std::isgraph(ch); }));
194 std::istringstream lineStream(line);
198 std::optional<uint16_t> year;
199 std::optional<uint16_t> month;
200 std::optional<uint16_t> day;
201 std::optional<int32_t> hour;
202 std::optional<uint16_t> minute;
203 std::optional<long double> second = 0L;
204 std::optional<uint16_t> gpsWeek;
205 std::optional<long double> gpsToW;
206 Eigen::Vector3d lla_position{ std::nan(
""), std::nan(
""), std::nan(
"") };
207 Eigen::Vector3d e_position{ std::nan(
""), std::nan(
""), std::nan(
"") };
208 Eigen::Vector3d n_velocity{ std::nan(
""), std::nan(
""), std::nan(
"") };
209 Eigen::Vector3d e_velocity{ std::nan(
""), std::nan(
""), std::nan(
"") };
211 Eigen::Matrix3d e_posVar = Eigen::Matrix3d::Zero() * std::nan(
"");
212 Eigen::Matrix3d e_velVar = Eigen::Matrix3d::Zero() * std::nan(
"");
213 Eigen::Matrix3d n_posVar = Eigen::Matrix3d::Zero() * std::nan(
"");
214 Eigen::Matrix3d n_velVar = Eigen::Matrix3d::Zero() * std::nan(
"");
220 if (lineStream >> cell)
223 cell.erase(std::ranges::find_if(cell, [](
int ch) {
return std::iscntrl(ch); }), cell.end());
231 if (column ==
"GpsWeek")
233 gpsWeek =
static_cast<uint16_t
>(std::stoul(cell));
235 else if (column ==
"GpsToW")
237 gpsToW = std::stold(cell);
243 else if (column.starts_with(
"Date"))
245 timeSystem = column.ends_with(
"-GPST") ?
GPST :
UTC;
250 year =
static_cast<uint16_t
>(std::stoi(ymd.at(0)));
251 month =
static_cast<uint16_t
>(std::stoi(ymd.at(1)));
252 day =
static_cast<uint16_t
>(std::stoi(ymd.at(2)));
255 else if (column.starts_with(
"Time"))
260 hour =
static_cast<uint16_t
>(std::stoi(hms.at(0)));
261 if (column.ends_with(
"-JST")) { *hour -= 9; }
262 minute =
static_cast<uint16_t
>(std::stoi(hms.at(1)));
263 second = std::stold(hms.at(2));
266 else if (column ==
"x-ecef(m)" || column ==
"x-ecef")
268 e_position.x() = std::stod(cell);
270 else if (column ==
"y-ecef(m)" || column ==
"y-ecef")
272 e_position.y() = std::stod(cell);
274 else if (column ==
"z-ecef(m)" || column ==
"z-ecef")
276 e_position.z() = std::stod(cell);
278 else if (column ==
"latitude(deg)" || column ==
"latitude")
280 lla_position(0) =
deg2rad(std::stod(cell));
282 else if (column ==
"longitude(deg)" || column ==
"longitude")
284 lla_position(1) =
deg2rad(std::stod(cell));
286 else if (column ==
"height(m)" || column ==
"height")
288 lla_position(2) = std::stod(cell);
290 else if (column ==
"Q")
292 obs->Q =
static_cast<uint8_t
>(std::stoul(cell));
294 else if (column ==
"ns")
296 obs->ns =
static_cast<uint8_t
>(std::stoul(cell));
298 else if (column ==
"sdx(m)" || column ==
"sdx")
300 e_posVar(0, 0) = std::pow(std::stod(cell), 2);
302 else if (column ==
"sdy(m)" || column ==
"sdy")
304 e_posVar(1, 1) = std::pow(std::stod(cell), 2);
306 else if (column ==
"sdz(m)" || column ==
"sdz")
308 e_posVar(2, 2) = std::pow(std::stod(cell), 2);
310 else if (column ==
"sdn(m)" || column ==
"sdn")
312 n_posVar(0, 0) = std::pow(std::stod(cell), 2);
314 else if (column ==
"sde(m)" || column ==
"sde")
316 n_posVar(1, 1) = std::pow(std::stod(cell), 2);
318 else if (column ==
"sdu(m)" || column ==
"sdu")
320 n_posVar(2, 2) = std::pow(std::stod(cell), 2);
322 else if (column ==
"sdxy(m)" || column ==
"sdxy")
324 e_posVar(0, 1) = std::stod(cell);
325 e_posVar(0, 1) = gcem::sgn(e_posVar(0, 1)) * std::pow(e_posVar(0, 1), 2);
326 e_posVar(1, 0) = -e_posVar(0, 1);
328 else if (column ==
"sdyz(m)" || column ==
"sdyz")
330 e_posVar(1, 2) = std::stod(cell);
331 e_posVar(1, 2) = gcem::sgn(e_posVar(1, 2)) * std::pow(e_posVar(1, 2), 2);
332 e_posVar(2, 1) = -e_posVar(1, 2);
334 else if (column ==
"sdzx(m)" || column ==
"sdzx")
336 e_posVar(2, 0) = std::stod(cell);
337 e_posVar(2, 0) = gcem::sgn(e_posVar(2, 0)) * std::pow(e_posVar(2, 0), 2);
338 e_posVar(0, 2) = -e_posVar(2, 0);
340 else if (column ==
"sdne(m)" || column ==
"sdne")
342 n_posVar(0, 1) = std::stod(cell);
343 n_posVar(0, 1) = gcem::sgn(n_posVar(0, 1)) * std::pow(n_posVar(0, 1), 2);
344 n_posVar(1, 0) = -n_posVar(0, 1);
346 else if (column ==
"sdeu(m)" || column ==
"sdeu")
348 n_posVar(1, 2) = std::stod(cell);
349 n_posVar(1, 2) = gcem::sgn(n_posVar(1, 2)) * std::pow(n_posVar(1, 2), 2);
350 n_posVar(2, 1) = -n_posVar(1, 2);
352 else if (column ==
"sdun(m)" || column ==
"sdun")
354 n_posVar(2, 0) = std::stod(cell);
355 n_posVar(2, 0) = gcem::sgn(n_posVar(2, 0)) * std::pow(n_posVar(2, 0), 2);
356 n_posVar(0, 2) = -n_posVar(2, 0);
358 else if (column ==
"age(s)" || column ==
"age")
360 obs->age = std::stod(cell);
362 else if (column ==
"ratio")
364 obs->ratio = std::stod(cell);
366 else if (column ==
"vn(m/s)" || column ==
"vn")
368 n_velocity(0) = std::stod(cell);
370 else if (column ==
"ve(m/s)" || column ==
"ve")
372 n_velocity(1) = std::stod(cell);
374 else if (column ==
"vu(m/s)" || column ==
"vu")
376 n_velocity(2) = -std::stod(cell);
378 else if (column ==
"vx(m/s)" || column ==
"vx")
380 e_velocity(0) = std::stod(cell);
382 else if (column ==
"vy(m/s)" || column ==
"vy")
384 e_velocity(1) = std::stod(cell);
386 else if (column ==
"vz(m/s)" || column ==
"vz")
388 e_velocity(2) = std::stod(cell);
390 else if (column ==
"sdvn(m/s)" || column ==
"sdvn")
392 n_velVar(0, 0) = std::pow(std::stod(cell), 2);
394 else if (column ==
"sdve(m/s)" || column ==
"sdve")
396 n_velVar(1, 1) = std::pow(std::stod(cell), 2);
398 else if (column ==
"sdvu(m/s)" || column ==
"sdvu")
400 n_velVar(2, 2) = std::pow(std::stod(cell), 2);
402 else if (column ==
"sdvne(m/s)" || column ==
"sdvne")
404 n_velVar(0, 1) = std::stod(cell);
405 n_velVar(0, 1) = gcem::sgn(n_velVar(0, 1)) * std::pow(n_velVar(0, 1), 2);
406 n_velVar(1, 0) = -n_velVar(0, 1);
408 else if (column ==
"sdveu(m/s)" || column ==
"sdveu")
410 n_velVar(1, 2) = std::stod(cell);
411 n_velVar(1, 2) = gcem::sgn(n_velVar(1, 2)) * std::pow(n_velVar(1, 2), 2);
412 n_velVar(2, 1) = -n_velVar(1, 2);
414 else if (column ==
"sdvun(m/s)" || column ==
"sdvun")
416 n_velVar(2, 0) = std::stod(cell);
417 n_velVar(2, 0) = gcem::sgn(n_velVar(2, 0)) * std::pow(n_velVar(2, 0), 2);
418 n_velVar(0, 2) = -n_velVar(2, 0);
420 else if (column ==
"sdvx(m/s)" || column ==
"sdvx")
422 e_velVar(0, 0) = std::pow(std::stod(cell), 2);
424 else if (column ==
"sdvy(m/s)" || column ==
"sdvy")
426 e_velVar(1, 1) = std::pow(std::stod(cell), 2);
428 else if (column ==
"sdvz(m/s)" || column ==
"sdvz")
430 e_velVar(2, 2) = std::pow(std::stod(cell), 2);
432 else if (column ==
"sdvxy(m/s)" || column ==
"sdvxy")
434 e_velVar(0, 1) = std::stod(cell);
435 e_velVar(0, 1) = gcem::sgn(e_velVar(0, 1)) * std::pow(e_velVar(0, 1), 2);
436 e_velVar(1, 0) = -e_velVar(0, 1);
438 else if (column ==
"sdvyz(m/s)" || column ==
"sdvyz")
440 e_velVar(1, 2) = std::stod(cell);
441 e_velVar(1, 2) = gcem::sgn(e_velVar(1, 2)) * std::pow(e_velVar(1, 2), 2);
442 e_velVar(2, 1) = -e_velVar(1, 2);
444 else if (column ==
"sdvzx(m/s)" || column ==
"sdvzx")
446 e_velVar(2, 0) = std::stod(cell);
447 e_velVar(2, 0) = gcem::sgn(e_velVar(2, 0)) * std::pow(e_velVar(2, 0), 2);
448 e_velVar(0, 2) = -e_velVar(2, 0);
458 if (gpsWeek.has_value() && gpsToW.has_value())
460 obs->insTime =
InsTime(0, gpsWeek.value(), gpsToW.value());
462 else if (year.has_value() && month.has_value() && day.has_value()
463 && hour.has_value() && minute.has_value() && second.has_value())
465 obs->insTime =
InsTime(year.value(), month.value(), day.value(),
466 hour.value(), minute.value(), second.value(),
470 if (!e_position.hasNaN()) { obs->setPosition_e(e_position); }
471 else if (!lla_position.hasNaN()) { obs->setPosition_lla(lla_position); }
473 if (!e_velocity.hasNaN()) { obs->setVelocity_e(e_velocity); }
474 else if (!n_velocity.hasNaN()) { obs->setVelocity_n(n_velocity); }
476 if (!e_velVar.hasNaN() && !e_posVar.hasNaN())
479 cov.block<3, 3>(0, 0) = e_posVar;
480 cov.block<3, 3>(3, 3) = e_velVar;
481 obs->setPosVelCovarianceMatrix_e(cov);
483 else if (!n_velVar.hasNaN() && !n_posVar.hasNaN())
486 cov.block<3, 3>(0, 0) = n_posVar;
487 cov.block<3, 3>(3, 3) = n_velVar;
488 obs->setPosVelCovarianceMatrix_n(cov);
490 else if (!e_posVar.hasNaN())
492 obs->setPosCovarianceMatrix_e(e_posVar);
494 else if (!n_posVar.hasNaN())
496 obs->setPosCovarianceMatrix_n(n_posVar);