fix(roadmarkings): replace triangle list with cube primitive, add axis arrows (#183)

* fix: align example trace and tests with OSI road marking spec

Closes #114
The example trace (MovingHostWithStopLine.mcap) used the standard vehicle
coordinate system for the road marking, but the OSI spec defines a
different local frame for road markings:

  - local x = surface normal (upward from ground)
  - local y = lateral
  - local z = driving direction (bottom-to-top of marking image)

Fixed the trace to use spec-compliant values:
  - dimension: length=0.004 (protrusion), width=4.0 (lateral), height=0.3 (driving dir)
  - orientation: roll=0, pitch=-pi/2, yaw=pi (rotates local frame correctly)

Updated tests to use realistic spec-compliant default values.
Expanded code comment with full coordinate system explanation and spec link.

* fix(roadmarkings): use spec-compliant trace dimensions and add rotation test
* fix: use correct road marking orientation in example file
* fix: add road marking orientation variation example
---------

Signed-off-by: jdsika <carlo.van-driesten@bmw.de>
Signed-off-by: Thomas Sedlmayer <tsedlmayer@pmsfit.de>
Co-authored-by: Thomas Sedlmayer <tsedlmayer@pmsfit.de>

Author: jdsika

example-data/MovingHostWithStopLine.mcap

@@ -154,6 +154,7 @@ function buildSceneEntities(
       PREFIX_ROAD_MARKING,
       OSI_GLOBAL_FRAME,
       time,
+      panelSettings,
     );
 
     if (result != undefined) {

example-data/MovingHostWithStopLineVariations.mcap

@@ -1,7 +1,8 @@
-import { Point3 } from "@foxglove/schemas";
+import { GroundTruthPanelSettings } from "@converters";
+import { ArrowPrimitive } from "@foxglove/schemas";
 import { RoadMarking, TrafficSign_MainSign_Classification_Type } from "@lichtblick/asam-osi-types";
 import { Time } from "@lichtblick/suite";
-import { pointListToTriangleListPrimitive } from "@utils/primitives/lines";
+import { buildObjectAxes, objectToCubePrimitive } from "@utils/primitives/objects";
 import { generateSceneEntityId, PartialSceneEntity } from "@utils/scene";
 import { DeepRequired } from "ts-essentials";
 
@@ -14,6 +15,7 @@ export function buildRoadMarkingEntity(
   id_prefix: string,
   frame_id: string,
   time: Time,
+  config: GroundTruthPanelSettings | undefined,
 ): PartialSceneEntity | undefined {
   if (
     roadMarking.classification.traffic_main_sign_type !==
@@ -22,45 +24,41 @@ export function buildRoadMarkingEntity(
     return undefined;
   }
 
-  const roadMarkingPoints = [
-    {
-      position: {
-        x: roadMarking.base.position.x,
-        y: roadMarking.base.position.y,
-        z: roadMarking.base.position.z,
-      } as Point3,
-      width: roadMarking.base.dimension.width,
-      height: roadMarking.base.dimension.height,
-    },
-    {
-      position: {
-        x: roadMarking.base.position.x + roadMarking.base.dimension.length,
-        y: roadMarking.base.position.y,
-        z: roadMarking.base.position.z,
-      } as Point3,
-      width: roadMarking.base.dimension.width,
-      height: roadMarking.base.dimension.height,
-    },
-  ];
+  const pos = roadMarking.base.position;
+  const ori = roadMarking.base.orientation;
+  const dim = roadMarking.base.dimension;
 
-  // Define color and opacity based on OSI classification
-  const rgb = ROAD_MARKING_COLOR[roadMarking.classification.monochrome_color];
-  const color = { r: rgb.r, g: rgb.g, b: rgb.b, a: 1 };
+  // Road markings use BaseStationary, same as other objects. The orientation
+  // quaternion handles whatever local-frame convention the trace author used.
+  // See: https://opensimulationinterface.github.io/osi-antora-generator/asamosi/latest/gen/structosi3_1_1RoadMarking.html
+  const cube = objectToCubePrimitive(
+    pos.x,
+    pos.y,
+    pos.z,
+    ori.roll,
+    ori.pitch,
+    ori.yaw,
+    dim.width,
+    dim.length,
+    dim.height,
+    { ...ROAD_MARKING_COLOR[roadMarking.classification.monochrome_color], a: 1 },
+  );
 
-  // Set option for dashed lines
-  const options = {
-    dashed: false,
-    arrows: false,
-    invertArrows: false,
-  };
+  function buildAxes(): ArrowPrimitive[] {
+    if (!(config?.showAxes ?? false)) {
+      return [];
+    }
+    return buildObjectAxes(roadMarking);
+  }
 
   return {
     timestamp: time,
     frame_id,
     id: generateSceneEntityId(id_prefix, roadMarking.id.value),
     lifetime: { sec: 0, nsec: 0 },
     frame_locked: true,
-    triangles: [pointListToTriangleListPrimitive(roadMarkingPoints, color, options)],
+    cubes: [cube],
+    arrows: buildAxes(),
     metadata: buildRoadMarkingMetadata(roadMarking),
   };
 }

src/converters/groundTruth/sceneUpdateConverter.ts

@@ -7,7 +7,12 @@
  */
 
 import { Color, CubePrimitive, ModelPrimitive, Vector3, ArrowPrimitive } from "@foxglove/schemas";
-import { StationaryObject, MovingObject, TrafficLight } from "@lichtblick/asam-osi-types";
+import {
+  StationaryObject,
+  MovingObject,
+  TrafficLight,
+  RoadMarking,
+} from "@lichtblick/asam-osi-types";
 import { ColorCode } from "@utils/helper";
 import { eulerToQuaternion, quaternionMultiplication } from "@utils/math";
 import { DeepRequired } from "ts-essentials";
@@ -81,7 +86,8 @@ export function buildAxisArrow(
   osiObject:
     | DeepRequired<StationaryObject>
     | DeepRequired<MovingObject>
-    | DeepRequired<TrafficLight>,
+    | DeepRequired<TrafficLight>
+    | DeepRequired<RoadMarking>,
   axis_color: Color,
   orientation: Vector3 = { x: 0, y: 0, z: 0 },
   shaft_length: number,
@@ -118,7 +124,8 @@ export function buildObjectAxes(
   osiObject:
     | DeepRequired<StationaryObject>
     | DeepRequired<MovingObject>
-    | DeepRequired<TrafficLight>,
+    | DeepRequired<TrafficLight>
+    | DeepRequired<RoadMarking>,
   shaft_length = 0.154,
   shaft_diameter = 0.02,
   head_length = 0.046,

src/features/roadmarkings/index.ts

@@ -0,0 +1,250 @@
+import { buildRoadMarkingEntity } from "@features/roadmarkings";
+import {
+  RoadMarking,
+  RoadMarking_Classification_Color,
+  RoadMarking_Classification_Type,
+  TrafficSign_MainSign_Classification_Type,
+} from "@lichtblick/asam-osi-types";
+import { eulerToQuaternion } from "@utils/math";
+import { DeepRequired } from "ts-essentials";
+
+import { GroundTruthPanelSettings } from "@/converters/groundTruth/types";
+
+const DEFAULT_TEST_CONFIG: GroundTruthPanelSettings = {
+  caching: true,
+  showAxes: true,
+  showPhysicalLanes: true,
+  showLogicalLanes: false,
+  showReferenceLines: true,
+  showBoundingBox: true,
+  show3dModels: false,
+  defaultModelPath: "/opt/models/vehicles/",
+};
+
+function createRoadMarking(
+  overrides: Partial<{
+    id: number;
+    x: number;
+    y: number;
+    z: number;
+    roll: number;
+    pitch: number;
+    yaw: number;
+    width: number;
+    length: number;
+    height: number;
+    mainSignType: number;
+    color: number;
+  }> = {},
+): DeepRequired<RoadMarking> {
+  return {
+    id: { value: overrides.id ?? 1 },
+    base: {
+      position: {
+        x: overrides.x ?? 0,
+        y: overrides.y ?? 0,
+        z: overrides.z ?? 0,
+      },
+      orientation: {
+        roll: overrides.roll ?? 0,
+        pitch: overrides.pitch ?? 0,
+        yaw: overrides.yaw ?? 0,
+      },
+      dimension: {
+        width: overrides.width ?? 4,
+        length: overrides.length ?? 0.3,
+        height: overrides.height ?? 0.004,
+      },
+    },
+    classification: {
+      type: RoadMarking_Classification_Type.PAINTED_TRAFFIC_SIGN,
+      monochrome_color: overrides.color ?? RoadMarking_Classification_Color.WHITE,
+      traffic_main_sign_type:
+        overrides.mainSignType ?? TrafficSign_MainSign_Classification_Type.STOP,
+      value: 0,
+      color_description: "",
+      traffic_supplementary_sign_type: 0,
+    },
+  } as unknown as DeepRequired<RoadMarking>;
+}
+
+const PREFIX = "road_marking";
+const FRAME = "global";
+const TIME = { sec: 0, nsec: 0 };
+
+describe("buildRoadMarkingEntity", () => {
+  describe("filtering", () => {
+    it("returns undefined for non-STOP road markings", () => {
+      const marking = createRoadMarking({ mainSignType: 0 });
+      const result = buildRoadMarkingEntity(marking, PREFIX, FRAME, TIME, undefined);
+      expect(result).toBeUndefined();
+    });
+
+    it("returns entity for STOP road markings", () => {
+      const marking = createRoadMarking();
+      const result = buildRoadMarkingEntity(marking, PREFIX, FRAME, TIME, undefined);
+      expect(result).toBeDefined();
+      expect(result!.id).toBe("road_marking_1");
+    });
+  });
+
+  describe("position and centering", () => {
+    it("places the cube centered on base.position", () => {
+      const marking = createRoadMarking({ x: 100, y: 200, z: 5 });
+      const result = buildRoadMarkingEntity(marking, PREFIX, FRAME, TIME, undefined);
+      expect(result).toBeDefined();
+
+      const cube = result!.cubes![0]!;
+      expect(cube.pose!.position).toEqual({ x: 100, y: 200, z: 5 });
+    });
+
+    it("does not offset the cube from base.position (old bug: started at edge)", () => {
+      const marking = createRoadMarking({ x: 50, y: 30, z: 0, length: 10 });
+      const result = buildRoadMarkingEntity(marking, PREFIX, FRAME, TIME, undefined);
+      expect(result).toBeDefined();
+
+      const cube = result!.cubes![0]!;
+      // The cube must be centered at position, not shifted by length/height
+      expect(cube.pose!.position).toEqual({ x: 50, y: 30, z: 0 });
+    });
+  });
+
+  describe("orientation", () => {
+    it("applies orientation from base.orientation", () => {
+      const yaw = Math.PI / 4;
+      const marking = createRoadMarking({ yaw });
+      const result = buildRoadMarkingEntity(marking, PREFIX, FRAME, TIME, undefined);
+      expect(result).toBeDefined();
+
+      const cube = result!.cubes![0]!;
+      const expected = eulerToQuaternion(0, 0, yaw);
+      expect(cube.pose!.orientation!.w).toBeCloseTo(expected.w, 6);
+      expect(cube.pose!.orientation!.x).toBeCloseTo(expected.x, 6);
+      expect(cube.pose!.orientation!.y).toBeCloseTo(expected.y, 6);
+      expect(cube.pose!.orientation!.z).toBeCloseTo(expected.z, 6);
+    });
+
+    it("applies combined roll, pitch, yaw orientation", () => {
+      const marking = createRoadMarking({
+        roll: 0.1,
+        pitch: -Math.PI / 2,
+        yaw: Math.PI / 3,
+      });
+      const result = buildRoadMarkingEntity(marking, PREFIX, FRAME, TIME, undefined);
+      expect(result).toBeDefined();
+
+      const cube = result!.cubes![0]!;
+      const expected = eulerToQuaternion(0.1, -Math.PI / 2, Math.PI / 3);
+      expect(cube.pose!.orientation!.w).toBeCloseTo(expected.w, 6);
+      expect(cube.pose!.orientation!.x).toBeCloseTo(expected.x, 6);
+      expect(cube.pose!.orientation!.y).toBeCloseTo(expected.y, 6);
+      expect(cube.pose!.orientation!.z).toBeCloseTo(expected.z, 6);
+    });
+  });
+
+  describe("dimension mapping", () => {
+    it("maps OSI dimensions correctly to cube size", () => {
+      // objectToCubePrimitive: size.x=length, size.y=width, size.z=height
+      const marking = createRoadMarking({
+        length: 0.3,
+        width: 4.0,
+        height: 0.004,
+      });
+      const result = buildRoadMarkingEntity(marking, PREFIX, FRAME, TIME, undefined);
+      expect(result).toBeDefined();
+
+      const cube = result!.cubes![0]!;
+      expect(cube.size!.x).toBeCloseTo(0.3); // length (along local x)
+      expect(cube.size!.y).toBeCloseTo(4.0); // width (along local y)
+      expect(cube.size!.z).toBeCloseTo(0.004); // height (along local z)
+    });
+
+    it("passes through non-trivial orientation as quaternion", () => {
+      const marking = createRoadMarking({
+        length: 0.3,
+        width: 4.0,
+        height: 0.004,
+        roll: 0.1,
+        pitch: 0.2,
+        yaw: 0.3,
+      });
+      const result = buildRoadMarkingEntity(marking, PREFIX, FRAME, TIME, undefined);
+      expect(result).toBeDefined();
+
+      const cube = result!.cubes![0]!;
+      const expected = eulerToQuaternion(0.1, 0.2, 0.3);
+      expect(cube.pose!.orientation).toEqual(expected);
+      expect(cube.size).toEqual({ x: 0.3, y: 4.0, z: 0.004 });
+    });
+  });
+
+  describe("color", () => {
+    it("uses the classification color", () => {
+      const marking = createRoadMarking({ color: RoadMarking_Classification_Color.RED });
+      const result = buildRoadMarkingEntity(marking, PREFIX, FRAME, TIME, undefined);
+      expect(result).toBeDefined();
+
+      const cube = result!.cubes![0]!;
+      expect(cube.color!.a).toBe(1);
+      // RED color should have high r component
+      expect(cube.color!.r).toBeGreaterThan(0.5);
+    });
+  });
+
+  describe("metadata", () => {
+    it("includes type, color, width, and height in metadata", () => {
+      const marking = createRoadMarking({ width: 5, height: 0.3 });
+      const result = buildRoadMarkingEntity(marking, PREFIX, FRAME, TIME, undefined);
+      expect(result).toBeDefined();
+
+      const metadata = result!.metadata!;
+      expect(metadata).toEqual(
+        expect.arrayContaining([
+          { key: "type", value: "PAINTED_TRAFFIC_SIGN" },
+          { key: "color", value: "WHITE" },
+          { key: "width", value: "5" },
+          { key: "height", value: "0.3" },
+        ]),
+      );
+    });
+  });
+
+  describe("scene entity properties", () => {
+    it("sets correct frame_id, timestamp, and frame_locked", () => {
+      const marking = createRoadMarking({ id: 42 });
+      const result = buildRoadMarkingEntity(marking, PREFIX, "test_frame", { sec: 10, nsec: 500 }, undefined);
+      expect(result).toBeDefined();
+
+      expect(result!.frame_id).toBe("test_frame");
+      expect(result!.timestamp).toEqual({ sec: 10, nsec: 500 });
+      expect(result!.frame_locked).toBe(true);
+      expect(result!.id).toBe("road_marking_42");
+    });
+  });
+
+  describe("axes display", () => {
+    it("shows axes when config.showAxes is true", () => {
+      const marking = createRoadMarking();
+      const config = { ...DEFAULT_TEST_CONFIG, showAxes: true };
+      const result = buildRoadMarkingEntity(marking, PREFIX, FRAME, TIME, config);
+      expect(result).toBeDefined();
+      expect(result!.arrows).toBeDefined();
+      expect(result!.arrows!.length).toBe(3);
+    });
+
+    it("hides axes when config.showAxes is false", () => {
+      const marking = createRoadMarking();
+      const config = { ...DEFAULT_TEST_CONFIG, showAxes: false };
+      const result = buildRoadMarkingEntity(marking, PREFIX, FRAME, TIME, config);
+      expect(result).toBeDefined();
+      expect(result!.arrows).toEqual([]);
+    });
+
+    it("hides axes when no config is provided", () => {
+      const marking = createRoadMarking();
+      const result = buildRoadMarkingEntity(marking, PREFIX, FRAME, TIME, undefined);
+      expect(result).toBeDefined();
+      expect(result!.arrows).toEqual([]);
+    });
+  });
+});