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 <models/shape/rcd_circle.h>
00026 #include <cmath>
00027 #include <sys/time.h>
00028 #include <cstdlib>
00029
00030 using namespace std;
00031 using namespace fawkes;
00032
00033 namespace firevision {
00034 #if 0
00035 }
00036 #endif
00037
00038 #define TBY_GRAYSCALE
00039 #ifdef TBY_GRAYSCALE
00040 #define TEST_IF_IS_A_PIXEL(x) ((x)>230)
00041 #else
00042 #define TEST_IF_IS_A_PIXEL(x) ((x)==0)
00043 #endif // TBY_GRAYSCALE
00044
00045 #define TBY_SQUARED_DIST(x1,y1,x2,y2) \
00046 (((x1)-(x2))*((x1)-(x2))+((y1)-(y2))*((y1)-(y2)))
00047 #define TBY_RADIUS_DIFF(x1, y1, x2, y2, r) \
00048 (((x1)-(x2))*((x1)-(x2))+((y1)-(y2))*((y1)-(y2))-(r)*(r))
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065 RcdCircleModel::RcdCircleModel(unsigned int max_failures,
00066 unsigned int min_pixels,
00067 unsigned int min_interpix_dist,
00068 unsigned int max_dist_p4,
00069 unsigned int max_dist_a,
00070 float hw_ratio,
00071 float hollow_rate,
00072 float max_time
00073 )
00074 {
00075
00076 RCD_MAX_FAILURES = max_failures;
00077 RCD_MIN_PIXELS = min_pixels;
00078 RCD_MIN_INTERPIX_DIST = min_interpix_dist;
00079 RCD_MAX_DIST_P4 = max_dist_p4;
00080 RCD_MAX_DIST_A = max_dist_a;
00081 RCD_HW_RATIO = hw_ratio;
00082 RCD_MAX_TIME = max_time;
00083 RCD_ROI_HOLLOW_RATE = hollow_rate;
00084
00085 }
00086
00087
00088 RcdCircleModel::~RcdCircleModel(void)
00089 {
00090 m_Circles.clear();
00091 }
00092
00093 int RcdCircleModel::parseImage( unsigned char* buf,
00094 ROI *roi )
00095 {
00096
00097 unsigned char *buffer = roi->get_roi_buffer_start(buf);
00098 unsigned char *line_start = buffer;
00099
00100 unsigned int x, y;
00101 vector<point_t> pixels,
00102 remove_list;
00103 unsigned int f = 0;
00104 int count;
00105 int num_circles = 0;
00106 struct timeval start, now;
00107
00108
00109 m_Circles.clear();
00110
00111 const unsigned int roi_hollow_top = (int)(roi->height * ((1.0f - RCD_ROI_HOLLOW_RATE) / 2));
00112 const unsigned int roi_hollow_bottom = roi->height - roi_hollow_top;
00113 const unsigned int roi_hollow_left = (int)(roi->width * ((1.0f - RCD_ROI_HOLLOW_RATE) / 2));
00114 const unsigned int roi_hollow_right = roi->width - roi_hollow_left;
00115
00116
00117
00118
00119 buffer = roi->get_roi_buffer_start(buf);
00120 line_start = buffer;
00121
00122
00123
00124 unsigned int boundary_right = 0;
00125 unsigned int boundary_bottom = 0;
00126
00127 gettimeofday(&start, NULL);
00128
00129 pixels.clear();
00130
00131
00132 for (y = 0; y < roi_hollow_top; ++y) {
00133 for (x = 0; x < roi->width; ++x) {
00134 if (TEST_IF_IS_A_PIXEL(*buffer)) {
00135 point_t pt={x, y};
00136 pixels.push_back(pt);
00137 if (x > boundary_right) boundary_right = x;
00138 boundary_bottom = y;
00139 }
00140
00141 ++buffer;
00142 }
00143 line_start += roi->line_step;
00144 buffer = line_start;
00145 }
00146
00147 for (y = roi_hollow_top; y < roi_hollow_bottom; ++y) {
00148 for (x = 0; x < roi_hollow_left; ++x) {
00149 if (TEST_IF_IS_A_PIXEL(*buffer)) {
00150 point_t pt={x, y};
00151 pixels.push_back(pt);
00152 if (x > boundary_right) boundary_right = x;
00153 boundary_bottom = y;
00154 }
00155
00156 ++buffer;
00157 }
00158 buffer+=(roi_hollow_right - roi_hollow_left);
00159 for (x = roi_hollow_right; x < roi->width; ++x) {
00160 if (TEST_IF_IS_A_PIXEL(*buffer)) {
00161 point_t pt={x, y};
00162 pixels.push_back(pt);
00163 if (x > boundary_right) boundary_right = x;
00164 boundary_bottom = y;
00165 }
00166
00167 ++buffer;
00168 }
00169 line_start += roi->line_step;
00170 buffer = line_start;
00171 }
00172
00173 for (y = roi_hollow_bottom; y < roi->height; ++y) {
00174 for (x = 0; x < roi->width; ++x) {
00175 if (TEST_IF_IS_A_PIXEL(*buffer)) {
00176 point_t pt={x, y};
00177 pixels.push_back(pt);
00178 }
00179
00180 ++buffer;
00181 }
00182 line_start += roi->line_step;
00183 buffer = line_start;
00184 }
00185
00186
00187 point_t p[4];
00188 center_in_roi_t center;
00189 float radius;
00190 vector< point_t >::iterator pos;
00191
00192 if (pixels.size() < RCD_MIN_PIXELS) {
00193 return 0;
00194 }
00195
00196 do {
00197
00198
00199 for (int i=0; i < 4; ++i) {
00200 int ri = rand() % ((int)pixels.size());
00201 pos = pixels.begin() + ri;
00202 p[i] = *pos;
00203 pixels.erase(pos);
00204 remove_list.push_back(p[i]);
00205 }
00206
00207 if (TBY_SQUARED_DIST(p[1].x, p[1].y, p[2].x, p[2].y) < RCD_MIN_INTERPIX_DIST ||
00208 TBY_SQUARED_DIST(p[2].x, p[2].y, p[0].x, p[0].y) < RCD_MIN_INTERPIX_DIST ||
00209 TBY_SQUARED_DIST(p[0].x, p[0].y, p[1].x, p[1].y) < RCD_MIN_INTERPIX_DIST ) {
00210
00211
00212 ++f;
00213
00214
00215 pixels.push_back(p[0]);
00216 pixels.push_back(p[1]);
00217 pixels.push_back(p[2]);
00218 pixels.push_back(p[3]);
00219
00220 remove_list.clear();
00221
00222 gettimeofday(&now, NULL);
00223 continue;
00224 }
00225
00226
00227
00228 calcCircle(p[0], p[1], p[2], center, radius);
00229
00230
00231 int r = (int)sqrt(TBY_SQUARED_DIST(center.x, center.y, pixels[3].x, pixels[3].y));
00232 int dist = (int)(r - radius);
00233 dist = (dist >= 0) ? dist : -dist;
00234 if (radius <= 0 || (unsigned int)dist > RCD_MAX_DIST_P4 ) {
00235 ++f;
00236
00237
00238 pixels.push_back(p[0]);
00239 pixels.push_back(p[1]);
00240 pixels.push_back(p[2]);
00241 pixels.push_back(p[3]);
00242
00243 remove_list.clear();
00244
00245 gettimeofday(&now, NULL);
00246 continue;
00247 }
00248
00249
00250 count=0;
00251 for (unsigned int i=0; i < pixels.size(); ++i) {
00252 int r = (int)sqrt(TBY_SQUARED_DIST(center.x, center.y, pixels[i].x, pixels[i].y));
00253 int dist = (int)(r-radius);
00254 dist = (dist >= 0) ? dist : -dist;
00255 if ((unsigned int)dist <= RCD_MAX_DIST_A) {
00256 pos = pixels.begin() + i;
00257 ++count;
00258
00259 remove_list.push_back(pixels[i]);
00260 pixels.erase(pos--);
00261 }
00262 }
00263
00264
00265
00266 if ( (float)count >
00267 ( boundary_right > boundary_bottom ? boundary_right : boundary_bottom ) * RCD_HW_RATIO ) {
00268
00269 if ( radius > TBY_CIRCLE_RADIUS_MIN &&
00270 radius < TBY_CIRCLE_RADIUS_MAX ) {
00271
00272
00273
00274 Circle c;
00275 c.fitCircle(remove_list);
00276 c.count = count;
00277
00278
00279 m_Circles.push_back(c);
00280 }
00281 remove_list.clear();
00282 ++num_circles;
00283 } else {
00284
00285 ++f;
00286
00287 while(!remove_list.empty()) {
00288 pixels.push_back(remove_list.back());
00289 remove_list.pop_back();
00290 }
00291 gettimeofday(&now, NULL);
00292 continue;
00293 }
00294
00295 gettimeofday(&now, NULL);
00296
00297 diff_sec = now.tv_sec - start.tv_sec;
00298 diff_usec = now.tv_usec - start.tv_usec;
00299 if (diff_usec < 0) {
00300 diff_sec -= 1;
00301 diff_usec += 1000000;
00302 }
00303
00304 f_diff_sec = diff_sec + diff_usec / 1000000.f;
00305
00306 } while( (f < RCD_MAX_FAILURES) && (pixels.size() > RCD_MIN_PIXELS) &&
00307 ( f_diff_sec < RCD_MAX_TIME) );
00308
00309 return num_circles;
00310 }
00311
00312 int RcdCircleModel::getShapeCount(void) const
00313 {
00314 return m_Circles.size();
00315 }
00316
00317 Circle* RcdCircleModel::getShape(int id) const
00318 {
00319 if (id < 0 || (unsigned int)id >= m_Circles.size())
00320 {
00321 return NULL;
00322 }
00323 else
00324 {
00325 return const_cast<Circle*>(&m_Circles[id]);
00326 }
00327 }
00328
00329 Circle* RcdCircleModel::getMostLikelyShape(void) const
00330 {
00331 int cur=0;
00332 switch (m_Circles.size())
00333 {
00334 case 0:
00335 return NULL;
00336 case 1:
00337 return const_cast<Circle*>(&m_Circles[0]);
00338 default:
00339 for (unsigned int i=1; i < m_Circles.size(); ++i)
00340 if (m_Circles[i].radius > m_Circles[cur].radius)
00341 cur = i;
00342 return const_cast<Circle*>(&m_Circles[cur]);
00343 }
00344 }
00345
00346 void RcdCircleModel::calcCircle(
00347 const point_t& p1,
00348 const point_t& p2,
00349 const point_t& p3,
00350 center_in_roi_t& center,
00351 float& radius)
00352
00353
00354
00355 {
00356 const int &x1=p1.x, &y1=p1.y, &x2=p2.x, &y2=p2.y, &x3=p3.x, &y3=p3.y;
00357 float dx, dy;
00358 int div = 2*((x2-x1)*(y3-y1)-(x3-x1)*(y2-y1));
00359
00360 if (div == 0)
00361 {
00362
00363 radius = -1.0;
00364 return;
00365 }
00366 center.x = ((float)((x2*x2+y2*y2-x1*x1-y1*y1)*(y3-y1)
00367 -(x3*x3+y3*y3-x1*x1-y1*y1)*(y2-y1))
00368 /div);
00369 center.y = ((float)((x2-x1)*(x3*x3+y3*y3-x1*x1-y1*y1)
00370 -(x3-x1)*(x2*x2+y2*y2-x1*x1-y1*y1))
00371 /div);
00372 dx = center.x - x1;
00373 dy = center.y - y1;
00374 radius = (float)sqrt(dx*dx+dy*dy);
00375 }
00376
00377 }