js-subpcd/src/index.ts

🌟 High-performance JavaScript/TypeScript QuadTree point cloud filtering and processing library. Published on npm as js-subpcd with PCL.js compatible API for spatial filtering, subsampling, and nearest neighbor search.

/**
 * JavaScript QuadTree Point Cloud Filtering Package
 * PCL.js compatible API for point cloud processing with QuadTree spatial indexing
 */

import { QuadTree, Box, Point, Circle } from 'js-quadtree';
import * as fs from 'fs';

// Type definitions
export interface Point3DData {
    x: number;
    y: number;
    z: number;
    data?: any;
}

export interface CoordinateArrays {
    x: number[];
    y: number[];
    z: number[];
}

export interface Bounds {
    minX: number;
    minY: number;
    maxX: number;
    maxY: number;
}

export interface LayerInfo {
    layer: number;
    gridSize: number;
    totalCells: number;
    cellSize: number;
    spacingX: number;
    spacingY: number;
    depth: number;
}



export interface InitOptions {
    url?: string;
    [key: string]: any;
}

/**
 * Point class representing XYZ coordinates, compatible with PCL.js PointXYZ
 */
export class PointXYZ {
    public x: number;
    public y: number;
    public z: number;

    constructor(x: number = 0, y: number = 0, z: number = 0) {
        this.x = x;
        this.y = y;
        this.z = z;
    }

    /**
     * Create a copy of this point
     */
    clone(): PointXYZ {
        return new PointXYZ(this.x, this.y, this.z);
    }

    /**
     * Calculate distance to another point
     */
    distanceTo(other: PointXYZ): number {
        const dx = this.x - other.x;
        const dy = this.y - other.y;
        const dz = this.z - other.z;
        return Math.sqrt(dx * dx + dy * dy + dz * dz);
    }
}

/**
 * Legacy Point3D class for backward compatibility
 */
export class Point3D extends PointXYZ {
    public data?: any;

    constructor(x: number, y: number, z: number, data: any = null) {
        super(x, y, z);
        this.data = data;
    }
}

/**
 * Point Cloud data structure, compatible with PCL.js patterns
 */
export class PointCloud {
    public points: PointXYZ[];
    public pointType: typeof PointXYZ;
    public width: number;
    public height: number;
    public isOrganized: boolean;

    constructor(pointType: typeof PointXYZ = PointXYZ) {
        this.points = [];
        this.pointType = pointType;
        this.width = 0;
        this.height = 1; // Unorganized point cloud
        this.isOrganized = false;
    }

    /**
     * Get the number of points in the cloud
     */
    size(): number {
        return this.points.length;
    }

    /**
     * Check if the point cloud is empty
     */
    empty(): boolean {
        return this.points.length === 0;
    }

    /**
     * Add a point to the cloud
     */
    addPoint(point: PointXYZ): void {
        this.points.push(point);
        this.width = this.points.length;
    }

    /**
     * Get point at index
     */
    at(index: number): PointXYZ {
        return this.points[index];
    }

    /**
     * Clear all points
     */
    clear(): void {
        this.points = [];
        this.width = 0;
    }
}

/**
 * Data loader for point cloud files, following PCL.js async patterns
 */
export class DataLoader {
    public cloud: PointCloud;
    public bounds: number[] | null;

    constructor() {
        this.cloud = new PointCloud(PointXYZ);
        this.bounds = null;
    }

    /**
     * Load point cloud data from file (async, PCL.js style)
     */
    async loadPCDData(input: string | ArrayBuffer): Promise<PointCloud> {
        try {
            let data: string;
            if (typeof input === 'string') {
                // File path
                data = fs.readFileSync(input, 'utf8');
            } else if (input instanceof ArrayBuffer) {
                // ArrayBuffer data (PCL.js style)
                data = new TextDecoder().decode(input);
            } else {
                throw new Error('Input must be file path or ArrayBuffer');
            }

            return this._parsePointCloudData(data);
        } catch (error: any) {
            throw new Error(`Failed to load point cloud data: ${error.message}`);
        }
    }

