RGB-D Codec Prototype for WebRTC

rgbdcodec.cpp

/**
 * RGB-D WebRTC CODEC
 * RealSense等のDepthデータをRGB画像に埋め込んでWebRTCで伝送するためのエンコーダー/デコーダー
 */

/**
 * チャンネル分離方式
 * 画像を4分割し、1つの領域に深度情報を格納
 */
class RGBDCodec {
    constructor(options = {}) {
        // 設定パラメータ
        this.depthScale = options.depthScale || 0.001; // RealSenseのデフォルトスケール
        this.maxDepth = options.maxDepth || 10.0; // 最大深度値(m)
        this.depthBits = options.depthBits || 16; // 深度の精度（ビット）
        this.compressionMode = options.compressionMode || 'LSB'; // 'LSB' or 'CHANNEL'
    }

    /**
     * RGB画像とDepth画像を1つのRGB画像にエンコード
     * @param {ImageData} rgbImageData - RGB画像データ
     * @param {Uint16Array} depthData - Depth画像データ（16bit）
     * @param {number} width - 画像幅
     * @param {number} height - 画像高さ
     * @returns {ImageData} エンコードされた画像データ
     */
    encode(rgbImageData, depthData, width, height) {
        if (this.compressionMode === 'LSB') {
            return this.encodeLSB(rgbImageData, depthData, width, height);
        } else if (this.compressionMode === 'CHANNEL') {
            return this.encodeChannel(rgbImageData, depthData, width, height);
        } else if (this.compressionMode === 'EXTENDED') {
            return this.encodeExtended(rgbImageData, depthData, width, height);
        }
    }

    /**
     * LSB（最下位ビット）埋め込み方式
     * RGBの各チャンネルの下位ビットに深度情報を埋め込む
     */
    encodeLSB(rgbImageData, depthData, width, height) {
        const encodedData = new Uint8ClampedArray(rgbImageData.data);
        
        for (let i = 0; i < width * height; i++) {
            const pixelIndex = i * 4;
            const depthValue = depthData[i];
            
            // 深度値を正規化（0-65535 → 0-4095の12bit）
            const normalizedDepth = Math.min(
                Math.floor((depthValue * this.depthScale / this.maxDepth) * 4095),
                4095
            );
            
            // 12bitの深度値をRGBチャンネルに分散
            // R: 上位4bit, G: 中位4bit, B: 下位4bit
            const depthR = (normalizedDepth >> 8) & 0x0F;
            const depthG = (normalizedDepth >> 4) & 0x0F;
            const depthB = normalizedDepth & 0x0F;
            
            // 元のRGB値の下位4bitを深度値で置換
            encodedData[pixelIndex] = (encodedData[pixelIndex] & 0xF0) | depthR;     // R
            encodedData[pixelIndex + 1] = (encodedData[pixelIndex + 1] & 0xF0) | depthG; // G
            encodedData[pixelIndex + 2] = (encodedData[pixelIndex + 2] & 0xF0) | depthB; // B
            // Alpha値はそのまま維持
        }
        
        return new ImageData(encodedData, width, height);
    }

