152 lines
5.5 KiB
C++
152 lines
5.5 KiB
C++
#include "io/gpr/GpsTrack.hpp"
|
||
|
||
#include <gtest/gtest.h>
|
||
|
||
#include <cmath>
|
||
#include <cstdio>
|
||
#include <filesystem>
|
||
#include <fstream>
|
||
|
||
using namespace geopro::io::gpr;
|
||
|
||
// 经纬→米：纬 1° ≈ 111320 m；经按 cos(lat0) 缩。
|
||
TEST(GpsTrack, LonLatToLocalMatchesPhysics) {
|
||
const double lat0 = 30.204, lon0 = 120.244;
|
||
// 原点处归零。
|
||
XY o = lonLatToLocalM(lat0, lon0, lat0, lon0);
|
||
EXPECT_NEAR(o.x, 0.0, 1e-9);
|
||
EXPECT_NEAR(o.y, 0.0, 1e-9);
|
||
|
||
// 北移 0.001° 纬 → y ≈ 111.32 m，x≈0。
|
||
XY north = lonLatToLocalM(lat0 + 0.001, lon0, lat0, lon0);
|
||
EXPECT_NEAR(north.y, 111.32, 0.01);
|
||
EXPECT_NEAR(north.x, 0.0, 1e-6);
|
||
|
||
// 东移 0.001° 经 → x ≈ 111.32*cos(30.204°) ≈ 96.2 m。
|
||
XY east = lonLatToLocalM(lat0, lon0 + 0.001, lat0, lon0);
|
||
const double expX = 111.32 * std::cos(lat0 * 3.14159265358979323846 / 180.0);
|
||
EXPECT_NEAR(east.x, expX, 0.01);
|
||
EXPECT_NEAR(east.y, 0.0, 1e-6);
|
||
}
|
||
|
||
// 直线轨迹：frac=0/0.5/1 → 位置/航向正确。
|
||
TEST(GpsTrack, InterpStraightLine) {
|
||
std::vector<XY> tr = {{0, 0}, {10, 0}, {20, 0}}; // 沿 +X 直线，长 20
|
||
auto p0 = interpAlongTrack(tr, 0.0);
|
||
EXPECT_NEAR(p0.pos.x, 0.0, 1e-9);
|
||
EXPECT_NEAR(p0.hx, 1.0, 1e-9);
|
||
EXPECT_NEAR(p0.hy, 0.0, 1e-9);
|
||
|
||
auto pm = interpAlongTrack(tr, 0.5);
|
||
EXPECT_NEAR(pm.pos.x, 10.0, 1e-9);
|
||
EXPECT_NEAR(pm.pos.y, 0.0, 1e-9);
|
||
|
||
auto p1 = interpAlongTrack(tr, 1.0);
|
||
EXPECT_NEAR(p1.pos.x, 20.0, 1e-9);
|
||
EXPECT_NEAR(p1.hx, 1.0, 1e-9);
|
||
}
|
||
|
||
// 折线（L 形）：里程一半落在拐角后，航向应是第二段方向。
|
||
TEST(GpsTrack, InterpPolyline) {
|
||
std::vector<XY> tr = {{0, 0}, {10, 0}, {10, 10}}; // 先 +X 10，再 +Y 10，总长 20
|
||
auto pm = interpAlongTrack(tr, 0.5); // 里程 10 = 正好拐角
|
||
EXPECT_NEAR(pm.pos.x, 10.0, 1e-9);
|
||
EXPECT_NEAR(pm.pos.y, 0.0, 1e-9);
|
||
|
||
auto p75 = interpAlongTrack(tr, 0.75); // 里程 15 → 第二段中点 (10,5)
|
||
EXPECT_NEAR(p75.pos.x, 10.0, 1e-9);
|
||
EXPECT_NEAR(p75.pos.y, 5.0, 1e-9);
|
||
EXPECT_NEAR(p75.hx, 0.0, 1e-9); // 第二段沿 +Y
|
||
EXPECT_NEAR(p75.hy, 1.0, 1e-9);
|
||
}
|
||
|
||
// 通道横向定位：航向 (hx,hy)，垂直 = (-hy,hx)；通道位置 = trace 位置 + o_c×垂直。
|
||
TEST(GpsTrack, ChannelLateralPlacement) {
|
||
auto ph = interpAlongTrack({{0, 0}, {0, 10}}, 0.5); // 沿 +Y，航向 (0,1)
|
||
EXPECT_NEAR(ph.hx, 0.0, 1e-9);
|
||
EXPECT_NEAR(ph.hy, 1.0, 1e-9);
|
||
// 垂直(航向) = (-hy, hx) = (-1, 0)。偏移 +0.5 → 通道在 (-0.5, 5)。
|
||
const double perpX = -ph.hy, perpY = ph.hx;
|
||
const double oc = 0.5;
|
||
const double cx = ph.pos.x + oc * perpX;
|
||
const double cy = ph.pos.y + oc * perpY;
|
||
EXPECT_NEAR(cx, -0.5, 1e-9);
|
||
EXPECT_NEAR(cy, 5.0, 1e-9);
|
||
}
|
||
|
||
// ---- 中心线投影（曲线坐标核心，Task G2）----
|
||
|
||
// 直线中心线沿 +X：里程 s = 点 X，横偏 d = 带符号 Y（左法向 (0,1) → 上方 d>0）。
|
||
TEST(GpsTrack, ProjectStraightCenterline) {
|
||
std::vector<XY> center = {{0, 0}, {10, 0}, {20, 0}}; // 沿 +X，长 20
|
||
|
||
// 正中点 (5,0)：s=5, d=0。
|
||
auto a = projectToCenterline(center, {5.0, 0.0});
|
||
EXPECT_NEAR(a.s, 5.0, 1e-9);
|
||
EXPECT_NEAR(a.d, 0.0, 1e-9);
|
||
|
||
// 点 (8, +3)：脚点 (8,0)，s=8；左法向 (0,1) → d=+3。
|
||
auto b = projectToCenterline(center, {8.0, 3.0});
|
||
EXPECT_NEAR(b.s, 8.0, 1e-9);
|
||
EXPECT_NEAR(b.d, 3.0, 1e-9);
|
||
|
||
// 点 (8, -3)：右侧 → d=-3。
|
||
auto c = projectToCenterline(center, {8.0, -3.0});
|
||
EXPECT_NEAR(c.s, 8.0, 1e-9);
|
||
EXPECT_NEAR(c.d, -3.0, 1e-9);
|
||
}
|
||
|
||
// L 形弯中心线：拐角后的点 s 单调增长（被"拉直"沿 X 继续展开），d 符号正确。
|
||
TEST(GpsTrack, ProjectBentCenterlineMonotonicS) {
|
||
std::vector<XY> center = {{0, 0}, {10, 0}, {10, 10}}; // 先 +X 10，再 +Y 10
|
||
|
||
// 第一段中点 (5,0)：s=5。
|
||
auto p1 = projectToCenterline(center, {5.0, 0.0});
|
||
EXPECT_NEAR(p1.s, 5.0, 1e-9);
|
||
|
||
// 拐角后第二段中点 (10,5)：s = 10(首段) + 5 = 15（沿路继续增长，未甩进横向）。
|
||
auto p2 = projectToCenterline(center, {10.0, 5.0});
|
||
EXPECT_NEAR(p2.s, 15.0, 1e-9);
|
||
EXPECT_NEAR(p2.d, 0.0, 1e-9);
|
||
|
||
// s 单调：拐角后点的里程 > 拐角前点的里程。
|
||
EXPECT_GT(p2.s, p1.s);
|
||
|
||
// 第二段（方向 +Y，左法向 (-1,0)）：点 (8,5) 在中心线左侧 → d>0。
|
||
auto pl = projectToCenterline(center, {8.0, 5.0});
|
||
EXPECT_NEAR(pl.s, 15.0, 1e-9);
|
||
EXPECT_NEAR(pl.d, 2.0, 1e-9); // (p-foot)·(-1,0) = (8-10)*(-1)=2
|
||
}
|
||
|
||
// 重采样+平滑：均匀里程间距、首尾保留、平滑不外扩里程。
|
||
TEST(GpsTrack, ResampleUniformSpacing) {
|
||
std::vector<XY> poly = {{0, 0}, {3, 0}, {3, 4}}; // 长 3+4=7
|
||
auto rs = resampleAndSmooth(poly, /*step=*/1.0, /*smoothWindow=*/0);
|
||
ASSERT_GE(rs.size(), 7u);
|
||
// 相邻点间距 ≈ step（除末尾收尾段）。
|
||
for (std::size_t i = 1; i + 1 < rs.size(); ++i) {
|
||
const double d = std::hypot(rs[i].x - rs[i - 1].x, rs[i].y - rs[i - 1].y);
|
||
EXPECT_NEAR(d, 1.0, 1e-6);
|
||
}
|
||
// 末点保留原终点。
|
||
EXPECT_NEAR(rs.back().x, 3.0, 1e-6);
|
||
EXPECT_NEAR(rs.back().y, 4.0, 1e-6);
|
||
}
|
||
|
||
// 解析真实格式的 .gps（tab 分隔，含 N/E/M 标记列）。
|
||
TEST(GpsTrack, ParsesGpsFile) {
|
||
auto tmp = std::filesystem::temp_directory_path() / "geopro_gps_test.gps";
|
||
{
|
||
std::ofstream f(tmp);
|
||
f << "2023-06-03\t14:42:23:000\t30.21402519\tN\t120.24466077\tE\t9.390\tM\t4\r\n";
|
||
f << "2023-06-03\t14:42:23:203\t30.21402388\tN\t120.24466074\tE\t9.386\tM\t4\r\n";
|
||
f << "garbage line should be skipped\n";
|
||
}
|
||
GpsTrack t = parseGps(tmp.string());
|
||
std::filesystem::remove(tmp);
|
||
ASSERT_EQ(t.pts.size(), 2u);
|
||
EXPECT_NEAR(t.pts[0].lat, 30.21402519, 1e-8);
|
||
EXPECT_NEAR(t.pts[0].lon, 120.24466077, 1e-8);
|
||
EXPECT_NEAR(t.pts[0].elev, 9.390, 1e-6);
|
||
}
|