Fawkes API  Fawkes Development Version
field_drawer.cpp
1 /***************************************************************************
2  * field_drawer.cpp - Drawer for a soccer field
3  *
4  * Created: Tue Sep 23 00:00:00 2008
5  * Copyright 2008 Christof Rath <christof.rath@gmail.com>
6  *
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include <fvutils/draw/field_drawer.h>
23 
24 #include <core/exceptions/software.h>
25 #include <fvutils/base/roi.h>
26 #include <fvutils/draw/drawer.h>
27 #include <fvutils/ipc/shm_image.h>
28 
29 #include <cmath>
30 #include <cstring>
31 #include <stdio.h>
32 
33 using namespace fawkes;
34 
35 namespace firevision {
36 #if 0 /* just to make Emacs auto-indent happy */
37 }
38 #endif
39 
40 /** @class FieldDrawer <fvutils/draw/field_drawer.h>
41  * This class is used to draw a soccer field.
42  *
43  * @author Christof Rath
44  */
45 /** @var float FieldDrawer::_img_buffer
46  * The pointer to the target image buffer
47  */
48 /** @var float FieldDrawer::_img_width
49  * The width of the target image buffer
50  */
51 /** @var float FieldDrawer::_img_height
52  * The height of the target image buffer
53  */
54 
55 /**
56  * Created a new field object
57  *
58  * @param lines the field lines container
59  */
60 FieldDrawer::FieldDrawer(const FieldLines &lines) :
61  __lines(lines)
62 {
63  __points = NULL;
64  __points_est = NULL;
65 
66  clear_own_pos();
67 
71 
74 
75  set_color_own_pos_est(YUV_t::yellow()); //yellowish
77 }
78 
79 /**
80  * Destructor.
81  */
83 {
84 }
85 
86 
87 /**
88  * Sets the angular offset between body and head (along the body axis)
89  * @param head_yaw angular offset
90  */
91 void
93 {
94  __head_yaw = head_yaw;
95 }
96 
97 /**
98  * Own position setter.
99  * Sets the (calculated) own position on the field
100  * @param own_position as calculated by the localization
101  */
102 void
104 {
105  __own_position = own_position;
106 }
107 
108 /**
109  * Own position estimate setter.
110  * Sets the position estimate (e.g. by triangulation, odometry, ...)
111  * @param own_position_estimate as estimated
112  */
113 void
115 {
116  __own_pos_est = own_position_estimate;
117 }
118 
119 /**
120  * Clears the own position.
121  * Used (e.g.) if the own position couldn't be calculated
122  */
123 void
125 {
126  __own_position.ori = 12345;
127  __own_pos_est.ori = 12345;
128  __head_yaw = 12345;
129  __points = NULL;
130  __points_est = NULL;
131 
132  _img_buffer = NULL;
133  _img_width = 0;
134  _img_height = 0;
135 }
136 
137 /**
138  * Setter for detected line points
139  *
140  * @param points a list of line points (relative to the center of the field!)
141  */
142 void
143 FieldDrawer::set_line_points(const fld_line_points_t *points)
144 {
145  __points = points;
146 }
147 
148 /**
149  * Setter for detected line points
150  *
151  * @param points_est a list of line points (relative to the center of the field!)
152  */
153 void
154 FieldDrawer::set_line_points_est(const fld_line_points_t *points_est)
155 {
156  __points_est = points_est;
157 }
158 
159 
160 /**
161  * Calculates the conversion factor between field size and image size
162  *
163  * @param img_width of the target image
164  * @param img_height of the target image
165  * @param draw_landscape true if the image should be drawn landscape
166  * @return the conversion factor
167  */
168 float
169 FieldDrawer::get_scale(unsigned int img_width, unsigned int img_height, bool draw_landscape) const
170 {
171  float f_width = (draw_landscape ? __lines.get_field_length() : __lines.get_field_width());
172  float f_height = (draw_landscape ? __lines.get_field_width() : __lines.get_field_length());
173  return std::min(img_width / f_width, img_height / f_height);
174 }
175 
176 /**
177  * Sets the background color (outside the field)
178  * @param color to be used
179  */
180 void
182 {
183  __c_background = color;
184 }
185 
186 /**
187  * Sets the field color
188  * @param color to be used
189  */
190 void
192 {
193  __c_field = color;
194 }
195 
196 /**
197  * Sets the lines color
198  * @param color to be used
199  */
200 void
202 {
203  __c_lines = color;
204 }
205 
206 /**
207  * Sets the line points color
208  * @param color to be used
209  */
210 void
212 {
213  __c_line_points = color;
214 }
215 
216 /**
217  * Sets the line points color
218  * @param color to be used
219  */
220 void
222 {
223  __c_line_points_est = color;
224 }
225 
226 /**
227  * Sets the own position color
228  * @param color to be used
229  */
230 void
232 {
233  __c_own_pos = color;
234 }
235 
236 /**
237  * Sets the own position estimates color
238  * @param color to be used
239  */
240 void
242 {
243  __c_own_pos_est = color;
244 }
245 
246 
247 
248 /**
249  * Draws the field (including the own position [est]).
250  * The position [est] and line points [est] gets reseted after drawing
251  *
252  * @param yuv422_planar the image buffer
253  * @param img_width the image width
254  * @param img_height the image height
255  * @param draw_background true if the background (field and border) should be drawn
256  * @param draw_landscape true if the field should be drawn landscape
257  */
258 void
259 FieldDrawer::draw_field(unsigned char *yuv422_planar, unsigned int img_width, unsigned int img_height,
260  bool draw_background, bool draw_landscape)
261 {
262  _img_buffer = yuv422_planar;
263  _img_width = img_width;
264  _img_height = img_height;
265 
266  float f_width = (draw_landscape ? __lines.get_field_length() : __lines.get_field_width());
267  float f_height = (draw_landscape ? __lines.get_field_width() : __lines.get_field_length());
268  float scale = std::min(_img_width / f_width, _img_height / f_height);
269 
270  if (draw_background) {
271  unsigned int draw_width = static_cast<unsigned int>(f_width * scale);
272  unsigned int draw_height = static_cast<unsigned int>(f_height * scale);
273  unsigned int u_offset = _img_width * _img_height;
274  unsigned int v_offset = u_offset + u_offset / 2;
275 
276  if (_img_width == draw_width) {//use memcpy
277  unsigned int offset = (_img_height - draw_height) / 2;
278  memset(_img_buffer, __c_background.Y, offset * _img_width);
279  memset(_img_buffer + offset * _img_width, __c_field.Y, draw_height * _img_width);
280  memset(_img_buffer + (offset + draw_height) * _img_width, __c_background.Y, offset * _img_width);
281 
282  offset /= 2;
283  draw_height /= 2;
284 
285  memset(_img_buffer + u_offset, __c_background.U, offset * _img_width);
286  memset(_img_buffer + u_offset + offset * _img_width, __c_field.U, draw_height * _img_width);
287  memset(_img_buffer + u_offset + (offset + draw_height) * _img_width, __c_background.U, offset * _img_width);
288 
289  memset(_img_buffer + v_offset, __c_background.V, offset * _img_width);
290  memset(_img_buffer + v_offset + offset * _img_width, __c_field.V, draw_height * _img_width);
291  memset(_img_buffer + v_offset + (offset + draw_height) * _img_width, __c_background.V, offset * _img_width);
292  } else {
293  //center the field
294  unsigned int sx = (_img_width - draw_width) / 2;
295  unsigned int sy = (_img_height - draw_height) / 2;
296 
297  ROI f_roi(sx,sy, draw_width,draw_height, _img_width,_img_height);
298  for (unsigned int x = 0; x < _img_width; ++x) {
299  for (unsigned int y = 0; y < _img_height; ++y) {
300  if (f_roi.contains(x, y)) {
301  _img_buffer[y * _img_width + x] = __c_field.Y;
302  _img_buffer[(y * _img_width + x) / 2 + u_offset] = __c_field.U;
303  _img_buffer[(y * _img_width + x) / 2 + v_offset] = __c_field.V;
304  } else {
305  _img_buffer[y * _img_width + x] = __c_background.Y;
306  _img_buffer[(y * _img_width + x) / 2 + u_offset] = __c_background.U;
307  _img_buffer[(y * _img_width + x) / 2 + v_offset] = __c_background.V;
308  }
309  }
310  }
311  }
312  } else {
313  unsigned int size = _img_width * _img_height;
314  memset(_img_buffer, 0, size);
315  memset(_img_buffer + size, 128, size);
316  } //END: if (draw_background)
317 
318 
319  draw_lines(__c_lines, draw_landscape, scale);
320 
321  cart_coord_2d_t f_offs = __lines.get_field_offsets();
322  unsigned int center_x = std::max(0, static_cast<int>(_img_width / 2) + static_cast<int>(f_offs.x * scale));
323  unsigned int center_y = std::max(0, static_cast<int>(_img_height / 2) + static_cast<int>(f_offs.y * scale));
324 
325  if (__own_pos_est.ori != 12345) {
326  Drawer d;
328  d.set_color(__c_own_pos_est);
329  unsigned int r = _img_width / 40;
330  int x = static_cast<int>(__own_pos_est.x * scale);
331  int y = static_cast<int>(__own_pos_est.y * scale);
332  int dx = static_cast<int>(r * cosf(__own_pos_est.ori));
333  int dy = static_cast<int>(r * sinf(__own_pos_est.ori));
334 
335  if (draw_landscape) {
336  x += center_x;
337  y = center_y - y;
338  d.draw_circle(x, y, r);
339  d.draw_line(x, y, x + dx, y - dy);
340  } else {
341  x += center_y;
342  y = center_x - y;
343  d.draw_circle(y, x, r);
344  d.draw_line(y, x, y + dy, x - dx);
345  }
346 
347  if(__head_yaw != 12345) {
348  int hx = static_cast<int>(r * cosf(__own_pos_est.ori + __head_yaw));
349  int hy = static_cast<int>(r * sinf(__own_pos_est.ori + __head_yaw));
350  int hdx = static_cast<int>((r + 4) * cosf(__own_pos_est.ori + __head_yaw));
351  int hdy = static_cast<int>((r + 4) * sinf(__own_pos_est.ori + __head_yaw));
352 
353  if (draw_landscape) d.draw_line(x + hx, y - hy, x + hdx, y - hdy);
354  else d.draw_line(y + hy, x - hx, y + hdy, x - hdx);
355  }
356  }
357 
358  if (__own_position.ori != 12345) {
359  Drawer d;
361  d.set_color(__c_own_pos);
362  unsigned int r = _img_width / 40;
363  int x = static_cast<int>(__own_position.x * scale);
364  int y = static_cast<int>(__own_position.y * scale);
365  int dx = static_cast<int>(r * cosf(__own_position.ori));
366  int dy = static_cast<int>(r * sinf(__own_position.ori));
367 
368  if (draw_landscape) {
369  x += center_x;
370  y = center_y - y;
371  d.draw_circle(x, y, r);
372  d.draw_line(x, y, x + dx, y - dy);
373  } else {
374  x += center_y;
375  y = center_x - y;
376  d.draw_circle(y, x, r);
377  d.draw_line(y, x, y + dy, x - dx);
378  }
379 
380  if(__head_yaw != 12345) {
381  int hx = static_cast<int>(r * cosf(__own_position.ori + __head_yaw));
382  int hy = static_cast<int>(r * sinf(__own_position.ori + __head_yaw));
383  int hdx = static_cast<int>((r + 4) * cosf(__own_position.ori + __head_yaw));
384  int hdy = static_cast<int>((r + 4) * sinf(__own_position.ori + __head_yaw));
385 
386  if (draw_landscape) d.draw_line(x + hx, y - hy, x + hdx, y - hdy);
387  else d.draw_line(y + hy, x - hx, y + hdy, x - hdx);
388  }
389  }
390 
391  draw_line_points(draw_landscape, scale);
392  clear_own_pos();
393 }
394 
395 /**
396  * Draws the line points
397  * @param draw_landscape true if the field should be drawn landscape
398  * @param scale the pre calculated scale (conversion factor between image size and field size - if 0 the value gets calculated)
399  */
400 void
401 FieldDrawer::draw_line_points(bool draw_landscape, float scale) const
402 {
403  if (!scale) {
404  if (draw_landscape) scale = std::min(_img_width / __lines.get_field_length(), _img_height / __lines.get_field_width());
405  else scale = std::min(_img_width / __lines.get_field_width(), _img_height / __lines.get_field_length());
406  }
407 
408  cart_coord_2d_t f_offs = __lines.get_field_offsets();
409  unsigned int center_x = std::max(0, static_cast<int>(_img_width / 2) + static_cast<int>(f_offs.x * scale));
410  unsigned int center_y = std::max(0, static_cast<int>(_img_height / 2) + static_cast<int>(f_offs.y * scale));
411 
412  Drawer d;
414 
415  if (__points_est) {
416  d.set_color(__c_line_points_est);
417  for (fld_line_points_t::const_iterator it = __points_est->begin(); it != __points_est->end(); ++it) {
418  unsigned int y = static_cast<unsigned int>(center_y - (draw_landscape ? it->y : it->x) * scale);
419  unsigned int x =static_cast<unsigned int>((draw_landscape ? it->x : it->y) * scale + center_x);
420 
421  d.draw_cross(x, y, 4);
422  }
423  }
424 
425  if (__points) {
426  d.set_color(__c_line_points);
427  for (fld_line_points_t::const_iterator it = __points->begin(); it != __points->end(); ++it) {
428  unsigned int y = static_cast<unsigned int>(center_y - (draw_landscape ? it->y : it->x) * scale);
429  unsigned int x = static_cast<unsigned int>((draw_landscape ? it->x : it->y) * scale + center_x);
430 
431  d.draw_cross(x, y, 4);
432  }
433  }
434 }
435 
436 
437 /**
438  * Draws the field lines to a SharedMemoryImageBuffer
439  *
440  * @param color of the lines
441  * @param draw_landscape if true (default) the field is supposed to be landscape
442  * @param scale the conversation factor between [m] and [px] (if 0 this value gets calculated)
443  */
444 void
445 FieldDrawer::draw_lines(YUV_t color, bool draw_landscape, float scale) const
446 {
447  if (!scale) {
448  if (draw_landscape) scale = std::min(_img_width / __lines.get_field_length(), _img_height / __lines.get_field_width());
449  else scale = std::min(_img_width / __lines.get_field_width(), _img_height / __lines.get_field_length());
450  }
451 
452  cart_coord_2d_t f_offs = __lines.get_field_offsets();
453  int f_off_x = static_cast<int>(f_offs.x * scale);
454  int f_off_y = static_cast<int>(f_offs.y * scale);
455 
456  unsigned int off_x = std::max(0, static_cast<int>(_img_width / 2) + f_off_x);
457  unsigned int off_y = std::max(0, static_cast<int>(_img_height / 2) + f_off_y);
458 
459  Drawer d;
461  d.set_color(color);
462 
463  for (FieldLines::const_iterator it = __lines.begin(); it != __lines.end(); ++it) {
464  unsigned int sx = static_cast<unsigned int>((draw_landscape ? (*it).start.x : (*it).start.y) * scale);
465  unsigned int sy = static_cast<unsigned int>((draw_landscape ? (*it).start.y : (*it).start.x) * scale);
466  unsigned int ex = static_cast<unsigned int>((draw_landscape ? (*it).end.x : (*it).end.y) * scale);
467  unsigned int ey = static_cast<unsigned int>((draw_landscape ? (*it).end.y : (*it).end.x) * scale);
468 
469  d.draw_line(off_x + sx, off_y + sy, off_x + ex, off_y + ey);
470  }
471 
472  for (field_circles_t::const_iterator it = __lines.get_circles().begin(); it != __lines.get_circles().end(); ++it) {
473  unsigned int cx = static_cast<unsigned int>((draw_landscape ? it->center.x : it->center.y) * scale);
474  unsigned int cy = static_cast<unsigned int>((draw_landscape ? it->center.y : it->center.x) * scale);
475  unsigned int r = static_cast<unsigned int>(it->radius * scale);
476  //TODO: Draw only arcs for corner circle, etc.
477  d.draw_circle(off_x + cx, off_y + cy, r);
478  }
479 }
480 
481 } // end namespace firevision
void set_color_lines(YUV_t color)
Sets the lines color.
void set_color_own_pos_est(YUV_t color)
Sets the own position estimates color.
float get_field_length() const
Field length getter.
Definition: field_lines.h:42
unsigned char V
V component.
Definition: yuv.h:62
static YUV_t_struct yellow()
Definition: yuv.h:83
virtual void draw_lines(YUV_t color, bool draw_landscape=true, float scale=0) const
Draws the field lines to a SharedMemoryImageBuffer.
void clear_own_pos()
Clears the own position.
unsigned int _img_width
The width of the target image buffer.
Definition: field_drawer.h:69
Cartesian coordinates (2D).
Definition: types.h:59
void draw_cross(unsigned int x_center, unsigned int y_center, unsigned int width)
Draws a cross.
Definition: drawer.cpp:467
Fawkes library namespace.
float get_scale(unsigned int img_width, unsigned int img_height, bool draw_landscape=true) const
Calculates the conversion factor between field size and image size.
Draw to an image.
Definition: drawer.h:34
virtual void draw_field(unsigned char *yuv422_planar, unsigned int img_width, unsigned int img_height, bool draw_background=true, bool draw_landscape=true)
Draws the field (including the own position [est]).
Region of interest.
Definition: roi.h:58
virtual ~FieldDrawer()
Destructor.
unsigned char Y
Y component.
Definition: yuv.h:60
static YUV_t_struct cyan()
Definition: yuv.h:79
void draw_line(unsigned int x_start, unsigned int y_start, unsigned int x_end, unsigned int y_end)
Draw line.
Definition: drawer.cpp:381
static YUV_t_struct black()
Definition: yuv.h:77
const field_circles_t & get_circles() const
Get circles.
Definition: field_lines.h:45
void set_color_background(YUV_t color)
Sets the background color (outside the field)
void set_buffer(unsigned char *buffer, unsigned int width, unsigned int height)
Set the buffer to draw to.
Definition: drawer.cpp:62
void set_line_points(const fld_line_points_t *points)
Setter for detected line points.
float x
x coordinate in meters
Definition: types.h:111
Position on the field.
Definition: types.h:110
unsigned char * _img_buffer
The pointer to the target image buffer.
Definition: field_drawer.h:68
bool contains(unsigned int x, unsigned int y)
Check if this ROI contains the given coordinates.
Definition: roi.cpp:302
float get_field_width() const
Field width getter.
Definition: field_lines.h:43
static YUV_t_struct green()
Definition: yuv.h:78
void set_color_line_points(YUV_t color)
Sets the line points color.
float y
y coordinate
Definition: types.h:61
void set_head_yaw(float head_yaw)
Sets the angular offset between body and head (along the body axis)
void set_color_own_pos(YUV_t color)
Sets the own position color.
unsigned char U
U component.
Definition: yuv.h:61
unsigned int _img_height
The height of the target image buffer.
Definition: field_drawer.h:70
void draw_circle(int center_x, int center_y, unsigned int radius)
Draw circle.
Definition: drawer.cpp:102
This class acts as a container for lines on a soccer field.
Definition: field_lines.h:36
YUV pixel.
Definition: yuv.h:59
float ori
orientation
Definition: types.h:113
fawkes::cart_coord_2d_t get_field_offsets() const
Offset getter.
Definition: field_lines.h:44
void set_own_pos(fawkes::field_pos_t own_position)
Own position setter.
void set_color_field(YUV_t color)
Sets the field color.
float y
y coordinate in meters
Definition: types.h:112
virtual void draw_line_points(bool draw_landscape=true, float scale=0) const
Draws the line points.
static YUV_t_struct white()
Definition: yuv.h:76
void set_color_line_points_est(YUV_t color)
Sets the line points color.
void set_own_pos_est(fawkes::field_pos_t own_position_estimate)
Own position estimate setter.
void set_color(unsigned char y, unsigned char u, unsigned char v)
Set drawing color.
Definition: drawer.cpp:77
void set_line_points_est(const fld_line_points_t *points_est)
Setter for detected line points.
float x
x coordinate
Definition: types.h:60