Fawkes API  Fawkes Development Version
mirror_calib.cpp
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 != &copy) {
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 != &copy) {
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