    /**
     * 解像度拡張方式
     * 画像幅を拡張してRGBの右側に深度情報を配置
     */
    encodeExtended(rgbImageData, depthData, width, height) {
        // 深度データを格納するのに必要な追加幅を計算
        // 深度データは16bitなので、RGBチャンネルに分散して格納
        const depthPixelsNeeded = depthData.length;
        const depthWidth = Math.ceil(Math.sqrt(depthPixelsNeeded));
        const depthHeight = Math.ceil(depthPixelsNeeded / depthWidth);
        
        // 新しい画像サイズを計算（横に拡張）
        const newWidth = width + depthWidth;
        const newHeight = Math.max(height, depthHeight);
        
        const encodedData = new Uint8ClampedArray(newWidth * newHeight * 4);
        
        // 元のRGBデータをコピー
        for (let y = 0; y < height; y++) {
            for (let x = 0; x < width; x++) {
                const srcIndex = (y * width + x) * 4;
                const dstIndex = (y * newWidth + x) * 4;
                
                encodedData[dstIndex] = rgbImageData.data[srcIndex];         // R
                encodedData[dstIndex + 1] = rgbImageData.data[srcIndex + 1]; // G
                encodedData[dstIndex + 2] = rgbImageData.data[srcIndex + 2]; // B
                encodedData[dstIndex + 3] = rgbImageData.data[srcIndex + 3]; // A
            }
        }
        
        // 深度データを右側の領域に配置
        for (let i = 0; i < depthData.length; i++) {
            const depthX = i % depthWidth;
            const depthY = Math.floor(i / depthWidth);
            const x = width + depthX;
            const y = depthY;
            
            if (x < newWidth && y < newHeight) {
                const pixelIndex = (y * newWidth + x) * 4;
                const depthValue = depthData[i];
                
                // 16bit深度値を3つの8bitチャンネルに分散
                // より高精度な深度情報を保持
                const depthHigh = (depthValue >> 8) & 0xFF;
                const depthLow = depthValue & 0xFF;
                const depthExtra = Math.floor((depthValue * this.depthScale / this.maxDepth) * 255);
                
                encodedData[pixelIndex] = depthHigh;     // R: 上位8bit
                encodedData[pixelIndex + 1] = depthLow;  // G: 下位8bit
                encodedData[pixelIndex + 2] = depthExtra; // B: 正規化された深度値
                encodedData[pixelIndex + 3] = 255;       // A: マーカー（深度領域識別用）
            }
        }
        
        // 残りの深度領域を黒で埋める
        for (let y = 0; y < newHeight; y++) {
            for (let x = width; x < newWidth; x++) {
                const pixelIndex = (y * newWidth + x) * 4;
                if (encodedData[pixelIndex + 3] !== 255) { // 深度データがない場合
                    encodedData[pixelIndex] = 0;     // R
                    encodedData[pixelIndex + 1] = 0; // G
                    encodedData[pixelIndex + 2] = 0; // B
                    encodedData[pixelIndex + 3] = 0; // A: 空領域マーカー
                }
            }
        }
        
        const result = new ImageData(encodedData, newWidth, newHeight);
        
        // メタデータを保存（デコード時に必要）
        result.originalWidth = width;
        result.originalHeight = height;
        result.depthWidth = depthWidth;
        result.depthHeight = depthHeight;
        
        return result;
    }
    encodeChannel(rgbImageData, depthData, width, height) {
        const encodedData = new Uint8ClampedArray(rgbImageData.data);
        const halfWidth = Math.floor(width / 2);
        const halfHeight = Math.floor(height / 2);
        
        // 右下の領域に深度情報を格納
        for (let y = halfHeight; y < height; y++) {
            for (let x = halfWidth; x < width; x++) {
                const originalIndex = y * width + x;
                const depthIndex = (y - halfHeight) * halfWidth + (x - halfWidth);
                const pixelIndex = originalIndex * 4;
                
                if (depthIndex < depthData.length) {
                    const depthValue = depthData[depthIndex];
                    
                    // 16bit深度値を2つの8bitに分割
                    const depthHigh = (depthValue >> 8) & 0xFF;
                    const depthLow = depthValue & 0xFF;
                    
                    encodedData[pixelIndex] = depthHigh;     // R
                    encodedData[pixelIndex + 1] = depthLow;  // G
                    encodedData[pixelIndex + 2] = 0;         // B（未使用）
                    encodedData[pixelIndex + 3] = 255;       // A
                }
            }
        }
        
        return new ImageData(encodedData, width, height);
    }

    /**
     * エンコードされた画像からRGBと深度データを分離デコード
     * @param {ImageData} encodedImageData - エンコードされた画像データ
     * @returns {Object} {rgbData: ImageData, depthData: Uint16Array}
     */
    decode(encodedImageData) {
        if (this.compressionMode === 'LSB') {
            return this.decodeLSB(encodedImageData);
        } else if (this.compressionMode === 'CHANNEL') {
            return this.decodeChannel(encodedImageData);
        } else if (this.compressionMode === 'EXTENDED') {
            return this.decodeExtended(encodedImageData);
        }
    }

