Fawkes API Fawkes Development Version
fc_accum.cpp
1/***************************************************************************
2 * fc_accum.cpp - Implementation of 'fitted circle' accumulator
3 * used by Randomized Stable Circle Fitting Algorithm
4 *
5 * Generated: Fri Sep 09 2005 22:50:12
6 * Copyright 2005 Hu Yuxiao <Yuxiao.Hu@rwth-aachen.de>
7 *
8 ****************************************************************************/
9
10/* This program is free software; you can redistribute it and/or modify
11 * it under the terms of the GNU General Public License as published by
12 * the Free Software Foundation; either version 2 of the License, or
13 * (at your option) any later version. A runtime exception applies to
14 * this software (see LICENSE.GPL_WRE file mentioned below for details).
15 *
16 * This program is distributed in the hope that it will be useful,
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19 * GNU Library General Public License for more details.
20 *
21 * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
22 */
23
24#include <fvmodels/shape/accumulators/fc_accum.h>
25
26#include <cmath>
27#include <cstdio>
28
29using namespace fawkes;
30
31namespace firevision {
32
33const float FittedCircle::TOO_SMALL_DELTA = 1.0e-3f;
34
35/** @class FittedCircle <fvmodels/shape/accumulators/fc_accum.h>
36 * FittedCircle accumulator.
37 */
38
39/** Constructor. */
41{
42 reset();
43}
44
45/** Destructor. */
47{
48}
49
50/** Reset. */
51void
53{
54 count = 0;
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;
60 }
61 current_circle = 0;
62 point_added = false;
63}
64
65/** Add point.
66 * @param pt point
67 * @return distance from circle center
68 */
69float
71{
72 int next_circle = 1 - current_circle;
73 point_added = true;
74
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;
78
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;
82
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;
86
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;
91
92 float dist;
93
94 Circle *p = fitCircle(&circle_matrices[next_circle]);
95 if (p) {
96 float dx = p->center.x - pt.x;
97 float dy = p->center.y - pt.y;
98 float r = p->radius;
99 dist = fabs(sqrt(dx * dx + dy * dy) - r);
100 } else {
101 dist = 0;
102 }
103
104 return dist;
105}
106
107/** Remove point.
108 * @param pt point
109 */
110void
112{
113 int next_circle = 1 - current_circle;
114 point_added = true;
115
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;
119
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;
123
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;
127
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;
132}
133
134/** Distance.
135 * @param pt point
136 * @param current current
137 * @return distance
138 */
139float
140FittedCircle::distanceTo(const upoint_t &pt, bool current)
141{
142 int id = current ? current_circle : (1 - current_circle);
143 Circle *p = fitCircle(&circle_matrices[id]);
144 if (p) {
145 float dx = p->center.x - pt.x;
146 float dy = p->center.y - pt.y;
147 return fabs(sqrt(dx * dx + dy * dy) - p->radius);
148 } else {
149 // There is no circle, perhaps it is a line...
150 return 100000.0f; // temporarily... should fit a line...
151 }
152}
153
154/** Commit. */
155void
157{
158 if (point_added) {
159 current_circle = 1 - current_circle;
160 point_added = false;
161 ++count;
162 }
163}
164
165/** Get count.
166 * @return count
167 */
168int
170{
171 return count;
172}
173
174/** Get circle.
175 * @return circle
176 */
177Circle *
179{
180 return fitCircle(const_cast<circle_matrix *>(&circle_matrices[current_circle]));
181}
182
183/** Fit circle.
184 * @param p circle matrix
185 * @return circle
186 */
187Circle *
188FittedCircle::fitCircle(circle_matrix *p) const
189{
190 // solve the resulting 3 by 3 equations
191 static Circle c;
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) {
195 printf("A=\n");
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);
199 printf("b=\n");
200 printf("\t%f\t%f\t%f\n", p->b0, p->b1, p->b2);
201 printf("Delta too small: %e\n", delta);
202 return NULL;
203 } else {
204 c.center.x =
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)
207 / delta);
208 c.center.y =
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)
211 / delta);
212 c.radius =
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)
215 / delta
216 + c.center.x * c.center.x + c.center.y * c.center.y);
217 c.count = count;
218 return &c;
219 }
220}
221
222} // end namespace firevision
Circle shape.
Definition: circle.h:43
float radius
Radius of object.
Definition: circle.h:59
center_in_roi_t center
Center of object in ROI.
Definition: circle.h:57
int count
Number of pixels.
Definition: circle.h:61
Circle * getCircle(void) const
Get circle.
Definition: fc_accum.cpp:178
~FittedCircle(void)
Destructor.
Definition: fc_accum.cpp:46
void removePoint(const fawkes::upoint_t &)
Remove point.
Definition: fc_accum.cpp:111
float distanceTo(const fawkes::upoint_t &, bool current=true)
Distance.
Definition: fc_accum.cpp:140
void reset(void)
Reset.
Definition: fc_accum.cpp:52
void commit(void)
Commit.
Definition: fc_accum.cpp:156
float addPoint(const fawkes::upoint_t &)
Add point.
Definition: fc_accum.cpp:70
int getCount(void) const
Get count.
Definition: fc_accum.cpp:169
FittedCircle(void)
Constructor.
Definition: fc_accum.cpp:40
Fawkes library namespace.
Point with cartesian coordinates as unsigned integers.
Definition: types.h:35
unsigned int x
x coordinate
Definition: types.h:36
unsigned int y
y coordinate
Definition: types.h:37
float y
y in pixels
Definition: types.h:40
float x
x in pixels
Definition: types.h:39