    /**
     * Load points from a text file (legacy method)
     */
    loadFromFile(filename: string): PointXYZ[] {
        try {
            const data = fs.readFileSync(filename, 'utf8');
            const cloud = this._parsePointCloudData(data);
            return cloud.points;
        } catch (error: any) {
            throw new Error(`Failed to load from file: ${error.message}`);
        }
    }

    /**
     * Parse point cloud data from string
     */
    _parsePointCloudData(data: string): PointCloud {
        const lines = data.split('\n').filter(line => line.trim() !== '');
        const cloud = new PointCloud(PointXYZ);
        
        for (const line of lines) {
            const coords = line.trim().split(/\s+/).map(Number);
            if (coords.length >= 3 && !isNaN(coords[0]) && !isNaN(coords[1]) && !isNaN(coords[2])) {
                const point = new PointXYZ(coords[0], coords[1], coords[2]);
                cloud.addPoint(point);
            }
        }
        
        this.cloud = cloud;
        this._calculateBounds();
        return cloud;
    }

    /**
     * Calculate bounding box from loaded points
     */
    private _calculateBounds(): void {
        if (this.cloud.empty()) {
            this.bounds = null;
            return;
        }
        
        const points = this.cloud.points;
        let minX = points[0].x, minY = points[0].y, maxX = points[0].x, maxY = points[0].y;
        
        for (const point of points) {
            minX = Math.min(minX, point.x);
            minY = Math.min(minY, point.y);
            maxX = Math.max(maxX, point.x);
            maxY = Math.max(maxY, point.y);
        }
        
        this.bounds = [minX, minY, maxX, maxY];
    }

    /**
     * Get coordinate arrays (legacy compatibility)
     */
    getCoordinateArrays(): CoordinateArrays {
        const coordinateArrays: CoordinateArrays = { x: [], y: [], z: [] };
        for (const point of this.cloud.points) {
            coordinateArrays.x.push(point.x);
            coordinateArrays.y.push(point.y);
            coordinateArrays.z.push(point.z);
        }
        return coordinateArrays;
    }

    /**
     * Get bounding box
     */
    getBounds(): number[] | null {
        return this.bounds;
    }

    /**
     * Get points array (legacy compatibility)
     */
    get points(): PointXYZ[] {
        return this.cloud.points;
    }

    /**
     * Set points array (legacy compatibility)
     */
    set points(points: PointXYZ[]) {
        this.cloud.clear();
        for (const point of points) {
            this.cloud.addPoint(point);
        }
        this._calculateBounds();
    }
}

/**
 * QuadTree Manager using js-quadtree library
 */
export class QuadTreeManager {
    public quadtree: QuadTree | null;
    public points: Point3D[];
    public bounds: number[] | null;
    public coordinateArrays: CoordinateArrays | null;
    public sparseThreshold: number;

    constructor() {
        this.quadtree = null;
        this.points = [];
        this.bounds = null;
        this.coordinateArrays = null;
        this.sparseThreshold = 0.5;
    }

    /**
     * Build quadtree from Point3D objects
     */
    buildTree(points: Point3D[], bounds: number[], maxElements: number = 10, maxDepth: number = 8): void {
        this.points = points;
        this.bounds = bounds;
        
        const [minX, minY, maxX, maxY] = bounds;
        const width = maxX - minX;
        const height = maxY - minY;
        
        // Create quadtree with bounding box (x, y, width, height)
        const boundingArea = new Box(minX, minY, width, height);
        const config = {
            capacity: maxElements,
            maximumDepth: maxDepth,
            removeEmptyNodes: false
        };
        
        this.quadtree = new QuadTree(boundingArea, config);
        
        // Insert all points with their index as data
        for (let i = 0; i < points.length; i++) {
            const point = points[i];
            const qtPoint = new Point(point.x, point.y, { index: i, z: point.z });
            this.quadtree.insert(qtPoint);
        }
    }

