1 #include "common/math/kasa.h"
2
3 #include <stdint.h>
4 #include <sys/types.h>
5
6 #include "common/math/mat.h"
7
kasaReset(struct KasaFit * kasa)8 void kasaReset(struct KasaFit *kasa) {
9 kasa->acc_mean_x = kasa->acc_mean_y = kasa->acc_mean_z = 0.0f;
10 kasa->acc_x = kasa->acc_y = kasa->acc_z = kasa->acc_w = 0.0f;
11 kasa->acc_xx = kasa->acc_xy = kasa->acc_xz = kasa->acc_xw = 0.0f;
12 kasa->acc_yy = kasa->acc_yz = kasa->acc_yw = 0.0f;
13 kasa->acc_zz = kasa->acc_zw = 0.0f;
14 kasa->nsamples = 0;
15 }
16
kasaInit(struct KasaFit * kasa)17 void kasaInit(struct KasaFit *kasa) { kasaReset(kasa); }
18
kasaAccumulate(struct KasaFit * kasa,float x,float y,float z)19 void kasaAccumulate(struct KasaFit *kasa, float x, float y, float z) {
20 // KASA fit runs into numerical accuracy issues for large offset and small
21 // radii. Assuming that all points are on an sphere we can substract the
22 // first x,y,z value from all incoming data, making sure that the sphere will
23 // always go through 0,0,0 ensuring the highest possible numerical accuracy.
24 if (kasa->nsamples == 0) {
25 kasa->acc_mean_x = x;
26 kasa->acc_mean_y = y;
27 kasa->acc_mean_z = z;
28 }
29
30 x = x - kasa->acc_mean_x;
31 y = y - kasa->acc_mean_y;
32 z = z - kasa->acc_mean_z;
33
34 // Accumulation.
35 float w = x * x + y * y + z * z;
36
37 kasa->acc_x += x;
38 kasa->acc_y += y;
39 kasa->acc_z += z;
40 kasa->acc_w += w;
41
42 kasa->acc_xx += x * x;
43 kasa->acc_xy += x * y;
44 kasa->acc_xz += x * z;
45 kasa->acc_xw += x * w;
46
47 kasa->acc_yy += y * y;
48 kasa->acc_yz += y * z;
49 kasa->acc_yw += y * w;
50
51 kasa->acc_zz += z * z;
52 kasa->acc_zw += z * w;
53
54 kasa->nsamples += 1;
55 }
56
kasaNormalize(struct KasaFit * kasa)57 bool kasaNormalize(struct KasaFit *kasa) {
58 if (kasa->nsamples == 0) {
59 return false;
60 }
61
62 float inv = 1.0f / kasa->nsamples;
63
64 kasa->acc_x *= inv;
65 kasa->acc_y *= inv;
66 kasa->acc_z *= inv;
67 kasa->acc_w *= inv;
68
69 kasa->acc_xx *= inv;
70 kasa->acc_xy *= inv;
71 kasa->acc_xz *= inv;
72 kasa->acc_xw *= inv;
73
74 kasa->acc_yy *= inv;
75 kasa->acc_yz *= inv;
76 kasa->acc_yw *= inv;
77
78 kasa->acc_zz *= inv;
79 kasa->acc_zw *= inv;
80
81 return true;
82 }
83
kasaFit(struct KasaFit * kasa,struct Vec3 * bias,float * radius,float max_fit,float min_fit)84 int kasaFit(struct KasaFit *kasa, struct Vec3 *bias, float *radius,
85 float max_fit, float min_fit) {
86 // A * out = b
87 // (4 x 4) (4 x 1) (4 x 1)
88 struct Mat44 A;
89 A.elem[0][0] = kasa->acc_xx;
90 A.elem[0][1] = kasa->acc_xy;
91 A.elem[0][2] = kasa->acc_xz;
92 A.elem[0][3] = kasa->acc_x;
93 A.elem[1][0] = kasa->acc_xy;
94 A.elem[1][1] = kasa->acc_yy;
95 A.elem[1][2] = kasa->acc_yz;
96 A.elem[1][3] = kasa->acc_y;
97 A.elem[2][0] = kasa->acc_xz;
98 A.elem[2][1] = kasa->acc_yz;
99 A.elem[2][2] = kasa->acc_zz;
100 A.elem[2][3] = kasa->acc_z;
101 A.elem[3][0] = kasa->acc_x;
102 A.elem[3][1] = kasa->acc_y;
103 A.elem[3][2] = kasa->acc_z;
104 A.elem[3][3] = 1.0f;
105
106 struct Vec4 b;
107 initVec4(&b, -kasa->acc_xw, -kasa->acc_yw, -kasa->acc_zw, -kasa->acc_w);
108
109 struct Size4 pivot;
110 mat44DecomposeLup(&A, &pivot);
111
112 struct Vec4 out;
113 mat44Solve(&A, &out, &b, &pivot);
114
115 // sphere: (x - xc)^2 + (y - yc)^2 + (z - zc)^2 = r^2
116 //
117 // xc = -out[0] / 2, yc = -out[1] / 2, zc = -out[2] / 2
118 // r = sqrt(xc^2 + yc^2 + zc^2 - out[3])
119
120 struct Vec3 v;
121 initVec3(&v, out.x, out.y, out.z);
122 vec3ScalarMul(&v, -0.5f);
123
124 float r_square = vec3Dot(&v, &v) - out.w;
125 float r = (r_square > 0) ? sqrtf(r_square) : 0;
126
127 // Need to correct the bias with the first sample, which was used to shift
128 // the sphere in order to have best accuracy.
129 initVec3(bias, v.x + kasa->acc_mean_x, v.y + kasa->acc_mean_y,
130 v.z + kasa->acc_mean_z);
131 *radius = r;
132
133 int success = 0;
134 if (r > min_fit && r < max_fit) {
135 success = 1;
136 }
137
138 return success;
139 }
140