[数学] 最小二乗平面をプログラムで求める

概要

前回書いたLU分解の記事を用いて、今回は「最小二乗平面」を求めるプログラムについて書きたいと思います。
前回の記事で書いた通り、現在作っているVRコンテンツで利用するためのものです。
今回はこちらの記事（最小二乗平面の求め方 - エスオーエル）を参考にしました。

最小二乗平面とは？

以前、最小二乗法を使ってトレンドラインを計算するという記事を書きましたが、こちらは２次元データでの求め方でした。
それに対して、最小二乗「平面」ということからも分かるように、３次元の点群データを元に、その点郡すべてからの距離が最小の二乗となるような「平面」を求めるものです。

理論について詳しくはないですが、どうやらこうしたデータ郡の中で平面を求めるには最小二乗法が最適であることが分かっているようです。

最小二乗法について

最小二乗平面は3次元のデータを利用することを書きましたが、そもそも最小二乗法とはどんな方法なのでしょうか。
Wikipediaから引用させてもらうと以下のように書かれています。

最小二乗法(さいしょうにじょうほう、さいしょうじじょうほう；最小自乗法とも書く、英: least squares method)は、測定で得られた数値の組を、適当なモデルから想定される1次関数、対数曲線など特定の関数を用いて近似するときに、想定する関数が測定値に対してよい近似となるように、残差の二乗和を最小とするような係数を決定する方法、あるいはそのような方法によって近似を行うことである。

大事な点は以下です。

残差の二乗和を最小とするような係数を決定する方法

今回の記事で言えば、求めた平面（3次元データの点群から求めた理想的な平面）から、実際の観測データまでの高さ（残差）の二乗の和（$\sum{}$）を最小とする係数を求める方法、ということです。
要は、全部の点群のおよそ中心を通る「いい感じの平面」を求めるってことですね。

ちなみにこれを求める理由は、前述のように「理想的な」平面が求まれば、以後の点の予測や、あるいは観測済みデータを理想的な平面に投影した位置の点を求めることができるようになるためです。

平面の方程式

導出は以下のようになります。

点$P(x_0, y_0, z_0)$を通り、法線ベクトルを$\vec{n} = (a, b, c)$とする平面の方程式は、平面上の任意の点$x$を用いて以下のように内積を用いて計算できます。

a(x - x_0) + b(y - y_0) + c(z - z_0) = 0

$x - x_0$などは、平面上の「ベクトル」を求めたものです。
そして内積は、直行するベクトル同士を掛けると$0$になる性質を利用しての計算ですね。
つまり平面上のベクトルと法線の内積は$0$になる、ということです。

そして一般形にすると以下のように表されます。

ax + by + cz + d = 0

このとき、平面の法線ベクトルは$\vec{n} = (a, b, c)$となります。

参考: 平面の方程式

最小二乗平面での平面の方程式

さて、最小二乗平面を求める場合、平面からの距離を測定したいわけなので、$z$を高さ、$x, y$を縦横に取って、以下のように表します。

z = a + bx + cy

最小二乗平面は、最小二乗法から求めた$a, b, c$を上記の方程式に当てはめて求められる平面となります。
つまり、どの点からも最小距離の二乗となる平面、ということです。（これが「理想的な」平面です）

最小二乗平面を求める

今回計算したいと思っているのは、VRゲームで利用するとある値です。
その値とは、ユーザの手の位置を観測（サンプリング）し、その点群の中から今回の最小二乗平面を求めたいと思っています。
つまり、$x, y, z$の値を毎フレーム取得し、そのデータを元にするわけです。

なので、測定データをそれぞれ$x_i, y_i, z_i$とします。

求め方としては、求めたい$a, b, c$を未知数として、今回求めたい平面と各データとの高さの差を$E_i$と置きます。
つまり式にすると以下のようになります。

E_i = (a + bx_i + cy_i) - z_i

仮に、点が平面上にある場合は$z_i$と方程式の値が同じで結果は（差）は$0$となります。
なので$0$以外が差になる、というわけですね。

そしてこの$E_i$の二乗の値の和を$F$と置きます。
式にすると以下のようになります。

F = E_1^2 + E_2^2 + ... + E_n^2 = \sum{E_i^2}