    /**
     * Build quadtree from coordinate arrays
     */
    buildTreeFromArrays(xCoords: number[], yCoords: number[], zCoords: number[], bounds: number[], maxElements: number = 10, maxDepth: number = 8): void {
        this.coordinateArrays = { x: xCoords, y: yCoords, z: zCoords };
        this.bounds = bounds;
        
        const [minX, minY, maxX, maxY] = bounds;
        const width = maxX - minX;
        const height = maxY - minY;
        
        // Create quadtree with bounding box (x, y, width, height)
        const boundingArea = new Box(minX, minY, width, height);
        const config = {
            capacity: maxElements,
            maximumDepth: maxDepth,
            removeEmptyNodes: false
        };
        
        this.quadtree = new QuadTree(boundingArea, config);
        
        // Insert all points using coordinate arrays
        for (let i = 0; i < xCoords.length; i++) {
            const qtPoint = new Point(xCoords[i], yCoords[i], { index: i, z: zCoords[i] });
            this.quadtree.insert(qtPoint);
        }
    }

    /**
     * Query points within a rectangular region
     */
    queryRegion(minX: number, minY: number, maxX: number, maxY: number): Point3D[] {
        if (!this.quadtree) {
            return [];
        }
        
        const width = maxX - minX;
        const height = maxY - minY;
        const queryBox = new Box(minX, minY, width, height);
        
        const results = this.quadtree.query(queryBox);
        const resultPoints: Point3D[] = [];
        
        for (const qtPoint of results) {
            const index = qtPoint.data.index;
            
            if (this.coordinateArrays) {
                // Using coordinate arrays
                if (index >= 0 && index < this.coordinateArrays.x.length) {
                    const point = new Point3D(
                        this.coordinateArrays.x[index],
                        this.coordinateArrays.y[index],
                        this.coordinateArrays.z[index]
                    );
                    resultPoints.push(point);
                }
            } else if (this.points) {
                // Using Point3D objects
                if (index >= 0 && index < this.points.length) {
                    resultPoints.push(this.points[index]);
                }
            }
        }
        
        return resultPoints;
    }

    /**
     * Query point indices within a rectangular region (optimized)
     */
    queryRegionIndices(minX: number, minY: number, maxX: number, maxY: number): number[] {
        if (!this.quadtree) {
            return [];
        }
        
        const width = maxX - minX;
        const height = maxY - minY;
        const queryBox = new Box(minX, minY, width, height);
        
        const results = this.quadtree.query(queryBox);
        const indices: number[] = [];
        
        for (const qtPoint of results) {
            const index = qtPoint.data.index;
            
            // Validate index bounds
            if (this.coordinateArrays) {
                if (index >= 0 && index < this.coordinateArrays.x.length) {
                    indices.push(index);
                }
            } else if (this.points) {
                if (index >= 0 && index < this.points.length) {
                    indices.push(index);
                }
            }
        }
        
        return indices;
    }

    /**
     * Find nearest neighbors to a point
     */
    findNearestNeighbors(x: number, y: number, count: number = 1, maxDistance: number | null = null): Point3D[] {
        if (!this.quadtree) {
            return [];
        }
        
        // Get all points and calculate distances manually
        // js-quadtree doesn't have built-in nearest neighbor search
        const allPoints = this.quadtree.getAllPoints();
        const distances: [Point3D, number][] = [];
        
        for (const qtPoint of allPoints) {
            const distance = Math.sqrt(
                Math.pow(qtPoint.x - x, 2) + Math.pow(qtPoint.y - y, 2)
            );
            
            if (maxDistance === null || distance <= maxDistance) {
                const index = qtPoint.data.index;
                let point3d: Point3D | undefined;
                
                if (this.coordinateArrays) {
                    point3d = new Point3D(
                        this.coordinateArrays.x[index],
                        this.coordinateArrays.y[index],
                        this.coordinateArrays.z[index]
                    );
                } else if (this.points) {
                    point3d = this.points[index];
                }
                
                if (point3d) {
                    distances.push([point3d, distance]);
                }
            }
        }
        
        // Sort by distance and return top count
        distances.sort((a, b) => a[1] - b[1]);
        return distances.slice(0, count).map(([point, distance]) => point);
    }

