[RSDK-1705] add colormap for SLAM UI

web/frontend/package.json

@@ -1,6 +1,6 @@
 {
   "name": "@viamrobotics/remote-control",
-  "version": "0.2.10",
+  "version": "0.2.11",
   "license": "Apache-2.0",
   "type": "module",
   "files": [

web/frontend/src/components/slam-2d-render.vue

@@ -8,6 +8,36 @@ import { MapControls } from 'three/examples/jsm/controls/OrbitControls';
 import { PCDLoader } from 'three/examples/jsm/loaders/PCDLoader';
 import type { commonApi } from '@viamrobotics/sdk';
 
+/*
+ * // Leaving additional color map commented for if we want to change to a different scheme.
+ * // const colorMapViridis = [
+ * //   [253, 231, 37],
+ * //   [181, 222, 43],
+ * //   [110, 206, 88],
+ * //   [53, 183, 121],
+ * //   [31, 158, 137],
+ * //   [38, 130, 142],
+ * //   [49, 104, 142],
+ * //   [62, 73, 137],
+ * //   [72, 40, 120],
+ * //   [68,1,84]
+ * // ]
+ */
+
+// this color map is greyscale
+const colorMapGrey = [
+  [247, 247, 247],
+  [239, 239, 239],
+  [223, 223, 223],
+  [202, 202, 202],
+  [168, 168, 168],
+  [135, 135, 135],
+  [109, 109, 109],
+  [95, 95, 95],
+  [74, 74, 74],
+  [0, 0, 0],
+];
+
 const props = defineProps<{
   name: string
 
@@ -78,6 +108,21 @@ const disposeScene = () => {
   scene.clear();
 };
 
+// Find the desired color bucket for a given probability. This assumes the probability will be a value from 0 to 100
+const probToColorMapBucket = (normProb: number, numBuckets: number): number => {
+  const prob = Math.max(Math.min(100, normProb * 255), 0);
+  return Math.floor((numBuckets - 1) * prob / 100);
+};
+
+/*
+ * Map the color of a pixel to a color bucket value.
+ * normProb is the probability value normalized by the size of a byte(255) to be between 0 to 1.
+ */
+const colorBuckets = (normProb: number): THREE.Vector3 => {
+  return colorMapGrey.map(([red, green, blue]) =>
+    new THREE.Vector3(red, green, blue).multiplyScalar(1 / 255))[probToColorMapBucket(normProb, colorMapGrey.length)]!;
+};
+
 const updateCloud = (pointcloud: Uint8Array) => {
   disposeScene();
 
@@ -110,6 +155,15 @@ const updateCloud = (pointcloud: Uint8Array) => {
   intersectionPlane.position.set(center.x, 0, center.z);
   raycaster.objects = [intersectionPlane];
 
+  const colors = points.geometry.attributes.color;
+  // if the PCD has a color attribute defined, convert those colors using the colorMap
+  if (colors instanceof THREE.BufferAttribute || colors instanceof THREE.InterleavedBufferAttribute) {
+    for (let i = 0; i < colors.count; i += 1) {
+      const colorMapPoint = colorBuckets(colors.getZ(i));
+      colors.setXYZ(i, colorMapPoint.x, colorMapPoint.y, colorMapPoint.z);
+    }
+  }
+
   scene.add(points);
   scene.add(marker);
   scene.add(intersectionPlane);