👻
Kinect Fusionで作る3Dスキャナその１

おおひら
2025/11/18に公開
Unity
tech
 1. はじめに
 1.1 概要Kinect Fusion[1] は、センサーから取得した複数の深度画像を統合して、3Dモデルをリアルタイムで生成する技術です。
本稿は、Kinect Fusionのアルゴリズムを理解し実装できるようになることが目的です。
各節で疑似コードは示していきますが、具体的な実装は以下のリポジトリにあるので、興味ある方は参照してください。
https://github.com/abist-co-ltd/TestKinectFusion

 1.2 完成イメージ以下は、提供しているリポジトリの実行動画と、最終的に出力したスキャンメッシュの画像です。
※ 提供しているリポジトリでは、オフライン(Unity Editor)でのスキャンを行っており、使用データセットはTUMデータセットのfreiburg1_xyzです。

 1.3 この記事で学べることICP（Iterative Closest Point）
TSDF（Truncated Signed Distance Function）
ボリューメトリック統合の仕組み
レイキャスティングによる表面抽出

 1.4 開発環境

項目
内容


OS
macOS Tahoe 26.0.1

チップ
Apple M4

メモリ
24GB

Unity
2022.3.11f1 (LTS)


 2. Kinect Fusionの全体アーキテクチャKinect Fusionは、以下のステップを毎フレーム繰り返すことで実現しています。
1. 深度画像取得
   ↓
2. 点群生成と法線計算
   ↓
3. カメラトラッキング（ICP）(※ 1フレーム目はスキップ)
   ↓
4. ボリューメトリック統合（TSDF更新）
   ↓
5. レイキャスティング（表面再構築）
   ↓
6. 次フレームへ
実装イメージは以下の通りです。
void MainLoop() 
{
    while (HasNextDepthFrame()) 
    {
        DepthImage depth = GetNextDepthFrame();

        if (frameIndex == 0) 
        {
            // 1フレーム目は初期化のみ
            currPose = IdentityMatrix();
        } 
        else 
        {
            // レイキャスティング（表面再構築）
            VertexMap modelVertexMap;
            NormalMap modelNormalMap;
            DepthMap modelDepthMap;
            TSDFVolume_RenderSurfacePrediction(
                tsdfVolume,
                intrinsics,
                currPose,
                &modelVertexMap,
                &modelNormalMap,
                &modelDepthMap
            );

            // カメラトラッキング (ICP)
            Matrix4x4 deltaPose = SolveICP(
                depth,
                modelVertexMap,
                modelNormalMap,
                modelDepthMap,
                intrinsics
            );
            currPose = MatrixMultiply(deltaPose, currPose);
        }

        // ボリューメトリック統合（TSDF更新）
        TSDFVolume_IntegrateDepthFrame(
            tsdfVolume,
            depth,
            currPose,
            intrinsics
        );

        ++frameIndex;
    }
}


 3. 深度画像の前処理センサーから取得した深度画像にはノイズが含まれます。
ノイズを低減するための一般的な手法として、Kinect Fusion では バイラテラルフィルタ が用いられています。
本稿では、実装の簡便さのため、前処理は行っていませんが、理論的にはバイラテラルフィルタを適用することでメッシュ再構成の精度が向上します。

 4. 深度画像から点群生成
 4.1 3次元点群の生成深度画像を取得できたら、深度画像からカメラ座標系の3次元点群を生成します。

各ピクセル (u,v) の深度値 d(u,v) をカメラ内部パラメータ行列 K を用いて3D位置に変換します。

P(u,v) = d(u,v) * K^{-1} * [u, v, 1]^T 
ここで

d(u,v): ピクセル(u,v)の深度値 \\
K =
\begin{bmatrix}
f_x & 0 & c_x \\
0 & f_y & c_y \\
0 & 0 & 1
\end{bmatrix}
実装イメージは以下の通りです。
for(int index = 0; index < width * height; ++index)
{
    float z = depth[index];

    uint y = index / width;
    uint x = index % width;

    // ピクセル座標 (u,v) => カメラ座標 (x,y,z)
    float vx = (x - cx) / fx * z;
    float vy = (y - cy) / fy * z;
    float vz = z;

    vertexMap[index] = float3(vx, vy, vz);
}

 4.2 法線ベクトルの計算各ピクセル位置における3次元点群から、

隣接点の差分ベクトルの外積を用いて法線ベクトルを計算します。

n(u,v) = (P(u+1,v) - P(u-1,v)) × (P(u,v+1) - P(u,v-1)) \\
n = n / ||n|| (正規化)
実装イメージは以下の通りです
for(int index = 0; index < width * height; ++index)
{
    // 隣接点を取得
    uint idxX1 = index + 1;
    uint idxX0 = index - 1;
    uint idxY1 = index + width;
    uint idxY0 = index - width;
    float3 vx1 = vertexMap[idxX1];
    float3 vx0 = vertexMap[idxX0];
    float3 vy1 = vertexMap[idxY1];
    float3 vy0 = vertexMap[idxY0];

    // 法線を計算
    float3 dx = vx1 - vx0;
    float3 dy = vy1 - vy0;
    float3 n = cross(dy, dx);
    n = normalize(n);

    normalMap[index] = n;
}

 5. ICPによるカメラトラッキングKinect Fusionでは、カメラの位置推定にICPを用いています。

