24 #include <fvmodels/shape/accumulators/fc_accum.h>
31 namespace firevision {
33 const float FittedCircle::TOO_SMALL_DELTA = 1.0e-3f;
40 FittedCircle::FittedCircle(
void)
46 FittedCircle::~FittedCircle(
void)
52 FittedCircle::reset(
void)
55 for (
int i = 0; i < 2; ++i) {
56 circle_matrices[i].A00 = circle_matrices[i].A01 = circle_matrices[i].A02 = 0.0f;
57 circle_matrices[i].A10 = circle_matrices[i].A11 = circle_matrices[i].A12 = 0.0f;
58 circle_matrices[i].A20 = circle_matrices[i].A21 = circle_matrices[i].A22 = 0.0f;
59 circle_matrices[i].b0 = circle_matrices[i].b1 = circle_matrices[i].b2 = 0.0f;
70 FittedCircle::addPoint(
const upoint_t &pt)
72 int next_circle = 1 - current_circle;
75 circle_matrices[next_circle].A00 += 4 * pt.
x * pt.
x;
76 circle_matrices[next_circle].A01 += 4 * pt.
x * pt.
y;
77 circle_matrices[next_circle].A02 += 2 * pt.
x;
79 circle_matrices[next_circle].A10 += 4 * pt.
y * pt.
x;
80 circle_matrices[next_circle].A11 += 4 * pt.
y * pt.
y;
81 circle_matrices[next_circle].A12 += 2 * pt.
y;
83 circle_matrices[next_circle].A20 += 2 * pt.
x;
84 circle_matrices[next_circle].A21 += 2 * pt.
y;
85 circle_matrices[next_circle].A22 += 1;
87 float r2 = pt.
x * pt.
x + pt.
y * pt.
y;
88 circle_matrices[next_circle].b0 += 2 * r2 * pt.
x;
89 circle_matrices[next_circle].b1 += 2 * r2 * pt.
y;
90 circle_matrices[next_circle].b2 += r2;
94 Circle *p = fitCircle(&circle_matrices[next_circle]);
99 dist = fabs(sqrt(dx * dx + dy * dy) - r);
111 FittedCircle::removePoint(
const upoint_t &pt)
113 int next_circle = 1 - current_circle;
116 circle_matrices[next_circle].A00 -= 4 * pt.
x * pt.
x;
117 circle_matrices[next_circle].A01 -= 4 * pt.
x * pt.
y;
118 circle_matrices[next_circle].A02 -= 2 * pt.
x;
120 circle_matrices[next_circle].A10 -= 4 * pt.
y * pt.
x;
121 circle_matrices[next_circle].A11 -= 4 * pt.
y * pt.
y;
122 circle_matrices[next_circle].A12 -= 2 * pt.
y;
124 circle_matrices[next_circle].A20 -= 2 * pt.
x;
125 circle_matrices[next_circle].A21 -= 2 * pt.
y;
126 circle_matrices[next_circle].A22 -= 1;
128 float r2 = pt.
x * pt.
x + pt.
y * pt.
y;
129 circle_matrices[next_circle].b0 -= 2 * r2 * pt.
x;
130 circle_matrices[next_circle].b1 -= 2 * r2 * pt.
y;
131 circle_matrices[next_circle].b2 -= r2;
140 FittedCircle::distanceTo(
const upoint_t &pt,
bool current)
142 int id = current ? current_circle : (1 - current_circle);
143 Circle *p = fitCircle(&circle_matrices[
id]);
147 return fabs(sqrt(dx * dx + dy * dy) - p->
radius);
156 FittedCircle::commit(
void)
159 current_circle = 1 - current_circle;
169 FittedCircle::getCount(
void)
const
178 FittedCircle::getCircle(
void)
const
180 return fitCircle(const_cast<circle_matrix *>(&circle_matrices[current_circle]));
188 FittedCircle::fitCircle(circle_matrix *p)
const
192 float delta = +p->A00 * p->A11 * p->A22 + p->A01 * p->A12 * p->A20 + p->A02 * p->A10 * p->A21
193 - p->A00 * p->A12 * p->A21 - p->A01 * p->A10 * p->A22 - p->A02 * p->A11 * p->A20;
194 if (delta > -TOO_SMALL_DELTA && delta < TOO_SMALL_DELTA) {
196 printf(
"\t%f\t%f\t%f\n", p->A00, p->A01, p->A02);
197 printf(
"\t%f\t%f\t%f\n", p->A10, p->A11, p->A12);
198 printf(
"\t%f\t%f\t%f\n", p->A20, p->A21, p->A22);
200 printf(
"\t%f\t%f\t%f\n", p->b0, p->b1, p->b2);
201 printf(
"Delta too small: %e\n", delta);
205 (float)((+p->b0 * p->A11 * p->A22 + p->A01 * p->A12 * p->b2 + p->A02 * p->b1 * p->A21
206 - p->b0 * p->A12 * p->A21 - p->A01 * p->b1 * p->A22 - p->A02 * p->A11 * p->b2)
209 (float)((+p->A00 * p->b1 * p->A22 + p->b0 * p->A12 * p->A20 + p->A02 * p->A10 * p->b2
210 - p->A00 * p->A12 * p->b2 - p->b0 * p->A10 * p->A22 - p->A02 * p->b1 * p->A20)
213 (float)sqrt((+p->A00 * p->A11 * p->b2 + p->A01 * p->b1 * p->A20 + p->b0 * p->A10 * p->A21
214 - p->A00 * p->b1 * p->A21 - p->A01 * p->A10 * p->b2 - p->b0 * p->A11 * p->A20)