Sphere Fitting program
Dependents: NineIMUAttitude_MadgwickFilter
Revision 1:05e1d9336ca8, committed 2020-12-26
- Comitter:
- aktk
- Date:
- Sat Dec 26 14:20:24 2020 +0000
- Parent:
- 0:bdae9d79b923
- Commit message:
- Add Comment;
Changed in this revision
SphereFitting.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r bdae9d79b923 -r 05e1d9336ca8 SphereFitting.cpp --- a/SphereFitting.cpp Sat Dec 26 13:54:49 2020 +0000 +++ b/SphereFitting.cpp Sat Dec 26 14:20:24 2020 +0000 @@ -1,6 +1,6 @@ #include "SphereFitting.hpp" -float const EPS = 1e-8; +float const EPSILON = 1e-8; bool is_zero(float val); @@ -50,7 +50,6 @@ X[3][2] += z; X[3][3] += 1; - // 右辺 Y[0] += x *(x * x + y * y + z * z); Y[1] += y *(x * x + y * y + z * z); Y[2] += z *(x * x + y * y + z * z); @@ -62,12 +61,16 @@ { float temp; - for (int i = 0; i < 4; i++) for (int j = 0; j < 4; j++) X[i][j] *= 2; - + // Completing making expression + for (int i = 0; i < 4; i++) + for (int j = 0; j < 4; j++) + X[i][j] *= 2; + + // Linear Algebrical Procedure for(int i = 0 ; i < 4 ; i++ ) { - // ピボット選択的みたいな処理 for(int j = i+1 ; j < 4 ; j++ ) { - if( is_zero(X[i][j]) == false ) + // Each proc lasts until X[i][j] is sufficiently small + if(-EPSILON < X[i][j] && X[i][j] < EPSILON) break; for(int k = 0 ; k < 4 ; k++ ) @@ -75,13 +78,11 @@ Y[i] += Y[j]; } - // 対角成分を1に temp = X[i][i]; for(int j = i ; j < 4; j++ ) X[i][j] /= temp; Y[i] /= temp; - // 前進消去 for(int j = i+1 ; j < 4 ; j++ ) { temp = X[j][i]; @@ -91,14 +92,10 @@ } } - // 後進消去 for(int i = 4-1 ; i >= 0 ; i-- ) for(int j = i - 1 ; j >= 0 ; j-- ) Y[j] -= X[j][i] * Y[i]; - - // a,b,c,dのパラメータがY[0],Y[1],Y[2],Y[3]に代入されているので - // a,b,c,rを代入して関数を終わる P->cx = Y[0]; P->cy = Y[1]; P->cz = Y[2]; @@ -115,9 +112,3 @@ if (ret_r != NULL) *ret_r = P->r; } - -// ほぼ0ならtrueを返す関数 -bool is_zero(float val) -{ - return -EPS < val && val < EPS; -} \ No newline at end of file