93 lines
3.3 KiB
C++
93 lines
3.3 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);
|
||
}
|
||
|
||
// 解析真实格式的 .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);
|
||
}
|