3D座標変換の基礎まとめ

はじめに

座標変換関連の処理を業務で触ることになったのですが、触れるのが大学受験勉強以来だったので、基礎的な要素も含めて自分の復習用まとめます。

この記事はあくまでも個人的なメモとして作成されています。そのため、使用したライブラリやツールのバージョンについては特に記載しておりません。コードの動作環境に依存する可能性があるため、ご利用の際はご自身の環境に合わせたバージョンで適切にインストールしてお使いください。

以下の基本的な処理をpythonとcppの両方でまとめます。

• オイラー角 -> 回転行列
• オイラー角 -> transform matrix
• transform matrix -> delta pose
• 極座標 -> 直交座標
• 直交座標 -> PointCloud
• PointCloud -> Depth Map

この記事の対象者

3D空間における座標変換の基礎や、それをPythonとcppで実装する方法を知りたい人向けです。

各処理について

オイラー角 -> 回転行列

python(Rotationを使用する場合)

import numpy as np
from scipy.spatial.transform import Rotation as R

def euler_to_rotation_matrix(roll, pitch, yaw):
    r = R.from_euler('xyz', [roll, pitch, yaw], degrees=False)
    rotation_matrix = r.as_matrix()  # 3x3の回転行列を取得

    return rotation_matrix

roll = np.radians(30)   # X軸周りの回転角（ロール）
pitch = np.radians(45)  # Y軸周りの回転角（ピッチ）
yaw = np.radians(60)    # Z軸周りの回転角（ヨー）

transform_matrix = euler_to_rotation_matrix(roll, pitch, yaw)
print(transform_matrix)

Python(Rotationを使用しない場合)

import numpy as np

def euler_to_rotation_matrix(roll, pitch, yaw):
    # Roll (X軸周りの回転)
    R_x = np.array([
        [1, 0, 0],
        [0, np.cos(roll), -np.sin(roll)],
        [0, np.sin(roll), np.cos(roll)]
    ])

    # Pitch (Y軸周りの回転)
    R_y = np.array([
        [np.cos(pitch), 0, np.sin(pitch)],
        [0, 1, 0],
        [-np.sin(pitch), 0, np.cos(pitch)]
    ])

    # Yaw (Z軸周りの回転)
    R_z = np.array([
        [np.cos(yaw), -np.sin(yaw), 0],
        [np.sin(yaw), np.cos(yaw), 0],
        [0, 0, 1]
    ])

    # 回転行列を合成
    R = np.dot(R_z, np.dot(R_y, R_x))

    return R

# オイラー角を指定（ラジアン）

roll = np.radians(30)   # X軸周りの回転角（ロール）
pitch = np.radians(45)  # Y軸周りの回転角（ピッチ）
yaw = np.radians(60)    # Z軸周りの回転角（ヨー）

# 回転行列を計算

rotation_matrix = euler_to_rotation_matrix(roll, pitch, yaw)
print("Rotation Matrix:")
print(rotation_matrix)

cpp(標準ライブラリ)

#include <cmath>
#include <array>


double DegreesToRadians(double degrees) {
    return degrees * M_PI / 180.0;
}

std::array<std::array<double, 3>, 3>euler_to_rotation_matrix(double roll, double pitch, double yaw) {
    // Roll (X軸周りの回転)
    std::array<std::array<double, 3>, 3> R_x = {{
        {1, 0, 0},
        {0, std::cos(roll), -std::sin(roll)},
        {0, std::sin(roll), std::cos(roll)}
    }};

    // Pitch (Y軸周りの回転)
    std::array<std::array<double, 3>, 3> R_y = {{
        {std::cos(pitch), 0, std::sin(pitch)},
        {0, 1, 0},
        {-std::sin(pitch), 0, std::cos(pitch)}
    }};

    // Yaw (Z軸周りの回転)
    std::array<std::array<double, 3>, 3> R_z = {{
        {std::cos(yaw), -std::sin(yaw), 0},
        {std::sin(yaw), std::cos(yaw), 0},
        {0, 0, 1}
    }};

    std::array<std::array<double, 3>, 3> R_zy;
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            R_zy[i][j] = R_z[i][0] * R_y[0][j] + R_z[i][1] * R_y[1][j] + R_z[i][2] * R_y[2][j];
        }
    }

    std::array<std::array<double, 3>, 3> R_final;
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            R_final[i][j] = R_zy[i][0] * R_x[0][j] + R_zy[i][1] * R_x[1][j] + R_zy[i][2] * R_x[2][j];
        }
    }

    return R_final;
}

int main() {
    double roll = DegreesToRadians(30);
    double pitch = DegreesToRadians(45);
    double yaw = DegreesToRadians(60);

    std::array<std::array<double, 3>, 3> rotation_matrix = euler_to_rotation_matrix(roll, pitch, yaw);

    return 0;
}

cpp(Eigen)

#include <Eigen/Dense>
#include <cmath>


double DegreesToRadians(double degrees) {
    return degrees * M_PI / 180.0;
}

Eigen::Matrix3d euler_to_rotation_matrix(double roll, double pitch, double yaw) {
    Eigen::Matrix3d rotation_matrix;
    // AngleAxisdは掛け算すると、内部的に回転行列に変換されてから掛け算される
    rotation_matrix = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()) *
                      Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) *
                      Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())

    // Eigen::Matrix3d R_x;
    // R_x << 1, 0, 0,
    //         0, std::cos(roll), -std::sin(roll),
    //         0, std::sin(roll), std::cos(roll);

    // Eigen::Matrix3d R_y;
    // R_y << std::cos(pitch), 0, std::sin(pitch),
    //        0, 1, 0,
    //        -std::sin(pitch), 0, std::cos(pitch);

    // Eigen::Matrix3d R_z;
    // R_z << std::cos(yaw), -std::sin(yaw), 0,
    //        std::sin(yaw), std::cos(yaw), 0,
    //        0, 0, 1;

    // Eigen::Matrix3d R = R_z * R_y * R_x;

    return rotation_matrix
}

int main() {
    double roll = DegreesToRadians(30);
    double pitch = DegreesToRadians(45);
    double yaw = DegreesToRadians(60);

    Eigen::Matrix3d rotation_matrix = euler_to_rotation_matrix(roll, pitch, yaw);

    return 0;
}

オイラー角 -> transform matrix

tranform matrix の有用性

メモ

4x4行列がなぜ便利なのか？

結論

4x4の変換行列を使うと、回転と平行移動を一度に計算できる。だから座標変換が簡単になる。

1. 状況の設定

• 3次元空間の点 $P = (1, 2, 3)$ を考える。
• この点に次の変換をする：
  • 回転: Z軸を中心に90度回転。
  • 平行移動: x方向に2、y方向に3、z方向に5だけ動かす。

2. 回転行列と平行移動ベクトルの定義

Z軸回転90度の回転行列

Z軸を90度回転する回転行列はこうなる：

$$R_z(90^\circ) = \begin{bmatrix} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 1 \end{bmatrix}$$

平行移動ベクトル

$x = 2, y = 3, z = 5$ の平行移動を行う4x4の変換行列はこうなる：

$$T = \begin{bmatrix} 0 & -1 & 0 & 2 \\ 1 & 0 & 0 & 3 \\ 0 & 0 & 1 & 5 \\ 0 & 0 & 0 & 1 \end{bmatrix}$$

左上の3x3が回転行列で、右側の列が平行移動ベクトル。

3. 座標点を同次座標に変換

点 $P = (1, 2, 3)$ を同次座標に変換する。同次座標では、4次元にするために1を追加する：

$$P_{\text{homogeneous}} = \begin{bmatrix} 1 \\ 2 \\ 3 \\ 1 \end{bmatrix}$$

4. 変換行列と座標点をかける

変換行列 $T$ と $P_{\text{homogeneous}}$ をかける：

$$P' = T \cdot P_{\text{homogeneous}} = \begin{bmatrix} 0 & -1 & 0 & 2 \\ 1 & 0 & 0 & 3 \\ 0 & 0 & 1 & 5 \\ 0 & 0 & 0 & 1 \end{bmatrix} \cdot \begin{bmatrix} 1 \\ 2 \\ 3 \\ 1 \end{bmatrix}$$

計算の流れ

1行目：

$$0 \cdot 1 + (-1) \cdot 2 + 0 \cdot 3 + 2 \cdot 1 = -2 + 2 = 0$$

2行目：

$$1 \cdot 1 + 0 \cdot 2 + 0 \cdot 3 + 3 \cdot 1 = 1 + 3 = 4$$