冒頭でも書いたように、この和（$F$）が最小になる$a, b, c$を求めるのが今回の目的です。

求め方は、参考にした記事から引用させてもらうと

Fはa,b,cについて2次の式なので、最小値を求めることは、
a,b,cについて偏微分して0となる極値を求めることになります。

とのこと。
なのでそれぞれの係数に対して偏微分してみましょう。

試しに$b$からやってみます。
$E_i^2$は以下になります。

E_i^2 = (a + bx_i + cy_i - z_i)^2

これを展開すると、

a^2 + b^2x_i^2 + c^2y_i^2 + z_i^2 + 2abx_i + 2acy_i - 2az_i + 2bcx_iy_i - 2bx_iz_i - 2cy_iz_i

これを$b$について偏微分すると、

2ax_i + 2x_i^2b + 2cx_iy_i - 2x_iz_i 

となり、これを整理すると以下を得ます。

2(a + bx_i + cy_i - z_i)x_i

きれいに元の式の平方が出てきましたね。すごい。
そしてこれの和を求めているので、最終的には以下になります。

\frac{\partial{F}}{\partial{b}} = 2\sum{(a + bx_i + cy_i - z_i)x_i} = 0

同様にして、これらをすべての係数に対して行うと、最終的に以下を得ます。

\frac{\partial{F}}{\partial{a}} = 2\sum{(a + bx_i + cy_i - z_i)} = 0 \\
\frac{\partial{F}}{\partial{b}} = 2\sum{(a + bx_i + cy_i - z_i)x_i} = 0 \\
\frac{\partial{F}}{\partial{c}} = 2\sum{(a + bx_i + cy_i - z_i)y_i} = 0

3つの未知数に対して3つの式が作れたので、あとはこれを連立方程式として解くことで$a, b, c$それぞれの値を求めることができます。

LU分解を用いて未知数を求める

連立方程式を解くには行列にして計算するとやりやすいです。
そしてその行列にした計算をさらに解きやすくするのが、前回書いた記事（[数学] LU分解法をプログラムで書いてみた）で紹介している「LU分解法」です。
（他にもいくつか方法があるようです）

上記の連立方程式を、前回の記事と同じ記号を用いて行列に分解して表してみましょう。

A = 
\begin{bmatrix}
\sum{1} & \sum{x_i} & \sum{y_i} \\
\sum{x_i} & \sum{x_i^2} & \sum{x_iy_i} \\
\sum{y_i} & \sum{x_iy_i} & \sum{y_i^2}

\end{bmatrix}, \\

\vec{x} = 
\begin{bmatrix}
a \\
b \\
c
\end{bmatrix}, \\

\vec{b} = 
\begin{bmatrix}
\sum{z_i} \\
\sum{{x_i}{z_i}} \\
\sum{{y_i}{z_i}}
\end{bmatrix}

すると、以下のように表すことができます。

A\vec{x} = \vec{b}

さらにこれを、LU分解を用いて変形すると、

LU\vec{x} = \vec{b}

となります。
そして、行列Aを分解した行列Lと行列Uを使って、以下の2次方程式に変形します。

U\vec{x} = \vec{y} \\
L\vec{y} = \vec{b}

ここまでくれば、あとは前回実装したLU分解を用いて計算を行うことができそうです。
ということで、実際にプログラムに落とし込んだのが以下になります。

サンプリングデータを合計する

float[] SumSamplingData(Vector3[] data)
{
    // xの合計値
    float x = 0;

    // x^2の合計値
    float x2 = 0;

    // x * yの合計値
    float xy = 0;

    // x * zの合計値
    float xz = 0;

    // yの合計値
    float y = 0;

    // y^2の合計値
    float y2 = 0;

    // y * zの合計値
    float yz = 0;

    // zの合計値
    float z = 0;

    // 計測したデータから、各種必要なsumを得る
    for (int i = 0; i < data.Length; i++)
    {
        Vector3 v = data[i];

        // 最小二乗平面との誤差は高さの差を計算するので、（今回の式の都合上）Yの値をZに入れて計算する
        float vx = v.x;
        float vy = v.z;
        float vz = v.y;

        x += vx;
        x2 += (vx * vx);
        xy += (vx * vy);
        xz += (vx * vz);

        y += vy;
        y2 += (vy * vy);
        yz += (vy * vz);

        z += vz;
    }

    // matA[0, 0]要素は要素数と同じ（\sum{1}のため）
    float l = 1 * data.Length;

    // 求めた和を行列の要素として2次元配列を生成
    float[,] matA = new float[,]
    {
        {l,  x,  y},
        {x, x2, xy},
        {y, xy, y2},
    };

    float[] b = new float[]
    {
        z, xz, yz
    };

    // 求めた値を使ってLU分解→結果を求める
    return LUDecomposition(matA, b);
}

