点群の楕円体フィッティング

何らかのセンサで取得した3次元空間の点群を楕円体にフィッティングする。加速度センサや地磁気センサを想定しているため、楕円体の自由度は6自由度(回転を含まない楕円体)。

点群の準備

適当な点群が欲しいのでいい感じのクラスを作っておく。

class Ellipsoid
{
    public double x { get; set; }
    public double y { get; set; }
    public double z { get; set; }
    public double a { get; set; }
    public double b { get; set; }
    public double c { get; set; }

    public Ellipsoid(double x, double y, double z, double a, double b, double c)
    {
        this.x = x;
        this.y = y;
        this.z = z;
        this.a = a;
        this.b = b;
        this.c = c;
    }

    public double[] Next(Random random)
    {
        double u = random.NextDouble() * 2 - 1;
        double v = random.NextDouble() * 2 - 1;
        double w = random.NextDouble() * 2 - 1;
        double r = Math.Sqrt(u * u + v * v + w * w);
        u /= r;
        v /= r;
        w /= r;

        return (new double[3]
        {
            x + a * u,
            y + b * v,
            z + c * w,
        });
    }
}

NextにRandomを渡すと適当な方向の単位ベクトルを作り、6自由度に当てはめて点を返す。

Random r = new Random(1234);
Ellipsoid elli = new Ellipsoid(1.23, 2.34, 3.45, 45.6, 56.7, 67.8);

みたいに宣言しておくと、elli.Next(r)というようにしたときに

-09.070 +52.583 -23.987
+32.414 -11.596 +50.027
+42.464 +05.791 +32.106
-19.025 -08.761 +62.726
+41.035 -01.587 +36.192
+19.450 +12.576 -57.486
-29.175 -31.542 +33.643

みたいな数値列が出てくる。

フィッティング

行列を使った最小二乗法でフィッティングする。
行列の計算にMathNetを使っている。

string fmt1 = "00.000";
string fmt2 = "+" + fmt1 + ";-" + fmt1 + "; " + fmt1;

Random r = new Random(1234);
Ellipsoid elli = new Ellipsoid(1.23, 2.34, 3.45, 45.6, 56.7, 67.8);

Matrix<double> mtx = DenseMatrix.OfArray(new double[7, 6]);
for (int i = 0; i < mtx.RowCount; i++)
{
    double[] xyz = elli.Next(r);

    mtx[i, 0] = xyz[0] * xyz[0];
    mtx[i, 1] = xyz[1] * xyz[1];
    mtx[i, 2] = xyz[2] * xyz[2];
    mtx[i, 3] = xyz[0];
    mtx[i, 4] = xyz[1];
    mtx[i, 5] = xyz[2];
}
Vector<double> vec = DenseVector.OfArray(Enumerable.Range(0, mtx.RowCount).Select(_ => 1.0).ToArray());
var v = (mtx.Transpose() * mtx).Inverse() * mtx.Transpose() * vec;

Matrix<double> mtx2 = DenseMatrix.OfArray(new double[3, 3] {
    { 2 * v[0], 0, 0 },
    { 0, 2 * v[1], 0 },
    { 0, 0, 2 * v[2] }
});
Vector<double> vec2 = DenseVector.OfArray(new double[3] {
    -v[3], -v[4], -v[5]
});
var w = (mtx2.Transpose() * mtx2).Inverse() * mtx2.Transpose() * vec2;

Console.WriteLine("x0 " + w[0].ToString(fmt2));
Console.WriteLine("y0 " + w[1].ToString(fmt2));
Console.WriteLine("z0 " + w[2].ToString(fmt2));

double tmp = (1 - (v[3] * w[0] + v[4] * w[1] + v[5] * w[2]) / 2);
Console.WriteLine("a   " + Math.Sqrt(tmp / v[0]).ToString(fmt1));
Console.WriteLine("b   " + Math.Sqrt(tmp / v[1]).ToString(fmt1));
Console.WriteLine("c   " + Math.Sqrt(tmp / v[2]).ToString(fmt1));

x0 +01.230
y0 +02.340
z0 +03.450
a   45.600
b   56.700
c   67.800

正しく6要素を推定できている。

任意サンプル数

上記コードの方式では、使用する点数が固定されているという難点がある。バッチ方式で使うなら大きな問題ではないが、リアルタイムにサンプリングしながら推定する場合は困る。

バッチ方式の場合は点群をポイント数x未知数の行列に入れて、最後に転置したり掛けたり反転したりしている。
フィルタ方式の場合は未知数x未知数の行列を用意しておき、サンプルごとに転置と積の和をとって、任意の場所で反転したりする。

Matrix<double> mtx = DenseMatrix.OfArray(new double[6, 6]);
Vector<double> vec = DenseVector.OfArray(new double[6]);
for (int k = 0; k < 7; k++)
{
    double[] xyz = elli.Next(r);
    Vector<double> a = DenseVector.OfArray(new double[6] {
        xyz[0] * xyz[0], xyz[1] * xyz[1], xyz[2] * xyz[2], xyz[0], xyz[1], xyz[2] });
    Matrix<double> b = a.ToRowMatrix();
    mtx += b.Transpose() * b;
    vec += a;
}
var v = mtx.Inverse() * vec;
// 以降は前述と同じ処理

今回はforを抜けてからvを求めているが、任意の時点で6要素を求めることができる。ただし点数が十分に多く、様々な姿勢でサンプリングする必要がある。
自乗や積和を繰り返すので表現できる範囲を超えたり精度が劣化したりしないような工夫が必要になる。
inverse以外の行列演算はさほど難しい処理ではないので、実際にマイコンで走らせたりする場合はdouble配列を直接扱うような処理になるはず。inverseは行列ライブラリを持ってきたり、ARMであればCMSISライブラリに含まれているけど、入力の行列が非破壊なライブラリを使う必要がある。

参考にしたページ

楕円体 - Wikipedia
最小二乗法の行列表現（一変数，多変数，多項式） | 高校数学の美しい物語
連立方程式の行列解
三次元空間中の点群への楕円体の当てはめ