3行目：

$$0 \cdot 1 + 0 \cdot 2 + 1 \cdot 3 + 5 \cdot 1 = 3 + 5 = 8$$

4行目は常に 1 なので、そのまま。

5. 結果の座標

計算結果は：

$$P' = \begin{bmatrix} 0 \\ 4 \\ 8 \\ 1 \end{bmatrix}$$

つまり、元の点 $P = (1, 2, 3)$ に Z軸90度回転と平行移動をした結果、新しい座標 $P' = (0, 4, 8)$ になる。

python

import numpy as np
from scipy.spatial.transform import Rotation as R

def euler_to_transform_matrix(roll, pitch, yaw, translation):
    # オイラー角を使って回転行列を作成
    r = R.from_euler('xyz', [roll, pitch, yaw], degrees=False)  # xyzの順序でオイラー角を指定
    rotation_matrix = r.as_matrix()  # 3x3の回転行列を取得

    # 4x4の変換行列を初期化
    transform_matrix = np.eye(4)

    # 回転行列を変換行列の一部にセット
    transform_matrix[:3, :3] = rotation_matrix

    # 並進ベクトルをセット (x, y, z)
    transform_matrix[:3, 3] = translation

    return transform_matrix

# 例: オイラー角と並進ベクトルを指定
roll = np.radians(30)   # X軸周りの回転角（ロール）
pitch = np.radians(45)  # Y軸周りの回転角（ピッチ）
yaw = np.radians(60)    # Z軸周りの回転角（ヨー）
translation = [1, 2, 3]  # 並進ベクトル (x, y, z)

# 変換行列を計算
transform_matrix = euler_to_transform_matrix(roll, pitch, yaw, translation)
print(transform_matrix)

cpp

#include <Eigen/Dense>
#include <cmath>


double DegreesToRadians(double degrees) {
    return degrees * M_PI / 180.0;
}

Eigen::Matrix3d euler_to_rotation_matrix(double roll, double pitch, double yaw) {
    Eigen::Matrix3d rotation_matrix;
    rotation_matrix = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()) *
                      Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) *
                      Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())

    Eigen::Matrix4d transform_matrix = Eigen::Matrix4d::Identity();

    transform_matrix.block<3, 3>(0, 0) = rotation_matrix;

    transform_matrix.block<3, 1>(0, 3) = translation;

    return rotation_matrix
}

int main() {
    double roll = DegreesToRadians(30);
    double pitch = DegreesToRadians(45);
    double yaw = DegreesToRadians(60);

    Eigen::Matrix3d rotation_matrix = euler_to_rotation_matrix(roll, pitch, yaw);

    return 0;
}

transform matrix -> delta pose

python

import numpy as np

def compute_delta_pose(T1, T2):
    T1_inv = np.linalg.inv(T1)
    delta_pose = np.dot(T1_inv, T2)

    return delta_pose

T1 = np.array([
    [np.cos(np.pi / 4), -np.sin(np.pi / 4), 0, 1],
    [np.sin(np.pi / 4), np.cos(np.pi / 4), 0, 0],
    [0, 0, 1, 0],
    [0, 0, 0, 1],
])

T2 = np.array([
    [np.cos(np.pi / 2), -np.sin(np.pi / 2), 0, 2],
    [np.sin(np.pi / 2), np.cos(np.pi / 2), 0, 0],
    [0, 0, 1, 0],
    [0, 0, 0, 1],
])

delta_pose = compute_delta_pose(T1, T2)

print(f"Delta Pose: {delta_pose}")

cpp

#include <iostream>
#include <Eigen/Dense>

Eigen::Matrix4d compute_delta_pose(const Eigen::Matrix4d& T1, const Eigen::Matrix4d& T2) {
    Eigen::Matrix4d T1_inv = T1.inverse();

    Eigen::Matrix4d delta_pose = T1_inv * T2;

    return delta_pose;

int main() {
    Eigen::Matrix4d T1 = Eigen::Matrix4d::Identity();
    T1(0, 3) = 1.0;
    T1.block<3, 3>(0, 0) = Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ()).toRotationMatrix();

    Eigen::Matrix4d T2 = Eigen::Matrix4d::Identity();
    T2(0, 3) = 2.0;
    T2.block<3, 3>(0, 0) = Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ()).toRotationMatrix();

    Eigen::Matrix4d delta_pose = compute_delta_pose(T1, T2);

    std::cout << "Delta Pose:\n" << delta_pose << std::endl;

    Eigen::Matrix3d delta_rotation = delta_pose.block<3, 3>(0, 0);
    std::cout << "Delta Rotation (relative rotation matrix):\n" << delta_rotation << std::endl;

    Eigen::Vector3d delta_translation = delta_pose.block<3, 1>(0, 3);
    std::cout << "Delta Translation (relative translation vector):\n" << delta_translation.transpose() << std::endl;

    return 0;
}

}

デルタポーズとは

メモ

1. フレームとポーズの関係

「ポーズ」とは、3D空間での位置と向きを指す。これは「変換行列」で表されていて、カメラやロボットの位置と向きを記述するときに使う。

• pose1: 基準座標系（ワールド座標系など）から見たフレーム1の位置と向き。
• pose2: 同じ基準座標系から見たフレーム2の位置と向き。

2. デルタポーズ（相対的な位置）とは？

デルタポーズっていうのは、フレーム1から見たフレーム2の位置と向きを表すもの。フレーム1を基準にして、「フレーム2はどこにいる？」っていうのを求める感じ。

なぜ逆行列をかけるとデルタポーズが求められるのか

メモ

逆行列でデルタポーズが求められる理由

逆行列を使うのは、フレーム1の座標系からフレーム2を観測するため。

逆行列の役割

フレーム1の位置と向きは pose1 で表せます。しかし、フレーム1の視点からフレーム2を見たい場合は、フレーム1を基準にした変換が必要です。ここで使うのが逆行列。

逆行列の使い方

• 順方向の変換: pose1 を使えば、基準座標系からフレーム1への変換ができる。
• 逆方向の変換: フレーム1の視点でフレーム2の位置を求めるには、pose1 の逆行列を使ってフレーム1を基準にする。

数式

フレーム1からフレーム2への相対位置（デルタポーズ）は、次の式で計算する。

$$\text{delta\_pose} = \text{pose1}^{-1} \cdot \text{pose2}$$

逆行列を使うことで、フレーム1を基準にフレーム2がどこにあるかがわかる。

極座標 -> 直交座標

python

import numpy as np

def degrees_to_radians(degrees):
    return np.radians(degrees)

def spherical_to_cartesian(range_data, azimuth_deg, elevation_deg):
    azimuth_rad = degrees_to_radians(azimuth_deg)
    elevation_rad = degrees_to_radians(elevation_deg)

    x = range_data * np.cos(elevation_rad) * np.cos(azimuth_rad)
    y = range_data * np.cos(elevation_rad) * np.sin(azimuth_rad)
    z = range_data * np.sin(elevation_rad)

    return x, y, z

x, y, z = spherical_to_cartesian(10, 30, 60)

cpp

#include <iostream>
#include <cmath>

double degreesToRadians(double degress) {
    return degrees * M_PI / 180.0;
}

void sphericalToCartesian(double range, double azimuth_deg, double elevation_deg, double &x, double &y, double &z) {
    double azimuth_rad = degreesToRadians(azimuth_deg);
    double elevation_rad = degreesToRadians(elevation_deg);

    x = range * cos(elevation_rad) * cos(azimuth_rad);
    y = range * cos(elevation_rad) * sin(azimuth_rad);
    z = range * sin(elevation_rad);
}

int main() {
    double range = 10.0;
    double azimuth_deg = 30.0;
    double elevation_deg = 15.0;

    double x, y, z;

    sphericalToCartesian(range, azimuth_deg, elevation_deg, x, y, z);

    std::cout << "3D座標: X=" << x << ", Y=" << y << ", Z=" << z << std::endl;

    return 0;
}

直交座標 -> PointCloud

python

import open3d as o3d
import numpy as np

x = [1, 1, 1, 1]
y = [2, 3, 4, 5]
z = [1, 1, 1, 1]
intensities = [10, 20, 30, 40] # 光の反射強度

points = [
    [x, y, z] for x, y, z in zip(z, y, z)
]

# 反射強度をOpen3Dで扱えるようにするため、強度をカラーとして設定
# 光の反射強度を0-1の範囲に正規化し、グレースケールカラーとして設定
pcd = o3d.geometry.PointCloud()
pcd = o3d.utility.Vector3dVector(points)

