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