diff --git a/frontend/pc-tool/src/components/Tool/Setting.vue b/frontend/pc-tool/src/components/Tool/Setting.vue index b23bcffc..7bfada25 100644 --- a/frontend/pc-tool/src/components/Tool/Setting.vue +++ b/frontend/pc-tool/src/components/Tool/Setting.vue @@ -324,7 +324,7 @@ let options = {} as any; switch (type) { case 'pointSize': - options.pointSize = config.pointSize * 10; + options.pointSize = config.pointSize; break; case 'colorType': options.colorMode = config.pointColorMode || 0; diff --git a/frontend/pc-tool/src/packages/pc-render/loader/PCDFile/index.ts b/frontend/pc-tool/src/packages/pc-render/loader/PCDFile/index.ts new file mode 100644 index 00000000..8675db62 --- /dev/null +++ b/frontend/pc-tool/src/packages/pc-render/loader/PCDFile/index.ts @@ -0,0 +1,188 @@ +import { + str2ab, + ab2str, + parseHeader, + getPointsFromTextData, + getPointsFromDataView, + setWithTypeToDataView, +} from './lib'; + +export type DataType = 'ascii' | 'binary'; +export type Type = 'F' | 'I' | 'U'; +export type SizeType = 1 | 2 | 4 | 8; + +export interface IConfig { + fields: string[]; + data: DataType; + size: SizeType[]; + type: Type[]; + count: number[]; + offset: number[]; + width: number; + height: number; + viewpoint: number[]; + rowSize: number; + points: number; + version: string; + raw: string; +} + +export default class PCDFile { + // header + version = 0.7; + fields: IConfig['fields'] = []; + size: IConfig['size'] = []; + type: IConfig['type'] = []; + count: IConfig['count'] = []; + offset: IConfig['offset'] = []; + width = 1; + height = 0; + points = 0; + rowSize = 0; + viewpoint: IConfig['viewpoint'] = [0, 0, 0, 1, 0, 0, 0]; + // + data: DataType = 'binary'; + isList = false; + pointsDataList: number[] = []; + pointsDataMap: Record = {}; + littleEndian = true; + constructor(pointsData: number[] | Record, config: Partial) { + const fields = config.fields || []; + this.fields = fields; + this.data = config.data || 'binary'; + + let length = 0; + if (Array.isArray(pointsData)) { + this.isList = true; + this.pointsDataList = pointsData; + length = Math.floor(this.pointsDataList.length / fields.length); + } else { + this.isList = false; + this.pointsDataMap = pointsData; + length = this.pointsDataMap[fields[0]].length; + } + + this.points = length; + this.height = length; + + this.size = config.size ? config.size : fields.map((e) => 4); + this.type = config.type ? config.type : fields.map((e) => 'F'); + this.count = fields.map((e) => 1); + + const offset = [] as number[]; + let rowSize = 0; + fields.forEach((e, index) => { + offset.push(rowSize); + rowSize += this.size[index]; + }); + this.offset = offset; + this.rowSize = rowSize; + } + + static parse(buffer: ArrayBuffer, fieldFilter?: string[], littleEndian = true) { + // let chunkData = ab2str(new Uint8Array(buffer)); + const chunkData = ab2str(new Uint8Array(buffer, 0, Math.min(buffer.byteLength, 1000))); + const header = parseHeader(chunkData); + + let dataMap = {} as Record; + if (header.data === 'ascii') { + const textData = ab2str(new Uint8Array(buffer)); + const pcdData = textData.substring(header.raw.length); + dataMap = getPointsFromTextData(pcdData, header, fieldFilter); + } else if (header.data === 'binary') { + const dataview = new DataView(buffer, header.raw.length); + dataMap = getPointsFromDataView(dataview, header, fieldFilter, littleEndian); + } else { + throw 'only support ascii or binary format'; + } + + if (Object.keys(dataMap).length === 0) throw 'no point data'; + + return new PCDFile(dataMap, header); + } + + getHeader(name: string) { + let value = this[name]; + name = name.toUpperCase(); + if (Array.isArray(value)) value = value.join(' '); + return `${name} ${value}`; + } + + getHeaderInfo() { + const version = '# .PCD v.7 - Point Cloud Data file format'; + const headers = [ + version, + this.getHeader('fields'), + this.getHeader('size'), + this.getHeader('type'), + this.getHeader('count'), + this.getHeader('width'), + this.getHeader('height'), + this.getHeader('viewpoint'), + this.getHeader('points'), + this.getHeader('data'), + ]; + + return headers.join('\n'); + } + + getDataBuffer() { + let header = this.getHeaderInfo(); + if (this.data === 'ascii') { + let textData = header; + textData += this.getPointAsciiData(); + return str2ab(textData); + } else { + // binary + header += '\n'; + const bufferSize = header.length + this.points * this.rowSize; + const arrayBuffer = new ArrayBuffer(bufferSize); + + // write header + const headerView = new Uint8Array(arrayBuffer, 0, header.length); + for (let i = 0; i < header.length; i++) { + headerView[i] = header.charCodeAt(i); + } + + // write data + const dataview = new DataView(arrayBuffer, header.length); + this.setPointsToDataView(dataview); + + return arrayBuffer; + } + } + + getPointAsciiData() { + const { pointsDataList, pointsDataMap, isList, points, fields } = this; + let textData = ''; + for (let i = 0; i < points; i++) { + textData += '\n'; + if (isList) { + textData += fields.map((e, j) => pointsDataList[i * fields.length + j]).join(' '); + } else { + textData += fields.map((e, j) => pointsDataMap[e][i]).join(' '); + } + } + return textData; + } + + setPointsToDataView(dataview: DataView) { + const { pointsDataList, pointsDataMap, isList, points, fields } = this; + const littleEndian = this.littleEndian; + for (let i = 0; i < points; i++) { + for (let j = 0; j < fields.length; j++) { + const type = this.type[j]; + const size = this.size[j]; + let data; + if (isList) { + data = pointsDataList[i * fields.length + j]; + } else { + data = pointsDataMap[fields[j]][i]; + } + + const offset = i * this.rowSize + this.offset[j]; + setWithTypeToDataView(dataview, offset, littleEndian, type, size, data); + } + } + } +} diff --git a/frontend/pc-tool/src/packages/pc-render/loader/PCDFile/lib.ts b/frontend/pc-tool/src/packages/pc-render/loader/PCDFile/lib.ts new file mode 100644 index 00000000..14f5ded6 --- /dev/null +++ b/frontend/pc-tool/src/packages/pc-render/loader/PCDFile/lib.ts @@ -0,0 +1,316 @@ +import { Type, IConfig, SizeType } from './index'; + +export function ab2str(array: ArrayBuffer) { + let s = ''; + for (let i = 0, il = array.byteLength; i < il; i++) { + s += String.fromCharCode(array[i]); + } + return s; +} + +export function str2ab(str: string) { + const array = new Uint8Array(str.length); + for (let i = 0; i < str.length; i++) { + array[i] = str.charCodeAt(i); + } + return array.buffer; +} + +export function correctNumber(n: number) { + return isFinite(n) ? n : 0; +} +export function correctXYZ(x: number, y: number, z: number) { + return { + x: correctNumber(x), + y: correctNumber(y), + z: correctNumber(z), + }; +} + +export function setWithTypeToDataView( + dataview: DataView, + offset: number, + littleEndian: boolean, + type: Type, + size: number, + data: any, +) { + switch (type) { + case 'F': + switch (size) { + case 4: + dataview.setFloat32(offset, data, littleEndian); + return; + case 8: + dataview.setFloat64(offset, data, littleEndian); + return; + default: + break; + } + break; + case 'U': + switch (size) { + case 1: + dataview.setUint8(offset, data); + return; + case 2: + dataview.setUint16(offset, data, littleEndian); + return; + case 4: + dataview.setUint32(offset, data, littleEndian); + return; + default: + break; + } + break; + case 'I': + switch (size) { + case 1: + dataview.setInt8(offset, data); + return; + case 2: + dataview.setInt16(offset, data, littleEndian); + return; + case 4: + dataview.setInt32(offset, data, littleEndian); + return; + default: + break; + } + break; + default: + break; + } +} + +export function parseHeader(textData: string) { + const header = {} as any; + const dataStart = textData.search(/[\r\n]DATA\s(\S*)[\f\r\t\v]*\n/i); + if (dataStart == -1) { + throw 'PCD-Format: not found DATA'; + } + + const result = /[\r\n]DATA\s(\S*)[\f\r\t\v]*\n/i.exec(textData) as any[]; + header.raw = textData.substring(0, dataStart + result[0].length); + header.str = header.raw.replace(/#.*/gi, ''); + + // parse + header.version = /VERSION (.*)/i.exec(header.str); + header.fields = /FIELDS (.*)/i.exec(header.str); + header.size = /SIZE (.*)/i.exec(header.str); + header.type = /TYPE (.*)/i.exec(header.str); + header.count = /COUNT (.*)/i.exec(header.str); + header.width = /WIDTH (.*)/i.exec(header.str); + header.height = /HEIGHT (.*)/i.exec(header.str); + header.viewpoint = /VIEWPOINT (.*)/i.exec(header.str); + header.points = /POINTS (.*)/i.exec(header.str); + header.data = /DATA (.*)/i.exec(header.str); + + // evaluate + if (header.version != undefined) header.version = parseFloat(header.version[1]); + + if (header.fields != undefined) + header.fields = header.fields[1].split(' ').map((e: any) => String(e).toLowerCase()); + + if (header.type != undefined) + header.type = header.type[1].split(' ').map((e: any) => String(e).toUpperCase()); + + if (header.width != undefined) header.width = parseInt(header.width[1]); + + if (header.height != undefined) header.height = parseInt(header.height[1]); + + if (header.viewpoint != undefined) + header.viewpoint = header.viewpoint[1].split(' ').map(parseFloat); + + if (header.points != undefined) header.points = parseInt(header.points[1], 10); + + if (header.points == undefined) header.points = header.width * header.height; + + if (header.data != undefined) header.data = header.data[1]; + + if (header.size != undefined) { + header.size = header.size[1].split(' ').map(function (x: any) { + return parseInt(x, 10); + }); + } + + if (header.count != undefined) { + header.count = header.count[1].split(' ').map(function (x: any) { + return parseInt(x, 10); + }); + } else { + header.count = []; + for (let i = 0, l = header.fields.length; i < l; i++) { + header.count.push(1); + } + } + + header.offset = []; + let sizeSum = 0; + for (let i = 0, l = header.fields.length; i < l; i++) { + header.offset.push(sizeSum); + if (header.data === 'ascii') { + sizeSum += header.count[i]; + } else { + sizeSum += header.size[i] * header.count[i]; + } + } + + // for binary only + header.rowSize = sizeSum; + + return header as IConfig; +} + +export function getPointsFromTextData(textData: string, header: IConfig, fieldFilter?: string[]) { + let fields = header.fields; + if (fieldFilter) { + fields = fields.filter((e) => fieldFilter.includes(e)); + } + + const dataMap = {} as Record; + const lines = textData.split('\n'); + + for (let i = 0; i < lines.length; i++) { + if (lines[i] === '') continue; + const line = lines[i].split(' '); + for (let j = 0; j < fields.length; j++) { + const field = fields[j]; + const type = header.type[j]; + const offset = header.offset[j]; + + const text = line[offset]; + const item = correctNumber(getWithTypeFromText(text, type)); + dataMap[field] = dataMap[field] || []; + dataMap[field].push(item); + } + } + return dataMap; +} + +export function getWithTypeFromText(text: string, type: Type) { + switch (type) { + case 'F': + return parseFloat(text); + case 'U': + case 'I': + return parseInt(text); + default: + break; + } + throw 'PCD-Format: parse data failed'; +} + +export function getPointsFromDataView( + dataview: DataView, + header: IConfig, + fieldFilter?: string[], + littleEndian = true, +) { + let fields = header.fields; + if (fieldFilter) { + fields = fields.filter((e) => fieldFilter.includes(e)); + } + + const dataMap = {} as Record; + for (let i = 0; i < header.points; i++) { + for (let j = 0; j < fields.length; j++) { + const field = fields[j]; + let type = header.type[j]; + const size = header.size[j]; + // const count = header.count[j]; + if (field === 'rgb') { + type = 'U'; + } + const offset = i * header.rowSize + header.offset[j]; + const item = correctNumber( + getWithTypeFromDataView(dataview, offset, littleEndian, type, size), + ); + dataMap[field] = dataMap[field] || new (typeArray(type, size))(header.points); + dataMap[field][i] = item; + } + } + return dataMap; +} + +export function getWithTypeFromDataView( + dataview: DataView, + offset: number, + littleEndian: boolean, + type: Type, + size: SizeType, +) { + switch (type) { + case 'F': + switch (size) { + case 4: + return dataview.getFloat32(offset, littleEndian); + case 8: + return dataview.getFloat64(offset, littleEndian); + default: + break; + } + break; + case 'U': + switch (size) { + case 1: + return dataview.getUint8(offset); + case 2: + return dataview.getUint16(offset, littleEndian); + case 4: + return dataview.getUint32(offset, littleEndian); + default: + break; + } + break; + case 'I': + switch (size) { + case 1: + return dataview.getInt8(offset); + case 2: + return dataview.getInt16(offset, littleEndian); + case 4: + return dataview.getInt32(offset, littleEndian); + default: + break; + } + break; + default: + break; + } + throw 'PCD-Format: parse data failed'; +} +export function typeArray(type: Type, size: SizeType) { + switch (type) { + case 'F': + switch (size) { + case 4: + return Float32Array; + case 8: + default: + return Float64Array; + } + case 'U': + switch (size) { + case 1: + return Uint8Array; + case 2: + return Uint16Array; + case 4: + default: + return Uint32Array; + } + case 'I': + switch (size) { + case 1: + return Int8Array; + case 2: + return Int16Array; + case 4: + default: + return Int32Array; + } + default: + return Array; + } +} diff --git a/frontend/pc-tool/src/packages/pc-render/loader/PCDLoader.ts b/frontend/pc-tool/src/packages/pc-render/loader/PCDLoader.ts index ef21f838..eb508f61 100644 --- a/frontend/pc-tool/src/packages/pc-render/loader/PCDLoader.ts +++ b/frontend/pc-tool/src/packages/pc-render/loader/PCDLoader.ts @@ -1,4 +1,5 @@ -import { FileLoader, Loader, LoaderUtils, MathUtils } from 'three'; +import { FileLoader, Loader, LoaderUtils, MathUtils, Cache } from 'three'; +import PCDFile from './PCDFile'; type ICallBack = (args?: any) => void; type PCDData = 'ascii' | 'binary_compressed' | 'binary'; @@ -6,12 +7,64 @@ type PCDFields = 'x' | 'y' | 'z' | 'i' | 'intensity' | 'rgb' | 'normal_x' | 'nor interface IPCDHeader { data: PCDData; offset: { [key in PCDFields]: number }; + fields: string[]; + type: Type[]; headerLen: number; height: number; points: number; - size: number[]; + size: SizeType[]; rowSize: number; } +type Type = 'F' | 'I' | 'U'; +type SizeType = 1 | 2 | 4 | 8; +Cache.enabled = false; +export function getWithTypeFromDataView( + dataview: DataView, + offset: number, + littleEndian: boolean, + type: Type, + size: SizeType, +) { + switch (type) { + case 'F': + switch (size) { + case 4: + return dataview.getFloat32(offset, littleEndian); + case 8: + return dataview.getFloat64(offset, littleEndian); + default: + break; + } + break; + case 'U': + switch (size) { + case 1: + return dataview.getUint8(offset); + case 2: + return dataview.getUint16(offset, littleEndian); + case 4: + return dataview.getUint32(offset, littleEndian); + default: + break; + } + break; + case 'I': + switch (size) { + case 1: + return dataview.getInt8(offset); + case 2: + return dataview.getInt16(offset, littleEndian); + case 4: + return dataview.getInt32(offset, littleEndian); + default: + break; + } + break; + default: + break; + } + throw 'PCD-Format: parse data failed'; +} export function correctNumber(n: number) { return isFinite(n) ? n : 0; } @@ -42,12 +95,10 @@ class PCDLoader extends Loader { url, function (data) { try { - onLoad(scope.parse(data, url)); + onLoad(scope.parse2(data)); } catch (e) { if (onError) { onError(e); - } else { - console.error(e); } scope.manager.itemError(url); @@ -324,7 +375,10 @@ class PCDLoader extends Loader { if (PCDheader.data === 'binary') { const dataview = new DataView(data, PCDheader.headerLen); const offset = PCDheader.offset; - + const iFieldIndex = PCDheader.fields.findIndex((field) => + ['i', 'intensity'].includes(field), + ); + const iFieldKey = PCDheader.fields[iFieldIndex]; for (let i = 0, row = 0; i < PCDheader.points; i++, row += PCDheader.rowSize) { if (offset.x !== undefined) { position.push( @@ -338,12 +392,14 @@ class PCDLoader extends Loader { ); } - if (offset.i !== undefined) { - const _i = dataview.getFloat32(row + offset.i, this.littleEndian); - intensity.push(_i); - maxIntensity = Math.max(_i, maxIntensity); - } else if (offset.intensity !== undefined) { - const _i = dataview.getFloat32(row + offset.intensity, this.littleEndian); + if (iFieldKey != undefined) { + const _i = getWithTypeFromDataView( + dataview, + row + offset.i, + this.littleEndian, + PCDheader.type[iFieldIndex], + PCDheader.size[iFieldIndex], + ); intensity.push(_i); maxIntensity = Math.max(_i, maxIntensity); } @@ -371,6 +427,41 @@ class PCDLoader extends Loader { intensity, }; } + parse2(data: any) { + const pcdData = PCDFile.parse(data).pointsDataMap; + const { + x = [], + y = [], + z = [], + intensity = [], + i = [], + rgb = [], + } = pcdData; + + const pointN = x.length; + const _position = new Float32Array(pointN * 3); + const _color = new Uint8Array(pointN * 3); + const targetI = intensity.length > 0 ? intensity : i.length > 0 ? i : undefined; + const hasColor = rgb.length > 0; + for (let i = 0; i < pointN; i++) { + _position[i * 3] = x[i]; + _position[i * 3 + 1] = y[i]; + _position[i * 3 + 2] = z[i]; + if (hasColor) { + _color[i * 3] = (rgb[i] >> 16) & 0x0000ff; + _color[i * 3 + 1] = (rgb[i] >> 8) & 0x0000ff; + _color[i * 3 + 2] = (rgb[i] >> 0) & 0x0000ff; + } + if (targetI && targetI[i] > 0 && targetI[i] < 1) { + targetI[i] = targetI[i] * 255; + } + } + return { + position: _position, + color: hasColor ? _color : [], + intensity: targetI, + } + } } export default PCDLoader;