Fawkes API
Fawkes Development Version
|
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