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
00026 #include <models/shape/rht_circle.h>
00027
00028 #include <cmath>
00029 #include <sys/time.h>
00030
00031 using namespace std;
00032 using namespace fawkes;
00033
00034 #define TEST_IF_IS_A_PIXEL(x) ((x)>230)
00035
00036 #define TBY_SQUARED_DIST(x1,y1,x2,y2) \
00037 (((x1)-(x2))*((x1)-(x2))+((y1)-(y2))*((y1)-(y2)))
00038 #define TBY_RADIUS_DIFF(x1, y1, x2, y2, r) \
00039 (sqrt(((x1)-(x2))*((x1)-(x2))+((y1)-(y2))*((y1)-(y2))-(r)*(r)))
00040
00041
00042 namespace firevision {
00043 #if 0
00044 }
00045 #endif
00046
00047 const float RhtCircleModel::RHT_MIN_RADIUS = 40.0;
00048 const float RhtCircleModel::RHT_MAX_RADIUS = 500.0;
00049
00050
00051
00052
00053
00054
00055 RhtCircleModel::RhtCircleModel(void)
00056 {
00057 }
00058
00059
00060
00061 RhtCircleModel::~RhtCircleModel(void)
00062 {
00063 m_Circles.clear();
00064 }
00065
00066
00067
00068
00069
00070
00071 int
00072 RhtCircleModel::parseImage( unsigned char* buf,
00073 ROI *roi )
00074 {
00075
00076 unsigned char *buffer = roi->get_roi_buffer_start(buf);
00077
00078 unsigned int x, y;
00079 vector<point_t> pixels;
00080 struct timeval start, end;
00081
00082
00083 accumulator.reset();
00084
00085
00086 m_Circles.clear();
00087
00088
00089 const int RHT_MAX_TIME = 10000;
00090 const int RHT_MAX_ITER = 1000;
00091
00092
00093 const float RHT_MIN_VOTE_RATE = 0.0f;
00094
00095
00096 const int RHT_XY_SCALE = 8;
00097 const int RHT_RADIUS_SCALE= 8;
00098
00099
00100
00101 const float RHT_FITTING_DIST_DIFF = 15.0f;
00102
00103
00104 const float ROI_HOLLOW_RATE = 0.70f;
00105
00106 const unsigned int roi_hollow_top = (int)(roi->height * ((1.0f - ROI_HOLLOW_RATE) / 2));
00107 const unsigned int roi_hollow_bottom = roi->height - roi_hollow_top;
00108 const unsigned int roi_hollow_left = (int)(roi->width * ((1.0f - ROI_HOLLOW_RATE) / 2));
00109 const unsigned int roi_hollow_right = roi->width - roi_hollow_left;
00110
00111
00112
00113
00114 unsigned char *line_start = buffer;
00115
00116 gettimeofday(&start, NULL);
00117 end.tv_usec = start.tv_usec;
00118
00119
00120 for (y = 0; y < roi_hollow_top; ++y) {
00121 for (x = 0; x < roi->width; ++x) {
00122 if (TEST_IF_IS_A_PIXEL(*buffer)) {
00123 point_t pt={x, y};
00124 pixels.push_back(pt);
00125 }
00126
00127 ++buffer;
00128 }
00129 line_start += roi->line_step;
00130 buffer = line_start;
00131 }
00132
00133 for (y = roi_hollow_top; y < roi_hollow_bottom; ++y) {
00134 for (x = 0; x < roi_hollow_left; ++x) {
00135 if (TEST_IF_IS_A_PIXEL(*buffer)) {
00136 point_t pt={x, y};
00137 pixels.push_back(pt);
00138 }
00139
00140 ++buffer;
00141 }
00142 buffer+=(roi_hollow_right - roi_hollow_left);
00143 for (x = roi_hollow_right; x < roi->width; ++x) {
00144 if (TEST_IF_IS_A_PIXEL(*buffer)) {
00145 point_t pt={x, y};
00146 pixels.push_back(pt);
00147 }
00148
00149 ++buffer;
00150 }
00151 line_start += roi->line_step;
00152 buffer = line_start;
00153 }
00154
00155 for (y = roi_hollow_bottom; y < roi->height; ++y) {
00156 for (x = 0; x < roi->width; ++x) {
00157 if (TEST_IF_IS_A_PIXEL(*buffer)) {
00158 point_t pt={x, y};
00159 pixels.push_back(pt);
00160 }
00161
00162 ++buffer;
00163 }
00164 line_start += roi->line_step;
00165 buffer = line_start;
00166 }
00167
00168
00169 point_t p[3];
00170 center_in_roi_t center;
00171 float radius;
00172 vector< point_t >::iterator pos;
00173 int num_iter = 0;
00174 int num_points = (int) pixels.size();
00175 if (num_points == 0) {
00176
00177 return 0;
00178 }
00179
00180 while( (num_iter++ < RHT_MAX_ITER) &&
00181 ( ((end.tv_usec - start.tv_usec) < RHT_MAX_TIME) ||
00182 ((end.tv_usec + 1000000 - start.tv_usec) < RHT_MAX_TIME) )
00183
00184 ) {
00185
00186
00187 for (int i=0; i < 3; ++i) {
00188 int ri = rand() % num_points;
00189 pos = pixels.begin() + ri;
00190 p[i] = *pos;
00191 }
00192
00193
00194
00195 calcCircle(p[0], p[1], p[2], center, radius);
00196
00197
00198 if (radius > RHT_MIN_RADIUS && radius < RHT_MAX_RADIUS) {
00199 accumulator.accumulate((int)(center.x / RHT_XY_SCALE),
00200 (int)(center.y / RHT_XY_SCALE),
00201 (int)(radius / RHT_RADIUS_SCALE));
00202 }
00203
00204 gettimeofday(&end, NULL);
00205 }
00206
00207
00208 int max, x_max, y_max, r_max;
00209 max = accumulator.getMax(x_max, y_max, r_max);
00210
00211 cout << "Max vote is with " << max << " of the " << num_iter << " ones." << endl;
00212
00213
00214 if (max > num_iter * RHT_MIN_VOTE_RATE)
00215 {
00216 center_in_roi_t center;
00217 center.x = (float)(x_max * RHT_XY_SCALE + RHT_XY_SCALE/2);
00218 center.y = (float)(y_max * RHT_XY_SCALE + RHT_XY_SCALE/2);
00219 float c_r = (float)(r_max * RHT_RADIUS_SCALE + RHT_RADIUS_SCALE/2);
00220
00221
00222 for(vector< point_t >::iterator pos = pixels.begin(); pos != pixels.end(); )
00223 {
00224 if (TBY_RADIUS_DIFF(pos->x, pos->y, center.x, center.y, c_r)
00225 > RHT_FITTING_DIST_DIFF)
00226 {
00227 pixels.erase(pos);
00228 }
00229 else
00230 {
00231 pos++;
00232 }
00233 }
00234
00235 Circle c;
00236 c.fitCircle(pixels);
00237 c.count = max;
00238 m_Circles.push_back(c);
00239
00240
00241
00242
00243
00244
00245 return 0;
00246 }
00247
00248
00249
00250
00251 return 1;
00252 }
00253
00254
00255 int
00256 RhtCircleModel::getShapeCount(void) const
00257 {
00258 return m_Circles.size();
00259 }
00260
00261 Circle *
00262 RhtCircleModel::getShape(int id) const
00263 {
00264 if (id < 0 || (unsigned int)id >= m_Circles.size()) {
00265 return NULL;
00266 } else {
00267 return const_cast<Circle*>(&m_Circles[id]);
00268 }
00269 }
00270
00271
00272 Circle *
00273 RhtCircleModel::getMostLikelyShape(void) const
00274 {
00275 if (m_Circles.size() == 0) {
00276 return NULL;
00277 } else if (m_Circles.size() == 1) {
00278 return const_cast<Circle*>(&m_Circles[0]);
00279 } else {
00280 int cur=0;
00281 for (unsigned int i=1; i < m_Circles.size(); ++i) {
00282 if (m_Circles[i].count > m_Circles[cur].count) {
00283 cur = i;
00284 }
00285 }
00286 return const_cast<Circle*>(&m_Circles[cur]);
00287 }
00288 }
00289
00290
00291 void
00292 RhtCircleModel::calcCircle(
00293 const point_t& p1,
00294 const point_t& p2,
00295 const point_t& p3,
00296 center_in_roi_t& center,
00297 float& radius)
00298
00299
00300
00301 {
00302 const int &x1=p1.x, &y1=p1.y, &x2=p2.x, &y2=p2.y, &x3=p3.x, &y3=p3.y;
00303 float dx, dy;
00304 int div = 2*((x2-x1)*(y3-y1)-(x3-x1)*(y2-y1));
00305
00306 if (div == 0)
00307 {
00308
00309 radius = -1.0;
00310 return;
00311 }
00312 center.x = ((float)((x2*x2+y2*y2-x1*x1-y1*y1)*(y3-y1)
00313 -(x3*x3+y3*y3-x1*x1-y1*y1)*(y2-y1))
00314 /div);
00315 center.y = ((float)((x2-x1)*(x3*x3+y3*y3-x1*x1-y1*y1)
00316 -(x3-x1)*(x2*x2+y2*y2-x1*x1-y1*y1))
00317 /div);
00318 dx = center.x - x1;
00319 dy = center.y - y1;
00320 radius = (float)sqrt(dx*dx+dy*dy);
00321 }
00322
00323 }