00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
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
00033 }
00034 #endif
00035
00036
00037
00038
00039
00040
00041 Circle::Circle()
00042 {
00043 center.x=center.y=0.0f;
00044 radius = -1.0f;
00045 count = 0;
00046 }
00047
00048
00049
00050
00051
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
00061
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
00071
00072
00073
00074 void
00075 Circle::fitCircle (vector< point_t > &points)
00076 {
00077
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
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
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 }