diff --git a/packages/den/image/decodings.ts b/packages/den/image/decodings.ts index 9b9727cef7..7108b1b5f4 100644 --- a/packages/den/image/decodings.ts +++ b/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, diff --git a/packages/studio-base/src/panels/ThreeDeeRender/renderables/Images/decodeImage.ts b/packages/studio-base/src/panels/ThreeDeeRender/renderables/Images/decodeImage.ts index 1ddc597b8d..228d201e87 100644 --- a/packages/studio-base/src/panels/ThreeDeeRender/renderables/Images/decodeImage.ts +++ b/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); diff --git a/packages/studio-base/src/panels/ThreeDeeRender/stories/ImageMode/ImageMode.stories.tsx b/packages/studio-base/src/panels/ThreeDeeRender/stories/ImageMode/ImageMode.stories.tsx index 3ea95ebde4..2917d872c0 100644 --- a/packages/studio-base/src/panels/ThreeDeeRender/stories/ImageMode/ImageMode.stories.tsx +++ b/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 = { + 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 ( + + + + ); + }, +}; + +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 = { + 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 ( + + + + ); + }, +};