normalizer = 255.0 / np.max(intensities)
colors = np.tile(intensities.reshape(-1, 1) * normalizer, (1, 3))
pcd.colors = o3d.utility.Vector3dVector(colors)

o3d.io.write_point_cloud("point_cloud_with_intensity_open3d.ply", pcd)

o3d.visualization.draw_geometries([pcd])

cpp

#include <open3d/Open3D.h>
#include <iostream>
#include <vector>
#include <cmath>

int main() {
    std::vector<double> x = {1, 1, 1, 1};
    std::vector<double> y = {2, 3, 4, 5};
    std::vector<double> z = {1, 1, 1, 1};
    std::vector<double> intensity_list = {100, 80, 90, 60}

    std::vector<Eigen::Vector3d> points;
    std::vector<Eigen::Vector3d> colors;

    double max_intensity = *std::max_element(intensity_list.begin(), intensity_list.end());

    for (size_t i = 0; i < x.size(); ++i) {
        points.emplace_back(x[i], y[i], z[i])

        double normalized_intensity = intensity_list[i] / max_intensity;
        colors.emplace_back(normalized_intensity, normalized_intensity, normalized_intensity)
    }

    auto pcd = std::make_shared<open3d::geometry::PointCloud>();

    pcd->points_ = points;
    pcd->colors_ = colors;

    open3d::io::WritePointCloud("point_cloud_with_intensity_open3d.ply", *pcd);
    open3d::visualization::DrawGeometries({pcd});

    return 0;
}

PointCloud -> Depth Map

カメラ行列について

メモ

カメラ行列の説明

1. 三角形の相似による説明

カメラで撮影した画像は、物体がカメラから遠くにあるほど小さく、近いほど大きく映る。この現象は、三角形の相似を使って説明できる。

相似の関係

• 物体の点 $P = (X, Y, Z)$ は、カメラのレンズからの距離 $Z$ にある。
• この点をカメラの画像平面に投影すると、対応する画像上の点 $p = (u, v)$ が得られる。

カメラの焦点距離 $f$ を持つ画像平面上において、次のような相似関係が成り立つ：

$$\frac{X}{Z} = \frac{u}{f_x}, \quad \frac{Y}{Z} = \frac{v}{f_y}$$

ここで、物体の3D座標 $X, Y$ とカメラの焦点距離 $f_x, f_y$ は、画像上の座標 $u, v$ と相似関係にある。

拡大縮小

この相似関係を使って、画像平面上の点 $u$ と $v$ は次のように計算される：

$$u = f_x \cdot \frac{X}{Z}, \quad v = f_y \cdot \frac{Y}{Z}$$

つまり、3D空間の $X$ と $Y$ に焦点距離 $f_x$ と $f_y$ を掛けてスケール調整することで、画像上の位置（ピクセル座標） $u, v$ が得られる。

2. 焦点距離と画像平面の中心

カメラ行列には、さらに画像平面の中心を示す $c_x$ と $c_y$ が含まれる。これらは、画像平面の中心を基準にピクセル座標を補正するために使われる。これにより、画像上の3D点の位置が正しく計算される。

3D点の投影

カメラ行列 $K$ を使って、3D点 $P = (X, Y, Z)$ を2D画像に投影する場合、そのピクセル座標 $u, v$ は次のように計算される：

$$u = f_x \cdot \frac{X}{Z} + c_x$$

$$v = f_y \cdot \frac{Y}{Z} + c_y$$

ここで、$c_x$ と $c_y$ は画像の中心を表し、正しいピクセル位置への補正を行っている。この補正がなければ、3D点の投影が画像の中央からずれた位置に表示されることになる。

参考になった記事

https://mem-archive.com/2018/02/21/post-157/

https://www.nikon-image.com/enjoy/phototech/manual/19/01.html

python

import numpy as np
import matplotlib.pyplot as plt

