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