diff --git a/src/core/algo/GeoVolumeBuilder.cpp b/src/core/algo/GeoVolumeBuilder.cpp index 2e09313..201ce71 100644 --- a/src/core/algo/GeoVolumeBuilder.cpp +++ b/src/core/algo/GeoVolumeBuilder.cpp @@ -93,13 +93,20 @@ double principalAxisAngle(const std::vector& xs, return 0.5 * std::atan2(2.0 * sxy, sxx - syy); } +// ---- 距离权(可测纯函数)---- +double distanceWeight(double distCells) { + return 1.0 / (1.0 + distCells * distCells); +} + GeoBuildResult buildGeoVolume(const std::vector& lines, const GeoGridSpec& spec, const std::string& outStoreDir, - int pyramidLevels) { + int pyramidLevels, bool curvilinear) { using geopro::io::gpr::interpAlongTrack; using geopro::io::gpr::lonLatToLocalM; using geopro::io::gpr::PosHeading; + using geopro::io::gpr::projectToCenterline; + using geopro::io::gpr::resampleAndSmooth; using geopro::io::gpr::XY; if (lines.empty()) throw std::runtime_error("buildGeoVolume: 无测线"); @@ -147,34 +154,75 @@ GeoBuildResult buildGeoVolume(const std::vector& lines, } if (samples <= 0) throw std::runtime_error("buildGeoVolume: samples<=0"); - // ---- 2) PCA 求路向主轴角,旋转使路向 = X'、横路 = Y' ---- - // 收集全体通道平面点(trace 位置 + 通道横偏)求主轴。 - std::vector px, py; - for (std::size_t i = 0; i < scales.size(); ++i) { - const LineScale& sc = scales[i]; - const std::int64_t nt = sc.totalTraces; - if (nt < 1 || sc.trackM.size() < 2) continue; - // 稀疏采样道(每线最多 ~200 个采样点)求 PCA,省内存。 - const std::int64_t stride = std::max(1, nt / 200); - for (std::int64_t t = 0; t < nt; t += stride) { - const double frac = nt > 1 ? static_cast(t) / (nt - 1) : 0.0; - PosHeading ph = interpAlongTrack(sc.trackM, frac); - px.push_back(ph.pos.x); - py.push_back(ph.pos.y); + // ---- 2) 平面映射:PCA 旋转(G1) 或 中心线曲线坐标(G2)---- + // 两路均产出一个把通道平面点 (cx,cy)[局部米] → 网格 (gi,gj) + 距 cell 中心 + // 距离的映射;G1 旋转、G2 投影中心线。下方包围盒/累加复用同一映射。 + double rotRad = 0.0; // G1 路向主轴角;G2 恒 0(已沿中心线展开) + double cosR = 1.0, sinR = 0.0; + std::vector centerline; // G2:均匀重采样+平滑后的中心线 + double halfWidthM = 0.0; // G2:横向半幅(cell 中心偏移 = d + halfWidth) + + if (curvilinear) { + // 取沿路里程最长那条线的轨迹作中心线参考。 + std::size_t bestLine = 0; + double bestLen = -1.0; + for (std::size_t i = 0; i < scales.size(); ++i) { + const auto& tr = scales[i].trackM; + if (tr.size() < 2) continue; + double len = 0.0; + for (std::size_t k = 1; k < tr.size(); ++k) + len += std::hypot(tr[k].x - tr[k - 1].x, tr[k].y - tr[k - 1].y); + if (len > bestLen) { + bestLen = len; + bestLine = i; + } } + if (bestLen <= 0.0) + throw std::runtime_error("buildGeoVolume: 曲线版无有效中心线"); + // 重采样间距取 cellXY(细分够密),轻平滑窗口 2 点。 + centerline = resampleAndSmooth(scales[bestLine].trackM, spec.cellXY, 2); + if (centerline.size() < 2) + throw std::runtime_error("buildGeoVolume: 中心线重采样退化"); + } else { + // 收集全体通道平面点(trace 位置)求 PCA 主轴。 + std::vector px, py; + for (std::size_t i = 0; i < scales.size(); ++i) { + const LineScale& sc = scales[i]; + const std::int64_t nt = sc.totalTraces; + if (nt < 1 || sc.trackM.size() < 2) continue; + const std::int64_t stride = std::max(1, nt / 200); + for (std::int64_t t = 0; t < nt; t += stride) { + const double frac = nt > 1 ? static_cast(t) / (nt - 1) : 0.0; + PosHeading ph = interpAlongTrack(sc.trackM, frac); + px.push_back(ph.pos.x); + py.push_back(ph.pos.y); + } + } + rotRad = principalAxisAngle(px, py); + cosR = std::cos(-rotRad); + sinR = std::sin(-rotRad); } - const double rotRad = principalAxisAngle(px, py); - const double cosR = std::cos(-rotRad), sinR = std::sin(-rotRad); - auto rotate = [&](double x, double y, double& rx, double& ry) { - rx = x * cosR - y * sinR; - ry = x * sinR + y * cosR; + + // 平面映射:(cx,cy) → 路向坐标 (mx,my)。G1=旋转;G2=中心线 (s,d)。 + auto planeMap = [&](double cx, double cy, double& mx, double& my) { + if (curvilinear) { + auto pr = projectToCenterline(centerline, XY{cx, cy}); + mx = pr.s; + my = pr.d; + } else { + mx = cx * cosR - cy * sinR; + my = cx * sinR + cy * cosR; + } }; - // ---- 求旋转后包围盒(含通道横偏)→ 网格维度 + 原点 ---- + // ---- 求包围盒(含通道横偏)→ 网格维度 + 原点 ---- + // G2 曲线版:横向半幅 halfWidth 用 |d| 的分位(98%),对短截/斜插桩线的离群 + // d 鲁棒——否则少数离群点把 ny 撑到虚高、整片 d 区间空着拉低填充率。 double minX = std::numeric_limits::infinity(); double minY = std::numeric_limits::infinity(); double maxX = -std::numeric_limits::infinity(); double maxY = -std::numeric_limits::infinity(); + std::vector dVals; // G2:收集带符号 d 求分位(横向范围可不对称) for (std::size_t i = 0; i < scales.size(); ++i) { const LineScale& sc = scales[i]; const std::int64_t nt = sc.totalTraces; @@ -187,12 +235,13 @@ GeoBuildResult buildGeoVolume(const std::vector& lines, for (double oc : {sc.chanX.front(), sc.chanX.back()}) { const double cx = ph.pos.x + oc * perpX; const double cy = ph.pos.y + oc * perpY; - double rx, ry; - rotate(cx, cy, rx, ry); - minX = std::min(minX, rx); - maxX = std::max(maxX, rx); - minY = std::min(minY, ry); - maxY = std::max(maxY, ry); + double mx, my; + planeMap(cx, cy, mx, my); + minX = std::min(minX, mx); + maxX = std::max(maxX, mx); + minY = std::min(minY, my); + maxY = std::max(maxY, my); + if (curvilinear) dVals.push_back(my); } } } @@ -205,16 +254,43 @@ GeoBuildResult buildGeoVolume(const std::vector& lines, if (cell <= 0.0 || range <= 0.0) return 1; return static_cast(std::ceil(range / cell)) + 1; }; - const int nx = cells(maxX - minX, spec.cellXY); // 沿路(长轴 X') - const int ny = cells(maxY - minY, spec.cellXY); // 横路 Y' - const int nz = cells(maxDepth, spec.cellZ); // 深度 Z + + // G2:中心线为某条边线时,d 多为单侧分布(非 ±对称);用带符号 d 的 + // [1%,99%] 分位作横向范围(dLoM..dHiM),鲁棒于离群桩线、且不浪费空白半侧。 + double dLoM = 0.0, dHiM = 0.0; + int nx = 0, ny = 0; + if (curvilinear) { + if (!dVals.empty()) { + std::sort(dVals.begin(), dVals.end()); + const double maxIdx = static_cast(dVals.size() - 1); + const std::size_t loI = + static_cast(std::max(0.0, 0.01 * maxIdx)); + const std::size_t hiI = + static_cast(std::min(maxIdx, 0.99 * maxIdx)); + dLoM = dVals[loI]; + dHiM = dVals[hiI]; + } + if (dHiM - dLoM <= 0.0) { // 退化兜底 + dLoM = -spec.cellXY; + dHiM = spec.cellXY; + } + halfWidthM = -dLoM; // gy = (d - dLoM)/cellXY(横向原点对齐 dLoM) + minX = 0.0; // s 从 0 起 + nx = cells(maxX, spec.cellXY); + ny = cells(dHiM - dLoM, spec.cellXY); + } else { + nx = cells(maxX - minX, spec.cellXY); // 沿路(长轴 X') + ny = cells(maxY - minY, spec.cellXY); // 横路 Y' + } + const int nz = cells(maxDepth, spec.cellZ); // 深度 Z const std::size_t cellCount = static_cast(nx) * ny * nz; - // ---- 3) 逐线逐道逐通道逐样本 → grid sum/count(重叠均值)---- + // ---- 3) 逐线逐道逐通道逐样本 → grid 加权累加 ---- + // sum = Σ(w·v),wsum = Σw;w=1(G1 等权)或距 cell 中心的距离权(G2)。 std::vector sum(cellCount, 0.0); - std::vector cnt(cellCount, 0); + std::vector wsum(cellCount, 0.0); auto cellIdx = [&](int gi, int gj, int gk) -> std::size_t { return (static_cast(gk) * ny + gj) * nx + gi; @@ -232,9 +308,6 @@ GeoBuildResult buildGeoVolume(const std::vector& lines, // 各通道 .iprb 区间读(与 chanX 文件顺序对齐:iprb[c] ↔ chanX[c])。 const auto& iprbPaths = lines[i].iprb; - if (static_cast(iprbPaths.size()) != nChan) { - // 通道数与 .ord 有效通道不一致:按较小者对齐,避免越界。 - } const int useChan = std::min(nChan, static_cast(iprbPaths.size())); for (std::int64_t t0 = 0; t0 < nt; t0 += kChunk) { @@ -254,12 +327,26 @@ GeoBuildResult buildGeoVolume(const std::vector& lines, const double oc = sc.chanX[c]; const double cx = ph.pos.x + oc * perpX; const double cy = ph.pos.y + oc * perpY; - double rx, ry; - rotate(cx, cy, rx, ry); - const int gi = static_cast(std::lround((rx - minX) / spec.cellXY)); - const int gj = static_cast(std::lround((ry - minY) / spec.cellXY)); + double mx, my; + planeMap(cx, cy, mx, my); + // G1:mx/my 为旋转后绝对坐标;G2:mx=s、my=d(横向原点对齐 dLoM, + // 即 halfWidthM=-dLoM,gyf=(d-dLoM)/cellXY)。 + const double gxf = (mx - minX) / spec.cellXY; + const double gyf = + curvilinear ? (my + halfWidthM) / spec.cellXY + : (my - minY) / spec.cellXY; + const int gi = static_cast(std::lround(gxf)); + const int gj = static_cast(std::lround(gyf)); if (gi < 0 || gi >= nx || gj < 0 || gj >= ny) continue; + // 距离权:到 cell 中心的横/纵平面距离(cell 单位)。G1 等权 w=1。 + double w = 1.0; + if (curvilinear) { + const double dxc = gxf - gi; // 已是 cell 单位 + const double dyc = gyf - gj; + w = distanceWeight(std::sqrt(dxc * dxc + dyc * dyc)); + } + for (int s = 0; s < samples; ++s) { const double depth = geopro::io::gpr::depthOfSample(s, sc.header); const int gk = static_cast(std::lround(depth / spec.cellZ)); @@ -267,19 +354,19 @@ GeoBuildResult buildGeoVolume(const std::vector& lines, const double v = static_cast( bs[c].data[static_cast(lt) * samples + s]); const std::size_t idx = cellIdx(gi, gj, gk); - sum[idx] += v; - ++cnt[idx]; + sum[idx] += w * v; + wsum[idx] += w; } } } } } - // ---- 4) 均值 + 求值域 ---- + // ---- 4) 加权均值 + 求值域 ---- std::int64_t filled = 0; for (std::size_t idx = 0; idx < cellCount; ++idx) { - if (cnt[idx] == 0) continue; - const double mean = sum[idx] / cnt[idx]; + if (wsum[idx] <= 0.0) continue; + const double mean = sum[idx] / wsum[idx]; sum[idx] = mean; // 复用 sum 存均值 vmin = std::min(vmin, mean); vmax = std::max(vmax, mean); @@ -300,7 +387,7 @@ GeoBuildResult buildGeoVolume(const std::vector& lines, for (int gj = 0; gj < ny; ++gj) for (int gi = 0; gi < nx; ++gi) { const std::size_t idx = cellIdx(gi, gj, gk); - vol.at(gi, gj, gk) = (cnt[idx] == 0) + vol.at(gi, gj, gk) = (wsum[idx] <= 0.0) ? ScalarVolumeI16::kBlank : quant.toQ(sum[idx]); } @@ -308,7 +395,8 @@ GeoBuildResult buildGeoVolume(const std::vector& lines, BuiltI16 built; built.vol = std::move(vol); built.quant = quant; - built.origin = {minX, minY, 0.0}; + // G2 横向原点对齐 dLoM(曲线坐标系:X=沿路里程 s、Y=横向偏移 d)。 + built.origin = {minX, curvilinear ? dLoM : minY, 0.0}; built.spacing = {spec.cellXY, spec.cellXY, spec.cellZ}; built.vminPhys = vmin; built.vmaxPhys = vmax; @@ -325,7 +413,7 @@ GeoBuildResult buildGeoVolume(const std::vector& lines, r.ny = ny; r.nz = nz; r.oxM = minX; - r.oyM = minY; + r.oyM = curvilinear ? dLoM : minY; r.rotRad = rotRad; r.filled = filled; r.total = static_cast(cellCount); diff --git a/src/core/algo/GeoVolumeBuilder.hpp b/src/core/algo/GeoVolumeBuilder.hpp index 10babbf..ed89145 100644 --- a/src/core/algo/GeoVolumeBuilder.hpp +++ b/src/core/algo/GeoVolumeBuilder.hpp @@ -19,6 +19,11 @@ struct GeoGridSpec { double principalAxisAngle(const std::vector& xs, const std::vector& ys); +// 曲线版重叠累加的距离权(distCells = 样本到 cell 中心的平面距离,单位 cell)。 +// w = 1/(1+distCells^2):距中心越近权越大 → 加权均值偏向近点,减少 RTK +// 微错位的等权稀释糊化。distCells=0 → w=1(最大);单调递减。 +double distanceWeight(double distCells); + // 真实数据建体的产物指标(维度/原点/旋转角/填充率)。 struct GeoBuildResult { int nx = 0, ny = 0, nz = 0; @@ -48,9 +53,15 @@ struct GeoLineInput { // 5) 扫值域定 Quant(offset=中点),量化写 store + buildPyramid。 // // 内存有界:逐线流式读道(readIprbRange 区间),只持 grid 的 sum/count 累加缓冲。 +// +// curvilinear=false(默认,G1):全局单一 PCA 旋转 → 地理盒网格,重叠等权均值。 +// curvilinear=true(G2):取沿路里程最长那条线的 GPS 轨迹作中心线(重采样+轻平滑), +// 把每点投影得 (沿路里程 s, 带符号横偏 d) → 网格 X=s/Y=d/Z=深度,把弯路"拉直"; +// 重叠按到 cell 中心的距离加权累加(w=1/(1+(dist/cellXY)^2)),偏向近点。 GeoBuildResult buildGeoVolume(const std::vector& lines, const GeoGridSpec& spec, - const std::string& outStoreDir, int pyramidLevels); + const std::string& outStoreDir, int pyramidLevels, + bool curvilinear = false); } // namespace geopro::core diff --git a/src/io/gpr/GpsTrack.cpp b/src/io/gpr/GpsTrack.cpp index fd00805..5004b9a 100644 --- a/src/io/gpr/GpsTrack.cpp +++ b/src/io/gpr/GpsTrack.cpp @@ -1,7 +1,9 @@ #include "io/gpr/GpsTrack.hpp" +#include #include #include +#include #include #include @@ -106,4 +108,107 @@ PosHeading interpAlongTrack(const std::vector& trackM, double frac) { return r; } +CenterlineProj projectToCenterline(const std::vector& centerline, + const XY& p) { + CenterlineProj r; + const std::size_t n = centerline.size(); + if (n == 0) return r; + if (n == 1) { + r.s = 0.0; + r.d = std::hypot(p.x - centerline[0].x, p.y - centerline[0].y); + return r; + } + + // 逐段求脚点(夹到段内),取距 p 最近者;累计里程 + 带符号横偏。 + double bestDist2 = std::numeric_limits::infinity(); + double cum = 0.0; // 段起点累计里程 + for (std::size_t i = 1; i < n; ++i) { + const XY& a = centerline[i - 1]; + const XY& b = centerline[i]; + const double ex = b.x - a.x, ey = b.y - a.y; + const double segLen2 = ex * ex + ey * ey; + const double segLen = std::sqrt(segLen2); + + // p 在段上的投影参数 u(夹到 [0,1])。 + double u = 0.0; + if (segLen2 > 0.0) + u = ((p.x - a.x) * ex + (p.y - a.y) * ey) / segLen2; + if (u < 0.0) u = 0.0; + if (u > 1.0) u = 1.0; + + const double footX = a.x + u * ex; + const double footY = a.y + u * ey; + const double dx = p.x - footX, dy = p.y - footY; + const double dist2 = dx * dx + dy * dy; + + if (dist2 < bestDist2) { + bestDist2 = dist2; + r.s = cum + u * segLen; + // 带符号横偏:左法向 n=(-ty,tx)(单位段向量)。退化段 → d=到脚点距离。 + if (segLen > 0.0) { + const double tx = ex / segLen, ty = ey / segLen; + const double nx = -ty, ny = tx; // 左法向 + r.d = dx * nx + dy * ny; + } else { + r.d = std::sqrt(dist2); + } + } + cum += segLen; + } + return r; +} + +std::vector resampleAndSmooth(const std::vector& polyline, double step, + int smoothWindow) { + const std::size_t n = polyline.size(); + if (n < 2 || step <= 0.0) return polyline; + + // 累计里程。 + std::vector cum(n, 0.0); + for (std::size_t i = 1; i < n; ++i) + cum[i] = cum[i - 1] + + std::hypot(polyline[i].x - polyline[i - 1].x, + polyline[i].y - polyline[i - 1].y); + const double total = cum.back(); + if (total <= 0.0) return polyline; + + // 均匀里程取样(含终点)。 + std::vector out; + const int m = static_cast(std::floor(total / step)); + out.reserve(static_cast(m) + 2); + std::size_t seg = 1; + for (int k = 0; k <= m; ++k) { + const double target = k * step; + while (seg < n && cum[seg] < target) ++seg; + if (seg >= n) seg = n - 1; + const double segLen = cum[seg] - cum[seg - 1]; + const double lf = segLen > 0.0 ? (target - cum[seg - 1]) / segLen : 0.0; + XY q; + q.x = polyline[seg - 1].x + (polyline[seg].x - polyline[seg - 1].x) * lf; + q.y = polyline[seg - 1].y + (polyline[seg].y - polyline[seg - 1].y) * lf; + out.push_back(q); + } + if (out.empty() || std::hypot(out.back().x - polyline.back().x, + out.back().y - polyline.back().y) > 1e-9) + out.push_back(polyline.back()); + + // 轻度滑动平均(端点收缩窗口;不平滑首尾以保里程范围)。 + if (smoothWindow <= 0 || out.size() < 3) return out; + std::vector sm = out; + const int N = static_cast(out.size()); + for (int i = 1; i < N - 1; ++i) { + int w = std::min({smoothWindow, i, N - 1 - i}); + double sx = 0, sy = 0; + int cnt = 0; + for (int j = i - w; j <= i + w; ++j) { + sx += out[j].x; + sy += out[j].y; + ++cnt; + } + sm[i].x = sx / cnt; + sm[i].y = sy / cnt; + } + return sm; +} + } // namespace geopro::io::gpr diff --git a/src/io/gpr/GpsTrack.hpp b/src/io/gpr/GpsTrack.hpp index 5996851..e892ca4 100644 --- a/src/io/gpr/GpsTrack.hpp +++ b/src/io/gpr/GpsTrack.hpp @@ -41,6 +41,26 @@ struct PosHeading { }; PosHeading interpAlongTrack(const std::vector& trackM, double frac); +// 把点 p 投影到中心线折线 centerline(局部米),返回沿线里程 s 与带符号横向偏移 d。 +// - s:p 在中心线上最近点的累计里程(米,>=0;起点 0)。 +// - d:p 到中心线的带符号横向距离(米)。符号由最近段的左法向定: +// 段方向 t=(tx,ty),左法向 n=(-ty,tx);d = (p-foot)·n。 +// 即 p 在中心线左侧 d>0、右侧 d<0。 +// - 最近点取所有段上投影脚点(夹到段内)距 p 最近者。 +// - centerline 少于 2 点 → s=0、d 取到该点的距离(无方向,d=0)。 +struct CenterlineProj { + double s = 0; // 沿线里程(米) + double d = 0; // 带符号横向偏移(米) +}; +CenterlineProj projectToCenterline(const std::vector& centerline, + const XY& p); + +// 把任意折线(局部米)重采样为均匀间距 step 的有序点 + 轻度滑动平均平滑, +// 得规整中心线参考。空/单点原样返回;step<=0 原样返回。 +// smoothWindow:滑动平均窗口半径(点数,0=不平滑);端点收缩窗口。 +std::vector resampleAndSmooth(const std::vector& polyline, double step, + int smoothWindow); + } // namespace geopro::io::gpr #endif // GEOPRO_IO_GPR_GPSTRACK_HPP diff --git a/tests/core/test_geo_volume_builder.cpp b/tests/core/test_geo_volume_builder.cpp index 1c3343a..243d3d5 100644 --- a/tests/core/test_geo_volume_builder.cpp +++ b/tests/core/test_geo_volume_builder.cpp @@ -97,6 +97,46 @@ GeoLineInput makeLine(const fs::path& dir, const std::string& tag, double lat0, return in; } +// 写一条 L 形弯轨迹的 .gps:先沿北(纬增,lon 固定)走半程,再沿东(经增,lat 固定)走半程。 +void writeLGps(const fs::path& path, double lat0, double lon0, double dLatHalf, + double dLonHalf, int pts) { + std::ofstream f(path); + const int half = pts / 2; + for (int i = 0; i < pts; ++i) { + double lat, lon; + if (i <= half) { + const double frac = half > 0 ? static_cast(i) / half : 0.0; + lat = lat0 + dLatHalf * frac; // 北段:纬增 + lon = lon0; + } else { + const double frac = + (pts - 1 - half) > 0 + ? static_cast(i - half) / (pts - 1 - half) + : 0.0; + lat = lat0 + dLatHalf; // 拐角后纬固定 + lon = lon0 + dLonHalf * frac; // 东段:经增 + } + f << "2023-06-03\t00:00:00:000\t" << std::fixed << std::setprecision(10) + << lat << "\tN\t" << lon << "\tE\t9.0\tM\t4\n"; + } +} + +GeoLineInput makeLLine(const fs::path& dir, const std::string& tag, double lat0, + double lon0, double dLatHalf, double dLonHalf, int traces, + std::int16_t val) { + const int samples = 8; + writeChannel(dir / (tag + "_A01.iprb"), samples, traces, val); + writeChannel(dir / (tag + "_A02.iprb"), samples, traces, val); + writeOrd(dir / (tag + ".ord")); + writeLGps(dir / (tag + ".gps"), lat0, lon0, dLatHalf, dLonHalf, traces); + GeoLineInput in; + in.iprb = {(dir / (tag + "_A01.iprb")).string(), + (dir / (tag + "_A02.iprb")).string()}; + in.ord = (dir / (tag + ".ord")).string(); + in.gps = (dir / (tag + ".gps")).string(); + return in; +} + } // namespace TEST(GeoVolumeBuilder, BuildsSyntheticLinesOverlapAveraged) { @@ -152,3 +192,66 @@ TEST(GeoVolumeBuilder, BuildsSyntheticLinesOverlapAveraged) { fs::remove_all(tmp, ec); } + +// L 形弯线:PCA 版把拐后段甩进大 Y'(虚高 ny、稀疏),曲线版沿 X 拉直展开 +// (ny 接近真实横向、填充更密)。断言曲线版 ny < PCA 版 ny 且填充率更高。 +TEST(GeoVolumeBuilder, CurvilinearStraightensBendVsPca) { + const fs::path tmp = fs::temp_directory_path() / "geopro_geovol_curv_test"; + std::error_code ec; + fs::remove_all(tmp, ec); + fs::create_directories(tmp); + + // L 形:北 ~111m(0.001° 纬)+ 东 ~96m(0.001° 经)。 + const double lat0 = 30.200, lon0 = 120.244; + std::vector lines = {makeLLine( + tmp, "synL_001", lat0, lon0, /*dLatHalf=*/0.001, /*dLonHalf=*/0.001, + /*traces=*/120, /*val=*/150)}; + + GeoGridSpec spec{/*cellXY=*/0.5, /*cellZ=*/0.1}; + + const std::string storePca = (tmp / "pca").string(); + GeoBuildResult rPca = + buildGeoVolume(lines, spec, storePca, /*pyramidLevels=*/0, + /*curvilinear=*/false); + + const std::string storeCurv = (tmp / "curv").string(); + GeoBuildResult rCurv = + buildGeoVolume(lines, spec, storeCurv, /*pyramidLevels=*/0, + /*curvilinear=*/true); + + const double fillPca = + rPca.total > 0 ? static_cast(rPca.filled) / rPca.total : 0.0; + const double fillCurv = + rCurv.total > 0 ? static_cast(rCurv.filled) / rCurv.total : 0.0; + + // 曲线版把弯路拉直:横向维度 ny 明显小于 PCA 版。 + EXPECT_LT(rCurv.ny, rPca.ny); + // 填充更密。 + EXPECT_GT(fillCurv, fillPca); + // 曲线版仍建出有效体。 + EXPECT_GT(rCurv.filled, 0); + EXPECT_GT(rCurv.nx, 50); + + fs::remove_all(tmp, ec); +} + +// 距离权纯函数:距 cell 中心越近权越大,单调递减,中心处取最大 1。 +TEST(GeoVolumeBuilder, DistanceWeightDecreasesWithDistance) { + EXPECT_DOUBLE_EQ(distanceWeight(0.0), 1.0); // 中心权最大 + EXPECT_LT(distanceWeight(1.0), distanceWeight(0.5)); // 单调递减 + EXPECT_LT(distanceWeight(2.0), distanceWeight(1.0)); + EXPECT_GT(distanceWeight(0.5), 0.0); +} + +// 加权均值偏向近点:同 cell 两样本,近点(dist=0.1)值 100、远点(dist=0.9)值 300。 +// 等权均值 = 200;距离加权均值应明显 < 200(偏向近点的 100)。 +TEST(GeoVolumeBuilder, WeightedMeanFavorsNearerPoint) { + const double vNear = 100.0, vFar = 300.0; + const double wNear = distanceWeight(0.1); + const double wFar = distanceWeight(0.9); + const double weighted = + (wNear * vNear + wFar * vFar) / (wNear + wFar); + const double equal = 0.5 * (vNear + vFar); // 200 + EXPECT_LT(weighted, equal); // 偏离等权 + EXPECT_LT(weighted - vNear, vFar - weighted); // 更靠近近点值 +} diff --git a/tests/io/gpr/test_gps_track.cpp b/tests/io/gpr/test_gps_track.cpp index 410417d..4a87f77 100644 --- a/tests/io/gpr/test_gps_track.cpp +++ b/tests/io/gpr/test_gps_track.cpp @@ -74,6 +74,65 @@ TEST(GpsTrack, ChannelLateralPlacement) { EXPECT_NEAR(cy, 5.0, 1e-9); } +// ---- 中心线投影(曲线坐标核心,Task G2)---- + +// 直线中心线沿 +X:里程 s = 点 X,横偏 d = 带符号 Y(左法向 (0,1) → 上方 d>0)。 +TEST(GpsTrack, ProjectStraightCenterline) { + std::vector 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 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 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"; diff --git a/tools/gpr_poc/main.cpp b/tools/gpr_poc/main.cpp index 5bdc757..cd775c2 100644 --- a/tools/gpr_poc/main.cpp +++ b/tools/gpr_poc/main.cpp @@ -693,7 +693,8 @@ int cmdBuildGeo(int argc, char** argv) { const Args a = parseArgs(argc, argv, 2); if (a.positional.empty()) { std::cerr << "用法: gpr_poc build-geo [--cellXY 0.5] [--cellZ 0.1] " - "[--out ] [--levels 4] [--maxLines N]\n"; + "[--out ] [--levels 4] [--maxLines N] " + "[--curvilinear]\n"; return 2; } const std::string dir = a.positional[0]; @@ -701,12 +702,14 @@ int cmdBuildGeo(int argc, char** argv) { const double cellZ = std::stod(a.get("cellZ", "0.1")); const int levels = std::stoi(a.get("levels", "4")); const int maxLines = std::stoi(a.get("maxLines", "0")); + const bool curvilinear = a.kv.count("curvilinear") > 0; // 曲线版(G2) const std::string out = a.get("out", (fs::temp_directory_path() / "gpr_store_geo").string()); std::cout << "[build-geo] dir=" << dir << " cellXY=" << cellXY - << " cellZ=" << cellZ << " levels=" << levels << " out=" << out - << "\n"; + << " cellZ=" << cellZ << " levels=" << levels + << " mode=" << (curvilinear ? "曲线(中心线)" : "PCA") + << " out=" << out << "\n"; // 发现线号 → 各线 iprb/ord(复用 discoverLine)+ .gps(按线号匹配)。 std::vector lineNos = discoverLines(dir); @@ -757,7 +760,7 @@ int cmdBuildGeo(int argc, char** argv) { Stopwatch sw; const geopro::core::GeoGridSpec spec{cellXY, cellZ}; const geopro::core::GeoBuildResult r = - geopro::core::buildGeoVolume(lines, spec, out, levels); + geopro::core::buildGeoVolume(lines, spec, out, levels, curvilinear); const double buildMs = sw.elapsedMs(); const double fillRate = @@ -767,6 +770,8 @@ int cmdBuildGeo(int argc, char** argv) { const double peak = Probe::peakMemMB(); std::cout << "\n=== build-geo 指标(真实 RTK 几何统一路向体)===\n"; + std::cout << "网格模式 : " << (curvilinear ? "曲线(中心线展开)" : "PCA 旋转") + << "\n"; std::cout << "测线数 : " << lines.size() << "\n"; std::cout << "体维度 : " << r.nx << " x " << r.ny << " x " << r.nz << "\n"; @@ -4057,7 +4062,7 @@ void usage() { "[--out ] [--levels 3] [--sliceXBricks 8] " "[--maxLines N]\n" " gpr_poc build-geo [--cellXY 0.5] [--cellZ 0.1] " - "[--out ] [--levels 4] [--maxLines N]\n" + "[--out ] [--levels 4] [--maxLines N] [--curvilinear]\n" " gpr_poc load \n" " gpr_poc selftest\n" " gpr_poc offscreen-smoke\n"