kivantium活動日記

プログラムを使っていろいろやります

レーベンバーグ・マーカート法による球の方程式推定

目的

前回のエントリで紹介したレーベンバーグ・マーカート法の応用として、球の方程式を推定してみます。Kinectなどのセンサで球面の一部を観測し、そのデータから球の中心・半径を求めることを目指します。

サンプルデータの生成

下準備として、球面の座標データを生成します。中心O、半径rの球の方程式は
{ \displaystyle
x^2+y^2+z^2=r^2
}
になります。またz座標をz0と定めると、x,yは円
{ \displaystyle
x^2+y^2=r^2-z_{0}^2
}
上の点と見ることができます。この時、あるθを用いて
{ \displaystyle
x=\sqrt{r^2-z_{0}^2} cos\theta}
{ \displaystyle
y=\sqrt{r^2-z_{0}^2} sin\theta
}
と表すことができます。

また、カメラから得たデータにはノイズが含まれているのが普通です。そこで各点の座標に正規分布に従った乱数を加えることでノイズを表現することにします。

以上のことを考えて、次のようなコードになりました。

#include <stdio.h>
#include <math.h>
#include <time.h>
#include <stdlib.h>
#include <levmar.h>
#define PI 3.1415926

/* 乱数生成に関する部分は、C言語による乱数生成
 * (http://www.sat.t.u-tokyo.ac.jp/~omi/random_variables_generation.html)
 * を参考にしました*/

//一様乱数の生成
double Uniform( void ){
    return ((double)rand()+1.0)/((double)RAND_MAX+2.0);
}

//正規分布する乱数の生成
double rand_normal( double mu, double sigma ){
    double z=sqrt( -2.0*log(Uniform()) ) * sin( 2.0*M_PI*Uniform() );
    return mu + sigma*z;
 }

int main(){
    double x, y, z, theta;
    double sigma = 0.01;			 //乱数の広がり具合
    srand((unsigned)time(NULL)); //乱数の初期化
    for(z=1.0; z>=0.6; z-=0.01){
        for(theta=0.0; theta<=0.7*PI; theta+=PI/100){
                x = sqrt(1-z*z)*sin(theta)+rand_normal(0, sigma);
    	        y = sqrt(1-z*z)*cos(theta)+rand_normal(0, sigma);
                printf("%lf, %lf, %lf\n", x, y, z);
       	}
    }
    return 0;
}

このコードの出力結果をsphere.txtに保存します。
このコードの場合、中心座標が(0,0,0)、半径1になります。

なお、sphere.txtの中身をgnuplotで出力するとこんな感じです。球の一部っぽい感じが分かると思います。f:id:kivantium:20140502000518p:plain

球の方程式を推定する

今回求めたいのは、中心座標(x0,y0,z0)と、半径rです。そこで、各データの座標(x,y,z)に対し、
{ \displaystyle
\Sigma (x-x_0)^2+(y-y_0)^2+(z-z_0)^2-r^2
}
が最小になるような中心座標(x0,y0,z0)と半径rをレーベンバーグ・マーカート法で求めることにします。

コードは以下の通りです。

#include <stdio.h>
#include <math.h>
#include <time.h>
#include <float.h>
#include <stdlib.h>
#include <levmar.h>
#include <vector>
#define PI 3.1415926

//2乗を求める関数
double twice(double x){
    return x*x;
}
class Points{
public:
	std::vector<double> x,y,z;
};

//最小二乗法を行う関数
void func(double *p, double *hx, int m, int n, void *adata);

//func内で数字を扱う関係上面倒なのでグローバル変数に
//double x[2800], y[2800], z[2800];

int main(){
    FILE *fp;
    double tmpx, tmpy, tmpz, x, y, z, theta;
    int i, index_x, index_y, index_z;
    double p[4];              //p[0]から順に中心のx,y,z座標、半径r
    double* x2 = NULL;
    int m = 4;                //求めたい変数の数
    int n;                    //サンプルとなる点の数
    int itmax = 10000000;     //反復法を行う回数
    double* opts = NULL;      //デフォルト値を使用
    double* info = NULL;      //最小化に関するパラメーター、ここでは使わない
    double* work = NULL;      //メモリを自動確保
    double* covar = NULL;     //必要ないのでNULL
//    void* adata = NULL;       //追加のデータを入れる配列。これも使わない
    Points *adata = new Points();   //ファイルを開く(エラーチェック)
    if((fp=fopen("sphere.txt", "r"))==NULL){
        printf("File open error!!\n");
        return 1;
    }

    //最大値の推測
    tmpx = FLT_MIN;
    tmpy = FLT_MIN;
    tmpz = FLT_MIN;

    //x,y,zの最大値を求める
    for(i=0; fscanf(fp, "%lf, %lf, %lf", &x, &y, &z) != EOF; i++){
        
        if(x>tmpx) tmpx=x, index_x=i; 
        if(y>tmpy) tmpy=y, index_y=i;
        if(z>tmpz) tmpz=z, index_z=i;
        adata->x.push_back(x);
        adata->y.push_back(y); 
        adata->z.push_back(z); 
    }
    n=i+1;

    //それぞれの座標が最大値になる点の重心が中心に近いと予想してみる
    p[0]=(adata->x[index_x]+adata->x[index_y]+adata->x[index_z])/3.0;        //xの推定値
    p[1]=(adata->y[index_x]+adata->y[index_y]+adata->y[index_z])/3.0;        //yの推定値
    p[2]=(adata->z[index_x]+adata->z[index_y]+adata->z[index_z])/3.0;        //zの推定値
    p[3]=sqrt(twice(adata->x[index_x]-adata->x[index_y])
            +twice(adata->y[index_x]-adata->y[index_y])
            +twice(adata->z[index_x]-adata->z[index_y]));           
    //rの推定値(x座標最大の点とy座標最大の点の距離)

    dlevmar_dif(func, p, x2, m, n, itmax, opts, info, work, covar, adata); //pを求める
    printf("(x,y,z)=(%lf,%lf,%lf), r=%lf\n", p[0],p[1],p[2],p[3]);    //中心点と半径の出力

    return 0;
}
void func(double *p, double *hx, int m, int n, void *adata){
    int i;
	Points *data = static_cast<Points*>(adata);
    //全ての点に対して最小二乗法を実行
    for(i=0; i<data->x.size();i++) hx[i] = twice(twice(data->x[i]-p[0])
        +twice(data->y[i]-p[1])
        +twice(data->z[i]-p[2])
        -twice(p[3]));
}
(x,y,z)=(0.002596,0.002731,0.004987), r=0.994502

小数第2位までの精度ですが、正しい値を求めることができました。