From 170a1f1016a65c221dcf9783f8a5766673644a3e Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Sat, 22 Aug 2026 09:33:11 +0900 Subject: [PATCH] validator: judge road-link gaps by lane-boundary contact geom.road-link-gap compared the two roads' reference-line endpoints and subtracted |laneOffsetA - laneOffsetB| as slack. That proxy breaks whenever the lane offset varies with s or the roads meet at an angle: on a measured real map all ten of its reports were false positives whose lane boundaries in fact touched to 0.000 m. Measure the thing the rule claims to measure instead. At each contact end, build the road's lane-boundary cross section in world coordinates - the centre line (reference line shifted by at that station) plus the running sum of lane widths outward along the reference-line normal - and report the minimum distance between the two sections. Lane offset is handled by construction, so the slack term is gone. The lane section and the record covering the contact station are the ones evaluated, and a link with no contactPoint takes the nearer of the target's two ends. Comparing minimum distance asks whether the sections touch, not whether they are congruent: lane counts differ across a link at merges, ramps and junction connectors (91 of 156 links on one map), so requiring congruence would trade one class of false positive for another. A road wholly displaced from its neighbour is still reported. Default threshold 0.3 m, down from 0.5, now that the measurement is direct. Mutations still 10/10; no MAP_DEFECT error on any fixture or on the 26-map external corpus. soderleden road 7 keeps its warning: its lanes really are tens of metres from the road it declares as predecessor. --- .../__tests__/validator/geometry.test.ts | 175 +++++++++++++++++- .../src/validator/layers/geometry.ts | 175 +++++++++++++++--- .../drawtonomy-sdk/src/validator/types.ts | 6 +- 3 files changed, 314 insertions(+), 42 deletions(-) diff --git a/packages/drawtonomy-sdk/__tests__/validator/geometry.test.ts b/packages/drawtonomy-sdk/__tests__/validator/geometry.test.ts index f000f85..afb12d7 100644 --- a/packages/drawtonomy-sdk/__tests__/validator/geometry.test.ts +++ b/packages/drawtonomy-sdk/__tests__/validator/geometry.test.ts @@ -114,14 +114,23 @@ describe('checkGeometry', () => { }) describe('road link contact', () => { - const pair = (bx: number, offsetA = 0, offsetB = 0): string => ` + // Road A runs along y = 0 from x = 0 to x = 100. Road B starts at + // (bx, by). Each carries one 3.5 m right lane, so a road's lane boundaries + // sit at its centre line (the reference line shifted by its laneOffset) + // and 3.5 m to the right of it. + const pair = ( + bx: number, + opts: { by?: number; offsetA?: string; offsetB?: string } = {} + ): string => { + const { by = 0, offsetA = 'a="0" b="0"', offsetB = 'a="0" b="0"' } = opts + return `
- +
@@ -130,9 +139,9 @@ describe('checkGeometry', () => { - + - +
@@ -140,6 +149,48 @@ describe('checkGeometry', () => { ` + } + + /** + * Two roads linked end-to-start, with each side's lane content injectable. + * Road A runs along y = 0 from x = 0 to x = 100; road B starts at + * (100, by). Defaults give both a single 3.5 m right lane, so their lane + * boundaries coincide and the pair is clean. + */ + const twoRoads = (opts: { + by?: number + aLanes?: string + bLanes?: string + aSections?: string + bOffset?: string + }): string => { + const { + by = 0, + aLanes = '', + bLanes = '', + aSections, + bOffset = '', + } = opts + const aLaneXml = + aSections ?? + `
${aLanes}` + return ` + +
+ + + + ${aLaneXml} + + + + + ${bOffset} +
${bLanes} + + +` + } it('accepts roads that touch', () => { expect(geomRules(pair(100))).toEqual([]) @@ -153,10 +204,116 @@ describe('checkGeometry', () => { expect(validateOpenDrive(pair(150)).verdict).toBe('yellow') }) - it('subtracts the lane-offset difference', () => { - // fabriksgatan's pattern: a connecting road with laneOffset 1.75 linking - // to a mainline with 0. The reference lines are 1.75 m apart by design. - expect(geomRules(pair(101.75, 1.75, 0))).toEqual([]) + it('reports the lane distance, not the reference-line distance', () => { + const found = findingsFor(pair(150)).filter(f => f.rule === 'geom.road-link-gap') + expect(found[0].message).toContain('lane boundaries') + expect(found[0].message).toContain('50.000 m apart') + }) + + // The false positive this rule exists to avoid. Road B's reference line is + // pushed 5 m off road A's by a laneOffset that ramps along s, so a check + // comparing reference-line endpoints (even after subtracting the offset + // difference at the contact) sees a gap — while the lanes themselves meet + // exactly. Measured on a real map, every report of the old proxy was of + // this shape and every one had a true lane distance of 0.000 m. + it('accepts lanes that meet while the reference lines are far apart', () => { + // A: no offset, so its centre is y = 0 and its lane edge y = -3.5. + // B: reference line at y = -5, laneOffset ramping from +5 at s = 0, so + // its centre is also y = 0 at the contact and its lane edge y = -3.5. + const xml = pair(100, { by: -5, offsetB: 'a="5" b="-0.02"' }) + expect(geomRules(xml)).toEqual([]) + }) + + it('still detects lanes that are genuinely apart', () => { + // Same construction, but B's offset leaves its lanes 1.5 m off A's. + const xml = pair(100, { by: -5, offsetB: 'a="3.5" b="-0.02"' }) + const found = findingsFor(xml).filter(f => f.rule === 'geom.road-link-gap') + expect(found.length).toBeGreaterThan(0) + expect(found[0].message).toContain('1.500 m apart') + }) + + it('honours a caller-supplied threshold', () => { + const xml = pair(100, { by: -5, offsetB: 'a="3.5" b="-0.02"' }) + expect(geomRules(xml, { geometry: { roadLinkGapMeters: 2 } })).toEqual([]) + }) + + // A lateral mismatch is not a gap. Lane counts and widths routinely differ + // across a link (merges, ramps and junction connectors: 91 of the 156 + // links on one real map), so the rule asks whether the two cross sections + // *touch*, not whether they are congruent. What it must catch is the two + // sections being wholly apart, which is the soderleden defect. + it('accepts a link where the roads touch but the lane counts differ', () => { + const xml = twoRoads({ + aLanes: '', + bLanes: + '' + + '', + }) + expect(geomRules(xml)).toEqual([]) + }) + + it('measures a multi-section road at the section covering the contact', () => { + // Road A's lane widens to 5 m from s = 50, and road B is shifted so that + // it meets that widened edge. Reading A's *first* section would place its + // edge 1.5 m away and report a gap that does not exist. + const xml = twoRoads({ + aSections: + '
' + + '' + + '
' + + '', + // B is a lone boundary pair 5 m below the reference line: its centre is + // at y = -5.0, exactly A's widened outer edge. + by: -5, + bLanes: '', + }) + expect(xml).toContain('laneSection s="50"') + expect(geomRules(xml)).toEqual([]) + + // The same B against a road whose lane never widens is 1.5 m short. + const narrow = twoRoads({ + by: -5, + bLanes: '', + }) + const found = findingsFor(narrow).filter(f => f.rule === 'geom.road-link-gap') + expect(found.length).toBeGreaterThan(0) + expect(found[0].message).toContain('1.500 m apart') + }) + + it('evaluates the width record covering the contact, not the first', () => { + // One lane section, two records: 3.5 m up to sOffset 50 and 6.0 m + // after it. The contact at s = 100 must read 6.0 m, putting A's outer + // edge at y = -6.0 where B's lone boundary sits. + const xml = twoRoads({ + aLanes: + '' + + '' + + '' + + '', + by: -6, + bLanes: '', + }) + expect(geomRules(xml)).toEqual([]) + + // Reading only the first record would have put A's edge at -3.5, which + // is where this B sits — and that must be reported as 2.5 m away. + const wrong = twoRoads({ + aLanes: + '' + + '' + + '' + + '', + by: -3.5, + bLanes: '', + }) + const found = findingsFor(wrong).filter(f => f.rule === 'geom.road-link-gap') + expect(found.length).toBeGreaterThan(0) + expect(found[0].message).toContain('2.500 m apart') + }) + + it('takes the nearer end when the link declares no contact point', () => { + const xml = pair(100).replace(' contactPoint="start"', '') + expect(geomRules(xml)).toEqual([]) }) }) @@ -177,7 +334,7 @@ describe('checkGeometry', () => { expect(DEFAULT_GEOMETRY_THRESHOLDS).toEqual({ planViewGapMeters: 0.02, planViewHeadingRad: 0.005, - roadLinkGapMeters: 0.5, + roadLinkGapMeters: 0.3, lengthMismatchRatio: 0.01, negativeWidthToleranceMeters: 0.001, }) diff --git a/packages/drawtonomy-sdk/src/validator/layers/geometry.ts b/packages/drawtonomy-sdk/src/validator/layers/geometry.ts index 1102977..d41e691 100644 --- a/packages/drawtonomy-sdk/src/validator/layers/geometry.ts +++ b/packages/drawtonomy-sdk/src/validator/layers/geometry.ts @@ -24,24 +24,40 @@ // road length vs sum observed max 4.0e-16 default 1 % (huge) // // Between roads it is a different story, and the reason is structural rather -// than a matter of precision. `road@length`-scale gaps at a link are normal: +// than a matter of precision. `road@length`-scale gaps between *reference +// lines* at a link are normal: // // 1. A road carrying a has its reference line laterally shifted // from the lane geometry. Two roads with different offsets meet along -// their *lanes* while their reference lines stay apart by exactly the -// offset difference. fabriksgatan's connecting roads (laneOffset a=1.75) -// linking to mainlines (a=0) produce a uniform 1.75 m; multi_intersections -// produces 3.75 m the same way. Both maps are correct. +// their *lanes* while their reference lines stay apart. fabriksgatan's +// connecting roads (laneOffset a=1.75) linking to mainlines (a=0) produce +// a uniform 1.75 m; multi_intersections produces 3.75 m the same way. Both +// maps are correct. // 2. Some shipped maps simply contain link records their geometry does not // honour: soderleden road 7 declares its predecessor to be road 2 at that -// road's end, 66 m from where road 7 actually begins. esmini drives this -// map regardless, because it routes through the junction rather than the -// stray link. +// road's end, tens of metres from where road 7 actually begins. esmini +// drives this map regardless, because it routes through the junction +// rather than the stray link. // -// So the road-link check is reported as a *warning*: it is real evidence worth -// surfacing, but it must not redden a map that ships and works. The lane offset -// difference is subtracted before comparing, which removes cause (1) exactly -// and leaves cause (2) visible. +// An earlier revision of this rule compared reference-line endpoints and +// subtracted |laneOffsetA - laneOffsetB| as slack. That proxy is wrong whenever +// the offset varies with s or the two roads meet at an angle: measured over a +// real map, every one of its ten reports was a false positive whose lanes in +// fact touched to 0.000 m. The rule therefore measures what it actually claims +// to measure — the distance between the two roads' *lane boundary cross +// sections* at the contact — which removes cause (1) by construction, with no +// slack term to tune, and leaves cause (2) visible. +// +// What is compared is the *minimum* distance between the two boundary sets, +// i.e. "do these two cross sections touch anywhere", not "are they congruent". +// That is deliberate: lane counts and widths routinely differ across a link at +// merges, ramps and junction connectors (91 of 156 links on one measured map), +// so requiring congruence would re-introduce false positives of a new shape. +// A road whose lanes are wholly displaced from its neighbour's — the soderleden +// case — still separates every pair of boundaries and is still reported. +// +// The check is still reported as a *warning*: it is real evidence worth +// surfacing, but it must not redden a map that ships and works. // // Lane widths need a tolerance for the opposite reason — not structure, but // arithmetic. Merge and exit lanes are authored to close at exactly zero, e.g. @@ -53,7 +69,12 @@ // the two cleanly rather than splitting a continuum. import { evalGeometry } from '../../exporter/odrGeometry.js' -import type { OdrMap, OdrRoad, OdrWidth } from '../../exporter/opendriveParser.js' +import type { + OdrLaneSection, + OdrMap, + OdrRoad, + OdrWidth, +} from '../../exporter/opendriveParser.js' import type { OdrFinding, ResolvedGeometryThresholds } from '../types.js' /** Wrap an angle to (-pi, pi]. */ @@ -102,6 +123,91 @@ function poseAtEnd(road: OdrRoad, at: 'start' | 'end'): Pose | null { const stationAtEnd = (road: OdrRoad, at: 'start' | 'end'): number => at === 'start' ? 0 : road.length +/** + * Width of a lane at `ds` metres into its lane section. + * + * A `` record applies from its own `sOffset` until the next one, and its + * polynomial is in `ds - sOffset`. Records before the first `sOffset` (which a + * conforming file does not produce, but a hand-edited one might) fall back to + * the first record evaluated at its own origin. + */ +function laneWidthAt(widths: readonly OdrWidth[], ds: number): number { + if (widths.length === 0) return 0 + let applicable = widths[0] + for (const rec of widths) { + if (rec.sOffset <= ds) applicable = rec + else break + } + return evalCubic(applicable, Math.max(ds - applicable.sOffset, 0)) +} + +/** The lane section covering station `s` (the last one starting at or before it). */ +function sectionAt(road: OdrRoad, s: number): OdrLaneSection | null { + if (road.laneSections.length === 0) return null + let applicable = road.laneSections[0] + for (const section of road.laneSections) { + if (section.s <= s + 1e-9) applicable = section + else break + } + return applicable +} + +interface Point { + x: number + y: number +} + +/** + * World positions of every lane boundary at a road's start or end. + * + * The boundaries are the centre line (the reference line shifted laterally by + * ``) plus the running sum of lane widths outward in each + * direction: left lanes accumulate in +t, right lanes in -t, where the lateral + * unit vector is the reference-line normal `(-sin hdg, cos hdg)`. + * + * Returns null when the road has no plan view to evaluate. A road without lane + * sections still yields its centre point, which is the best available + * statement of where it ends. + */ +function laneCrossSection(road: OdrRoad, at: 'start' | 'end'): Point[] | null { + const pose = poseAtEnd(road, at) + if (!pose) return null + + const s = stationAtEnd(road, at) + const nx = -Math.sin(pose.hdg) + const ny = Math.cos(pose.hdg) + + const offsets: number[] = [laneOffsetAt(road, s)] + const section = sectionAt(road, s) + if (section) { + // `ds` is measured from the section's own start, which is where a + // `` record's sOffset is relative to. + const ds = Math.max(s - section.s, 0) + for (const side of [section.left, section.right] as const) { + let t = offsets[0] + for (const lane of side) { + const width = laneWidthAt(lane.widths, ds) + t += side === section.left ? width : -width + offsets.push(t) + } + } + } + + return offsets.map(t => ({ x: pose.x + nx * t, y: pose.y + ny * t })) +} + +/** Smallest distance between any point of `a` and any point of `b`. */ +function minPointDistance(a: readonly Point[], b: readonly Point[]): number { + let best = Infinity + for (const p of a) { + for (const q of b) { + const d = Math.hypot(p.x - q.x, p.y - q.y) + if (d < best) best = d + } + } + return best +} + export function checkGeometry( map: OdrMap, thresholds: ResolvedGeometryThresholds @@ -219,28 +325,37 @@ export function checkGeometry( if (!target) continue // layer 2 reported the dangling link const myEnd = which === 'successor' ? 'end' : 'start' - const theirEnd = link.contactPoint ?? (which === 'successor' ? 'start' : 'end') - const mine = poseAtEnd(road, myEnd) - const theirs = poseAtEnd(target, theirEnd) - if (!mine || !theirs) continue - - // Subtract the lane-offset difference: two roads whose lanes meet can - // have reference lines apart by exactly that amount, and flagging it - // would redden fabriksgatan and multi_intersections, which are correct. - const myOffset = laneOffsetAt(road, stationAtEnd(road, myEnd)) - const theirOffset = laneOffsetAt(target, stationAtEnd(target, theirEnd)) - const offsetSlack = Math.abs(myOffset - theirOffset) - - const raw = Math.hypot(mine.x - theirs.x, mine.y - theirs.y) - const gap = Math.max(raw - offsetSlack, 0) + const mine = laneCrossSection(road, myEnd) + if (!mine) continue + + // With an explicit contactPoint there is one cross section to compare + // against. Without one, the link is under-specified, so both of the + // target's ends are tried and the nearer is taken: reporting the larger + // of two readings would be inventing a defect the document never + // asserted. + const theirEnds: readonly ('start' | 'end')[] = link.contactPoint + ? [link.contactPoint] + : ['start', 'end'] + + let gap = Infinity + let theirEnd: 'start' | 'end' = theirEnds[0] + for (const end of theirEnds) { + const theirs = laneCrossSection(target, end) + if (!theirs) continue + const d = minPointDistance(mine, theirs) + if (d < gap) { + gap = d + theirEnd = end + } + } + if (!Number.isFinite(gap)) continue + if (gap > thresholds.roadLinkGapMeters) { findings.push({ severity: 'warning', category: 'MAP_DEFECT', rule: 'geom.road-link-gap', - message: - `road ${road.id} <${which}> declares road ${target.id} at its ${theirEnd}, but the two reference lines are ${gap.toFixed(3)} m apart` + - (offsetSlack > 0 ? ` (after allowing ${offsetSlack.toFixed(3)} m of lane offset)` : ''), + message: `road ${road.id} <${which}> declares road ${target.id} at its ${theirEnd}, but the lane boundaries of the two roads are ${gap.toFixed(3)} m apart at the contact`, location: { roadId: road.id }, }) } diff --git a/packages/drawtonomy-sdk/src/validator/types.ts b/packages/drawtonomy-sdk/src/validator/types.ts index 0997977..6d40070 100644 --- a/packages/drawtonomy-sdk/src/validator/types.ts +++ b/packages/drawtonomy-sdk/src/validator/types.ts @@ -90,8 +90,8 @@ export interface OdrGeometryThresholds { */ planViewHeadingRad?: number /** - * Maximum allowed position gap at a road-to-road link contact point (m). - * Default 0.5. + * Maximum allowed distance between the lane-boundary cross sections of two + * linked roads at their contact point (m). Default 0.3. */ roadLinkGapMeters?: number /** @@ -131,7 +131,7 @@ export interface ResolvedGeometryThresholds { export const DEFAULT_GEOMETRY_THRESHOLDS: ResolvedGeometryThresholds = { planViewGapMeters: 0.02, planViewHeadingRad: 0.005, - roadLinkGapMeters: 0.5, + roadLinkGapMeters: 0.3, lengthMismatchRatio: 0.01, negativeWidthToleranceMeters: 0.001, }