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 
29 using namespace fawkes;
30 
31 namespace firevision {
32 
33 const 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. */
40 FittedCircle::FittedCircle(void)
41 {
42  reset();
43 }
44 
45 /** Destructor. */
46 FittedCircle::~FittedCircle(void)
47 {
48 }
49 
50 /** Reset. */
51 void
52 FittedCircle::reset(void)
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  */
69 float
70 FittedCircle::addPoint(const upoint_t &pt)
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  */
110 void
111 FittedCircle::removePoint(const upoint_t &pt)
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  */
139 float
140 FittedCircle::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. */
155 void
156 FittedCircle::commit(void)
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  */
168 int
169 FittedCircle::getCount(void) const
170 {
171  return count;
172 }
173 
174 /** Get circle.
175  * @return circle
176  */
177 Circle *
178 FittedCircle::getCircle(void) const
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  */
187 Circle *
188 FittedCircle::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
Fawkes library namespace.
unsigned int y
y coordinate
Definition: types.h:37
unsigned int x
x coordinate
Definition: types.h:36
float x
x in pixels
Definition: types.h:39
Circle shape.
Definition: circle.h:42
Point with cartesian coordinates as unsigned integers.
Definition: types.h:34
int count
Number of pixels.
Definition: circle.h:61
center_in_roi_t center
Center of object in ROI.
Definition: circle.h:57
float y
y in pixels
Definition: types.h:40
float radius
Radius of object.
Definition: circle.h:59