Fawkes API Fawkes Development Version

fit_accum.cpp

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 namespace firevision {
00032 #if 0 /* just to make Emacs auto-indent happy */
00033 }
00034 #endif
00035 
00036 const float FitAccum::TOO_SMALL_DELTA = 1.0e-3f;
00037 
00038 /** @class FitAccum <models/shape/accumulators/fit_accum.h>
00039  * FIT Accumulator.
00040  */
00041 
00042 /** Constructor. */
00043 FitAccum::FitAccum(void)
00044 {
00045         reset();
00046 }
00047 
00048 /** Destructor. */
00049 FitAccum::~FitAccum(void)
00050 {
00051 }
00052 
00053 /** Reset. */
00054 void
00055 FitAccum::reset(void)
00056 {
00057         count = 0;
00058         A00 = A01 = A02 = 0.0f;
00059         A10 = A11 = A12 = 0.0f;
00060         A20 = A21 = A22 = 0.0f;
00061         b0  = b1  = b2  = 0.0f;
00062 }
00063 
00064 /** Add point.
00065  * @param pt point
00066  */
00067 void
00068 FitAccum::addPoint(const point_t& pt)
00069 {
00070         ++count;
00071 
00072         A00 += 4 * pt.x * pt.x;
00073         A01 += 4 * pt.x * pt.y;
00074         A02 += 2 * pt.x;
00075 
00076         A10 += 4 * pt.y * pt.x;
00077         A11 += 4 * pt.y * pt.y;
00078         A12 += 2 * pt.y;
00079 
00080         A20 += 2 * pt.x;
00081         A21 += 2 * pt.y;
00082         A22 += 1;
00083 
00084         float r2 = pt.x * pt.x + pt.y * pt.y;
00085         b0  += 2 * r2 * pt.x;
00086         b1  += 2 * r2 * pt.y;
00087         b2  += r2;
00088 }
00089 
00090 
00091 /** Remove point.
00092  * @param pt point
00093  */
00094 void
00095 FitAccum::removePoint(const point_t& pt)
00096 {
00097         --count;
00098         A00 -= 4 * pt.x * pt.x;
00099         A01 -= 4 * pt.x * pt.y;
00100         A02 -= 2 * pt.x;
00101 
00102         A10 -= 4 * pt.y * pt.x;
00103         A11 -= 4 * pt.y * pt.y;
00104         A12 -= 2 * pt.y;
00105 
00106         A20 -= 2 * pt.x;
00107         A21 -= 2 * pt.y;
00108         A22 -= 1;
00109 
00110         float r2 = pt.x * pt.x + pt.y * pt.y;
00111         b0  -= 2 * r2 * pt.x;
00112         b1  -= 2 * r2 * pt.y;
00113         b2  -= r2;
00114 }
00115 
00116 /** Get count.
00117  * @return count
00118  */
00119 int
00120 FitAccum::getCount(void) const
00121 {
00122         return count;
00123 }
00124 
00125 /** Get circle.
00126  * @return circle
00127  */
00128 Circle*
00129 FitAccum::getCircle(void) const
00130 {
00131         // solve the resulting 3 by 3 equations
00132         static Circle c;
00133 
00134         float delta =           + A00 * A11 * A22 + A01 * A12 * A20 + A02 * A10 * A21
00135                                 - A00 * A12 * A21 - A01 * A10 * A22 - A02 * A11 * A20;
00136 
00137         if (delta > -TOO_SMALL_DELTA && delta < TOO_SMALL_DELTA)
00138         {
00139 // printf("A=\n");
00140 // printf("\t%f\t%f\t%f\n", A00, A01, A02);
00141 // printf("\t%f\t%f\t%f\n", A10, A11, A12);
00142 // printf("\t%f\t%f\t%f\n", A20, A21, A22);
00143 // printf("b=\n");
00144 // printf("\t%f\t%f\t%f\n", b0, b1, b2);
00145 // printf("Delta too small: %e\n", delta);
00146                 return NULL;
00147         }
00148         else
00149         {
00150                 c.center.x = (float)( ( + b0  * A11 * A22 + A01 * A12 * b2  + A02 * b1  * A21
00151                                         - b0  * A12 * A21 - A01 * b1  * A22 - A02 * A11 * b2  ) / delta);
00152                 c.center.y = (float)( ( + A00 * b1  * A22 + b0  * A12 * A20 + A02 * A10 * b2
00153                                         - A00 * A12 * b2  - b0  * A10 * A22 - A02 * b1  * A20 ) / delta);
00154                 c.radius = (float)sqrt((+ A00 * A11 * b2  + A01 * b1  * A20 + b0  * A10 * A21
00155                                         - A00 * b1  * A21 - A01 * A10 * b2  - b0  * A11 * A20 ) / delta
00156                                         + c.center.x * c.center.x + c.center.y * c.center.y);
00157                 c.count = count;
00158                 return &c;
00159         }
00160 }
00161 
00162 } // end namespace firevision
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends