fc_accum.cpp

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 <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 /* 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 <models/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

Generated on 1 Mar 2011 for Fawkes API by  doxygen 1.6.1