Fawkes API  Fawkes Development Version
mirror_calib.cpp
1 
2 /***************************************************************************
3  * mirror_calib.cpp - Mirror calibration tool
4  *
5  * Created: Fri Dec 07 18:35:40 2007
6  * Copyright 2007 Daniel Beck
7  * Copyright 2009 Christoph Schwering
8  *
9  ****************************************************************************/
10 
11 /* This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL file in the doc directory.
22  */
23 
24 #include "mirror_calib.h"
25 
26 #include <core/exception.h>
27 #include <utils/math/angle.h>
28 
29 #include <fvutils/color/yuv.h>
30 #include <fvutils/readers/pnm.h>
31 
32 #include <fvfilters/sobel.h>
33 #include <fvfilters/sharpen.h>
34 #include <fvfilters/median.h>
35 #include <fvfilters/or.h>
36 #include <fvfilters/laplace.h>
37 #include <fvfilters/min.h>
38 
39 #include <algorithm>
40 #include <iostream>
41 #include <string>
42 #include <cstring>
43 #include <cstdio>
44 #include <cassert>
45 
46 /* If defined, the center point is re-calculated by averaging the first
47  * marks. */
48 //#define RECALCULATE_CENTER_POINT
49 
50 #define FILTER_HOLES
51 
52 /* If defined, very small holes are ignored. These small holes occur because a
53  * single mark is sometimes recognized as *two* different marks due to the edge
54  * detection. */
55 #define FILTER_MINI_HOLES
56 
57 using namespace fawkes;
58 
59 namespace firevision {
60 
61 /** @class MirrorCalibTool mirror_calib.h
62  * This class encapsulates the routines necessary for interactive mirror
63  * calibration.
64  *
65  * The input is N pairs (degree,image) and (optionally) a filter mask.
66  * The calibration runs in multiple phases:
67  * (1) Edge detection.
68  * (2) Assume center point in the middle of the image unless it is set
69  * explicitly.
70  * (3) Pre-marking: Find the edges that lie at `degree' wrt. Y axis in image.
71  * (4) Final marking: Try to filter false-positive marks.
72  * (5) Centering: Average the first marks in each direction to get the `real'
73  * center point of the mirror.
74  * (6) Again do steps (3) and (5) with the new center point.
75  * (7) Generate bulb files. The polar coordinates of each pixel are determined
76  * as follows:
77  * (a) Determine the polar coordinates of the pixel.
78  * (b) Get the two mark streams that enclose the pixel.
79  * (c) Do linear interpolation in both mark streams to get two distances.
80  * (d) Add the weightened distances (the nearer the pixel is to the mark
81  * stream the higher is its weight).
82  * (e) Now we have an estimated distance. We need to multiply the angle
83  * with -1.0 (!!!) to consider the fact that the image is mirrored:
84  * what looks to left from the robot on the image, is on the right of
85  * the robot in reality!
86  * Note that all the time we worked on the mirrored image with angles
87  * that were correct from the perspective of the image. But to make them
88  * correct from the perspective of the robot, we have to mirror
89  * everything.
90  */
91 
92 namespace {
93  const unsigned int ORIENTATION_COUNT = (ORI_DEG_360 - ORI_DEG_0);
94  const unsigned int MARK_COUNT = 6;
95  const float MARK_DISTANCE = 29.7;
96  const float CARPET_DISTANCE_FROM_ROBOT_CENTER = 5.0;
97  const unsigned char DARK = 0;
98  const unsigned char BRIGHT = 255;
99  const unsigned char MARK_LUMA = 128;
100  const unsigned char MARK_CHROMINANCE = 255;
101  const int MIN_WIDTH_OF_BIGGEST_LINE = 50;
102  const float APPROX_LINE_WIDTH_LOSS = 0.20f;
103  const float MIN_BRIGHT_FRACTION = 0.20f;
104  const int HIGHLIGHT_RADIUS = 2;
105  const int FALSE_POSITIVE_HOLE_SIZE = 5;
106 }
107 
108 
109 /**
110  * The result of a step contains a YUV buffer.
111  * The different steps are descripted in the MirrorCalibTool's doc.
112  */
114  private:
115  unsigned char* buffer_;
116  size_t buflen_;
117  int width_;
118  int height_;
119  unsigned int* refcount_;
120 
121  public:
122  /** Constructor.
123  * @param buflen
124  * @param width
125  * @param height */
126  StepResult(size_t buflen, int width, int height)
127  : buffer_(new unsigned char[buflen]),
128  buflen_(buflen),
129  width_(width),
130  height_(height),
131  refcount_(new unsigned int)
132  {
133  *refcount_ = 1;
134  }
135 
136  /** Constructor.
137  * @param copy */
138  StepResult(const StepResult& copy)
139  : buffer_(copy.buffer_),
140  buflen_(copy.buflen_),
141  width_(copy.width_),
142  height_(copy.height_),
143  refcount_(copy.refcount_)
144  {
145  ++*refcount_;
146  }
147 
148  /** Assignment.
149  * @param copy result to assign
150  * @return reference to this instance */
152  {
153  if (this != &copy) {
154  if (--*refcount_ == 0) {
155  delete[] buffer_;
156  delete refcount_;
157  }
158  buffer_ = copy.buffer_;
159  refcount_ = copy.refcount_;
160  buflen_ = copy.buflen_;
161  width_ = copy.width_;
162  height_ = copy.height_;
163  ++*refcount_;
164  }
165  return *this;
166  }
167 
168  /** Destructor. */
170  {
171  if (--*refcount_ == 0) {
172  delete[] buffer_;
173  delete refcount_;
174  }
175  }
176 
177  /** YUV buffer.
178  * @return YUV buffer */
179  inline unsigned char* yuv_buffer() { return buffer_; }
180 
181  /** YUV buffer.
182  * @return YUV buffer */
183  inline const unsigned char* yuv_buffer() const { return buffer_; }
184 
185  /** YUV buffer's length.
186  * @return buffer length */
187  inline size_t buflen() const { return buflen_; }
188 
189  /** Width of YUV buffer.
190  * @return height */
191  inline int width() const { return width_; }
192 
193  /** Height of YUV buffer.
194  * @return height */
195  inline int height() const { return height_; }
196 }; // StepResult
197 
198 
199 /**
200  * Abstract Point class.
201  */
203  public:
204  const int x; /**< X coordinate. */
205  const int y; /**< Y coordinate. */
206 
207  /** Constructor.
208  * @param x
209  * @param y */
210  Point(int x, int y)
211  : x(x),
212  y(y)
213  {
214  }
215 
216  /** Length of the vector the point.
217  * @return length of the vector the point.
218  * @see atan() for polar coordinates. */
219  PolarRadius length() const
220  {
221  return static_cast<PolarRadius>(sqrt(x*x + y*y));
222  }
223 
224  /** Atan(y, x) of the point.
225  * @return Atan(y, x) of the point.
226  * @see length() for polar coordinates. */
227  inline PolarAngle atan() const
228  {
229  return normalize_rad(atan2(y, x));
230  }
231 
232  /** Assignment.
233  * @return A copy.
234  * @param p */
236  {
237  if (&p == this) {
238  return *this;
239  }
240  return Point(p.x, p.y);
241  }
242 }; // Point
243 
244 
245 /**
246  * A cartesian point is a 2d point which can have negative X and Y coords.
247  */
249 : public Point
250 {
251  public:
252  /** Constructor.
253  * @param phi
254  * @param length */
255  CartesianPoint(PolarAngle phi, PolarRadius length)
256  : Point(length * cos(phi),
257  length * sin(phi))
258  {
259  }
260 
261  /** Constructor.
262  * @param x
263  * @param y */
264  CartesianPoint(int x, int y)
265  : Point(x, y)
266  {
267  }
268 
269  /** Rotates the vector to the point counter-clockwise and returns the vector
270  * to the point.
271  * @param rotate_phi
272  * @return Counter-clockwise rotated point. */
273  CartesianPoint rotate(PolarAngle rotate_phi) const
274  {
275  const PolarRadius len = length();
276  const PolarAngle phi = atan() + rotate_phi;
277  const int x = len * cos(phi);
278  const int y = len * sin(phi);
279  return CartesianPoint(x, y);
280  }
281 }; // CartesianPoint
282 
283 
284 /**
285  * A pixel point is a 2d point with positive X and Y coords.
286  */
288 : public Point
289 {
290  public:
291  /** Constructor.
292  * @param x
293  * @param y */
294  PixelPoint(int x, int y)
295  : Point(x, y)
296  {
297  }
298 
299  /** Rotates the vector to the point counter-clockwise and returns the vector
300  * to the point.
301  * @param rotate_phi Angle.
302  * @return Counter-clockwise rotated point. */
303  PixelPoint rotate(PolarAngle rotate_phi) const
304  {
305  const PolarRadius len = length();
306  const PolarAngle phi = atan() + rotate_phi;
307  const int x = len * cos(phi);
308  const int y = len * sin(phi);
309  return PixelPoint(x, y);
310  }
311 }; // PixelPoint
312 
313 
314 /**
315  * Wraps an image so that access to (0, 0) is mapped to the middle of the
316  * image and so on. The result is a cartesian coordinate system with X and
317  * Y axis.
318  */
320 {
321  private:
322  unsigned char* buf_;
323  const int width_;
324  const int height_;
325  const PixelPoint center_;
326  const PolarAngle phi_;
327  const unsigned char* mask_;
328 
329  public:
330  /** Constructor.
331  * @param res The step result that contains the base image.
332  * @param phi The angle by which the image is rotated counter-clockwise.
333  * @param center The center of the image from the robot's point of view; i.e.
334  * the center of the cone mirror.
335  * @param mask The mask that indicates which pixels should be ignored. */
337  PolarAngle phi,
338  PixelPoint center,
339  const unsigned char* mask = 0)
340  : buf_(const_cast<unsigned char*>(res.yuv_buffer())),
341  width_(res.width()),
342  height_(res.height()),
343  center_(center),
344  phi_(phi),
345  mask_(mask)
346  {
347  }
348 
349  /** Constructor.
350  * @param res The step result that contains the base image.
351  * @param phi The angle by which the image is rotated counter-clockwise.
352  * @param mask The mask that indicates which pixels should be ignored. */
354  PolarAngle phi,
355  const unsigned char* mask = 0)
356  : buf_(const_cast<unsigned char*>(res.yuv_buffer())),
357  width_(res.width()),
358  height_(res.height()),
359  center_(PixelPoint(res.width()/2, res.height()/2)),
360  phi_(phi),
361  mask_(mask)
362  {
363  }
364 
365  /** Constructor.
366  * @param buf The base YUV buffer.
367  * @param width Image width.
368  * @param height Image height.
369  * @param phi The angle by which the image is rotated counter-clockwise.
370  * @param center The center of the image from the robot's point of view; i.e.
371  * the center of the cone mirror. */
372  CartesianImage(const unsigned char* buf,
373  int width,
374  int height,
375  PolarAngle phi,
376  const PixelPoint& center)
377  : buf_(const_cast<unsigned char*>(buf)),
378  width_(width),
379  height_(height),
380  center_(center),
381  phi_(phi),
382  mask_(0)
383  {
384  }
385 
386  /** Constructor.
387  * @param buf The base YUV buffer.
388  * @param width Image width.
389  * @param height Image height.
390  * @param phi The angle by which the image is rotated counter-clockwise.
391  * @param center The center of the image from the robot's point of view; i.e.
392  * the center of the cone mirror. */
393  CartesianImage(unsigned char* buf,
394  int width,
395  int height,
396  PolarAngle phi,
397  const PixelPoint& center)
398  : buf_(buf),
399  width_(width),
400  height_(height),
401  center_(center),
402  phi_(phi),
403  mask_(0)
404  {
405  }
406 
407  /** Constructor.
408  * @param buf The base YUV buffer.
409  * @param width Image width.
410  * @param height Image height.
411  * @param phi The angle by which the image is rotated counter-clockwise. */
412  CartesianImage(const unsigned char* buf,
413  int width,
414  int height,
415  PolarAngle phi)
416  : buf_(const_cast<unsigned char*>(buf)),
417  width_(width),
418  height_(height),
419  center_(PixelPoint(width/2, height/2)),
420  phi_(phi),
421  mask_(0)
422  {
423  }
424 
425  /** Constructor.
426  * @param buf The base YUV buffer.
427  * @param width Image width.
428  * @param height Image height.
429  * @param phi The angle by which the image is rotated counter-clockwise. */
430  CartesianImage(unsigned char* buf,
431  int width,
432  int height,
433  PolarAngle phi)
434  : buf_(buf),
435  width_(width),
436  height_(height),
437  center_(PixelPoint(width/2, height/2)),
438  phi_(phi),
439  mask_(0)
440  {
441  }
442 
443  /** Get Buffer.
444  * @return buffer
445  */
446  inline unsigned char* buf() { return buf_; }
447  /** Mask.
448  * @return mask
449  */
450  inline const unsigned char* mask() const { return mask_; }
451  /** Buffer.
452  * @return const buffer
453  */
454  inline const unsigned char* buf() const { return buf_; }
455  /** Center of buffer.
456  * @return center point of buffer
457  */
458  inline const PixelPoint& center() const { return center_; }
459  /** Width of buffer.
460  * @return width of buffer
461  */
462  inline const int width() const { return width_; }
463  /** Height of buffer.
464  * @return height of buffer
465  */
466  inline const int height() const { return height_; }
467  /** Phi angle.
468  * @return Phi angle
469  */
470  inline const PolarAngle phi() const { return phi_; }
471 
472  /** Converts a cartesian point to a pixel point.
473  * @param p The point.
474  * @return converted point
475  */
476  inline PixelPoint
477  to_pixel(const CartesianPoint& p) const
478  {
479  const CartesianPoint rp = p.rotate(-1.0 * phi());
480  return PixelPoint(center().x + rp.x, center().y - rp.y);
481  }
482 
483  /** Converts a pixel point to a cartesian point.
484  * @param p The point.
485  * @return converted point
486  */
487  inline CartesianPoint
488  to_cartesian(const PixelPoint& p) const
489  {
490  const CartesianPoint cp(p.x - center().x, center().y - p.y);
491  return cp.rotate(phi());
492  }
493 
494  /** Indicates whether the image contains a pixel point.
495  * @param p The point.
496  * @return true if pixel point is in image, false otherwise
497  */
498  inline bool
499  contains(const PixelPoint& p) const
500  {
501  return 0 <= p.x && p.x <= width()-1 && 0 <= p.y && p.y <= height()-1;
502  }
503 
504  /** Indicates whether the image contains a cartesian point.
505  * @param p The point.
506  * @return true if cartesian point is in image, false otherwise
507  */
508  inline bool
509  contains(const CartesianPoint& p) const
510  {
511  return contains(to_pixel(p));
512  }
513 
514  /** Get luminance at a given point.
515  * @param p The point.
516  * @return luminance at given point
517  */
518  inline unsigned char
519  get(const CartesianPoint& p) const
520  {
521  if (!contains(p)) {
522  throw fawkes::Exception("Point p is out of image");
523  }
524  PixelPoint pp = to_pixel(p);
525  const YUV_t ignr(0);
526  if (mask() == 0 ||
527  (YUV422_PLANAR_Y_AT(mask(), width(), pp.x, pp.y) != ignr.Y &&
528  YUV422_PLANAR_U_AT(mask(), width(), height(), pp.x, pp.y) != ignr.U &&
529  YUV422_PLANAR_V_AT(mask(), width(), height(), pp.x, pp.y) != ignr.V)) {
530  return YUV422_PLANAR_Y_AT(buf(), width(), pp.x, pp.y);
531  } else {
532  if (mask() != 0) {
533  //printf("Ignoring (%lf,%d) = (%d,%d)\n", p.atan(), p.length(), pp.x, pp.y);
534  }
535  return 0;
536  }
537  }
538 
539  /** Get Maximum cartesian X coordinate of the image.
540  * @return Maximum cartesian X coordinate of the image.
541  */
542  inline int max_x() const { return std::max(center().x, width() - center().x); }
543  /** Get Maximum cartesian Y coordinate of the image.
544  * @return Maximum cartesian Y coordinate of the image.
545  */
546  inline int max_y() const { return std::max(center().y, height() - center().y); }
547  /** Maximum polar radius of the image.
548  * @return Maximum polar radius of the image.
549  */
550  inline PolarRadius max_radius() const {
551  return static_cast<PolarRadius>(sqrt(max_x()*max_x() + max_y()*max_y()));
552  }
553 
554  /** Sets the luminance Y and chrominance U at a given pixel point.
555  * @param p The point whose luminance Y and chrominance U is changed.
556  * @param luma The luminance Y.
557  * @param chrominance The chrominance U. */
558  void
560  unsigned char luma,
561  unsigned char chrominance)
562  {
563  if (!contains(p)) {
564  throw fawkes::Exception("Point p is out of image");
565  }
566  YUV422_PLANAR_Y_AT(buf(), width(), p.x, p.y) = luma;
567  YUV422_PLANAR_U_AT(buf(), width(), height(), p.x, p.y) = chrominance;
568  }
569 
570  /** Sets the luminance Y and chrominance U at a given cartesian point.
571  * @param p The point whose luminance Y and chrominance U is changed.
572  * @param luma The luminance Y.
573  * @param chrominance The chrominance U. */
574  void
576  unsigned char luma,
577  unsigned char chrominance)
578  {
579  set_color(to_pixel(p), luma, chrominance);
580  }
581 
582  /** Get relative amount of bright pixels.
583  * @param from One point of the rectangle.
584  * @param to Opposite point of the rectangle.
585  * @return relative amount of BRIGHT pixels in the rectangle denoted
586  * by the bottom-left (x1, y1) and the top-right (x2, y2). */
587  float
589  const CartesianPoint& to) const
590  {
591  const int from_x = from.x < to.x ? from.x : to.x;
592  const int to_x = from.x > to.x ? from.x : to.x;
593  const int from_y = from.y < to.y ? from.y : to.y;
594  const int to_y = from.y > to.y ? from.y : to.y;
595  int pixel_count = 0;
596  int bright_count = 0;
597  for (int x = from_x; x <= to_x; x++) {
598  for (int y = from_y; y <= to_y; y++) {
599  const CartesianPoint p(x, y);
600  if (contains(p)) {
601  if (get(p) == BRIGHT) {
602  ++bright_count;
603  }
604  }
605  ++pixel_count;
606  }
607  }
608  return static_cast<float>(static_cast<double>(bright_count)
609  / static_cast<double>(pixel_count));
610  }
611 
612  /** Indicates whether at pixel point p there is a highlighted line.
613  * @param p The assumed center point of the line.
614  * @param length The length of the line
615  * @return true if pixel belongs to line, false otherwise
616  */
617  bool
619  int length) const
620  {
621  for (int y_offset = 0; y_offset <= 1; y_offset++) {
622  const CartesianPoint vec(p.atan() + deg2rad(90.0), length / 2 + 1);
623  const CartesianPoint from(p.x - vec.x, p.y - vec.y);
624  const CartesianPoint to(p.x + vec.x, p.y + vec.y);
625  if (bright_fraction(from, to) >= MIN_BRIGHT_FRACTION) {
626  return true;
627  }
628  }
629  return false;
630  }
631 
632  /** Highlightes a line at pixel point p.
633  * @param p The center point of the line.
634  * @param length The length of the line. */
635  void
637  int length)
638  {
639  const CartesianPoint vec(p.atan() + deg2rad(90.0), length / 2);
640  const CartesianPoint from(p.x - vec.x, p.y - vec.y);
641  const CartesianPoint to(p.x + vec.x, p.y + vec.y);
642  draw_line(from, to);
643  }
644 
645  /** Draws a line from p to q.
646  * @param p First point of the line.
647  * @param q First point of the line. */
648  void
650  const PixelPoint& q)
651  {
652  draw_line(to_cartesian(p), to_cartesian(q));
653  }
654 
655  /** Draws a line from p to q.
656  * @param p First point of the line.
657  * @param q First point of the line. */
658  void
660  const CartesianPoint& q)
661  {
662  const CartesianPoint distVec(q.x - p.x, q.y - p.y);
663  for (PolarRadius length = 0; length <= distVec.length(); length++) {
664  const CartesianPoint step(distVec.atan(), length);
665  const CartesianPoint linePoint(p.x + step.x, p.y + step.y);
666  if (contains(linePoint)) {
667  set_color(linePoint, MARK_LUMA, MARK_CHROMINANCE);
668  }
669  }
670  }
671 
672  /** Highlights a pixel, i.e. sets the luminance and chrominance to
673  * MARK_LUMA and MARK_CHROMINANCE.
674  * @param p The pixel. */
675  void
677  {
678  const int R = HIGHLIGHT_RADIUS;
679  for (int xx = p.x-R; xx <= p.x+R; xx++) {
680  for (int yy = p.y-R; yy <= p.y+R; yy++) {
681  const PixelPoint pp(xx, yy);
682  if (contains(pp)) {
683  set_color(pp, MARK_LUMA, MARK_CHROMINANCE);
684  }
685  }
686  }
687  }
688 
689  /** Highlights a point, i.e. sets the luminance and chrominance to
690  * MARK_LUMA and MARK_CHROMINANCE.
691  * @param p The point. */
692  void
694  {
695  const int R = HIGHLIGHT_RADIUS;
696  for (int xx = p.x-R; xx <= p.x+R; xx++) {
697  for (int yy = p.y-R; yy <= p.y+R; yy++) {
698  const CartesianPoint hp(xx, yy);
699  if (contains(hp)) {
700  set_color(hp, MARK_LUMA, MARK_CHROMINANCE);
701  }
702  }
703  }
704  }
705 }; // CartesianImage
706 
707 
708 /** @class MirrorCalibTool::ConvexPolygon libs/fvmodels/mirror/mirror_calib.h
709  * Represents a convex polygon. It is defined by a sequence of points in
710  * clock-wise-order.
711  */
712 
713 /** Check if point is inside convex polygon.
714  * The point r is converted to PixelPoint wrt img.
715  * @param img image in which to check
716  * @param r cartesian point to check
717  * @return true if the point is inside the convex polygon
718  */
719 bool
720 MirrorCalibTool::ConvexPolygon::contains(const CartesianImage& img,
721  const CartesianPoint& r) const
722 {
723  return contains(img.to_pixel(r));
724 }
725 
726 
727 /** Check if pixel point is inside convex polygon.
728  * This is the case if for all points p, q in the polygon p_1, ..., p_n
729  * where p = p_i, q = p_{i+1} for some i or p = p_n, q = p_1 it holds
730  * (p, q, r) does not form a left turn (if they do, they are
731  * counter-clock-wise).
732  * @param r point to check
733  * @return true if the point is inside the convex polygon
734  */
735 bool
736 MirrorCalibTool::ConvexPolygon::contains(const PixelPoint& r) const
737 {
738  for (std::vector<PixelPoint>::size_type i = 1; i <= size(); i++) {
739  const PixelPoint& p = at(i-1);
740  const PixelPoint& q = at(i % size());
741  double val = (q.x - p.x) * (r.y - p.y) - (r.x - p.x) * (q.y - p.y);
742  if (val < 0) { // (p, q, r) forms a left turn
743  return false;
744  }
745  }
746  return true;
747 }
748 
749 
750 /** A hole is a sequence of pixels between two lines. */
752  public:
753  int index; /**< The index of the hole. */
754  PolarRadius from_length; /**< Beginning of the hole. */
755  PolarRadius to_length; /**< Ending of the hole. */
756 
757  /** Constructor.
758  * @param index The index of the hole.
759  * @param from_length The beginning of the hole in pixels.
760  * @param to_length The ending of the hole in pixels. */
761  Hole(int index,
762  PolarRadius from_length,
763  PolarRadius to_length)
764  : index(index),
765  from_length(from_length),
766  to_length(to_length)
767  {
768  }
769 
770  /** The size of the hole in pixels.
771  * @return radius of hole
772  */
773  inline PolarRadius size() const { return to_length - from_length; }
774 }; // Hole
775 
776 
777 /** A container for a YUV-buffer etc. */
779 {
780  public:
781  /** Constructor.
782  * @param yuv_buffer The YUV buffer.
783  * @param buflen The buffer's size.
784  * @param width The width.
785  * @param height The height.
786  * @param ori The orientation. */
787  Image(const unsigned char* yuv_buffer,
788  size_t buflen,
789  int width,
790  int height,
791  PolarAngle ori)
792  : yuv_buffer_(new unsigned char[buflen]),
793  buflen_(buflen),
794  width_(width),
795  height_(height),
796  ori_(ori),
797  refcount_(new unsigned int)
798  {
799  memcpy(yuv_buffer_, yuv_buffer, buflen);
800  *refcount_ = 1;
801  }
802 
803  /** Constructor.
804  * @param copy */
805  Image(const Image& copy)
806  : yuv_buffer_(copy.yuv_buffer_),
807  buflen_(copy.buflen_),
808  width_(copy.width_),
809  height_(copy.height_),
810  ori_(copy.ori_),
811  results_(copy.results_),
812  premarks_(copy.premarks_),
813  marks_(copy.marks_),
814  refcount_(copy.refcount_)
815  {
816  ++*refcount_;
817  }
818 
819  /** Assignment.
820  * @param copy image to copy
821  * @return this image
822  */
823  Image& operator=(const Image& copy)
824  {
825  if (this != &copy) {
826  if (--*refcount_ == 0) {
827  delete[] yuv_buffer_;
828  delete refcount_;
829  }
830  yuv_buffer_ = copy.yuv_buffer_;
831  buflen_ = copy.buflen_;
832  width_ = copy.width_;
833  height_ = copy.height_;
834  ori_ = copy.ori_;
835  results_ = copy.results_;
836  premarks_ = copy.premarks_;
837  marks_ = copy.marks_;
838  refcount_ = copy.refcount_;
839  ++*copy.refcount_;
840  }
841  return *this;
842  }
843 
844  /** Destructor. */
846  {
847  if (--*refcount_ == 0) {
848  delete[] yuv_buffer_;
849  delete refcount_;
850  }
851  }
852 
853  /** YUV buffer.
854  * @return YUV buffer
855  */
856  inline unsigned char* yuv_buffer() { return yuv_buffer_; }
857  /** YUV buffer.
858  * @return
859  */
860  inline const unsigned char* yuv_buffer() const { return yuv_buffer_; }
861  /** YUV buffer's length.
862  * @return YUV buffer's length
863  */
864  inline size_t buflen() const { return buflen_; }
865  /** YUV buffer's width.
866  * @return YUV buffer's width
867  */
868  inline int width() const { return width_; }
869  /** YUV buffer's height.
870  * @return YUV buffer's height
871  */
872  inline int height() const { return height_; }
873  /** Angle of marks wrt X axis.
874  * @return Angle of marks wrt X axis
875  */
876  inline PolarAngle ori() const { return ori_; }
877  /** List of results.
878  * @return List of results
879  */
880  inline StepResultList& results() { return results_; }
881  /** List of results.
882  * @return List of results
883  */
884  inline const StepResultList& results() const { return results_; }
885  /** The premarks.
886  * @return The premarks
887  */
888  inline const MarkList& premarks() { return premarks_; }
889  /** The (final) marks.
890  * @return
891  */
892  inline MarkList& marks() { return marks_; }
893  /** The (final) marks.
894  * @return The (final) marks
895  */
896  inline const MarkList& marks() const { return marks_; }
897 
898  /** Appends a result.
899  * @param r The new result. */
900  inline void add_result(const StepResult& r) { results_.push_back(r); }
901  /** Returns the i-th result.
902  * @param i The index of the result starting with 0.
903  * @return result
904  */
905  inline StepResult& result(int i) { return results_[i]; }
906  /** Returns the i-th result.
907  * @param i The index of the result starting with 0.
908  * @return result
909  */
910  inline const StepResult& result(int i) const { return results_[i]; }
911  /** The premarks.
912  * @param premarks The list of premarks. */
913  inline void set_premarks(const MarkList& premarks) { premarks_ = premarks; }
914  /** The (final) marks.
915  * @param marks The list of marks. */
916  inline void set_marks(const MarkList& marks) { marks_ = marks; }
917 
918  private:
919  unsigned char* yuv_buffer_;
920  size_t buflen_;
921  int width_;
922  int height_;
923  PolarAngle ori_;
924  StepResultList results_;
925  MarkList premarks_;
926  MarkList marks_;
927  unsigned int* refcount_;
928 }; // Image
929 
930 
931 /** Constructor. */
932 MirrorCalibTool::MirrorCalibTool()
933  : img_yuv_buffer_(0),
934  img_center_x_(-1),
935  img_center_y_(-1),
936  img_yuv_mask_(0),
937  state_(CalibrationState())
938  , bulb_(0)
939 {
940 }
941 
942 
943 /** Destructor. */
945 {
946  if (img_yuv_buffer_) {
947  delete[] img_yuv_buffer_;
948  }
949  if (img_yuv_mask_) {
950  delete[] img_yuv_mask_;
951  }
952  if (bulb_) {
953  delete bulb_;
954  }
955 }
956 
957 
958 /**
959  * Converts an angle wrt. the Y axis (!) to the robots view to the needed image
960  * rotation so that the things, which lie at angle `ori' wrt. the Y axis, lie
961  * on the X axis of the rotated image.
962  * For example: if the marks are 120 degrees counter-clock-wise from the robot,
963  * the image needs to be rotated 120 degrees clock-wise (then the marks are
964  * in front of the robot, i.e. Y axis) and then 90 degrees clock-wise
965  * (to get the marks from the Y axis to the X axis).
966  *
967  * The reason that we want to have the marks on the X axis is that calculating
968  * with them is then a bit easier (because then their polar angle is always
969  * 0.0).
970  * @param ori The orientation.
971  * @return angle
972  */
973 MirrorCalibTool::PolarAngle
974 MirrorCalibTool::relativeOrientationToImageRotation(PolarAngle ori)
975 {
976  return normalize_rad(-1.0 * ori + deg2rad(-90.0));
977 }
978 
979 
980 /**
981  * Converts the rotation of the image to the orientation relative to the Y axis.
982  * See the documentation of relativeOrientationToImageRotation() of which this
983  * is the inverse.
984  * @param ori The orientation.
985  * @return angle
986  */
987 MirrorCalibTool::PolarAngle
988 MirrorCalibTool::imageRotationToRelativeOrientation(PolarAngle ori)
989 {
990  return normalize_rad(-1.0 * (ori + deg2rad(90.0)));
991 }
992 
993 
994 /**
995  * Loads a PNM mask file for the robot's bars.
996  * The mask is used to ignore those pixels that are part of the robot's bar.
997  * @param mask_file_name The mask file name.
998  */
999 void
1000 MirrorCalibTool::load_mask(const char* mask_file_name)
1001 {
1002  if (img_yuv_mask_) {
1003  delete[] img_yuv_mask_;
1004  }
1005  PNMReader reader(mask_file_name);
1006  size_t size = colorspace_buffer_size(reader.colorspace(),
1007  reader.pixel_width(),
1008  reader.pixel_height());
1009  img_yuv_mask_ = new unsigned char[size];
1010  if (img_center_x_ == -1) {
1011  img_center_x_ = reader.pixel_width() / 2;
1012  }
1013  if (img_center_y_ == -1) {
1014  img_center_y_ = reader.pixel_height() / 2;
1015  }
1016  reader.set_buffer(img_yuv_mask_);
1017  reader.read();
1018 }
1019 
1020 
1021 /** Store image for calibration process.
1022  * @param yuv_buffer The image's yuv_buffer. It may be freeed by the caller
1023  * immediately, the MirrorCalibTool works with a copy of it.
1024  * @param buflen The length of yuv_buffer.
1025  * @param width The width of the image.
1026  * @param height The height of the image.
1027  * @param ori The polar angle in degrees (!) relative to the Y axis of
1028  * the image (!) where the marks are in the image (!) (not in
1029  * in reality from the robot's perspective).
1030  */
1031 void
1032 MirrorCalibTool::push_back(const unsigned char* yuv_buffer,
1033  size_t buflen,
1034  int width,
1035  int height,
1036  double ori)
1037 {
1038  ori = relativeOrientationToImageRotation(ori);
1039  Image src_img(yuv_buffer, buflen, width, height, ori);
1040  if (img_center_x_ == -1) {
1041  img_center_x_ = width / 2;
1042  }
1043  if (img_center_y_ == -1) {
1044  img_center_y_ = height / 2;
1045  }
1046  source_images_.push_back(src_img);
1047 }
1048 
1049 
1050 /** Aborts the calibration process. */
1051 void
1053 {
1054  img_center_x_ = -1;
1055  img_center_y_ = -1;
1056  state_ = CalibrationState();
1057  source_images_.clear();
1058  premarks_.clear();
1059  mark_map_.clear();
1060 }
1061 
1062 
1063 /** Applies a sobel filter for edge detection in some direction.
1064  * @param src The source YUV buffer.
1065  * @param dst The destination YUV buffer.
1066  * @param width The width.
1067  * @param height The height.
1068  * @param ori The orientation. */
1069 void
1070 MirrorCalibTool::apply_sobel(unsigned char* src,
1071  unsigned char* dst,
1072  int width,
1073  int height,
1074  orientation_t ori)
1075 {
1076  ROI* roi = ROI::full_image(width, height);
1077  FilterSobel filter;
1078  filter.set_src_buffer(src, roi, ori, 0);
1079  filter.set_dst_buffer(dst, roi);
1080  filter.apply();
1081 }
1082 
1083 
1084 /** Applies a sharpening filter.
1085  * @param src The source YUV buffer.
1086  * @param dst The destination YUV buffer.
1087  * @param width The width.
1088  * @param height The height. */
1089 void
1090 MirrorCalibTool::apply_sharpen(unsigned char* src,
1091  unsigned char* dst,
1092  int width,
1093  int height)
1094 {
1095  ROI* roi = ROI::full_image(width, height);
1096  FilterSharpen filter;
1097  filter.set_src_buffer(src, roi, 0);
1098  filter.set_dst_buffer(dst, roi);
1099  filter.apply();
1100 }
1101 
1102 
1103 /** Applies a median filter.
1104  * @param src The source YUV buffer.
1105  * @param dst The destination YUV buffer.
1106  * @param width The width.
1107  * @param height The height.
1108  * @param i The median parameter. */
1109 void
1110 MirrorCalibTool::apply_median(unsigned char* src,
1111  unsigned char* dst,
1112  int width,
1113  int height,
1114  int i)
1115 {
1116  ROI* roi = ROI::full_image(width, height);
1117  FilterMedian filter(i);
1118  filter.set_src_buffer(src, roi, 0);
1119  filter.set_dst_buffer(dst, roi);
1120  filter.apply();
1121 }
1122 
1123 
1124 /** Applies a minimum filter.
1125  * @param src The source YUV buffer.
1126  * @param dst The destination YUV buffer.
1127  * @param width The width.
1128  * @param height The height. */
1129 void
1130 MirrorCalibTool::apply_min(unsigned char* src,
1131  unsigned char* dst,
1132  int width,
1133  int height)
1134 {
1135  ROI* roi = ROI::full_image(width, height);
1136  FilterMin filter;
1137  filter.set_src_buffer(src, roi, 0);
1138  filter.set_dst_buffer(dst, roi);
1139  filter.apply();
1140 }
1141 
1142 
1143 /** Applies a disjunction filter.
1144  * @param src1 The first source YUV buffer.
1145  * @param src2 The second source YUV buffer.
1146  * @param dst The destination YUV buffer.
1147  * @param width The width.
1148  * @param height The height. */
1149 void
1150 MirrorCalibTool::apply_or(unsigned char* src1,
1151  unsigned char* src2,
1152  unsigned char* dst,
1153  int width,
1154  int height)
1155 {
1156  ROI* roi = ROI::full_image(width, height);
1157  FilterOr filter;
1158  filter.set_src_buffer(src1, roi, 0);
1159  filter.set_src_buffer(src2, roi, 1);
1160  filter.set_dst_buffer(dst, roi);
1161  filter.apply();
1162 }
1163 
1164 
1165 /** Sets all pixels to black or white (i.e. maximum contrast).
1166  * @param buf The YUV buffer.
1167  * @param buflen Buffer's length. */
1168 void
1169 MirrorCalibTool::make_contrast(unsigned char* buf,
1170  size_t buflen)
1171 {
1172  for (unsigned int i = 0; i < buflen/2; i++) {
1173  buf[i] = (buf[i] >= 75) ? BRIGHT : DARK;
1174  }
1175 }
1176 
1177 
1178 /** Sets all chrominance values to 128.
1179  * @param buf The YUV buffer.
1180  * @param buflen Buffer's length. */
1181 void
1182 MirrorCalibTool::make_grayscale(unsigned char* buf,
1183  size_t buflen)
1184 {
1185  memset(buf + buflen/2, 128, buflen - buflen/2);
1186 }
1187 
1188 
1189 /** Get description of next step.
1190  * @return string that describes what's done in next_step().
1191  */
1192 const char*
1194 {
1195  switch (state_.step) {
1196  case SHARPENING: return "Sharpen";
1197  case EDGE_DETECTION: return "Edge detection";
1198  case COMBINATION: return "Combining images";
1199  case CENTERING: return "Finding center point";
1200  case PRE_MARKING: return "First marking";
1201  case FINAL_MARKING: return "Final marking";
1202  case DONE: return "Direction done";
1203  default: return "Invalid state";
1204  }
1205 }
1206 
1207 
1208 /** Finds the first marks. This is the first step in finding marks.
1209  * @return mark list
1210  */
1211 MirrorCalibTool::MarkList
1212 MirrorCalibTool::premark(const StepResult& prev,
1213  const unsigned char* yuv_mask,
1214  StepResult& result,
1215  PolarAngle phi,
1216  const PixelPoint& center)
1217 {
1218  const ConvexPolygon empty_polygon;
1219  return premark(empty_polygon, prev, yuv_mask, result, phi, center);
1220 }
1221 
1222 
1223 /** Finds the first marks. This is the first step in finding marks.
1224  * @return mark list
1225  */
1226 MirrorCalibTool::MarkList
1227 MirrorCalibTool::premark(const ConvexPolygon& polygon,
1228  const StepResult& prev,
1229  const unsigned char* yuv_mask,
1230  StepResult& result,
1231  PolarAngle phi,
1232  const PixelPoint& center)
1233 {
1234  const CartesianImage prev_img(prev, phi, center, yuv_mask);
1235  CartesianImage res_img(result, phi, center, yuv_mask);
1236  int width = MIN_WIDTH_OF_BIGGEST_LINE;
1237  MarkList premarks;
1238  for (PolarRadius length = 0; length < prev_img.max_radius(); length++)
1239  {
1240  const CartesianPoint p(0.0, length);
1241  if (polygon.contains(prev_img, p) && prev_img.is_line(p, width)) {
1242  premarks.push_back(length);
1243  res_img.highlight_line(p, width);
1244  }
1245  if (length % 25 == 0) {
1246  width *= (1.0f - APPROX_LINE_WIDTH_LOSS);
1247  }
1248  }
1249  return premarks;
1250 }
1251 
1252 
1253 /** Searches for holes between the pre-marks. Helper for mark().
1254  * @return holes
1255  */
1256 MirrorCalibTool::HoleList
1257 MirrorCalibTool::search_holes(const MarkList& premarks)
1258 {
1259  HoleList holes;
1260  PolarRadius prev_radius = -1;
1261  for (MarkList::const_iterator it = premarks.begin();
1262  it != premarks.end(); it++)
1263  {
1264  PolarRadius radius = *it;
1265  if (prev_radius != -1 && prev_radius + 1 < radius) {
1266  Hole hole(holes.size(), prev_radius, radius-1);
1267  holes.push_back(hole);
1268  }
1269  prev_radius = radius;
1270  }
1271  return holes;
1272 }
1273 
1274 
1275 /** Removes all but the n biggest holes (size depends on their position).
1276  * Helper for mark(). */
1277 MirrorCalibTool::HoleList
1278 MirrorCalibTool::filter_biggest_holes(const HoleList& holes,
1279  unsigned int n)
1280 {
1281 #ifdef FILTER_HOLES
1282  HoleList biggest = holes;
1283 #ifdef FILTER_MINI_HOLES
1284 restart: // XXX ugly :-)
1285  for (HoleList::iterator it = biggest.begin(); it != biggest.end(); it++)
1286  {
1287  if (it->size() <= FALSE_POSITIVE_HOLE_SIZE) {
1288  biggest.erase(it);
1289  goto restart;
1290  }
1291  }
1292 #endif
1293 
1294  HoleList filtered;
1295  for (unsigned int from = 0; from < biggest.size(); from++)
1296  {
1297  unsigned int to;
1298  for (to = from + 1; to < biggest.size(); to++) {
1299  if ((to - from + 1) > n) {
1300  to = from + n;
1301  break;
1302  }
1303  if (biggest[to - 1].size() < biggest[to].size()) {
1304  break;
1305  }
1306  }
1307  to--; // in all three break cases, to must be decremented
1308  if (to - from + 1 > filtered.size()) {
1309  filtered.clear();
1310  for (unsigned int j = from; j <= to; j++) {
1311  filtered.push_back(biggest[j]);
1312  }
1313  }
1314  }
1315  return filtered;
1316 #else
1317  HoleList biggest;
1318  for (HoleList::const_iterator it = holes.begin(); it != holes.end(); ++it)
1319  {
1320  const Hole& hole = *it;
1321 #ifdef FILTER_MINI_HOLES
1322  if (hole.size() < FALSE_POSITIVE_HOLE_SIZE) {
1323  // very small holes are usually false-positives
1324  continue;
1325  }
1326 #endif
1327  if (biggest.size() == 1 && hole.size() > biggest.front().size()) {
1328  // often the first determined hole is a part of the robot
1329  //biggest.erase(biggest.begin());
1330  }
1331  biggest.push_back(hole);
1332  if (biggest.size() == n) {
1333  break;
1334  }
1335  }
1336  return biggest;
1337 #endif
1338 }
1339 
1340 
1341 /** Calculates the position of the marks between holes. Helper for mark(). */
1342 MirrorCalibTool::MarkList
1343 MirrorCalibTool::determine_marks(const HoleList& holes)
1344 {
1345  HoleList biggest = filter_biggest_holes(holes, MARK_COUNT - 1);
1346  std::cout << "Filtered Holes: " << biggest.size() << std::endl;
1347  MarkList marks;
1348  for (HoleList::const_iterator prev, iter = biggest.begin();
1349  iter != biggest.end(); iter++) {
1350  const Hole& hole = *iter;
1351  if (iter == biggest.begin()) {
1352  const PolarRadius length = hole.from_length;
1353  marks.push_back(length);
1354  } else {
1355  const PolarRadius length = (hole.from_length + prev->to_length) / 2;
1356  marks.push_back(length);
1357  }
1358  if (iter+1 == biggest.end()) {
1359  const PolarRadius length = hole.to_length;
1360  marks.push_back(length);
1361  }
1362  prev = iter;
1363  }
1364  return marks;
1365 }
1366 
1367 
1368 /** Sets marks between all holes. The last step of marking. */
1369 MirrorCalibTool::MarkList
1370 MirrorCalibTool::mark(const MarkList& premarks,
1371  const unsigned char* yuv_mask,
1372  StepResult& result,
1373  PolarAngle phi,
1374  const PixelPoint& center)
1375 {
1376  std::cout << "Premarked places: " << premarks.size() << std::endl;
1377  HoleList holes = search_holes(premarks);
1378  std::cout << "Found Holes: " << holes.size() << std::endl;
1379  MarkList marks = determine_marks(holes);
1380  std::cout << "Found Marks: " << marks.size() << std::endl;
1381 
1382  CartesianImage res_img(result, phi, center, yuv_mask);
1383  for (MarkList::const_iterator iter = marks.begin();
1384  iter != marks.end(); iter++)
1385  {
1386  const PolarAngle angle = 0.0;
1387  const PolarRadius radius = *iter;
1388  res_img.highlight_point(CartesianPoint(angle, radius));
1389  std::cout << "Highlighting Mark at " << *iter << std::endl;
1390  }
1391  return marks;
1392 }
1393 
1394 
1395 void
1396 MirrorCalibTool::goto_next_state()
1397 {
1398  const Image& src_img = source_images_[state_.image_index];
1399  switch (state_.step) {
1400  case SHARPENING:
1401  state_.step = EDGE_DETECTION;
1402  break;
1403  case EDGE_DETECTION:
1404  if (src_img.results().size() <= ORIENTATION_COUNT) {
1405  state_.step = EDGE_DETECTION;
1406  } else {
1407  state_.step = COMBINATION;
1408  }
1409  break;
1410  case COMBINATION:
1411  if (src_img.results().size() < 2 * ORIENTATION_COUNT) {
1412  state_.step = COMBINATION;
1413  } else if (state_.image_index + 1 < source_images_.size()) {
1414  state_.step = SHARPENING;
1415  state_.image_index++;
1416  } else {
1417  state_.step = PRE_MARKING;
1418  state_.centering_done = false;
1419  state_.image_index = 0;
1420  }
1421  break;
1422  case CENTERING:
1423  state_.step = PRE_MARKING;
1424  state_.centering_done = true;
1425  state_.image_index = 0;
1426  break;
1427  case PRE_MARKING:
1428  state_.step = FINAL_MARKING;
1429  break;
1430  case FINAL_MARKING:
1431  if (state_.image_index + 1 < source_images_.size()) {
1432  state_.step = PRE_MARKING;
1433  state_.image_index++;
1434 #ifdef RECALCULATE_CENTER_POINT
1435  } else if (!state_.centering_done) {
1436  state_.step = CENTERING;
1437  state_.image_index = 0;
1438 #endif
1439  } else {
1440  state_.step = DONE;
1441  state_.image_index = 0;
1442  }
1443  break;
1444  case DONE:
1445  state_.step = DONE;
1446  state_.image_index = (state_.image_index + 1) % source_images_.size();
1447  break;
1448  default:
1449  throw fawkes::Exception("Invalid Calibration State");
1450  }
1451  printf("Next step: %s\n", get_state_description());
1452 }
1453 
1454 
1455 /** Performs one step in the calibration process.
1456  * In each step, a filter is applied to the image. The resulting image of
1457  * the n-th step is stored in the stepResults-vector on the (n-1)-th position.
1458  */
1459 void
1461 {
1462  printf("Performing step for image %u / %u, %s\n",
1463  (unsigned int)state_.image_index + 1,
1464  (unsigned int)source_images_.size(),
1466  if (state_.image_index >= source_images_.size()) {
1467  return;
1468  }
1469  Image& src_img = source_images_[state_.image_index];
1470  StepResult result(src_img.buflen(), src_img.width(), src_img.height());
1471  switch (state_.step) {
1472  case SHARPENING:
1473  {
1474  apply_sharpen(src_img.yuv_buffer(), result.yuv_buffer(),
1475  src_img.width(), src_img.height());
1476  make_grayscale(result.yuv_buffer(), result.buflen());
1477  }
1478  src_img.add_result(result);
1479  set_last_yuv_buffer(result.yuv_buffer());
1480  goto_next_state();
1481  return;
1482  case EDGE_DETECTION:
1483  {
1484  StepResult& prev = src_img.result(0);
1485  const int offset = (src_img.results().size() - 1) % ORIENTATION_COUNT;
1486  const orientation_t ori = static_cast<orientation_t>(ORI_DEG_0+offset);
1487  apply_sobel(prev.yuv_buffer(), result.yuv_buffer(),
1488  prev.width(), prev.height(), ori);
1489  make_grayscale(result.yuv_buffer(), result.buflen());
1490  make_contrast(result.yuv_buffer(), result.buflen());
1491  }
1492  printf("Edge detection in %u\n", (unsigned) src_img.results().size());
1493  src_img.add_result(result);
1494  set_last_yuv_buffer(result.yuv_buffer());
1495  assert(!memcmp(result.yuv_buffer(),
1497  result.buflen()));
1498  goto_next_state();
1499  return;
1500  case COMBINATION:
1501  {
1502  const int index1 = src_img.results().size() - ORIENTATION_COUNT == 1
1503  ? src_img.results().size() - ORIENTATION_COUNT
1504  : src_img.results().size() - 1;
1505  const int index2 = src_img.results().size() - ORIENTATION_COUNT + 1;
1506  assert(index1 != index2);
1507  printf("ORing: %d = or(%d, %d)\n", (unsigned) src_img.results().size(),
1508  index1, index2);
1509  StepResult& prev1 = src_img.result(index1);
1510  StepResult& prev2 = src_img.result(index2);
1511  assert(&prev1 != &prev2);
1512  assert(prev1.width() == prev2.width());
1513  assert(prev1.height() == prev2.height());
1514  apply_or(prev1.yuv_buffer(), prev2.yuv_buffer(), result.yuv_buffer(),
1515  prev1.width(), prev1.height());
1516  make_grayscale(result.yuv_buffer(), result.buflen());
1517  assert(memcmp(prev1.yuv_buffer(),
1518  prev2.yuv_buffer(),
1519  result.buflen()));
1520  assert(memcmp(result.yuv_buffer(),
1521  prev1.yuv_buffer(),
1522  result.buflen()));
1523  assert(memcmp(result.yuv_buffer(),
1524  prev1.yuv_buffer(),
1525  result.buflen()));
1526  }
1527  src_img.add_result(result);
1528  set_last_yuv_buffer(result.yuv_buffer());
1529  assert(!memcmp(result.yuv_buffer(),
1531  result.buflen()));
1532  goto_next_state();
1533  return;
1534  case CENTERING:
1535  {
1536  const StepResult& orig = src_img.result(0);
1537  memcpy(result.yuv_buffer(), orig.yuv_buffer(), result.buflen());
1538  const PixelPoint center = calculate_center(source_images_);
1539  img_center_x_ = center.x;
1540  img_center_y_ = center.y;
1541  std::cout << "Found center (" << center.x << ", "<< center.y << ")"
1542  << std::endl;
1543  CartesianImage img(result, relativeOrientationToImageRotation(0.0),
1544  center);
1545  img.highlight_point(CartesianPoint(0, 0));
1546  }
1547  src_img.add_result(result);
1548  set_last_yuv_buffer(result.yuv_buffer());
1549  goto_next_state();
1550  return;
1551  case PRE_MARKING:
1552  {
1553  const StepResult& prev = src_img.result(2 * ORIENTATION_COUNT - 1);
1554  memcpy(result.yuv_buffer(), prev.yuv_buffer(), result.buflen());
1555  const unsigned char* mask = img_yuv_mask_;
1556  const PixelPoint center(img_center_x_, img_center_y_);
1557  MarkList premarks = premark(prev, mask, result, src_img.ori(), center);
1558  src_img.set_premarks(premarks);
1559  apply_sharpen(src_img.yuv_buffer(), result.yuv_buffer(),
1560  src_img.width(), src_img.height());
1561  }
1562  src_img.add_result(result);
1563  set_last_yuv_buffer(result.yuv_buffer());
1564  goto_next_state();
1565  return;
1566  case FINAL_MARKING:
1567  {
1568  const StepResult& orig = src_img.result(0);
1569  memcpy(result.yuv_buffer(), orig.yuv_buffer(), result.buflen());
1570  const unsigned char* mask = img_yuv_mask_;
1571  const PixelPoint center(img_center_x_, img_center_y_);
1572  const MarkList marks = mark(src_img.premarks(), mask, result,
1573  src_img.ori(), center);
1574  src_img.set_marks(marks);
1575  const PolarAngle ori = src_img.ori();
1576  std::cout << "Marking done for orientation "
1577  << rad2deg(ori)
1578  << " = "
1579  << rad2deg(imageRotationToRelativeOrientation(ori))
1580  << std::endl;
1581  }
1582  src_img.add_result(result);
1583  set_last_yuv_buffer(result.yuv_buffer());
1584  goto_next_state();
1585  return;
1586  case DONE:
1587  {
1588  for (ImageList::iterator it = source_images_.begin();
1589  it != source_images_.end(); it++)
1590  {
1591  // cleanup: except the first result (whose yuv_buffer is needed)
1592  // no results are needed any more
1593  StepResultList results = it->results();
1594  results.erase(results.begin() + 1, results.end());
1595 
1596  const Image& src_img = *it;
1597  const PolarAngle ori = src_img.ori();
1598  MarkList marks = src_img.marks();
1599  mark_map_[imageRotationToRelativeOrientation(ori)] = marks;
1600  }
1601 
1602  const StepResult& prev = src_img.result(0);
1603  const PixelPoint center(img_center_x_, img_center_y_);
1604  CartesianImage img(result, relativeOrientationToImageRotation(0.0),
1605  center);
1606  memcpy(result.yuv_buffer(), prev.yuv_buffer(), result.buflen());
1607  img.highlight_pixel(center);
1608  img.highlight_point(CartesianPoint(0, 0)); // should be same as center
1609  for (MarkMap::const_iterator map_iter = mark_map_.begin();
1610  map_iter != mark_map_.end(); map_iter++)
1611  {
1612  const PolarAngle phi = map_iter->first;
1613  const MarkList& list = map_iter->second;
1614  printf("%3.f: ", rad2deg(phi));
1615  for (MarkList::const_iterator list_iter = list.begin();
1616  list_iter != list.end(); list_iter++)
1617  {
1618  const PolarAngle angle = phi;
1619  const PolarRadius radius = *list_iter;
1620  img.highlight_point(CartesianPoint(angle, radius));
1621  PixelPoint pp = img.to_pixel(CartesianPoint(angle, radius));
1622  printf("%3d (%d, %d)", radius, pp.x, pp.y);
1623  }
1624  printf("\n");
1625  }
1626  }
1627  src_img.add_result(result);
1628  set_last_yuv_buffer(result.yuv_buffer());
1629  goto_next_state();
1630  return;
1631  }
1632 }
1633 
1634 
1635 /**
1636  * Returns the center point determined by the mean of the first marks found in
1637  * the different directions.
1638  * The angle of the returned cartesian point is relative to the robots
1639  * orientation (and thus must be rotated by 90 degrees counter-clock-wise
1640  * to be displayed correctly).
1641  * @return the estimated center point relative to the robots orientation
1642  */
1644 MirrorCalibTool::calculate_center(const ImageList& images)
1645 {
1646  int x = 0;
1647  int y = 0;
1648  for (ImageList::const_iterator it = images.begin();
1649  it != images.end(); it++)
1650  {
1651  const Image& image = *it;
1652  const PolarRadius radius = image.marks().at(0);
1653  const unsigned char* null_buf = 0;
1654  const CartesianImage cart_image(null_buf, image.width(), image.height(),
1655  image.ori());
1656  const CartesianPoint point(0.0, radius);
1657  const PixelPoint pixel = cart_image.to_pixel(point);
1658  x += pixel.x;
1659  y += pixel.y;
1660  }
1661  return PixelPoint(x / images.size(), y / images.size());
1662 
1663  /*
1664  return PixelPoint(500, 500);
1665  int x = 0;
1666  int y = 0;
1667  int count = 0;
1668  for (MarkMap::const_iterator it = mark_map.begin();
1669  it != mark_map.end(); it++) {
1670  PolarAngle angle = it->first;
1671  PolarRadius dist = *(it->second.begin());
1672  CartesianPoint p(angle, dist);
1673  x += p.x;
1674  y += p.y;
1675  ++count;
1676  }
1677  CartesianPoint center(x/count, y/count);
1678  return center;
1679  */
1680 }
1681 
1682 
1683 /**
1684  * Searches the two angles in the MarkMap mark_map that are nearest to angle.
1685  * @param angle The reference angle to which the nearest marks are searched.
1686  * @param mark_map The mark map.
1687  * @return The two angles nearest to angle contained in mark_map (and therefore
1688  * keys to MarkLists in mark_map).
1689  */
1690 MirrorCalibTool::PolarAnglePair
1691 MirrorCalibTool::find_nearest_neighbors(PolarAngle angle,
1692  const MarkMap& mark_map)
1693 {
1694  typedef std::vector<PolarAngle> AngleList;
1695  AngleList diffs;
1696  for (MarkMap::const_iterator it = mark_map.begin();
1697  it != mark_map.end(); it++)
1698  {
1699  const PolarAngle mark_angle = it->first;
1700  const PolarAngle diff = normalize_mirror_rad(mark_angle - angle);
1701  diffs.push_back(diff);
1702 #if 0
1703  std::cout << "Finding nearest neighbors: "
1704  << "ref="<<angle<<"="<<rad2deg(angle)<<" "
1705  << "to="<<mark_angle<<"="<<rad2deg(mark_angle)<<" "
1706  << "diff="<<diff<<"="<<rad2deg(diff) << std::endl;
1707 #endif
1708  }
1709  bool min1_init = false;
1710  AngleList::size_type min1_index = 0;
1711  bool min2_init = false;
1712  AngleList::size_type min2_index = 0;
1713  for (int i = 0; static_cast<AngleList::size_type>(i) < diffs.size(); i++) {
1714  if (!min1_init || fabs(diffs[min1_index]) >= fabs(diffs[i])) {
1715  min2_index = min1_index;
1716  min2_init = min1_init;
1717  min1_index = i;
1718  min1_init = true;
1719  } else if (!min2_init || fabs(diffs[min2_index]) >= fabs(diffs[i])) {
1720  min2_index = i;
1721  min2_init = true;
1722  }
1723  }
1724  if (!min1_init) {
1725  throw fawkes::Exception("First minimum diff. angle not found");
1726  }
1727  if (!min2_init) {
1728  throw fawkes::Exception("Second minimum diff. angle not found");
1729  }
1730  PolarAngle min1 = 0.0;
1731  PolarAngle min2 = 0.0;
1732  AngleList::size_type i = 0;
1733  for (MarkMap::const_iterator it = mark_map.begin();
1734  it != mark_map.end(); it++)
1735  {
1736  if (i == min1_index) {
1737  min1 = it->first;
1738  } else if (i == min2_index) {
1739  min2 = it->first;
1740  }
1741  i++;
1742  }
1743 #if 0
1744  std::cout << "Found nearest neighbor 1: "
1745  << "ref="<<angle<<"="<<rad2deg(angle)<<" "
1746  << "to="<<min1<<"="<<rad2deg(min1) <<" "
1747  << "index="<<min1_index << std::endl;
1748  std::cout << "Found nearest neighbor 2: "
1749  << "ref="<<angle<<"="<<rad2deg(angle)<<" "
1750  << "to="<<min2<<"="<<rad2deg(min2) <<" "
1751  << "index="<<min2_index << std::endl;
1752 #endif
1753  return PolarAnglePair(min1, min2);
1754 }
1755 
1756 
1757 /**
1758  * Calculates the real distance to the n-th mark.
1759  * @param n The index of the mark, starting at 0.
1760  * @return The distance in centimeters.
1761  */
1762 MirrorCalibTool::RealDistance
1763 MirrorCalibTool::calculate_real_distance(int n)
1764 {
1765  return static_cast<int>(CARPET_DISTANCE_FROM_ROBOT_CENTER +
1766  static_cast<float>(n + 1) * MARK_DISTANCE);
1767 }
1768 
1769 
1770 MirrorCalibTool::RealDistance
1771 MirrorCalibTool::interpolate(PolarRadius radius,
1772  const MarkList& marks)
1773 {
1774  if (marks.size() < 2) {
1775  throw fawkes::Exception("Not enough marks for interpolation");
1776  }
1777  for (MarkList::size_type i = 0; i < marks.size(); i++)
1778  {
1779  // linear interpolation, x0, x1 = 'Stuetzstellen', f0, f1 = 'Stuetzwerte'
1780  if (i == 0 && radius < marks[i]) {
1781  const PolarRadius x0 = 0;
1782  const PolarRadius x1 = marks[i];
1783  const PolarRadius x = radius;
1784  const RealDistance f0 = 0;
1785  const RealDistance f1 = calculate_real_distance(i);
1786 #if 0
1787  std::cout << "A_Interpolate " << x << " between "
1788  << x0 << "->" << f0 << " "
1789  << x1 << "->" << f1 << std::endl;
1790 #endif
1791  return static_cast<RealDistance>(static_cast<double>(f0) +
1792  static_cast<double>(x - x0) *
1793  static_cast<double>(f1 - f0) /
1794  static_cast<double>(x1 - x0));
1795  } else if (i > 0 && marks[i-1] <= radius &&
1796  (radius < marks[i] || i+1 == marks.size())) {
1797  const PolarRadius x0 = marks[i-1];
1798  const PolarRadius x1 = marks[i];
1799  const PolarRadius x = radius;
1800  const RealDistance f0 = calculate_real_distance(i-1);
1801  const RealDistance f1 = calculate_real_distance(i);
1802 #if 0
1803  std::cout << "B_Interpolate " << x << " between "
1804  << x0 << "->" << f0 << " "
1805  << x1 << "->" << f1 << std::endl;
1806 #endif
1807  return static_cast<RealDistance>(static_cast<double>(f0) +
1808  static_cast<double>(x - x0) *
1809  static_cast<double>(f1 - f0) /
1810  static_cast<double>(x1 - x0));
1811  }
1812  }
1813  throw fawkes::Exception("Interpolation Error");
1814 }
1815 
1816 
1817 /**
1818  * Generates a new bulb based on the given data.
1819  * @param width The width of the image.
1820  * @param height The height of the image.
1821  * @param center The center pixel point of the image.
1822  * @param mark_map The map of marks where the key is the orientation of the
1823  * marks and the value is a sequence of distances to the marks.
1824  */
1825 Bulb
1826 MirrorCalibTool::generate(int width,
1827  int height,
1828  const PixelPoint& center,
1829  const MarkMap& mark_map)
1830 {
1831  const unsigned char* null_img_buf = 0;
1832  CartesianImage img(null_img_buf, width, height,
1833  relativeOrientationToImageRotation(0.0), center);
1834  Bulb bulb(width, height);
1835  bulb.setCenter(center.x, center.y);
1836  bulb.setOrientation(0.0);
1837  for (int x = 0; x < width; x++)
1838  {
1839  for (int y = 0; y < height; y++)
1840  {
1841  const PixelPoint pp(x, y);
1842  const CartesianPoint cp = img.to_cartesian(pp);
1843  const PolarAngle ori_to_robot = cp.atan();
1844  const PolarAnglePair nearest_neighbors =
1845  find_nearest_neighbors(ori_to_robot, mark_map);
1846  const PolarAngle diff1 =
1847  fabs(normalize_mirror_rad(nearest_neighbors.first - ori_to_robot));
1848  const PolarAngle diff2 =
1849  fabs(normalize_mirror_rad(nearest_neighbors.second - ori_to_robot));
1850  const PolarAngle norm = diff1 + diff2;
1851  assert(norm != 0.0);
1852  const double weight1 = 1.0 - diff1 / norm;
1853  const double weight2 = 1.0 - diff2 / norm;
1854 #if 0
1855  std::cout << "PixelPoint("<< x <<", "<< y <<")"<< std::endl;
1856  std::cout << "CartesianPoint("<< cp.x <<", "<< cp.y <<")"<< std::endl;
1857  std::cout << "Radius, Angle: " << cp.length() << " "
1858  << ori_to_robot << " = "
1859  << rad2deg(ori_to_robot) << std::endl;
1860  std::cout << "Neighbor 1: "
1861  << normalize_rad(nearest_neighbors.first) << " = "
1862  << rad2deg(normalize_rad(nearest_neighbors.first))
1863  << std::endl;
1864  std::cout << "Neighbor 2: "
1865  << normalize_rad(nearest_neighbors.second) << " = "
1866  << rad2deg(normalize_rad(nearest_neighbors.second))
1867  << std::endl;
1868  std::cout << "Diff 1: " << diff1 << " = " << rad2deg(diff1) << std::endl;
1869  std::cout << "Diff 2: " << diff2 << " = " << rad2deg(diff2) << std::endl;
1870  std::cout << "Norm Factor: " << norm << " = " << rad2deg(norm)
1871  << std::endl;
1872  std::cout << "Weight 1: " << weight1 << " = " << rad2deg(weight1)
1873  << std::endl;
1874  std::cout << "Weight 2: " << weight2 << " = " << rad2deg(weight2)
1875  << std::endl;
1876 #endif
1877  assert(0.0 <= weight1 && weight1 <= 1.0);
1878  assert(0.0 <= weight2 && weight2 <= 1.0);
1879  assert(1.0 - 10e-5 < weight1 + weight2 && weight1 + weight2 < 1.0 + 10e-5);
1880  const MarkList& marks1 = mark_map.at(nearest_neighbors.first);
1881  const MarkList& marks2 = mark_map.at(nearest_neighbors.second);
1882  const RealDistance dist1 = interpolate(cp.length(), marks1);
1883  const RealDistance dist2 = interpolate(cp.length(), marks2);
1884 #if 0
1885  std::cout << "Real 1 " << dist1 << std::endl;
1886  std::cout << "Real 2 " << dist2 << std::endl;
1887 #endif
1888  const RealDistance weighted_mean_dist = dist1 * weight1 + dist2 * weight2;
1889  const float world_dist_in_meters = weighted_mean_dist / 100.0f;
1890  const float world_phi_rel_to_robot = -1.0f * ori_to_robot;
1891  // world_phi_rel_to_robot must be multiplied with -1 because the image is
1892  // mirrored: what's on the right in the image, is on the left of the robot
1893  // in reality
1894 #if 0
1895  std::cout << "Dist 1: " << dist1 << std::endl;
1896  std::cout << "Dist 2: " << dist2 << std::endl;
1897  std::cout << "World Dist: " << world_dist_in_meters << std::endl;
1898  std::cout << "World Phi: " << ori_to_robot << " = "
1899  << rad2deg(ori_to_robot) << std::endl;
1900 #endif
1901  if (world_dist_in_meters > 0) {
1902  bulb.setWorldPoint(x, y, world_dist_in_meters, world_phi_rel_to_robot);
1903  }
1904  }
1905  }
1906  return bulb;
1907 }
1908 
1909 
1910 void
1911 MirrorCalibTool::set_last_yuv_buffer(const unsigned char* last_buf)
1912 {
1913  last_yuv_buffer_ = last_buf;
1914 }
1915 
1916 
1917 /** Get last created YUV buffer.
1918  * This is the result of the most recent step. Memory management is
1919  * done by MirrorCalibTool.
1920  * @return most recent result's YUV buffer
1921  */
1922 const unsigned char*
1924 {
1925  return last_yuv_buffer_;
1926 }
1927 
1928 
1929 /** Get the assumed distance and orientation of a pixel point.
1930  * @param x The X coordinate of the pixel that is to be evaluated.
1931  * @param y The Y coordinate of the pixel that is to be evaluated.
1932  * @param dist_ret The distance in the real world to the pixel.
1933  * @param ori_ret The orientation in the real world to the pixel.
1934  */
1935 void
1936 MirrorCalibTool::eval(unsigned int x, unsigned int y,
1937  float* dist_ret, float* ori_ret)
1938 {
1939  if (!bulb_) {
1940  printf("No bulb loaded, cannot evaluate.\n");
1941  return;
1942  }
1943  polar_coord_2d_t coord;
1944  coord = bulb_->getWorldPointRelative(x, y);
1945 
1946  *dist_ret = coord.r;
1947  *ori_ret = coord.phi;
1948 }
1949 
1950 
1951 /** Loads a calibration file.
1952  * @param filename Name of the file containing the calibration data.
1953  */
1954 void
1955 MirrorCalibTool::load(const char* filename)
1956 {
1957  bulb_ = new Bulb(filename);
1958 }
1959 
1960 
1961 /** Saves calibration data to a file.
1962  * @param filename The name of the file.
1963  */
1964 void
1965 MirrorCalibTool::save(const char* filename)
1966 {
1967  if (state_.step == DONE) {
1968  const Image& src_img = source_images_[state_.image_index];
1969  const PixelPoint center(img_center_x_, img_center_y_);
1970  Bulb bulb = generate(src_img.width(), src_img.height(), center, mark_map_);
1971  bulb.save(filename);
1972  } else {
1973  std::cout << "Can't save in the middle of the calibration" << std::endl;
1974  }
1975 }
1976 
1977 
1978 /** Draws a line from the image center in the given angle.
1979  * @param yuv_buffer The in/out parameter for the image.
1980  * @param angle_deg Angle of line in degrees.
1981  * @param center_x X-coordinate of center point in image pixels.
1982  * @param center_y Y-coordinate of center point in image pixels.
1983  * @param width Width of image.
1984  * @param height Height of image.
1985  */
1986 void
1987 MirrorCalibTool::draw_line(unsigned char* yuv_buffer,
1988  double angle_deg,
1989  int center_x,
1990  int center_y,
1991  int width,
1992  int height)
1993 {
1994  const PolarAngle angle = normalize_rad(deg2rad(angle_deg));
1995  CartesianImage img(yuv_buffer, width, height,
1996  relativeOrientationToImageRotation(0.0),
1997  PixelPoint(center_x, center_y));
1998  for (PolarRadius length = 0; length < img.max_radius(); length++)
1999  {
2000  const CartesianPoint p(angle, length);
2001  if (img.contains(p)) {
2002  img.set_color(p, MARK_LUMA, MARK_CHROMINANCE);
2003  }
2004  }
2005 }
2006 
2007 
2008 /** Draws a crosshair with the lines in the directions of the keys of
2009  * the mark map.
2010  * @param yuv_buffer The in/out parameter for the image.
2011  */
2012 void
2013 MirrorCalibTool::draw_mark_lines(unsigned char* yuv_buffer)
2014 {
2015  for (ImageList::const_iterator it = source_images_.begin();
2016  it != source_images_.end(); it++)
2017  {
2018  const Image& src_img = *it;
2019  CartesianImage img(yuv_buffer, src_img.width(), src_img.height(),
2020  src_img.ori(), PixelPoint(img_center_x_, img_center_y_));
2021  for (PolarRadius length = 0; length < img.max_radius(); length++)
2022  {
2023  const PolarAngle angle = 0.0;
2024  const CartesianPoint p(angle, length);
2025  if (img.contains(p)) {
2026  img.set_color(p, MARK_LUMA, MARK_CHROMINANCE);
2027  }
2028  }
2029  }
2030 }
2031 
2032 
2033 /** Draws a crosshair in the YUV-buffer. The crosshair consists of three lines
2034  * from the origin (0, 0) with the angles 0 degree, 120 degrees, 240 degrees.
2035  * @param yuv_buffer The in/out parameter for the image.
2036  * @param center_x X-coordinate of center point in image pixels.
2037  * @param center_y Y-coordinate of center point in image pixels.
2038  * @param width Width of image.
2039  * @param height Height of image.
2040  */
2041 void
2042 MirrorCalibTool::draw_crosshair(unsigned char* yuv_buffer,
2043  int center_x,
2044  int center_y,
2045  int width,
2046  int height)
2047 {
2048  const PolarAngle POSITIONS[] = { normalize_rad(deg2rad( 0.0f)),
2049  normalize_rad(deg2rad(-120.0f)),
2050  normalize_rad(deg2rad( 120.0f)) };
2051  const int POSITION_COUNT = sizeof POSITIONS / sizeof(double);
2052  CartesianImage img(yuv_buffer, width, height,
2053  relativeOrientationToImageRotation(0.0),
2054  PixelPoint(center_x, center_y));
2055  for (int i = 0; i < POSITION_COUNT; i++)
2056  {
2057  const PolarAngle angle = POSITIONS[i];
2058  for (PolarRadius length = 0; length < img.max_radius(); length++)
2059  {
2060  const CartesianPoint p(angle, length);
2061  if (img.contains(p)) {
2062  img.set_color(p, MARK_LUMA, MARK_CHROMINANCE);
2063  }
2064  }
2065  }
2066 }
2067 
2068 }
2069 
void load_mask(const char *mask_file_name)
Loads a PNM mask file for the robot&#39;s bars.
Point(int x, int y)
Constructor.
int center_y() const
Center Y accessor.
Definition: mirror_calib.h:80
unsigned char V
V component.
Definition: yuv.h:59
void set_color(const CartesianPoint &p, unsigned char luma, unsigned char chrominance)
Sets the luminance Y and chrominance U at a given cartesian point.
static void draw_crosshair(unsigned char *yuv_buffer, int center_x, int center_y, int width, int height)
Draws a crosshair in the YUV-buffer.
void highlight_pixel(const PixelPoint &p)
Highlights a pixel, i.e.
Point operator=(const Point &p)
Assignment.
const int width() const
Width of buffer.
PNM file reader.
Definition: pnm.h:36
virtual void apply()
Apply the filter.
Definition: or.cpp:55
PixelPoint rotate(PolarAngle rotate_phi) const
Rotates the vector to the point counter-clockwise and returns the vector to the point.
virtual void set_src_buffer(unsigned char *buf, ROI *roi, orientation_t ori=ORI_HORIZONTAL, unsigned int buffer_num=0)
Set source buffer with orientation.
Definition: filter.cpp:93
The result of a step contains a YUV buffer.
void eval(unsigned int x, unsigned int y, float *x_ret, float *y_ret)
Get the assumed distance and orientation of a pixel point.
PolarRadius length() const
Length of the vector the point.
float normalize_rad(float angle_rad)
Normalize angle in radian between 0 (inclusive) and 2*PI (exclusive).
Definition: angle.h:93
Fawkes library namespace.
const StepResultList & results() const
List of results.
void draw_line(const CartesianPoint &p, const CartesianPoint &q)
Draws a line from p to q.
A container for a YUV-buffer etc.
A pixel point is a 2d point with positive X and Y coords.
StepResult(const StepResult &copy)
Constructor.
DrawingManipulator * set_color(float r, float g, float b)
Creates a drawing manipulator which sets the given color.
int width() const
Width of YUV buffer.
CartesianImage(const unsigned char *buf, int width, int height, PolarAngle phi, const PixelPoint &center)
Constructor.
PixelPoint to_pixel(const CartesianPoint &p) const
Converts a cartesian point to a pixel point.
PolarRadius from_length
Beginning of the hole.
A hole is a sequence of pixels between two lines.
Or filter.
Definition: or.h:37
void set_color(const PixelPoint &p, unsigned char luma, unsigned char chrominance)
Sets the luminance Y and chrominance U at a given pixel point.
int index
The index of the hole.
Region of interest.
Definition: roi.h:58
CartesianPoint(PolarAngle phi, PolarRadius length)
Constructor.
CartesianImage(unsigned char *buf, int width, int height, PolarAngle phi, const PixelPoint &center)
Constructor.
const int height() const
Height of buffer.
PolarRadius size() const
The size of the hole in pixels.
CartesianPoint to_cartesian(const PixelPoint &p) const
Converts a pixel point to a cartesian point.
void draw_mark_lines(unsigned char *yuv_buffer)
Draws a crosshair with the lines in the directions of the keys of the mark map.
bool contains(const CartesianPoint &p) const
Indicates whether the image contains a cartesian point.
virtual void apply()
Apply the filter.
Definition: sobel.cpp:119
unsigned char Y
Y component.
Definition: yuv.h:57
virtual colorspace_t colorspace()
Get colorspace from the just read image.
Definition: pnm.cpp:144
const unsigned char * yuv_buffer() const
YUV buffer.
int max_x() const
Get Maximum cartesian X coordinate of the image.
virtual void apply()
Apply the filter.
Definition: median.cpp:56
Hole(int index, PolarRadius from_length, PolarRadius to_length)
Constructor.
void add_result(const StepResult &r)
Appends a result.
Wraps an image so that access to (0, 0) is mapped to the middle of the image and so on...
Polar coordinates.
Definition: types.h:53
virtual void apply()
Apply the filter.
Definition: sharpen.cpp:53
static ROI * full_image(unsigned int width, unsigned int height)
Get full image ROI for given size.
Definition: roi.cpp:560
MarkList & marks()
The (final) marks.
PolarRadius max_radius() const
Maximum polar radius of the image.
StepResultList & results()
List of results.
Sharpen filter.
Definition: sharpen.h:37
void highlight_line(const CartesianPoint &p, int length)
Highlightes a line at pixel point p.
void push_back(const unsigned char *yuv_buffer, size_t buflen, int width, int height, double ori)
Store image for calibration process.
void load(const char *filename)
Loads a calibration file.
bool is_line(const CartesianPoint &p, int length) const
Indicates whether at pixel point p there is a highlighted line.
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
Definition: angle.h:75
StepResult & operator=(const StepResult &copy)
Assignment.
int max_y() const
Get Maximum cartesian Y coordinate of the image.
static void draw_line(unsigned char *yuv_buffer, double angle_deg, int center_x, int center_y, int width, int height)
Draws a line from the image center in the given angle.
void save(const char *filename)
Saves calibration data to a file.
virtual void read()
Read data from file.
Definition: pnm.cpp:162
Base class for exceptions in Fawkes.
Definition: exception.h:36
const unsigned char * buf() const
Buffer.
virtual void apply()
Apply the filter.
Definition: min.cpp:50
CartesianPoint rotate(PolarAngle rotate_phi) const
Rotates the vector to the point counter-clockwise and returns the vector to the point.
int height() const
YUV buffer&#39;s height.
PixelPoint(int x, int y)
Constructor.
int width() const
YUV buffer&#39;s width.
unsigned char * yuv_buffer()
YUV buffer.
const unsigned char * mask() const
Mask.
virtual unsigned int pixel_height()
Get height of read image in pixels.
Definition: pnm.cpp:156
const MarkList & marks() const
The (final) marks.
CartesianImage(const unsigned char *buf, int width, int height, PolarAngle phi)
Constructor.
virtual fawkes::polar_coord_2d_t getWorldPointRelative(unsigned int image_x, unsigned int image_y) const
Get relative coordinate based on image coordinates.
Definition: bulb.cpp:364
void abort()
Aborts the calibration process.
void draw_line(const PixelPoint &p, const PixelPoint &q)
Draws a line from p to q.
Median filter.
Definition: median.h:37
A cartesian point is a 2d point which can have negative X and Y coords.
Bulb mirror lookup table.
Definition: bulb.h:38
virtual void setOrientation(float angle)
Set orientation of the omni-camera device.
Definition: bulb.cpp:516
const PixelPoint & center() const
Center of buffer.
size_t buflen() const
YUV buffer&#39;s length.
CartesianImage(const StepResult &res, PolarAngle phi, const unsigned char *mask=0)
Constructor.
int height() const
Height of YUV buffer.
float rad2deg(float rad)
Convert an angle given in radians to degrees.
Definition: angle.h:48
StepResult(size_t buflen, int width, int height)
Constructor.
size_t buflen() const
YUV buffer&#39;s length.
CartesianImage(const StepResult &res, PolarAngle phi, PixelPoint center, const unsigned char *mask=0)
Constructor.
const unsigned char * get_last_yuv_buffer() const
Get last created YUV buffer.
float bright_fraction(const CartesianPoint &from, const CartesianPoint &to) const
Get relative amount of bright pixels.
float r
distance
Definition: types.h:54
Image(const unsigned char *yuv_buffer, size_t buflen, int width, int height, PolarAngle ori)
Constructor.
StepResult & result(int i)
Returns the i-th result.
Image & operator=(const Image &copy)
Assignment.
PolarAngle ori() const
Angle of marks wrt X axis.
Minimum filter.
Definition: min.h:34
const StepResult & result(int i) const
Returns the i-th result.
unsigned char U
U component.
Definition: yuv.h:58
void set_marks(const MarkList &marks)
The (final) marks.
const PolarAngle phi() const
Phi angle.
PolarRadius to_length
Ending of the hole.
void set_premarks(const MarkList &premarks)
The premarks.
YUV pixel.
Definition: yuv.h:56
float phi
angle
Definition: types.h:55
void next_step()
Performs one step in the calibration process.
Image(const Image &copy)
Constructor.
virtual unsigned int pixel_width()
Get width of read image in pixels.
Definition: pnm.cpp:150
const char * get_state_description() const
Get description of next step.
virtual void setCenter(unsigned int image_x, unsigned int image_y)
Set center of omni-camera to given image pixel.
Definition: bulb.cpp:498
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:37
void highlight_point(const CartesianPoint &p)
Highlights a point, i.e.
int center_x() const
Center X accessor.
Definition: mirror_calib.h:77
PolarAngle atan() const
Atan(y, x) of the point.
void save(const char *filename)
Save LUT from file.
Definition: bulb.cpp:287
virtual void setWorldPoint(unsigned int image_x, unsigned int image_y, float world_r, float world_phi)
Set a world point mapping.
Definition: bulb.cpp:447
CartesianImage(unsigned char *buf, int width, int height, PolarAngle phi)
Constructor.
const MarkList & premarks()
The premarks.
virtual void set_buffer(unsigned char *yuv422planar_buffer)
Set buffer that the read image should be written to.
Definition: pnm.cpp:138
virtual void set_dst_buffer(unsigned char *buf, ROI *roi)
Set the destination buffer.
Definition: filter.cpp:134
bool contains(const PixelPoint &p) const
Indicates whether the image contains a pixel point.
CartesianPoint(int x, int y)
Constructor.
Sobel filter.
Definition: sobel.h:37
unsigned char * yuv_buffer()
YUV buffer.
const unsigned char * yuv_buffer() const
YUV buffer.