3D空間シミュレータ (ワールド座標 ～ カメラ座標 ～ スクリーン座標変換)

# 作ったもの

JavaScript + Three.js で3D空間シミュレータ(?)を作成しました
Three.js を利用して
ワールド座標系 → カメラ座標系 → スクリーン座標系 の変換をしています

オブジェクトはごく簡単な電柱(赤)と地面(黒)
電柱には、スクリーン座標系の bounding box が描画されます

ドローンがジンバルとカメラを積んで地上を撮影しているイメージです

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/102797/68ade88d-0ab3-e432-d41d-375d6458f4e5.png)

画面下部の「デモ」「デモ2」「デモ3」を押すと簡単なアニメーションをします

# コード

テキストエディタなどを開きコピペしてHTMLで保存、ブラウザで開けば動作します

## (追記) 少しだけ解説

`worldCoords_pole`
`worldCoords_ground`
が、それぞれ電柱、地面の点群データです。
`[x, y, z]`の3D座標のリストです。

`drawPoint()`関数を呼び出すと
上記２つの点群にふくまれる3D座標データをひとつづつ取り出して
ワールド系からカメラ系の座標への変換 (`worldToCameraCoords`)
カメラ座標系からスクリーン座標系への変換 (`perspectiveProjection`)
を行います

`prespectiveProjection`の中ではZ座標が0または負の点、
すなわちカメラの背後にある点は描画対象から外しています

そしてキャンバスに指定色で描画します

最後に `bbox` が 1 ならば、
一番左、一番右、一番上、一番下の点の座標を見つけて長方形 (bounding box) を描画します

アニメーションは `animationZ` 関数で
Z軸を連続的に変化させて実現しています

