フォーマットの練習を兼ねて、3次元点群の球面当てはめを行う最小二乗法について記載してみました。
問題設定
3次元空間に点があり、その点群に最もフィットする球面を考える。
ユースケースとしては、磁気センサで地磁気を測る際のキャリブレーションで、オフセット値を算出する際に用いる。
目的関数の導出
フィットさせる $ n $ 個の点を $ (x_1, y_1, z_1), ... , (x_n, y_n, z_n) $ とする。球は、中心を $ (x_0, y_0, z_0) $、半径を $ r $ として、
$$ (x - x_0)^2 + (y - y_0)^2 + (z - z_0)^2 = r^2 $$
と表すことができる。そこで、最小としたい目的関数 $ J $ を
$$ J = \sum_{k=1}^n \Bigl\{ (x_k - x_0)^2 + (y_k - y_0)^2 + (z_k - z_0)^2 - r^2 \Bigr\}^2 $$
とおく。
偏微分
$ J $ を変形して、
\begin{align}
J &= \sum_{k=1}^n \Bigl \{ x_k^2 + y_k^2 + z_k^2 - 2x_0x_k -2y_0y_k -2z_0z_k + x_0^2 + y_0^2 + z_0^2 - r^2 \Bigr \}^2 \\
&= \sum_{k=1}^n \Bigl \{ x_k^2 + y_k^2 + z_k^2 - X_0x_k -Y_0y_k -Z_0z_k - R \Bigr \}^2
\end{align}
ここで
X_0 = 2x_0, Y_0 = 2y_0, Z_0 = 2z_0 \\
R = r^2 - x_0^2 - y_0^2 - z_0^2
とおいた。さて、目的関数 $ J $ の極小点では
$$ \frac{\partial J}{\partial R} = \frac{\partial J}{\partial X_0} = \frac{\partial J}{\partial Y_0} = \frac{\partial J}{\partial Z_0} = 0 $$
となる。
$ R $ の偏微分 $ \frac{\partial J}{\partial R} $ は、
\frac{\partial J}{\partial R} = -2 \sum_{k=1}^n \Bigl \{ x_k^2 + y_k^2 + z_k^2 - X_0x_k -Y_0y_k -Z_0z_k - R \Bigr \} = 0
\begin{align}
\Sigma ( x_k^2 + y_k^2 + z_k^2 ) &= \Sigma R + X_0\Sigma x_k +Y_0 \Sigma y_k +Z_0 \Sigma z_k \\
\Sigma ( x_k^2 + y_k^2 + z_k^2 ) &=
\begin{pmatrix}
n & \Sigma x_k & \Sigma y_k & \Sigma z_k
\end{pmatrix}
\begin{pmatrix}
R \\
X_0 \\
Y_0 \\
Z_0
\end{pmatrix}
\end{align}
と変形できる。$ X_0 $ の偏微分は
\frac{\partial J}{\partial X_0} = -2 \sum_{k=1}^n x_k \Bigl \{ x_k^2 + y_k^2 + z_k^2 - X_0x_k -Y_0y_k -Z_0z_k - R \Bigr \} = 0
\begin{align}
\Sigma x_k ( x_k^2 + y_k^2 + z_k^2 ) &= \Sigma R x_k + X_0\Sigma x_k^2 +Y_0 \Sigma x_k y_k +Z_0 \Sigma x_k z_k \\
\Sigma x_k ( x_k^2 + y_k^2 + z_k^2 ) &=
\begin{pmatrix}
\Sigma x_k & \Sigma x_k^2 & \Sigma x_k y_k & \Sigma x_k z_k
\end{pmatrix}
\begin{pmatrix}
R \\
X_0 \\
Y_0 \\
Z_0
\end{pmatrix}
\end{align}
$ Y_0 $、$ Z_0 $ の偏微分も同様であり、
\begin{align}
\Sigma y_k ( x_k^2 + y_k^2 + z_k^2 ) &=
\begin{pmatrix}
\Sigma y_k & \Sigma x_k y_k & \Sigma y_k^2 & \Sigma y_k z_k
\end{pmatrix}
\begin{pmatrix}
R \\
X_0 \\
Y_0 \\
Z_0
\end{pmatrix}
\\
\Sigma z_k ( x_k^2 + y_k^2 + z_k^2 ) &=
\begin{pmatrix}
\Sigma z_k & \Sigma x_k z_k & \Sigma y_k z_k& \Sigma z_k^2
\end{pmatrix}
\begin{pmatrix}
R \\
X_0 \\
Y_0 \\
Z_0
\end{pmatrix}
\end{align}
以上から、
\begin{pmatrix}
\Sigma ( x_k^2 + y_k^2 + z_k^2 ) \\
\Sigma x_k ( x_k^2 + y_k^2 + z_k^2 ) \\
\Sigma y_k ( x_k^2 + y_k^2 + z_k^2 ) \\
\Sigma z_k ( x_k^2 + y_k^2 + z_k^2 )
\end{pmatrix}
=
\begin{pmatrix}
n & \Sigma x_k & \Sigma y_k & \Sigma z_k \\
\Sigma x_k & \Sigma x_k^2 & \Sigma x_k y_k & \Sigma x_k z_k \\
\Sigma y_k & \Sigma x_k y_k & \Sigma y_k^2 & \Sigma y_k z_k \\
\Sigma z_k & \Sigma x_k z_k & \Sigma y_k z_k & \Sigma z_k^2
\end{pmatrix}
\begin{pmatrix}
R \\
X_0 \\
Y_0 \\
Z_0
\end{pmatrix}
を得る。右辺の4×4行列の逆行列が存在すれば、
\begin{pmatrix}
R \\
X_0 \\
Y_0 \\
Z_0
\end{pmatrix}
=
\begin{pmatrix}
n & \Sigma x_k & \Sigma y_k & \Sigma z_k \\
\Sigma x_k & \Sigma x_k^2 & \Sigma x_k y_k & \Sigma x_k z_k \\
\Sigma y_k & \Sigma x_k y_k & \Sigma y_k^2 & \Sigma y_k z_k \\
\Sigma z_k & \Sigma x_k z_k & \Sigma y_k z_k & \Sigma z_k^2
\end{pmatrix}
^{-1}
\begin{pmatrix}
\Sigma ( x_k^2 + y_k^2 + z_k^2 ) \\
\Sigma x_k ( x_k^2 + y_k^2 + z_k^2 ) \\
\Sigma y_k ( x_k^2 + y_k^2 + z_k^2 ) \\
\Sigma z_k ( x_k^2 + y_k^2 + z_k^2 )
\end{pmatrix}
となり、これを解けば球が推定できる。ただし
$$ x_0 = \frac{1}{2}X_0, y_0 = \frac{1}{2}Y_0, z_0 = \frac{1}{2}Z_0, r = \sqrt{R+x_0^2+y_0^2+z_0^2} $$
C言語プログラミング
以上をC言語で解くプログラムを作成した。様々な（特に逆行列を計算する）ライブラリが存在しているが、何の変哲もないただの C で、掃き出し法で作成した。
LSMSphere.c
/// Point は double x, y, z をメンバーに持つ構造体。
int LSMSphere(Point *pointList, int pointListNum, Point *Answer, double *radius)
{
// データ不足
if (pointListNum < 4)
{
return -1;
}
// mat は逆行列を求める 4x4 行列
// vec は matの逆行列を掛けるベクトル
double mat[4][4];
double vec[4];
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
mat[i][j] = 0;
}
vec[i] = 0;
}
for (int i = 0; i < pointListNum; i++)
{
double mx = pointList[i].x;
double my = pointList[i].y;
double mz = pointList[i].z;
mat[0][0] += 1; // まあループの外で pointListNum を入れれば良いんだけど...
mat[0][1] += mx;
mat[0][2] += my;
mat[0][3] += mz;
mat[1][1] += (mx * mx);
mat[1][2] += (mx * my);
mat[1][3] += (mx * mz);
mat[2][2] += (my * my);
mat[2][3] += (my * mz);
mat[3][3] += (mz * mz);
double rho = (mx * mx) + (my * my) + (mz * mz);
vec[0] += rho;
vec[1] += (rho * mx);
vec[2] += (rho * my);
vec[3] += (rho * mz);
}
// 対称行列なので半分はここで入れる
mat[1][0] = mat[0][1];
mat[2][0] = mat[0][2];
mat[2][1] = mat[1][2];
mat[3][0] = mat[0][3];
mat[3][1] = mat[1][3];
mat[3][2] = mat[2][3];
// 掃き出し法で mat の逆行列 matinv を算出
double matinv[4][4];
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
matinv[i][j] = 0;
if (i == j)
{
matinv[i][j] = 1;
}
}
}
double tmp;
for (int i = 0; i < 4; i++)
{
tmp = 1 / mat[i][i];
for (int j = 0; j < 4; j++)
{
mat[i][j] *= tmp;
matinv[i][j] *= tmp;
}
for (int j = 0; j < 4; j++)
{
if (i == j)
{
continue;
}
tmp = mat[j][i];
for (int k = 0; k < 4; k++)
{
mat[j][k] -= (mat[i][k] * tmp);
matinv[j][k] -= (matinv[i][k] * tmp);
}
}
}
// 最終結果計算
double R = 0;
double X0 = 0;
double Y0 = 0;
double Z0 = 0;
for (int j = 0; j < 4; j++)
{
R += (matinv[0][j] * vec[j]);
X0 += (matinv[1][j] * vec[j]);
Y0 += (matinv[2][j] * vec[j]);
Z0 += (matinv[3][j] * vec[j]);
}
*Radius = sqrt(R + (X0 * X0 + Y0 * Y0 + Z0 * Z0) / 4.0);
Answer->x = X0 / 2.0;
Answer->y = Y0 / 2.0;
Answer->z = Z0 / 2.0;
return 0;
}