00001 /***************************************************************************
00002  *  fit_accum.cpp - Implementation of 'fitted circle' accumulator
00003  *                  used by Fix-Point RCD Algorithm
00004  *
00005  *  Generated: Sat Sep 10 2005 17:28: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/fit_accum.h>
00025
00026 #include <models/shape/circle.h>
00027 #include <cmath>
00028
00029 using namespace fawkes;
00030
00031 const float FitAccum::TOO_SMALL_DELTA = 1.0e-3f;
00032 
00033 /** @class FitAccum <models/shape/accumulators/fit_accum.h>
00034  * FIT Accumulator.
00035  */
00036 
00037 /** Constructor. */
00038 FitAccum::FitAccum(void)
00039 {
00040         reset();
00041 }
00042 
00043 /** Destructor. */
00044 FitAccum::~FitAccum(void)
00045 {
00046 }
00047 
00048 /** Reset. */
00049 void
00050 FitAccum::reset(void)
00051 {
00052         count = 0;
00053         A00 = A01 = A02 = 0.0f;
00054         A10 = A11 = A12 = 0.0f;
00055         A20 = A21 = A22 = 0.0f;
00056         b0  = b1  = b2  = 0.0f;
00057 }
00058 
00059 /** Add point.
00060  * @param pt point
00061  */
00062 void
00063 FitAccum::addPoint(const point_t& pt)
00064 {
00065         ++count;
00066
00067         A00 += 4 * pt.x * pt.x;
00068         A01 += 4 * pt.x * pt.y;
00069         A02 += 2 * pt.x;
00070
00071         A10 += 4 * pt.y * pt.x;
00072         A11 += 4 * pt.y * pt.y;
00073         A12 += 2 * pt.y;
00074
00075         A20 += 2 * pt.x;
00076         A21 += 2 * pt.y;
00077         A22 += 1;
00078
00079         float r2 = pt.x * pt.x + pt.y * pt.y;
00080         b0  += 2 * r2 * pt.x;
00081         b1  += 2 * r2 * pt.y;
00082         b2  += r2;
00083 }
00084
00085 
00086 /** Remove point.
00087  * @param pt point
00088  */
00089 void
00090 FitAccum::removePoint(const point_t& pt)
00091 {
00092         --count;
00093         A00 -= 4 * pt.x * pt.x;
00094         A01 -= 4 * pt.x * pt.y;
00095         A02 -= 2 * pt.x;
00096
00097         A10 -= 4 * pt.y * pt.x;
00098         A11 -= 4 * pt.y * pt.y;
00099         A12 -= 2 * pt.y;
00100
00101         A20 -= 2 * pt.x;
00102         A21 -= 2 * pt.y;
00103         A22 -= 1;
00104
00105         float r2 = pt.x * pt.x + pt.y * pt.y;
00106         b0  -= 2 * r2 * pt.x;
00107         b1  -= 2 * r2 * pt.y;
00108         b2  -= r2;
00109 }
00110 
00111 /** Get count.
00112  * @return count
00113  */
00114 int
00115 FitAccum::getCount(void) const
00116 {
00117         return count;
00118 }
00119 
00120 /** Get circle.
00121  * @return circle
00122  */
00123 Circle*
00124 FitAccum::getCircle(void) const
00125 {
00126         // solve the resulting 3 by 3 equations
00127         static Circle c;
00128
00129         float delta =           + A00 * A11 * A22 + A01 * A12 * A20 + A02 * A10 * A21
00130                                 - A00 * A12 * A21 - A01 * A10 * A22 - A02 * A11 * A20;
00131
00132         if (delta > -TOO_SMALL_DELTA && delta < TOO_SMALL_DELTA)
00133         {
00134 // printf("A=\n");
00135 // printf("\t%f\t%f\t%f\n", A00, A01, A02);
00136 // printf("\t%f\t%f\t%f\n", A10, A11, A12);
00137 // printf("\t%f\t%f\t%f\n", A20, A21, A22);
00138 // printf("b=\n");
00139 // printf("\t%f\t%f\t%f\n", b0, b1, b2);
00140 // printf("Delta too small: %e\n", delta);
00141                 return NULL;
00142         }
00143         else
00144         {
00145                 c.center.x = (float)( ( + b0  * A11 * A22 + A01 * A12 * b2  + A02 * b1  * A21
00146                                         - b0  * A12 * A21 - A01 * b1  * A22 - A02 * A11 * b2  ) / delta);
00147                 c.center.y = (float)( ( + A00 * b1  * A22 + b0  * A12 * A20 + A02 * A10 * b2
00148                                         - A00 * A12 * b2  - b0  * A10 * A22 - A02 * b1  * A20 ) / delta);
00149                 c.radius = (float)sqrt((+ A00 * A11 * b2  + A01 * b1  * A20 + b0  * A10 * A21
00150                                         - A00 * b1  * A21 - A01 * A10 * b2  - b0  * A11 * A20 ) / delta
00151                                         + c.center.x * c.center.x + c.center.y * c.center.y);
00152                 c.count = count;
00153                 return &c;
00154         }
00155 }
00156