最小二乗法による球面フィッティングのテスト

sphere_fitting_test.c

#include <stdio.h>
#include <math.h>
#include <time.h>
#include <stdlib.h>

#define N_DIM 4

#define EPS 1e-8
#define N_SAMPLE 256

void sphere_fitting(float *x,float *y,float *z,float *a,float *b,float *c,float *r);
void solve(float A[N_DIM][N_DIM],float B[N_DIM]);
bool is_zero(float val);
float random(float min,float max);

int main(void){
	// テスト用のx,y,zのデータを格納する配列
	float x[N_SAMPLE];
	float y[N_SAMPLE];
	float z[N_SAMPLE];
	// 推定するパラメータ
	float a = 1.0,b = 2.0,c = 3.0,r = 4.0;[
	// 推定した結果を保存する変数
	float out_a,out_b,out_c,out_r;
	// テスト用のデータ生成に使うヘンス
	float theta,phi;
	
	
	srand(time(NULL));
	
	for(int i = 0 ; i < N_SAMPLE ; i++ ){
		// (a,b,c)を中心に半径rの球面の座標を乱数を加えて作成する
		theta = random(0,2.0*M_PI);
		phi = random(0,M_PI);
		
		x[i] = a + r * cos(theta)*cos(phi) + random(-0.2,0.2);
		y[i] = b + r * sin(theta)*cos(phi) + random(-0.2,0.2);
		z[i] = c + r * sin(phi) + random(-0.2,0.2);
	}
	
	// 最小二乗法で計算する
	sphere_fitting(x,y,z, &out_a, &out_b, &out_c, &out_r);
	
	// 結果を表示する
	printf("original\t(a,b,c,r) = (%lf,%lf,%lf,%lf)\n",a,b,c,r);
	printf("estimate\t(a,b,c,r) = (%lf,%lf,%lf,%lf)\n",out_a,out_b,out_c,out_r);
	
	return 0;
}

// 最小二乗法で球面フィッティングをする関数
void sphere_fitting(float *x,float *y,float *z,float *a,float *b,float *c,float *r){
	float A[4][4] = {{0,0,0,0},{0,0,0,0},{0,0,0,0},{0,0,0,0}};
	float B[4] = {0.0 , 0.0 ,0.0 , 0.0};
	
	// ※後から2を掛けた方が良いとかそういうツッコミは無しで
	for(int i = 0 ; i < N_SAMPLE ; i++ ){
		// 左辺(係数行列)
		A[0][0] += 2.0 * x[i] * x[i];
		A[0][1] += 2.0 * x[i] * y[i];
		A[0][2] += 2.0 * x[i] * z[i];
		A[0][3] += 2.0 * x[i];
		
		A[1][0] += 2.0 * y[i] * x[i];
		A[1][1] += 2.0 * y[i] * y[i];
		A[1][2] += 2.0 * y[i] * z[i];
		A[1][3] += 2.0 * y[i];
		
		A[2][0] += 2.0 * z[i] * x[i];
		A[2][1] += 2.0 * z[i] * y[i];
		A[2][2] += 2.0 * z[i] * z[i];
		A[2][3] += 2.0 * z[i];
		
		A[3][0] += 2.0 * x[i];
		A[3][1] += 2.0 * y[i];
		A[3][2] += 2.0 * z[i];
		A[3][3] += 2.0 * 1.0;
		
		// 右辺
		B[0] += x[i]*(x[i]*x[i]+y[i]*y[i]+z[i]*z[i]);
		B[1] += y[i]*(x[i]*x[i]+y[i]*y[i]+z[i]*z[i]);
		B[2] += z[i]*(x[i]*x[i]+y[i]*y[i]+z[i]*z[i]);
		B[3] += x[i]*x[i]+y[i]*y[i]+z[i]*z[i];
	}
	solve(A,B);		// 連立方程式を計算
	
	// a,b,c,dのパラメータがB[0],B[1],B[2],B[3]に代入されているので
	// a,b,c,rを代入して関数を終わる
	*a = B[0];
	*b = B[1];
	*c = B[2];
	*r = sqrt(B[0]*B[0]+B[1]*B[1]+B[2]*B[2]+2*B[3]);
}

// 連立方程式を求める関数
void solve(float A[N_DIM][N_DIM],float B[N_DIM]){
	float temp;
	
	for(int i = 0 ; i < N_DIM ; i++ ){
		// ピボット選択的みたいな処理
		for(int j = i+1 ; j < N_DIM ; j++ ){
			if( is_zero(A[i][j] == false ))
				break;
			
			for(int k = 0 ; k < N_DIM ; k++ )
				A[i][k] += A[j][k];
			B[i] += B[j];
		}
		
		// 対角成分を1に
		temp = A[i][i];
		for(int j = i ; j < N_DIM ; j++ )
			A[i][j] /= temp;
		B[i] /= temp;
		
		// 前進消去
		for(int j = i+1 ; j < N_DIM ; j++ ){
			temp = A[j][i];
			
			for(int k = i ; k < N_DIM ; k++ )
				A[j][k] -= temp * A[i][k];
			B[j] -= temp * B[i];
		}
	}
	
	// 後進消去
	for(int i = N_DIM-1 ; i >= 0 ; i-- )
		for(int j = i - 1 ; j >= 0 ; j-- )
			B[j] -= A[j][i] * B[i];
}

// ほぼ0ならtrueを返す関数
bool is_zero(float val){
	return -EPS < val && val < EPS;
}

// [low,high)の乱数を発生させる関数
float random(float low,float high){
	float r = (float)(rand()) / RAND_MAX;
	
	return low + high * r;
}