    /**
     * LSB方式のデコード
     */
    decodeLSB(encodedImageData) {
        const width = encodedImageData.width;
        const height = encodedImageData.height;
        const data = encodedImageData.data;
        
        const rgbData = new Uint8ClampedArray(data.length);
        const depthData = new Uint16Array(width * height);
        
        for (let i = 0; i < width * height; i++) {
            const pixelIndex = i * 4;
            
            // RGB値から深度情報を抽出
            const depthR = data[pixelIndex] & 0x0F;
            const depthG = data[pixelIndex + 1] & 0x0F;
            const depthB = data[pixelIndex + 2] & 0x0F;
            
            // 12bit深度値を復元
            const normalizedDepth = (depthR << 8) | (depthG << 4) | depthB;
            depthData[i] = Math.floor((normalizedDepth / 4095) * (this.maxDepth / this.depthScale));
            
            // RGB値を復元（下位4bitを0で埋める）
            rgbData[pixelIndex] = data[pixelIndex] & 0xF0;
            rgbData[pixelIndex + 1] = data[pixelIndex + 1] & 0xF0;
            rgbData[pixelIndex + 2] = data[pixelIndex + 2] & 0xF0;
            rgbData[pixelIndex + 3] = data[pixelIndex + 3];
        }
        
        return {
            rgbData: new ImageData(rgbData, width, height),
            depthData: depthData
        };
    }

    /**
     * チャンネル分離方式のデコード
     */
    decodeChannel(encodedImageData) {
        const width = encodedImageData.width;
        const height = encodedImageData.height;
        const data = encodedImageData.data;
        const halfWidth = Math.floor(width / 2);
        const halfHeight = Math.floor(height / 2);
        
        const rgbData = new Uint8ClampedArray(data);
        const depthData = new Uint16Array(halfWidth * halfHeight);
        
        // 右下領域から深度データを抽出
        for (let y = halfHeight; y < height; y++) {
            for (let x = halfWidth; x < width; x++) {
                const originalIndex = y * width + x;
                const depthIndex = (y - halfHeight) * halfWidth + (x - halfWidth);
                const pixelIndex = originalIndex * 4;
                
                if (depthIndex < depthData.length) {
                    const depthHigh = data[pixelIndex];
                    const depthLow = data[pixelIndex + 1];
                    depthData[depthIndex] = (depthHigh << 8) | depthLow;
                    
                    // 深度領域をクリア
                    rgbData[pixelIndex] = 0;
                    rgbData[pixelIndex + 1] = 0;
                    rgbData[pixelIndex + 2] = 0;
                }
            }
        }
        
        return {
            rgbData: new ImageData(rgbData, width, height),
            depthData: depthData
        };
    }

