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
32using namespace fawkes;
33
34namespace 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 */
56FieldDrawer::FieldDrawer(const FieldLines &lines) : lines_(lines)
57{
58 points_ = NULL;
59 points_est_ = NULL;
60
62
66
69
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 */
85void
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 */
96void
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 */
107void
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 */
117void
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 */
136void
137FieldDrawer::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 */
147void
148FieldDrawer::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 */
161float
162FieldDrawer::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 */
173void
175{
176 c_background_ = color;
177}
178
179/**
180 * Sets the field color
181 * @param color to be used
182 */
183void
185{
186 c_field_ = color;
187}
188
189/**
190 * Sets the lines color
191 * @param color to be used
192 */
193void
195{
196 c_lines_ = color;
197}
198
199/**
200 * Sets the line points color
201 * @param color to be used
202 */
203void
205{
206 c_line_points_ = color;
207}
208
209/**
210 * Sets the line points color
211 * @param color to be used
212 */
213void
215{
216 c_line_points_est_ = color;
217}
218
219/**
220 * Sets the own position color
221 * @param color to be used
222 */
223void
225{
226 c_own_pos_ = color;
227}
228
229/**
230 * Sets the own position estimates color
231 * @param color to be used
232 */
233void
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 */
249void
250FieldDrawer::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);
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 */
409void
410FieldDrawer::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 */
463void
464FieldDrawer::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
Draw to an image.
Definition: drawer.h:32
void set_color(unsigned char y, unsigned char u, unsigned char v)
Set drawing color.
Definition: drawer.cpp:71
void draw_cross(unsigned int x_center, unsigned int y_center, unsigned int width)
Draws a cross.
Definition: drawer.cpp:449
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
void set_buffer(unsigned char *buffer, unsigned int width, unsigned int height)
Set the buffer to draw to.
Definition: drawer.cpp:58
void draw_circle(int center_x, int center_y, unsigned int radius)
Draw circle.
Definition: drawer.cpp:94
void set_line_points(const fld_line_points_t *points)
Setter for detected line points.
void set_line_points_est(const fld_line_points_t *points_est)
Setter for detected line points.
void set_color_own_pos(YUV_t color)
Sets the own position color.
void set_own_pos(fawkes::field_pos_t own_position)
Own position setter.
void clear_own_pos()
Clears the own position.
void set_color_own_pos_est(YUV_t color)
Sets the own position estimates color.
void set_own_pos_est(fawkes::field_pos_t own_position_estimate)
Own position estimate setter.
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]).
void set_color_field(YUV_t color)
Sets the field color.
void set_color_line_points(YUV_t color)
Sets the line points color.
void set_color_lines(YUV_t color)
Sets the lines color.
void set_head_yaw(float head_yaw)
Sets the angular offset between body and head (along the body axis)
void set_color_line_points_est(YUV_t color)
Sets the line points color.
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.
unsigned int _img_width
The width of the target image buffer.
Definition: field_drawer.h:68
virtual ~FieldDrawer()
Destructor.
unsigned char * _img_buffer
The pointer to the target image buffer.
Definition: field_drawer.h:67
unsigned int _img_height
The height of the target image buffer.
Definition: field_drawer.h:69
virtual void draw_lines(YUV_t color, bool draw_landscape=true, float scale=0) const
Draws the field lines to a SharedMemoryImageBuffer.
void set_color_background(YUV_t color)
Sets the background color (outside the field)
FieldDrawer(const FieldLines &lines)
Created a new field object.
virtual void draw_line_points(bool draw_landscape=true, float scale=0) const
Draws the line points.
This class acts as a container for lines on a soccer field.
Definition: field_lines.h:35
fawkes::cart_coord_2d_t get_field_offsets() const
Offset getter.
Definition: field_lines.h:51
float get_field_length() const
Field length getter.
Definition: field_lines.h:41
const field_circles_t & get_circles() const
Get circles.
Definition: field_lines.h:56
float get_field_width() const
Field width getter.
Definition: field_lines.h:46
Region of interest.
Definition: roi.h:55
bool contains(unsigned int x, unsigned int y)
Check if this ROI contains the given coordinates.
Definition: roi.cpp:281
Fawkes library namespace.
Cartesian coordinates (2D).
Definition: types.h:65
float y
y coordinate
Definition: types.h:67
float x
x coordinate
Definition: types.h:66
Position on the field.
Definition: types.h:125
float y
y coordinate in meters
Definition: types.h:127
float ori
orientation
Definition: types.h:128
float x
x coordinate in meters
Definition: types.h:126
YUV pixel.
Definition: yuv.h:58
unsigned char V
V component.
Definition: yuv.h:61
static YUV_t_struct cyan()
Definition: yuv.h:91
static YUV_t_struct green()
Definition: yuv.h:86
static YUV_t_struct white()
Definition: yuv.h:76
unsigned char U
U component.
Definition: yuv.h:60
static YUV_t_struct yellow()
Definition: yuv.h:111
static YUV_t_struct black()
Definition: yuv.h:81
unsigned char Y
Y component.
Definition: yuv.h:59