Go to Qiita Advent Calendar 2024 Top
LoginSignup
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

@Tomonobu3110(とものぶ)

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

Last updated at Posted at 2024-05-13

作ったもの

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

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

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

image.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軸を連続的に変化させて実現しています

<!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空間でグリグリする

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

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

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

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

EOF

0
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?