Fawkes API  Fawkes Development Version
circle.cpp
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 <fvmodels/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 <fvmodels/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