fc_accum.cpp
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/fc_accum.h>
00025
00026 #include <cmath>
00027 #include <cstdio>
00028
00029 using namespace fawkes;
00030
00031 namespace firevision {
00032 #if 0
00033 }
00034 #endif
00035
00036 const float FittedCircle::TOO_SMALL_DELTA = 1.0e-3f;
00037
00038
00039
00040
00041
00042
00043 FittedCircle::FittedCircle(void)
00044 {
00045 reset();
00046 }
00047
00048
00049 FittedCircle::~FittedCircle(void)
00050 {
00051 }
00052
00053
00054 void
00055 FittedCircle::reset(void)
00056 {
00057 count = 0;
00058 for (int i=0; i<2; ++i)
00059 {
00060 circle_matrices[i].A00 = circle_matrices[i].A01 = circle_matrices[i].A02 = 0.0f;
00061 circle_matrices[i].A10 = circle_matrices[i].A11 = circle_matrices[i].A12 = 0.0f;
00062 circle_matrices[i].A20 = circle_matrices[i].A21 = circle_matrices[i].A22 = 0.0f;
00063 circle_matrices[i].b0 = circle_matrices[i].b1 = circle_matrices[i].b2 = 0.0f;
00064 }
00065 current_circle = 0;
00066 point_added = false;
00067 }
00068
00069
00070
00071
00072
00073
00074 float
00075 FittedCircle::addPoint(const point_t& pt)
00076 {
00077 int next_circle = 1 - current_circle;
00078 point_added = true;
00079
00080 circle_matrices[next_circle].A00 += 4 * pt.x * pt.x;
00081 circle_matrices[next_circle].A01 += 4 * pt.x * pt.y;
00082 circle_matrices[next_circle].A02 += 2 * pt.x;
00083
00084 circle_matrices[next_circle].A10 += 4 * pt.y * pt.x;
00085 circle_matrices[next_circle].A11 += 4 * pt.y * pt.y;
00086 circle_matrices[next_circle].A12 += 2 * pt.y;
00087
00088 circle_matrices[next_circle].A20 += 2 * pt.x;
00089 circle_matrices[next_circle].A21 += 2 * pt.y;
00090 circle_matrices[next_circle].A22 += 1;
00091
00092 float r2 = pt.x * pt.x + pt.y * pt.y;
00093 circle_matrices[next_circle].b0 += 2 * r2 * pt.x;
00094 circle_matrices[next_circle].b1 += 2 * r2 * pt.y;
00095 circle_matrices[next_circle].b2 += r2;
00096
00097 float dist;
00098
00099 Circle* p = fitCircle(&circle_matrices[next_circle]);
00100 if (p)
00101 {
00102 float dx = p->center.x - pt.x;
00103 float dy = p->center.y - pt.y;
00104 float r = p->radius;
00105 dist = fabs(sqrt(dx*dx+dy*dy)-r);
00106 }
00107 else
00108 {
00109 dist = 0;
00110 }
00111
00112 return dist;
00113 }
00114
00115
00116
00117
00118
00119 void
00120 FittedCircle::removePoint(const point_t& pt)
00121 {
00122 int next_circle = 1 - current_circle;
00123 point_added = true;
00124
00125 circle_matrices[next_circle].A00 -= 4 * pt.x * pt.x;
00126 circle_matrices[next_circle].A01 -= 4 * pt.x * pt.y;
00127 circle_matrices[next_circle].A02 -= 2 * pt.x;
00128
00129 circle_matrices[next_circle].A10 -= 4 * pt.y * pt.x;
00130 circle_matrices[next_circle].A11 -= 4 * pt.y * pt.y;
00131 circle_matrices[next_circle].A12 -= 2 * pt.y;
00132
00133 circle_matrices[next_circle].A20 -= 2 * pt.x;
00134 circle_matrices[next_circle].A21 -= 2 * pt.y;
00135 circle_matrices[next_circle].A22 -= 1;
00136
00137 float r2 = pt.x * pt.x + pt.y * pt.y;
00138 circle_matrices[next_circle].b0 -= 2 * r2 * pt.x;
00139 circle_matrices[next_circle].b1 -= 2 * r2 * pt.y;
00140 circle_matrices[next_circle].b2 -= r2;
00141 }
00142
00143
00144
00145
00146
00147
00148
00149 float
00150 FittedCircle::distanceTo(const point_t& pt, bool current)
00151 {
00152 int id = current?current_circle:(1-current_circle);
00153 Circle* p = fitCircle(&circle_matrices[id]);
00154 if (p)
00155 {
00156 float dx = p->center.x - pt.x;
00157 float dy = p->center.y - pt.y;
00158 return fabs(sqrt(dx*dx+dy*dy)-p->radius);
00159 }
00160 else
00161 {
00162
00163 return 100000.0f;
00164 }
00165 }
00166
00167
00168
00169 void
00170 FittedCircle::commit(void)
00171 {
00172 if (point_added)
00173 {
00174 current_circle = 1 - current_circle;
00175 point_added = false;
00176 ++count;
00177 }
00178 }
00179
00180
00181
00182
00183 int
00184 FittedCircle::getCount(void) const
00185 {
00186 return count;
00187 }
00188
00189
00190
00191
00192
00193 Circle*
00194 FittedCircle::getCircle(void) const
00195 {
00196 return fitCircle(const_cast<circle_matrix*>(&circle_matrices[current_circle]));
00197 }
00198
00199
00200
00201
00202
00203
00204 Circle*
00205 FittedCircle::fitCircle(circle_matrix* p) const
00206 {
00207
00208 static Circle c;
00209 float delta = + p->A00 * p->A11 * p->A22 + p->A01 * p->A12 * p->A20 + p->A02 * p->A10 * p->A21
00210 - p->A00 * p->A12 * p->A21 - p->A01 * p->A10 * p->A22 - p->A02 * p->A11 * p->A20;
00211 if (delta > -TOO_SMALL_DELTA && delta < TOO_SMALL_DELTA)
00212 {
00213 printf("A=\n");
00214 printf("\t%f\t%f\t%f\n", p->A00, p->A01, p->A02);
00215 printf("\t%f\t%f\t%f\n", p->A10, p->A11, p->A12);
00216 printf("\t%f\t%f\t%f\n", p->A20, p->A21, p->A22);
00217 printf("b=\n");
00218 printf("\t%f\t%f\t%f\n", p->b0, p->b1, p->b2);
00219 printf("Delta too small: %e\n", delta);
00220 return NULL;
00221 }
00222 else
00223 {
00224 c.center.x = (float)( ( + p->b0 * p->A11 * p->A22 + p->A01 * p->A12 * p->b2 + p->A02 * p->b1 * p->A21
00225 - p->b0 * p->A12 * p->A21 - p->A01 * p->b1 * p->A22 - p->A02 * p->A11 * p->b2 ) / delta);
00226 c.center.y = (float)( ( + p->A00 * p->b1 * p->A22 + p->b0 * p->A12 * p->A20 + p->A02 * p->A10 * p->b2
00227 - p->A00 * p->A12 * p->b2 - p->b0 * p->A10 * p->A22 - p->A02 * p->b1 * p->A20 ) / delta);
00228 c.radius = (float)sqrt((+ p->A00 * p->A11 * p->b2 + p->A01 * p->b1 * p->A20 + p->b0 * p->A10 * p->A21
00229 - p->A00 * p->b1 * p->A21 - p->A01 * p->A10 * p->b2 - p->b0 * p->A11 * p->A20 ) / delta
00230 + c.center.x * c.center.x + c.center.y * c.center.y);
00231 c.count = count;
00232 return &c;
00233 }
00234 }
00235
00236 }