Fawkes API Fawkes Development Version
|
00001 /*************************************************************************** 00002 * field_drawer.cpp - Drawer for a soccer field 00003 * 00004 * Created: Tue Sep 23 00:00:00 2008 00005 * Copyright 2008 Christof Rath <christof.rath@gmail.com> 00006 * 00007 ****************************************************************************/ 00008 00009 /* This program is free software; you can redistribute it and/or modify 00010 * it under the terms of the GNU General Public License as published by 00011 * the Free Software Foundation; either version 2 of the License, or 00012 * (at your option) any later version. 00013 * 00014 * This program is distributed in the hope that it will be useful, 00015 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00016 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00017 * GNU Library General Public License for more details. 00018 * 00019 * Read the full text in the LICENSE.GPL file in the doc directory. 00020 */ 00021 00022 #include <fvutils/draw/field_drawer.h> 00023 00024 #include <core/exceptions/software.h> 00025 #include <fvutils/base/roi.h> 00026 #include <fvutils/draw/drawer.h> 00027 #include <fvutils/ipc/shm_image.h> 00028 00029 #include <cmath> 00030 #include <cstring> 00031 #include <stdio.h> 00032 00033 using namespace fawkes; 00034 00035 namespace firevision { 00036 #if 0 /* just to make Emacs auto-indent happy */ 00037 } 00038 #endif 00039 00040 /** @class FieldDrawer <fvutils/draw/field_drawer.h> 00041 * This class is used to draw a soccer field. 00042 * 00043 * @author Christof Rath 00044 */ 00045 /** @var float FieldDrawer::_img_buffer 00046 * The pointer to the target image buffer 00047 */ 00048 /** @var float FieldDrawer::_img_width 00049 * The width of the target image buffer 00050 */ 00051 /** @var float FieldDrawer::_img_height 00052 * The height of the target image buffer 00053 */ 00054 00055 /** 00056 * Created a new field object 00057 * 00058 * @param lines the field lines container 00059 */ 00060 FieldDrawer::FieldDrawer(const FieldLines &lines) : 00061 __lines(lines) 00062 { 00063 __points = NULL; 00064 __points_est = NULL; 00065 00066 clear_own_pos(); 00067 00068 set_color_background(YUV_t::black()); 00069 set_color_field(YUV_t::green()); 00070 set_color_lines(YUV_t::white()); 00071 00072 set_color_own_pos(YUV_t::cyan()); 00073 set_color_line_points(YUV_t::cyan()); 00074 00075 set_color_own_pos_est(YUV_t::yellow()); //yellowish 00076 set_color_line_points_est(YUV_t::yellow()); 00077 } 00078 00079 /** 00080 * Destructor. 00081 */ 00082 FieldDrawer::~FieldDrawer() 00083 { 00084 } 00085 00086 00087 /** 00088 * Sets the angular offset between body and head (along the body axis) 00089 * @param head_yaw angular offset 00090 */ 00091 void 00092 FieldDrawer::set_head_yaw(float head_yaw) 00093 { 00094 __head_yaw = head_yaw; 00095 } 00096 00097 /** 00098 * Own position setter. 00099 * Sets the (calculated) own position on the field 00100 * @param own_position as calculated by the localization 00101 */ 00102 void 00103 FieldDrawer::set_own_pos(field_pos_t own_position) 00104 { 00105 __own_position = own_position; 00106 } 00107 00108 /** 00109 * Own position estimate setter. 00110 * Sets the position estimate (e.g. by triangulation, odometry, ...) 00111 * @param own_position_estimate as estimated 00112 */ 00113 void 00114 FieldDrawer::set_own_pos_est(field_pos_t own_position_estimate) 00115 { 00116 __own_pos_est = own_position_estimate; 00117 } 00118 00119 /** 00120 * Clears the own position. 00121 * Used (e.g.) if the own position couldn't be calculated 00122 */ 00123 void 00124 FieldDrawer::clear_own_pos() 00125 { 00126 __own_position.ori = 12345; 00127 __own_pos_est.ori = 12345; 00128 __head_yaw = 12345; 00129 __points = NULL; 00130 __points_est = NULL; 00131 00132 _img_buffer = NULL; 00133 _img_width = 0; 00134 _img_height = 0; 00135 } 00136 00137 /** 00138 * Setter for detected line points 00139 * 00140 * @param points a list of line points (relative to the center of the field!) 00141 */ 00142 void 00143 FieldDrawer::set_line_points(const fld_line_points_t *points) 00144 { 00145 __points = points; 00146 } 00147 00148 /** 00149 * Setter for detected line points 00150 * 00151 * @param points_est a list of line points (relative to the center of the field!) 00152 */ 00153 void 00154 FieldDrawer::set_line_points_est(const fld_line_points_t *points_est) 00155 { 00156 __points_est = points_est; 00157 } 00158 00159 00160 /** 00161 * Calculates the conversion factor between field size and image size 00162 * 00163 * @param img_width of the target image 00164 * @param img_height of the target image 00165 * @param draw_landscape true if the image should be drawn landscape 00166 * @return the conversion factor 00167 */ 00168 float 00169 FieldDrawer::get_scale(unsigned int img_width, unsigned int img_height, bool draw_landscape) const 00170 { 00171 float f_width = (draw_landscape ? __lines.get_field_length() : __lines.get_field_width()); 00172 float f_height = (draw_landscape ? __lines.get_field_width() : __lines.get_field_length()); 00173 return std::min(img_width / f_width, img_height / f_height); 00174 } 00175 00176 /** 00177 * Sets the background color (outside the field) 00178 * @param color to be used 00179 */ 00180 void 00181 FieldDrawer::set_color_background(YUV_t color) 00182 { 00183 __c_background = color; 00184 } 00185 00186 /** 00187 * Sets the field color 00188 * @param color to be used 00189 */ 00190 void 00191 FieldDrawer::set_color_field(YUV_t color) 00192 { 00193 __c_field = color; 00194 } 00195 00196 /** 00197 * Sets the lines color 00198 * @param color to be used 00199 */ 00200 void 00201 FieldDrawer::set_color_lines(YUV_t color) 00202 { 00203 __c_lines = color; 00204 } 00205 00206 /** 00207 * Sets the line points color 00208 * @param color to be used 00209 */ 00210 void 00211 FieldDrawer::set_color_line_points(YUV_t color) 00212 { 00213 __c_line_points = color; 00214 } 00215 00216 /** 00217 * Sets the line points color 00218 * @param color to be used 00219 */ 00220 void 00221 FieldDrawer::set_color_line_points_est(YUV_t color) 00222 { 00223 __c_line_points_est = color; 00224 } 00225 00226 /** 00227 * Sets the own position color 00228 * @param color to be used 00229 */ 00230 void 00231 FieldDrawer::set_color_own_pos(YUV_t color) 00232 { 00233 __c_own_pos = color; 00234 } 00235 00236 /** 00237 * Sets the own position estimates color 00238 * @param color to be used 00239 */ 00240 void 00241 FieldDrawer::set_color_own_pos_est(YUV_t color) 00242 { 00243 __c_own_pos_est = color; 00244 } 00245 00246 00247 00248 /** 00249 * Draws the field (including the own position [est]). 00250 * The position [est] and line points [est] gets reseted after drawing 00251 * 00252 * @param yuv422_planar the image buffer 00253 * @param img_width the image width 00254 * @param img_height the image height 00255 * @param draw_background true if the background (field and border) should be drawn 00256 * @param draw_landscape true if the field should be drawn landscape 00257 */ 00258 void 00259 FieldDrawer::draw_field(unsigned char *yuv422_planar, unsigned int img_width, unsigned int img_height, 00260 bool draw_background, bool draw_landscape) 00261 { 00262 _img_buffer = yuv422_planar; 00263 _img_width = img_width; 00264 _img_height = img_height; 00265 00266 float f_width = (draw_landscape ? __lines.get_field_length() : __lines.get_field_width()); 00267 float f_height = (draw_landscape ? __lines.get_field_width() : __lines.get_field_length()); 00268 float scale = std::min(_img_width / f_width, _img_height / f_height); 00269 00270 if (draw_background) { 00271 unsigned int draw_width = static_cast<unsigned int>(f_width * scale); 00272 unsigned int draw_height = static_cast<unsigned int>(f_height * scale); 00273 unsigned int u_offset = _img_width * _img_height; 00274 unsigned int v_offset = u_offset + u_offset / 2; 00275 00276 if (_img_width == draw_width) {//use memcpy 00277 unsigned int offset = (_img_height - draw_height) / 2; 00278 memset(_img_buffer, __c_background.Y, offset * _img_width); 00279 memset(_img_buffer + offset * _img_width, __c_field.Y, draw_height * _img_width); 00280 memset(_img_buffer + (offset + draw_height) * _img_width, __c_background.Y, offset * _img_width); 00281 00282 offset /= 2; 00283 draw_height /= 2; 00284 00285 memset(_img_buffer + u_offset, __c_background.U, offset * _img_width); 00286 memset(_img_buffer + u_offset + offset * _img_width, __c_field.U, draw_height * _img_width); 00287 memset(_img_buffer + u_offset + (offset + draw_height) * _img_width, __c_background.U, offset * _img_width); 00288 00289 memset(_img_buffer + v_offset, __c_background.V, offset * _img_width); 00290 memset(_img_buffer + v_offset + offset * _img_width, __c_field.V, draw_height * _img_width); 00291 memset(_img_buffer + v_offset + (offset + draw_height) * _img_width, __c_background.V, offset * _img_width); 00292 } else { 00293 //center the field 00294 unsigned int sx = (_img_width - draw_width) / 2; 00295 unsigned int sy = (_img_height - draw_height) / 2; 00296 00297 ROI f_roi(sx,sy, draw_width,draw_height, _img_width,_img_height); 00298 for (unsigned int x = 0; x < _img_width; ++x) { 00299 for (unsigned int y = 0; y < _img_height; ++y) { 00300 if (f_roi.contains(x, y)) { 00301 _img_buffer[y * _img_width + x] = __c_field.Y; 00302 _img_buffer[(y * _img_width + x) / 2 + u_offset] = __c_field.U; 00303 _img_buffer[(y * _img_width + x) / 2 + v_offset] = __c_field.V; 00304 } else { 00305 _img_buffer[y * _img_width + x] = __c_background.Y; 00306 _img_buffer[(y * _img_width + x) / 2 + u_offset] = __c_background.U; 00307 _img_buffer[(y * _img_width + x) / 2 + v_offset] = __c_background.V; 00308 } 00309 } 00310 } 00311 } 00312 } else { 00313 unsigned int size = _img_width * _img_height; 00314 memset(_img_buffer, 0, size); 00315 memset(_img_buffer + size, 128, size); 00316 } //END: if (draw_background) 00317 00318 00319 draw_lines(__c_lines, draw_landscape, scale); 00320 00321 cart_coord_2d_t f_offs = __lines.get_field_offsets(); 00322 unsigned int center_x = std::max(0, static_cast<int>(_img_width / 2) + static_cast<int>(f_offs.x * scale)); 00323 unsigned int center_y = std::max(0, static_cast<int>(_img_height / 2) + static_cast<int>(f_offs.y * scale)); 00324 00325 if (__own_pos_est.ori != 12345) { 00326 Drawer d; 00327 d.set_buffer(_img_buffer, _img_width, _img_height); 00328 d.set_color(__c_own_pos_est); 00329 unsigned int r = _img_width / 40; 00330 int x = static_cast<int>(__own_pos_est.x * scale); 00331 int y = static_cast<int>(__own_pos_est.y * scale); 00332 int dx = static_cast<int>(r * cosf(__own_pos_est.ori)); 00333 int dy = static_cast<int>(r * sinf(__own_pos_est.ori)); 00334 00335 if (draw_landscape) { 00336 x += center_x; 00337 y = center_y - y; 00338 d.draw_circle(x, y, r); 00339 d.draw_line(x, y, x + dx, y - dy); 00340 } else { 00341 x += center_y; 00342 y = center_x - y; 00343 d.draw_circle(y, x, r); 00344 d.draw_line(y, x, y + dy, x - dx); 00345 } 00346 00347 if(__head_yaw != 12345) { 00348 int hx = static_cast<int>(r * cosf(__own_pos_est.ori + __head_yaw)); 00349 int hy = static_cast<int>(r * sinf(__own_pos_est.ori + __head_yaw)); 00350 int hdx = static_cast<int>((r + 4) * cosf(__own_pos_est.ori + __head_yaw)); 00351 int hdy = static_cast<int>((r + 4) * sinf(__own_pos_est.ori + __head_yaw)); 00352 00353 if (draw_landscape) d.draw_line(x + hx, y - hy, x + hdx, y - hdy); 00354 else d.draw_line(y + hy, x - hx, y + hdy, x - hdx); 00355 } 00356 } 00357 00358 if (__own_position.ori != 12345) { 00359 Drawer d; 00360 d.set_buffer(_img_buffer, _img_width, _img_height); 00361 d.set_color(__c_own_pos); 00362 unsigned int r = _img_width / 40; 00363 int x = static_cast<int>(__own_position.x * scale); 00364 int y = static_cast<int>(__own_position.y * scale); 00365 int dx = static_cast<int>(r * cosf(__own_position.ori)); 00366 int dy = static_cast<int>(r * sinf(__own_position.ori)); 00367 00368 if (draw_landscape) { 00369 x += center_x; 00370 y = center_y - y; 00371 d.draw_circle(x, y, r); 00372 d.draw_line(x, y, x + dx, y - dy); 00373 } else { 00374 x += center_y; 00375 y = center_x - y; 00376 d.draw_circle(y, x, r); 00377 d.draw_line(y, x, y + dy, x - dx); 00378 } 00379 00380 if(__head_yaw != 12345) { 00381 int hx = static_cast<int>(r * cosf(__own_position.ori + __head_yaw)); 00382 int hy = static_cast<int>(r * sinf(__own_position.ori + __head_yaw)); 00383 int hdx = static_cast<int>((r + 4) * cosf(__own_position.ori + __head_yaw)); 00384 int hdy = static_cast<int>((r + 4) * sinf(__own_position.ori + __head_yaw)); 00385 00386 if (draw_landscape) d.draw_line(x + hx, y - hy, x + hdx, y - hdy); 00387 else d.draw_line(y + hy, x - hx, y + hdy, x - hdx); 00388 } 00389 } 00390 00391 draw_line_points(draw_landscape, scale); 00392 clear_own_pos(); 00393 } 00394 00395 /** 00396 * Draws the line points 00397 * @param draw_landscape true if the field should be drawn landscape 00398 * @param scale the pre calculated scale (conversion factor between image size and field size - if 0 the value gets calculated) 00399 */ 00400 void 00401 FieldDrawer::draw_line_points(bool draw_landscape, float scale) const 00402 { 00403 if (!scale) { 00404 if (draw_landscape) scale = std::min(_img_width / __lines.get_field_length(), _img_height / __lines.get_field_width()); 00405 else scale = std::min(_img_width / __lines.get_field_width(), _img_height / __lines.get_field_length()); 00406 } 00407 00408 cart_coord_2d_t f_offs = __lines.get_field_offsets(); 00409 unsigned int center_x = std::max(0, static_cast<int>(_img_width / 2) + static_cast<int>(f_offs.x * scale)); 00410 unsigned int center_y = std::max(0, static_cast<int>(_img_height / 2) + static_cast<int>(f_offs.y * scale)); 00411 00412 Drawer d; 00413 d.set_buffer(_img_buffer, _img_width, _img_height); 00414 00415 if (__points_est) { 00416 d.set_color(__c_line_points_est); 00417 for (fld_line_points_t::const_iterator it = __points_est->begin(); it != __points_est->end(); ++it) { 00418 unsigned int y = static_cast<unsigned int>(center_y - (draw_landscape ? it->y : it->x) * scale); 00419 unsigned int x =static_cast<unsigned int>((draw_landscape ? it->x : it->y) * scale + center_x); 00420 00421 d.draw_cross(x, y, 4); 00422 } 00423 } 00424 00425 if (__points) { 00426 d.set_color(__c_line_points); 00427 for (fld_line_points_t::const_iterator it = __points->begin(); it != __points->end(); ++it) { 00428 unsigned int y = static_cast<unsigned int>(center_y - (draw_landscape ? it->y : it->x) * scale); 00429 unsigned int x = static_cast<unsigned int>((draw_landscape ? it->x : it->y) * scale + center_x); 00430 00431 d.draw_cross(x, y, 4); 00432 } 00433 } 00434 } 00435 00436 00437 /** 00438 * Draws the field lines to a SharedMemoryImageBuffer 00439 * 00440 * @param color of the lines 00441 * @param draw_landscape if true (default) the field is supposed to be landscape 00442 * @param scale the conversation factor between [m] and [px] (if 0 this value gets calculated) 00443 */ 00444 void 00445 FieldDrawer::draw_lines(YUV_t color, bool draw_landscape, float scale) const 00446 { 00447 if (!scale) { 00448 if (draw_landscape) scale = std::min(_img_width / __lines.get_field_length(), _img_height / __lines.get_field_width()); 00449 else scale = std::min(_img_width / __lines.get_field_width(), _img_height / __lines.get_field_length()); 00450 } 00451 00452 cart_coord_2d_t f_offs = __lines.get_field_offsets(); 00453 int f_off_x = static_cast<int>(f_offs.x * scale); 00454 int f_off_y = static_cast<int>(f_offs.y * scale); 00455 00456 unsigned int off_x = std::max(0, static_cast<int>(_img_width / 2) + f_off_x); 00457 unsigned int off_y = std::max(0, static_cast<int>(_img_height / 2) + f_off_y); 00458 00459 Drawer d; 00460 d.set_buffer(_img_buffer, _img_width, _img_height); 00461 d.set_color(color); 00462 00463 for (FieldLines::const_iterator it = __lines.begin(); it != __lines.end(); ++it) { 00464 unsigned int sx = static_cast<unsigned int>((draw_landscape ? (*it).start.x : (*it).start.y) * scale); 00465 unsigned int sy = static_cast<unsigned int>((draw_landscape ? (*it).start.y : (*it).start.x) * scale); 00466 unsigned int ex = static_cast<unsigned int>((draw_landscape ? (*it).end.x : (*it).end.y) * scale); 00467 unsigned int ey = static_cast<unsigned int>((draw_landscape ? (*it).end.y : (*it).end.x) * scale); 00468 00469 d.draw_line(off_x + sx, off_y + sy, off_x + ex, off_y + ey); 00470 } 00471 00472 for (field_circles_t::const_iterator it = __lines.get_circles().begin(); it != __lines.get_circles().end(); ++it) { 00473 unsigned int cx = static_cast<unsigned int>((draw_landscape ? it->center.x : it->center.y) * scale); 00474 unsigned int cy = static_cast<unsigned int>((draw_landscape ? it->center.y : it->center.x) * scale); 00475 unsigned int r = static_cast<unsigned int>(it->radius * scale); 00476 //TODO: Draw only arcs for corner circle, etc. 00477 d.draw_circle(off_x + cx, off_y + cy, r); 00478 } 00479 } 00480 00481 } // end namespace firevision