    /**
     * 解像度拡張方式のデコード
     */
    decodeExtended(encodedImageData) {
        const totalWidth = encodedImageData.width;
        const totalHeight = encodedImageData.height;
        const data = encodedImageData.data;
        
        // メタデータから元の寸法を取得
        const originalWidth = encodedImageData.originalWidth || Math.floor(totalWidth * 0.6); // フォールバック
        const originalHeight = encodedImageData.originalHeight || totalHeight;
        const depthWidth = encodedImageData.depthWidth || (totalWidth - originalWidth);
        const depthHeight = encodedImageData.depthHeight || totalHeight;
        
        // RGB画像データを復元
        const rgbData = new Uint8ClampedArray(originalWidth * originalHeight * 4);
        for (let y = 0; y < originalHeight; y++) {
            for (let x = 0; x < originalWidth; x++) {
                const srcIndex = (y * totalWidth + x) * 4;
                const dstIndex = (y * originalWidth + x) * 4;
                
                rgbData[dstIndex] = data[srcIndex];         // R
                rgbData[dstIndex + 1] = data[srcIndex + 1]; // G
                rgbData[dstIndex + 2] = data[srcIndex + 2]; // B
                rgbData[dstIndex + 3] = data[srcIndex + 3]; // A
            }
        }
        
        // 深度データを復元
        const depthData = new Uint16Array(originalWidth * originalHeight);
        let depthIndex = 0;
        
        for (let y = 0; y < depthHeight && y < totalHeight; y++) {
            for (let x = 0; x < depthWidth; x++) {
                const srcX = originalWidth + x;
                if (srcX < totalWidth && depthIndex < depthData.length) {
                    const pixelIndex = (y * totalWidth + srcX) * 4;
                    
                    // Alpha値が255の場合のみ深度データとして処理
                    if (data[pixelIndex + 3] === 255) {
                        const depthHigh = data[pixelIndex];
                        const depthLow = data[pixelIndex + 1];
                        depthData[depthIndex] = (depthHigh << 8) | depthLow;
                        depthIndex++;
                    }
                }
            }
        }
        
        // 実際に復元された深度データのサイズに合わせて配列をトリム
        const actualDepthData = new Uint16Array(depthIndex);
        for (let i = 0; i < depthIndex; i++) {
            actualDepthData[i] = depthData[i];
        }
        
        return {
            rgbData: new ImageData(rgbData, originalWidth, originalHeight),
            depthData: actualDepthData,
            depthWidth: Math.ceil(Math.sqrt(actualDepthData.length)),
            depthHeight: Math.ceil(actualDepthData.length / Math.ceil(Math.sqrt(actualDepthData.length)))
        };
    }
    /**
     * 解像度拡張方式での深度データ可視化
     * @param {Uint16Array} depthData - 深度データ
     * @param {number} depthWidth - 深度データの幅
     * @param {number} depthHeight - 深度データの高さ
     * @param {HTMLCanvasElement} canvas - 表示用Canvas
     */
    visualizeExtendedDepth(depthData, depthWidth, depthHeight, canvas) {
        canvas.width = depthWidth;
        canvas.height = depthHeight;
        const ctx = canvas.getContext('2d');
        const imageData = ctx.createImageData(depthWidth, depthHeight);
        
        for (let i = 0; i < depthData.length; i++) {
            const depth = depthData[i];
            const normalizedDepth = Math.min(depth * this.depthScale / this.maxDepth, 1.0);
            const grayValue = Math.floor((1.0 - normalizedDepth) * 255);
            
            const pixelIndex = i * 4;
            imageData.data[pixelIndex] = grayValue;     // R
            imageData.data[pixelIndex + 1] = grayValue; // G
            imageData.data[pixelIndex + 2] = grayValue; // B
            imageData.data[pixelIndex + 3] = 255;       // A
        }
        
        ctx.putImageData(imageData, 0, 0);
    }

    /**
     * エンコード済み画像の構造を可視化（デバッグ用）
     * @param {ImageData} encodedImageData - エンコード済み画像
     * @param {HTMLCanvasElement} canvas - 表示用Canvas
     */
    visualizeEncodedStructure(encodedImageData, canvas) {
        canvas.width = encodedImageData.width;
        canvas.height = encodedImageData.height;
        const ctx = canvas.getContext('2d');
        
        if (this.compressionMode === 'EXTENDED') {
            // RGB領域と深度領域の境界を可視化
            const originalWidth = encodedImageData.originalWidth || Math.floor(encodedImageData.width * 0.6);
            
            // 元の画像を描画
            ctx.putImageData(encodedImageData, 0, 0);
            
            // 境界線を描画
            ctx.strokeStyle = 'red';
            ctx.lineWidth = 2;
            ctx.beginPath();
            ctx.moveTo(originalWidth, 0);
            ctx.lineTo(originalWidth, encodedImageData.height);
            ctx.stroke();
            
            // ラベルを描画
            ctx.fillStyle = 'red';
            ctx.font = '16px Arial';
            ctx.fillText('RGB', 10, 30);
            ctx.fillText('DEPTH', originalWidth + 10, 30);
        } else {
            ctx.putImageData(encodedImageData, 0, 0);
        }
    }
}

