Fawkes API  Fawkes Development Version
fit_accum.cpp
1 /***************************************************************************
2  * fit_accum.cpp - Implementation of 'fitted circle' accumulator
3  * used by Fix-Point RCD Algorithm
4  *
5  * Generated: Sat Sep 10 2005 17:28: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/fit_accum.h>
25 #include <fvmodels/shape/circle.h>
26 
27 #include <cmath>
28 
29 using namespace fawkes;
30 
31 namespace firevision {
32 
33 const float FitAccum::TOO_SMALL_DELTA = 1.0e-3f;
34 
35 /** @class FitAccum <fvmodels/shape/accumulators/fit_accum.h>
36  * FIT Accumulator.
37  */
38 
39 /** Constructor. */
40 FitAccum::FitAccum(void)
41 {
42  reset();
43 }
44 
45 /** Destructor. */
46 FitAccum::~FitAccum(void)
47 {
48 }
49 
50 /** Reset. */
51 void
52 FitAccum::reset(void)
53 {
54  count = 0;
55  A00 = A01 = A02 = 0.0f;
56  A10 = A11 = A12 = 0.0f;
57  A20 = A21 = A22 = 0.0f;
58  b0 = b1 = b2 = 0.0f;
59 }
60 
61 /** Add point.
62  * @param pt point
63  */
64 void
65 FitAccum::addPoint(const upoint_t &pt)
66 {
67  ++count;
68 
69  A00 += 4 * pt.x * pt.x;
70  A01 += 4 * pt.x * pt.y;
71  A02 += 2 * pt.x;
72 
73  A10 += 4 * pt.y * pt.x;
74  A11 += 4 * pt.y * pt.y;
75  A12 += 2 * pt.y;
76 
77  A20 += 2 * pt.x;
78  A21 += 2 * pt.y;
79  A22 += 1;
80 
81  float r2 = pt.x * pt.x + pt.y * pt.y;
82  b0 += 2 * r2 * pt.x;
83  b1 += 2 * r2 * pt.y;
84  b2 += r2;
85 }
86 
87 /** Remove point.
88  * @param pt point
89  */
90 void
91 FitAccum::removePoint(const upoint_t &pt)
92 {
93  --count;
94  A00 -= 4 * pt.x * pt.x;
95  A01 -= 4 * pt.x * pt.y;
96  A02 -= 2 * pt.x;
97 
98  A10 -= 4 * pt.y * pt.x;
99  A11 -= 4 * pt.y * pt.y;
100  A12 -= 2 * pt.y;
101 
102  A20 -= 2 * pt.x;
103  A21 -= 2 * pt.y;
104  A22 -= 1;
105 
106  float r2 = pt.x * pt.x + pt.y * pt.y;
107  b0 -= 2 * r2 * pt.x;
108  b1 -= 2 * r2 * pt.y;
109  b2 -= r2;
110 }
111 
112 /** Get count.
113  * @return count
114  */
115 int
116 FitAccum::getCount(void) const
117 {
118  return count;
119 }
120 
121 /** Get circle.
122  * @return circle
123  */
124 Circle *
125 FitAccum::getCircle(void) const
126 {
127  // solve the resulting 3 by 3 equations
128  static Circle c;
129 
130  float delta = +A00 * A11 * A22 + A01 * A12 * A20 + A02 * A10 * A21 - A00 * A12 * A21
131  - A01 * A10 * A22 - A02 * A11 * A20;
132 
133  if (delta > -TOO_SMALL_DELTA && delta < TOO_SMALL_DELTA) {
134  // printf("A=\n");
135  // printf("\t%f\t%f\t%f\n", A00, A01, A02);
136  // printf("\t%f\t%f\t%f\n", A10, A11, A12);
137  // printf("\t%f\t%f\t%f\n", A20, A21, A22);
138  // printf("b=\n");
139  // printf("\t%f\t%f\t%f\n", b0, b1, b2);
140  // printf("Delta too small: %e\n", delta);
141  return NULL;
142  } else {
143  c.center.x = (float)((+b0 * A11 * A22 + A01 * A12 * b2 + A02 * b1 * A21 - b0 * A12 * A21
144  - A01 * b1 * A22 - A02 * A11 * b2)
145  / delta);
146  c.center.y = (float)((+A00 * b1 * A22 + b0 * A12 * A20 + A02 * A10 * b2 - A00 * A12 * b2
147  - b0 * A10 * A22 - A02 * b1 * A20)
148  / delta);
149  c.radius = (float)sqrt((+A00 * A11 * b2 + A01 * b1 * A20 + b0 * A10 * A21 - A00 * b1 * A21
150  - A01 * A10 * b2 - b0 * A11 * A20)
151  / delta
152  + c.center.x * c.center.x + c.center.y * c.center.y);
153  c.count = count;
154  return &c;
155  }
156 }
157 
158 } // end namespace firevision
firevision::Circle
Circle shape.
Definition: circle.h:43
fawkes::upoint_t
Point with cartesian coordinates as unsigned integers.
Definition: types.h:35
firevision::Circle::center
center_in_roi_t center
Center of object in ROI.
Definition: circle.h:57
firevision::Circle::radius
float radius
Radius of object.
Definition: circle.h:59
fawkes
Fawkes library namespace.
fawkes::upoint_t::y
unsigned int y
y coordinate
Definition: types.h:37
firevision::Circle::count
int count
Number of pixels.
Definition: circle.h:61
firevision::center_in_roi_t::x
float x
x in pixels
Definition: types.h:39
firevision::center_in_roi_t::y
float y
y in pixels
Definition: types.h:40
fawkes::upoint_t::x
unsigned int x
x coordinate
Definition: types.h:36