def points_to_depth_image(points, K, R, t, image_size=(480, 640)):

    # カメラ座標系への変換
    points_cam = np.dot(R, points.T) + t.reshape(3, 1)

    # 奥行き方向(Z)が0以下の点は除外
    valid_cam = points_cam[2, :] > 0
    points_cam = points_cam[:, valid_indices]

    # カメラ行列を使って2D平面に投影
    points_2d = np.dot(K, points_cam)

    # 正規化してピクセル座標に変換
    points_2d = points_2d[:2, :] / points_2d[2, :]

    # 深度(Z値)を取り出す
    depths = points_cam[2, :]

    # 画像を初期化する
    depth_image = np.zeros(image_size)

    # 2Dピクセル座標に深度をマッピング
    image_width = image_size[1]
    image_height = image_size[0]
    for i in range(points_2d.shape[1]):
        u, v = int(points_2d[0, i]), int(points_2d[1, i])
        if 0 <= u < image_width and 0 <= v < image_height:
            depth_image[v, u] = depths[i]

    return depth_image


points = np.array([
    [1, 1, 1],
    [1, 2, 1],
    [1, 3, 1],
    [1, 4, 1],
    [1, 5, 1]
])

# カメラの内部パラメータ行列
K = np.array([[800, 0, 320],
              [0, 800, 240],
              [0, 0, 1]])

# カメラの外部パラメータ
R = np.eye(3)
t = np.array([0, 0, 0])

depth_image = project_points_to_image_and_depth(points, K, R, t)

plt.imshow(depth_image, cmap="plasma")
plt.colorbar(label="Depth (Z-axis)")
plt.title('Depth Image from LiDAR Point Cloud')
plt.xlabel('X (pixels)')
plt.ylabel('Y (pixels)')
plt.show()

cpp

#include <iostream>
#include <Eigen/Dense>
#include <opencv2/opencv.hpp>

using namespace std;
using namespace Eigen;


cv::Mat points_to_depth_image(const MatrixXd& points, const Matrix3d& K, const Matrix3d& R, const Vector3d& t, const cv::Size& image_size) {
    // カメラ座標系への変換
    MatrixXd points_cam = (R * points.transpose()).colwise() + t;

    // 奥行き方向(Z)が0以下の点を除外
    std::vector<int> valid_indices;
    for (int i = 0; i < points_cam.cols(); ++i) {
        if (points_cam(2, i) > 0) {
            valid_indices.push_back(i);
        }
    }

    // 有効な点群のみを取得
    MatrixXd points_cam_valid(3, valid_indices.size());
    for (size_t i = 0; i < valid_indices.size(); ++i) {
        points_cam_valid.col(i) = points_cam.col(valid_indices[i]);
    }

    // カメラ行列を使って2D平面に投影
    MatrixXd points_2d = K * points_cam_valid;

    // 正規化してピクセル座標に変換
    for (size_t i = 0; i < points_2d.cols(); ++i) {
        points_2d(0, i) /= points_2d(2, i);
        points_2d(1, i) /= points_2d(2, i);
    }

    // 深度を取得
    VectorXd depths = points_cam_valid.row(2);

    // 画像を初期化する
    cv::Mat depth_image = cv::Mat::zeros(image_size, CV_32F);
    // cv::Mat depth_image = cv::Mat::zeros(image_size, CV_64F); // 高精度画像が欲しい場合

    // 2Dピクセル座標に深度をマッピング
    for (size_t i = 0; i < depths.cols(); ++i) {
        // 画像平面上の座標を示す時横軸は u, 縦軸は v で表すらしい（x, y の表記は3D空間の座標を表す時に使用）
        int u = static_cast<int>(points_2d(0, i));
        int v = static_cast<int>(points_2d(1, i));

        if (0 <= u && u < image_size.width && 0 <= v && v < image_size.height) {
            depth_image.at<float>(v, u) = depths(i)
            // depth_image.at<double>(v, u) = depths(i) // 高精度画像を使用する場合
        }
    }

    return depth_image;
}

int main() {
    MatrixXd points(5, 3);
    points << 1, 1, 1,
              1, 2, 1,
              1, 3, 1,
              1, 4, 1,
              1, 5, 1;

    Matrix3d K;
    K << 800, 0, 320,
         0, 800, 240,
         0, 0, 1;

    Matrix3d R = Matrix3d::Identity();
    Vector3d t(0, 0, 0);

    cv::Size image_size(640, 480);

    cv::Mat depth_image = points_to_depth_image(points, K, R, T, image_size)

    cv::imshow("Depth Image from LiDAR Point Cloud", depth_image)
    cv::waitKey(0);

    return 0
}