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 #if 0 /* just to make Emacs auto-indent happy */
33 }
34 #endif
35 
36 const float FittedCircle::TOO_SMALL_DELTA = 1.0e-3f;
37 
38 /** @class FittedCircle <fvmodels/shape/accumulators/fc_accum.h>
39  * FittedCircle accumulator.
40  */
41 
42 /** Constructor. */
43 FittedCircle::FittedCircle(void)
44 {
45  reset();
46 }
47 
48 /** Destructor. */
49 FittedCircle::~FittedCircle(void)
50 {
51 }
52 
53 /** Reset. */
54 void
55 FittedCircle::reset(void)
56 {
57  count = 0;
58  for (int i=0; i<2; ++i)
59  {
60  circle_matrices[i].A00 = circle_matrices[i].A01 = circle_matrices[i].A02 = 0.0f;
61  circle_matrices[i].A10 = circle_matrices[i].A11 = circle_matrices[i].A12 = 0.0f;
62  circle_matrices[i].A20 = circle_matrices[i].A21 = circle_matrices[i].A22 = 0.0f;
63  circle_matrices[i].b0 = circle_matrices[i].b1 = circle_matrices[i].b2 = 0.0f;
64  }
65  current_circle = 0;
66  point_added = false;
67 }
68 
69 
70 /** Add point.
71  * @param pt point
72  * @return distance from circle center
73  */
74 float
75 FittedCircle::addPoint(const upoint_t& pt)
76 {
77  int next_circle = 1 - current_circle;
78  point_added = true;
79 
80  circle_matrices[next_circle].A00 += 4 * pt.x * pt.x;
81  circle_matrices[next_circle].A01 += 4 * pt.x * pt.y;
82  circle_matrices[next_circle].A02 += 2 * pt.x;
83 
84  circle_matrices[next_circle].A10 += 4 * pt.y * pt.x;
85  circle_matrices[next_circle].A11 += 4 * pt.y * pt.y;
86  circle_matrices[next_circle].A12 += 2 * pt.y;
87 
88  circle_matrices[next_circle].A20 += 2 * pt.x;
89  circle_matrices[next_circle].A21 += 2 * pt.y;
90  circle_matrices[next_circle].A22 += 1;
91 
92  float r2 = pt.x * pt.x + pt.y * pt.y;
93  circle_matrices[next_circle].b0 += 2 * r2 * pt.x;
94  circle_matrices[next_circle].b1 += 2 * r2 * pt.y;
95  circle_matrices[next_circle].b2 += r2;
96 
97  float dist;
98 
99  Circle* p = fitCircle(&circle_matrices[next_circle]);
100  if (p)
101  {
102  float dx = p->center.x - pt.x;
103  float dy = p->center.y - pt.y;
104  float r = p->radius;
105  dist = fabs(sqrt(dx*dx+dy*dy)-r);
106  }
107  else
108  {
109  dist = 0;
110  }
111 
112  return dist;
113 }
114 
115 
116 /** Remove point.
117  * @param pt point
118  */
119 void
120 FittedCircle::removePoint(const upoint_t& pt)
121 {
122  int next_circle = 1 - current_circle;
123  point_added = true;
124 
125  circle_matrices[next_circle].A00 -= 4 * pt.x * pt.x;
126  circle_matrices[next_circle].A01 -= 4 * pt.x * pt.y;
127  circle_matrices[next_circle].A02 -= 2 * pt.x;
128 
129  circle_matrices[next_circle].A10 -= 4 * pt.y * pt.x;
130  circle_matrices[next_circle].A11 -= 4 * pt.y * pt.y;
131  circle_matrices[next_circle].A12 -= 2 * pt.y;
132 
133  circle_matrices[next_circle].A20 -= 2 * pt.x;
134  circle_matrices[next_circle].A21 -= 2 * pt.y;
135  circle_matrices[next_circle].A22 -= 1;
136 
137  float r2 = pt.x * pt.x + pt.y * pt.y;
138  circle_matrices[next_circle].b0 -= 2 * r2 * pt.x;
139  circle_matrices[next_circle].b1 -= 2 * r2 * pt.y;
140  circle_matrices[next_circle].b2 -= r2;
141 }
142 
143 
144 /** Distance.
145  * @param pt point
146  * @param current current
147  * @return distance
148  */
149 float
150 FittedCircle::distanceTo(const upoint_t& pt, bool current)
151 {
152  int id = current?current_circle:(1-current_circle);
153  Circle* p = fitCircle(&circle_matrices[id]);
154  if (p)
155  {
156  float dx = p->center.x - pt.x;
157  float dy = p->center.y - pt.y;
158  return fabs(sqrt(dx*dx+dy*dy)-p->radius);
159  }
160  else
161  {
162  // There is no circle, perhaps it is a line...
163  return 100000.0f; // temporarily... should fit a line...
164  }
165 }
166 
167 
168 /** Commit. */
169 void
170 FittedCircle::commit(void)
171 {
172  if (point_added)
173  {
174  current_circle = 1 - current_circle;
175  point_added = false;
176  ++count;
177  }
178 }
179 
180 /** Get count.
181  * @return count
182  */
183 int
184 FittedCircle::getCount(void) const
185 {
186  return count;
187 }
188 
189 
190 /** Get circle.
191  * @return circle
192  */
193 Circle*
194 FittedCircle::getCircle(void) const
195 {
196  return fitCircle(const_cast<circle_matrix*>(&circle_matrices[current_circle]));
197 }
198 
199 
200 /** Fit circle.
201  * @param p circle matrix
202  * @return circle
203  */
204 Circle*
205 FittedCircle::fitCircle(circle_matrix* p) const
206 {
207  // solve the resulting 3 by 3 equations
208  static Circle c;
209  float delta = + p->A00 * p->A11 * p->A22 + p->A01 * p->A12 * p->A20 + p->A02 * p->A10 * p->A21
210  - p->A00 * p->A12 * p->A21 - p->A01 * p->A10 * p->A22 - p->A02 * p->A11 * p->A20;
211  if (delta > -TOO_SMALL_DELTA && delta < TOO_SMALL_DELTA)
212  {
213 printf("A=\n");
214 printf("\t%f\t%f\t%f\n", p->A00, p->A01, p->A02);
215 printf("\t%f\t%f\t%f\n", p->A10, p->A11, p->A12);
216 printf("\t%f\t%f\t%f\n", p->A20, p->A21, p->A22);
217 printf("b=\n");
218 printf("\t%f\t%f\t%f\n", p->b0, p->b1, p->b2);
219 printf("Delta too small: %e\n", delta);
220  return NULL;
221  }
222  else
223  {
224  c.center.x = (float)( ( + p->b0 * p->A11 * p->A22 + p->A01 * p->A12 * p->b2 + p->A02 * p->b1 * p->A21
225  - p->b0 * p->A12 * p->A21 - p->A01 * p->b1 * p->A22 - p->A02 * p->A11 * p->b2 ) / delta);
226  c.center.y = (float)( ( + p->A00 * p->b1 * p->A22 + p->b0 * p->A12 * p->A20 + p->A02 * p->A10 * p->b2
227  - p->A00 * p->A12 * p->b2 - p->b0 * p->A10 * p->A22 - p->A02 * p->b1 * p->A20 ) / delta);
228  c.radius = (float)sqrt((+ p->A00 * p->A11 * p->b2 + p->A01 * p->b1 * p->A20 + p->b0 * p->A10 * p->A21
229  - p->A00 * p->b1 * p->A21 - p->A01 * p->A10 * p->b2 - p->b0 * p->A11 * p->A20 ) / delta
230  + c.center.x * c.center.x + c.center.y * c.center.y);
231  c.count = count;
232  return &c;
233  }
234 }
235 
236 } // end namespace firevision
Fawkes library namespace.
unsigned int y
y coordinate
Definition: types.h:36
unsigned int x
x coordinate
Definition: types.h:35
float x
x in pixels
Definition: types.h:40
Circle shape.
Definition: circle.h:45
Point with cartesian coordinates as unsigned integers.
Definition: types.h:34
int count
Number of pixels.
Definition: circle.h:64
center_in_roi_t center
Center of object in ROI.
Definition: circle.h:60
float y
y in pixels
Definition: types.h:41
float radius
Radius of object.
Definition: circle.h:62