    /**
     * Get statistics about the quadtree
     */
    getStatistics(): { totalPoints: number } {
        if (!this.quadtree) {
            return { totalPoints: 0 };
        }
        
        const totalPoints = this.coordinateArrays ? 
            this.coordinateArrays.x.length : 
            (this.points ? this.points.length : 0);
        
        return {
            totalPoints: totalPoints
        };
    }

    /**
     * Check if quadtree is built
     */
    isBuilt(): boolean {
        return this.quadtree !== null;
    }

    /**
     * Clear the quadtree
     */
    clear(): void {
        if (this.quadtree) {
            this.quadtree.clear();
        }
        this.quadtree = null;
        this.points = [];
        this.bounds = null;
        this.coordinateArrays = null;
    }
}

/**
 * QuadTree Filter for point cloud filtering operations
 */
export class QuadTreeFilter {
    public minCellSize: number;
    public dataLoader: DataLoader;
    public quadtreeManager: QuadTreeManager;
    public layerSchedule: LayerInfo[] | null;
    public points: PointXYZ[];
    public bounds: number[] | null;

    // Caching
    private _cachedPointsHash: string | null;
    private _cachedBounds: number[] | null;
    private _cachedLayerSchedule: LayerInfo[] | null;
    private _treeBuilt: boolean;

    constructor(minCellSize: number) {
        this.minCellSize = minCellSize;
        this.dataLoader = new DataLoader();
        this.quadtreeManager = new QuadTreeManager();
        this.layerSchedule = null;
        this.points = [];
        this.bounds = null;
        
        // Caching
        this._cachedPointsHash = null;
        this._cachedBounds = null;
        this._cachedLayerSchedule = null;
        this._treeBuilt = false;
    }

    /**
     * Load points from file
     */
    loadFromFile(filename: string): void {
        this.points = this.dataLoader.loadFromFile(filename);
        this.bounds = this.dataLoader.getBounds();
    }

    /**
     * Filter points by grid spacing
     */
    filterPointsByGridSpacing(xCoords: number[], yCoords: number[], zCoords: number[], minLateralSpacing: number): Point3D[] {
        // Calculate points hash for caching
        const pointsHash = this._calculatePointsHash(xCoords, yCoords, zCoords);
        
        if (this._cachedPointsHash !== pointsHash || !this._treeBuilt) {
            // Calculate bounds
            if (xCoords.length === 0) {
                return [];
            }
            
            // Use efficient loops instead of spread operator to avoid stack overflow
            let minX = xCoords[0], maxX = xCoords[0];
            let minY = yCoords[0], maxY = yCoords[0];
            
            for (let i = 1; i < xCoords.length; i++) {
                if (xCoords[i] < minX) minX = xCoords[i];
                if (xCoords[i] > maxX) maxX = xCoords[i];
            }
            
            for (let i = 1; i < yCoords.length; i++) {
                if (yCoords[i] < minY) minY = yCoords[i];
                if (yCoords[i] > maxY) maxY = yCoords[i];
            }
            
            this.bounds = [minX, minY, maxX, maxY];
            
            // Calculate layer schedule
            this.layerSchedule = this.calculateLayerSchedule(this.bounds, this.minCellSize);
            
            // Build quadtree
            this.quadtreeManager.buildTreeFromArrays(xCoords, yCoords, zCoords, this.bounds);
            
            // Update cache
            this._cachedPointsHash = pointsHash;
            this._cachedBounds = this.bounds;
            this._cachedLayerSchedule = this.layerSchedule;
            this._treeBuilt = true;
        } else {
            // Use cached values
            this.bounds = this._cachedBounds;
            this.layerSchedule = this._cachedLayerSchedule;
        }
        
        // Filter points based on grid spacing
        return this._filterByGridSpacing(minLateralSpacing);
    }