/**
 * WebRTC統合用のヘルパークラス
 */
class RGBDWebRTCStreamer {
    constructor(codecOptions = {}) {
        this.codec = new RGBDCodec(codecOptions);
        this.stream = null;
        this.canvas = null;
        this.ctx = null;
    }

    /**
     * RealSenseからのデータをWebRTC Streamに変換
     */
    async createStreamFromRGBD(rgbImageData, depthData, width, height) {
        if (!this.canvas) {
            this.canvas = document.createElement('canvas');
            this.canvas.width = width;
            this.canvas.height = height;
            this.ctx = this.canvas.getContext('2d');
        }

        // エンコード
        const encodedImageData = this.codec.encode(rgbImageData, depthData, width, height);
        
        // Canvasに描画
        this.ctx.putImageData(encodedImageData, 0, 0);
        
        // MediaStreamを作成
        if (!this.stream) {
            this.stream = this.canvas.captureStream(30); // 30fps
        }
        
        return this.stream;
    }

    /**
     * 受信したVideoTrackからRGB-Dデータを復元
     */
    async decodeFromVideoTrack(videoElement) {
        if (!this.canvas) {
            this.canvas = document.createElement('canvas');
            this.ctx = this.canvas.getContext('2d');
        }

        this.canvas.width = videoElement.videoWidth;
        this.canvas.height = videoElement.videoHeight;
        
        // VideoからCanvasに描画
        this.ctx.drawImage(videoElement, 0, 0);
        
        // ImageDataを取得
        const imageData = this.ctx.getImageData(0, 0, this.canvas.width, this.canvas.height);
        
        // デコード
        return this.codec.decode(imageData);
    }
}

// 使用例
/*
// 1. LSB方式（視覚品質重視、解像度変更なし）
const codecLSB = new RGBDCodec({ compressionMode: 'LSB', maxDepth: 5.0 });

// 2. チャンネル方式（深度精度重視、1/4解像度）
const codecChannel = new RGBDCodec({ compressionMode: 'CHANNEL', maxDepth: 5.0 });

// 3. 解像度拡張方式（最高品質、解像度拡張）
const codecExtended = new RGBDCodec({ compressionMode: 'EXTENDED', maxDepth: 5.0 });

// 送信側
const streamer = new RGBDWebRTCStreamer({ compressionMode: 'EXTENDED', maxDepth: 5.0 });

// RealSenseからのデータ（仮想的な例）
const rgbImageData = getRGBFromRealSense(); // 640x480
const depthData = getDepthFromRealSense();  // 307200 pixels (640x480)

// 解像度拡張方式でエンコード（例：640x480 → 880x480）
const encodedImage = codecExtended.encode(rgbImageData, depthData, 640, 480);
console.log(`Original: 640x480, Encoded: ${encodedImage.width}x${encodedImage.height}`);

const stream = await streamer.createStreamFromRGBD(rgbImageData, depthData, 640, 480);
// このstreamをWebRTC PeerConnectionに追加

// 受信側
const receivedVideo = document.getElementById('receivedVideo');
const decodedData = await streamer.decodeFromVideoTrack(receivedVideo);
console.log('復元されたRGBデータ:', decodedData.rgbData.width, 'x', decodedData.rgbData.height);
console.log('復元された深度データ:', decodedData.depthData.length, 'pixels');

// デバッグ用可視化
const debugCanvas = document.getElementById('debugCanvas');
codecExtended.visualizeEncodedStructure(encodedImage, debugCanvas);

const depthCanvas = document.getElementById('depthCanvas');
codecExtended.visualizeExtendedDepth(
    decodedData.depthData, 
    decodedData.depthWidth, 
    decodedData.depthHeight, 
    depthCanvas
);
*/