243 lines
9.6 KiB
C++
243 lines
9.6 KiB
C++
#include "TrajectoryCalculator.h"
|
||
#include <algorithm>
|
||
#include <cmath>
|
||
|
||
namespace {
|
||
|
||
double planarDistance(const QVector3D &a, const QVector3D &b)
|
||
{
|
||
const double dx = static_cast<double>(a.x()) - b.x();
|
||
const double dy = static_cast<double>(a.y()) - b.y();
|
||
return std::sqrt(dx * dx + dy * dy);
|
||
}
|
||
|
||
QVector3D lerpPoint(const QVector3D &a, const QVector3D &b, double t)
|
||
{
|
||
return a * static_cast<float>(1.0 - t) + b * static_cast<float>(t);
|
||
}
|
||
|
||
QVector3D catmullRom(const QVector3D &p0, const QVector3D &p1, const QVector3D &p2, const QVector3D &p3, double t)
|
||
{
|
||
const float tf = static_cast<float>(t);
|
||
const float t2 = tf * tf;
|
||
const float t3 = t2 * tf;
|
||
return 0.5f * ((2.0f * p1) + (-p0 + p2) * tf + (2.0f * p0 - 5.0f * p1 + 4.0f * p2 - p3) * t2 +
|
||
(-p0 + 3.0f * p1 - 3.0f * p2 + p3) * t3);
|
||
}
|
||
|
||
} // namespace
|
||
|
||
double TrajectoryCalculator::headingAt(int traceIdx, const QVector<QVector3D> &gpsPositions)
|
||
{
|
||
const int n = gpsPositions.size();
|
||
if (n <= 1) return 0.0;
|
||
|
||
if (traceIdx < n - 1) {
|
||
double deltaX = gpsPositions[traceIdx + 1].x() - gpsPositions[traceIdx].x(); // 北向变化
|
||
double deltaY = gpsPositions[traceIdx + 1].y() - gpsPositions[traceIdx].y(); // 东向变化
|
||
return std::atan2(deltaY, deltaX);
|
||
} else {
|
||
// 最后一点复用前一点方向
|
||
double deltaX = gpsPositions[traceIdx].x() - gpsPositions[traceIdx - 1].x();
|
||
double deltaY = gpsPositions[traceIdx].y() - gpsPositions[traceIdx - 1].y();
|
||
return std::atan2(deltaY, deltaX);
|
||
}
|
||
}
|
||
|
||
bool TrajectoryCalculator::computeTrajectories(SurveyLine &line, const SurveyGeometry &geom)
|
||
{
|
||
const QVector<QVector3D> &gps = line.gpsPositions;
|
||
const int nRtk = gps.size();
|
||
if (nRtk <= 0) return false;
|
||
|
||
const int chCount = geom.channelCount;
|
||
if (chCount <= 0) return false;
|
||
|
||
// 通道相对x偏移优先使用头文件CH_X_OFFSETS推导出的逐通道数组；旧工程无数组时按首通道对称补齐。
|
||
QVector<double> chXRel = geom.channelXRel;
|
||
if (chXRel.size() != chCount) {
|
||
chXRel.resize(chCount);
|
||
const double lastXRel = -geom.ch1XRel;
|
||
for (int c = 0; c < chCount; ++c) {
|
||
chXRel[c] = (chCount > 1)
|
||
? geom.ch1XRel + (lastXRel - geom.ch1XRel) * c / (chCount - 1.0)
|
||
: geom.ch1XRel;
|
||
}
|
||
}
|
||
|
||
// 预分配 channelTrajectories
|
||
line.channelTrajectories.resize(chCount);
|
||
for (int c = 0; c < chCount; ++c) {
|
||
line.channelTrajectories[c].resize(nRtk);
|
||
}
|
||
|
||
// 遍历每个 RTK 点
|
||
for (int i = 0; i < nRtk; ++i) {
|
||
double thetaY = headingAt(i, gps);
|
||
double thetaX = thetaY + M_PI / 2.0;
|
||
|
||
// 旋转矩阵 R = [cos(theta_x), cos(theta_y); sin(theta_x), sin(theta_y)]
|
||
// 作用：将设备相对坐标系（x=右, y=前）转换为绝对坐标系（X=北, Y=东）
|
||
double r11 = std::cos(thetaX);
|
||
double r12 = std::cos(thetaY);
|
||
double r21 = std::sin(thetaX);
|
||
double r22 = std::sin(thetaY);
|
||
|
||
// RTK 相对天线中心的偏移向量（设备坐标系）
|
||
double rtkLocalX = geom.rtkOffsetX; // 横向偏移
|
||
double rtkLocalY = geom.rtkOffsetY; // 纵向偏移（前方为正）
|
||
|
||
// 转换为绝对坐标系偏移
|
||
double rtkAbsOffsetX = r11 * rtkLocalX + r12 * rtkLocalY;
|
||
double rtkAbsOffsetY = r21 * rtkLocalX + r22 * rtkLocalY;
|
||
|
||
// 天线中心绝对坐标 = RTK - 偏移量
|
||
double antX = gps[i].x() - rtkAbsOffsetX;
|
||
double antY = gps[i].y() - rtkAbsOffsetY;
|
||
double antZ = gps[i].z();
|
||
|
||
// 遍历每个通道
|
||
for (int c = 0; c < chCount; ++c) {
|
||
// 通道相对坐标（设备坐标系）
|
||
double chLocalX = chXRel[c];
|
||
double chLocalY = 0.0; // MATLAB 中 ch_y_rel = zeros(...)
|
||
|
||
// 转换为绝对坐标系偏移
|
||
double chAbsOffsetX = r11 * chLocalX + r12 * chLocalY;
|
||
double chAbsOffsetY = r21 * chLocalX + r22 * chLocalY;
|
||
|
||
// 通道绝对坐标
|
||
double chX = antX + chAbsOffsetX;
|
||
double chY = antY + chAbsOffsetY;
|
||
double chZ = antZ;
|
||
|
||
line.channelTrajectories[c][i] = QVector3D(static_cast<float>(chX),
|
||
static_cast<float>(chY),
|
||
static_cast<float>(chZ));
|
||
}
|
||
}
|
||
|
||
// 同步更新 GPRDataModel 中 traces 的 position
|
||
// 数据存储顺序：trace0_ch0, trace0_ch1, ... trace0_chN, trace1_ch0, ...
|
||
// 即 globalIdx = traceInChannel * chCount + channel
|
||
const int tracesPerChannel = line.data.getTracesPerChannel();
|
||
for (int c = 0; c < chCount; ++c) {
|
||
for (int t = 0; t < tracesPerChannel && t < nRtk; ++t) {
|
||
int globalIdx = t * chCount + c;
|
||
if (globalIdx >= 0 && globalIdx < line.data.traces.size()) {
|
||
line.data.traces[globalIdx].position = line.channelTrajectories[c][t];
|
||
}
|
||
}
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
QVector<QVector3D> TrajectoryCalculator::resampleTrajectoryLinear(const QVector<QVector3D> &input, int targetCount)
|
||
{
|
||
QVector<QVector3D> output;
|
||
if (targetCount <= 0 || input.isEmpty()) return output;
|
||
if (input.size() == 1 || targetCount == 1) {
|
||
output.resize(targetCount);
|
||
std::fill(output.begin(), output.end(), input.first());
|
||
return output;
|
||
}
|
||
|
||
output.reserve(targetCount);
|
||
const double scale = static_cast<double>(input.size() - 1) / qMax(1, targetCount - 1);
|
||
for (int i = 0; i < targetCount; ++i) {
|
||
const double src = i * scale;
|
||
const int i0 = qBound(0, static_cast<int>(std::floor(src)), input.size() - 1);
|
||
const int i1 = qBound(0, i0 + 1, input.size() - 1);
|
||
output.append(lerpPoint(input[i0], input[i1], src - i0));
|
||
}
|
||
return output;
|
||
}
|
||
|
||
QVector<QVector3D> TrajectoryCalculator::resampleTrajectorySpline(const QVector<QVector3D> &input, int targetCount)
|
||
{
|
||
if (input.size() < 4) return resampleTrajectoryLinear(input, targetCount);
|
||
QVector<QVector3D> output;
|
||
if (targetCount <= 0) return output;
|
||
if (targetCount == 1) return QVector<QVector3D>{input.first()};
|
||
|
||
output.reserve(targetCount);
|
||
const double scale = static_cast<double>(input.size() - 1) / qMax(1, targetCount - 1);
|
||
for (int i = 0; i < targetCount; ++i) {
|
||
const double src = i * scale;
|
||
const int i1 = qBound(0, static_cast<int>(std::floor(src)), input.size() - 1);
|
||
const int i2 = qBound(0, i1 + 1, input.size() - 1);
|
||
const int i0 = qBound(0, i1 - 1, input.size() - 1);
|
||
const int i3 = qBound(0, i2 + 1, input.size() - 1);
|
||
output.append(catmullRom(input[i0], input[i1], input[i2], input[i3], src - i1));
|
||
}
|
||
return output;
|
||
}
|
||
|
||
TrajectoryFilterResult TrajectoryCalculator::filterAndInterpolateTrajectory(const QVector<QVector3D> &input,
|
||
const TrajectoryFilterOptions &options,
|
||
int targetCount)
|
||
{
|
||
TrajectoryFilterResult result;
|
||
const int n = input.size();
|
||
result.keepMask = QVector<bool>(n, true);
|
||
result.distanceOutlierMask = QVector<bool>(n, false);
|
||
result.speedOutlierMask = QVector<bool>(n, false);
|
||
result.angleOutlierMask = QVector<bool>(n, false);
|
||
if (n <= 0 || targetCount <= 0) return result;
|
||
|
||
for (int i = 1; i < n; ++i) {
|
||
const double dist = planarDistance(input[i - 1], input[i]);
|
||
if (options.distanceFilterEnabled && dist > options.maxSegmentDistanceM) {
|
||
result.distanceOutlierMask[i] = true;
|
||
result.keepMask[i] = false;
|
||
}
|
||
if (options.speedFilterEnabled && options.traceIntervalSec > 1e-9 && dist / options.traceIntervalSec > options.maxSpeedMps) {
|
||
result.speedOutlierMask[i] = true;
|
||
result.keepMask[i] = false;
|
||
}
|
||
}
|
||
|
||
if (options.angleFilterEnabled) {
|
||
const double thresholdRad = options.maxTurnAngleDeg * M_PI / 180.0;
|
||
for (int i = 1; i + 1 < n; ++i) {
|
||
const double d0 = planarDistance(input[i - 1], input[i]);
|
||
const double d1 = planarDistance(input[i], input[i + 1]);
|
||
if (d0 < 1e-6 || d1 < 1e-6) continue;
|
||
const double ax = input[i].x() - input[i - 1].x();
|
||
const double ay = input[i].y() - input[i - 1].y();
|
||
const double bx = input[i + 1].x() - input[i].x();
|
||
const double by = input[i + 1].y() - input[i].y();
|
||
const double cosv = std::clamp((ax * bx + ay * by) / (d0 * d1), -1.0, 1.0);
|
||
const double turn = std::acos(cosv);
|
||
if (turn > thresholdRad) {
|
||
result.angleOutlierMask[i] = true;
|
||
result.keepMask[i] = false;
|
||
}
|
||
}
|
||
}
|
||
|
||
if (options.preserveEndpoints && n > 0) {
|
||
result.keepMask[0] = true;
|
||
result.keepMask[n - 1] = true;
|
||
}
|
||
|
||
QVector<QVector3D> kept;
|
||
kept.reserve(n);
|
||
for (int i = 0; i < n; ++i) {
|
||
if (result.keepMask.value(i, true)) kept.append(input[i]);
|
||
}
|
||
if (kept.size() < 2) {
|
||
kept = input;
|
||
result.warnings.append(QStringLiteral("过滤后有效轨迹点过少，已使用原始轨迹插值。"));
|
||
}
|
||
|
||
if (options.interpolationMode == TrajectoryFilterOptions::InterpolationMode::Spline) {
|
||
if (kept.size() < 4) result.warnings.append(QStringLiteral("样条插值点数不足，已回退到线性插值。"));
|
||
result.outputPositions = resampleTrajectorySpline(kept, targetCount);
|
||
} else {
|
||
result.outputPositions = resampleTrajectoryLinear(kept, targetCount);
|
||
}
|
||
return result;
|
||
}
|