Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * circle.cpp - Implementation of a circle shape finder 00004 * 00005 * Created: Thu May 16 00:00:00 2005 00006 * Copyright 2005 Tim Niemueller [www.niemueller.de] 00007 * Hu Yuxiao <Yuxiao.Hu@rwth-aachen.de> 00008 * 00009 ****************************************************************************/ 00010 00011 /* This program is free software; you can redistribute it and/or modify 00012 * it under the terms of the GNU General Public License as published by 00013 * the Free Software Foundation; either version 2 of the License, or 00014 * (at your option) any later version. A runtime exception applies to 00015 * this software (see LICENSE.GPL_WRE file mentioned below for details). 00016 * 00017 * This program is distributed in the hope that it will be useful, 00018 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00019 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00020 * GNU Library General Public License for more details. 00021 * 00022 * Read the full text in the LICENSE.GPL_WRE file in the doc directory. 00023 */ 00024 00025 #include <cmath> 00026 #include <models/shape/circle.h> 00027 00028 using namespace std; 00029 using namespace fawkes; 00030 00031 namespace firevision { 00032 #if 0 /* just to make Emacs auto-indent happy */ 00033 } 00034 #endif 00035 00036 /** @class Circle <models/shape/circle.h> 00037 * Circle shape. 00038 */ 00039 00040 /** Constructor. */ 00041 Circle::Circle() 00042 { 00043 center.x=center.y=0.0f; 00044 radius = -1.0f; 00045 count = 0; 00046 } 00047 00048 /** Constructor. 00049 * @param c center 00050 * @param r radius 00051 * @param n number of pixels 00052 */ 00053 Circle::Circle(const center_in_roi_t& c, float r, int n) 00054 { 00055 center = c; 00056 radius = r; 00057 count = n; 00058 } 00059 00060 /** Print info. 00061 * @param stream stream to print to 00062 */ 00063 void 00064 Circle::printToStream(std::ostream &stream) 00065 { 00066 stream << "center=(" << center.x << "," << center.y << ")" 00067 << " radius=" << radius << " count= " << count; 00068 } 00069 00070 /** Fit circle. 00071 * Fit a circle through the given points. 00072 * @param points points to fit circle through. 00073 */ 00074 void 00075 Circle::fitCircle (vector< point_t > &points) 00076 { 00077 // due to fixed width, do not use arrays to save addressing time... 00078 double A00=0.0, A01=0.0, A02=0.0; 00079 double A10=0.0, A11=0.0, A12=0.0; 00080 double A20=0.0, A21=0.0, A22=0.0; 00081 double b0 =0.0, b1 =0.0, b2 =0.0; 00082 00083 // generating A'A and A'b 00084 int count = points.size(); 00085 for (int i = 0; i < count; i++) 00086 { 00087 point_t &t = points[i]; 00088 double x0 = 2.0f * t.x; 00089 double y0 = 2.0f * t.y; 00090 double b = (double)(t.x * t.x + t.y * t.y); 00091 A00 += x0 * x0; 00092 A01 += x0 * y0; 00093 A02 += x0; 00094 A10 += y0 * x0; 00095 A11 += y0 * y0; 00096 A12 += y0; 00097 A20 += x0; 00098 A21 += y0; 00099 A22 += 1.0; 00100 b0 += x0 * b; 00101 b1 += y0 * b; 00102 b2 += b; 00103 } 00104 00105 // solve the resulting 3 by 3 equations 00106 double delta = + A00 * A11 * A22 + A01 * A12 * A20 + A02 * A10 * A21 00107 - A00 * A12 * A21 - A01 * A10 * A22 - A02 * A11 * A20; 00108 center.x = (float)( ( + b0 * A11 * A22 + A01 * A12 * b2 + A02 * b1 * A21 00109 - b0 * A12 * A21 - A01 * b1 * A22 - A02 * A11 * b2 ) / delta); 00110 center.y = (float)( ( + A00 * b1 * A22 + b0 * A12 * A20 + A02 * A10 * b2 00111 - A00 * A12 * b2 - b0 * A10 * A22 - A02 * b1 * A20 ) / delta); 00112 radius = (float)sqrt( ( + A00 * A11 * b2 + A01 * b1 * A20 + b0 * A10 * A21 00113 - A00 * b1 * A21 - A01 * A10 * b2 - b0 * A11 * A20 ) / delta 00114 + center.x * center.x + center.y * center.y); 00115 count = points.size(); 00116 } 00117 00118 00119 void 00120 Circle::setMargin( unsigned int margin ) 00121 { 00122 this->margin = margin; 00123 } 00124 00125 00126 bool 00127 Circle::isClose( unsigned int in_roi_x, unsigned int in_roi_y ) 00128 { 00129 float dx = in_roi_x - center.x; 00130 float dy = in_roi_y - center.y; 00131 00132 float dist = sqrt( dx * dx + dy * dy ); 00133 00134 return ( (dist <= (radius + margin)) && 00135 (dist >= (radius - margin)) ); 00136 00137 } 00138 00139 } // end namespace firevision