Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * mirror_calib.cpp - Mirror calibration tool 00004 * 00005 * Created: Fri Dec 07 18:35:40 2007 00006 * Copyright 2007 Daniel Beck 00007 * Copyright 2009 Christoph Schwering 00008 * 00009 ****************************************************************************/ 00010 00011 /* This program is free software; you can redistribute it and/or modify 00012 * it under the terms of the GNU General Public License as published by 00013 * the Free Software Foundation; either version 2 of the License, or 00014 * (at your option) any later version. 00015 * 00016 * This program is distributed in the hope that it will be useful, 00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00019 * GNU Library General Public License for more details. 00020 * 00021 * Read the full text in the LICENSE.GPL file in the doc directory. 00022 */ 00023 00024 #include "mirror_calib.h" 00025 00026 #include <core/exception.h> 00027 #include <utils/math/angle.h> 00028 00029 #include <fvutils/color/yuv.h> 00030 #include <fvutils/readers/pnm.h> 00031 00032 #include <fvfilters/sobel.h> 00033 #include <fvfilters/sharpen.h> 00034 #include <fvfilters/median.h> 00035 #include <fvfilters/or.h> 00036 #include <fvfilters/laplace.h> 00037 #include <fvfilters/min.h> 00038 00039 #include <algorithm> 00040 #include <iostream> 00041 #include <string> 00042 #include <cstring> 00043 #include <cstdio> 00044 #include <cassert> 00045 00046 /* If defined, the center point is re-calculated by averaging the first 00047 * marks. */ 00048 //#define RECALCULATE_CENTER_POINT 00049 00050 #define FILTER_HOLES 00051 00052 /* If defined, very small holes are ignored. These small holes occur because a 00053 * single mark is sometimes recognized as *two* different marks due to the edge 00054 * detection. */ 00055 #define FILTER_MINI_HOLES 00056 00057 using namespace fawkes; 00058 00059 namespace firevision { 00060 00061 /** @class MirrorCalibTool mirror_calib.h 00062 * This class encapsulates the routines necessary for interactive mirror 00063 * calibration. 00064 * 00065 * The input is N pairs (degree,image) and (optionally) a filter mask. 00066 * The calibration runs in multiple phases: 00067 * (1) Edge detection. 00068 * (2) Assume center point in the middle of the image unless it is set 00069 * explicitly. 00070 * (3) Pre-marking: Find the edges that lie at `degree' wrt. Y axis in image. 00071 * (4) Final marking: Try to filter false-positive marks. 00072 * (5) Centering: Average the first marks in each direction to get the `real' 00073 * center point of the mirror. 00074 * (6) Again do steps (3) and (5) with the new center point. 00075 * (7) Generate bulb files. The polar coordinates of each pixel are determined 00076 * as follows: 00077 * (a) Determine the polar coordinates of the pixel. 00078 * (b) Get the two mark streams that enclose the pixel. 00079 * (c) Do linear interpolation in both mark streams to get two distances. 00080 * (d) Add the weightened distances (the nearer the pixel is to the mark 00081 * stream the higher is its weight). 00082 * (e) Now we have an estimated distance. We need to multiply the angle 00083 * with -1.0 (!!!) to consider the fact that the image is mirrored: 00084 * what looks to left from the robot on the image, is on the right of 00085 * the robot in reality! 00086 * Note that all the time we worked on the mirrored image with angles 00087 * that were correct from the perspective of the image. But to make them 00088 * correct from the perspective of the robot, we have to mirror 00089 * everything. 00090 */ 00091 00092 namespace { 00093 const unsigned int ORIENTATION_COUNT = (ORI_DEG_360 - ORI_DEG_0); 00094 const unsigned int MARK_COUNT = 6; 00095 const float MARK_DISTANCE = 29.7; 00096 const float CARPET_DISTANCE_FROM_ROBOT_CENTER = 5.0; 00097 const unsigned char DARK = 0; 00098 const unsigned char BRIGHT = 255; 00099 const unsigned char MARK_LUMA = 128; 00100 const unsigned char MARK_CHROMINANCE = 255; 00101 const int MIN_WIDTH_OF_BIGGEST_LINE = 50; 00102 const float APPROX_LINE_WIDTH_LOSS = 0.20f; 00103 const float MIN_BRIGHT_FRACTION = 0.20f; 00104 const int HIGHLIGHT_RADIUS = 2; 00105 const int FALSE_POSITIVE_HOLE_SIZE = 5; 00106 } 00107 00108 00109 /** 00110 * The result of a step contains a YUV buffer. 00111 * The different steps are descripted in the MirrorCalibTool's doc. 00112 */ 00113 class MirrorCalibTool::StepResult { 00114 private: 00115 unsigned char* buffer_; 00116 size_t buflen_; 00117 int width_; 00118 int height_; 00119 unsigned int* refcount_; 00120 00121 public: 00122 /** Constructor. 00123 * @param buflen 00124 * @param width 00125 * @param height */ 00126 StepResult(size_t buflen, int width, int height) 00127 : buffer_(new unsigned char[buflen]), 00128 buflen_(buflen), 00129 width_(width), 00130 height_(height), 00131 refcount_(new unsigned int) 00132 { 00133 *refcount_ = 1; 00134 } 00135 00136 /** Constructor. 00137 * @param copy */ 00138 StepResult(const StepResult& copy) 00139 : buffer_(copy.buffer_), 00140 buflen_(copy.buflen_), 00141 width_(copy.width_), 00142 height_(copy.height_), 00143 refcount_(copy.refcount_) 00144 { 00145 ++*refcount_; 00146 } 00147 00148 /** Assignment. 00149 * @param copy result to assign 00150 * @return reference to this instance */ 00151 StepResult& operator=(const StepResult& copy) 00152 { 00153 if (this != ©) { 00154 if (--*refcount_ == 0) { 00155 delete[] buffer_; 00156 delete refcount_; 00157 } 00158 buffer_ = copy.buffer_; 00159 refcount_ = copy.refcount_; 00160 buflen_ = copy.buflen_; 00161 width_ = copy.width_; 00162 height_ = copy.height_; 00163 ++*refcount_; 00164 } 00165 return *this; 00166 } 00167 00168 /** Destructor. */ 00169 ~StepResult() 00170 { 00171 if (--*refcount_ == 0) { 00172 delete[] buffer_; 00173 delete refcount_; 00174 } 00175 } 00176 00177 /** YUV buffer. 00178 * @return YUV buffer */ 00179 inline unsigned char* yuv_buffer() { return buffer_; } 00180 00181 /** YUV buffer. 00182 * @return YUV buffer */ 00183 inline const unsigned char* yuv_buffer() const { return buffer_; } 00184 00185 /** YUV buffer's length. 00186 * @return buffer length */ 00187 inline size_t buflen() const { return buflen_; } 00188 00189 /** Width of YUV buffer. 00190 * @return height */ 00191 inline int width() const { return width_; } 00192 00193 /** Height of YUV buffer. 00194 * @return height */ 00195 inline int height() const { return height_; } 00196 }; // StepResult 00197 00198 00199 /** 00200 * Abstract Point class. 00201 */ 00202 class MirrorCalibTool::Point { 00203 public: 00204 const int x; /**< X coordinate. */ 00205 const int y; /**< Y coordinate. */ 00206 00207 /** Constructor. 00208 * @param x 00209 * @param y */ 00210 Point(int x, int y) 00211 : x(x), 00212 y(y) 00213 { 00214 } 00215 00216 /** Length of the vector the point. 00217 * @return length of the vector the point. 00218 * @see atan() for polar coordinates. */ 00219 PolarRadius length() const 00220 { 00221 return static_cast<PolarRadius>(sqrt(x*x + y*y)); 00222 } 00223 00224 /** Atan(y, x) of the point. 00225 * @return Atan(y, x) of the point. 00226 * @see length() for polar coordinates. */ 00227 inline PolarAngle atan() const 00228 { 00229 return normalize_rad(atan2(y, x)); 00230 } 00231 00232 /** Assignment. 00233 * @return A copy. 00234 * @param p */ 00235 Point operator=(const Point& p) 00236 { 00237 if (&p == this) { 00238 return *this; 00239 } 00240 return Point(p.x, p.y); 00241 } 00242 }; // Point 00243 00244 00245 /** 00246 * A cartesian point is a 2d point which can have negative X and Y coords. 00247 */ 00248 class MirrorCalibTool::CartesianPoint 00249 : public Point 00250 { 00251 public: 00252 /** Constructor. 00253 * @param phi 00254 * @param length */ 00255 CartesianPoint(PolarAngle phi, PolarRadius length) 00256 : Point(length * cos(phi), 00257 length * sin(phi)) 00258 { 00259 } 00260 00261 /** Constructor. 00262 * @param x 00263 * @param y */ 00264 CartesianPoint(int x, int y) 00265 : Point(x, y) 00266 { 00267 } 00268 00269 /** Rotates the vector to the point counter-clockwise and returns the vector 00270 * to the point. 00271 * @param rotate_phi 00272 * @return Counter-clockwise rotated point. */ 00273 CartesianPoint rotate(PolarAngle rotate_phi) const 00274 { 00275 const PolarRadius len = length(); 00276 const PolarAngle phi = atan() + rotate_phi; 00277 const int x = len * cos(phi); 00278 const int y = len * sin(phi); 00279 return CartesianPoint(x, y); 00280 } 00281 }; // CartesianPoint 00282 00283 00284 /** 00285 * A pixel point is a 2d point with positive X and Y coords. 00286 */ 00287 class MirrorCalibTool::PixelPoint 00288 : public Point 00289 { 00290 public: 00291 /** Constructor. 00292 * @param x 00293 * @param y */ 00294 PixelPoint(int x, int y) 00295 : Point(x, y) 00296 { 00297 } 00298 00299 /** Rotates the vector to the point counter-clockwise and returns the vector 00300 * to the point. 00301 * @param rotate_phi Angle. 00302 * @return Counter-clockwise rotated point. */ 00303 PixelPoint rotate(PolarAngle rotate_phi) const 00304 { 00305 const PolarRadius len = length(); 00306 const PolarAngle phi = atan() + rotate_phi; 00307 const int x = len * cos(phi); 00308 const int y = len * sin(phi); 00309 return PixelPoint(x, y); 00310 } 00311 }; // PixelPoint 00312 00313 00314 /** 00315 * Wraps an image so that access to (0, 0) is mapped to the middle of the 00316 * image and so on. The result is a cartesian coordinate system with X and 00317 * Y axis. 00318 */ 00319 class MirrorCalibTool::CartesianImage 00320 { 00321 private: 00322 unsigned char* buf_; 00323 const int width_; 00324 const int height_; 00325 const PixelPoint center_; 00326 const PolarAngle phi_; 00327 const unsigned char* mask_; 00328 00329 public: 00330 /** Constructor. 00331 * @param res The step result that contains the base image. 00332 * @param phi The angle by which the image is rotated counter-clockwise. 00333 * @param center The center of the image from the robot's point of view; i.e. 00334 * the center of the cone mirror. 00335 * @param mask The mask that indicates which pixels should be ignored. */ 00336 CartesianImage(const StepResult& res, 00337 PolarAngle phi, 00338 PixelPoint center, 00339 const unsigned char* mask = 0) 00340 : buf_(const_cast<unsigned char*>(res.yuv_buffer())), 00341 width_(res.width()), 00342 height_(res.height()), 00343 center_(center), 00344 phi_(phi), 00345 mask_(mask) 00346 { 00347 } 00348 00349 /** Constructor. 00350 * @param res The step result that contains the base image. 00351 * @param phi The angle by which the image is rotated counter-clockwise. 00352 * @param mask The mask that indicates which pixels should be ignored. */ 00353 CartesianImage(const StepResult& res, 00354 PolarAngle phi, 00355 const unsigned char* mask = 0) 00356 : buf_(const_cast<unsigned char*>(res.yuv_buffer())), 00357 width_(res.width()), 00358 height_(res.height()), 00359 center_(PixelPoint(res.width()/2, res.height()/2)), 00360 phi_(phi), 00361 mask_(mask) 00362 { 00363 } 00364 00365 /** Constructor. 00366 * @param buf The base YUV buffer. 00367 * @param width Image width. 00368 * @param height Image height. 00369 * @param phi The angle by which the image is rotated counter-clockwise. 00370 * @param center The center of the image from the robot's point of view; i.e. 00371 * the center of the cone mirror. */ 00372 CartesianImage(const unsigned char* buf, 00373 int width, 00374 int height, 00375 PolarAngle phi, 00376 const PixelPoint& center) 00377 : buf_(const_cast<unsigned char*>(buf)), 00378 width_(width), 00379 height_(height), 00380 center_(center), 00381 phi_(phi), 00382 mask_(0) 00383 { 00384 } 00385 00386 /** Constructor. 00387 * @param buf The base YUV buffer. 00388 * @param width Image width. 00389 * @param height Image height. 00390 * @param phi The angle by which the image is rotated counter-clockwise. 00391 * @param center The center of the image from the robot's point of view; i.e. 00392 * the center of the cone mirror. */ 00393 CartesianImage(unsigned char* buf, 00394 int width, 00395 int height, 00396 PolarAngle phi, 00397 const PixelPoint& center) 00398 : buf_(buf), 00399 width_(width), 00400 height_(height), 00401 center_(center), 00402 phi_(phi), 00403 mask_(0) 00404 { 00405 } 00406 00407 /** Constructor. 00408 * @param buf The base YUV buffer. 00409 * @param width Image width. 00410 * @param height Image height. 00411 * @param phi The angle by which the image is rotated counter-clockwise. */ 00412 CartesianImage(const unsigned char* buf, 00413 int width, 00414 int height, 00415 PolarAngle phi) 00416 : buf_(const_cast<unsigned char*>(buf)), 00417 width_(width), 00418 height_(height), 00419 center_(PixelPoint(width/2, height/2)), 00420 phi_(phi), 00421 mask_(0) 00422 { 00423 } 00424 00425 /** Constructor. 00426 * @param buf The base YUV buffer. 00427 * @param width Image width. 00428 * @param height Image height. 00429 * @param phi The angle by which the image is rotated counter-clockwise. */ 00430 CartesianImage(unsigned char* buf, 00431 int width, 00432 int height, 00433 PolarAngle phi) 00434 : buf_(buf), 00435 width_(width), 00436 height_(height), 00437 center_(PixelPoint(width/2, height/2)), 00438 phi_(phi), 00439 mask_(0) 00440 { 00441 } 00442 00443 /** Get Buffer. 00444 * @return buffer 00445 */ 00446 inline unsigned char* buf() { return buf_; } 00447 /** Mask. 00448 * @return mask 00449 */ 00450 inline const unsigned char* mask() const { return mask_; } 00451 /** Buffer. 00452 * @return const buffer 00453 */ 00454 inline const unsigned char* buf() const { return buf_; } 00455 /** Center of buffer. 00456 * @return center point of buffer 00457 */ 00458 inline const PixelPoint& center() const { return center_; } 00459 /** Width of buffer. 00460 * @return width of buffer 00461 */ 00462 inline const int width() const { return width_; } 00463 /** Height of buffer. 00464 * @return height of buffer 00465 */ 00466 inline const int height() const { return height_; } 00467 /** Phi angle. 00468 * @return Phi angle 00469 */ 00470 inline const PolarAngle phi() const { return phi_; } 00471 00472 /** Converts a cartesian point to a pixel point. 00473 * @param p The point. 00474 * @return converted point 00475 */ 00476 inline PixelPoint 00477 to_pixel(const CartesianPoint& p) const 00478 { 00479 const CartesianPoint rp = p.rotate(-1.0 * phi()); 00480 return PixelPoint(center().x + rp.x, center().y - rp.y); 00481 } 00482 00483 /** Converts a pixel point to a cartesian point. 00484 * @param p The point. 00485 * @return converted point 00486 */ 00487 inline CartesianPoint 00488 to_cartesian(const PixelPoint& p) const 00489 { 00490 const CartesianPoint cp(p.x - center().x, center().y - p.y); 00491 return cp.rotate(phi()); 00492 } 00493 00494 /** Indicates whether the image contains a pixel point. 00495 * @param p The point. 00496 * @return true if pixel point is in image, false otherwise 00497 */ 00498 inline bool 00499 contains(const PixelPoint& p) const 00500 { 00501 return 0 <= p.x && p.x <= width()-1 && 0 <= p.y && p.y <= height()-1; 00502 } 00503 00504 /** Indicates whether the image contains a cartesian point. 00505 * @param p The point. 00506 * @return true if cartesian point is in image, false otherwise 00507 */ 00508 inline bool 00509 contains(const CartesianPoint& p) const 00510 { 00511 return contains(to_pixel(p)); 00512 } 00513 00514 /** Get luminance at a given point. 00515 * @param p The point. 00516 * @return luminance at given point 00517 */ 00518 inline unsigned char 00519 get(const CartesianPoint& p) const 00520 { 00521 if (!contains(p)) { 00522 throw fawkes::Exception("Point p is out of image"); 00523 } 00524 PixelPoint pp = to_pixel(p); 00525 const YUV_t ignr(0); 00526 if (mask() == 0 || 00527 (YUV422_PLANAR_Y_AT(mask(), width(), pp.x, pp.y) != ignr.Y && 00528 YUV422_PLANAR_U_AT(mask(), width(), height(), pp.x, pp.y) != ignr.U && 00529 YUV422_PLANAR_V_AT(mask(), width(), height(), pp.x, pp.y) != ignr.V)) { 00530 return YUV422_PLANAR_Y_AT(buf(), width(), pp.x, pp.y); 00531 } else { 00532 if (mask() != 0) { 00533 //printf("Ignoring (%lf,%d) = (%d,%d)\n", p.atan(), p.length(), pp.x, pp.y); 00534 } 00535 return 0; 00536 } 00537 } 00538 00539 /** Get Maximum cartesian X coordinate of the image. 00540 * @return Maximum cartesian X coordinate of the image. 00541 */ 00542 inline int max_x() const { return std::max(center().x, width() - center().x); } 00543 /** Get Maximum cartesian Y coordinate of the image. 00544 * @return Maximum cartesian Y coordinate of the image. 00545 */ 00546 inline int max_y() const { return std::max(center().y, height() - center().y); } 00547 /** Maximum polar radius of the image. 00548 * @return Maximum polar radius of the image. 00549 */ 00550 inline PolarRadius max_radius() const { 00551 return static_cast<PolarRadius>(sqrt(max_x()*max_x() + max_y()*max_y())); 00552 } 00553 00554 /** Sets the luminance Y and chrominance U at a given pixel point. 00555 * @param p The point whose luminance Y and chrominance U is changed. 00556 * @param luma The luminance Y. 00557 * @param chrominance The chrominance U. */ 00558 void 00559 set_color(const PixelPoint& p, 00560 unsigned char luma, 00561 unsigned char chrominance) 00562 { 00563 if (!contains(p)) { 00564 throw fawkes::Exception("Point p is out of image"); 00565 } 00566 YUV422_PLANAR_Y_AT(buf(), width(), p.x, p.y) = luma; 00567 YUV422_PLANAR_U_AT(buf(), width(), height(), p.x, p.y) = chrominance; 00568 } 00569 00570 /** Sets the luminance Y and chrominance U at a given cartesian point. 00571 * @param p The point whose luminance Y and chrominance U is changed. 00572 * @param luma The luminance Y. 00573 * @param chrominance The chrominance U. */ 00574 void 00575 set_color(const CartesianPoint& p, 00576 unsigned char luma, 00577 unsigned char chrominance) 00578 { 00579 set_color(to_pixel(p), luma, chrominance); 00580 } 00581 00582 /** Get relative amount of bright pixels. 00583 * @param from One point of the rectangle. 00584 * @param to Opposite point of the rectangle. 00585 * @return relative amount of BRIGHT pixels in the rectangle denoted 00586 * by the bottom-left (x1, y1) and the top-right (x2, y2). */ 00587 float 00588 bright_fraction(const CartesianPoint& from, 00589 const CartesianPoint& to) const 00590 { 00591 const int from_x = from.x < to.x ? from.x : to.x; 00592 const int to_x = from.x > to.x ? from.x : to.x; 00593 const int from_y = from.y < to.y ? from.y : to.y; 00594 const int to_y = from.y > to.y ? from.y : to.y; 00595 int pixel_count = 0; 00596 int bright_count = 0; 00597 for (int x = from_x; x <= to_x; x++) { 00598 for (int y = from_y; y <= to_y; y++) { 00599 const CartesianPoint p(x, y); 00600 if (contains(p)) { 00601 if (get(p) == BRIGHT) { 00602 ++bright_count; 00603 } 00604 } 00605 ++pixel_count; 00606 } 00607 } 00608 return static_cast<float>(static_cast<double>(bright_count) 00609 / static_cast<double>(pixel_count)); 00610 } 00611 00612 /** Indicates whether at pixel point p there is a highlighted line. 00613 * @param p The assumed center point of the line. 00614 * @param length The length of the line 00615 * @return true if pixel belongs to line, false otherwise 00616 */ 00617 bool 00618 is_line(const CartesianPoint& p, 00619 int length) const 00620 { 00621 for (int y_offset = 0; y_offset <= 1; y_offset++) { 00622 const CartesianPoint vec(p.atan() + deg2rad(90.0), length / 2 + 1); 00623 const CartesianPoint from(p.x - vec.x, p.y - vec.y); 00624 const CartesianPoint to(p.x + vec.x, p.y + vec.y); 00625 if (bright_fraction(from, to) >= MIN_BRIGHT_FRACTION) { 00626 return true; 00627 } 00628 } 00629 return false; 00630 } 00631 00632 /** Highlightes a line at pixel point p. 00633 * @param p The center point of the line. 00634 * @param length The length of the line. */ 00635 void 00636 highlight_line(const CartesianPoint& p, 00637 int length) 00638 { 00639 const CartesianPoint vec(p.atan() + deg2rad(90.0), length / 2); 00640 const CartesianPoint from(p.x - vec.x, p.y - vec.y); 00641 const CartesianPoint to(p.x + vec.x, p.y + vec.y); 00642 draw_line(from, to); 00643 } 00644 00645 /** Draws a line from p to q. 00646 * @param p First point of the line. 00647 * @param q First point of the line. */ 00648 void 00649 draw_line(const PixelPoint& p, 00650 const PixelPoint& q) 00651 { 00652 draw_line(to_cartesian(p), to_cartesian(q)); 00653 } 00654 00655 /** Draws a line from p to q. 00656 * @param p First point of the line. 00657 * @param q First point of the line. */ 00658 void 00659 draw_line(const CartesianPoint& p, 00660 const CartesianPoint& q) 00661 { 00662 const CartesianPoint distVec(q.x - p.x, q.y - p.y); 00663 for (PolarRadius length = 0; length <= distVec.length(); length++) { 00664 const CartesianPoint step(distVec.atan(), length); 00665 const CartesianPoint linePoint(p.x + step.x, p.y + step.y); 00666 if (contains(linePoint)) { 00667 set_color(linePoint, MARK_LUMA, MARK_CHROMINANCE); 00668 } 00669 } 00670 } 00671 00672 /** Highlights a pixel, i.e. sets the luminance and chrominance to 00673 * MARK_LUMA and MARK_CHROMINANCE. 00674 * @param p The pixel. */ 00675 void 00676 highlight_pixel(const PixelPoint& p) 00677 { 00678 const int R = HIGHLIGHT_RADIUS; 00679 for (int xx = p.x-R; xx <= p.x+R; xx++) { 00680 for (int yy = p.y-R; yy <= p.y+R; yy++) { 00681 const PixelPoint pp(xx, yy); 00682 if (contains(pp)) { 00683 set_color(pp, MARK_LUMA, MARK_CHROMINANCE); 00684 } 00685 } 00686 } 00687 } 00688 00689 /** Highlights a point, i.e. sets the luminance and chrominance to 00690 * MARK_LUMA and MARK_CHROMINANCE. 00691 * @param p The point. */ 00692 void 00693 highlight_point(const CartesianPoint& p) 00694 { 00695 const int R = HIGHLIGHT_RADIUS; 00696 for (int xx = p.x-R; xx <= p.x+R; xx++) { 00697 for (int yy = p.y-R; yy <= p.y+R; yy++) { 00698 const CartesianPoint hp(xx, yy); 00699 if (contains(hp)) { 00700 set_color(hp, MARK_LUMA, MARK_CHROMINANCE); 00701 } 00702 } 00703 } 00704 } 00705 }; // CartesianImage 00706 00707 00708 /** @class MirrorCalibTool::ConvexPolygon libs/fvmodels/mirror/mirror_calib.h 00709 * Represents a convex polygon. It is defined by a sequence of points in 00710 * clock-wise-order. 00711 */ 00712 00713 /** Check if point is inside convex polygon. 00714 * The point r is converted to PixelPoint wrt img. 00715 * @param img image in which to check 00716 * @param r cartesian point to check 00717 * @return true if the point is inside the convex polygon 00718 */ 00719 bool 00720 MirrorCalibTool::ConvexPolygon::contains(const CartesianImage& img, 00721 const CartesianPoint& r) const 00722 { 00723 return contains(img.to_pixel(r)); 00724 } 00725 00726 00727 /** Check if pixel point is inside convex polygon. 00728 * This is the case if for all points p, q in the polygon p_1, ..., p_n 00729 * where p = p_i, q = p_{i+1} for some i or p = p_n, q = p_1 it holds 00730 * (p, q, r) does not form a left turn (if they do, they are 00731 * counter-clock-wise). 00732 * @param r point to check 00733 * @return true if the point is inside the convex polygon 00734 */ 00735 bool 00736 MirrorCalibTool::ConvexPolygon::contains(const PixelPoint& r) const 00737 { 00738 for (std::vector<PixelPoint>::size_type i = 1; i <= size(); i++) { 00739 const PixelPoint& p = at(i-1); 00740 const PixelPoint& q = at(i % size()); 00741 double val = (q.x - p.x) * (r.y - p.y) - (r.x - p.x) * (q.y - p.y); 00742 if (val < 0) { // (p, q, r) forms a left turn 00743 return false; 00744 } 00745 } 00746 return true; 00747 } 00748 00749 00750 /** A hole is a sequence of pixels between two lines. */ 00751 class MirrorCalibTool::Hole { 00752 public: 00753 int index; /**< The index of the hole. */ 00754 PolarRadius from_length; /**< Beginning of the hole. */ 00755 PolarRadius to_length; /**< Ending of the hole. */ 00756 00757 /** Constructor. 00758 * @param index The index of the hole. 00759 * @param from_length The beginning of the hole in pixels. 00760 * @param to_length The ending of the hole in pixels. */ 00761 Hole(int index, 00762 PolarRadius from_length, 00763 PolarRadius to_length) 00764 : index(index), 00765 from_length(from_length), 00766 to_length(to_length) 00767 { 00768 } 00769 00770 /** The size of the hole in pixels. 00771 * @return radius of hole 00772 */ 00773 inline PolarRadius size() const { return to_length - from_length; } 00774 }; // Hole 00775 00776 00777 /** A container for a YUV-buffer etc. */ 00778 class MirrorCalibTool::Image 00779 { 00780 public: 00781 /** Constructor. 00782 * @param yuv_buffer The YUV buffer. 00783 * @param buflen The buffer's size. 00784 * @param width The width. 00785 * @param height The height. 00786 * @param ori The orientation. */ 00787 Image(const unsigned char* yuv_buffer, 00788 size_t buflen, 00789 int width, 00790 int height, 00791 PolarAngle ori) 00792 : yuv_buffer_(new unsigned char[buflen]), 00793 buflen_(buflen), 00794 width_(width), 00795 height_(height), 00796 ori_(ori), 00797 refcount_(new unsigned int) 00798 { 00799 memcpy(yuv_buffer_, yuv_buffer, buflen); 00800 *refcount_ = 1; 00801 } 00802 00803 /** Constructor. 00804 * @param copy */ 00805 Image(const Image& copy) 00806 : yuv_buffer_(copy.yuv_buffer_), 00807 buflen_(copy.buflen_), 00808 width_(copy.width_), 00809 height_(copy.height_), 00810 ori_(copy.ori_), 00811 results_(copy.results_), 00812 premarks_(copy.premarks_), 00813 marks_(copy.marks_), 00814 refcount_(copy.refcount_) 00815 { 00816 ++*refcount_; 00817 } 00818 00819 /** Assignment. 00820 * @param copy image to copy 00821 * @return this image 00822 */ 00823 Image& operator=(const Image& copy) 00824 { 00825 if (this != ©) { 00826 if (--*refcount_ == 0) { 00827 delete[] yuv_buffer_; 00828 delete refcount_; 00829 } 00830 yuv_buffer_ = copy.yuv_buffer_; 00831 buflen_ = copy.buflen_; 00832 width_ = copy.width_; 00833 height_ = copy.height_; 00834 ori_ = copy.ori_; 00835 results_ = copy.results_; 00836 premarks_ = copy.premarks_; 00837 marks_ = copy.marks_; 00838 refcount_ = copy.refcount_; 00839 ++*copy.refcount_; 00840 } 00841 return *this; 00842 } 00843 00844 /** Destructor. */ 00845 ~Image() 00846 { 00847 if (--*refcount_ == 0) { 00848 delete[] yuv_buffer_; 00849 delete refcount_; 00850 } 00851 } 00852 00853 /** YUV buffer. 00854 * @return YUV buffer 00855 */ 00856 inline unsigned char* yuv_buffer() { return yuv_buffer_; } 00857 /** YUV buffer. 00858 * @return 00859 */ 00860 inline const unsigned char* yuv_buffer() const { return yuv_buffer_; } 00861 /** YUV buffer's length. 00862 * @return YUV buffer's length 00863 */ 00864 inline size_t buflen() const { return buflen_; } 00865 /** YUV buffer's width. 00866 * @return YUV buffer's width 00867 */ 00868 inline int width() const { return width_; } 00869 /** YUV buffer's height. 00870 * @return YUV buffer's height 00871 */ 00872 inline int height() const { return height_; } 00873 /** Angle of marks wrt X axis. 00874 * @return Angle of marks wrt X axis 00875 */ 00876 inline PolarAngle ori() const { return ori_; } 00877 /** List of results. 00878 * @return List of results 00879 */ 00880 inline StepResultList& results() { return results_; } 00881 /** List of results. 00882 * @return List of results 00883 */ 00884 inline const StepResultList& results() const { return results_; } 00885 /** The premarks. 00886 * @return The premarks 00887 */ 00888 inline const MarkList& premarks() { return premarks_; } 00889 /** The (final) marks. 00890 * @return 00891 */ 00892 inline MarkList& marks() { return marks_; } 00893 /** The (final) marks. 00894 * @return The (final) marks 00895 */ 00896 inline const MarkList& marks() const { return marks_; } 00897 00898 /** Appends a result. 00899 * @param r The new result. */ 00900 inline void add_result(const StepResult& r) { results_.push_back(r); } 00901 /** Returns the i-th result. 00902 * @param i The index of the result starting with 0. 00903 * @return result 00904 */ 00905 inline StepResult& result(int i) { return results_[i]; } 00906 /** Returns the i-th result. 00907 * @param i The index of the result starting with 0. 00908 * @return result 00909 */ 00910 inline const StepResult& result(int i) const { return results_[i]; } 00911 /** The premarks. 00912 * @param premarks The list of premarks. */ 00913 inline void set_premarks(const MarkList& premarks) { premarks_ = premarks; } 00914 /** The (final) marks. 00915 * @param marks The list of marks. */ 00916 inline void set_marks(const MarkList& marks) { marks_ = marks; } 00917 00918 private: 00919 unsigned char* yuv_buffer_; 00920 size_t buflen_; 00921 int width_; 00922 int height_; 00923 PolarAngle ori_; 00924 StepResultList results_; 00925 MarkList premarks_; 00926 MarkList marks_; 00927 unsigned int* refcount_; 00928 }; // Image 00929 00930 00931 /** Constructor. */ 00932 MirrorCalibTool::MirrorCalibTool() 00933 : img_yuv_buffer_(0), 00934 img_center_x_(-1), 00935 img_center_y_(-1), 00936 img_yuv_mask_(0), 00937 state_(CalibrationState()) 00938 , bulb_(0) 00939 { 00940 } 00941 00942 00943 /** Destructor. */ 00944 MirrorCalibTool::~MirrorCalibTool() 00945 { 00946 if (img_yuv_buffer_) { 00947 delete[] img_yuv_buffer_; 00948 } 00949 if (img_yuv_mask_) { 00950 delete[] img_yuv_mask_; 00951 } 00952 if (bulb_) { 00953 delete bulb_; 00954 } 00955 } 00956 00957 00958 /** 00959 * Converts an angle wrt. the Y axis (!) to the robots view to the needed image 00960 * rotation so that the things, which lie at angle `ori' wrt. the Y axis, lie 00961 * on the X axis of the rotated image. 00962 * For example: if the marks are 120 degrees counter-clock-wise from the robot, 00963 * the image needs to be rotated 120 degrees clock-wise (then the marks are 00964 * in front of the robot, i.e. Y axis) and then 90 degrees clock-wise 00965 * (to get the marks from the Y axis to the X axis). 00966 * 00967 * The reason that we want to have the marks on the X axis is that calculating 00968 * with them is then a bit easier (because then their polar angle is always 00969 * 0.0). 00970 * @param ori The orientation. 00971 * @return angle 00972 */ 00973 MirrorCalibTool::PolarAngle 00974 MirrorCalibTool::relativeOrientationToImageRotation(PolarAngle ori) 00975 { 00976 return normalize_rad(-1.0 * ori + deg2rad(-90.0)); 00977 } 00978 00979 00980 /** 00981 * Converts the rotation of the image to the orientation relative to the Y axis. 00982 * See the documentation of relativeOrientationToImageRotation() of which this 00983 * is the inverse. 00984 * @param ori The orientation. 00985 * @return angle 00986 */ 00987 MirrorCalibTool::PolarAngle 00988 MirrorCalibTool::imageRotationToRelativeOrientation(PolarAngle ori) 00989 { 00990 return normalize_rad(-1.0 * (ori + deg2rad(90.0))); 00991 } 00992 00993 00994 /** 00995 * Loads a PNM mask file for the robot's bars. 00996 * The mask is used to ignore those pixels that are part of the robot's bar. 00997 * @param mask_file_name The mask file name. 00998 */ 00999 void 01000 MirrorCalibTool::load_mask(const char* mask_file_name) 01001 { 01002 if (img_yuv_mask_) { 01003 delete[] img_yuv_mask_; 01004 } 01005 PNMReader reader(mask_file_name); 01006 size_t size = colorspace_buffer_size(reader.colorspace(), 01007 reader.pixel_width(), 01008 reader.pixel_height()); 01009 img_yuv_mask_ = new unsigned char[size]; 01010 if (img_center_x_ == -1) { 01011 img_center_x_ = reader.pixel_width() / 2; 01012 } 01013 if (img_center_y_ == -1) { 01014 img_center_y_ = reader.pixel_height() / 2; 01015 } 01016 reader.set_buffer(img_yuv_mask_); 01017 reader.read(); 01018 } 01019 01020 01021 /** Store image for calibration process. 01022 * @param yuv_buffer The image's yuv_buffer. It may be freeed by the caller 01023 * immediately, the MirrorCalibTool works with a copy of it. 01024 * @param buflen The length of yuv_buffer. 01025 * @param width The width of the image. 01026 * @param height The height of the image. 01027 * @param ori The polar angle in degrees (!) relative to the Y axis of 01028 * the image (!) where the marks are in the image (!) (not in 01029 * in reality from the robot's perspective). 01030 */ 01031 void 01032 MirrorCalibTool::push_back(const unsigned char* yuv_buffer, 01033 size_t buflen, 01034 int width, 01035 int height, 01036 double ori) 01037 { 01038 ori = relativeOrientationToImageRotation(ori); 01039 Image src_img(yuv_buffer, buflen, width, height, ori); 01040 if (img_center_x_ == -1) { 01041 img_center_x_ = width / 2; 01042 } 01043 if (img_center_y_ == -1) { 01044 img_center_y_ = height / 2; 01045 } 01046 source_images_.push_back(src_img); 01047 } 01048 01049 01050 /** Aborts the calibration process. */ 01051 void 01052 MirrorCalibTool::abort() 01053 { 01054 img_center_x_ = -1; 01055 img_center_y_ = -1; 01056 state_ = CalibrationState(); 01057 source_images_.clear(); 01058 premarks_.clear(); 01059 mark_map_.clear(); 01060 } 01061 01062 01063 /** Applies a sobel filter for edge detection in some direction. 01064 * @param src The source YUV buffer. 01065 * @param dst The destination YUV buffer. 01066 * @param width The width. 01067 * @param height The height. 01068 * @param ori The orientation. */ 01069 void 01070 MirrorCalibTool::apply_sobel(unsigned char* src, 01071 unsigned char* dst, 01072 int width, 01073 int height, 01074 orientation_t ori) 01075 { 01076 ROI* roi = ROI::full_image(width, height); 01077 FilterSobel filter; 01078 filter.set_src_buffer(src, roi, ori, 0); 01079 filter.set_dst_buffer(dst, roi); 01080 filter.apply(); 01081 } 01082 01083 01084 /** Applies a sharpening filter. 01085 * @param src The source YUV buffer. 01086 * @param dst The destination YUV buffer. 01087 * @param width The width. 01088 * @param height The height. */ 01089 void 01090 MirrorCalibTool::apply_sharpen(unsigned char* src, 01091 unsigned char* dst, 01092 int width, 01093 int height) 01094 { 01095 ROI* roi = ROI::full_image(width, height); 01096 FilterSharpen filter; 01097 filter.set_src_buffer(src, roi, 0); 01098 filter.set_dst_buffer(dst, roi); 01099 filter.apply(); 01100 } 01101 01102 01103 /** Applies a median filter. 01104 * @param src The source YUV buffer. 01105 * @param dst The destination YUV buffer. 01106 * @param width The width. 01107 * @param height The height. 01108 * @param i The median parameter. */ 01109 void 01110 MirrorCalibTool::apply_median(unsigned char* src, 01111 unsigned char* dst, 01112 int width, 01113 int height, 01114 int i) 01115 { 01116 ROI* roi = ROI::full_image(width, height); 01117 FilterMedian filter(i); 01118 filter.set_src_buffer(src, roi, 0); 01119 filter.set_dst_buffer(dst, roi); 01120 filter.apply(); 01121 } 01122 01123 01124 /** Applies a minimum filter. 01125 * @param src The source YUV buffer. 01126 * @param dst The destination YUV buffer. 01127 * @param width The width. 01128 * @param height The height. */ 01129 void 01130 MirrorCalibTool::apply_min(unsigned char* src, 01131 unsigned char* dst, 01132 int width, 01133 int height) 01134 { 01135 ROI* roi = ROI::full_image(width, height); 01136 FilterMin filter; 01137 filter.set_src_buffer(src, roi, 0); 01138 filter.set_dst_buffer(dst, roi); 01139 filter.apply(); 01140 } 01141 01142 01143 /** Applies a disjunction filter. 01144 * @param src1 The first source YUV buffer. 01145 * @param src2 The second source YUV buffer. 01146 * @param dst The destination YUV buffer. 01147 * @param width The width. 01148 * @param height The height. */ 01149 void 01150 MirrorCalibTool::apply_or(unsigned char* src1, 01151 unsigned char* src2, 01152 unsigned char* dst, 01153 int width, 01154 int height) 01155 { 01156 ROI* roi = ROI::full_image(width, height); 01157 FilterOr filter; 01158 filter.set_src_buffer(src1, roi, 0); 01159 filter.set_src_buffer(src2, roi, 1); 01160 filter.set_dst_buffer(dst, roi); 01161 filter.apply(); 01162 } 01163 01164 01165 /** Sets all pixels to black or white (i.e. maximum contrast). 01166 * @param buf The YUV buffer. 01167 * @param buflen Buffer's length. */ 01168 void 01169 MirrorCalibTool::make_contrast(unsigned char* buf, 01170 size_t buflen) 01171 { 01172 for (unsigned int i = 0; i < buflen/2; i++) { 01173 buf[i] = (buf[i] >= 75) ? BRIGHT : DARK; 01174 } 01175 } 01176 01177 01178 /** Sets all chrominance values to 128. 01179 * @param buf The YUV buffer. 01180 * @param buflen Buffer's length. */ 01181 void 01182 MirrorCalibTool::make_grayscale(unsigned char* buf, 01183 size_t buflen) 01184 { 01185 memset(buf + buflen/2, 128, buflen - buflen/2); 01186 } 01187 01188 01189 /** Get description of next step. 01190 * @return string that describes what's done in next_step(). 01191 */ 01192 const char* 01193 MirrorCalibTool::get_state_description() const 01194 { 01195 switch (state_.step) { 01196 case SHARPENING: return "Sharpen"; 01197 case EDGE_DETECTION: return "Edge detection"; 01198 case COMBINATION: return "Combining images"; 01199 case CENTERING: return "Finding center point"; 01200 case PRE_MARKING: return "First marking"; 01201 case FINAL_MARKING: return "Final marking"; 01202 case DONE: return "Direction done"; 01203 default: return "Invalid state"; 01204 } 01205 } 01206 01207 01208 /** Finds the first marks. This is the first step in finding marks. 01209 * @return mark list 01210 */ 01211 MirrorCalibTool::MarkList 01212 MirrorCalibTool::premark(const StepResult& prev, 01213 const unsigned char* yuv_mask, 01214 StepResult& result, 01215 PolarAngle phi, 01216 const PixelPoint& center) 01217 { 01218 const ConvexPolygon empty_polygon; 01219 return premark(empty_polygon, prev, yuv_mask, result, phi, center); 01220 } 01221 01222 01223 /** Finds the first marks. This is the first step in finding marks. 01224 * @return mark list 01225 */ 01226 MirrorCalibTool::MarkList 01227 MirrorCalibTool::premark(const ConvexPolygon& polygon, 01228 const StepResult& prev, 01229 const unsigned char* yuv_mask, 01230 StepResult& result, 01231 PolarAngle phi, 01232 const PixelPoint& center) 01233 { 01234 const CartesianImage prev_img(prev, phi, center, yuv_mask); 01235 CartesianImage res_img(result, phi, center, yuv_mask); 01236 int width = MIN_WIDTH_OF_BIGGEST_LINE; 01237 MarkList premarks; 01238 for (PolarRadius length = 0; length < prev_img.max_radius(); length++) 01239 { 01240 const CartesianPoint p(0.0, length); 01241 if (polygon.contains(prev_img, p) && prev_img.is_line(p, width)) { 01242 premarks.push_back(length); 01243 res_img.highlight_line(p, width); 01244 } 01245 if (length % 25 == 0) { 01246 width *= (1.0f - APPROX_LINE_WIDTH_LOSS); 01247 } 01248 } 01249 return premarks; 01250 } 01251 01252 01253 /** Searches for holes between the pre-marks. Helper for mark(). 01254 * @return holes 01255 */ 01256 MirrorCalibTool::HoleList 01257 MirrorCalibTool::search_holes(const MarkList& premarks) 01258 { 01259 HoleList holes; 01260 PolarRadius prev_radius = -1; 01261 for (MarkList::const_iterator it = premarks.begin(); 01262 it != premarks.end(); it++) 01263 { 01264 PolarRadius radius = *it; 01265 if (prev_radius != -1 && prev_radius + 1 < radius) { 01266 Hole hole(holes.size(), prev_radius, radius-1); 01267 holes.push_back(hole); 01268 } 01269 prev_radius = radius; 01270 } 01271 return holes; 01272 } 01273 01274 01275 /** Removes all but the n biggest holes (size depends on their position). 01276 * Helper for mark(). */ 01277 MirrorCalibTool::HoleList 01278 MirrorCalibTool::filter_biggest_holes(const HoleList& holes, 01279 unsigned int n) 01280 { 01281 #ifdef FILTER_HOLES 01282 HoleList biggest = holes; 01283 #ifdef FILTER_MINI_HOLES 01284 restart: // XXX ugly :-) 01285 for (HoleList::iterator it = biggest.begin(); it != biggest.end(); it++) 01286 { 01287 if (it->size() <= FALSE_POSITIVE_HOLE_SIZE) { 01288 biggest.erase(it); 01289 goto restart; 01290 } 01291 } 01292 #endif 01293 01294 HoleList filtered; 01295 for (unsigned int from = 0; from < biggest.size(); from++) 01296 { 01297 unsigned int to; 01298 for (to = from + 1; to < biggest.size(); to++) { 01299 if ((to - from + 1) > n) { 01300 to = from + n; 01301 break; 01302 } 01303 if (biggest[to - 1].size() < biggest[to].size()) { 01304 break; 01305 } 01306 } 01307 to--; // in all three break cases, to must be decremented 01308 if (to - from + 1 > filtered.size()) { 01309 filtered.clear(); 01310 for (unsigned int j = from; j <= to; j++) { 01311 filtered.push_back(biggest[j]); 01312 } 01313 } 01314 } 01315 return filtered; 01316 #else 01317 HoleList biggest; 01318 for (HoleList::const_iterator it = holes.begin(); it != holes.end(); ++it) 01319 { 01320 const Hole& hole = *it; 01321 #ifdef FILTER_MINI_HOLES 01322 if (hole.size() < FALSE_POSITIVE_HOLE_SIZE) { 01323 // very small holes are usually false-positives 01324 continue; 01325 } 01326 #endif 01327 if (biggest.size() == 1 && hole.size() > biggest.front().size()) { 01328 // often the first determined hole is a part of the robot 01329 //biggest.erase(biggest.begin()); 01330 } 01331 biggest.push_back(hole); 01332 if (biggest.size() == n) { 01333 break; 01334 } 01335 } 01336 return biggest; 01337 #endif 01338 } 01339 01340 01341 /** Calculates the position of the marks between holes. Helper for mark(). */ 01342 MirrorCalibTool::MarkList 01343 MirrorCalibTool::determine_marks(const HoleList& holes) 01344 { 01345 HoleList biggest = filter_biggest_holes(holes, MARK_COUNT - 1); 01346 std::cout << "Filtered Holes: " << biggest.size() << std::endl; 01347 MarkList marks; 01348 for (HoleList::const_iterator prev, iter = biggest.begin(); 01349 iter != biggest.end(); iter++) { 01350 const Hole& hole = *iter; 01351 if (iter == biggest.begin()) { 01352 const PolarRadius length = hole.from_length; 01353 marks.push_back(length); 01354 } else { 01355 const PolarRadius length = (hole.from_length + prev->to_length) / 2; 01356 marks.push_back(length); 01357 } 01358 if (iter+1 == biggest.end()) { 01359 const PolarRadius length = hole.to_length; 01360 marks.push_back(length); 01361 } 01362 prev = iter; 01363 } 01364 return marks; 01365 } 01366 01367 01368 /** Sets marks between all holes. The last step of marking. */ 01369 MirrorCalibTool::MarkList 01370 MirrorCalibTool::mark(const MarkList& premarks, 01371 const unsigned char* yuv_mask, 01372 StepResult& result, 01373 PolarAngle phi, 01374 const PixelPoint& center) 01375 { 01376 std::cout << "Premarked places: " << premarks.size() << std::endl; 01377 HoleList holes = search_holes(premarks); 01378 std::cout << "Found Holes: " << holes.size() << std::endl; 01379 MarkList marks = determine_marks(holes); 01380 std::cout << "Found Marks: " << marks.size() << std::endl; 01381 01382 CartesianImage res_img(result, phi, center, yuv_mask); 01383 for (MarkList::const_iterator iter = marks.begin(); 01384 iter != marks.end(); iter++) 01385 { 01386 const PolarAngle angle = 0.0; 01387 const PolarRadius radius = *iter; 01388 res_img.highlight_point(CartesianPoint(angle, radius)); 01389 std::cout << "Highlighting Mark at " << *iter << std::endl; 01390 } 01391 return marks; 01392 } 01393 01394 01395 void 01396 MirrorCalibTool::goto_next_state() 01397 { 01398 const Image& src_img = source_images_[state_.image_index]; 01399 switch (state_.step) { 01400 case SHARPENING: 01401 state_.step = EDGE_DETECTION; 01402 break; 01403 case EDGE_DETECTION: 01404 if (src_img.results().size() <= ORIENTATION_COUNT) { 01405 state_.step = EDGE_DETECTION; 01406 } else { 01407 state_.step = COMBINATION; 01408 } 01409 break; 01410 case COMBINATION: 01411 if (src_img.results().size() < 2 * ORIENTATION_COUNT) { 01412 state_.step = COMBINATION; 01413 } else if (state_.image_index + 1 < source_images_.size()) { 01414 state_.step = SHARPENING; 01415 state_.image_index++; 01416 } else { 01417 state_.step = PRE_MARKING; 01418 state_.centering_done = false; 01419 state_.image_index = 0; 01420 } 01421 break; 01422 case CENTERING: 01423 state_.step = PRE_MARKING; 01424 state_.centering_done = true; 01425 state_.image_index = 0; 01426 break; 01427 case PRE_MARKING: 01428 state_.step = FINAL_MARKING; 01429 break; 01430 case FINAL_MARKING: 01431 if (state_.image_index + 1 < source_images_.size()) { 01432 state_.step = PRE_MARKING; 01433 state_.image_index++; 01434 #ifdef RECALCULATE_CENTER_POINT 01435 } else if (!state_.centering_done) { 01436 state_.step = CENTERING; 01437 state_.image_index = 0; 01438 #endif 01439 } else { 01440 state_.step = DONE; 01441 state_.image_index = 0; 01442 } 01443 break; 01444 case DONE: 01445 state_.step = DONE; 01446 state_.image_index = (state_.image_index + 1) % source_images_.size(); 01447 break; 01448 default: 01449 throw fawkes::Exception("Invalid Calibration State"); 01450 } 01451 printf("Next step: %s\n", get_state_description()); 01452 } 01453 01454 01455 /** Performs one step in the calibration process. 01456 * In each step, a filter is applied to the image. The resulting image of 01457 * the n-th step is stored in the stepResults-vector on the (n-1)-th position. 01458 */ 01459 void 01460 MirrorCalibTool::next_step() 01461 { 01462 printf("Performing step for image %u / %u, %s\n", 01463 (unsigned int)state_.image_index + 1, 01464 (unsigned int)source_images_.size(), 01465 get_state_description()); 01466 if (state_.image_index >= source_images_.size()) { 01467 return; 01468 } 01469 Image& src_img = source_images_[state_.image_index]; 01470 StepResult result(src_img.buflen(), src_img.width(), src_img.height()); 01471 switch (state_.step) { 01472 case SHARPENING: 01473 { 01474 apply_sharpen(src_img.yuv_buffer(), result.yuv_buffer(), 01475 src_img.width(), src_img.height()); 01476 make_grayscale(result.yuv_buffer(), result.buflen()); 01477 } 01478 src_img.add_result(result); 01479 set_last_yuv_buffer(result.yuv_buffer()); 01480 goto_next_state(); 01481 return; 01482 case EDGE_DETECTION: 01483 { 01484 StepResult& prev = src_img.result(0); 01485 const int offset = (src_img.results().size() - 1) % ORIENTATION_COUNT; 01486 const orientation_t ori = static_cast<orientation_t>(ORI_DEG_0+offset); 01487 apply_sobel(prev.yuv_buffer(), result.yuv_buffer(), 01488 prev.width(), prev.height(), ori); 01489 make_grayscale(result.yuv_buffer(), result.buflen()); 01490 make_contrast(result.yuv_buffer(), result.buflen()); 01491 } 01492 printf("Edge detection in %u\n", (unsigned) src_img.results().size()); 01493 src_img.add_result(result); 01494 set_last_yuv_buffer(result.yuv_buffer()); 01495 assert(!memcmp(result.yuv_buffer(), 01496 get_last_yuv_buffer(), 01497 result.buflen())); 01498 goto_next_state(); 01499 return; 01500 case COMBINATION: 01501 { 01502 const int index1 = src_img.results().size() - ORIENTATION_COUNT == 1 01503 ? src_img.results().size() - ORIENTATION_COUNT 01504 : src_img.results().size() - 1; 01505 const int index2 = src_img.results().size() - ORIENTATION_COUNT + 1; 01506 assert(index1 != index2); 01507 printf("ORing: %d = or(%d, %d)\n", (unsigned) src_img.results().size(), 01508 index1, index2); 01509 StepResult& prev1 = src_img.result(index1); 01510 StepResult& prev2 = src_img.result(index2); 01511 assert(&prev1 != &prev2); 01512 assert(prev1.width() == prev2.width()); 01513 assert(prev1.height() == prev2.height()); 01514 apply_or(prev1.yuv_buffer(), prev2.yuv_buffer(), result.yuv_buffer(), 01515 prev1.width(), prev1.height()); 01516 make_grayscale(result.yuv_buffer(), result.buflen()); 01517 assert(memcmp(prev1.yuv_buffer(), 01518 prev2.yuv_buffer(), 01519 result.buflen())); 01520 assert(memcmp(result.yuv_buffer(), 01521 prev1.yuv_buffer(), 01522 result.buflen())); 01523 assert(memcmp(result.yuv_buffer(), 01524 prev1.yuv_buffer(), 01525 result.buflen())); 01526 } 01527 src_img.add_result(result); 01528 set_last_yuv_buffer(result.yuv_buffer()); 01529 assert(!memcmp(result.yuv_buffer(), 01530 get_last_yuv_buffer(), 01531 result.buflen())); 01532 goto_next_state(); 01533 return; 01534 case CENTERING: 01535 { 01536 const StepResult& orig = src_img.result(0); 01537 memcpy(result.yuv_buffer(), orig.yuv_buffer(), result.buflen()); 01538 const PixelPoint center = calculate_center(source_images_); 01539 img_center_x_ = center.x; 01540 img_center_y_ = center.y; 01541 std::cout << "Found center (" << center.x << ", "<< center.y << ")" 01542 << std::endl; 01543 CartesianImage img(result, relativeOrientationToImageRotation(0.0), 01544 center); 01545 img.highlight_point(CartesianPoint(0, 0)); 01546 } 01547 src_img.add_result(result); 01548 set_last_yuv_buffer(result.yuv_buffer()); 01549 goto_next_state(); 01550 return; 01551 case PRE_MARKING: 01552 { 01553 const StepResult& prev = src_img.result(2 * ORIENTATION_COUNT - 1); 01554 memcpy(result.yuv_buffer(), prev.yuv_buffer(), result.buflen()); 01555 const unsigned char* mask = img_yuv_mask_; 01556 const PixelPoint center(img_center_x_, img_center_y_); 01557 MarkList premarks = premark(prev, mask, result, src_img.ori(), center); 01558 src_img.set_premarks(premarks); 01559 apply_sharpen(src_img.yuv_buffer(), result.yuv_buffer(), 01560 src_img.width(), src_img.height()); 01561 } 01562 src_img.add_result(result); 01563 set_last_yuv_buffer(result.yuv_buffer()); 01564 goto_next_state(); 01565 return; 01566 case FINAL_MARKING: 01567 { 01568 const StepResult& orig = src_img.result(0); 01569 memcpy(result.yuv_buffer(), orig.yuv_buffer(), result.buflen()); 01570 const unsigned char* mask = img_yuv_mask_; 01571 const PixelPoint center(img_center_x_, img_center_y_); 01572 const MarkList marks = mark(src_img.premarks(), mask, result, 01573 src_img.ori(), center); 01574 src_img.set_marks(marks); 01575 const PolarAngle ori = src_img.ori(); 01576 std::cout << "Marking done for orientation " 01577 << rad2deg(ori) 01578 << " = " 01579 << rad2deg(imageRotationToRelativeOrientation(ori)) 01580 << std::endl; 01581 } 01582 src_img.add_result(result); 01583 set_last_yuv_buffer(result.yuv_buffer()); 01584 goto_next_state(); 01585 return; 01586 case DONE: 01587 { 01588 for (ImageList::iterator it = source_images_.begin(); 01589 it != source_images_.end(); it++) 01590 { 01591 // cleanup: except the first result (whose yuv_buffer is needed) 01592 // no results are needed any more 01593 StepResultList results = it->results(); 01594 results.erase(results.begin() + 1, results.end()); 01595 01596 const Image& src_img = *it; 01597 const PolarAngle ori = src_img.ori(); 01598 MarkList marks = src_img.marks(); 01599 mark_map_[imageRotationToRelativeOrientation(ori)] = marks; 01600 } 01601 01602 const StepResult& prev = src_img.result(0); 01603 const PixelPoint center(img_center_x_, img_center_y_); 01604 CartesianImage img(result, relativeOrientationToImageRotation(0.0), 01605 center); 01606 memcpy(result.yuv_buffer(), prev.yuv_buffer(), result.buflen()); 01607 img.highlight_pixel(center); 01608 img.highlight_point(CartesianPoint(0, 0)); // should be same as center 01609 for (MarkMap::const_iterator map_iter = mark_map_.begin(); 01610 map_iter != mark_map_.end(); map_iter++) 01611 { 01612 const PolarAngle phi = map_iter->first; 01613 const MarkList& list = map_iter->second; 01614 printf("%3.f: ", rad2deg(phi)); 01615 for (MarkList::const_iterator list_iter = list.begin(); 01616 list_iter != list.end(); list_iter++) 01617 { 01618 const PolarAngle angle = phi; 01619 const PolarRadius radius = *list_iter; 01620 img.highlight_point(CartesianPoint(angle, radius)); 01621 PixelPoint pp = img.to_pixel(CartesianPoint(angle, radius)); 01622 printf("%3d (%d, %d)", radius, pp.x, pp.y); 01623 } 01624 printf("\n"); 01625 } 01626 } 01627 src_img.add_result(result); 01628 set_last_yuv_buffer(result.yuv_buffer()); 01629 goto_next_state(); 01630 return; 01631 } 01632 } 01633 01634 01635 /** 01636 * Returns the center point determined by the mean of the first marks found in 01637 * the different directions. 01638 * The angle of the returned cartesian point is relative to the robots 01639 * orientation (and thus must be rotated by 90 degrees counter-clock-wise 01640 * to be displayed correctly). 01641 * @return the estimated center point relative to the robots orientation 01642 */ 01643 MirrorCalibTool::PixelPoint 01644 MirrorCalibTool::calculate_center(const ImageList& images) 01645 { 01646 int x = 0; 01647 int y = 0; 01648 for (ImageList::const_iterator it = images.begin(); 01649 it != images.end(); it++) 01650 { 01651 const Image& image = *it; 01652 const PolarRadius radius = image.marks().at(0); 01653 const unsigned char* null_buf = 0; 01654 const CartesianImage cart_image(null_buf, image.width(), image.height(), 01655 image.ori()); 01656 const CartesianPoint point(0.0, radius); 01657 const PixelPoint pixel = cart_image.to_pixel(point); 01658 x += pixel.x; 01659 y += pixel.y; 01660 } 01661 return PixelPoint(x / images.size(), y / images.size()); 01662 01663 /* 01664 return PixelPoint(500, 500); 01665 int x = 0; 01666 int y = 0; 01667 int count = 0; 01668 for (MarkMap::const_iterator it = mark_map.begin(); 01669 it != mark_map.end(); it++) { 01670 PolarAngle angle = it->first; 01671 PolarRadius dist = *(it->second.begin()); 01672 CartesianPoint p(angle, dist); 01673 x += p.x; 01674 y += p.y; 01675 ++count; 01676 } 01677 CartesianPoint center(x/count, y/count); 01678 return center; 01679 */ 01680 } 01681 01682 01683 /** 01684 * Searches the two angles in the MarkMap mark_map that are nearest to angle. 01685 * @param angle The reference angle to which the nearest marks are searched. 01686 * @param mark_map The mark map. 01687 * @return The two angles nearest to angle contained in mark_map (and therefore 01688 * keys to MarkLists in mark_map). 01689 */ 01690 MirrorCalibTool::PolarAnglePair 01691 MirrorCalibTool::find_nearest_neighbors(PolarAngle angle, 01692 const MarkMap& mark_map) 01693 { 01694 typedef std::vector<PolarAngle> AngleList; 01695 AngleList diffs; 01696 for (MarkMap::const_iterator it = mark_map.begin(); 01697 it != mark_map.end(); it++) 01698 { 01699 const PolarAngle mark_angle = it->first; 01700 const PolarAngle diff = normalize_mirror_rad(mark_angle - angle); 01701 diffs.push_back(diff); 01702 #if 0 01703 std::cout << "Finding nearest neighbors: " 01704 << "ref="<<angle<<"="<<rad2deg(angle)<<" " 01705 << "to="<<mark_angle<<"="<<rad2deg(mark_angle)<<" " 01706 << "diff="<<diff<<"="<<rad2deg(diff) << std::endl; 01707 #endif 01708 } 01709 bool min1_init = false; 01710 AngleList::size_type min1_index = 0; 01711 bool min2_init = false; 01712 AngleList::size_type min2_index = 0; 01713 for (int i = 0; static_cast<AngleList::size_type>(i) < diffs.size(); i++) { 01714 if (!min1_init || fabs(diffs[min1_index]) >= fabs(diffs[i])) { 01715 min2_index = min1_index; 01716 min2_init = min1_init; 01717 min1_index = i; 01718 min1_init = true; 01719 } else if (!min2_init || fabs(diffs[min2_index]) >= fabs(diffs[i])) { 01720 min2_index = i; 01721 min2_init = true; 01722 } 01723 } 01724 if (!min1_init) { 01725 throw fawkes::Exception("First minimum diff. angle not found"); 01726 } 01727 if (!min2_init) { 01728 throw fawkes::Exception("Second minimum diff. angle not found"); 01729 } 01730 PolarAngle min1 = 0.0; 01731 PolarAngle min2 = 0.0; 01732 AngleList::size_type i = 0; 01733 for (MarkMap::const_iterator it = mark_map.begin(); 01734 it != mark_map.end(); it++) 01735 { 01736 if (i == min1_index) { 01737 min1 = it->first; 01738 } else if (i == min2_index) { 01739 min2 = it->first; 01740 } 01741 i++; 01742 } 01743 #if 0 01744 std::cout << "Found nearest neighbor 1: " 01745 << "ref="<<angle<<"="<<rad2deg(angle)<<" " 01746 << "to="<<min1<<"="<<rad2deg(min1) <<" " 01747 << "index="<<min1_index << std::endl; 01748 std::cout << "Found nearest neighbor 2: " 01749 << "ref="<<angle<<"="<<rad2deg(angle)<<" " 01750 << "to="<<min2<<"="<<rad2deg(min2) <<" " 01751 << "index="<<min2_index << std::endl; 01752 #endif 01753 return PolarAnglePair(min1, min2); 01754 } 01755 01756 01757 /** 01758 * Calculates the real distance to the n-th mark. 01759 * @param n The index of the mark, starting at 0. 01760 * @return The distance in centimeters. 01761 */ 01762 MirrorCalibTool::RealDistance 01763 MirrorCalibTool::calculate_real_distance(int n) 01764 { 01765 return static_cast<int>(CARPET_DISTANCE_FROM_ROBOT_CENTER + 01766 static_cast<float>(n + 1) * MARK_DISTANCE); 01767 } 01768 01769 01770 MirrorCalibTool::RealDistance 01771 MirrorCalibTool::interpolate(PolarRadius radius, 01772 const MarkList& marks) 01773 { 01774 if (marks.size() < 2) { 01775 throw fawkes::Exception("Not enough marks for interpolation"); 01776 } 01777 for (MarkList::size_type i = 0; i < marks.size(); i++) 01778 { 01779 // linear interpolation, x0, x1 = 'Stuetzstellen', f0, f1 = 'Stuetzwerte' 01780 if (i == 0 && radius < marks[i]) { 01781 const PolarRadius x0 = 0; 01782 const PolarRadius x1 = marks[i]; 01783 const PolarRadius x = radius; 01784 const RealDistance f0 = 0; 01785 const RealDistance f1 = calculate_real_distance(i); 01786 #if 0 01787 std::cout << "A_Interpolate " << x << " between " 01788 << x0 << "->" << f0 << " " 01789 << x1 << "->" << f1 << std::endl; 01790 #endif 01791 return static_cast<RealDistance>(static_cast<double>(f0) + 01792 static_cast<double>(x - x0) * 01793 static_cast<double>(f1 - f0) / 01794 static_cast<double>(x1 - x0)); 01795 } else if (i > 0 && marks[i-1] <= radius && 01796 (radius < marks[i] || i+1 == marks.size())) { 01797 const PolarRadius x0 = marks[i-1]; 01798 const PolarRadius x1 = marks[i]; 01799 const PolarRadius x = radius; 01800 const RealDistance f0 = calculate_real_distance(i-1); 01801 const RealDistance f1 = calculate_real_distance(i); 01802 #if 0 01803 std::cout << "B_Interpolate " << x << " between " 01804 << x0 << "->" << f0 << " " 01805 << x1 << "->" << f1 << std::endl; 01806 #endif 01807 return static_cast<RealDistance>(static_cast<double>(f0) + 01808 static_cast<double>(x - x0) * 01809 static_cast<double>(f1 - f0) / 01810 static_cast<double>(x1 - x0)); 01811 } 01812 } 01813 throw fawkes::Exception("Interpolation Error"); 01814 } 01815 01816 01817 /** 01818 * Generates a new bulb based on the given data. 01819 * @param width The width of the image. 01820 * @param height The height of the image. 01821 * @param center The center pixel point of the image. 01822 * @param mark_map The map of marks where the key is the orientation of the 01823 * marks and the value is a sequence of distances to the marks. 01824 */ 01825 Bulb 01826 MirrorCalibTool::generate(int width, 01827 int height, 01828 const PixelPoint& center, 01829 const MarkMap& mark_map) 01830 { 01831 const unsigned char* null_img_buf = 0; 01832 CartesianImage img(null_img_buf, width, height, 01833 relativeOrientationToImageRotation(0.0), center); 01834 Bulb bulb(width, height); 01835 bulb.setCenter(center.x, center.y); 01836 bulb.setOrientation(0.0); 01837 for (int x = 0; x < width; x++) 01838 { 01839 for (int y = 0; y < height; y++) 01840 { 01841 const PixelPoint pp(x, y); 01842 const CartesianPoint cp = img.to_cartesian(pp); 01843 const PolarAngle ori_to_robot = cp.atan(); 01844 const PolarAnglePair nearest_neighbors = 01845 find_nearest_neighbors(ori_to_robot, mark_map); 01846 const PolarAngle diff1 = 01847 fabs(normalize_mirror_rad(nearest_neighbors.first - ori_to_robot)); 01848 const PolarAngle diff2 = 01849 fabs(normalize_mirror_rad(nearest_neighbors.second - ori_to_robot)); 01850 const PolarAngle norm = diff1 + diff2; 01851 assert(norm != 0.0); 01852 const double weight1 = 1.0 - diff1 / norm; 01853 const double weight2 = 1.0 - diff2 / norm; 01854 #if 0 01855 std::cout << "PixelPoint("<< x <<", "<< y <<")"<< std::endl; 01856 std::cout << "CartesianPoint("<< cp.x <<", "<< cp.y <<")"<< std::endl; 01857 std::cout << "Radius, Angle: " << cp.length() << " " 01858 << ori_to_robot << " = " 01859 << rad2deg(ori_to_robot) << std::endl; 01860 std::cout << "Neighbor 1: " 01861 << normalize_rad(nearest_neighbors.first) << " = " 01862 << rad2deg(normalize_rad(nearest_neighbors.first)) 01863 << std::endl; 01864 std::cout << "Neighbor 2: " 01865 << normalize_rad(nearest_neighbors.second) << " = " 01866 << rad2deg(normalize_rad(nearest_neighbors.second)) 01867 << std::endl; 01868 std::cout << "Diff 1: " << diff1 << " = " << rad2deg(diff1) << std::endl; 01869 std::cout << "Diff 2: " << diff2 << " = " << rad2deg(diff2) << std::endl; 01870 std::cout << "Norm Factor: " << norm << " = " << rad2deg(norm) 01871 << std::endl; 01872 std::cout << "Weight 1: " << weight1 << " = " << rad2deg(weight1) 01873 << std::endl; 01874 std::cout << "Weight 2: " << weight2 << " = " << rad2deg(weight2) 01875 << std::endl; 01876 #endif 01877 assert(0.0 <= weight1 && weight1 <= 1.0); 01878 assert(0.0 <= weight2 && weight2 <= 1.0); 01879 assert(1.0 - 10e-5 < weight1 + weight2 && weight1 + weight2 < 1.0 + 10e-5); 01880 const MarkList& marks1 = mark_map.at(nearest_neighbors.first); 01881 const MarkList& marks2 = mark_map.at(nearest_neighbors.second); 01882 const RealDistance dist1 = interpolate(cp.length(), marks1); 01883 const RealDistance dist2 = interpolate(cp.length(), marks2); 01884 #if 0 01885 std::cout << "Real 1 " << dist1 << std::endl; 01886 std::cout << "Real 2 " << dist2 << std::endl; 01887 #endif 01888 const RealDistance weighted_mean_dist = dist1 * weight1 + dist2 * weight2; 01889 const float world_dist_in_meters = weighted_mean_dist / 100.0f; 01890 const float world_phi_rel_to_robot = -1.0f * ori_to_robot; 01891 // world_phi_rel_to_robot must be multiplied with -1 because the image is 01892 // mirrored: what's on the right in the image, is on the left of the robot 01893 // in reality 01894 #if 0 01895 std::cout << "Dist 1: " << dist1 << std::endl; 01896 std::cout << "Dist 2: " << dist2 << std::endl; 01897 std::cout << "World Dist: " << world_dist_in_meters << std::endl; 01898 std::cout << "World Phi: " << ori_to_robot << " = " 01899 << rad2deg(ori_to_robot) << std::endl; 01900 #endif 01901 if (world_dist_in_meters > 0) { 01902 bulb.setWorldPoint(x, y, world_dist_in_meters, world_phi_rel_to_robot); 01903 } 01904 } 01905 } 01906 return bulb; 01907 } 01908 01909 01910 void 01911 MirrorCalibTool::set_last_yuv_buffer(const unsigned char* last_buf) 01912 { 01913 last_yuv_buffer_ = last_buf; 01914 } 01915 01916 01917 /** Get last created YUV buffer. 01918 * This is the result of the most recent step. Memory management is 01919 * done by MirrorCalibTool. 01920 * @return most recent result's YUV buffer 01921 */ 01922 const unsigned char* 01923 MirrorCalibTool::get_last_yuv_buffer() const 01924 { 01925 return last_yuv_buffer_; 01926 } 01927 01928 01929 /** Get the assumed distance and orientation of a pixel point. 01930 * @param x The X coordinate of the pixel that is to be evaluated. 01931 * @param y The Y coordinate of the pixel that is to be evaluated. 01932 * @param dist_ret The distance in the real world to the pixel. 01933 * @param ori_ret The orientation in the real world to the pixel. 01934 */ 01935 void 01936 MirrorCalibTool::eval(unsigned int x, unsigned int y, 01937 float* dist_ret, float* ori_ret) 01938 { 01939 if (!bulb_) { 01940 printf("No bulb loaded, cannot evaluate.\n"); 01941 return; 01942 } 01943 polar_coord_2d_t coord; 01944 coord = bulb_->getWorldPointRelative(x, y); 01945 01946 *dist_ret = coord.r; 01947 *ori_ret = coord.phi; 01948 } 01949 01950 01951 /** Loads a calibration file. 01952 * @param filename Name of the file containing the calibration data. 01953 */ 01954 void 01955 MirrorCalibTool::load(const char* filename) 01956 { 01957 bulb_ = new Bulb(filename); 01958 } 01959 01960 01961 /** Saves calibration data to a file. 01962 * @param filename The name of the file. 01963 */ 01964 void 01965 MirrorCalibTool::save(const char* filename) 01966 { 01967 if (state_.step == DONE) { 01968 const Image& src_img = source_images_[state_.image_index]; 01969 const PixelPoint center(img_center_x_, img_center_y_); 01970 Bulb bulb = generate(src_img.width(), src_img.height(), center, mark_map_); 01971 bulb.save(filename); 01972 } else { 01973 std::cout << "Can't save in the middle of the calibration" << std::endl; 01974 } 01975 } 01976 01977 01978 /** Draws a line from the image center in the given angle. 01979 * @param yuv_buffer The in/out parameter for the image. 01980 * @param angle_deg Angle of line in degrees. 01981 * @param center_x X-coordinate of center point in image pixels. 01982 * @param center_y Y-coordinate of center point in image pixels. 01983 * @param width Width of image. 01984 * @param height Height of image. 01985 */ 01986 void 01987 MirrorCalibTool::draw_line(unsigned char* yuv_buffer, 01988 double angle_deg, 01989 int center_x, 01990 int center_y, 01991 int width, 01992 int height) 01993 { 01994 const PolarAngle angle = normalize_rad(deg2rad(angle_deg)); 01995 CartesianImage img(yuv_buffer, width, height, 01996 relativeOrientationToImageRotation(0.0), 01997 PixelPoint(center_x, center_y)); 01998 for (PolarRadius length = 0; length < img.max_radius(); length++) 01999 { 02000 const CartesianPoint p(angle, length); 02001 if (img.contains(p)) { 02002 img.set_color(p, MARK_LUMA, MARK_CHROMINANCE); 02003 } 02004 } 02005 } 02006 02007 02008 /** Draws a crosshair with the lines in the directions of the keys of 02009 * the mark map. 02010 * @param yuv_buffer The in/out parameter for the image. 02011 */ 02012 void 02013 MirrorCalibTool::draw_mark_lines(unsigned char* yuv_buffer) 02014 { 02015 for (ImageList::const_iterator it = source_images_.begin(); 02016 it != source_images_.end(); it++) 02017 { 02018 const Image& src_img = *it; 02019 CartesianImage img(yuv_buffer, src_img.width(), src_img.height(), 02020 src_img.ori(), PixelPoint(img_center_x_, img_center_y_)); 02021 for (PolarRadius length = 0; length < img.max_radius(); length++) 02022 { 02023 const PolarAngle angle = 0.0; 02024 const CartesianPoint p(angle, length); 02025 if (img.contains(p)) { 02026 img.set_color(p, MARK_LUMA, MARK_CHROMINANCE); 02027 } 02028 } 02029 } 02030 } 02031 02032 02033 /** Draws a crosshair in the YUV-buffer. The crosshair consists of three lines 02034 * from the origin (0, 0) with the angles 0 degree, 120 degrees, 240 degrees. 02035 * @param yuv_buffer The in/out parameter for the image. 02036 * @param center_x X-coordinate of center point in image pixels. 02037 * @param center_y Y-coordinate of center point in image pixels. 02038 * @param width Width of image. 02039 * @param height Height of image. 02040 */ 02041 void 02042 MirrorCalibTool::draw_crosshair(unsigned char* yuv_buffer, 02043 int center_x, 02044 int center_y, 02045 int width, 02046 int height) 02047 { 02048 const PolarAngle POSITIONS[] = { normalize_rad(deg2rad( 0.0f)), 02049 normalize_rad(deg2rad(-120.0f)), 02050 normalize_rad(deg2rad( 120.0f)) }; 02051 const int POSITION_COUNT = sizeof POSITIONS / sizeof(double); 02052 CartesianImage img(yuv_buffer, width, height, 02053 relativeOrientationToImageRotation(0.0), 02054 PixelPoint(center_x, center_y)); 02055 for (int i = 0; i < POSITION_COUNT; i++) 02056 { 02057 const PolarAngle angle = POSITIONS[i]; 02058 for (PolarRadius length = 0; length < img.max_radius(); length++) 02059 { 02060 const CartesianPoint p(angle, length); 02061 if (img.contains(p)) { 02062 img.set_color(p, MARK_LUMA, MARK_CHROMINANCE); 02063 } 02064 } 02065 } 02066 } 02067 02068 } 02069