まずはICPアルゴリズムの基礎から始めましょう。

 5.1 そもそもICPとは？Kinect Fusion では、毎フレームごとに「カメラがどれだけ前フレームから動いたか（姿勢変化）」を推定します。

このカメラ姿勢を求めるために利用されるのが ICP（Iterative Closest Point） アルゴリズムです。
ICP は、現在の深度画像から得られた点群を、前のフレームまでに再構築されたモデル(詳細は後述)と重ね合わせることで、

カメラの最適な位置・姿勢を推定します。
つまり、前フレームと現在フレームの点群を一致させる、以下のような「剛体変換」を見つける問題です。

T = [R | t]
ここで、

R：3×3の回転行列（カメラの向きを表す）

t：3次元ベクトル（カメラの位置のずれを表す）

 5.2 Point-to-Plane ICPの定式化Kinect FusioinにおけるICPでは、点と面（法線） の距離を最小化する「Point-to-Plane」を考えます。
これは、深度画像から得られる法線情報を活用することで、より安定した収束を得ることができるのが特徴です。
その誤差関数は次のように書けます。

E(R, t) = \sum_i \big( (R p_i + t - q_i) \cdot n_i \big)^2 \\
= \sum_i e_i^2
ここで

p_i ：現在フレームの点（カメラ座標系）

q_i ：対応するモデル上の点（既知）

n_i ：モデル点 q_i における法線ベクトル

e_i ：各対応点の誤差
この式は、「カメラの回転・並進によって p_i を動かした結果、モデル面 q_i にどれだけ近づいたか」を表しています。
誤差を小さくするほど、カメラ姿勢が正しくなります。
ただし、このままだと直接最小化するのは難しいため、小さな回転の近似を使います。
小さな回転ベクトル \boldsymbol{r} = [r_x, r_y, r_z] を使うとき、回転行列 (R) は次のように近似できます。

R \approx I + [\mathbf{r}]_\times
ここで [\mathbf{r}]_\times は「外積を行列で表現したもの（反対称行列）」です。
これを代入すると

e_i \approx n_i^{\top}(p_i + \boldsymbol{r} \times p_i + t - q_i)
整理して

e_i \approx n_i^{\top}(p_i - q_i) + n_i^{\top}(\boldsymbol{r} \times p_i) + n_i^{\top} t
スカラー三重積の恒等式より

\mathbf{n}_i^\top (\boldsymbol{r} \times p_i) = (p_i \times \mathbf{n}_i)^\top \boldsymbol{r}
したがって

e_i \approx n_i^{\top}(p_i - q_i) + (p_i \times \mathbf{n}_i)^\top \boldsymbol{r} + n_i^{\top} t
これは 未知変数が (\boldsymbol{x} = [r_x, r_y, r_z, t_x, t_y, t_z]^T) の

一次式です。

e_i \approx a_i^{\top} \boldsymbol{x} + b_i
ただし

a_i = [(p_i \times n_i)^{\top} \quad n_i^{\top}]

b_i = n_i^{\top}(p_i - q_i)
したがって

e_i = (p_i \times n_i)^{\top} \boldsymbol{r} + n_i^{\top} t + n_i^{\top}(p_i - q_i)
全点 (i = 1 \ldots N) をまとめると

\begin{bmatrix}
e_1 \\
e_2 \\
\vdots \\ 
e_N \\
\end{bmatrix}
=
\underbrace{\begin{bmatrix}
(p_1 \times n_1)^{\top} & n_1^{\top} \\
(p_2 \times n_2)^{\top} & n_2^{\top} \\
\vdots & \vdots \\
(p_N \times n_N)^{\top} & n_N^{\top}
\end{bmatrix}}_{A}
\underbrace{\begin{bmatrix}
\boldsymbol{r} \\
\boldsymbol{t}
\end{bmatrix}}_\mathbf{x}
+
\underbrace{\begin{bmatrix}
n_1^{\top}(p_1 - q_1) \\
n_2^{\top}(p_2 - q_2) \\
\vdots \\
n_N^{\top}(p_N - q_N)
\end{bmatrix}}_\mathbf{b}
よって、誤差関数は以下と等価です。

E(\mathbf{x}) = ||A\mathbf{x} + \mathbf{b}||^2
誤差 (E(\mathbf{x})) が最小になるとき、その勾配（傾き）がゼロになるので、微分します。

\frac{\partial E}{\partial \mathbf{x}}
= 2 A^{\top} (A \mathbf{x} + \mathbf{b}) = 0
整理すると

A^{\top}A \mathbf{x} = -A^{\top}\mathbf{b}
この連立方程式を解くことで、\mathbf{x} を求め、カメラ姿勢（回転 R と並進 t）を更新していきます。

 5.3 対応点探索通常の ICP では、対応点を求めるために「最近傍探索（Nearest Neighbor Search）」を行います。