```JavaScript
<!DOCTYPE html>
<html lang="en">
<head>
    <meta charset="UTF-8">
    <meta name="viewport" content="width=device-width, initial-scale=1.0">
    <title>Draw Points on Canvas</title>
    <script src="https://cdnjs.cloudflare.com/ajax/libs/three.js/110/three.min.js"></script>
    <style>
      #root_panel {
        display: flex; /* Flexboxレイアウトを使用 */
        justify-content: space-between; /* 要素間の間隔を均等に配置 */
      }
      #canvas_panel,
      #operation_panel {
        flex: 1; /* 幅を均等に分割 */
        padding: 20px; /* 内側の余白を追加（任意） */
        box-sizing: border-box; /* paddingを要素のサイズに含める */
      }
      #operation_panel > div {
        margin-bottom: 20px; /* 各セクション（div要素）の下に20pxのマージンを追加 */
      }
    </style>
</head>
<body>
    <div id="root_panel">
      <div id="canvas_panel">
        <canvas id="myCanvas" width="900" height="600" style="border: 1px solid black;"></canvas>
      </div> <!-- canvas_panel -->

      <div id="operation_panel">
        <div id="title">
          Control Panel
        </div>
        <div id="orientation">
          <button onclick="changeOrientation('pitch', 5)">Increase Pitch (+5)</button>
          <button onclick="changeOrientation('pitch', -5)">Decrease Pitch (-5)</button>
          <button onclick="setOrientation('pitch', 0)">Pitch 0</button>
          <br>
          <button onclick="changeOrientation('roll', 5)">Increase Roll (+5)</button>
          <button onclick="changeOrientation('roll', -5)">Decrease Roll (-5)</button>
          <button onclick="setOrientation('roll', 0)">Roll 0</button>
          <br>
          <button onclick="changeOrientation('yaw', 5)">Increase Yaw (+5)</button>
          <button onclick="changeOrientation('yaw', -5)">Decrease Yaw (-5)</button>
          <button onclick="setOrientation('yaw', 0)">Yaw 0</button>
          <br>
          <button onclick="resetOrientation()">Reset Orientation</button>
          <br>
          <div>
              <label>Yaw:</label>
              <span id="yawValue">0</span>
              <br>
              <label>Roll:</label>
              <span id="rollValue">0</span>
              <br>
              <label>Pitch:</label>
              <span id="pitchValue">0</span>
          </div>
        </div> <!-- orientation -->

        <div id="position">
          <button onclick="changePosition('x', 1)">Increase X (+1)</button>
          <button onclick="changePosition('x', -1)">Decrease X (-1)</button>
          <button onclick="setPosition('x', 0)">X 0</button>
          <br>
          <button onclick="changePosition('y', 1)">Increase Y (+1)</button>
          <button onclick="changePosition('y', -1)">Decrease Y (-1)</button>
          <button onclick="setPosition('y', 0)">Y 0</button>
          <br>
          <button onclick="changePosition('z', 1)">Increase Z (+1)</button>
          <button onclick="changePosition('z', -1)">Decrease Z (-1)</button>
          <button onclick="setPosition('z', 0)">Z 0</button>
          <br>
          <button onclick="resetPosition()">Reset Position</button>
          <br>
          <div>
              <label>X:</label>
              <span id="xValue">0</span>
              <br>
              <label>Y:</label>
              <span id="yValue">0</span>
              <br>
              <label>Z:</label>
              <span id="zValue">0</span>
          </div>
        </div> <!-- position -->
        
        <div id="focal_length">
          <button onclick="changeFocalLength(10)">Increase Focal Length (+10)</button>
          <button onclick="changeFocalLength(-10)">Decrease Focal Length (-10)</button>
          <br>
          <button onclick="changeFocalLength(100)">Increase Focal Length (+100)</button>
          <button onclick="changeFocalLength(-100)">Decrease Focal Length (-100)</button>
          <br>
          <button onclick="resetFocalLength()">Reset Focal Length</button>
          <br>
          <div>
              <label>Focal Length:</label>
              <span id="focalLengthValue">200</span>
              <br>
          </div>
        </div> <!-- focal_length -->

        <div id="outputs">
            <label for="bbox">電柱bboxの表示:</label>
            <input type="checkbox" id="bbox" name="bbox" onchange="drawPoints()" checked>
            <br>
            <label>bbox:</label>
            <span id="bbox_output">(0,0) / (900,600)</span>
        </div>

        <div id="animation">
          <button onclick="animationZ()">zの値を変化させる</button>
          <button onclick="demo()">デモ</button>
          <button onclick="demo2()">デモ2</button>
          <button onclick="demo3()">デモ3</button>
        </div> <!-- animation -->
      </div> <!-- operation_panel -->
    </diiv> <!-- root_panel -->

    <script>
        // pole (= points) on the world.
        const worldCoords_pole = [
            // pole
            [0,   0, 10],
            [0,  -1, 10],
            [0,  -2, 10],
            [0,  -3, 10],
            [0,  -4, 10],
            [0,  -5, 10],
            [0,  -6, 10],
            [0,  -7, 10],
            [0,  -8, 10],
            [0,  -9, 10],
            [0, -10, 10],

            // lower arm
            [ 1,  -8, 10],
            [ 2,  -8, 10],
            [ 3,  -8, 10],
            [-1,  -8, 10],
            [-2,  -8, 10],
            [-3,  -8, 10],

            // upper arm
            [ 1, -10, 10],
            [ 2, -10, 10],
            [ 3, -10, 10],
            [-1, -10, 10],
            [-2, -10, 10],
            [-3, -10, 10],
        ];

        // ground (= points) on the world.
        const worldCoords_ground = [
            [ 0,  0,  5],
            [ 0,  0, 10],
            [ 0,  0, 15],

            [ 2.5,  0,  5],
            [ 2.5,  0, 15],

            [ 5,  0,  5],
            [ 5,  0,  7.5],
            [ 5,  0, 10],
            [ 5,  0, 12.5],
            [ 5,  0, 15],

            [-2.5,  0,  5],
            [-2.5,  0, 15],

            [-5,  0,  5],
            [-5,  0,  7.5],
            [-5,  0, 10],
            [-5,  0, 12.5],
            [-5,  0, 15],
        ];
        
        const worldCoords = [
            { coords: worldCoords_ground, color: "black", bbox: 0 },
            { coords: worldCoords_pole,   color: "red",   bbox: 1 },
        ];

        // Camera
        let cameraPosition    = { x: 0, y: -15, z: 0 };         // Camera position (x, y, z)
        let cameraOrientation = { pitch: 45, roll: 0, yaw: 0 }; // Initial camera orientation (in degrees)
        let focalLength       = 200;

        // LR1
        const sensor_w = 35.7; // mm
        const sensor_h = 23.8; // mm

        function drawPoints() {
            const canvas  = document.getElementById('myCanvas');
            canvas.width  = 900;
            canvas.height = 600;
            const ctx = canvas.getContext('2d');

            // Clear canvas with white background
            ctx.fillStyle = 'white';
            ctx.fillRect(0, 0, canvas.width, canvas.height);

            const imageWidth  = canvas.width;
            const imageHeight = canvas.height;
            
            worldCoords.forEach(coords => {
                // Convert world coordinates to camera coordinates
                const cameraCoords = worldToCameraCoords(coords.coords, cameraPosition, cameraOrientation);

                // Perform perspective projection to screen coordinates
                const screenCoords = perspectiveProjection(cameraCoords, focalLength, imageWidth, imageHeight);

                // Draw points on canvas
                const radius = 3;
                screenCoords.forEach(coord => {
                    const [x, y] = coord;

                    // Draw red circle around the point
                    ctx.beginPath();
                    ctx.arc(x, y, radius, 0, 2 * Math.PI);
                    ctx.fillStyle = coords.color;
                    ctx.fill();
                    ctx.closePath();
                });

                // BBox
                const checkbox = document.getElementById('bbox');
                const bbox_out = document.getElementById('bbox_output');
                if (checkbox.checked && 1 == coords.bbox) {
                    let minX = imageWidth;
                    let maxX = 0;
                    let minY = imageHeight;
                    let maxY = 0;
                    screenCoords.forEach(coord => {
                        const [x, y] = coord;
                        if (x < minX) { minX = x; }
                        if (maxX < x) { maxX = x; }
                        if (y < minY) { minY = y; }
                        if (maxY < y) { maxY = y; }
                    });

                    // 正規化
                    if (minX < 0) { minX = 0; }
                    if (minY < 0) { minY = 0; }
                    if (imageWidth  < maxX) { maxX = imageWidth;  }
                    if (imageHeight < maxY) { maxY = imageHeight; }

                    if (minX < maxX && minY < maxY) {
                        // 長方形の描画
                        ctx.beginPath();
                        ctx.rect(minX, minY, maxX - minX, maxY - minY);
                        ctx.strokeStyle = coords.color;
                        ctx.stroke();
                        ctx.closePath();
                        
                        // bbox情報
                        minX = Math.round(minX);
                        maxX = Math.round(maxX);
                        minY = Math.round(minY);
                        maxY = Math.round(maxY);
                        bbox_out.textContent = "(" + minX + "," + minY + ")-(" + maxX + "," + maxY + ") / (" + imageWidth + "," + imageHeight + ")";
                    } else {
                        bbox_out.textContent = "(not found)";
                    }
                }
                else if (!checkbox.checked) {
                    bbox_out.textContent = "(disabled)";
                }
            });

            // Update displayed orientation values
            document.getElementById('yawValue').textContent = cameraOrientation.yaw;
            document.getElementById('rollValue').textContent = cameraOrientation.roll;
            document.getElementById('pitchValue').textContent = cameraOrientation.pitch;

            // Update displayed position values
            document.getElementById('xValue').textContent = cameraPosition.x;
            document.getElementById('yValue').textContent = cameraPosition.y;
            document.getElementById('zValue').textContent = cameraPosition.z;

            // Update displayed focal length values
            const focalLength35 = Math.round(sensor_w * focalLength / imageWidth);
            document.getElementById('focalLengthValue').textContent = focalLength + " (" + focalLength35 + "mm / 35mm換算)";
        }

        function worldToCameraCoords(worldCoords, cameraPosition, cameraOrientation) {
            const { x: cx, y: cy, z: cz } = cameraPosition;
            const [ pitch, roll, yaw ] = Object.values(cameraOrientation).map(angle => angle * (Math.PI / 180)); // degrees to radians

            // Calculate rotation matrix
            const rotationMatrix = getRotationMatrix(yaw, pitch, roll);

            // Convert world coordinates to camera coordinates
            const translatedCoords = worldCoords.map(coord => new THREE.Vector3(...coord).sub(new THREE.Vector3(cx, cy, cz)));
            const cameraCoords     = translatedCoords.map(coord => coord.applyMatrix4(rotationMatrix));

            return cameraCoords;
        }

        function getRotationMatrix(yaw, pitch, roll) {
            const yawMatrix   = new THREE.Matrix4().makeRotationY(yaw);
            const pitchMatrix = new THREE.Matrix4().makeRotationX(pitch);
            const rollMatrix  = new THREE.Matrix4().makeRotationZ(roll);

            // yaw -> pitch -> roll の順で回転行列を合成
            const rotationMatrix = new THREE.Matrix4().multiplyMatrices(yawMatrix, pitchMatrix).multiply(rollMatrix);

            return rotationMatrix;
        }

        function perspectiveProjection(cameraCoords, focalLength, imageWidth, imageHeight) {
            const centerX = imageWidth  / 2;
            const centerY = imageHeight / 2;

            const screenCoords = [];

            cameraCoords.forEach(coord => {
                const x = coord.x;
                const y = coord.y;
                const z = coord.z;

                let screenX, screenY;
                if (0 < z) {
                    // 透視投影の計算
                    screenX = (focalLength * x) / z + centerX;
                    screenY = (focalLength * y) / z + centerY;
                    screenCoords.push([screenX, screenY]);
                }
/*
                if (z === 0) {
                    // カメラ座標系で点がカメラ位置と一致する場合、無限遠に投影
                    screenX = centerX;
                    screenY = centerY;
                } else {
                    // 透視投影の計算
                    screenX = (focalLength * x) / z + centerX;
                    screenY = (focalLength * y) / z + centerY;
                }

                screenCoords.push([screenX, screenY]);
*/
            });

            return screenCoords;
        }

        function changeOrientation(type, value) {
            cameraOrientation[type] += value;
            drawPoints();  // Redraw canvas with updated camera orientation
        }

        function setOrientation(type, value) {
            cameraOrientation[type] = value;
            drawPoints();  // Redraw canvas with updated camera orientation
        }

        function resetOrientation() {
            cameraOrientation = { pitch: 45, roll: 0, yaw: 0 };
            drawPoints();  // Redraw canvas with reset camera orientation
        }

        function changePosition(type, value) {
            cameraPosition[type] += value;
            drawPoints();  // Redraw canvas with updated camera orientation
        }

        function setPosition(type, value) {
            cameraPosition[type] = value;
            drawPoints();  // Redraw canvas with updated camera orientation
        }

        function resetPosition() {
            cameraPosition = { x: 0, y: -15, z: 0 };
            drawPoints();  // Redraw canvas with reset camera orientation
        }

        function changeFocalLength(value) {
            focalLength += value;
            drawPoints();  // Redraw canvas with updated camera orientation
        }
        
        function setFocalLength(value) {
            focalLength = value;
            drawPoints();  // Redraw canvas with updated camera orientation
        }

        function resetFocalLength() {
            focalLength = 200;
            drawPoints();  // Redraw canvas with reset camera orientation
        }

        function animationZ(startZ = -100, endZ = 15) {
            let z = startZ;
            const intervalId = setInterval(() => {
                var zValueElement = document.getElementById('zValue');
                zValueElement.style.color = 'red'; // テキストの色を赤に設定
                zValueElement.style.fontWeight = 'bold'; // フォントの太さを太字に設定
                z += 1;
                cameraPosition.z = z;
                drawPoints();
                if (endZ < z) {
                    clearInterval(intervalId);
                    zValueElement.style.color = 'black';
                    zValueElement.style.fontWeight = '';
                    alert("Z animation finished.");
                }
            }, 100);
        }

        function demo() {
            resetOrientation();
            resetPosition();
            setPosition('x', 5);
            setFocalLength(400);
            animationZ();
        }

        function demo2() {
            resetOrientation();
            resetPosition();
            setPosition('x', 2);
            setPosition('y', -25);
            setFocalLength(1260);
            animationZ(-40, 5);
        }

        function demo3() {
            resetOrientation();
            resetPosition();
            setOrientation('pitch', 90);
            setPosition('x', 0);
            setPosition('y', -50);
            setFocalLength(1260);
            animationZ(-15, 30);
        }

        // Initial draw
        drawPoints();
    </script>
</body>
</html>

```

# 参考にさせていただいたWebサイト

Qiita
OpenCVとピンホールカメラモデルを用いて、3D空間でグリグリする

https://qiita.com/iwatake2222/items/c3fe0139d37c4e01c07b

※ C++による実装が書いてある
※ 論理も結構ちゃんと書いてある

Qiita
カメラパラメータ・座標変換

https://qiita.com/S-Kaito/items/ace10e742227fd63bd4c

座標変換について調べてみた

https://dev.classmethod.jp/articles/convert-coordinates/

WebGL開発に役立つ重要な三角関数の数式・概念まとめ（Three.js編）

https://ics.media/entry/10657/

EOF