diff --git a/README.en.md b/README.en.md index 21eb010..26869e8 100644 --- a/README.en.md +++ b/README.en.md @@ -128,6 +128,7 @@ BicMap is built on top of the following open-source projects: * [Vue 3](https://vuejs.org) + [Vite](https://vite.dev) — Example portal and build toolchain * [urdf-loader](https://github.com/gkjohnson/urdf-loaders) — URDF robot model loading * [Fabric.js](https://fabricjs.com) — Canvas-based drawing engine for the map editor in the example portal +* [PCL.js](https://github.com/PointCloudLibrary/pcl) — Point cloud processing via WebAssembly ## Contributors diff --git a/README.md b/README.md index 6c81945..b7ea15f 100644 --- a/README.md +++ b/README.md @@ -128,6 +128,7 @@ BicMap 构建于以下开源项目之上： * [Vue 3](https://vuejs.org) + [Vite](https://vite.dev) — 示例门户与构建工具链 * [urdf-loader](https://github.com/gkjohnson/urdf-loaders) — URDF 机器人模型加载 * [Fabric.js](https://fabricjs.com) — 示例门户中地图编辑器的 Canvas 绘图引擎 +* [PCL.js](https://github.com/PointCloudLibrary/pcl) — 点云处理 ## Contributors diff --git a/package.json b/package.json index 07de53f..b5d912d 100644 --- a/package.json +++ b/package.json @@ -73,6 +73,7 @@ "maplibre-gl": "3.6.2", "three": "0.149.0", "urdf-loader": "^0.12.5", + "pcl.js": "^1.16.0", "vue": "^3.5.13" }, "devDependencies": { diff --git a/pnpm-lock.yaml b/pnpm-lock.yaml index a7937c0..c3c0819 100644 --- a/pnpm-lock.yaml +++ b/pnpm-lock.yaml @@ -47,6 +47,9 @@ importers: lucide-vue-next: specifier: ^0.577.0 version: 0.577.0(vue@3.5.13) + pcl.js: + specifier: ^1.16.0 + version: 1.16.0 sass-embedded: specifier: ^1.99.0 version: 1.99.0 @@ -1429,6 +1432,10 @@ packages: resolution: {integrity: sha512-XDF38WCH3z5OV/OVa8GKUNtLAyneuzbCisx7QUCF8Q6Nutx0WnJrQe5O+kOtBlLfRNUws98Y58Lblp+NJG5T4Q==} hasBin: true + pcl.js@1.16.0: + resolution: {integrity: sha512-/Us4by9meeEqG4EkB2T2S0VqpWdxREPqsgVhRmt4jdiXf5wrMkbR650II3hjjxkhmnIzVa6JfhWPITz1Fy1kyg==} + engines: {node: '>=14.0.0'} + perfect-debounce@2.1.0: resolution: {integrity: sha512-LjgdTytVFXeUgtHZr9WYViYSM/g8MkcTPYDlPa3cDqMirHjKiSZPYd6DoL7pK8AJQr+uWkQvCjHNdiMqsrJs+g==} @@ -4047,6 +4054,10 @@ snapshots: ieee754: 1.2.1 resolve-protobuf-schema: 2.1.0 + pcl.js@1.16.0: + dependencies: + three: 0.149.0 + perfect-debounce@2.1.0: {} picocolors@1.1.1: {} diff --git a/src/bicMap/core/pointClouds/pointCloud3D.js b/src/bicMap/core/pointClouds/pointCloud3D.js index 0a80035..5f4be52 100644 --- a/src/bicMap/core/pointClouds/pointCloud3D.js +++ b/src/bicMap/core/pointClouds/pointCloud3D.js @@ -12,6 +12,9 @@ const DEFAULT_OPTIONS = { [1, '#ff5a5a'] ], zRange: [0, 5], + // 逐点颜色 Float32Array（长度 = 点数 × 3，分量 0~1）。传入后优先于 pointColor / colorMap， + // 供调用方自带 RGB / 强度等着色结果的场景使用 + colors: null, heightScale: 1, heightOffset: 0, sizeAttenuation: false, @@ -56,6 +59,7 @@ function getColorFromZ(z, range, colorMap) { * @param {Object} map - maplibre 地图实例 * @param {Array<[number,number,number?]>} points - 点云数据 [lng, lat, z?] * @param {Object} [options] + * @param {Float32Array} [options.colors] - 逐点颜色（长度 = 点数 × 3，分量 0~1），优先级高于 pointColor / colorMap * @returns {Object} 控制器：update / show / hide / remove / getPoints / getOptions */ export function createPointCloud3D(map, points = [], options = {}) { @@ -112,13 +116,13 @@ export function createPointCloud3D(map, points = [], options = {}) { const colors = new Float32Array(count * 3) const baseColor = hexToRgb(opts.pointColor) const invMPM = 1 / metersPerMercator + const srcColors = opts.colors && opts.colors.length >= count * 3 ? opts.colors : null for (let i = 0; i < count; i++) { const p = pointsData[i] const z = p.length > 2 && p[2] !== undefined ? p[2] : 0 const altitude = z * opts.heightScale + opts.heightOffset const mc = maplibregl.MercatorCoordinate.fromLngLat({ lng: p[0], lat: p[1] }, altitude) - const color = opts.useColorMap ? getColorFromZ(z, opts.zRange, opts.colorMap) : baseColor const o = i * 3 // 东向米：mercator x 正方向即东，直接用 @@ -127,9 +131,17 @@ export function createPointCloud3D(map, points = [], options = {}) { positions[o + 1] = -(mc.y - originMC.y) * invMPM // 上向米：mercator z 正方向即高度，直接用 positions[o + 2] = (mc.z - originMC.z) * invMPM - colors[o] = color[0] - colors[o + 1] = color[1] - colors[o + 2] = color[2] + + if (srcColors) { + colors[o] = srcColors[o] + colors[o + 1] = srcColors[o + 1] + colors[o + 2] = srcColors[o + 2] + } else { + const color = opts.useColorMap ? getColorFromZ(z, opts.zRange, opts.colorMap) : baseColor + colors[o] = color[0] + colors[o + 1] = color[1] + colors[o + 2] = color[2] + } } if (geometry) geometry.dispose() diff --git a/src/examples/assets/home-thum/expand_PcdViewer.png b/src/examples/assets/home-thum/expand_PcdViewer.png new file mode 100644 index 0000000..0a39efe Binary files /dev/null and b/src/examples/assets/home-thum/expand_PcdViewer.png differ diff --git a/src/examples/expand/PcdViewer/constants.js b/src/examples/expand/PcdViewer/constants.js new file mode 100644 index 0000000..1307645 --- /dev/null +++ b/src/examples/expand/PcdViewer/constants.js @@ -0,0 +1,107 @@ +/* + * @Description: PCD 点云查看器常量配置（默认参数、配色、SLAM 底图与对齐参数） + * @FilePath: src/examples/expand/PcdViewer/constants.js + */ + +// 点大小（three.js PointsMaterial.size，单位像素） +export const DEFAULT_POINT_SIZE = 1.3 +export const MIN_POINT_SIZE = 0.3 +export const MAX_POINT_SIZE = 6 +export const POINT_SIZE_STEP = 0.1 + +// 着色模式 +export const COLOR_MODE = { + HEIGHT: 'height', // 按高度渐变 + RGB: 'rgb', // 使用点云自带 RGB + INTENSITY: 'intensity', // 使用点云自带反射强度 + SINGLE: 'single' // 单一颜色 +} + +export const DEFAULT_COLOR_MODE = COLOR_MODE.RGB + +export const COLOR_MODE_LABEL = { + [COLOR_MODE.HEIGHT]: '高度', + [COLOR_MODE.RGB]: 'RGB', + [COLOR_MODE.INTENSITY]: '强度', + [COLOR_MODE.SINGLE]: '单色' +} + +// 高度渐变配色（t ∈ [0,1] → 颜色），近似 turbo 色带，低→高 +export const HEIGHT_COLOR_STOPS = [ + [0, '#3b4cc0'], + [0.25, '#22c1dc'], + [0.5, '#48d17a'], + [0.75, '#f4d03f'], + [1, '#e74c3c'] +] + +// 单一着色模式下的点颜色 +export const SINGLE_COLOR = '#38e1ff' + +// 无底图时用黑色托点云 +export const BG_COLOR = '#051230cc' + +// SLAM 底图与 campus_vbr.pcd 共用同一套栅格：一个像素一格，XY 原位贴合。 +// resolution 0.2 对应源点云约 0.15~0.2m 的平面间距 +export const SLAM_MAP = { + startX: -163, + startY: -136, + xGridCount: 1608, + yGridCount: 1358, + resolution: 0.2, + zoomFactor: 2 +} + +// SLAM 底图在笛卡尔坐标系下的跨度与中心（米） +export const SLAM_WIDTH = SLAM_MAP.xGridCount * SLAM_MAP.resolution +export const SLAM_HEIGHT = SLAM_MAP.yGridCount * SLAM_MAP.resolution +export const SLAM_CENTER = [SLAM_MAP.startX + SLAM_WIDTH / 2, SLAM_MAP.startY + SLAM_HEIGHT / 2] + +// cartesianToGPS 会把 SLAM 米数再乘一次 resolution × zoomFactor 才落到真实经纬度， +// 高度方向必须用同一系数，否则点云会被竖直拉伸 +export const SLAM_GEO_SCALE = SLAM_MAP.resolution * SLAM_MAP.zoomFactor + +// 内置场景点云。pcd 放在本目录 samples/，由 index.vue 用 Vite ?url 引入， +// 这样 dev / build:web 都会打进产物，npm 库发布不会带上示例资源。 +// 底图是点云的俯视投影，XY 原位贴合；但点云是 2.5D 高度场，最低点贴地后 +// 主体（墙体/楼面，大约 7~8m）仍悬在平面底图上方，所以要再压一截离地高度。 +export const SCENE_PRESETS = [ + { id: 'campus', label: '园区扫描' } +] + +// 点云在 SLAM 坐标系中的摆放参数：x/y 为平移量（米）、rotation 为绕 Z 轴角度、z 为离地高度。 +// 默认零变换给外部 PCD；内置场景的额外偏移写在 SCENE_PRESETS[].align +export const DEFAULT_ALIGN = { + x: 0, + y: 0, + z: 0, + rotation: 0, + scale: 1 +} + +// 平移滑块量程：以底图跨度为界，够把点云从一角推到另一角 +export const OFFSET_X_RANGE = [-SLAM_WIDTH, SLAM_WIDTH] +export const OFFSET_Y_RANGE = [-SLAM_HEIGHT, SLAM_HEIGHT] + +export const ALIGN_STEP = 0.5 +export const ROTATION_STEP = 1 +export const SCALE_RANGE = [0.1, 5] +export const SCALE_STEP = 0.1 +export const HEIGHT_RANGE = [-10, 30] +export const HEIGHT_STEP = 0.5 + +// 初始视角，三项互不影响，改完刷新或点「重置视角」生效 +export const MAP_VIEW = { + pitch: 60, // 俯仰角（度），0 正俯视，越大越斜 + bearing: -15, // 旋转角（度），顺时针，0 为正北 + zoom: 19 // 缩放；null 表示按底图自适应 +} + +// 体素降采样：叶子尺寸（米），0 表示关闭 +export const DEFAULT_LEAF_SIZE = 0 +export const MAX_LEAF_SIZE = 0.5 +export const LEAF_SIZE_STEP = 0.01 + +// 统计离群点去除（StatisticalOutlierRemoval）参数 +export const SOR_MEAN_K = 30 +export const SOR_STDDEV_MUL = 1 diff --git a/src/examples/expand/PcdViewer/index.vue b/src/examples/expand/PcdViewer/index.vue new file mode 100644 index 0000000..d0b1e72 --- /dev/null +++ b/src/examples/expand/PcdViewer/index.vue @@ -0,0 +1,868 @@ + + + + + + diff --git a/src/examples/expand/PcdViewer/pclLoader.js b/src/examples/expand/PcdViewer/pclLoader.js new file mode 100644 index 0000000..8801a90 --- /dev/null +++ b/src/examples/expand/PcdViewer/pclLoader.js @@ -0,0 +1,410 @@ +/* + * @Description: PCD 解析与滤波。未压缩 binary 由 JS 直读；体素降采样 / 去噪走 pcl.js WASM。 + * @FilePath: src/examples/expand/PcdViewer/pclLoader.js + */ +import * as PCL from 'pcl.js' +import pclWasmUrl from 'pcl.js/pcl-core.wasm?url' + +import { SOR_MEAN_K, SOR_STDDEV_MUL } from './constants' + +// PCD 头里出现这些字段即认为点云自带颜色 +const RGB_FIELDS = ['rgb', 'rgba'] + +let initPromise = null + +/** + * 初始化 pcl.js 的 WASM 运行时，全局只执行一次 + * @returns {Promise} + */ +export function initPcl() { + if (!initPromise) { + // 不传 url 时 pcl.js 会按脚本目录去猜 wasm 位置，打包后必然 404，这里显式给出产物地址 + initPromise = PCL.init({ url: pclWasmUrl }).catch((error) => { + initPromise = null + throw error + }) + } + return initPromise +} + +/** + * 下载并解析 PCD 文件 + * @param {string} url PCD 文件地址 + * @param {object} [options] 见 parsePcd + * @returns {Promise} 见 parsePcd + */ +export async function loadPcdFromUrl(url, options) { + const response = await fetch(url) + if (!response.ok) throw new Error(`PCD 下载失败：${response.status} ${response.statusText}`) + return parsePcd(await response.arrayBuffer(), options) +} + +/** + * 解析 PCD。未压缩 binary 默认走 JS：campus_vbr 这类同时带 intensity+rgb 的点， + * pcl.js 没有对应点类型，loadPCDData 会在浏览器里把 C++ 异常抛成数字（例如 5766664）。 + * 体素降采样 / 去噪仍走 WASM。 + * @param {ArrayBuffer} buffer PCD 文件内容，支持 ascii / binary / binary_compressed + * @param {object} [options] + * @param {number} [options.leafSize=0] 体素栅格叶子尺寸（米），>0 时启用降采样 + * @param {boolean} [options.denoise=false] 是否做统计离群点去除 + * @returns {Promise<{ + * positions: Float32Array, + * colors: (Float32Array|null), + * intensities: (Float32Array|null), + * count: number, + * rawCount: number, + * bounds: object, + * fields: string[], + * dataType: string, + * cost: number + * }>} + */ +export async function parsePcd(buffer, options = {}) { + const { leafSize = 0, denoise = false } = options + const startedAt = performance.now() + const needsFilter = leafSize > 0 || denoise + + if (!needsFilter) { + const parsed = parseUncompressedPcd(buffer) + if (parsed) { + return { + ...parsed, + rawCount: parsed.count, + cost: Math.round(performance.now() - startedAt) + } + } + } + + try { + return await parsePcdWithPcl(buffer, { leafSize, denoise, startedAt }) + } catch (error) { + const fallback = parseUncompressedPcd(buffer) + if (fallback) { + console.warn('PCL 滤波失败，已回退为未滤波解析:', error) + return { + ...fallback, + rawCount: fallback.count, + cost: Math.round(performance.now() - startedAt) + } + } + throw wrapPclError(error) + } +} + +/** + * 把 pcl.js / Emscripten 抛出的指针数字转成可读错误 + * @param {unknown} error + * @returns {Error} + */ +function wrapPclError(error) { + if (error instanceof Error) return error + return new Error(`PCD 解析失败（PCL ${String(error)}）`) +} + +/** + * 解析未压缩 binary PCD。ascii / binary_compressed 返回 null，交给 PCL。 + * @param {ArrayBuffer} buffer + * @returns {object|null} + */ +function parseUncompressedPcd(buffer) { + const bytes = buffer instanceof Uint8Array ? buffer : new Uint8Array(buffer) + const header = readPcdHeader(bytes) + if (!header || header.dataType !== 'binary') return null + + const { fields, sizes, counts, pointCount, dataOffset } = header + const offsets = {} + let stride = 0 + fields.forEach((field, i) => { + offsets[field] = stride + stride += sizes[i] * counts[i] + }) + if (offsets.x === undefined || offsets.y === undefined || offsets.z === undefined) return null + if (dataOffset + pointCount * stride > bytes.byteLength) { + throw new Error('PCD 文件不完整：数据区短于声明点数') + } + + const view = new DataView(bytes.buffer, bytes.byteOffset + dataOffset) + const rgbField = RGB_FIELDS.find((field) => offsets[field] !== undefined) + const hasIntensity = offsets.intensity !== undefined + + const positions = new Float32Array(pointCount * 3) + const colors = rgbField ? new Float32Array(pointCount * 3) : null + const intensities = hasIntensity ? new Float32Array(pointCount) : null + const min = [Infinity, Infinity, Infinity] + const max = [-Infinity, -Infinity, -Infinity] + let intensityMin = Infinity + let intensityMax = -Infinity + let kept = 0 + + for (let i = 0; i < pointCount; i++) { + const base = i * stride + const x = view.getFloat32(base + offsets.x, true) + const y = view.getFloat32(base + offsets.y, true) + const z = view.getFloat32(base + offsets.z, true) + if (!Number.isFinite(x) || !Number.isFinite(y) || !Number.isFinite(z)) continue + + const dest = kept * 3 + positions[dest] = x + positions[dest + 1] = y + positions[dest + 2] = z + if (x < min[0]) min[0] = x + if (y < min[1]) min[1] = y + if (z < min[2]) min[2] = z + if (x > max[0]) max[0] = x + if (y > max[1]) max[1] = y + if (z > max[2]) max[2] = z + + if (colors) { + const packed = view.getUint32(base + offsets[rgbField], true) + colors[dest] = ((packed >> 16) & 255) / 255 + colors[dest + 1] = ((packed >> 8) & 255) / 255 + colors[dest + 2] = (packed & 255) / 255 + } + + if (intensities) { + const intensity = view.getFloat32(base + offsets.intensity, true) + intensities[kept] = intensity + if (intensity < intensityMin) intensityMin = intensity + if (intensity > intensityMax) intensityMax = intensity + } + + kept++ + } + + return { + positions: kept === pointCount ? positions : positions.subarray(0, kept * 3), + colors: colors && kept === pointCount ? colors : colors?.subarray(0, kept * 3) ?? null, + intensities: intensities && kept === pointCount ? intensities : intensities?.subarray(0, kept) ?? null, + count: kept, + bounds: buildBounds(min, max, kept, intensityMin, intensityMax), + fields, + dataType: header.dataType + } +} + +/** + * 读 PCD 文本头，定位 DATA 行 + * @param {Uint8Array} bytes + * @returns {{ fields: string[], sizes: number[], counts: number[], pointCount: number, dataType: string, dataOffset: number }|null} + */ +function readPcdHeader(bytes) { + const marker = indexOfDataLine(bytes) + if (!marker) return null + const header = new TextDecoder('ascii').decode(bytes.subarray(0, marker.lineEnd)) + const fields = /FIELDS (.+)/.exec(header)?.[1].trim().split(/\s+/) + const sizes = /SIZE (.+)/.exec(header)?.[1].trim().split(/\s+/).map(Number) + const counts = /COUNT (.+)/.exec(header)?.[1].trim().split(/\s+/).map(Number) ?? fields?.map(() => 1) + const pointCount = Number(/POINTS (\d+)/.exec(header)?.[1]) + if (!fields || !sizes || !Number.isFinite(pointCount)) return null + return { + fields, + sizes, + counts, + pointCount, + dataType: marker.dataType, + dataOffset: marker.lineEnd + 1 + } +} + +/** + * 找到 DATA 行末尾（数据区起点的前一个换行） + * @param {Uint8Array} bytes + * @returns {{ dataType: string, lineEnd: number }|null} + */ +function indexOfDataLine(bytes) { + const text = new TextDecoder('ascii').decode(bytes.subarray(0, Math.min(bytes.length, 4096))) + const match = /^DATA (ascii|binary_compressed|binary)\s*$/m.exec(text) + if (!match) return null + const lineEnd = text.indexOf('\n', match.index) + if (lineEnd < 0) return null + return { dataType: match[1], lineEnd } +} + +/** + * 用 PCL 解析并滤波 + * @param {ArrayBuffer} buffer + * @param {{ leafSize: number, denoise: boolean, startedAt: number }} options + */ +async function parsePcdWithPcl(buffer, { leafSize, denoise, startedAt }) { + await initPcl() + + const header = PCL.readPCDHeader(buffer) + const fields = header?.fields ?? [] + const { PT, kind } = resolvePointType(fields) + + // 每一步滤波都会产出新的 WASM 对象，统一登记后在 finally 里释放，避免 heap 泄漏 + const disposables = [] + const track = (cloud) => { + if (cloud && !disposables.includes(cloud)) disposables.push(cloud) + return cloud + } + + try { + let cloud = track(PCL.loadPCDData(buffer, PT)) + const rawCount = cloud.size + + // 含 NaN 的稀疏点云会污染包围盒，先剔除再进滤波 + if (!cloud.isDense) { + const { cloud: dense, indices } = PCL.removeNaNFromPointCloud(cloud) + indices?.manager?.delete() + cloud = track(dense) + } + + if (leafSize > 0) { + cloud = track( + applyFilter(new PCL.VoxelGrid(PT), cloud, (filter) => { + filter.setLeafSize(leafSize, leafSize, leafSize) + // 关闭时 VoxelGrid 只保留体素质心的 xyz，rgb / intensity 会被丢弃 + filter.setDownsampleAllData(kind !== 'xyz') + }) + ) + } + + if (denoise) { + cloud = track( + applyFilter(new PCL.StatisticalOutlierRemoval(PT), cloud, (filter) => { + filter.setMeanK(SOR_MEAN_K) + filter.setStddevMulThresh(SOR_STDDEV_MUL) + }) + ) + } + + return { + ...extractGeometry(cloud, kind), + rawCount, + fields, + dataType: header?.data ?? 'unknown', + cost: Math.round(performance.now() - startedAt) + } + } finally { + disposables.forEach((cloud) => { + if (!cloud.manager.isDeleted()) cloud.manager.delete() + }) + } +} + +/** + * 按 PCD 字段选择 PCL 点类型：类型选错会导致 rgb / intensity 被直接丢弃 + * @param {string[]} fields PCD 头中的 FIELDS + * @returns {{ PT: Function, kind: 'rgb'|'intensity'|'xyz' }} + */ +function resolvePointType(fields) { + if (fields.some((field) => RGB_FIELDS.includes(field))) { + return { PT: PCL.PointXYZRGB, kind: 'rgb' } + } + if (fields.includes('intensity')) { + return { PT: PCL.PointXYZI, kind: 'intensity' } + } + return { PT: PCL.PointXYZ, kind: 'xyz' } +} + +/** + * 执行一次 PCL 滤波并释放滤波器本身 + * @param {object} filter PCL 滤波器实例 + * @param {object} cloud 输入点云 + * @param {(filter: object) => void} configure 滤波参数配置回调 + * @returns {object} 滤波结果，滤波器不可用时原样返回输入点云 + */ +function applyFilter(filter, cloud, configure) { + configure(filter) + filter.setInputCloud(cloud) + const output = filter.filter() + filter.manager.delete() + return output ?? cloud +} + +/** + * 把 PCL 点云拷成 TypedArray，并顺带算出包围盒 + * @param {object} cloud PCL 点云 + * @param {'rgb'|'intensity'|'xyz'} kind 点云附带的属性种类 + * @returns {{ positions: Float32Array, colors: (Float32Array|null), intensities: (Float32Array|null), count: number, bounds: object }} + */ +function extractGeometry(cloud, kind) { + // pcl.js 的 Points.get() 每取一个点都会 new 一次包装对象，十万级点云下开销就已经很可观； + // 这里直接读底层 embind vector，一次循环填满 TypedArray + const nativePoints = cloud.points._native + const count = nativePoints.size() + + const positions = new Float32Array(count * 3) + const colors = kind === 'rgb' ? new Float32Array(count * 3) : null + const intensities = kind === 'intensity' ? new Float32Array(count) : null + + const min = [Infinity, Infinity, Infinity] + const max = [-Infinity, -Infinity, -Infinity] + let intensityMin = Infinity + let intensityMax = -Infinity + + for (let i = 0; i < count; i++) { + const point = nativePoints.get(i) + const offset = i * 3 + const { x, y, z } = point + + positions[offset] = x + positions[offset + 1] = y + positions[offset + 2] = z + + if (x < min[0]) min[0] = x + if (y < min[1]) min[1] = y + if (z < min[2]) min[2] = z + if (x > max[0]) max[0] = x + if (y > max[1]) max[1] = y + if (z > max[2]) max[2] = z + + if (colors) { + colors[offset] = point.r / 255 + colors[offset + 1] = point.g / 255 + colors[offset + 2] = point.b / 255 + } + + if (intensities) { + const intensity = point.intensity + intensities[i] = intensity + if (intensity < intensityMin) intensityMin = intensity + if (intensity > intensityMax) intensityMax = intensity + } + } + + return { + positions, + colors, + intensities, + count, + bounds: buildBounds(min, max, count, intensityMin, intensityMax) + } +} + +/** + * 由 min / max 推导出场景自适应需要的中心、尺寸与外接球半径 + * @param {number[]} min 包围盒最小点 + * @param {number[]} max 包围盒最大点 + * @param {number} count 点数，为 0 时返回单位包围盒兜底 + * @param {number} intensityMin 强度最小值 + * @param {number} intensityMax 强度最大值 + * @returns {{ min: number[], max: number[], center: number[], size: number[], radius: number, intensityRange: number[] }} + */ +function buildBounds(min, max, count, intensityMin, intensityMax) { + if (!count) { + return { + min: [0, 0, 0], + max: [0, 0, 0], + center: [0, 0, 0], + size: [1, 1, 1], + radius: 1, + intensityRange: [0, 1] + } + } + + const size = [max[0] - min[0], max[1] - min[1], max[2] - min[2]] + const center = [(min[0] + max[0]) / 2, (min[1] + max[1]) / 2, (min[2] + max[2]) / 2] + const radius = Math.max(Math.hypot(size[0], size[1], size[2]) / 2, 1e-3) + const hasIntensity = intensityMax > intensityMin + + return { + min, + max, + center, + size, + radius, + intensityRange: hasIntensity ? [intensityMin, intensityMax] : [0, 1] + } +} diff --git a/src/examples/expand/PcdViewer/pointColors.js b/src/examples/expand/PcdViewer/pointColors.js new file mode 100644 index 0000000..7ffc5ff --- /dev/null +++ b/src/examples/expand/PcdViewer/pointColors.js @@ -0,0 +1,108 @@ +/* + * @Description: 点云着色计算，输出逐点 RGB(0~1)，供 three.js 独立场景与 maplibre 点云图层共用 + * @FilePath: src/examples/expand/PcdViewer/pointColors.js + */ +import { COLOR_MODE, HEIGHT_COLOR_STOPS } from './constants' + +// 色带在模块级预解析成 [r,g,b]，避免逐点解析 hex 造成的开销 +const HEIGHT_RAMP = HEIGHT_COLOR_STOPS.map(([stop, hex]) => ({ stop, rgb: hexToRgb(hex) })) + +/** + * 当前点云数据实际支持的着色模式，顺序即面板上的展示顺序 + * @param {object} [cloud] parsePcd 的返回值 + * @returns {string[]} + */ +export function availableColorModes(cloud) { + const modes = [COLOR_MODE.HEIGHT, COLOR_MODE.SINGLE] + if (cloud?.colors) modes.unshift(COLOR_MODE.RGB) + if (cloud?.intensities) modes.unshift(COLOR_MODE.INTENSITY) + return modes +} + +/** + * 生成指定着色模式下的逐点颜色 + * @param {object} cloud parsePcd 的返回值 + * @param {string} mode COLOR_MODE 之一 + * @returns {Float32Array|null} 单色模式返回 null，由调用方用材质基色渲染 + */ +export function buildColors(cloud, mode) { + if (!cloud) return null + if (mode === COLOR_MODE.RGB) return cloud.colors ?? null + if (mode === COLOR_MODE.INTENSITY) return buildIntensityColors(cloud) + if (mode === COLOR_MODE.HEIGHT) return buildHeightColors(cloud) + return null +} + +/** + * 按 z 值映射到高度色带 + * @param {object} cloud parsePcd 的返回值 + * @returns {Float32Array} + */ +function buildHeightColors(cloud) { + const { positions, count, bounds } = cloud + const colors = new Float32Array(count * 3) + const zMin = bounds.min[2] + const zSpan = bounds.size[2] || 1 + + for (let i = 0; i < count; i++) { + const offset = i * 3 + writeRampColor((positions[offset + 2] - zMin) / zSpan, colors, offset) + } + return colors +} + +/** + * 按反射强度映射到高度色带 + * @param {object} cloud parsePcd 的返回值 + * @returns {Float32Array|null} 无 intensity 字段时返回 null + */ +function buildIntensityColors(cloud) { + const { intensities, count, bounds } = cloud + if (!intensities) return null + + const colors = new Float32Array(count * 3) + const [min, max] = bounds.intensityRange + const span = max - min || 1 + + for (let i = 0; i < count; i++) { + writeRampColor((intensities[i] - min) / span, colors, i * 3) + } + return colors +} + +/** + * 在色带上采样并写入颜色数组 + * @param {number} t 归一化位置，超出 [0,1] 会被截断 + * @param {Float32Array} out 目标数组 + * @param {number} offset 写入起始下标 + */ +function writeRampColor(t, out, offset) { + const clamped = t < 0 ? 0 : t > 1 ? 1 : t + + let index = 1 + while (index < HEIGHT_RAMP.length - 1 && clamped > HEIGHT_RAMP[index].stop) index++ + + const from = HEIGHT_RAMP[index - 1] + const to = HEIGHT_RAMP[index] + const span = to.stop - from.stop + const k = span > 0 ? (clamped - from.stop) / span : 0 + + out[offset] = from.rgb[0] + (to.rgb[0] - from.rgb[0]) * k + out[offset + 1] = from.rgb[1] + (to.rgb[1] - from.rgb[1]) * k + out[offset + 2] = from.rgb[2] + (to.rgb[2] - from.rgb[2]) * k +} + +/** + * #rrggbb → [r,g,b]（0~1） + * @param {string} hex 十六进制颜色 + * @returns {number[]} + */ +function hexToRgb(hex) { + const matched = /^#?([a-f\d]{2})([a-f\d]{2})([a-f\d]{2})$/i.exec(hex) + if (!matched) return [1, 1, 1] + return [ + parseInt(matched[1], 16) / 255, + parseInt(matched[2], 16) / 255, + parseInt(matched[3], 16) / 255 + ] +} diff --git a/src/examples/expand/PcdViewer/samples/samples.pcd b/src/examples/expand/PcdViewer/samples/samples.pcd new file mode 100644 index 0000000..964a803 Binary files /dev/null and b/src/examples/expand/PcdViewer/samples/samples.pcd differ diff --git a/src/examples/expand/PcdViewer/slamPlacement.js b/src/examples/expand/PcdViewer/slamPlacement.js new file mode 100644 index 0000000..b831b15 --- /dev/null +++ b/src/examples/expand/PcdViewer/slamPlacement.js @@ -0,0 +1,82 @@ +/* + * @Description: 把 PCD 点云按对齐参数摆进 SLAM 坐标系，并换算成 maplibre 点云图层要的经纬度点位 + * @FilePath: src/examples/expand/PcdViewer/slamPlacement.js + */ +import mapUtils from '../../../bicMap/core/utils/mapUtils' + +import { SLAM_GEO_SCALE, SLAM_HEIGHT, SLAM_MAP, SLAM_WIDTH } from './constants' + +/** + * 对点云施加「缩放 → 绕 Z 旋转 → 平移」，再逐点转成 [lng, lat, altitude] + * @param {object} cloud parsePcd 的返回值 + * @param {object} align 对齐参数 { x, y, z, rotation, scale } + * @param {Float32Array|null} colors 逐点颜色，与点位一一对应 + * @returns {{ points: Array<[number,number,number]>, colors: (Float32Array|null), stride: number }} + */ +export function placeOnSlamMap(cloud, align, colors) { + const { positions, count, bounds } = cloud + + const points = new Array(count) + const outColors = colors ? new Float32Array(count * 3) : null + + const radians = (align.rotation * Math.PI) / 180 + const cos = Math.cos(radians) + const sin = Math.sin(radians) + const { scale } = align + // 点云先贴到地面（最低点落到 z=0），再叠加用户设定的离地高度 + const zBase = align.z - bounds.min[2] * scale + + for (let i = 0; i < count; i++) { + const source = i * 3 + const localX = positions[source] * scale + const localY = positions[source + 1] * scale + + const x = localX * cos - localY * sin + align.x + const y = localX * sin + localY * cos + align.y + const z = positions[source + 2] * scale + zBase + + const gps = mapUtils.cartesianToGPS({ + x, + y, + scale: SLAM_MAP.resolution, + zoomFactor: SLAM_MAP.zoomFactor + }) + + // 水平方向被 resolution × zoomFactor 压缩过，高度要用同一系数才不会被拉伸 + points[i] = [gps.longitude, gps.latitude, z * SLAM_GEO_SCALE] + + if (outColors) { + outColors[source] = colors[source] + outColors[source + 1] = colors[source + 1] + outColors[source + 2] = colors[source + 2] + } + } + + return { points, colors: outColors, stride: 1 } +} + +/** + * SLAM 底图四至的经纬度范围，供地图取景使用 + * @returns {number[][]} [[西南 lng, lat], [东北 lng, lat]] + */ +export function slamMapBounds() { + return [ + slamToLngLat([SLAM_MAP.startX, SLAM_MAP.startY]), + slamToLngLat([SLAM_MAP.startX + SLAM_WIDTH, SLAM_MAP.startY + SLAM_HEIGHT]) + ] +} + +/** + * SLAM 笛卡尔坐标 → [lng, lat] + * @param {number[]} point [x, y]，单位米 + * @returns {number[]} + */ +export function slamToLngLat([x, y]) { + const gps = mapUtils.cartesianToGPS({ + x, + y, + scale: SLAM_MAP.resolution, + zoomFactor: SLAM_MAP.zoomFactor + }) + return [gps.longitude, gps.latitude] +} diff --git a/src/router/index.js b/src/router/index.js index bfd55e7..828bbdb 100644 --- a/src/router/index.js +++ b/src/router/index.js @@ -167,10 +167,16 @@ const routes = [ meta: { title: '地图编辑' } }, { - path: '/expand/GraphicDrawing', - name: 'GraphicDrawing', - component: () => import('../examples/expand/GraphicDrawing/index.vue'), - meta: { title: '图形绘制' } + path: '/expand/PcdViewer', + name: 'pcdViewer', + component: () => import('../examples/expand/PcdViewer/index.vue'), + meta: { title: 'PCD点云加载' } + }, + { + path: '/expand/PathPlanning', + name: 'PathPlanning', + component: () => import('../examples/base/pathPlanning/index.vue'), + meta: { title: '路径规划' } }, { path: '/base/passableArea', diff --git a/vite.config.js b/vite.config.js index 9884860..25661d2 100644 --- a/vite.config.js +++ b/vite.config.js @@ -71,7 +71,7 @@ export default defineConfig(({ command, mode }) => { vue() ], publicDir, - assetsInclude: ['**/*.bmp'], + assetsInclude: ['**/*.bmp', '**/*.pcd'], resolve: { alias: { '@': resolve(__dirname, 'src')