しかし Kinect Fusion では、前フレームまでに計算したモデル表面と、現在フレームで得た頂点と法線 を活用して対応点を求めます。
通常、これは最近傍探索よりかなり効率的です。
手順としては以下の通りです：
前フレーム（または統合モデル）の表面に対してカメラからレイキャスト(詳細は後述)して、モデルの頂点マップ V_{\text{model}} と法線マップ N_{\text{model}} を得る。
現在フレームの頂点マップ V_{\text{curr}} を、推定中の姿勢変換 T でモデル座標系に変換する。
2.で変換後の頂点を再び画像平面に投影し、そのピクセル座標でモデル側の対応点を取得する。
これにより、空間的な探索を行わずに対応点を高速に求めることができます。
対応点の有効性判定
対応点として満たすべき条件には例えば以下のようなものがあります。
対応点間の距離が閾値以内
法線の角度差が閾値以内

 5.4 マルチスケールICPKinect Fusion では、ICP を一度だけでなく、

粗い解像度から細かい解像度へと段階的に繰り返す「マルチスケールICP」を行います。
これは、低解像度の深度画像から大まかな動きを推定し、

その結果を初期値として高解像度で精密に合わせ込む手法です。
各スケールにおいて、次の処理を行います：
深度画像をダウンサンプリングし、対応する頂点マップ・法線マップを生成
ICPを一定回数（例：10回）繰り返し、姿勢を更新
次のスケール（より高解像度）の初期値として使用

 5.5 疑似コード以上で理論の説明は終わったので、最後にICP全体の疑似コードを示します。
Matrix4x4 SolveICP(
    DepthPyramid D[],          // 各スケールの深度画像
    VertexMap V[],             // 各スケールの頂点マップ
    NormalMap N[],             // 各スケールの法線マップ
    VertexMap modelVertexMap,  // モデルの頂点マップ
    NormalMap modelNormalMap,  // モデルの法線マップ
    Matrix4x4 T_init           // 初期カメラ姿勢
)
{
    Matrix4x4 T = T_init;

    // 粗いスケールから高解像度へ
    for (int level = MAX_LEVEL - 1; level >= 0; --level)
    {
        for (int iter = 0; iter < ICP_ITERATIONS; ++iter)
        {
            Matrix6x6 ATA = 0;     // A^T * A の累積
            Vector6 ATb = 0;       // A^T * b の累積

            for (int v = 0; v < height[level]; ++v)
            {
                for (int u = 0; u < width[level]; ++u)
                {
                    float3 p = V[level][v * width[level] + u];

                    // 現在の推定姿勢でモデル座標へ変換
                    float3 p_model = TransformPoint(T, p);

                    // モデル画像に再投影し、対応点と法線を取得
                    float3 q, n;
                    if (!ProjectAndLookup(p_model, modelVertexMap, modelNormalMap, &q, &n))
                        continue;

                    // 誤差 e = n・(p' - q)
                    float3 diff = p_model - q;
                    float e = dot(n, diff);

                    // 式の係数を構築し、行列に加算
                    float3 cross_pn = cross(p_model, n);
                    float a[6] = { cross_pn.x, cross_pn.y, cross_pn.z, n.x, n.y, n.z };

                    // ATA += a^T * a;
                    // ATb += a * e;
                    AccumulateATA_ATb(ATA, ATb, a, e);
                }
            }

            // 線形方程式を解く:  ATA * x = -ATb
            // 例えば、Cholesky分解やLU分解を使う
            Vector6 x = SolveLinearSystem(ATA, -ATb);

            // 解をもとにカメラ姿勢を更新
            Matrix4x4 dT = CreateIncrementalTransform(x);
            T = dT * T;
        }
    }

    return T;
}

 まとめここまでで、各フレームのカメラ位置（姿勢）をICPによって推定する方法を紹介しました。
次回の記事では、今回推定したカメラ姿勢を使って TSDFボリュームに点群を統合し、レイキャスティングによる表面再構築の仕組みについて解説していきます。

脚注
https://www.microsoft.com/en-us/research/wp-content/uploads/2016/02/ismar2011.pdf ↩︎
項目	内容
OS	macOS Tahoe 26.0.1
チップ	Apple M4
メモリ	24GB
Unity	2022.3.11f1 (LTS)
株式会社アビストイノベーションセンターPublication
1. はじめに

1.1 概要

1.2 完成イメージ

1.3 この記事で学べること

1.4 開発環境

2. Kinect Fusionの全体アーキテクチャ

3. 深度画像の前処理

4. 深度画像から点群生成

4.1 3次元点群の生成

4.2 法線ベクトルの計算

5. ICPによるカメラトラッキング

5.1 そもそもICPとは？

5.2 Point-to-Plane ICPの定式化

5.3 対応点探索

5.4 マルチスケールICP

5.5 疑似コード

まとめ

Discussion
