PointCloud color is not displayed

[aalbaali]

Description
I'm trying to overlay rgb colors on top of a point cloud by following this tutorial, but I'm unable to see the RGBA color mode in foxglove. I successfully managed to get the rgba fields in the point cloud as seen below, but I can't see the rgba mode. How can I access the rgba color mode?

• Version: 2.18.1
• Platform: Ubuntu 22.04

Steps To Reproduce
Create a point cloud schema that has fields x, y, z (float32) and red, blue, green, alpha (uint8).

Expected Behavior
Get RGBA color-mode option in foxglove.

[linear[bot]]

FG-10102 PointCloud color is not displayed

[jtbandes]

Using the sample code from the blog post I'm seeing it work as expected:

Could you share your mcap file here so we can reproduce the issue?

[aalbaali]

Thanks for getting back to me. I don't have an mcap file, but I have a rosbag file:

amro_wc1_clouds_2025-01-07-21-38-18.zip

The index.ts I used to relay the info is here:

import { ExtensionContext } from "@foxglove/extension";
import { PointCloud, RawImage } from "@foxglove/schemas";
import { Immutable, MessageEvent } from "@foxglove/studio";
import { Quaternion } from "three";

type cameraIntrinsics = {
	height: number;
	width: number;
	fx: number;
	fy: number;
	ppx: number;
	ppy: number;
	coeffs: Float64Array;
	model: string;
};

type CameraTransform = {
	xyz: [number, number, number];
	rpy: [number, number, number];
	intrinsics: cameraIntrinsics;
	timestamp: number;
};

type NumpyBuffer = {
	data: Uint8Array;
	dtype: string;
	shape: number[];
};

type Color = {
	r: number;
	g: number;
	b: number;
};

type VisionInputLight = {
	depth: NumpyBuffer;
	bgr: NumpyBuffer;
	camera_transform: CameraTransform;
	camera_key: string;
};
type CamerasOutputs = {
	cameras_outputs: VisionInputLight[];
};

function getRGBImage(bgr: NumpyBuffer): Color[][] {
	// Get 2D array of rgb values
	let leftImage: Uint8Array;
	if (bgr.shape[0] === 1) {
		leftImage = new Uint8Array(bgr.data);
	} else if (bgr.shape[0] === 2) {
		leftImage = new Uint8Array(bgr.data.slice(0, bgr.data.length / 2));
	} else {
		throw new Error("Invalid input image shape");
	}

	const height = bgr.shape[1];
	const width = bgr.shape[2];
	console.log(`Height: ${height}, Width: ${width}`);
	if (width == undefined || height == undefined) {
		throw new Error("width or height is undefined");
	}

	const rgb_matrix: Color[][] = [];
	const depth = 3;
	for (let i = 0; i < height; i++) {
		const row: Color[] = [];
		for (let j = 0; j < width; j++) {
			// Starting index (i.e., of the 'b' element)
			const flatIdx = i * width * depth + j * depth;
			const rcol = leftImage[flatIdx + 2];
			const gcol = leftImage[flatIdx + 1];
			const bcol = leftImage[flatIdx];
			if (rcol == undefined || gcol == undefined || bcol == undefined) {
				throw new Error("rcol, gcol, or bcol is undefined");
			}
			const rgb: Color = {
				r: rcol,
				g: gcol,
				b: bcol,
			};
			row.push(rgb);
		}
		rgb_matrix.push(row);
	}
	//console.log(`RGB matrix: ${rgb_matrix}`);
	return rgb_matrix;
}

function getFrameCloud(depth: number[][], rgb: Color[][], intrinsics: cameraIntrinsics): Uint8Array {
	// Convert the 2D depth image to a 3D point cloud, in the camera frame.
	const width = intrinsics.width;
	const height = intrinsics.height;
	const num_bytes_field = 3 * 4 + 4; // 3 floats for position, 4 bytes for color
	//const num_bytes_field = 3 * 4;
	const cloud: Uint8Array = new Uint8Array(width * height * num_bytes_field);
	console.log(`Width: ${width}, Height: ${height}, depth length: ${depth.length}, ${depth[0].length}`);
	for (let y = 0; y < height; y++) {
		const dataY = depth[y];
		if (dataY == undefined) {
			throw new Error("datax is undefined");
		}
		const rgbY = rgb[y];
		if (rgbY == undefined) {
			throw new Error("colorY is undefined");
		}

		for (let x = 0; x < width; x++) {
			// Deproject the (x, y, depth) point into 3D space
			const zpoint = dataY[x];
			if (zpoint == undefined) {
				throw new Error("xpoint, ypoint, or zpoint is undefined");
			}
			const xPoint = ((x - intrinsics.ppx) / intrinsics.fx) * zpoint;
			const yPoint = ((y - intrinsics.ppy) / intrinsics.fy) * zpoint;


			const rgb = rgbY[x];
			if (rgb == undefined) {
				throw new Error("color is undefined");
			}

			const position = new Float32Array([xPoint, yPoint, zpoint]);
			const position_uint8 = new Uint8Array(position.buffer);
			//const colors_uint8 = new Uint8Array([rgb.r, rgb.g, rgb.b, 128]);
			const colors_uint8 = new Uint8Array([128, 128, 128, 128]);

			const field_uint8 = new Uint8Array(num_bytes_field);
			field_uint8.set(position_uint8, 0);
			field_uint8.set(colors_uint8, 12);


			const idx = (y * width + x) * num_bytes_field;
			cloud.set(field_uint8, idx);

			//cloud.push([xPoint, yPoint, zpoint, rgb.r, rgb.g, rgb.b, 128]);
		}
	}
	console.log(`Cloud: ${cloud.length}`);
	return cloud;
}

