// ==UserScript==
// @name 平台端RGB着色（静默版）
// @namespace local.xj.platform.rgbcolor
// @version 2.2.0
// @description 让平台上本来点不亮的官方「rgb着色」直接用起来：数据自带 rgba 的就地解出颜色，没有颜色字段的用相机+标定做投影着色。v2.2 性能优化：pcd 只取头部探测字段、相机图与探测并行、逐相机到达即上色、空闲预热下一帧、启动零等待。不改数据、不重传、无调色面板。
// @author -
// @match *://*/pointcloud/*
// @match *://121.37.95.217:8080/*
// @grant none
// @run-at document-idle
// ==/UserScript==
(function () {
'use strict';
/* ================================================================
* 0. 参数（无 UI 开关；颜色一律按原始值写入，不做任何增强）
* ================================================================ */
const CFG = {
autoFollow: true, // 自动跟随帧变化
keepRgbMode: true, // 自动切到官方「rgb着色」并保持（用户手动改后不再抢）
prefetchNext: true, // 空闲时预热下一帧的相机图，切帧近乎瞬时（不用可设 false）
pollMs: 80,
imgCacheMax: 24,
pcdCacheMax: 2,
imgProxy: '', // 图片跨域读不了时可填代理前缀，例如 'http://127.0.0.1:8000/?url='
};
const LOG = '[平台RGB着色]';
const log = (...a) => console.log(LOG, ...a);
/* ================================================================
* 1. 平台对象（均按平台自身实现核对过）
* ================================================================ */
const getInstance = () => window.instance || null;
const getSM = () => {
const i = getInstance();
return (i && i.StoreManager) || null;
};
const getViewer = () => {
const i = getInstance();
return window.viewer || (i && i.viewer) || null;
};
function sensorParams() {
const SM = getSM();
if (!SM) return null;
try {
const sp = SM.configState().params.sensor_params;
return sp && typeof sp === 'object' ? sp : null;
} catch (e) { return null; }
}
function frameById(id) {
const SM = getSM();
if (!SM) return null;
try {
const g = SM.configGetter('frame');
if (!g) return null;
return (id !== null && id !== undefined ? g(id) : g()) || null;
} catch (e) { return null; }
}
function poseById(id) {
const SM = getSM();
if (!SM) return null;
try {
const g = SM.configGetter('getPose');
if (!g) return null;
const p = g(id);
return (p && p.length >= 16) ? p : null;
} catch (e) { return null; }
}
function imageUrlOf(frame, key) {
try {
const arr = frame && frame.slave_info && frame.slave_info[key];
const first = Array.isArray(arr) ? arr[0] : arr;
return (first && (first.url || first.src)) || null;
} catch (e) { return null; }
}
/* ================================================================
* 2. 4x4 工具（行/列主序都支持，用于坐标系判定）
* ================================================================ */
const colMajorTo4x4 = (el) => [
[el[0], el[4], el[8], el[12]],
[el[1], el[5], el[9], el[13]],
[el[2], el[6], el[10], el[14]],
[el[3], el[7], el[11], el[15]],
];
const rowMajorTo4x4 = (el) => [
[el[0], el[1], el[2], el[3]],
[el[4], el[5], el[6], el[7]],
[el[8], el[9], el[10], el[11]],
[el[12], el[13], el[14], el[15]],
];
function matMul(A, B) {
const C = [[0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0]];
for (let r = 0; r < 4; r++) {
for (let c = 0; c < 4; c++) {
let s = 0;
for (let k = 0; k < 4; k++) s += A[r][k] * B[k][c];
C[r][c] = s;
}
}
return C;
}
function matInv(M) {
const a = [];
for (let r = 0; r < 4; r++) a.push(M[r].slice().concat([r === 0 ? 1 : 0, r === 1 ? 1 : 0, r === 2 ? 1 : 0, r === 3 ? 1 : 0]));
for (let c = 0; c < 4; c++) {
let piv = c;
for (let r = c + 1; r < 4; r++) if (Math.abs(a[r][c]) > Math.abs(a[piv][c])) piv = r;
if (Math.abs(a[piv][c]) < 1e-12) return null;
const t = a[c]; a[c] = a[piv]; a[piv] = t;
const d = a[c][c];
for (let k = 0; k < 8; k++) a[c][k] /= d;
for (let r = 0; r < 4; r++) {
if (r === c) continue;
const f = a[r][c];
if (!f) continue;
for (let k = 0; k < 8; k++) a[r][k] -= f * a[c][k];
}
}
return [
[a[0][4], a[0][5], a[0][6], a[0][7]],
[a[1][4], a[1][5], a[1][6], a[1][7]],
[a[2][4], a[2][5], a[2][6], a[2][7]],
[a[3][4], a[3][5], a[3][6], a[3][7]],
];
}
/* ================================================================
* 3. 相机图像（缓存 / 跨域降级 / 并发去重）
* ================================================================ */
const imgCache = new Map();
const imgPending = new Map();
const imgFailed = new Map();
function decodeImage(url, useCors) {
return new Promise((resolve, reject) => {
const img = new Image();
if (useCors) img.crossOrigin = 'anonymous';
img.onload = () => {
try {
const w = img.naturalWidth || img.width;
const h = img.naturalHeight || img.height;
const cv = document.createElement('canvas');
cv.width = w; cv.height = h;
const ctx = cv.getContext('2d', { willReadFrequently: true });
ctx.drawImage(img, 0, 0);
resolve({ data: ctx.getImageData(0, 0, w, h).data, width: w, height: h });
} catch (e) { reject(new Error('画布跨域污染，无法读取像素')); }
};
img.onerror = () => reject(new Error('图片加载失败'));
img.src = url;
});
}
async function fetchDecode(url) {
const res = await fetch(url, { mode: 'cors', credentials: 'same-origin' });
if (!res.ok) throw new Error('HTTP ' + res.status);
const bmp = await createImageBitmap(await res.blob());
const cv = document.createElement('canvas');
cv.width = bmp.width; cv.height = bmp.height;
const ctx = cv.getContext('2d', { willReadFrequently: true });
ctx.drawImage(bmp, 0, 0);
const d = ctx.getImageData(0, 0, bmp.width, bmp.height).data;
if (bmp.close) bmp.close();
return { data: d, width: bmp.width, height: bmp.height };
}
/**
* 取图策略按“来源”记忆：同源走 ![]()
（能和平台共用同一份浏览器缓存与已解码位图）；
* 跨域必须先走 crossOrigin，否则会先下载一遍拿不到像素的图，再为 CORS 重下一遍。
* 第一次成功的方式会被记下来，后续同来源直接用，避免任何重复下载。
*/
const originStrategy = new Map();
function originOf(url) {
try { return new URL(url, location.href).origin; } catch (e) { return 'unknown'; }
}
function strategyOrder(url) {
const known = originStrategy.get(originOf(url));
const base = originOf(url) === location.origin ? ['plain', 'fetch', 'cors'] : ['cors', 'fetch', 'plain'];
if (!known) return base;
return [known].concat(base.filter((x) => x !== known));
}
const STRAT = {
plain: (u) => decodeImage(u, false),
cors: (u) => decodeImage(u, true),
fetch: fetchDecode,
};
async function loadImage(rawUrl) {
if (imgCache.has(rawUrl)) return imgCache.get(rawUrl);
if (imgPending.has(rawUrl)) return imgPending.get(rawUrl);
if (imgFailed.has(rawUrl)) return null;
const p = (async () => {
const urls = CFG.imgProxy ? [CFG.imgProxy + encodeURIComponent(rawUrl), rawUrl] : [rawUrl];
const errs = [];
for (const u of urls) {
for (const name of strategyOrder(u)) {
try {
const r = await STRAT[name](u);
originStrategy.set(originOf(u), name); // 记住这条来源可行的方式
return r;
} catch (e) { errs.push(name + ': ' + e.message); }
}
}
imgFailed.set(rawUrl, errs.join(' | '));
return null;
})();
imgPending.set(rawUrl, p);
const r = await p;
imgPending.delete(rawUrl);
if (r) {
imgCache.set(rawUrl, r);
if (imgCache.size > CFG.imgCacheMax) imgCache.delete(imgCache.keys().next().value);
}
return r;
}
/* ================================================================
* 4. PCD 直读
* 平台 loader 只认字段名 rgb；static_map.pcd 是 rgba，所以平台点不亮 rgb着色
* 这里把文件里的 rgb / rgba 直接解出来（支持 ascii / binary / binary_compressed）
* ================================================================ */
const pcdCache = new Map();
function lzfDecompress(input, outLen) {
const out = new Uint8Array(outLen);
let i = 0, o = 0;
while (i < input.length) {
let ctrl = input[i++];
if (ctrl < 32) {
ctrl++;
if (o + ctrl > outLen) break;
out.set(input.subarray(i, i + ctrl), o);
i += ctrl; o += ctrl;
} else {
let len = ctrl >> 5;
let ref = o - ((ctrl & 0x1f) << 8) - 1;
if (len === 7) len += input[i++];
ref -= input[i++];
if (ref < 0) break;
for (let k = 0; k <= len + 1; k++) {
if (o >= outLen) break;
out[o++] = out[ref++];
}
}
}
return out;
}
function parsePcdHeader(bytes) {
const headLen = Math.min(bytes.length, 8192);
let text = '';
for (let i = 0; i < headLen; i++) text += String.fromCharCode(bytes[i]);
// 逐行扫描：必须跳过 # 注释行，否则
// "# .PCD v0.7 - Point Cloud Data file format" 会被误当成 DATA 行
const map = {};
let dataType = null, dataOffset = -1, i = 0;
while (i < text.length) {
let end = text.indexOf('\n', i);
if (end < 0) end = text.length;
let line = text.slice(i, end);
const next = end + 1;
if (line.endsWith('\r')) line = line.slice(0, -1);
const t = line.trim();
if (t && t.charAt(0) !== '#') {
const parts = t.split(/\s+/);
const key = parts[0].toUpperCase();
if (key === 'DATA') {
dataType = (parts[1] || '').toLowerCase();
dataOffset = next; // 头部为纯 ASCII，字符偏移 == 字节偏移
break;
}
map[key] = parts.slice(1);
}
i = next;
}
if (!dataType || dataOffset < 0) return null;
const fields = map.FIELDS || [];
const sizes = (map.SIZE || []).map(Number);
const types = map.TYPE || [];
const counts = (map.COUNT || []).map(Number);
const points = parseInt((map.POINTS && map.POINTS[0]) || '0', 10);
if (!fields.length || !points) return null;
const cnt = counts.length ? counts : fields.map(() => 1);
let rowSize = 0;
const offset = {};
for (let k = 0; k < fields.length; k++) {
offset[fields[k]] = rowSize;
rowSize += (sizes[k] || 1) * (cnt[k] || 1);
}
return { fields, sizes, types, counts: cnt, points, dataType, dataOffset, rowSize, offset };
}
/** 取出数据段（自动处理 binary_compressed 的 LZF + SOA 布局） */
function dataOf(bytes, hdr) {
if (hdr.dataType === 'binary_compressed') {
const dv = new DataView(bytes.buffer, bytes.byteOffset + hdr.dataOffset);
const compSize = dv.getUint32(0, true);
const uncompSize = dv.getUint32(4, true);
const comp = new Uint8Array(bytes.buffer, bytes.byteOffset + hdr.dataOffset + 8, compSize);
return { data: lzfDecompress(comp, uncompSize), soa: true };
}
if (hdr.dataType === 'binary') {
return { data: new Uint8Array(bytes.buffer, bytes.byteOffset + hdr.dataOffset), soa: false };
}
return null; // ascii 单独处理
}
/** 解出 rgb / rgba 颜色（R 在 +2 字节、G +1、B +0，与平台 loader 的读法一致） */
function extractColors(bytes, hdr) {
let ci = hdr.fields.indexOf('rgb');
if (ci < 0) ci = hdr.fields.indexOf('rgba');
if (ci < 0) return null;
const fieldName = hdr.fields[ci];
if ((hdr.sizes[ci] || 0) !== 4) return null;
const n = hdr.points;
const out = new Float32Array(n * 3);
const pack = dataOf(bytes, hdr);
if (pack) {
const { data, soa } = pack;
const co = hdr.offset[fieldName];
const stride = hdr.sizes[ci] || 4;
for (let i = 0; i < n; i++) {
const base = soa ? (co * n + i * stride) : (i * hdr.rowSize + co);
const j = i * 3;
out[j] = data[base + 2] / 255;
out[j + 1] = data[base + 1] / 255;
out[j + 2] = data[base] / 255;
}
} else if (hdr.dataType === 'ascii') {
const txt = new TextDecoder().decode(bytes.subarray(hdr.dataOffset));
const lines = txt.split(/\r?\n/);
const isF = hdr.types[ci] === 'F';
for (let i = 0; i < n; i++) {
const parts = (lines[i] || '').trim().split(/\s+/);
if (parts.length <= ci) break;
const raw = parseFloat(parts[ci]);
let v;
if (isF) { const f = new Float32Array(1); f[0] = raw; v = new Uint32Array(f.buffer)[0]; }
else v = raw >>> 0;
const j = i * 3;
out[j] = ((v >> 16) & 255) / 255;
out[j + 1] = ((v >> 8) & 255) / 255;
out[j + 2] = (v & 255) / 255;
}
} else {
return null;
}
return { colors: out, fieldName, points: n };
}
/** 校验 pcd 与当前 geometry 是否同一份（点数 + 若干点 xyz 对齐） */
function verifySameCloud(bytes, hdr, posArr, count) {
if (hdr.points !== count) return { same: false, why: '点数不一致 pcd=' + hdr.points + ' 几何=' + count };
const pack = dataOf(bytes, hdr);
if (!pack) return { same: true, why: 'ascii 未校验' };
const { data, soa } = pack;
const dv = new DataView(data.buffer, data.byteOffset, data.byteLength);
const samples = [0, 1, 2, 100, 1000].filter((i) => i < count);
for (const i of samples) {
for (const pair of [['x', 0], ['y', 1], ['z', 2]]) {
const f = pair[0], k = pair[1];
const off = hdr.offset[f];
if (off === undefined) continue;
const at = soa ? (off * count + i * 4) : (i * hdr.rowSize + off);
if (at + 4 > dv.byteLength) continue;
const val = dv.getFloat32(at, true);
const geo = posArr[i * 3 + k];
if (!isFinite(val) || !isFinite(geo)) continue;
if (Math.abs(val - geo) > 1e-3) {
return { same: false, why: '坐标不对齐(第' + i + '点 ' + f + ': pcd=' + val + ' 几何=' + geo + ')' };
}
}
}
return { same: true, why: 'ok' };
}
/** 只取 pcd 头部（8KB）判断有没有 rgb/rgba 字段 —— 避免为探明字段名而下整个文件 */
const probeCache = new Map();
const dirNoColor = new Set(); // 同一批数据的其他帧同样没有颜色字段，可直接跳过探测
const dirOf = (u) => { const i = u.lastIndexOf('/'); return i > 0 ? u.slice(0, i) : u; };
async function probePcdHeader(url) {
if (probeCache.has(url)) return probeCache.get(url);
let res = null;
try {
res = await fetch(url, {
headers: { Range: 'bytes=0-8191' },
credentials: 'same-origin',
});
if (!res.ok && res.status !== 206) throw new Error('HTTP ' + res.status);
let bytes = null;
if (res.status === 206) {
bytes = new Uint8Array(await res.arrayBuffer());
} else if (res.body && typeof res.body.getReader === 'function') {
// 服务器忽略了 Range：读第一块就中止，不把整个文件拖下来
const reader = res.body.getReader();
const chunks = [];
let total = 0;
while (total < 8192) {
const r = await reader.read();
if (r.done) break;
chunks.push(r.value);
total += r.value.length;
}
try { await reader.cancel(); } catch (e) {}
bytes = new Uint8Array(total);
let o = 0;
for (const c of chunks) { bytes.set(c, o); o += c.length; }
} else {
bytes = new Uint8Array(await res.arrayBuffer());
}
const hdr = parsePcdHeader(bytes);
const rec = hdr ? { hdr } : null;
probeCache.set(url, rec);
if (probeCache.size > 200) probeCache.delete(probeCache.keys().next().value);
return rec;
} catch (e) {
probeCache.set(url, null);
return null;
}
}
async function readPcdFile(url) {
if (!url) return null;
if (pcdCache.has(url)) return pcdCache.get(url);
const res = await fetch(url, { credentials: 'same-origin' });
if (!res.ok) throw new Error('pcd HTTP ' + res.status);
const bytes = new Uint8Array(await res.arrayBuffer());
const hdr = parsePcdHeader(bytes);
if (!hdr) throw new Error('pcd 头解析失败（可能不是 PCD 或头部超长）');
const rec = { bytes, hdr };
pcdCache.set(url, rec);
if (pcdCache.size > CFG.pcdCacheMax) pcdCache.delete(pcdCache.keys().next().value);
return rec;
}
/* ================================================================
* 5. 投影（与平台 computePointWithoutBehindFilter 同一套公式）
* ================================================================ */
function project(x, y, z, cam) {
const e = cam.extrinsic;
if (!e || e.length < 3) return null;
const xc = e[0][0] * x + e[0][1] * y + e[0][2] * z + e[0][3];
const yc = e[1][0] * x + e[1][1] * y + e[1][2] * z + e[1][3];
const zc = e[2][0] * x + e[2][1] * y + e[2][2] * z + e[2][3];
if (!(zc > 0.01)) return null;
let u = xc / zc, v = yc / zc;
const k1 = cam.k1 || 0, k2 = cam.k2 || 0, k3 = cam.k3 || 0;
const p1 = cam.p1 || 0, p2 = cam.p2 || 0;
if (k1 || k2 || k3 || p1 || p2) {
const r2 = u * u + v * v;
const radial = 1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2;
const tx = 2 * p1 * u * v + p2 * (r2 + 2 * u * u);
const ty = p1 * (r2 + 2 * v * v) + 2 * p2 * u * v;
u = u * radial + tx;
v = v * radial + ty;
}
return [u * cam.fx + cam.cx, v * cam.fy + cam.cy, zc];
}
/** 本帧可用的相机任务（只列参数与图 URL，不触发加载） */
function cameraJobs(sp, frame) {
const jobs = [];
for (const key of Object.keys(sp)) {
const c = sp[key];
if (!c || c.fx === null || c.fx === undefined || !c.extrinsic) continue;
const model = c.camera_model || 'pinhole';
if (model !== 'pinhole' && model !== 'pinhole1') continue;
const url = imageUrlOf(frame, key);
if (!url) continue;
jobs.push({ key, cam: c, url });
}
return jobs;
}
async function loadCameraImages(sp, frame) {
const jobs = cameraJobs(sp, frame);
const imgs = await Promise.all(jobs.map((jb) => loadImage(jb.url)));
const out = [];
for (let i = 0; i < jobs.length; i++) {
if (!imgs[i]) continue;
out.push({ key: jobs[i].key, cam: jobs[i].cam, data: imgs[i].data, width: imgs[i].width, height: imgs[i].height });
}
return out;
}
/** 单台相机：只写"比已知更近"的点 —— 颜色随图像到达逐块出现，不必等 4 张全齐 */
function projectCamera(cam, img, ci, st) {
const { rg, pos, bestZ, bestU, bestCi, buf } = st;
const w = img.width, h = img.height, data = img.data;
for (let i = rg.start; i < rg.end; i++) {
const j = i * 3;
const pr = project(pos[j], pos[j + 1], pos[j + 2], cam);
if (!pr) continue;
const px = pr[0], py = pr[1], zc = pr[2];
if (px < 0 || px >= w || py < 0 || py >= h) continue;
if (zc >= bestZ[i]) continue;
bestZ[i] = zc; bestU[i] = px; bestCi[i] = ci;
const idx = ((py | 0) * w + (px | 0)) * 4;
buf[j] = data[idx] / 255;
buf[j + 1] = data[idx + 1] / 255;
buf[j + 2] = data[idx + 2] / 255;
}
}
/** 收尾统计：按"最终获胜相机"精确统计命中/亮度/像素离散 */
function finishStats(st) {
const { rg, buf, bestZ, bestU, bestCi, camStat } = st;
let hit = 0, lumSum = 0, darkN = 0, su = 0, su2 = 0, nPix = 0;
for (let i = rg.start; i < rg.end; i++) {
if (!(bestZ[i] < Infinity)) continue;
const j = i * 3;
const r = buf[j], g = buf[j + 1], b = buf[j + 2];
const lum = 0.299 * r + 0.587 * g + 0.114 * b;
hit++; lumSum += lum; if (lum < 0.02) darkN++;
su += bestU[i]; su2 += bestU[i] * bestU[i]; nPix++;
const ci = bestCi[i];
if (ci >= 0 && camStat[ci]) { camStat[ci].hit++; camStat[ci].lum += lum; }
}
const meanLum = hit ? lumSum / hit : 0;
const stdU = nPix ? Math.sqrt(Math.max(0, su2 / nPix - (su / nPix) ** 2)) : 0;
return { hit, meanLum, darkN, spread: stdU / (st.imgW || 768) };
}
const nowMs = () => (typeof performance !== 'undefined' && performance.now ? performance.now() : Date.now());
/** 合并多次渲染请求到一帧动画里，避免重复 rendererMain */
let renderPending = false;
function scheduleRender() {
if (renderPending) return;
renderPending = true;
const doIt = () => {
renderPending = false;
const v = getViewer();
if (!v) return;
try { v.rendererMain(); } catch (e) {}
};
if (typeof requestAnimationFrame === 'function') requestAnimationFrame(doIt);
else setTimeout(doIt, 16);
}
/** 采样一批点，统计"至少落在一台相机画面内"的比例 */
function sampleCoverage(pos, count, cams) {
if (!cams.length) return 0;
const step = Math.max(1, Math.floor(count / 4000));
let tried = 0, hit = 0;
for (let i = 0; i < count; i += step) {
const j = i * 3;
const x = pos[j], y = pos[j + 1], z = pos[j + 2];
tried++;
for (let ci = 0; ci < cams.length; ci++) {
const c = cams[ci];
const pr = project(x, y, z, c.cam);
if (!pr) continue;
if (pr[0] >= 0 && pr[0] < c.width && pr[1] >= 0 && pr[1] < c.height) { hit++; break; }
}
}
return tried ? hit / tried : 0;
}
/* ================================================================
* 6. 着色核心
* ================================================================ */
const ownColor = new WeakSet();
const busy = new WeakSet();
const sigOf = new WeakMap();
const _ids = new WeakMap();
let _idSeq = 0;
function arrayId(arr) {
if (!_ids.has(arr)) _ids.set(arr, ++_idSeq);
return _ids.get(arr);
}
function signatureOf(p) {
const g = p.geometry;
if (!g || !g.attributes || !g.attributes.position) return null;
const a = g.attributes.position;
return (p.taskId === undefined ? '-' : String(p.taskId)) + '|' + a.count + '|' + (a.array ? arrayId(a.array) : '');
}
function buildRanges(p, count) {
const v = getViewer(), SM = getSM();
let cur = null;
try { cur = SM.getCurFrameId(); } catch (e) {}
const table = v && v.cloudData && v.cloudData.comparisonTable;
const isMerged = (p.multiFrameIds && p.multiFrameIds.length) ||
(v && v.splicePCD && p === v.splicePCD) || p.name === 'split_points';
if (isMerged && Array.isArray(table) && table.length) {
return table.map((t) => ({ start: t.Indexes.start, end: t.Indexes.end + 1, frameId: t.pcdInfo }));
}
return [{ start: 0, end: count, frameId: (p.taskId !== null && p.taskId !== undefined) ? p.taskId : cur }];
}
/**
* 坐标系统判定：同一批点分别按三种假设投影，谁落进画面的比例高谁对。
* H1 点云在本帧雷达系：p_cam = T_v2c * p
* H2 点云在世界系，位姿按平台读法(列主序)：p_cam = T_v2c * inv(pose) * p
* H3 点云在世界系，位姿按 JSON 行主序： p_cam = T_v2c * inv(pose) * p
* 返回按命中率降序的假设列表（带合成后的 extrinsic）。
*/
function detectFrames(baseCam, pose, pos, count, cams) {
const covOf = (ext) => {
const cs = cams.map((c) => ({
cam: Object.assign({}, c.cam, { extrinsic: ext }),
width: c.width, height: c.height,
}));
return sampleCoverage(pos, count, cs);
};
const list = [{ name: '本帧雷达系', ext: baseCam.extrinsic, cov: covOf(baseCam.extrinsic) }];
if (pose) {
const invA = matInv(colMajorTo4x4(pose));
if (invA) {
const extA = matMul(baseCam.extrinsic, invA);
list.push({ name: '世界系(位姿列主序)', ext: extA, cov: covOf(extA) });
}
const invB = matInv(rowMajorTo4x4(pose));
if (invB) {
const extB = matMul(baseCam.extrinsic, invB);
list.push({ name: '世界系(位姿行主序)', ext: extB, cov: covOf(extB) });
}
}
list.sort((a, b) => b.cov - a.cov);
return list;
}
async function colorizePoints(p) {
const v = getViewer(), SM = getSM();
if (!v || !SM) return { ok: false, msg: '平台对象未就绪' };
const geom = p.geometry;
if (!geom || !geom.attributes || !geom.attributes.position) return { ok: false, msg: '点云未加载' };
const count = geom.attributes.position.count;
// 地图任务里除首帧外都是 0 点占位 pcd（上传脚本 create_empty_pcd 生成），不是错误
if (!count) return { ok: true, empty: true, msg: '本帧为空点云（占位帧），无需着色' };
// ① 平台原生已支持（文件字段名就是 rgb）→ 什么都不用做
if (geom.attributes.color && !ownColor.has(geom)) {
v.supportRgbColor = true;
return { ok: true, native: true, msg: '数据自带 rgb，平台原生支持' };
}
const ranges = buildRanges(p, count);
const buf = new Float32Array(count * 3);
const pos = geom.attributes.position.array;
const t0 = nowMs();
let filled = 0;
let src = null, note = null;
// 每个点的最近相机深度/像素/归属，供"逐相机到达即上色"用
const bestZ = new Float32Array(count).fill(Infinity);
const bestU = new Float32Array(count);
const bestCi = new Int8Array(count).fill(-1);
// 颜色属性先挂上去，这样每台相机图像一到就能立刻看到效果
let attr = geom.attributes.color;
if (!attr || attr.count !== count || !attr.array || attr.array.length !== count * 3) {
const Ctor = geom.attributes.position.constructor;
attr = new Ctor(buf, 3);
geom.setAttribute('color', attr);
} else {
attr.array.set(buf);
attr.needsUpdate = true;
}
const flush = () => { attr.needsUpdate = true; scheduleRender(); };
for (const rg of ranges) {
const frame = frameById(rg.frameId);
const sp = sensorParams();
const pcdUrl = p.url || (frame && frame.url) || null;
// ②+③ 并行：一边探 pcd 头(8KB)判断有没有 rgb/rgba，一边同时开始下 4 张相机图
// 同一批数据的后续帧字段布局相同，已知没有颜色字段就直接跳过这次探测（省一个来回）
// 但若几何里带 intensity，说明文件字段不止 xyz，必须真探测（地图就是 x y z intensity rgba label）
const canSkipProbe = pcdUrl && !geom.attributes.intensity && dirNoColor.has(dirOf(pcdUrl));
const [probe] = await Promise.all([
pcdUrl ? (canSkipProbe ? Promise.resolve({ hdr: { fields: [] } }) : probePcdHeader(pcdUrl)) : Promise.resolve(null),
sp ? loadCameraImages(sp, frame) : Promise.resolve([]),
]);
// ② 文件自带 rgb/rgba → 就地解出（地图那种 rgba 就走这里）
const hasColorField = !!(probe && probe.hdr &&
(probe.hdr.fields.indexOf('rgb') >= 0 || probe.hdr.fields.indexOf('rgba') >= 0));
if (probe && probe.hdr && !hasColorField && pcdUrl && probe.hdr.fields.length) {
dirNoColor.add(dirOf(pcdUrl));
}
if (hasColorField) {
try {
const rec = await readPcdFile(pcdUrl); // 确认有颜色字段才下整个文件
const chk = rec ? verifySameCloud(rec.bytes, rec.hdr, pos, count) : null;
if (rec && chk && !chk.same) {
note = 'pcd 与几何校验未通过：' + chk.why;
} else if (rec) {
const got = extractColors(rec.bytes, rec.hdr);
if (got) {
for (let i = rg.start; i < rg.end; i++) {
const j = i * 3;
buf[j] = got.colors[j]; buf[j + 1] = got.colors[j + 1]; buf[j + 2] = got.colors[j + 2];
}
filled += (rg.end - rg.start);
src = '文件字段 ' + got.fieldName + '(平台不认，已就地解出)';
attr.array.set(buf); flush();
continue;
}
}
} catch (e) {
note = 'pcd 读取失败：' + e.message;
}
}
// ③ 相机投影着色：平台约定点云在本帧传感器系（poseMatrix 只用于显示/叠帧换算），直接用 T_v2c
if (!sp) { note = note || '缺少标定参数 sensor_params'; continue; }
const jobs = cameraJobs(sp, frame);
if (!jobs.length) { note = note || ('本帧(' + rg.frameId + ')无可用相机图像（跨域或字段缺失）'); continue; }
const camStat = jobs.map((c) => ({ key: c.key, size: '待加载', hit: 0, lum: 0 }));
const st = { bestZ, bestU, bestCi, buf, rg, pos, camStat, imgW: 0, t0, firstMs: undefined };
// 每台相机图像一到就先给"比已知更近"的点上色，不再等 4 张全齐
let arrived = 0;
await Promise.all(jobs.map(async (jb, ci) => {
const img = await loadImage(jb.url);
if (!img) return;
camStat[ci].size = img.width + 'x' + img.height;
if (img.width > st.imgW) st.imgW = img.width;
projectCamera(jb.cam, img, ci, st);
flush();
arrived++;
if (st.firstMs === undefined) st.firstMs = Math.round(nowMs() - t0);
if (arrived < jobs.length) setStatus('着色中… ' + arrived + '/' + jobs.length + ' 相机');
}));
const fin = finishStats(st);
const hit = fin.hit, meanLum = fin.meanLum, spread = fin.spread;
window.__xjRgbLastStat = {
帧: rg.frameId, 点数: count, 命中: hit, 覆盖率: (hit / count * 100).toFixed(1) + '%',
平均亮度: meanLum.toFixed(3), 很暗点占比: hit ? (fin.darkN / hit * 100).toFixed(1) + '%' : '-',
像素横向离散: spread.toFixed(3),
首图上色耗时: (st.firstMs === undefined ? '-' : st.firstMs + 'ms'),
本帧总耗时: Math.round(nowMs() - t0) + 'ms',
各相机: camStat.map((c) => c.key + ':命中' + c.hit + ',亮度' + (c.hit ? (c.lum / c.hit).toFixed(3) : '-') + ',' + c.size),
};
log('本帧统计', window.__xjRgbLastStat);
// 退化/异常保护：像素挤在一条窄带里或采样明显发暗，说明投影关系不对，
// 这时候切到 rgb 着色只会显示一片异常颜色，宁可不切
if (hit && spread < 0.05) {
buf.fill(0); attr.array.set(buf); flush();
return { ok: false, msg: '投影退化(像素离散' + spread.toFixed(3) + ')：点云坐标系可能与标定不一致，已放弃切到rgb着色' };
}
if (hit > 100 && meanLum < 0.03) {
buf.fill(0); attr.array.set(buf); flush();
return { ok: false, msg: '采样颜色异常发暗(平均亮度' + meanLum.toFixed(3) + ')：相机图或标定可疑，已放弃切到rgb着色' };
}
filled += hit;
src = src || ('相机投影[命中 ' + (hit / count * 100).toFixed(0) + '% 亮度' + meanLum.toFixed(2) + ' 离散' + spread.toFixed(2) + ']');
if (!hit && !note) note = '投影未命中任何相机画面';
}
if (!filled) return { ok: false, msg: note || '未能着色' };
// 颜色属性在前面已经建好并挂上去了，这里只做最终提交
ownColor.add(geom);
attr.array.set(buf);
attr.needsUpdate = true;
// ★ 让平台自己认可"这份点云支持 rgb 着色"，于是官方「rgb着色」就能勾选
v.supportRgbColor = true;
if (CFG.keepRgbMode && !userOverride) setColorType(3);
try { v.rendererMain(); } catch (e) {}
const pct = ((filled / count) * 100).toFixed(1);
return { ok: true, msg: filled + '/' + count + ' 点(' + pct + '%) · ' + src };
}
/* ================================================================
* 7. 与平台着色模式联动（不抢用户手动选择）
* ================================================================ */
let userOverride = false;
let lastSeenColorType = null;
let originalColorType = null;
function setColorType(n) {
const v = getViewer(), SM = getSM();
if (!v || !SM) return;
try {
SM.updateStatusProperty('colorType', n);
v.setPCDColorModel(n, SM.statusState().colorValue);
v.rendererMain();
} catch (e) {}
}
function watchColorType(sawNew) {
const SM = getSM();
if (!SM) return;
let cur = null;
try { cur = SM.statusState().colorType; } catch (e) { return; }
if (lastSeenColorType !== null && cur !== lastSeenColorType && cur !== 3 && !sawNew) userOverride = true;
lastSeenColorType = cur;
}
/* ================================================================
* 8. 叠帧保险 + 主循环
* ================================================================ */
function patchSplice(v) {
if (!v || v.__xjSplicePatched || typeof v.splicePointCloud !== 'function') return;
const orig = v.splicePointCloud;
v.splicePointCloud = function (points) {
try {
const g = points && points.geometry;
if (g && g.attributes && g.attributes.position && !g.attributes.color) {
const Ctor = g.attributes.position.constructor;
g.setAttribute('color', new Ctor(new Float32Array(g.attributes.position.count * 3), 3));
ownColor.add(g);
}
} catch (e) {}
return orig.apply(this, arguments);
};
v.__xjSplicePatched = true;
}
function collectTargets() {
const v = getViewer();
if (!v) return [];
const out = [], seen = new Set();
const push = (p) => {
if (!p || !p.geometry || !p.geometry.attributes || !p.geometry.attributes.position) return;
if (seen.has(p)) return;
seen.add(p); out.push(p);
};
push(v.points);
for (const g of [v.pointCloudGroup, v.compareGroup, v.sseGroup]) {
if (g && Array.isArray(g.children)) g.children.forEach(push);
}
return out;
}
let ticking = false;
async function tick() {
if (ticking) return;
ticking = true;
try {
// 帧号一变就立刻把"图和 pcd 头"先拉起来，与平台自己加载点云的过程重叠，
// 等点云一出现基本只剩投影的十几毫秒
const SM0 = getSM();
if (SM0) {
let fid = null;
try { fid = SM0.getCurFrameId(); } catch (e) {}
warmFrame(fid);
}
patchSplice(getViewer());
const targets = collectTargets();
let sawNew = false;
for (const p of targets) {
const sig = signatureOf(p);
if (!sig || sigOf.get(p) === sig || busy.has(p)) continue;
sawNew = true;
busy.add(p);
try {
const r = await colorizePoints(p);
if (r && r.ok) { sigOf.set(p, sig); setStatus(r.msg, false, r.native); schedulePrefetch(); }
else if (r && r.msg) setStatus(r.msg, true);
} catch (e) {
setStatus('异常: ' + e.message, true);
log('colorize 异常', e);
} finally { busy.delete(p); }
}
watchColorType(sawNew);
refreshModeText();
} finally {
ticking = false;
}
}
function restore() {
const v = getViewer();
if (!v) return;
let n = 0;
for (const p of collectTargets()) {
const g = p.geometry;
if (!g || !ownColor.has(g)) continue;
try { if (g.attributes.color) { g.deleteAttribute('color'); n++; } } catch (e) {}
sigOf.delete(p);
}
v.supportRgbColor = false;
setColorType(originalColorType !== null && originalColorType !== 3 ? originalColorType : 1);
try { v.rendererMain(); } catch (e) {}
setStatus('已还原（' + n + ' 个点云）', false);
}
async function diagnose() {
const v = getViewer(), SM = getSM();
const out = {};
try {
const g = v && v.points && v.points.geometry;
out.点云已加载 = !!(g && g.attributes && g.attributes.position);
if (out.点云已加载) {
out.点数 = g.attributes.position.count;
out.几何属性 = Object.keys(g.attributes);
out.平台认为支持rgb = !!v.supportRgbColor;
out.平台认为支持强度 = !!v.supportIntensityColor;
}
} catch (e) { out._err = String(e); }
try { out.当前着色模式 = SM.statusState().colorType; } catch (e) {}
try {
const cfg = SM.configState();
out.data_type = cfg.params && cfg.params.data_type;
out.相机 = Object.keys((cfg.params && cfg.params.sensor_params) || {});
out.有poses = !!(cfg.params && cfg.params.poses && Object.keys(cfg.params.poses).length);
} catch (e) {}
try {
const p = v.points;
const url = (p && p.url) || ((frameById(null) || {}).url);
out.本帧pcd = url;
if (url) {
const rec = await readPcdFile(url);
if (rec) {
out.pcd字段 = rec.hdr.fields;
out.pcd类型 = rec.hdr.types;
out.pcd编码 = rec.hdr.dataType;
out.pcd点数 = rec.hdr.points;
out.有rgb字段 = rec.hdr.fields.indexOf('rgb') >= 0;
out.有rgba字段 = rec.hdr.fields.indexOf('rgba') >= 0;
if (p && p.geometry) {
out.pcd与几何一致 = verifySameCloud(rec.bytes, rec.hdr, p.geometry.attributes.position.array, p.geometry.attributes.position.count);
}
}
}
} catch (e) { out._err_pcd = String(e); }
try {
out.本帧位姿 = poseById(SM.getCurFrameId()) ? '有' : '无';
out.本帧着色统计 = window.__xjRgbLastStat || null;
} catch (e) {}
const camArr = [];
const cams = {};
const sp = sensorParams(), frame = frameById(null);
if (sp) {
for (const k of Object.keys(sp)) {
const u = imageUrlOf(frame, k);
const rec = { 标定: !!(sp[k] && sp[k].fx !== undefined), 图像: u || null };
if (u) {
const im = await loadImage(u);
rec.可读像素 = !!im;
if (im) {
rec.尺寸 = im.width + 'x' + im.height;
camArr.push({ key: k, cam: sp[k], data: im.data, width: im.width, height: im.height });
} else rec.失败原因 = imgFailed.get(u) || '未知';
}
cams[k] = rec;
}
}
out.相机明细 = cams;
// 参考：把点云按"本帧雷达系 / 世界系(位姿列主序) / 世界系(位姿行主序)"三种假设各投一遍
try {
const p = v.points;
if (p && p.geometry && p.geometry.attributes.position && camArr.length) {
const pos = p.geometry.attributes.position.array;
const cnt = p.geometry.attributes.position.count;
const hyps = detectFrames(camArr[0].cam, poseById(SM.getCurFrameId()), pos, cnt, camArr);
out.坐标系假设 = hyps.map((h) => ({ 假设: h.name, 命中率: (h.cov * 100).toFixed(1) + '%' }));
}
} catch (e) { out._err_hyp = String(e); }
log('诊断', out);
return out;
}
/* ================================================================
* 9. 极简 UI：一个状态胶囊（没有任何调色项）
* ================================================================ */
const CSS = `
#xjrgb-root{position:fixed;right:20px;top:74px;z-index:2147483000;font:12px/1.5 -apple-system,"Microsoft YaHei","PingFang SC",sans-serif;user-select:none}
#xjrgb-pill{display:flex;align-items:center;gap:6px;height:26px;padding:0 10px;border-radius:13px;cursor:pointer;
background:rgba(9,14,26,.82);border:1px solid rgba(56,189,248,.32);color:#cfe6ff;font-size:11px}
#xjrgb-dot{width:7px;height:7px;border-radius:50%;background:#22c55e;box-shadow:0 0 7px #22c55e}
#xjrgb-dot.err{background:#f87171;box-shadow:0 0 7px #f87171}
#xjrgb-card{display:none;margin-top:6px;width:256px;padding:9px 11px;border-radius:10px;
background:rgba(9,14,26,.94);border:1px solid rgba(56,189,248,.28);box-shadow:0 10px 30px rgba(0,0,0,.5)}
#xjrgb-card.open{display:block}
#xjrgb-status{font-size:11px;color:#9fb6d0;line-height:1.5;word-break:break-all;margin-bottom:7px}
.xjrgb-btns{display:flex;gap:6px}
.xjrgb-btn{flex:1;height:26px;border:1px solid rgba(56,189,248,.3);border-radius:6px;background:rgba(30,41,59,.6);
color:#9fb6d0;font-size:11px;cursor:pointer}
.xjrgb-btn:hover{color:#e6f7ff}
`;
let ui = null;
function setStatus(msg, isErr, native) {
if (!ui) return;
ui.status.textContent = msg || '';
ui.dot.className = isErr ? 'err' : '';
ui.pillTxt.textContent = native ? 'RGB 原生' : 'RGB';
}
function buildPill() {
if (document.getElementById('xjrgb-root')) return;
const style = document.createElement('style');
style.textContent = CSS;
document.head.appendChild(style);
const root = document.createElement('div');
root.id = 'xjrgb-root';
root.innerHTML = `
RGB
`;
document.body.appendChild(root);
ui = {
status: root.querySelector('#xjrgb-status'),
mode: root.querySelector('#xjrgb-mode'),
dot: root.querySelector('#xjrgb-dot'),
pillTxt: root.querySelector('#xjrgb-pill-txt'),
card: root.querySelector('#xjrgb-card'),
};
root.querySelector('#xjrgb-pill').addEventListener('click', () => ui.card.classList.toggle('open'));
root.querySelector('#xjrgb-restore').addEventListener('click', () => restore());
root.querySelector('#xjrgb-force').addEventListener('click', () => {
userOverride = false;
setColorType(3);
setStatus('已切到 rgb 着色');
});
root.querySelector('#xjrgb-diag').addEventListener('click', async () => {
setStatus('诊断中…');
try { const d = await diagnose(); log(d); setStatus('诊断结果已输出到 F12 Console'); }
catch (e) { setStatus('诊断失败: ' + e.message, true); }
});
}
const MODE_NAME = { 1: '高度着色', 2: '反射强度', 3: 'rgb着色', 4: '不着色' };
let lastModeText = '';
function refreshModeText() {
if (!ui) return;
try {
const cur = getSM().statusState().colorType;
const txt = '当前显示模式：' + (MODE_NAME[cur] || cur) +
(cur === 3 ? ' ✓' : '（点上方按钮可切回 rgb）');
if (txt === lastModeText) return; // 避免每 120ms 写一次 DOM
lastModeText = txt;
ui.mode.textContent = txt;
} catch (e) {}
}
/* ================================================================
* 10. 启动 / 预热
* ================================================================ */
function loop() {
if (CFG.autoFollow) tick();
setTimeout(loop, CFG.pollMs);
}
/** 空闲时把下一帧的相机图预热进缓存，切帧时几乎瞬时 */
let prefetchTimer = null;
function schedulePrefetch() {
if (!CFG.prefetchNext || prefetchTimer) return;
prefetchTimer = setTimeout(() => {
prefetchTimer = null;
prefetchNextFrame();
}, 1200);
}
/**
* 帧切换时立刻预热：相机图 + pcd 头。loadImage/probePcdHeader 自带去重与缓存，
* 所以这里只管发出去，不 await —— 目的是让下载和平台加载点云并行，而不是排在它后面。
*/
let lastWarmedFrame = null;
function warmFrame(fid) {
if (fid === null || fid === undefined) return;
if (fid === lastWarmedFrame) return;
try {
const frame = frameById(fid);
if (!frame) return; // config 还没就绪，下一轮再试
lastWarmedFrame = fid;
const sp = sensorParams();
if (sp) for (const jb of cameraJobs(sp, frame)) loadImage(jb.url);
const u = frame.url;
if (u && !dirNoColor.has(dirOf(u))) probePcdHeader(u);
} catch (e) {}
}
function prefetchNextFrame() {
try {
const SM = getSM();
if (!SM) return;
const cfg = SM.configState();
const ids = cfg.frameIds, frames = cfg.frames;
if (!ids || !frames) return;
const idx = ids[SM.getCurFrameId()];
if (idx === undefined || idx === null) return;
const nf = frames[idx + 1];
if (!nf || !nf.slave_info) return;
const sp = sensorParams();
if (!sp) return;
for (const key of Object.keys(sp)) {
const u = imageUrlOf(nf, key);
if (u && !imgCache.has(u) && !imgFailed.has(u) && !imgPending.has(u)) loadImage(u);
}
} catch (e) {}
}
function init() {
buildPill();
try { originalColorType = getSM() ? getSM().statusState().colorType : null; } catch (e) {}
log('已启动（静默版）', 'instance=' + !!window.instance, 'viewer=' + !!getViewer());
try { warmFrame(getSM().getCurFrameId()); } catch (e) {} // 首帧立刻开始预热
tick(); // 立刻试一次，不再固定等待
loop();
}
function waitReady(tries) {
tries = tries || 0;
if (window.instance && getViewer()) { setTimeout(init, 0); return; }
if (tries > 400) { init(); return; }
setTimeout(() => waitReady(tries + 1), 120);
}
waitReady();
})();