Fawkes API Fawkes Development Version

field_drawer.cpp

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
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends