78 lines
2.8 KiB
C++
78 lines
2.8 KiB
C++
#include "utils/TransformPath.h"
|
||
|
||
#include <QFile>
|
||
#include <QTextStream>
|
||
#include <QRegExp>
|
||
|
||
#include "common/SpdLogger.h"
|
||
|
||
|
||
TransformPath::TransformPath(QObject* parent /*= nullptr*/)
|
||
: QObject(parent) {
|
||
|
||
}
|
||
|
||
TransformPath::TransformPath(std::vector<Transform> transforms, QObject* parent /*= nullptr*/)
|
||
: QObject(parent)
|
||
, transforms_(std::move(transforms)){
|
||
|
||
}
|
||
|
||
TransformPath::~TransformPath() {
|
||
|
||
}
|
||
|
||
TransformPath* TransformPath::LoadFromFile(const QString& path, QObject* parent) {
|
||
LOG_INFO("Loading transform path from file: {}", path.toLocal8Bit().constData());
|
||
QFile file(path);
|
||
|
||
if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
|
||
LOG_WARN("Cannot open file for reading: path:{} error:{}",
|
||
path.toLocal8Bit().constData(), file.errorString().toLocal8Bit().constData());
|
||
return nullptr;
|
||
}
|
||
|
||
QTextStream in(&file);
|
||
|
||
std::vector<Transform> transforms;
|
||
|
||
std::vector<double> times;
|
||
|
||
while (!in.atEnd()) {
|
||
QString line = in.readLine();
|
||
if (line.trimmed().isEmpty()) {
|
||
continue;
|
||
}
|
||
|
||
// 期望格式：时间 经度 纬度 高度 方位角(Heading) 俯仰角(Pitch) 翻滚角(Roll)
|
||
// 示例：0.700000 \t 119.500000 \t 24.500000 \t 1000.000000 \t 0.000000 \t 0.000000 \t 0.000000
|
||
const QStringList parts = line.split(QRegExp("\\s+"), Qt::SkipEmptyParts);
|
||
if (parts.size() < 7) {
|
||
LOG_WARN("TransformPath: invalid line (need 7 numbers): {}", line.toStdString());
|
||
return {};
|
||
}
|
||
|
||
bool ok = false;
|
||
const double time = parts[0].toDouble(&ok); if (!ok) { LOG_WARN("invalid time: {}", parts[0].toStdString()); return {}; }
|
||
const double longitude= parts[1].toDouble(&ok); if (!ok) { LOG_WARN("invalid longitude: {}", parts[1].toStdString()); return {}; }
|
||
const double latitude = parts[2].toDouble(&ok); if (!ok) { LOG_WARN("invalid latitude: {}", parts[2].toStdString()); return {}; }
|
||
const double altitude = parts[3].toDouble(&ok); if (!ok) { LOG_WARN("invalid altitude: {}", parts[3].toStdString()); return {}; }
|
||
const double heading = parts[4].toDouble(&ok); if (!ok) { LOG_WARN("invalid heading: {}", parts[4].toStdString()); return {}; }
|
||
const double pitch = parts[5].toDouble(&ok); if (!ok) { LOG_WARN("invalid pitch: {}", parts[5].toStdString()); return {}; }
|
||
const double roll = parts[6].toDouble(&ok); if (!ok) { LOG_WARN("invalid roll: {}", parts[6].toStdString()); return {}; }
|
||
|
||
Transform transform;
|
||
transform.GetLocation().set(longitude, latitude, altitude);
|
||
transform.GetRotation().set(pitch, roll, heading);
|
||
|
||
times.push_back(time);
|
||
|
||
transforms.push_back(transform);
|
||
}
|
||
file.close();
|
||
|
||
TransformPath* tp = new TransformPath(std::move(transforms), parent);
|
||
tp->SetTimes(std::move(times));
|
||
return tp;
|
||
}
|