上記の部分で、計算に必要な各データの「和」を求めました。
これをLU分解を用いて連立方程式を解きます。
LU分解に関しては前回の記事でも書いていますが、前回の例はJavaScriptだったのでC#で再掲しておきます。

LU分解を行う

float[] LUDecomposition(float[,] aMatrix, float[] b)
{
    // 行列数（Vector3データの解析なので3x3行列）
    int N = aMatrix.GetLength(0);

    // L行列(零行列に初期化)
    float[,] lMatrix = new float[N, N];
    for (int i = 0; i < N; i++)
    {
        for (int j = 0; j < N; j++)
        {
            lMatrix[i, j] = 0;
        }
    }

    // U行列(対角要素を1に初期化)
    float[,] uMatrix = new float[N, N];
    for (int i = 0; i < N; i++)
    {
        for (int j = 0; j < N; j++)
        {
            uMatrix[i, j] = i == j ? 1f : 0;
        }
    }

    // 計算用のバッファ
    float[,] buffer = new float[N, N];
    for (int i = 0; i < N; i++)
    {
        for (int j = 0; j < N; j++)
        {
            buffer[i, j] = 0;
        }
    }

    // LU分解開始
    for (int i = 0; i < N; i++)
    {
        int n = N - i - 1;

        float l0 = lMatrix[i, i] = aMatrix[0, 0];

        // l1成分をコピー
        float[] l1 = new float[n];
        for (int j = 0; j < n; j++)
        {
            lMatrix[j + i + 1, i] = l1[j] =  aMatrix[j + 1, 0];
        }

        // u1^T成分をコピー
        float[] u1 = new float[n];
        for (int j = 0; j < n; j++)
        {
            uMatrix[i, j + i + 1] = u1[j] = aMatrix[0, j + 1] / l0;
        }

        // luを求める
        for (int j = 0; j < n; j++)
        {
            for (int k = 0; k < n; k++)
            {
                buffer[j, k] = l1[j] * u1[k];
            }
        }

        // A1を求める
        float[,] A1 = new float[n, n];
        for (int j = 0; j < n; j++)
        {
            for (int k = 0; k < n; k++)
            {
                A1[j, k] = aMatrix[j + 1, k + 1] - buffer[j, k];
            }
        }

        // A1を新しいaMatrixとして利用する
        aMatrix = A1;
    }

    // 求めたLU行列を使って連立方程式を解く
    float[] y = new float[N];
    for (int i = 0; i < N; i++)
    {
        float sum = 0;
        for (int k = 0; k <= i - 1; k++)
        {
            sum += lMatrix[i, k] * y[k];
        }
        y[i] = (b[i] - sum) / lMatrix[i, i];
    }

    float[] x = new float[N];
    for (int i = N - 1; i >= 0; i--)
    {
        float sum = 0;
        for (int j = i + 1; j <= N - 1; j++)
        {
            sum += uMatrix[i, j] * x[j];
        }
        x[i] = y[i] - sum;
    }

    return x;
}

これで、観測したデータ郡から理想的な平面を求めることができます。
あとは求まった平面の式を用いて、利用したデータを取得し利用するだけです。

実際に今回実装した内容では以下のように使っています。

利用例

// 最小二乗平面を用いた推測値を元に速度を求める
float[] result = CalcLeastSquaresPlane(samplingData);
float a = result[0];
float b = result[1];
float c = result[2];

// サンプリングした最後のデータを用いて、理想平面の値を求める
Vector3 v = samplingData.Last();

float y = a + (b * v.x) + (c * v.z);

// 実際に利用したいデータ
Vector3 vec = new Vector3(v.x, y, v.z);

これを使う前の計算方法と後でだいぶ違いが出たのでしっかり「理想的な」値が取れているようです。