Sphere Fitting program

Dependents:   NineIMUAttitude_MadgwickFilter

Files at this revision

API Documentation at this revision

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