    /**
     * Calculate hash for coordinate arrays
     */
    private _calculatePointsHash(xCoords: number[], yCoords: number[], zCoords: number[]): string {
        if (xCoords.length === 0) {
            return 'empty';
        }
        
        // Simple hash based on length and sample points
        const sampleData: number[] = [];
        
        // Add first few points
        for (let i = 0; i < Math.min(5, xCoords.length); i++) {
            sampleData.push(xCoords[i], yCoords[i], zCoords[i]);
        }
        
        // Add last few points if different from first
        if (xCoords.length > 5) {
            for (let i = Math.max(xCoords.length - 5, 5); i < xCoords.length; i++) {
                sampleData.push(xCoords[i], yCoords[i], zCoords[i]);
            }
        }
        
        // Create hash from length and sample data
        const hashStr = `${xCoords.length}_${JSON.stringify(sampleData).split('').reduce((a, b) => {
            a = ((a << 5) - a) + b.charCodeAt(0);
            return a & a;
        }, 0)}`;
        
        return hashStr;
    }

    /**
     * Calculate layer schedule for power-of-2 grids
     */
    calculateLayerSchedule(bounds: number[], minCellSize: number): LayerInfo[] {
        const [minX, minY, maxX, maxY] = bounds;
        const width = maxX - minX;
        const height = maxY - minY;
        const maxDimension = Math.max(width, height);
        
        const layers: LayerInfo[] = [];
        let layerNum = 0;
        
        // Power-of-2 grid sizes
        const gridSizes = [2, 4, 8, 16, 32, 64, 128, 256, 512, 1024, 2048];
        
        for (const gridSize of gridSizes) {
            // Calculate cell size for this grid
            const cellSize = maxDimension / gridSize;
            
            // Stop if cell size is smaller than minimum
            if (cellSize < minCellSize) {
                break;
            }
            
            // Calculate actual spacing
            const spacingX = width / gridSize;
            const spacingY = height / gridSize;
            
            const layerInfo: LayerInfo = {
                layer: layerNum,
                gridSize: gridSize,
                totalCells: gridSize * gridSize,
                cellSize: cellSize,
                spacingX: spacingX,
                spacingY: spacingY,
                depth: Math.log2(gridSize)
            };
            
            layers.push(layerInfo);
            layerNum++;
        }
        
        return layers;
    }

    /**
     * Filter points by grid spacing (internal method)
     */
    private _filterByGridSpacing(minLateralSpacing: number): Point3D[] {
        if (!this.bounds || !this.layerSchedule) {
            return [];
        }
        
        // Find appropriate layer based on min_lateral_spacing
        let targetLayer: LayerInfo | null = null;
        for (const layer of this.layerSchedule) {
            if (Math.min(layer.spacingX, layer.spacingY) >= minLateralSpacing) {
                targetLayer = layer;
                break;
            }
        }
        
        if (!targetLayer) {
            // Use the finest layer if no suitable layer found
            targetLayer = this.layerSchedule[this.layerSchedule.length - 1];
        }
        
        return this.filterByDepth(targetLayer.depth);
    }

    /**
     * Filter points by quadtree depth
     */
    filterByDepth(targetDepth: number): Point3D[] {
        if (!this.quadtreeManager.isBuilt()) {
            return [];
        }
        
        const filteredPoints: Point3D[] = [];
        const gridSize = Math.pow(2, targetDepth);
        
        if (this.bounds) {
            const [minX, minY, maxX, maxY] = this.bounds;
            const width = maxX - minX;
            const height = maxY - minY;
            
            const cellWidth = width / gridSize;
            const cellHeight = height / gridSize;
            
            // Sample one point per cell at target depth
            for (let i = 0; i < gridSize; i++) {
                for (let j = 0; j < gridSize; j++) {
                    const cellMinX = minX + i * cellWidth;
                    const cellMaxX = minX + (i + 1) * cellWidth;
                    const cellMinY = minY + j * cellHeight;
                    const cellMaxY = minY + (j + 1) * cellHeight;
                    
                    // Query point indices in this cell
                    const cellIndices = this.quadtreeManager.queryRegionIndices(
                        cellMinX, cellMinY, cellMaxX, cellMaxY
                    );
                    
                    // Take the first point in the cell
                    if (cellIndices.length > 0) {
                        const firstIndex = cellIndices[0];
                        const coords = this.quadtreeManager.coordinateArrays;
                        
                        if (coords && firstIndex < coords.x.length) {
                            const point = new Point3D(
                                coords.x[firstIndex],
                                coords.y[firstIndex],
                                coords.z[firstIndex]
                            );
                            filteredPoints.push(point);
                        }
                    }
                }
            }
        }
        
        return filteredPoints;
    }
}