function reshape2d(data: Float32Array, shape: number[]): number[][] {
	// Only supports 2D arrays
	if (shape[0] == undefined || shape[1] == undefined) {
		throw new Error("shape[0] or shape[1] is undefined");
	}
	const reshapedArray: number[][] = [];
	for (let i = 0; i < shape[0]; i++) {
		const row: number[] = [];
		for (let j = 0; j < shape[1]; j++) {
			const flatIdx = i * shape[1] + j;
			const gridIdx = data.slice(flatIdx, flatIdx + 1)[0]; // Assuming row-major order
			if (gridIdx == undefined) {
				throw new Error("Undefined index");
			}
			row.push(gridIdx);
		}
		reshapedArray.push(row);
	}

	return reshapedArray;
}

function decodeFloat32(data: Uint8Array): Float32Array {
	const decodedArray = new Float32Array(data.length / 4);
	const view = new DataView(data.buffer, data.byteOffset, data.byteLength); // Super important to specify byteOffset and byteLength
	for (let i = 0; i < decodedArray.length; i++) {
		const offset = i * 4;
		decodedArray[i] = view.getFloat32(offset, true); // little-endian
	}
	return decodedArray;
}

function getCameraKey(topic: string): string {
	const split_topic = topic.split("/");
	const camera_key: string | undefined = split_topic[split_topic.length - 1];
	if (camera_key == undefined) {
		throw new Error("camera_key is undefined");
	}
	return camera_key;
}

function CameraOutputsToVisionData(
	vision_datas: VisionInputLight[],
	camera_key: string,
): VisionInputLight | undefined {
	for (const vision_data of vision_datas) {
		if (vision_data.camera_key === camera_key) {
			return vision_data;
		}
	}
	return;
}

export function activate(extensionContext: ExtensionContext): void {
	// Alias a single cameras_outputs topic to multiple camera topics.
	// To visualize the different camera inputs with message converters, there must
	// be one topic per camera.
	// This function assumes there are up to three cameras: bottom, top, and middle.
	// Downstream converters should identify the correct camera to process, by splitting the topic name.
	// format: "/cameras/<camera_key>"

	extensionContext.registerTopicAliases(() => {
		return [
			{ sourceTopicName: "/cameras_outputs", name: "/cameras/bottom" },
			{ sourceTopicName: "/cameras_outputs", name: "/cameras/top" },
			{ sourceTopicName: "/cameras_outputs", name: "/cameras/middle" },
		];
	});
	extensionContext.registerMessageConverter({
		fromSchemaName: "dill_rospy_msgs/CamerasOutputs",
		toSchemaName: "foxglove.PointCloud",

		converter: (
			inputMessage: CamerasOutputs,
			event: Immutable<MessageEvent<CamerasOutputs>>,
		): PointCloud | undefined => {
			const camera_key: string = getCameraKey(event.topic);
			const vision_data: VisionInputLight | undefined = CameraOutputsToVisionData(
				inputMessage.cameras_outputs,
				camera_key,
			);
			if (vision_data == undefined) {
				return;
			}

			const decodedDepth = decodeFloat32(vision_data.depth.data);
			const reshapedDepth = reshape2d(decodedDepth, vision_data.depth.shape);
			const rgbImage = getRGBImage(vision_data.bgr);
			const frameCloud = getFrameCloud(reshapedDepth, rgbImage, vision_data.camera_transform.intrinsics);

			const pose = {
				position: { x: 0.0, y: 0.0, z: 0.0 },
				orientation: new Quaternion().identity(),
			};

			const pcMsg: PointCloud = {
				timestamp: { sec: Math.round(vision_data.camera_transform.timestamp), nsec: 0 },
				frame_id: camera_key,
				pose,
				point_stride: 3 * 4 + 4,
				fields: [
					{ name: "x", offset: 0, type: 7 },
					{ name: "y", offset: 4, type: 7 },
					{ name: "z", offset: 8, type: 7 },
					{ name: "red", offset: 12, type: 1 },
					{ name: "green", offset: 13, type: 1 },
					{ name: "blue", offset: 14, type: 1 },
					{ name: "alpha", offset: 15, type: 1 },
				],
				data: new Uint8Array(frameCloud.buffer),
			};

			return pcMsg;
		},
	});
}