diff --git a/buf.lock b/buf.lock index 3694d779..87f05d65 100644 --- a/buf.lock +++ b/buf.lock @@ -5,5 +5,5 @@ deps: commit: 62f35d8aed1149c291d606d958a7ce32 digest: b5:d66bf04adc77a0870bdc9328aaf887c7188a36fb02b83a480dc45ef9dc031b4d39fc6e9dc6435120ccf4fe5bfd5c6cb6592533c6c316595571f9a31420ab47fe - name: buf.build/viamrobotics/api - commit: eb251db530ed439a936944cbacd8ba04 - digest: b5:7dae7c86cfd8f0686b5f51fce11dca2523376b1c10175dd3a443215ba5567efe7c376c53e0f470917a5ddf24cf091ee56e8a19e5dee9d18f12090d15417076c5 + commit: 559617baef304105829214c1dee6f94e + digest: b5:e0af4fa98f5b4bf72b973614deb9290b068335623be58f68c02d374a91efd0742e5016a9a1581e33b74a5966231d24f8058544510edaff93812c0e3442b20a7b diff --git a/src/lib/attribute.ts b/src/lib/attribute.ts index 1987b088..11425aed 100644 --- a/src/lib/attribute.ts +++ b/src/lib/attribute.ts @@ -3,6 +3,7 @@ import { BufferAttribute, BufferGeometry } from 'three' import type { Metadata } from './metadata' import { STRIDE } from './buffer' +import type { LODLevel } from './loaders/pcd/messages' export const createBufferGeometry = (positions: Float32Array, { colors, opacities }: Metadata) => { const geometry = new BufferGeometry() @@ -19,6 +20,39 @@ export const createBufferGeometry = (positions: Float32Array, { colors, opacitie return geometry } +export interface LODGeometryLevel { + geometry: BufferGeometry + distance: number +} + +export const createLODGeometries = (levels: LODLevel[]): LODGeometryLevel[] => { + return levels.map((level) => ({ + geometry: createBufferGeometry(level.positions, { colors: level.colors ?? undefined }), + distance: level.distance, + })) +} + +export const updateLODGeometries = ( + existing: LODGeometryLevel[], + levels: LODLevel[] +): LODGeometryLevel[] => { + if (existing.length !== levels.length) { + for (const { geometry } of existing) { + geometry.dispose() + } + return createLODGeometries(levels) + } + + for (let i = 0; i < levels.length; i++) { + updateBufferGeometry(existing[i]!.geometry, levels[i]!.positions, { + colors: levels[i]!.colors ?? undefined, + }) + existing[i]!.distance = levels[i]!.distance + } + + return existing +} + export const updateBufferGeometry = ( geometry: BufferGeometry, positions: Float32Array, diff --git a/src/lib/components/Entities/Points.svelte b/src/lib/components/Entities/Points.svelte index 2b386046..39a0d409 100644 --- a/src/lib/components/Entities/Points.svelte +++ b/src/lib/components/Entities/Points.svelte @@ -3,8 +3,8 @@ import type { Snippet } from 'svelte' import { T, useTask, useThrelte } from '@threlte/core' - import { Portal } from '@threlte/extras' - import { OrthographicCamera, Points, PointsMaterial } from 'three' + import { Detailed, Portal } from '@threlte/extras' + import { LOD, OrthographicCamera, Points, PointsMaterial } from 'three' import { asColor, isSingleColor } from '$lib/buffer' import { traits, useTrait } from '$lib/ecs' @@ -26,21 +26,32 @@ const parent = useTrait(() => entity, traits.Parent) const pose = useTrait(() => entity, traits.Pose) const geometry = useTrait(() => entity, traits.BufferGeometry) + const lodData = useTrait(() => entity, traits.PointCloudLOD) + const opacity = useTrait(() => entity, traits.Opacity) const entityColor = useTrait(() => entity, traits.Color) const colors = useTrait(() => entity, traits.Colors) const entityPointSize = useTrait(() => entity, traits.PointSize) - const opacity = useTrait(() => entity, traits.Opacity) const invisible = useTrait(() => entity, traits.Invisible) const pointSize = $derived( entityPointSize.current ? entityPointSize.current * 0.001 : settings.current.pointSize ) const orthographic = $derived(settings.current.cameraMode === 'orthographic') + const hasLOD = $derived(lodData.current !== undefined && lodData.current.levels.length > 0) const points = new Points() const material = points.material as PointsMaterial material.toneMapped = false + let lodRef = $state() + + const lodPoints = $derived( + lodData.current?.levels.map((level) => ({ + points: new Points(level.geometry, material), + distance: level.distance, + })) ?? [] + ) + $effect.pre(() => { material.size = pointSize }) @@ -98,7 +109,7 @@ $effect.pre(() => { if (pose.current) { - poseToObject3d(pose.current, points) + poseToObject3d(pose.current, hasLOD && lodRef ? lodRef : points) } }) @@ -123,8 +134,24 @@ }) -{#if geometry.current} - + + {#if hasLOD} + + {#each lodPoints as { points: childPoints, distance } (childPoints)} + + {/each} + {@render children?.()} + + {:else if geometry.current} {@render children?.()} - -{/if} + {/if} + diff --git a/src/lib/ecs/traits.ts b/src/lib/ecs/traits.ts index d4da41a8..9b8b3a17 100644 --- a/src/lib/ecs/traits.ts +++ b/src/lib/ecs/traits.ts @@ -4,6 +4,8 @@ import { Geometry as ViamGeometry } from '@viamrobotics/sdk' import { type Entity, trait } from 'koota' import { BufferGeometry as ThreeBufferGeometry } from 'three' +import type { LODGeometryLevel } from '$lib/attribute' + import { createBox, createCapsule, createSphere } from '$lib/geometry' import { parsePlyInput } from '$lib/ply' @@ -110,6 +112,11 @@ export const Sphere = trait({ r: 200 }) export const BufferGeometry = trait(() => new ThreeBufferGeometry()) +export const PointCloudLOD = trait(() => ({ + levels: [] as LODGeometryLevel[], + diagonal: 0, +})) + export const GLTF = trait(() => ({ source: { url: '' } as { url: string } | { gltf: ThreeGltf } | { glb: Uint8Array }, animationName: '', diff --git a/src/lib/hooks/usePointclouds.svelte.ts b/src/lib/hooks/usePointclouds.svelte.ts index e5eaca7c..c4c17695 100644 --- a/src/lib/hooks/usePointclouds.svelte.ts +++ b/src/lib/hooks/usePointclouds.svelte.ts @@ -1,4 +1,4 @@ -import type { Entity } from 'koota' +import type { ConfigurableTrait, Entity } from 'koota' import { CameraClient } from '@viamrobotics/sdk' import { @@ -8,10 +8,15 @@ import { } from '@viamrobotics/svelte-sdk' import { getContext, setContext, untrack } from 'svelte' -import { createBufferGeometry, updateBufferGeometry } from '$lib/attribute' +import { + createBufferGeometry, + createLODGeometries, + updateBufferGeometry, + updateLODGeometries, +} from '$lib/attribute' import { RefetchRates } from '$lib/components/overlay/RefreshRate.svelte' import { traits, useWorld } from '$lib/ecs' -import { parsePcdInWorker } from '$lib/loaders/pcd' +import { parsePcdWithLOD } from '$lib/loaders/pcd' import { useEnvironment } from './useEnvironment.svelte' import { useLogs } from './useLogs.svelte' @@ -140,35 +145,57 @@ export const providePointclouds = (partID: () => string) => { } } - parsePcdInWorker(data) - .then(({ positions, colors }) => { + parsePcdWithLOD(data) + .then(({ levels, boundingBoxDiagonal }) => { if (disposed) { return } const existing = entities.get(queryKey) + const finest = levels.find((l) => l.level === 0) ?? levels[0]! const metadata = { - colors: colors ?? undefined, + colors: finest.colors ?? undefined, } if (existing) { const geometry = existing.get(traits.BufferGeometry) + const existingLOD = existing.get(traits.PointCloudLOD) if (geometry) { - updateBufferGeometry(geometry, positions, metadata) + updateBufferGeometry(geometry, finest.positions, metadata) return } + + if (existingLOD && levels.length > 1) { + // Update geometry buffers in place without setting the trait + // to avoid triggering re-renders and component remounts. + // BVH is not recomputed here — it drifts slightly between + // frames but avoids expensive main-thread recomputation. + updateLODGeometries(existingLOD.levels, levels) + } + + return } - const geometry = createBufferGeometry(positions, metadata) + const geometry = createBufferGeometry(finest.positions, metadata) - const entity = world.spawn( + const entityTraits: ConfigurableTrait[] = [ traits.Parent(name), traits.Name(`${name} pointcloud`), traits.BufferGeometry(geometry), - traits.Points - ) + traits.Points, + ] + + if (levels.length > 1) { + entityTraits.push( + traits.PointCloudLOD({ + levels: createLODGeometries(levels), + diagonal: boundingBoxDiagonal, + }) + ) + } + const entity = world.spawn(...entityTraits) entities.set(queryKey, entity) }) .catch((error) => { diff --git a/src/lib/loaders/pcd/index.ts b/src/lib/loaders/pcd/index.ts index 42fbe0ca..4e68384b 100644 --- a/src/lib/loaders/pcd/index.ts +++ b/src/lib/loaders/pcd/index.ts @@ -1,4 +1,4 @@ -import type { Message, SuccessMessage } from './messages' +import type { LODLevel, Message, SuccessMessage } from './messages' import { workerCode } from './worker.inline' @@ -6,37 +6,103 @@ const blob = new Blob([workerCode], { type: 'text/javascript' }) const url = URL.createObjectURL(blob) const worker = new Worker(url) +export interface LODResult { + levels: LODLevel[] + boundingBoxDiagonal: number +} + let requestId = 0 -const pending = new Map< - number, - { - resolve: (msg: SuccessMessage) => void - reject: (err: string) => void - } ->() + +type PendingEntry = + | { + mode: 'simple' + resolve: (msg: SuccessMessage) => void + reject: (err: string) => void + } + | { + mode: 'lod' + resolve: (result: LODResult) => void + reject: (err: string) => void + onProgress?: (level: LODLevel) => void + levels: LODLevel[] + diagonal: number + } + +const pending = new Map() worker.addEventListener('message', (event: MessageEvent) => { - const { id, ...rest } = event.data as Message + const msg = event.data + + const entry = pending.get(msg.id) + if (!entry) return + + if ('error' in msg) { + pending.delete(msg.id) + entry.reject(msg.error) + return + } - const promise = pending.get(id) + if ('lod' in msg) { + // Progressive LOD message + if (entry.mode === 'lod') { + entry.levels.push(msg.lod) + entry.diagonal = msg.boundingBoxDiagonal + entry.onProgress?.(msg.lod) - if (!promise) { + if (msg.done) { + pending.delete(msg.id) + entry.resolve({ + levels: entry.levels.sort((a, b) => a.level - b.level), + boundingBoxDiagonal: entry.diagonal, + }) + } + } else { + // Simple mode receiving LOD messages — accumulate and resolve with finest level + if (!('_levels' in entry)) { + ;(entry as PendingEntry & { _levels: LODLevel[] })._levels = [] + } + const extended = entry as PendingEntry & { _levels: LODLevel[] } + extended._levels.push(msg.lod) + + if (msg.done) { + pending.delete(msg.id) + const finest = extended._levels.find((l) => l.level === 0) ?? extended._levels[0]! + entry.resolve({ id: msg.id, positions: finest.positions, colors: finest.colors }) + } + } return } - pending.delete(id) + // Legacy single-message response (small cloud) + pending.delete(msg.id) - if ('error' in rest) { - promise.reject(rest.error) + if (entry.mode === 'lod') { + entry.resolve({ + levels: [{ level: 0, distance: 0, positions: msg.positions, colors: msg.colors }], + boundingBoxDiagonal: 0, + }) } else { - promise.resolve(rest as SuccessMessage) + entry.resolve(msg as SuccessMessage) } }) export const parsePcdInWorker = (data: Uint8Array): Promise => { return new Promise((resolve, reject) => { const id = ++requestId - pending.set(id, { resolve, reject }) + pending.set(id, { mode: 'simple', resolve, reject }) + + const copy = new Uint8Array(data) + worker.postMessage({ id, data: copy }, [copy.buffer]) + }) +} + +export const parsePcdWithLOD = ( + data: Uint8Array, + onProgress?: (level: LODLevel) => void +): Promise => { + return new Promise((resolve, reject) => { + const id = ++requestId + pending.set(id, { mode: 'lod', resolve, reject, onProgress, levels: [], diagonal: 0 }) const copy = new Uint8Array(data) worker.postMessage({ id, data: copy }, [copy.buffer]) diff --git a/src/lib/loaders/pcd/messages.ts b/src/lib/loaders/pcd/messages.ts index 81d5d81e..b84c64f7 100644 --- a/src/lib/loaders/pcd/messages.ts +++ b/src/lib/loaders/pcd/messages.ts @@ -4,8 +4,23 @@ export interface SuccessMessage { colors: Uint8Array | null } +export interface LODLevel { + level: number + distance: number + positions: Float32Array + colors: Uint8Array | null +} + +export interface LODProgressMessage { + id: number + lod: LODLevel + done: boolean + boundingBoxDiagonal: number +} + export type Message = | SuccessMessage + | LODProgressMessage | { id: number error: string diff --git a/src/lib/loaders/pcd/worker.ts b/src/lib/loaders/pcd/worker.ts index 5ffe565e..df5e7ea7 100644 --- a/src/lib/loaders/pcd/worker.ts +++ b/src/lib/loaders/pcd/worker.ts @@ -1,8 +1,203 @@ +import { Vector3 } from 'three' import { PCDLoader } from 'three/examples/jsm/loaders/PCDLoader.js' -import type { Message } from './messages' +import type { LODProgressMessage, Message } from './messages' const loader = new PCDLoader() +const size = new Vector3() + +const LOD_THRESHOLD = 100_000 + +/** + * Spatially-aware point cloud downsampling using a voxel grid, sized to + * approximate a target point count. + * + * The algorithm divides the bounding box into a uniform 3D grid of cubic cells. + * For each input point, it computes which cell the point falls into. If that + * cell hasn't been claimed yet, the point is kept; otherwise it's discarded. + * The result is at most one point per cell — a fast approximation of Poisson + * disk subsampling (and the same technique used by PCL and Open3D for point + * cloud downsampling). + * + * The cell size is derived from a target fraction of points to keep: + * + * targetCount = numPoints × fraction + * cellSize = ∛(volume / targetCount) + * + * This sizes each cell so that, for a uniform distribution, roughly one point + * lands per cell — yielding approximately `targetCount` output points. The + * actual output count depends on the spatial distribution: + * + * - Dense clusters are thinned aggressively (many points share a cell). + * - Sparse, isolated points are preserved (each occupies its own cell). + * + * This is exactly the behavior we want for LOD: redundant points in dense + * areas are removed first, while points that define the shape's structure + * in sparse areas are kept. + * + * The output count is approximate, not exact. For LOD purposes this is fine — + * what matters is spatial quality, not hitting a precise number. + * + * Cells are identified by encoding their (cx, cy, cz) grid coordinates into a + * single integer key: `cx + cy * gridSizeX + cz * gridSizeX * gridSizeY`. + * A Map tracks which cells are occupied (first point wins). + * + * Performance: O(n) — one pass to bucket points, one pass to extract results. + * + * @param positions - Source positions (3 floats per point: x, y, z) + * @param colors - Source colors, or null if the cloud has no color data + * @param cellSize - Side length of each cubic voxel cell + * @param minX - Bounding box minimum X + * @param minY - Bounding box minimum Y + * @param minZ - Bounding box minimum Z + * @param rangeX - Bounding box extent in X (maxX - minX) + * @param rangeY - Bounding box extent in Y (maxY - minY) + * @param colorStride - Number of color components per point (3 for RGB, 4 for RGBA) + */ +const voxelDownsample = ( + positions: Float32Array, + colors: Uint8Array | null, + cellSize: number, + minX: number, + minY: number, + minZ: number, + rangeX: number, + rangeY: number, + colorStride: number +): { positions: Float32Array; colors: Uint8Array | null } => { + const numPoints = positions.length / 3 + const gridSizeX = Math.ceil(rangeX / cellSize) + 1 + const gridSizeY = Math.ceil(rangeY / cellSize) + 1 + + const occupied = new Map() + + for (let i = 0; i < numPoints; i++) { + const cx = Math.floor((positions[i * 3]! - minX) / cellSize) + const cy = Math.floor((positions[i * 3 + 1]! - minY) / cellSize) + const cz = Math.floor((positions[i * 3 + 2]! - minZ) / cellSize) + const key = cx + cy * gridSizeX + cz * gridSizeX * gridSizeY + + if (!occupied.has(key)) { + occupied.set(key, i) + } + } + + const outPositions = new Float32Array(occupied.size * 3) + const outColors = colors ? new Uint8Array(occupied.size * colorStride) : null + + let j = 0 + for (const idx of occupied.values()) { + outPositions[j * 3] = positions[idx * 3]! + outPositions[j * 3 + 1] = positions[idx * 3 + 1]! + outPositions[j * 3 + 2] = positions[idx * 3 + 2]! + + if (outColors && colors) { + const srcOff = idx * colorStride + const dstOff = j * colorStride + for (let c = 0; c < colorStride; c++) { + outColors[dstOff + c] = colors[srcOff + c]! + } + } + + j++ + } + + return { positions: outPositions, colors: outColors } +} + +/** + * Counts how many voxel cells would be occupied at a given cell size, + * without allocating output arrays. Used to calibrate cell size before + * running the full downsampling pass. + */ +const countOccupied = ( + positions: Float32Array, + numPoints: number, + cellSize: number, + minX: number, + minY: number, + minZ: number, + rangeX: number, + rangeY: number +): number => { + const gridSizeX = Math.ceil(rangeX / cellSize) + 1 + const gridSizeY = Math.ceil(rangeY / cellSize) + 1 + const occupied = new Set() + + for (let i = 0; i < numPoints; i++) { + const cx = Math.floor((positions[i * 3]! - minX) / cellSize) + const cy = Math.floor((positions[i * 3 + 1]! - minY) / cellSize) + const cz = Math.floor((positions[i * 3 + 2]! - minZ) / cellSize) + occupied.add(cx + cy * gridSizeX + cz * gridSizeX * gridSizeY) + } + + return occupied.size +} + +/** + * Finds a voxel cell size that produces approximately `targetCount` output + * points, then runs the full downsampling at that cell size. + * + * Starts from an initial estimate (cbrt of volume / target) and iteratively + * adjusts: counts occupied cells, then scales the cell size by the overshoot + * ratio raised to a fractional power (~1/2.5, a compromise between the + * quadratic scaling of surface data and the cubic scaling of volumetric data). + * Converges in 2-3 iterations for typical point clouds. + */ +const voxelDownsampleToTarget = ( + positions: Float32Array, + colors: Uint8Array | null, + targetFraction: number, + volume: number, + minX: number, + minY: number, + minZ: number, + rangeX: number, + rangeY: number, + colorStride: number +): { positions: Float32Array; colors: Uint8Array | null } => { + const numPoints = positions.length / 3 + const targetCount = numPoints * targetFraction + + let cellSize = Math.cbrt(volume / targetCount) + + // Iteratively calibrate cell size to hit the target count + for (let iter = 0; iter < 3; iter++) { + const count = countOccupied( + positions, numPoints, cellSize, minX, minY, minZ, rangeX, rangeY + ) + + const ratio = count / targetCount + if (ratio > 0.9 && ratio < 1.1) break + + cellSize *= Math.pow(ratio, 1 / 2.5) + } + + return voxelDownsample( + positions, colors, cellSize, minX, minY, minZ, rangeX, rangeY, colorStride + ) +} + +const sendLODLevel = ( + id: number, + level: number, + distance: number, + positions: Float32Array, + colors: Uint8Array | null, + done: boolean, + diagonal: number +) => { + const msg: LODProgressMessage = { + id, + lod: { level, distance, positions, colors }, + done, + boundingBoxDiagonal: diagonal, + } + + const transfer: ArrayBuffer[] = [positions.buffer] + if (colors) transfer.push(colors.buffer) + postMessage(msg, transfer) +} globalThis.onmessage = async (event) => { const { data, id } = event.data @@ -31,10 +226,43 @@ globalThis.onmessage = async (event) => { } } - postMessage( - { positions, colors, id } satisfies Message, - colors ? [positions.buffer, colors.buffer] : [positions.buffer] - ) + const numPoints = positions.length / 3 + + if (numPoints >= LOD_THRESHOLD) { + pcd.geometry.computeBoundingBox() + const bbox = pcd.geometry.boundingBox! + bbox.getSize(size) + const diagonal = size.length() + + const colorStride = colors ? colors.length / numPoints : 0 + const { x: minX, y: minY, z: minZ } = bbox.min + const volume = Math.max(size.x * size.y * size.z, 1e-10) + + const lodConfigs = [ + { level: 4, fraction: 0.10, distance: Math.min(diagonal * 4, 15) }, + { level: 3, fraction: 0.35, distance: Math.min(diagonal * 1, 4) }, + { level: 2, fraction: 0.65, distance: Math.min(diagonal * 0.25, 1) }, + { level: 1, fraction: 0.90, distance: Math.min(diagonal * 0.05, 0.25) }, + ] + + // Send coarsest to finest + for (const { level, fraction, distance } of lodConfigs) { + const result = voxelDownsampleToTarget( + positions, colors, fraction, volume, + minX, minY, minZ, size.x, size.y, colorStride + ) + sendLODLevel(id, level, distance, result.positions, result.colors, false, diagonal) + } + + // LOD 0 (full resolution) + sendLODLevel(id, 0, 0, positions, colors, true, diagonal) + } else { + // Small cloud — single message, no LOD + postMessage( + { positions, colors, id } satisfies Message, + colors ? [positions.buffer, colors.buffer] : [positions.buffer] + ) + } } else { postMessage({ id, error: 'Failed to extract geometry' } satisfies Message) }