Fix & update yuyv/uyvy encodings

packages/den/image/decodings.ts

@@ -11,6 +11,10 @@
 //   found at http://www.apache.org/licenses/LICENSE-2.0
 //   You may not use this file except in compliance with the License.
 
+/**
+ * Adapted from:
+ * https://github.com/ros2/rviz/blob/e8838720e57b56cc0d50e05b00b28bee3c5dc9ee/rviz_default_plugins/src/rviz_default_plugins/displays/image/ros_image_texture.cpp#L332
+ */
 function yuvToRGBA8(
   y1: number,
   u: number,
@@ -20,20 +24,20 @@ function yuvToRGBA8(
   output: Uint8ClampedArray,
 ): void {
   // rgba
-  output[c] = y1 + 1.402 * v;
-  output[c + 1] = y1 - 0.34414 * u - 0.71414 * v;
-  output[c + 2] = y1 + 1.772 * u;
+  output[c] = y1 + Math.trunc((1403 * v) / 1000);
+  output[c + 1] = y1 - Math.trunc((344 * u) / 1000) - Math.trunc((714 * v) / 1000);
+  output[c + 2] = y1 + Math.trunc((1770 * u) / 1000);
   output[c + 3] = 255;
 
   // rgba
-  output[c + 4] = y2 + 1.402 * v;
-  output[c + 5] = y2 - 0.34414 * u - 0.71414 * v;
-  output[c + 6] = y2 + 1.772 * u;
+  output[c + 4] = y2 + Math.trunc((1403 * v) / 1000);
+  output[c + 5] = y2 - Math.trunc((344 * u) / 1000) - Math.trunc((714 * v) / 1000);
+  output[c + 6] = y2 + Math.trunc((1770 * u) / 1000);
   output[c + 7] = 255;
 }
 
-export function decodeYUV(
-  yuv: Int8Array,
+export function decodeUYVY(
+  yuv: Uint8Array,
   width: number,
   height: number,
   output: Uint8ClampedArray,
@@ -54,9 +58,8 @@ export function decodeYUV(
   }
 }
 
-// change name in the future do something more distinct
 export function decodeYUYV(
-  yuyv: Int8Array,
+  yuyv: Uint8Array,
   width: number,
   height: number,
   output: Uint8ClampedArray,

packages/studio-base/src/panels/ThreeDeeRender/renderables/Images/decodeImage.ts

@@ -14,7 +14,7 @@ import {
   decodeMono8,
   decodeRGB8,
   decodeRGBA8,
-  decodeYUV,
+  decodeUYVY,
   decodeYUYV,
 } from "@foxglove/den/image";
 import { RawImage } from "@foxglove/schemas";
@@ -35,6 +35,10 @@ export type RawImageOptions = {
   maxValue?: number;
 };
 
+/**
+ * See also:
+ * https://github.com/ros2/common_interfaces/blob/366eea24ffce6c87f8860cbcd27f4863f46ad822/sensor_msgs/include/sensor_msgs/image_encodings.hpp
+ */
 export function decodeRawImage(
   image: RosImage | RawImage,
   options: RawImageOptions,
@@ -45,11 +49,12 @@ export function decodeRawImage(
   const rawData = image.data as Uint8Array;
   switch (encoding) {
     case "yuv422":
-    case "uyuv":
-      decodeYUV(image.data as Int8Array, width, height, output);
+    case "uyvy":
+      decodeUYVY(rawData, width, height, output);
       break;
+    case "yuv422_yuy2":
     case "yuyv":
-      decodeYUYV(image.data as Int8Array, width, height, output);
+      decodeYUYV(rawData, width, height, output);
       break;
     case "rgb8":
       decodeRGB8(rawData, width, height, output);

packages/studio-base/src/panels/ThreeDeeRender/stories/ImageMode/ImageMode.stories.tsx

@@ -785,3 +785,132 @@ export const LargeImage: StoryObj = {
     );
   },
 };
+
+function makeYUYV(width: number, height: number) {
+  const result = new Uint8Array(2 * width * height);
+  for (let r = 0; r < height; r++) {
+    for (let c = 0; c < width; c += 2) {
+      const y1 = r === c ? 255 : 127;
+      const y2 = r === c + 1 ? 255 : 127;
+      const u = Math.trunc(255 * (c / width));
+      const v = Math.trunc(255 * (r / height));
+      result[2 * (r * width + c) + 0] = y1;
+      result[2 * (r * width + c) + 1] = u;
+      result[2 * (r * width + c) + 2] = y2;
+      result[2 * (r * width + c) + 3] = v;
+    }
+  }
+  return result;
+}
+
+function makeUYVY(width: number, height: number) {
+  const result = makeYUYV(width, height);
+  for (let i = 0; i < result.length; i += 4) {
+    const [y1, u, y2, v] = result.subarray(i, i + 4);
+    result[i + 0] = u!;
+    result[i + 1] = y1!;
+    result[i + 2] = v!;
+    result[i + 3] = y2!;
+  }
+  return result;
+}
+
+export const YUYV: StoryObj = {
+  render: function Story() {
+    const imageTopic = "camera";
+    const topics: Topic[] = useMemo(
+      () => [{ name: imageTopic, schemaName: "foxglove.RawImage" }],
+      [],
+    );
+
+    const width = 200;
+    const height = 150;
+    const cameraMessage: MessageEvent<RawImage> = {
+      topic: imageTopic,
+      receiveTime: { sec: 10, nsec: 0 },
+      message: {
+        timestamp: { sec: 10, nsec: 0 },
+        frame_id: "camera",
+        width,
+        height,
+        encoding: "yuyv",
+        step: width * 2,
+        data: makeYUYV(width, height),
+      },
+      schemaName: "foxglove.RawImage",
+      sizeInBytes: 0,
+    };
+
+    const fixture: Fixture = {
+      topics,
+      frame: {
+        [imageTopic]: [cameraMessage],
+      },
+      capabilities: [],
+      activeData: {
+        currentTime: { sec: 0, nsec: 0 },
+      },
+    };
+
+    return (
+      <PanelSetup fixture={fixture}>
+        <ImagePanel
+          overrideConfig={{
+            ...ImagePanel.defaultConfig,
+            imageMode: { imageTopic },
+          }}
+        />
+      </PanelSetup>
+    );
+  },
+};
+
+export const UYVY: StoryObj = {
+  render: function Story() {
+    const imageTopic = "camera";
+    const topics: Topic[] = useMemo(
+      () => [{ name: imageTopic, schemaName: "foxglove.RawImage" }],
+      [],
+    );
+
+    const width = 200;
+    const height = 150;
+    const cameraMessage: MessageEvent<RawImage> = {
+      topic: imageTopic,
+      receiveTime: { sec: 10, nsec: 0 },
+      message: {
+        timestamp: { sec: 10, nsec: 0 },
+        frame_id: "camera",
+        width,
+        height,
+        encoding: "uyvy",
+        step: width * 2,
+        data: makeUYVY(width, height),
+      },
+      schemaName: "foxglove.RawImage",
+      sizeInBytes: 0,
+    };
+
+    const fixture: Fixture = {
+      topics,
+      frame: {
+        [imageTopic]: [cameraMessage],
+      },
+      capabilities: [],
+      activeData: {
+        currentTime: { sec: 0, nsec: 0 },
+      },
+    };
+
+    return (
+      <PanelSetup fixture={fixture}>
+        <ImagePanel
+          overrideConfig={{
+            ...ImagePanel.defaultConfig,
+            imageMode: { imageTopic },
+          }}
+        />
+      </PanelSetup>
+    );
+  },
+};