// PCL.js compatible types
export interface PCLObject {
    PointXYZ: typeof PointXYZ;
    PointCloud: typeof PointCloud;
    DataLoader: typeof DataLoader;
    QuadTreeManager: typeof QuadTreeManager;
    QuadTreeFilter: typeof QuadTreeFilter;
    loadPCDData: (data: ArrayBuffer) => Promise<PointCloud>;
    savePCDDataASCII: (cloud: PointCloud) => ArrayBuffer;
}

/**
 * PCL.js compatible initialization function
 */
export async function init(options: InitOptions = {}): Promise<PCLObject> {
    // Simulate PCL.js async initialization
    return new Promise((resolve) => {
        setTimeout(() => {
            resolve({
                PointXYZ,
                PointCloud,
                DataLoader,
                QuadTreeManager,
                QuadTreeFilter,
                loadPCDData: async (data: ArrayBuffer): Promise<PointCloud> => {
                    const loader = new DataLoader();
                    return await loader.loadPCDData(data);
                },
                savePCDDataASCII: (cloud: PointCloud): ArrayBuffer => {
                    // Convert point cloud to PCD ASCII format
                    let pcdData = '# .PCD v0.7 - Point Cloud Data file format\n';
                    pcdData += 'VERSION 0.7\n';
                    pcdData += 'FIELDS x y z\n';
                    pcdData += 'SIZE 4 4 4\n';
                    pcdData += 'TYPE F F F\n';
                    pcdData += 'COUNT 1 1 1\n';
                    pcdData += `WIDTH ${cloud.width}\n`;
                    pcdData += `HEIGHT ${cloud.height}\n`;
                    pcdData += 'VIEWPOINT 0 0 0 1 0 0 0\n';
                    pcdData += `POINTS ${cloud.size()}\n`;
                    pcdData += 'DATA ascii\n';
                    
                    for (const point of cloud.points) {
                        pcdData += `${point.x} ${point.y} ${point.z}\n`;
                    }
                    
                    return new TextEncoder().encode(pcdData).buffer;
                }
            });
        }, 10); // Small delay to simulate async initialization
    });
}

/**
 * Load PCD data (global function, PCL.js style)
 */
export function loadPCDData(data: ArrayBuffer, pointType: typeof PointXYZ = PointXYZ): PointCloud {
    const loader = new DataLoader();
    const textData = new TextDecoder().decode(data);
    return loader._parsePointCloudData(textData);
}

/**
 * Save point cloud as PCD ASCII data
 */
export function savePCDDataASCII(cloud: PointCloud): ArrayBuffer {
    let pcdData = '# .PCD v0.7 - Point Cloud Data file format\n';
    pcdData += 'VERSION 0.7\n';
    pcdData += 'FIELDS x y z\n';
    pcdData += 'SIZE 4 4 4\n';
    pcdData += 'TYPE F F F\n';
    pcdData += 'COUNT 1 1 1\n';
    pcdData += `WIDTH ${cloud.width}\n`;
    pcdData += `HEIGHT ${cloud.height}\n`;
    pcdData += 'VIEWPOINT 0 0 0 1 0 0 0\n';
    pcdData += `POINTS ${cloud.size()}\n`;
    pcdData += 'DATA ascii\n';
    
    for (const point of cloud.points) {
        pcdData += `${point.x} ${point.y} ${point.z}\n`;
    }
    
    return new TextEncoder().encode(pcdData).buffer;
}

// Default export
const _default = {
    PointXYZ,
    PointCloud,
    Point3D,
    DataLoader,
    QuadTreeManager,
    QuadTreeFilter,
    init,
    loadPCDData,
    savePCDDataASCII
};

export default _default;