00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #include <models/shape/accumulators/fit_accum.h>
00025
00026 #include <models/shape/circle.h>
00027 #include <cmath>
00028
00029 using namespace fawkes;
00030
00031 namespace firevision {
00032 #if 0
00033 }
00034 #endif
00035
00036 const float FitAccum::TOO_SMALL_DELTA = 1.0e-3f;
00037
00038
00039
00040
00041
00042
00043 FitAccum::FitAccum(void)
00044 {
00045 reset();
00046 }
00047
00048
00049 FitAccum::~FitAccum(void)
00050 {
00051 }
00052
00053
00054 void
00055 FitAccum::reset(void)
00056 {
00057 count = 0;
00058 A00 = A01 = A02 = 0.0f;
00059 A10 = A11 = A12 = 0.0f;
00060 A20 = A21 = A22 = 0.0f;
00061 b0 = b1 = b2 = 0.0f;
00062 }
00063
00064
00065
00066
00067 void
00068 FitAccum::addPoint(const point_t& pt)
00069 {
00070 ++count;
00071
00072 A00 += 4 * pt.x * pt.x;
00073 A01 += 4 * pt.x * pt.y;
00074 A02 += 2 * pt.x;
00075
00076 A10 += 4 * pt.y * pt.x;
00077 A11 += 4 * pt.y * pt.y;
00078 A12 += 2 * pt.y;
00079
00080 A20 += 2 * pt.x;
00081 A21 += 2 * pt.y;
00082 A22 += 1;
00083
00084 float r2 = pt.x * pt.x + pt.y * pt.y;
00085 b0 += 2 * r2 * pt.x;
00086 b1 += 2 * r2 * pt.y;
00087 b2 += r2;
00088 }
00089
00090
00091
00092
00093
00094 void
00095 FitAccum::removePoint(const point_t& pt)
00096 {
00097 --count;
00098 A00 -= 4 * pt.x * pt.x;
00099 A01 -= 4 * pt.x * pt.y;
00100 A02 -= 2 * pt.x;
00101
00102 A10 -= 4 * pt.y * pt.x;
00103 A11 -= 4 * pt.y * pt.y;
00104 A12 -= 2 * pt.y;
00105
00106 A20 -= 2 * pt.x;
00107 A21 -= 2 * pt.y;
00108 A22 -= 1;
00109
00110 float r2 = pt.x * pt.x + pt.y * pt.y;
00111 b0 -= 2 * r2 * pt.x;
00112 b1 -= 2 * r2 * pt.y;
00113 b2 -= r2;
00114 }
00115
00116
00117
00118
00119 int
00120 FitAccum::getCount(void) const
00121 {
00122 return count;
00123 }
00124
00125
00126
00127
00128 Circle*
00129 FitAccum::getCircle(void) const
00130 {
00131
00132 static Circle c;
00133
00134 float delta = + A00 * A11 * A22 + A01 * A12 * A20 + A02 * A10 * A21
00135 - A00 * A12 * A21 - A01 * A10 * A22 - A02 * A11 * A20;
00136
00137 if (delta > -TOO_SMALL_DELTA && delta < TOO_SMALL_DELTA)
00138 {
00139
00140
00141
00142
00143
00144
00145
00146 return NULL;
00147 }
00148 else
00149 {
00150 c.center.x = (float)( ( + b0 * A11 * A22 + A01 * A12 * b2 + A02 * b1 * A21
00151 - b0 * A12 * A21 - A01 * b1 * A22 - A02 * A11 * b2 ) / delta);
00152 c.center.y = (float)( ( + A00 * b1 * A22 + b0 * A12 * A20 + A02 * A10 * b2
00153 - A00 * A12 * b2 - b0 * A10 * A22 - A02 * b1 * A20 ) / delta);
00154 c.radius = (float)sqrt((+ A00 * A11 * b2 + A01 * b1 * A20 + b0 * A10 * A21
00155 - A00 * b1 * A21 - A01 * A10 * b2 - b0 * A11 * A20 ) / delta
00156 + c.center.x * c.center.x + c.center.y * c.center.y);
00157 c.count = count;
00158 return &c;
00159 }
00160 }
00161
00162 }