Fawkes API
Fawkes Development Version
|
00001 /*************************************************************************** 00002 * fc_accum.cpp - Implementation of 'fitted circle' accumulator 00003 * used by Randomized Stable Circle Fitting Algorithm 00004 * 00005 * Generated: Fri Sep 09 2005 22:50:12 00006 * Copyright 2005 Hu Yuxiao <Yuxiao.Hu@rwth-aachen.de> 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. A runtime exception applies to 00014 * this software (see LICENSE.GPL_WRE file mentioned below for details). 00015 * 00016 * This program is distributed in the hope that it will be useful, 00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00019 * GNU Library General Public License for more details. 00020 * 00021 * Read the full text in the LICENSE.GPL_WRE file in the doc directory. 00022 */ 00023 00024 #include <fvmodels/shape/accumulators/fc_accum.h> 00025 00026 #include <cmath> 00027 #include <cstdio> 00028 00029 using namespace fawkes; 00030 00031 namespace firevision { 00032 #if 0 /* just to make Emacs auto-indent happy */ 00033 } 00034 #endif 00035 00036 const float FittedCircle::TOO_SMALL_DELTA = 1.0e-3f; 00037 00038 /** @class FittedCircle <fvmodels/shape/accumulators/fc_accum.h> 00039 * FittedCircle accumulator. 00040 */ 00041 00042 /** Constructor. */ 00043 FittedCircle::FittedCircle(void) 00044 { 00045 reset(); 00046 } 00047 00048 /** Destructor. */ 00049 FittedCircle::~FittedCircle(void) 00050 { 00051 } 00052 00053 /** Reset. */ 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 /** Add point. 00071 * @param pt point 00072 * @return distance from circle center 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 /** Remove point. 00117 * @param pt point 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 /** Distance. 00145 * @param pt point 00146 * @param current current 00147 * @return distance 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 // There is no circle, perhaps it is a line... 00163 return 100000.0f; // temporarily... should fit a line... 00164 } 00165 } 00166 00167 00168 /** Commit. */ 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 /** Get count. 00181 * @return count 00182 */ 00183 int 00184 FittedCircle::getCount(void) const 00185 { 00186 return count; 00187 } 00188 00189 00190 /** Get circle. 00191 * @return circle 00192 */ 00193 Circle* 00194 FittedCircle::getCircle(void) const 00195 { 00196 return fitCircle(const_cast<circle_matrix*>(&circle_matrices[current_circle])); 00197 } 00198 00199 00200 /** Fit circle. 00201 * @param p circle matrix 00202 * @return circle 00203 */ 00204 Circle* 00205 FittedCircle::fitCircle(circle_matrix* p) const 00206 { 00207 // solve the resulting 3 by 3 equations 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 } // end namespace firevision