diff --git a/package-lock.json b/package-lock.json index 13401d6..c7a84b3 100644 --- a/package-lock.json +++ b/package-lock.json @@ -1,12 +1,12 @@ { "name": "@ioai/rosview", - "version": "1.5.4", + "version": "1.6.0", "lockfileVersion": 3, "requires": true, "packages": { "": { "name": "@ioai/rosview", - "version": "1.5.4", + "version": "1.6.0", "license": "MIT", "devDependencies": { "@eslint/js": "^9.39.4", diff --git a/package.json b/package.json index a9852f8..680639b 100644 --- a/package.json +++ b/package.json @@ -1,6 +1,6 @@ { "name": "@ioai/rosview", - "version": "1.5.4", + "version": "1.6.0", "description": "High-performance robotics data visualization for MCAP, ROS bag, ROS2 db3, HDF5 and BVH — embeddable React component and standalone SPA", "keywords": [ "ros", diff --git a/scripts/gen-e2e-fixtures.mjs b/scripts/gen-e2e-fixtures.mjs index 309ecde..e8ada66 100644 --- a/scripts/gen-e2e-fixtures.mjs +++ b/scripts/gen-e2e-fixtures.mjs @@ -29,6 +29,7 @@ runNode('gen-test-mcap.mjs'); runNode('gen-test-mcap-pose.mjs'); runNode('gen-test-mcap-3cam.mjs'); runNode('gen-test-mcap-h264.mjs'); +runNode('gen-test-mcap-compressed-depth.mjs'); runPython('gen-test-hdf5.py'); runNode('gen-test-bvh.mjs'); diff --git a/scripts/gen-test-mcap-compressed-depth.mjs b/scripts/gen-test-mcap-compressed-depth.mjs new file mode 100644 index 0000000..e000c4b --- /dev/null +++ b/scripts/gen-test-mcap-compressed-depth.mjs @@ -0,0 +1,42 @@ +/** + * Minimal MCAP with 16UC1 compressedDepth CompressedImage messages for E2E. + */ +import fs from 'node:fs'; +import path from 'node:path'; +import { fileURLToPath } from 'node:url'; +import { + createIndexedMcapWriter, + encodeCompressedImageCdr, + readFixture, + registerCompressedImageChannel, + writeExample, +} from './mcap-fixture-utils.mjs'; + +const __dirname = path.dirname(fileURLToPath(import.meta.url)); +const payload = readFixture('image/compressed-depth-16uc1.bin'); +const format = fs.readFileSync( + path.join(__dirname, '../test-fixtures/image/compressed-depth-16uc1.format.txt'), + 'utf8', +).trim(); + +const { writer, writable } = await createIndexedMcapWriter(); + +const channelId = await registerCompressedImageChannel( + '/camera/realsense2_camera/aligned_depth_to_color/image_raw/compressed_depth', + writer, +); + +const frames = [1_000_000_000n, 2_000_000_000n, 3_000_000_000n]; +for (const [idx, ts] of frames.entries()) { + const stamp = { sec: Number(ts / 1_000_000_000n), nsec: Number(ts % 1_000_000_000n) }; + await writer.addMessage({ + channelId, + sequence: idx + 1, + logTime: ts, + publishTime: ts, + data: encodeCompressedImageCdr(stamp, format, payload), + }); +} + +await writer.end(); +writeExample('test_compressed_depth.mcap', writable.getBuffer()); diff --git a/src/features/panels/Image/ImagePanel.tsx b/src/features/panels/Image/ImagePanel.tsx index b0be675..2af9d4e 100644 --- a/src/features/panels/Image/ImagePanel.tsx +++ b/src/features/panels/Image/ImagePanel.tsx @@ -16,6 +16,7 @@ import { } from './core/imageTypes'; import { repairH264Seek } from './core/h264SeekRepair'; import { isH264MessageEvent, toWorkerFrame } from './core/messageFrameAdapter'; +import { applyDepthTopicPreset } from './core/depthColorDefaults'; import type { ImageConfig } from './defaults'; import { TopicQuickPicker } from '../framework/TopicQuickPicker'; import ImageRenderWorkerClass from './core/ImageRender.worker.ts?worker&inline'; @@ -306,7 +307,7 @@ export const ImagePanel: React.FC = (props) => {
setConfig((prev) => ({ ...prev, topic: nextTopic }))} + onChange={(nextTopic) => setConfig((prev) => applyDepthTopicPreset(nextTopic, prev))} typeIncludes={[...IMAGE_PANEL_TOPIC_INCLUDES]} placeholder={formatMessage({ id: 'panels.framework.topicPicker.imagePlaceholder' })} className="min-w-0 flex-1" diff --git a/src/features/panels/Image/ImagePanelSettings.tsx b/src/features/panels/Image/ImagePanelSettings.tsx index d9026e1..782acfd 100644 --- a/src/features/panels/Image/ImagePanelSettings.tsx +++ b/src/features/panels/Image/ImagePanelSettings.tsx @@ -13,7 +13,8 @@ import { } from '../framework/settings'; import { messageBus } from '@/core/pipeline/messageBus'; import { useTopicSeq } from '@/core/pipeline/useMessageBus'; -import { isRawImageMessage, isRawImageTopicSchema, IMAGE_PANEL_TOPIC_INCLUDES } from './core/imageTypes'; +import { isRawImageMessage, isRawImageTopicSchema, isCompressedImageMessage, depthEncodingFromFormat, IMAGE_PANEL_TOPIC_INCLUDES } from './core/imageTypes'; +import { applyDepthTopicPreset, defaultDepthMaxValue, defaultDepthMinValue } from './core/depthColorDefaults'; import type { ImageConfig } from './defaults'; const DEPTH_ENCODINGS = new Set(['mono16', '16uc1', '32fc1']); @@ -44,6 +45,9 @@ function useLastFrameEncoding(topic: string): string | null { if (isRawImageMessage(msg)) { return msg.encoding.trim().toLowerCase(); } + if (isCompressedImageMessage(msg)) { + return depthEncodingFromFormat(msg.format); + } return null; } @@ -99,6 +103,18 @@ export function ImagePanelSettings({ encodingForDepthSliders != null && DEPTH_ENCODINGS.has(encodingForDepthSliders) ? depthSliderBounds(encodingForDepthSliders) : { min: 0, max: 65535 }; + const sliderDefaultMax = + encodingForDepthSliders != null + ? defaultDepthMaxValue(encodingForDepthSliders, config.topic) + : sliderBounds.max; + const sliderDefaultMin = + encodingForDepthSliders != null + ? defaultDepthMinValue(encodingForDepthSliders, config.topic) + : sliderBounds.min; + + const applyTopicChange = (topic: string) => { + setConfig(applyDepthTopicPreset(topic, config)); + }; return (
@@ -109,7 +125,7 @@ export function ImagePanelSettings({ > setConfig({ ...config, topic })} + onChange={applyTopicChange} topics={topics} typeIncludes={[...IMAGE_PANEL_TOPIC_INCLUDES]} placeholder={formatMessage({ id: 'panels.image.settings.field.topic.placeholder' })} @@ -295,7 +311,7 @@ export function ImagePanelSettings({ )} > setConfig({ ...config, minValue })} min={sliderBounds.min} max={sliderBounds.max} @@ -310,7 +326,7 @@ export function ImagePanelSettings({ )} > setConfig({ ...config, maxValue })} min={sliderBounds.min} max={sliderBounds.max} diff --git a/src/features/panels/Image/core/ImageRender.worker.ts b/src/features/panels/Image/core/ImageRender.worker.ts index 8166478..cf3525b 100644 --- a/src/features/panels/Image/core/ImageRender.worker.ts +++ b/src/features/panels/Image/core/ImageRender.worker.ts @@ -1,11 +1,13 @@ /// import type { Time } from '@/core/types/ros'; +import { decodeCompressedDepth } from './compressedDepthDecoder'; import { decodeRawImage } from './rawDecoders'; import { getH264ChunkType } from './h264'; import { withTimeout } from './asyncTimeout'; import { getCompressedKind, + isCompressedDepthFormat, normalizeCompressedMime, type ImageSurfaceStatus, } from './imageTypes'; @@ -396,11 +398,27 @@ class ImageRenderWorkerRuntime { this.#emitStatus({ phase: 'decoding', receiveTime: frame.receiveTime }); try { if (frame.kind === 'compressed') { - const kind = getCompressedKind(frame.format); const bytes = ensureOwnedBytes(frame.data); if (bytes.byteLength === 0) { throw new Error(`Compressed image payload is empty: ${frame.format}`); } + + // ROS compressedDepth: PNG → 16UC1/32FC1, then same colormap path as RawImage. + if (isCompressedDepthFormat(frame.format)) { + const decoded = await decodeCompressedDepth(bytes, frame.format); + this.#renderRawFrame({ + receiveTime: frame.receiveTime, + encoding: decoded.encoding, + width: decoded.width, + height: decoded.height, + step: decoded.step, + isBigEndian: decoded.isBigEndian, + data: ensureOwnedBytes(decoded.data), + }); + return; + } + + const kind = getCompressedKind(frame.format); const sortKey = timeToKey(frame.receiveTime); if (kind === 'h264') { @@ -471,52 +489,14 @@ class ImageRenderWorkerRuntime { // Raw frame const bytes = ensureOwnedBytes(frame.data); - const pixelBytes = frame.width * frame.height * 4; - let rgba = this.#rawRgba; - if (!rgba || rgba.length !== pixelBytes) { - rgba = new Uint8ClampedArray(pixelBytes); - this.#rawRgba = rgba; - } - if (!this.#rawImageData || this.#rawImageData.width !== frame.width || this.#rawImageData.height !== frame.height) { - this.#rawImageData = new ImageData(rgba, frame.width, frame.height); - } - - const step = frame.step ?? (frame.width * bytesPerPixel(frame.encoding)); - const isBigEndian = frame.isBigEndian ?? false; - - decodeRawImage( - { - encoding: frame.encoding, - width: frame.width, - height: frame.height, - step, - is_bigendian: isBigEndian, - data: bytes, - }, - rgba, - this.#rawDecodeOptions, - ); - - // Store source bytes so rawDecodeOptions changes can re-decode immediately. - this.#disposeCachedBitmap(); - this.#cachedFrame = { - kind: 'raw', - width: frame.width, - height: frame.height, - encoding: frame.encoding, - step, - isBigEndian, - data: bytes, + this.#renderRawFrame({ receiveTime: frame.receiveTime, - }; - - this.#drawRawImageData(frame.width, frame.height); - this.#emitStatus({ - phase: 'ready', + encoding: frame.encoding, width: frame.width, height: frame.height, - encoding: frame.encoding, - receiveTime: frame.receiveTime, + step: frame.step ?? (frame.width * bytesPerPixel(frame.encoding)), + isBigEndian: frame.isBigEndian ?? false, + data: bytes, }); } catch (error) { this.#haltUntilReset = true; @@ -527,6 +507,60 @@ class ImageRenderWorkerRuntime { } } + #renderRawFrame(frame: { + receiveTime: Time; + encoding: string; + width: number; + height: number; + step: number; + isBigEndian: boolean; + data: Uint8Array; + }): void { + const pixelBytes = frame.width * frame.height * 4; + let rgba = this.#rawRgba; + if (!rgba || rgba.length !== pixelBytes) { + rgba = new Uint8ClampedArray(pixelBytes); + this.#rawRgba = rgba; + } + if (!this.#rawImageData || this.#rawImageData.width !== frame.width || this.#rawImageData.height !== frame.height) { + this.#rawImageData = new ImageData(rgba, frame.width, frame.height); + } + + decodeRawImage( + { + encoding: frame.encoding, + width: frame.width, + height: frame.height, + step: frame.step, + is_bigendian: frame.isBigEndian, + data: frame.data, + }, + rgba, + this.#rawDecodeOptions, + ); + + this.#disposeCachedBitmap(); + this.#cachedFrame = { + kind: 'raw', + width: frame.width, + height: frame.height, + encoding: frame.encoding, + step: frame.step, + isBigEndian: frame.isBigEndian, + data: frame.data, + receiveTime: frame.receiveTime, + }; + + this.#drawRawImageData(frame.width, frame.height); + this.#emitStatus({ + phase: 'ready', + width: frame.width, + height: frame.height, + encoding: frame.encoding, + receiveTime: frame.receiveTime, + }); + } + /** Re-decode the last raw frame with current rawDecodeOptions and redraw. */ #redrawRawCached(): void { const cached = this.#cachedFrame; @@ -611,7 +645,7 @@ class ImageRenderWorkerRuntime { data: Uint8Array, format: string, ): Promise { - const mime = normalizeCompressedMime(format); + const mime = normalizeCompressedMime(format, data); if (typeof ImageDecoder !== 'undefined') { let supported = this.#imageDecoderMimeSupported.get(mime); if (supported === undefined) { diff --git a/src/features/panels/Image/core/compressedDepthDecoder.test.ts b/src/features/panels/Image/core/compressedDepthDecoder.test.ts new file mode 100644 index 0000000..cb0c457 --- /dev/null +++ b/src/features/panels/Image/core/compressedDepthDecoder.test.ts @@ -0,0 +1,56 @@ +import fs from 'node:fs'; +import path from 'node:path'; +import { fileURLToPath } from 'node:url'; +import { describe, expect, it } from 'vitest'; +import { + decodeCompressedDepth, + findPngOffset, + parseCompressedDepthHeader, +} from './compressedDepthDecoder'; + +const __dirname = path.dirname(fileURLToPath(import.meta.url)); +const FIXTURE_16UC1 = path.join(__dirname, '../../../../../test-fixtures/image/compressed-depth-16uc1.bin'); +const FIXTURE_32FC1 = path.join(__dirname, '../../../../../test-fixtures/image/compressed-depth-32fc1.bin'); +const FIXTURE_FORMAT_16UC1 = '16UC1; compressedDepth'; + +describe('compressedDepthDecoder', () => { + it('finds the PNG signature after the ROS header', () => { + const fixture = fs.readFileSync(FIXTURE_16UC1); + expect(findPngOffset(fixture)).toBe(12); + }); + + it('decodes a RealSense 16UC1 compressed depth fixture', async () => { + const fixture = fs.readFileSync(FIXTURE_16UC1); + const decoded = await decodeCompressedDepth(fixture, FIXTURE_FORMAT_16UC1); + + expect(decoded.encoding).toBe('16uc1'); + expect(decoded.width).toBe(640); + expect(decoded.height).toBe(480); + expect(decoded.step).toBe(640 * 2); + expect(decoded.isBigEndian).toBe(false); + + const view = new DataView(decoded.data.buffer, decoded.data.byteOffset, decoded.data.byteLength); + expect(view.getUint16(0, true)).toBe(3524); + expect(view.getUint16(2, true)).toBe(3565); + }); + + it('dequantizes 32FC1 compressed depth PNG payloads', async () => { + const payload = fs.readFileSync(FIXTURE_32FC1); + const header = parseCompressedDepthHeader(payload, findPngOffset(payload)); + expect(header.depthParam).toEqual([10, 5]); + + const decoded = await decodeCompressedDepth(payload, '32FC1; compressedDepth'); + expect(decoded.encoding).toBe('32fc1'); + expect(decoded.width).toBe(2); + expect(decoded.height).toBe(1); + + const view = new DataView(decoded.data.buffer, decoded.data.byteOffset, decoded.data.byteLength); + expect(view.getFloat32(0, true)).toBeCloseTo(10 / (100 - 5), 5); + expect(view.getFloat32(4, true)).toBeCloseTo(10 / (200 - 5), 5); + }); + + it('rejects unsupported RVL compressed depth formats', async () => { + const fixture = fs.readFileSync(FIXTURE_16UC1); + await expect(decodeCompressedDepth(fixture, '16UC1; compressedDepth rvl')).rejects.toThrow(/RVL/); + }); +}); diff --git a/src/features/panels/Image/core/compressedDepthDecoder.ts b/src/features/panels/Image/core/compressedDepthDecoder.ts new file mode 100644 index 0000000..50f6352 --- /dev/null +++ b/src/features/panels/Image/core/compressedDepthDecoder.ts @@ -0,0 +1,131 @@ +/** + * Decode `sensor_msgs/CompressedImage` payloads with `compressedDepth` transport. + * + * Layout: optional 12-byte ROS header (quant params) + PNG (16-bit grayscale). + * Supported encodings: 16UC1 (direct mm samples) and 32FC1 (PNG uint16 dequantized to float). + */ + +import { decodeGrayscale16Png, findPngSignatureOffset } from './grayscale16PngDecoder'; +import { + depthEncodingFromFormat, + isCompressedDepthFormat, + parseCompressedImageFormat, + type DepthImageEncoding, +} from './imageTypes'; + +/** Bytes before PNG in standard ROS compressedDepth messages. */ +const ROS_COMPRESSED_DEPTH_HEADER_SIZE = 12; + +export interface CompressedDepthHeader { + compressionFormat: number; + depthParam: [number, number]; +} + +/** Raw image bytes ready for the same path as `sensor_msgs/Image`. */ +export interface DecodedCompressedDepth { + encoding: DepthImageEncoding; + width: number; + height: number; + step: number; + isBigEndian: false; + data: Uint8Array; +} + +export function findPngOffset(data: Uint8Array): number { + return findPngSignatureOffset(data); +} + +export function parseCompressedDepthHeader(data: Uint8Array, pngOffset: number): CompressedDepthHeader { + if (pngOffset < 4) { + return { compressionFormat: 0, depthParam: [0, 0] }; + } + const view = new DataView(data.buffer, data.byteOffset, data.byteLength); + return { + compressionFormat: view.getInt32(0, true), + depthParam: [view.getFloat32(4, true), view.getFloat32(8, true)], + }; +} + +async function decodePngPayload(data: Uint8Array): Promise<{ width: number; height: number; data: Uint8Array }> { + const pngOffset = findPngOffset(data); + if (pngOffset < 0) { + throw new Error('Compressed depth payload does not contain a PNG signature'); + } + const decoded = await decodeGrayscale16Png(data.subarray(pngOffset)); + return { + width: decoded.width, + height: decoded.height, + data: decoded.data, + }; +} + +async function decode16uc1(data: Uint8Array): Promise { + const { width, height, data: pixelBytes } = await decodePngPayload(data); + return { + encoding: '16uc1', + width, + height, + step: width * 2, + isBigEndian: false, + data: pixelBytes, + }; +} + +async function decode32fc1(data: Uint8Array, header: CompressedDepthHeader): Promise { + const { width, height, data: pixelBytes } = await decodePngPayload(data); + const [depthQuantA, depthQuantB] = header.depthParam; + const out = new Uint8Array(width * height * 4); + const outView = new DataView(out.buffer, out.byteOffset, out.byteLength); + const sampleView = new DataView(pixelBytes.buffer, pixelBytes.byteOffset, pixelBytes.byteLength); + + for (let i = 0; i < width * height; i++) { + const raw = sampleView.getUint16(i * 2, true); + let depth = 0; + if (raw !== 0) { + depth = depthQuantA / (raw - depthQuantB); + if (!Number.isFinite(depth)) { + depth = 0; + } + } + outView.setFloat32(i * 4, depth, true); + } + + return { + encoding: '32fc1', + width, + height, + step: width * 4, + isBigEndian: false, + data: out, + }; +} + +export async function decodeCompressedDepth(data: Uint8Array, format: string): Promise { + if (!isCompressedDepthFormat(format)) { + throw new Error(`Not a compressed depth format: ${format}`); + } + + const parsed = parseCompressedImageFormat(format); + if (parsed.depthCodec === 'rvl') { + throw new Error('RVL compressed depth is not supported yet'); + } + + const encoding = depthEncodingFromFormat(format); + if (!encoding) { + throw new Error(`Unsupported compressed depth encoding in format: ${format}`); + } + + const pngOffset = findPngOffset(data); + if (pngOffset < 0) { + throw new Error('Compressed depth payload is missing PNG data'); + } + if (pngOffset < ROS_COMPRESSED_DEPTH_HEADER_SIZE && data.byteLength < ROS_COMPRESSED_DEPTH_HEADER_SIZE) { + throw new Error('Compressed depth payload is too small'); + } + + const header = parseCompressedDepthHeader(data, pngOffset); + if (encoding === '16uc1') { + return decode16uc1(data); + } + return decode32fc1(data, header); +} diff --git a/src/features/panels/Image/core/depthColorDefaults.test.ts b/src/features/panels/Image/core/depthColorDefaults.test.ts new file mode 100644 index 0000000..ccb051b --- /dev/null +++ b/src/features/panels/Image/core/depthColorDefaults.test.ts @@ -0,0 +1,89 @@ +import { describe, expect, it } from 'vitest'; +import { + DEFAULT_DEPTH_16UC1_COLOR_MAX, + DEFAULT_DEPTH_16UC1_COLOR_MIN, + applyDepthTopicPreset, + defaultDepthMinValue, + getDepthColormapDefaults, + isDepthTopicName, + resolveDepthColorRange, +} from './depthColorDefaults'; +import { defaultImageConfig } from '../defaults'; + +describe('getDepthColormapDefaults', () => { + it('returns depth topic preset', () => { + expect(getDepthColormapDefaults('/camera/depth/image_raw/compressed')).toEqual({ + colorMode: 'colormap', + colorMap: 'turbo', + minValue: DEFAULT_DEPTH_16UC1_COLOR_MIN, + maxValue: DEFAULT_DEPTH_16UC1_COLOR_MAX, + }); + }); + + it('returns wrist preset for non-depth wrist topics', () => { + expect(getDepthColormapDefaults('/robot/wrist_cam/image')).toMatchObject({ + minValue: 0, + maxValue: 1000, + }); + }); + + it('returns null for unrelated topics', () => { + expect(getDepthColormapDefaults('/camera/rgb/image_raw')).toBeNull(); + }); +}); + +describe('resolveDepthColorRange', () => { + it('uses ROS-style defaults for 16uc1 when unset', () => { + expect(resolveDepthColorRange('16uc1', {})).toEqual({ + minValue: DEFAULT_DEPTH_16UC1_COLOR_MIN, + maxValue: DEFAULT_DEPTH_16UC1_COLOR_MAX, + }); + }); + + it('respects explicit user overrides', () => { + expect(resolveDepthColorRange('16uc1', { minValue: 0, maxValue: 5000 })).toEqual({ + minValue: 0, + maxValue: 5000, + }); + }); + + it('uses topic preset when topic is provided', () => { + expect(resolveDepthColorRange('mono16', {}, '/depth/compressed')).toEqual({ + minValue: DEFAULT_DEPTH_16UC1_COLOR_MIN, + maxValue: DEFAULT_DEPTH_16UC1_COLOR_MAX, + }); + }); +}); + +describe('isDepthTopicName', () => { + it('matches common depth topic patterns', () => { + expect(isDepthTopicName('/camera/aligned_depth_to_color/image_raw')).toBe(true); + expect(isDepthTopicName('/rgb/image_raw')).toBe(false); + }); +}); + +describe('defaultDepthMinValue', () => { + it('returns 200 for depth encodings without an explicit topic preset', () => { + expect(defaultDepthMinValue('16UC1')).toBe(DEFAULT_DEPTH_16UC1_COLOR_MIN); + }); +}); + +describe('applyDepthTopicPreset', () => { + it('merges depth colormap preset when topic matches', () => { + const next = applyDepthTopicPreset('/camera/depth/compressed', { + ...defaultImageConfig(), + topic: '', + colorMode: 'rgb', + }); + expect(next.topic).toBe('/camera/depth/compressed'); + expect(next.colorMode).toBe('colormap'); + expect(next.minValue).toBe(DEFAULT_DEPTH_16UC1_COLOR_MIN); + expect(next.maxValue).toBe(DEFAULT_DEPTH_16UC1_COLOR_MAX); + }); + + it('only updates topic for non-depth sources', () => { + const base = { ...defaultImageConfig(), colorMode: 'rgb' as const, minValue: 42 }; + const next = applyDepthTopicPreset('/camera/rgb/image_raw', base); + expect(next).toEqual({ ...base, topic: '/camera/rgb/image_raw' }); + }); +}); diff --git a/src/features/panels/Image/core/depthColorDefaults.ts b/src/features/panels/Image/core/depthColorDefaults.ts new file mode 100644 index 0000000..250e568 --- /dev/null +++ b/src/features/panels/Image/core/depthColorDefaults.ts @@ -0,0 +1,127 @@ +/** + * Default colormap range and topic presets for depth image rendering. + * + * 16UC1 compressed depth is typically millimeters. Defaults follow ROS + * image_view / RViz conventions: min 200 mm filters near-field noise and + * invalid zeros; max 10000 mm (10 m) spans the full turbo colormap for + * indoor sensors. + */ + +import type { ImageConfig } from '../defaults'; +import type { RawImageDecodeOptions } from './imageColorMode'; + +/** Lower bound (mm) for 16UC1 turbo colormap when unset. */ +export const DEFAULT_DEPTH_16UC1_COLOR_MIN = 200; + +/** Upper bound (mm) for 16UC1 turbo colormap when unset. */ +export const DEFAULT_DEPTH_16UC1_COLOR_MAX = 10000; + +/** Default max for 32FC1 float depth (meters). */ +export const DEFAULT_DEPTH_32FC1_MAX = 1; + +const DEPTH_TOPIC_RE = /depth|aligned_depth|compressed_depth/i; +const WRIST_TOPIC_RE = /wrist|hand|left|right|gripper|eef|end_effector/; + +export function normalizedDepthEncoding(encoding: string): string { + return encoding.trim().toLowerCase(); +} + +export function is16BitDepthEncoding(encoding: string): boolean { + const lower = normalizedDepthEncoding(encoding); + return lower === '16uc1' || lower === 'mono16'; +} + +export function is32FloatDepthEncoding(encoding: string): boolean { + return normalizedDepthEncoding(encoding) === '32fc1'; +} + +export function isDepthScalarEncoding(encoding: string): boolean { + return is16BitDepthEncoding(encoding) || is32FloatDepthEncoding(encoding); +} + +export function isDepthTopicName(topic: string): boolean { + return DEPTH_TOPIC_RE.test(topic); +} + +export interface DepthColormapPreset { + colorMode: 'colormap'; + colorMap: 'turbo'; + minValue: number; + maxValue: number; +} + +/** + * Colormap preset inferred from topic name when the user picks a source. + * Returns null for generic RGB or unknown topics. + */ +export function getDepthColormapDefaults(topic: string): DepthColormapPreset | null { + const tp = topic.trim().toLowerCase(); + if (!tp) { + return null; + } + + const isDepthTopic = isDepthTopicName(tp); + const isWrist = !isDepthTopic && WRIST_TOPIC_RE.test(tp); + const base = { colorMode: 'colormap' as const, colorMap: 'turbo' as const }; + + if (isDepthTopic) { + return { ...base, minValue: DEFAULT_DEPTH_16UC1_COLOR_MIN, maxValue: DEFAULT_DEPTH_16UC1_COLOR_MAX }; + } + if (isWrist) { + return { ...base, minValue: 0, maxValue: 1000 }; + } + return null; +} + +export function defaultDepthMinValue(encoding: string, topic?: string): number { + const topicPreset = topic ? getDepthColormapDefaults(topic) : null; + if (topicPreset) { + return topicPreset.minValue; + } + if (is16BitDepthEncoding(encoding)) { + return DEFAULT_DEPTH_16UC1_COLOR_MIN; + } + return 0; +} + +export function defaultDepthMaxValue(encoding: string, topic?: string): number { + const topicPreset = topic ? getDepthColormapDefaults(topic) : null; + if (topicPreset) { + return topicPreset.maxValue; + } + if (is32FloatDepthEncoding(encoding)) { + return DEFAULT_DEPTH_32FC1_MAX; + } + if (is16BitDepthEncoding(encoding)) { + return DEFAULT_DEPTH_16UC1_COLOR_MAX; + } + return 65535; +} + +/** Resolve effective min/max for colormap decoding, honoring explicit panel settings. */ +export function resolveDepthColorRange( + encoding: string, + options?: Partial, + topic?: string, +): { minValue: number; maxValue: number } { + return { + minValue: options?.minValue ?? defaultDepthMinValue(encoding, topic), + maxValue: options?.maxValue ?? defaultDepthMaxValue(encoding, topic), + }; +} + +/** Apply topic-based depth colormap preset when the user selects a new source. */ +export function applyDepthTopicPreset(topic: string, config: ImageConfig): ImageConfig { + const preset = getDepthColormapDefaults(topic); + if (!preset) { + return { ...config, topic }; + } + return { + ...config, + topic, + colorMode: preset.colorMode, + colorMap: preset.colorMap, + minValue: preset.minValue, + maxValue: preset.maxValue, + }; +} diff --git a/src/features/panels/Image/core/depthColorization.test.ts b/src/features/panels/Image/core/depthColorization.test.ts new file mode 100644 index 0000000..e54c006 --- /dev/null +++ b/src/features/panels/Image/core/depthColorization.test.ts @@ -0,0 +1,84 @@ +import fs from 'node:fs'; +import path from 'node:path'; +import { fileURLToPath } from 'node:url'; +import { describe, expect, it } from 'vitest'; +import { decodeCompressedDepth } from './compressedDepthDecoder'; +import { decodeRawImage } from './rawDecoders'; +import { getColorConverter, DEFAULT_RAW_IMAGE_DECODE_OPTIONS } from './imageColorMode'; +import { + DEFAULT_DEPTH_16UC1_COLOR_MAX, + DEFAULT_DEPTH_16UC1_COLOR_MIN, +} from './depthColorDefaults'; + +const __dirname = path.dirname(fileURLToPath(import.meta.url)); +const FIXTURE_16UC1 = path.join(__dirname, '../../../../../test-fixtures/image/compressed-depth-16uc1.bin'); + +function isDarkBlue(r: number, g: number, b: number): boolean { + return b > r && b > g && b > 40; +} + +describe('compressed depth colorization', () => { + it('maps near-zero depths to dark blue and keeps valid depths distinguishable', async () => { + const fixture = fs.readFileSync(FIXTURE_16UC1); + const decoded = await decodeCompressedDepth(fixture, '16UC1; compressedDepth'); + const view = new DataView(decoded.data.buffer, decoded.data.byteOffset, decoded.data.byteLength); + const { width, height } = decoded; + + const rgba = new Uint8ClampedArray(width * height * 4); + decodeRawImage( + { + encoding: '16uc1', + width, + height, + step: width * 2, + is_bigendian: false, + data: decoded.data, + }, + rgba, + { colorMode: 'colormap', colorMap: 'turbo' }, + ); + + let zeroCount = 0; + let zeroDarkBlue = 0; + let minDepth = Infinity; + let maxDepth = 0; + let minIndex = -1; + let maxIndex = -1; + for (let i = 0; i < width * height; i++) { + const value = view.getUint16(i * 2, true); + const o = i * 4; + if (value === 0) { + zeroCount++; + if (isDarkBlue(rgba[o], rgba[o + 1], rgba[o + 2])) zeroDarkBlue++; + } + if (value >= DEFAULT_DEPTH_16UC1_COLOR_MIN && value <= DEFAULT_DEPTH_16UC1_COLOR_MAX) { + if (value < minDepth) { + minDepth = value; + minIndex = i; + } + if (value > maxDepth) { + maxDepth = value; + maxIndex = i; + } + } + } + + expect(zeroCount).toBeGreaterThan(0); + expect(zeroDarkBlue / zeroCount).toBeGreaterThan(0.9); + expect(minIndex).toBeGreaterThanOrEqual(0); + expect(maxIndex).toBeGreaterThanOrEqual(0); + expect(maxDepth - minDepth).toBeGreaterThan(500); + + const convert = getColorConverter( + { ...DEFAULT_RAW_IMAGE_DECODE_OPTIONS, colorMode: 'colormap', colorMap: 'turbo' }, + DEFAULT_DEPTH_16UC1_COLOR_MIN, + DEFAULT_DEPTH_16UC1_COLOR_MAX, + ); + const nearPx = { r: 0, g: 0, b: 0, a: 0 }; + const farPx = { r: 0, g: 0, b: 0, a: 0 }; + convert(nearPx, minDepth); + convert(farPx, maxDepth); + expect(Math.abs(nearPx.g - farPx.g)).toBeGreaterThan(0.05); + expect(Math.abs(nearPx.b - farPx.b)).toBeGreaterThan(0.05); + }); +}); diff --git a/src/features/panels/Image/core/grayscale16PngDecoder.test.ts b/src/features/panels/Image/core/grayscale16PngDecoder.test.ts new file mode 100644 index 0000000..cefab68 --- /dev/null +++ b/src/features/panels/Image/core/grayscale16PngDecoder.test.ts @@ -0,0 +1,106 @@ +import fs from 'node:fs'; +import path from 'node:path'; +import { fileURLToPath } from 'node:url'; +import { describe, expect, it } from 'vitest'; +import { decodeGrayscale16Png } from './grayscale16PngDecoder'; + +const __dirname = path.dirname(fileURLToPath(import.meta.url)); +const FIXTURE_16UC1 = path.join(__dirname, '../../../../../test-fixtures/image/compressed-depth-16uc1.bin'); + +describe('decodeGrayscale16Png', () => { + it('decodes 16-bit grayscale PNG bytes from a compressed depth fixture', async () => { + const payload = fs.readFileSync(FIXTURE_16UC1); + const pngOffset = payload.indexOf(Buffer.from([0x89, 0x50, 0x4e, 0x47])); + expect(pngOffset).toBeGreaterThanOrEqual(0); + + const decoded = await decodeGrayscale16Png(payload.subarray(pngOffset)); + expect(decoded.width).toBe(640); + expect(decoded.height).toBe(480); + expect(decoded.data.byteLength).toBe(640 * 480 * 2); + + const view = new DataView(decoded.data.buffer, decoded.data.byteOffset, decoded.data.byteLength); + expect(view.getUint16(0, true)).toBeGreaterThan(0); + expect(view.getUint16(2, true)).toBeGreaterThan(0); + }); + + it('preserves 16-bit sample values through unfilter and byte-order swap', async () => { + const width = 2; + const height = 1; + // Filter None; PNG stores 16-bit grayscale big-endian: 1000, 2000 mm. + const filtered = new Uint8Array([0, 0x03, 0xe8, 0x07, 0xd0]); + + const idat = await (async () => { + if (typeof CompressionStream === 'undefined') { + return null; + } + const stream = new Blob([filtered.buffer]).stream().pipeThrough(new CompressionStream('deflate')); + return new Uint8Array(await new Response(stream).arrayBuffer()); + })(); + + if (!idat) { + return; + } + + const ihdr = new Uint8Array([ + 0x00, 0x00, 0x00, 0x02, // width + 0x00, 0x00, 0x00, 0x01, // height + 0x10, // bit depth 16 + 0x00, // grayscale + 0x00, + 0x00, + 0x00, + ]); + const ihdrChunk = wrapChunk('IHDR', ihdr); + const idatChunk = wrapChunk('IDAT', idat); + const iendChunk = wrapChunk('IEND', new Uint8Array(0)); + const png = concatBytes([ + new Uint8Array([0x89, 0x50, 0x4e, 0x47, 0x0d, 0x0a, 0x1a, 0x0a]), + ihdrChunk, + idatChunk, + iendChunk, + ]); + + const decoded = await decodeGrayscale16Png(png); + expect(decoded.width).toBe(width); + expect(decoded.height).toBe(height); + const view = new DataView(decoded.data.buffer, decoded.data.byteOffset, decoded.data.byteLength); + expect(view.getUint16(0, true)).toBe(1000); + expect(view.getUint16(2, true)).toBe(2000); + }); +}); + +function crc32(data: Uint8Array): number { + let c = 0xffffffff; + for (let i = 0; i < data.length; i++) { + c ^= data[i]; + for (let k = 0; k < 8; k++) { + c = c & 1 ? 0xedb88320 ^ (c >>> 1) : c >>> 1; + } + } + return (c ^ 0xffffffff) >>> 0; +} + +function wrapChunk(type: string, data: Uint8Array): Uint8Array { + const out = new Uint8Array(12 + data.length); + const view = new DataView(out.buffer); + view.setUint32(0, data.length, false); + out[4] = type.charCodeAt(0)!; + out[5] = type.charCodeAt(1)!; + out[6] = type.charCodeAt(2)!; + out[7] = type.charCodeAt(3)!; + out.set(data, 8); + const crcInput = out.subarray(4, 8 + data.length); + view.setUint32(8 + data.length, crc32(crcInput), false); + return out; +} + +function concatBytes(parts: Uint8Array[]): Uint8Array { + const total = parts.reduce((sum, part) => sum + part.length, 0); + const out = new Uint8Array(total); + let offset = 0; + for (const part of parts) { + out.set(part, offset); + offset += part.length; + } + return out; +} diff --git a/src/features/panels/Image/core/grayscale16PngDecoder.ts b/src/features/panels/Image/core/grayscale16PngDecoder.ts new file mode 100644 index 0000000..44c08d4 --- /dev/null +++ b/src/features/panels/Image/core/grayscale16PngDecoder.ts @@ -0,0 +1,135 @@ +/** + * Minimal 16-bit grayscale PNG decoder for ROS `compressedDepth` payloads. + * Uses the platform `DecompressionStream` API (no extra dependencies). + */ + +import { unfilter16BitGrayscaleScanlines } from './png16ScanlineUnfilter'; + +/** Standard PNG file signature. */ +export const PNG_SIGNATURE = new Uint8Array([0x89, 0x50, 0x4e, 0x47, 0x0d, 0x0a, 0x1a, 0x0a]); + +export interface Grayscale16PngImage { + width: number; + height: number; + /** Little-endian 16-bit samples, `width * height * 2` bytes. */ + data: Uint8Array; +} + +/** Locate the first PNG signature inside a buffer (e.g. after a ROS depth header). */ +export function findPngSignatureOffset(data: Uint8Array): number { + const limit = Math.max(0, data.byteLength - PNG_SIGNATURE.byteLength); + for (let offset = 0; offset <= limit; offset++) { + let matched = true; + for (let i = 0; i < PNG_SIGNATURE.byteLength; i++) { + if (data[offset + i] !== PNG_SIGNATURE[i]) { + matched = false; + break; + } + } + if (matched) { + return offset; + } + } + return -1; +} + +function readU32Be(view: DataView, offset: number): number { + return view.getUint32(offset, false); +} + +function concatIdatChunks(png: Uint8Array): Uint8Array { + const view = new DataView(png.buffer, png.byteOffset, png.byteLength); + const chunks: Uint8Array[] = []; + let offset = PNG_SIGNATURE.byteLength; + + while (offset + 8 <= png.byteLength) { + const length = readU32Be(view, offset); + const type = + String.fromCharCode(png[offset + 4], png[offset + 5], png[offset + 6], png[offset + 7]); + const dataStart = offset + 8; + const dataEnd = dataStart + length; + if (dataEnd + 4 > png.byteLength) { + throw new Error('PNG chunk exceeds buffer bounds'); + } + if (type === 'IDAT') { + chunks.push(png.subarray(dataStart, dataEnd)); + } + if (type === 'IEND') { + break; + } + offset = dataEnd + 4; + } + + if (chunks.length === 0) { + throw new Error('PNG is missing IDAT chunks'); + } + + const total = chunks.reduce((sum, chunk) => sum + chunk.byteLength, 0); + const out = new Uint8Array(total); + let writeOffset = 0; + for (const chunk of chunks) { + out.set(chunk, writeOffset); + writeOffset += chunk.byteLength; + } + return out; +} + +async function inflateZlib(zlibData: Uint8Array): Promise { + if (typeof DecompressionStream === 'undefined') { + throw new Error('DecompressionStream is not supported in this environment'); + } + const owned = new Uint8Array(zlibData.byteLength); + owned.set(zlibData); + const stream = new Blob([owned.buffer]).stream().pipeThrough(new DecompressionStream('deflate')); + return new Uint8Array(await new Response(stream).arrayBuffer()); +} + +function parseIhdr(png: Uint8Array): { width: number; height: number; bitDepth: number; colorType: number } { + if (png.byteLength < PNG_SIGNATURE.byteLength + 8 + 13) { + throw new Error('PNG buffer is too small'); + } + for (let i = 0; i < PNG_SIGNATURE.byteLength; i++) { + if (png[i] !== PNG_SIGNATURE[i]) { + throw new Error('Invalid PNG signature'); + } + } + + const view = new DataView(png.buffer, png.byteOffset, png.byteLength); + const firstLength = readU32Be(view, PNG_SIGNATURE.byteLength); + const firstType = String.fromCharCode( + png[PNG_SIGNATURE.byteLength + 4], + png[PNG_SIGNATURE.byteLength + 5], + png[PNG_SIGNATURE.byteLength + 6], + png[PNG_SIGNATURE.byteLength + 7], + ); + if (firstType !== 'IHDR' || firstLength !== 13) { + throw new Error('PNG is missing IHDR chunk'); + } + + const ihdrOffset = PNG_SIGNATURE.byteLength + 8; + return { + width: readU32Be(view, ihdrOffset), + height: readU32Be(view, ihdrOffset + 4), + bitDepth: png[ihdrOffset + 8], + colorType: png[ihdrOffset + 9], + }; +} + +/** Decode a 16-bit grayscale PNG into little-endian depth sample bytes. */ +export async function decodeGrayscale16Png(png: Uint8Array): Promise { + const { width, height, bitDepth, colorType } = parseIhdr(png); + if (width <= 0 || height <= 0) { + throw new Error(`Invalid PNG dimensions: ${width}x${height}`); + } + if (bitDepth !== 16 || colorType !== 0) { + throw new Error( + `Compressed depth PNG must be 16-bit grayscale (got depth=${bitDepth}, colorType=${colorType})`, + ); + } + + const idat = concatIdatChunks(png); + const filtered = await inflateZlib(idat); + const data = unfilter16BitGrayscaleScanlines(filtered, width, height); + + return { width, height, data }; +} diff --git a/src/features/panels/Image/core/imageColorMode.test.ts b/src/features/panels/Image/core/imageColorMode.test.ts new file mode 100644 index 0000000..880fada --- /dev/null +++ b/src/features/panels/Image/core/imageColorMode.test.ts @@ -0,0 +1,32 @@ +import { describe, expect, it } from 'vitest'; +import { DEFAULT_RAW_IMAGE_DECODE_OPTIONS, getColorConverter } from './imageColorMode'; + +describe('getColorConverter turbo', () => { + it('does not collapse mid-range values to pure green', () => { + const convert = getColorConverter( + { ...DEFAULT_RAW_IMAGE_DECODE_OPTIONS, colorMode: 'colormap', colorMap: 'turbo' }, + 0, + 10000, + ); + const px = { r: 0, g: 0, b: 0, a: 0 }; + convert(px, 3537); + const g = Math.round(px.g * 255); + const r = Math.round(px.r * 255); + const b = Math.round(px.b * 255); + expect(g).toBeLessThan(255); + expect(r + g + b).toBeGreaterThan(80); + expect(b).toBeGreaterThan(0); + }); + + it('maps zero to dark blue at the colormap minimum', () => { + const convert = getColorConverter( + { ...DEFAULT_RAW_IMAGE_DECODE_OPTIONS, colorMode: 'colormap', colorMap: 'turbo' }, + 0, + 10000, + ); + const px = { r: 0, g: 0, b: 0, a: 0 }; + convert(px, 0); + expect(Math.round(px.b * 255)).toBeGreaterThan(Math.round(px.g * 255)); + expect(Math.round(px.r * 255)).toBeLessThan(80); + }); +}); diff --git a/src/features/panels/Image/core/imageColorMode.ts b/src/features/panels/Image/core/imageColorMode.ts index 9c20954..951df7d 100644 --- a/src/features/panels/Image/core/imageColorMode.ts +++ b/src/features/panels/Image/core/imageColorMode.ts @@ -1,6 +1,5 @@ /** - * Depth / mono16 colorization (Foxglove-style), without THREE.js dependency. - * Ported from studio/packages/studio-base/src/panels/ThreeDeeRender/renderables/colorMode.ts + * Scalar depth colorization (turbo / rainbow / gradient) for mono16 and float depth. */ export interface ColorRGBA { @@ -121,11 +120,12 @@ function turboLinear(output: ColorRGBA, pct: number): void { const v4r = 1 * kRedVec4[0] + x * kRedVec4[1] + x2 * kRedVec4[2] + x3 * kRedVec4[3]; const v4g = 1 * kGreenVec4[0] + x * kGreenVec4[1] + x2 * kGreenVec4[2] + x3 * kGreenVec4[3]; const v4b = 1 * kBlueVec4[0] + x * kBlueVec4[1] + x2 * kBlueVec4[2] + x3 * kBlueVec4[3]; - const v2x = x2; - const v2y = x3; - const dot2r = v2x * kRedVec2[0] + v2y * kRedVec2[1]; - const dot2g = v2x * kGreenVec2[0] + v2y * kGreenVec2[1]; - const dot2b = v2x * kBlueVec2[0] + v2y * kBlueVec2[1]; + // GLSL: vec2 v2 = v4.zw * v4.z → (x^4, x^5) + const x4 = x2 * x2; + const x5 = x3 * x2; + const dot2r = x4 * kRedVec2[0] + x5 * kRedVec2[1]; + const dot2g = x4 * kGreenVec2[0] + x5 * kGreenVec2[1]; + const dot2b = x4 * kBlueVec2[0] + x5 * kBlueVec2[1]; output.r = clamp(v4r + dot2r, 0, 1); output.g = clamp(v4g + dot2g, 0, 1); output.b = clamp(v4b + dot2b, 0, 1); diff --git a/src/features/panels/Image/core/imageTypes.test.ts b/src/features/panels/Image/core/imageTypes.test.ts index 005fe34..5da4ce5 100644 --- a/src/features/panels/Image/core/imageTypes.test.ts +++ b/src/features/panels/Image/core/imageTypes.test.ts @@ -1,12 +1,16 @@ import { describe, expect, it } from 'vitest'; import { + depthEncodingFromFormat, getCompressedKind, + isCompressedDepthFormat, isCompressedVideoMessage, isH264CompressedFrameMessage, isImagePanelTopicSchema, isRawImageTopicSchema, normalizeCompressedMime, + parseCompressedImageFormat, prepareImageWorkerBytes, + sniffCompressedMime, snapshotBytes, } from './imageTypes'; @@ -114,11 +118,74 @@ describe('isImagePanelTopicSchema', () => { }); }); +describe('parseCompressedImageFormat', () => { + it('parses jpeg color transport strings', () => { + expect(parseCompressedImageFormat('rgb8; jpeg compressed bgr8')).toEqual({ + rawEncoding: 'rgb8', + transport: 'jpeg compressed bgr8', + depthCodec: undefined, + bitmapKind: 'jpeg', + }); + }); + + it('parses compressed depth transport strings', () => { + expect(parseCompressedImageFormat('16UC1; compressedDepth')).toEqual({ + rawEncoding: '16uc1', + transport: 'compressedDepth', + depthCodec: 'png', + bitmapKind: null, + }); + expect(parseCompressedImageFormat('32FC1; compressedDepth')).toMatchObject({ + rawEncoding: '32fc1', + depthCodec: 'png', + }); + expect(parseCompressedImageFormat('16UC1; compressedDepth rvl')).toMatchObject({ + rawEncoding: '16uc1', + depthCodec: 'rvl', + }); + }); +}); + +describe('isCompressedDepthFormat', () => { + it('recognizes ROS compressed depth formats', () => { + expect(isCompressedDepthFormat('16UC1; compressedDepth')).toBe(true); + expect(isCompressedDepthFormat('32FC1; compressedDepth')).toBe(true); + expect(isCompressedDepthFormat('rgb8; jpeg compressed bgr8')).toBe(false); + }); +}); + +describe('depthEncodingFromFormat', () => { + it('returns normalized depth encodings for compressed depth formats', () => { + expect(depthEncodingFromFormat('16UC1; compressedDepth')).toBe('16uc1'); + expect(depthEncodingFromFormat('32FC1; compressedDepth')).toBe('32fc1'); + expect(depthEncodingFromFormat('rgb8; jpeg compressed bgr8')).toBeNull(); + }); +}); + +describe('sniffCompressedMime', () => { + it('detects jpeg and png magic bytes', () => { + expect(sniffCompressedMime(new Uint8Array([0xff, 0xd8, 0xff, 0xe0]))).toBe('image/jpeg'); + expect( + sniffCompressedMime(new Uint8Array([0x89, 0x50, 0x4e, 0x47, 0x0d, 0x0a, 0x1a, 0x0a])), + ).toBe('image/png'); + }); +}); + describe('normalizeCompressedMime', () => { it('falls back to JPEG for raw-style format tokens on CompressedImage', () => { expect(getCompressedKind('bgr8')).toBeNull(); - expect(normalizeCompressedMime('bgr8')).toBe('image/jpeg'); - expect(normalizeCompressedMime('rgb8')).toBe('image/jpeg'); + expect(normalizeCompressedMime('bgr8', new Uint8Array([0xff, 0xd8, 0xff, 0xe0]))).toBe('image/jpeg'); + expect(normalizeCompressedMime('rgb8', new Uint8Array([0xff, 0xd8, 0xff, 0xe0]))).toBe('image/jpeg'); + }); + + it('does not treat compressed depth formats as JPEG', () => { + expect(() => normalizeCompressedMime('16UC1; compressedDepth')).toThrow(/Compressed depth/); + }); + + it('sniffs PNG when raw encoding token is unknown but payload is PNG', () => { + expect( + normalizeCompressedMime('16UC1', new Uint8Array([0x89, 0x50, 0x4e, 0x47, 0x0d, 0x0a, 0x1a, 0x0a])), + ).toBe('image/png'); }); it('still recognizes explicit codec hints in compound format strings', () => { diff --git a/src/features/panels/Image/core/imageTypes.ts b/src/features/panels/Image/core/imageTypes.ts index efe389d..3b29880 100644 --- a/src/features/panels/Image/core/imageTypes.ts +++ b/src/features/panels/Image/core/imageTypes.ts @@ -123,8 +123,89 @@ export type CompressedImageKind = | 'h264' | null; +export type CompressedDepthCodec = 'png' | 'rvl'; + +export type DepthImageEncoding = '16uc1' | '32fc1'; + +export interface ParsedCompressedImageFormat { + rawEncoding?: string; + transport?: string; + depthCodec?: CompressedDepthCodec; + bitmapKind: CompressedImageKind; +} + const COMPRESSED_KIND_RE = /\b(jpeg|jpg|png|webp|gif|avif|bmp|h264)\b/i; +const RAW_ENCODING_TOKENS = new Set([ + '16uc1', + '32fc1', + 'mono16', + 'mono8', + '8uc1', + 'rgb8', + 'bgr8', + 'rgba8', + 'bgra8', + '8uc3', +]); + +function normalizeFormatToken(token: string): string { + return token.trim().toLowerCase(); +} + +function depthEncodingFromRawToken(token: string): DepthImageEncoding | null { + const lower = normalizeFormatToken(token); + if (lower === '16uc1' || lower === 'mono16') { + return '16uc1'; + } + if (lower === '32fc1') { + return '32fc1'; + } + return null; +} + +function parseCompressedDepthTransport(transport: string): CompressedDepthCodec | undefined { + const lower = transport.trim().toLowerCase(); + if (!lower.includes('compresseddepth')) { + return undefined; + } + if (/\brvl\b/.test(lower)) { + return 'rvl'; + } + return 'png'; +} + +/** + * Structured parse of `sensor_msgs/CompressedImage.format`. + * Handles `rgb8; jpeg compressed bgr8`, `16UC1; compressedDepth`, etc. + */ +export function parseCompressedImageFormat(format: string): ParsedCompressedImageFormat { + const trimmed = format.trim(); + const parts = trimmed.split(';').map((part) => part.trim()).filter(Boolean); + const rawEncoding = parts[0] ? normalizeFormatToken(parts[0]) : undefined; + const transport = parts.length > 1 ? parts.slice(1).join(';').trim() : undefined; + const depthCodec = transport ? parseCompressedDepthTransport(transport) : undefined; + + return { + rawEncoding, + transport, + depthCodec, + bitmapKind: getCompressedKind(trimmed), + }; +} + +export function isCompressedDepthFormat(format: string): boolean { + const parsed = parseCompressedImageFormat(format); + return parsed.depthCodec != null && depthEncodingFromRawToken(parsed.rawEncoding ?? '') != null; +} + +export function depthEncodingFromFormat(format: string): DepthImageEncoding | null { + if (!isCompressedDepthFormat(format)) { + return null; + } + return depthEncodingFromRawToken(parseCompressedImageFormat(format).rawEncoding ?? ''); +} + /** * Classify `sensor_msgs/CompressedImage.format` for decode routing. * Handles `rgb8; jpeg compressed bgr8`, `jpeg`, `image/png`, `h264`, etc. @@ -151,7 +232,44 @@ export function getCompressedKind(format: string): CompressedImageKind { return null; } -export function normalizeCompressedMime(format: string): string { +export function sniffCompressedMime(data: Uint8Array): string | null { + if (data.byteLength >= 3 && data[0] === 0xff && data[1] === 0xd8 && data[2] === 0xff) { + return 'image/jpeg'; + } + if ( + data.byteLength >= 8 && + data[0] === 0x89 && + data[1] === 0x50 && + data[2] === 0x4e && + data[3] === 0x47 && + data[4] === 0x0d && + data[5] === 0x0a && + data[6] === 0x1a && + data[7] === 0x0a + ) { + return 'image/png'; + } + if ( + data.byteLength >= 12 && + data[0] === 0x52 && + data[1] === 0x49 && + data[2] === 0x46 && + data[3] === 0x46 && + data[8] === 0x57 && + data[9] === 0x45 && + data[10] === 0x42 && + data[11] === 0x50 + ) { + return 'image/webp'; + } + return null; +} + +export function normalizeCompressedMime(format: string, data?: Uint8Array): string { + if (isCompressedDepthFormat(format)) { + throw new Error(`Compressed depth format must not use bitmap MIME routing: ${format}`); + } + const kind = getCompressedKind(format); if (kind === 'jpeg') { return 'image/jpeg'; @@ -167,7 +285,7 @@ export function normalizeCompressedMime(format: string): string { ?.toLowerCase(); if (!firstToken) { - return 'image/jpeg'; + return data ? sniffCompressedMime(data) ?? 'image/jpeg' : 'image/jpeg'; } if (firstToken.startsWith('image/')) { return firstToken; @@ -175,8 +293,15 @@ export function normalizeCompressedMime(format: string): string { if (firstToken === 'jpg') { return 'image/jpeg'; } + if (RAW_ENCODING_TOKENS.has(firstToken)) { + const sniffed = data ? sniffCompressedMime(data) : null; + if (sniffed) { + return sniffed; + } + throw new Error(`Unsupported compressed image format token: ${format}`); + } // Unknown tokens (e.g. `bgr8` mislabeled on CompressedImage): many bags still carry JPEG bytes. - return 'image/jpeg'; + return data ? sniffCompressedMime(data) ?? 'image/jpeg' : 'image/jpeg'; } /** diff --git a/src/features/panels/Image/core/png16ScanlineUnfilter.test.ts b/src/features/panels/Image/core/png16ScanlineUnfilter.test.ts new file mode 100644 index 0000000..e939949 --- /dev/null +++ b/src/features/panels/Image/core/png16ScanlineUnfilter.test.ts @@ -0,0 +1,30 @@ +import { describe, expect, it } from 'vitest'; +import { unfilter16BitGrayscaleScanlines } from './png16ScanlineUnfilter'; + +describe('unfilter16BitGrayscaleScanlines', () => { + it('reconstructs Sub-filtered rows using 2-byte pixel stride', () => { + const width = 2; + const height = 1; + // Sub: first pixel stored raw (1000), second pixel as deltas from pixel0 high byte. + const inflated = new Uint8Array([1, 0x03, 0xe8, 0x04, 0xe8]); + + const out = unfilter16BitGrayscaleScanlines(inflated, width, height); + const view = new DataView(out.buffer, out.byteOffset, out.byteLength); + expect(view.getUint16(0, true)).toBe(1000); + expect(view.getUint16(2, true)).toBe(2000); + }); + + it('byte-swaps to little-endian after reconstruction', () => { + const inflated = new Uint8Array([ + 0, // None + 0x03, + 0xe8, + 0x07, + 0xd0, + ]); + + const out = unfilter16BitGrayscaleScanlines(inflated, 2, 1); + expect(out[0]).toBe(0xe8); + expect(out[1]).toBe(0x03); + }); +}); diff --git a/src/features/panels/Image/core/png16ScanlineUnfilter.ts b/src/features/panels/Image/core/png16ScanlineUnfilter.ts new file mode 100644 index 0000000..9fb28f6 --- /dev/null +++ b/src/features/panels/Image/core/png16ScanlineUnfilter.ts @@ -0,0 +1,175 @@ +/** + * PNG scanline reconstruction for 16-bit grayscale (color type 0). + * + * ROS `compressedDepth` PNGs use 2 bytes per pixel. Sub/Avg/Paeth filters must + * reference the previous *pixel* (2 bytes), not the previous byte — otherwise + * decoded depth values show horizontal striping. + */ + +const GRAYSCALE_16_BPP = 2; + +function paethPredictor(a: number, b: number, c: number): number { + const p = a + b - c; + const pa = Math.abs(p - a); + const pb = Math.abs(p - b); + const pc = Math.abs(p - c); + if (pa <= pb && pa <= pc) return a; + if (pb <= pc) return b; + return c; +} + +function unfilterNone(currentLine: Uint8Array, newLine: Uint8Array, bytesPerLine: number): void { + for (let i = 0; i < bytesPerLine; i++) { + newLine[i] = currentLine[i]!; + } +} + +function unfilterSub( + currentLine: Uint8Array, + newLine: Uint8Array, + bytesPerLine: number, + bytesPerPixel: number, +): void { + let i = 0; + for (; i < bytesPerPixel; i++) { + newLine[i] = currentLine[i]!; + } + for (; i < bytesPerLine; i++) { + newLine[i] = (currentLine[i] + newLine[i - bytesPerPixel]) & 0xff; + } +} + +function unfilterUp( + currentLine: Uint8Array, + newLine: Uint8Array, + prevLine: Uint8Array, + bytesPerLine: number, +): void { + if (prevLine.length === 0) { + for (let i = 0; i < bytesPerLine; i++) { + newLine[i] = currentLine[i]!; + } + return; + } + for (let i = 0; i < bytesPerLine; i++) { + newLine[i] = (currentLine[i] + prevLine[i]) & 0xff; + } +} + +function unfilterAverage( + currentLine: Uint8Array, + newLine: Uint8Array, + prevLine: Uint8Array, + bytesPerLine: number, + bytesPerPixel: number, +): void { + let i = 0; + if (prevLine.length === 0) { + for (; i < bytesPerPixel; i++) { + newLine[i] = currentLine[i]!; + } + for (; i < bytesPerLine; i++) { + newLine[i] = (currentLine[i] + (newLine[i - bytesPerPixel] >> 1)) & 0xff; + } + return; + } + for (; i < bytesPerPixel; i++) { + newLine[i] = (currentLine[i] + (prevLine[i] >> 1)) & 0xff; + } + for (; i < bytesPerLine; i++) { + newLine[i] = + (currentLine[i] + ((newLine[i - bytesPerPixel] + prevLine[i]) >> 1)) & 0xff; + } +} + +function unfilterPaeth( + currentLine: Uint8Array, + newLine: Uint8Array, + prevLine: Uint8Array, + bytesPerLine: number, + bytesPerPixel: number, +): void { + let i = 0; + if (prevLine.length === 0) { + for (; i < bytesPerPixel; i++) { + newLine[i] = currentLine[i]!; + } + for (; i < bytesPerLine; i++) { + newLine[i] = (currentLine[i] + newLine[i - bytesPerPixel]) & 0xff; + } + return; + } + for (; i < bytesPerPixel; i++) { + newLine[i] = (currentLine[i] + prevLine[i]) & 0xff; + } + for (; i < bytesPerLine; i++) { + newLine[i] = + (currentLine[i] + + paethPredictor( + newLine[i - bytesPerPixel], + prevLine[i], + prevLine[i - bytesPerPixel], + )) & + 0xff; + } +} + +/** + * Reconstruct raw scanlines from inflated PNG image data (one filter byte per row). + * Returns little-endian 16-bit sample bytes suitable for `decodeRawImage`. + */ +export function unfilter16BitGrayscaleScanlines( + inflated: Uint8Array, + width: number, + height: number, +): Uint8Array { + const bytesPerLine = width * GRAYSCALE_16_BPP; + const expectedLen = height * (1 + bytesPerLine); + if (inflated.byteLength !== expectedLen) { + throw new Error( + `PNG inflated length ${inflated.byteLength} !== expected ${expectedLen}`, + ); + } + + const out = new Uint8Array(height * bytesPerLine); + const empty = new Uint8Array(0); + let prevLine = empty; + let offset = 0; + + for (let row = 0; row < height; row++) { + const filterType = inflated[offset++]; + const currentLine = inflated.subarray(offset, offset + bytesPerLine); + offset += bytesPerLine; + const newLine = out.subarray(row * bytesPerLine, (row + 1) * bytesPerLine); + + switch (filterType) { + case 0: + unfilterNone(currentLine, newLine, bytesPerLine); + break; + case 1: + unfilterSub(currentLine, newLine, bytesPerLine, GRAYSCALE_16_BPP); + break; + case 2: + unfilterUp(currentLine, newLine, prevLine, bytesPerLine); + break; + case 3: + unfilterAverage(currentLine, newLine, prevLine, bytesPerLine, GRAYSCALE_16_BPP); + break; + case 4: + unfilterPaeth(currentLine, newLine, prevLine, bytesPerLine, GRAYSCALE_16_BPP); + break; + default: + throw new Error(`Unsupported PNG filter type: ${filterType}`); + } + prevLine = newLine; + } + + // PNG stores 16-bit samples big-endian; RawImage decode expects little-endian. + for (let i = 0; i < out.length; i += 2) { + const t = out[i]; + out[i] = out[i + 1]!; + out[i + 1] = t; + } + + return out; +} diff --git a/src/features/panels/Image/core/rawDecoders.test.ts b/src/features/panels/Image/core/rawDecoders.test.ts index 160232a..56ef29d 100644 --- a/src/features/panels/Image/core/rawDecoders.test.ts +++ b/src/features/panels/Image/core/rawDecoders.test.ts @@ -32,6 +32,23 @@ describe('decodeRawImage', () => { expect(Array.from(output)).toEqual([0, 0, 0, 255, 255, 255, 255, 255]); }); + it('decodes 16uc1 using default depth colormap range when maxValue is omitted', () => { + const output = new Uint8ClampedArray(4); + decodeRawImage( + { + encoding: '16UC1', + width: 1, + height: 1, + data: new Uint8Array([0x00, 0x00]), + }, + output, + { colorMode: 'colormap', colorMap: 'turbo' }, + ); + + expect(output[2]).toBeGreaterThan(output[1]); + expect(output[3]).toBe(255); + }); + it('decodes 16uc1 using grayscale normalization', () => { const output = new Uint8ClampedArray(4); decodeRawImage( diff --git a/src/features/panels/Image/core/rawDecoders.ts b/src/features/panels/Image/core/rawDecoders.ts index a46453c..e68dbbd 100644 --- a/src/features/panels/Image/core/rawDecoders.ts +++ b/src/features/panels/Image/core/rawDecoders.ts @@ -4,6 +4,7 @@ import { type ColorRGBA, type RawImageDecodeOptions, } from './imageColorMode'; +import { resolveDepthColorRange } from './depthColorDefaults'; export interface RawImageLike { encoding: string; @@ -194,8 +195,7 @@ function decodeFloat1c( throw new Error(`Float image row step (${step}) must be at least 4*width (${width * 4})`); } - const minV = colorOpts.minValue ?? 0; - const maxV = colorOpts.maxValue ?? 1; + const { minValue: minV, maxValue: maxV } = resolveDepthColorRange('32fc1', colorOpts); let converter; try { converter = getColorConverter(colorOpts, minV, maxV); @@ -259,13 +259,13 @@ function decodeMono16( isBigEndian: boolean, output: Uint8ClampedArray, colorOpts: RawImageDecodeOptions, + encoding: string, ): void { if (step < width * 2) { throw new Error(`Mono16 image row step (${step}) must be at least 2*width (${width * 2})`); } - const minValue = colorOpts.minValue ?? 0; - const maxValue = colorOpts.maxValue ?? 65535; + const { minValue, maxValue } = resolveDepthColorRange(encoding, colorOpts); let converter; try { converter = getColorConverter(colorOpts, minValue, maxValue); @@ -406,7 +406,7 @@ export function decodeRawImage( return; case 'mono16': case '16uc1': - decodeMono16(data, width, height, step, isBigEndian, output, colorOpts); + decodeMono16(data, width, height, step, isBigEndian, output, colorOpts, encoding); return; case '32fc1': decodeFloat1c(data, width, height, step, isBigEndian, output, colorOpts); diff --git a/test-fixtures/image/compressed-depth-16uc1.bin b/test-fixtures/image/compressed-depth-16uc1.bin new file mode 100644 index 0000000..317bd14 Binary files /dev/null and b/test-fixtures/image/compressed-depth-16uc1.bin differ diff --git a/test-fixtures/image/compressed-depth-16uc1.format.txt b/test-fixtures/image/compressed-depth-16uc1.format.txt new file mode 100644 index 0000000..44202cd --- /dev/null +++ b/test-fixtures/image/compressed-depth-16uc1.format.txt @@ -0,0 +1 @@ +16UC1; compressedDepth diff --git a/test-fixtures/image/compressed-depth-32fc1.bin b/test-fixtures/image/compressed-depth-32fc1.bin new file mode 100644 index 0000000..81cbe26 Binary files /dev/null and b/test-fixtures/image/compressed-depth-32fc1.bin differ diff --git a/tests/fixturePaths.ts b/tests/fixturePaths.ts index abe28e1..188cd9b 100644 --- a/tests/fixturePaths.ts +++ b/tests/fixturePaths.ts @@ -10,6 +10,7 @@ export const MCAP_BASIC = path.join(EXAMPLES_DIR, 'test_5s.mcap'); export const MCAP_POSE = path.join(EXAMPLES_DIR, 'test_pose.mcap'); export const MCAP_3CAM = path.join(EXAMPLES_DIR, 'test_3cam.mcap'); export const MCAP_H264 = path.join(EXAMPLES_DIR, 'test_h264.mcap'); +export const MCAP_COMPRESSED_DEPTH = path.join(EXAMPLES_DIR, 'test_compressed_depth.mcap'); export const HDF5_MINIMAL = path.join(EXAMPLES_DIR, 'test_minimal.hdf5'); export const BVH_MINIMAL = path.join(EXAMPLES_DIR, 'test_minimal.bvh'); @@ -17,6 +18,7 @@ export const MCAP_BASIC_URL = '/examples/test_5s.mcap'; export const MCAP_POSE_URL = '/examples/test_pose.mcap'; export const MCAP_3CAM_URL = '/examples/test_3cam.mcap'; export const MCAP_H264_URL = '/examples/test_h264.mcap'; +export const MCAP_COMPRESSED_DEPTH_URL = '/examples/test_compressed_depth.mcap'; export const HDF5_MINIMAL_URL = '/examples/test_minimal.hdf5'; export const BVH_MINIMAL_URL = '/examples/test_minimal.bvh'; diff --git a/tests/image-compressed-depth.spec.ts b/tests/image-compressed-depth.spec.ts new file mode 100644 index 0000000..a52dc58 --- /dev/null +++ b/tests/image-compressed-depth.spec.ts @@ -0,0 +1,29 @@ +import { test, expect } from '@playwright/test'; +import { MCAP_COMPRESSED_DEPTH, MCAP_COMPRESSED_DEPTH_URL, requireFixture } from './fixturePaths'; +import { attachBrowserDiagnostics, openFixtureByUrl } from './helpers/rosview'; + +test.describe.configure({ timeout: 120_000 }); + +test.beforeAll(() => { + requireFixture(MCAP_COMPRESSED_DEPTH); +}); + +test('16UC1 compressedDepth CompressedImage decodes without error', async ({ page }) => { + const diagnostics = attachBrowserDiagnostics(page); + await openFixtureByUrl(page, MCAP_COMPRESSED_DEPTH_URL, { diagnostics }); + + const play = page.getByRole('button', { name: 'Play playback' }); + if (await play.isVisible().catch(() => false)) { + await play.click(); + } + + await expect(page.getByTestId('image-panel')).toBeVisible({ timeout: 60_000 }); + await expect(page.getByText(/Failed to decode frame at index 0|Image decode failed/i)).toHaveCount(0); + + const imageStatus = page.getByTestId('image-panel-status'); + await expect(imageStatus).toBeVisible({ timeout: 90_000 }); + await expect(imageStatus).toHaveText(/640x480.*16uc1/i); + + expect(diagnostics.pageErrors.filter((entry) => /decode frame at index 0|Image decode failed/i.test(entry))).toEqual([]); + expect(diagnostics.consoleErrors.filter((entry) => /decode frame at index 0|Image decode failed/i.test(entry))).toEqual([]); +});