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 /** Constructor. */
714 MirrorCalibTool::ConvexPolygon::ConvexPolygon()
715 {
716 }
717 
718 /** Check if point is inside convex polygon.
719  * The point r is converted to PixelPoint wrt img.
720  * @param img image in which to check
721  * @param r cartesian point to check
722  * @return true if the point is inside the convex polygon
723  */
724 bool
725 MirrorCalibTool::ConvexPolygon::contains(const CartesianImage& img,
726  const CartesianPoint& r) const
727 {
728  return contains(img.to_pixel(r));
729 }
730 
731 
732 /** Check if pixel point is inside convex polygon.
733  * This is the case if for all points p, q in the polygon p_1, ..., p_n
734  * where p = p_i, q = p_{i+1} for some i or p = p_n, q = p_1 it holds
735  * (p, q, r) does not form a left turn (if they do, they are
736  * counter-clock-wise).
737  * @param r point to check
738  * @return true if the point is inside the convex polygon
739  */
740 bool
741 MirrorCalibTool::ConvexPolygon::contains(const PixelPoint& r) const
742 {
743  for (std::vector<PixelPoint>::size_type i = 1; i <= size(); i++) {
744  const PixelPoint& p = at(i-1);
745  const PixelPoint& q = at(i % size());
746  double val = (q.x - p.x) * (r.y - p.y) - (r.x - p.x) * (q.y - p.y);
747  if (val < 0) { // (p, q, r) forms a left turn
748  return false;
749  }
750  }
751  return true;
752 }
753 
754 
755 /** A hole is a sequence of pixels between two lines. */
757  public:
758  int index; /**< The index of the hole. */
759  PolarRadius from_length; /**< Beginning of the hole. */
760  PolarRadius to_length; /**< Ending of the hole. */
761 
762  /** Constructor.
763  * @param index The index of the hole.
764  * @param from_length The beginning of the hole in pixels.
765  * @param to_length The ending of the hole in pixels. */
766  Hole(int index,
767  PolarRadius from_length,
768  PolarRadius to_length)
769  : index(index),
770  from_length(from_length),
771  to_length(to_length)
772  {
773  }
774 
775  /** The size of the hole in pixels.
776  * @return radius of hole
777  */
778  inline PolarRadius size() const { return to_length - from_length; }
779 }; // Hole
780 
781 
782 /** A container for a YUV-buffer etc. */
784 {
785  public:
786  /** Constructor.
787  * @param yuv_buffer The YUV buffer.
788  * @param buflen The buffer's size.
789  * @param width The width.
790  * @param height The height.
791  * @param ori The orientation. */
792  Image(const unsigned char* yuv_buffer,
793  size_t buflen,
794  int width,
795  int height,
796  PolarAngle ori)
797  : yuv_buffer_(new unsigned char[buflen]),
798  buflen_(buflen),
799  width_(width),
800  height_(height),
801  ori_(ori),
802  refcount_(new unsigned int)
803  {
804  memcpy(yuv_buffer_, yuv_buffer, buflen);
805  *refcount_ = 1;
806  }
807 
808  /** Constructor.
809  * @param copy */
810  Image(const Image& copy)
811  : yuv_buffer_(copy.yuv_buffer_),
812  buflen_(copy.buflen_),
813  width_(copy.width_),
814  height_(copy.height_),
815  ori_(copy.ori_),
816  results_(copy.results_),
817  premarks_(copy.premarks_),
818  marks_(copy.marks_),
819  refcount_(copy.refcount_)
820  {
821  ++*refcount_;
822  }
823 
824  /** Assignment.
825  * @param copy image to copy
826  * @return this image
827  */
828  Image& operator=(const Image& copy)
829  {
830  if (this != &copy) {
831  if (--*refcount_ == 0) {
832  delete[] yuv_buffer_;
833  delete refcount_;
834  }
835  yuv_buffer_ = copy.yuv_buffer_;
836  buflen_ = copy.buflen_;
837  width_ = copy.width_;
838  height_ = copy.height_;
839  ori_ = copy.ori_;
840  results_ = copy.results_;
841  premarks_ = copy.premarks_;
842  marks_ = copy.marks_;
843  refcount_ = copy.refcount_;
844  ++*copy.refcount_;
845  }
846  return *this;
847  }
848 
849  /** Destructor. */
851  {
852  if (--*refcount_ == 0) {
853  delete[] yuv_buffer_;
854  delete refcount_;
855  }
856  }
857 
858  /** YUV buffer.
859  * @return YUV buffer
860  */
861  inline unsigned char* yuv_buffer() { return yuv_buffer_; }
862  /** YUV buffer.
863  * @return
864  */
865  inline const unsigned char* yuv_buffer() const { return yuv_buffer_; }
866  /** YUV buffer's length.
867  * @return YUV buffer's length
868  */
869  inline size_t buflen() const { return buflen_; }
870  /** YUV buffer's width.
871  * @return YUV buffer's width
872  */
873  inline int width() const { return width_; }
874  /** YUV buffer's height.
875  * @return YUV buffer's height
876  */
877  inline int height() const { return height_; }
878  /** Angle of marks wrt X axis.
879  * @return Angle of marks wrt X axis
880  */
881  inline PolarAngle ori() const { return ori_; }
882  /** List of results.
883  * @return List of results
884  */
885  inline StepResultList& results() { return results_; }
886  /** List of results.
887  * @return List of results
888  */
889  inline const StepResultList& results() const { return results_; }
890  /** The premarks.
891  * @return The premarks
892  */
893  inline const MarkList& premarks() { return premarks_; }
894  /** The (final) marks.
895  * @return
896  */
897  inline MarkList& marks() { return marks_; }
898  /** The (final) marks.
899  * @return The (final) marks
900  */
901  inline const MarkList& marks() const { return marks_; }
902 
903  /** Appends a result.
904  * @param r The new result. */
905  inline void add_result(const StepResult& r) { results_.push_back(r); }
906  /** Returns the i-th result.
907  * @param i The index of the result starting with 0.
908  * @return result
909  */
910  inline StepResult& result(int i) { return results_[i]; }
911  /** Returns the i-th result.
912  * @param i The index of the result starting with 0.
913  * @return result
914  */
915  inline const StepResult& result(int i) const { return results_[i]; }
916  /** The premarks.
917  * @param premarks The list of premarks. */
918  inline void set_premarks(const MarkList& premarks) { premarks_ = premarks; }
919  /** The (final) marks.
920  * @param marks The list of marks. */
921  inline void set_marks(const MarkList& marks) { marks_ = marks; }
922 
923  private:
924  unsigned char* yuv_buffer_;
925  size_t buflen_;
926  int width_;
927  int height_;
928  PolarAngle ori_;
929  StepResultList results_;
930  MarkList premarks_;
931  MarkList marks_;
932  unsigned int* refcount_;
933 }; // Image
934 
935 
936 /** Constructor. */
937 MirrorCalibTool::MirrorCalibTool()
938  : img_yuv_buffer_(0),
939  img_center_x_(-1),
940  img_center_y_(-1),
941  img_yuv_mask_(0),
942  state_(CalibrationState())
943  , bulb_(0)
944 {
945 }
946 
947 
948 /** Destructor. */
950 {
951  if (img_yuv_buffer_) {
952  delete[] img_yuv_buffer_;
953  }
954  if (img_yuv_mask_) {
955  delete[] img_yuv_mask_;
956  }
957  if (bulb_) {
958  delete bulb_;
959  }
960 }
961 
962 
963 /**
964  * Converts an angle wrt. the Y axis (!) to the robots view to the needed image
965  * rotation so that the things, which lie at angle `ori' wrt. the Y axis, lie
966  * on the X axis of the rotated image.
967  * For example: if the marks are 120 degrees counter-clock-wise from the robot,
968  * the image needs to be rotated 120 degrees clock-wise (then the marks are
969  * in front of the robot, i.e. Y axis) and then 90 degrees clock-wise
970  * (to get the marks from the Y axis to the X axis).
971  *
972  * The reason that we want to have the marks on the X axis is that calculating
973  * with them is then a bit easier (because then their polar angle is always
974  * 0.0).
975  * @param ori The orientation.
976  * @return angle
977  */
978 MirrorCalibTool::PolarAngle
979 MirrorCalibTool::relativeOrientationToImageRotation(PolarAngle ori)
980 {
981  return normalize_rad(-1.0 * ori + deg2rad(-90.0));
982 }
983 
984 
985 /**
986  * Converts the rotation of the image to the orientation relative to the Y axis.
987  * See the documentation of relativeOrientationToImageRotation() of which this
988  * is the inverse.
989  * @param ori The orientation.
990  * @return angle
991  */
992 MirrorCalibTool::PolarAngle
993 MirrorCalibTool::imageRotationToRelativeOrientation(PolarAngle ori)
994 {
995  return normalize_rad(-1.0 * (ori + deg2rad(90.0)));
996 }
997 
998 
999 /**
1000  * Loads a PNM mask file for the robot's bars.
1001  * The mask is used to ignore those pixels that are part of the robot's bar.
1002  * @param mask_file_name The mask file name.
1003  */
1004 void
1005 MirrorCalibTool::load_mask(const char* mask_file_name)
1006 {
1007  if (img_yuv_mask_) {
1008  delete[] img_yuv_mask_;
1009  }
1010  PNMReader reader(mask_file_name);
1011  size_t size = colorspace_buffer_size(reader.colorspace(),
1012  reader.pixel_width(),
1013  reader.pixel_height());
1014  img_yuv_mask_ = new unsigned char[size];
1015  if (img_center_x_ == -1) {
1016  img_center_x_ = reader.pixel_width() / 2;
1017  }
1018  if (img_center_y_ == -1) {
1019  img_center_y_ = reader.pixel_height() / 2;
1020  }
1021  reader.set_buffer(img_yuv_mask_);
1022  reader.read();
1023 }
1024 
1025 
1026 /** Store image for calibration process.
1027  * @param yuv_buffer The image's yuv_buffer. It may be freeed by the caller
1028  * immediately, the MirrorCalibTool works with a copy of it.
1029  * @param buflen The length of yuv_buffer.
1030  * @param width The width of the image.
1031  * @param height The height of the image.
1032  * @param ori The polar angle in degrees (!) relative to the Y axis of
1033  * the image (!) where the marks are in the image (!) (not in
1034  * in reality from the robot's perspective).
1035  */
1036 void
1037 MirrorCalibTool::push_back(const unsigned char* yuv_buffer,
1038  size_t buflen,
1039  int width,
1040  int height,
1041  double ori)
1042 {
1043  ori = relativeOrientationToImageRotation(ori);
1044  Image src_img(yuv_buffer, buflen, width, height, ori);
1045  if (img_center_x_ == -1) {
1046  img_center_x_ = width / 2;
1047  }
1048  if (img_center_y_ == -1) {
1049  img_center_y_ = height / 2;
1050  }
1051  source_images_.push_back(src_img);
1052 }
1053 
1054 
1055 /** Aborts the calibration process. */
1056 void
1058 {
1059  img_center_x_ = -1;
1060  img_center_y_ = -1;
1061  state_ = CalibrationState();
1062  source_images_.clear();
1063  premarks_.clear();
1064  mark_map_.clear();
1065 }
1066 
1067 
1068 /** Applies a sobel filter for edge detection in some direction.
1069  * @param src The source YUV buffer.
1070  * @param dst The destination YUV buffer.
1071  * @param width The width.
1072  * @param height The height.
1073  * @param ori The orientation. */
1074 void
1075 MirrorCalibTool::apply_sobel(unsigned char* src,
1076  unsigned char* dst,
1077  int width,
1078  int height,
1079  orientation_t ori)
1080 {
1081  ROI* roi = ROI::full_image(width, height);
1082  FilterSobel filter;
1083  filter.set_src_buffer(src, roi, ori, 0);
1084  filter.set_dst_buffer(dst, roi);
1085  filter.apply();
1086 }
1087 
1088 
1089 /** Applies a sharpening filter.
1090  * @param src The source YUV buffer.
1091  * @param dst The destination YUV buffer.
1092  * @param width The width.
1093  * @param height The height. */
1094 void
1095 MirrorCalibTool::apply_sharpen(unsigned char* src,
1096  unsigned char* dst,
1097  int width,
1098  int height)
1099 {
1100  ROI* roi = ROI::full_image(width, height);
1101  FilterSharpen filter;
1102  filter.set_src_buffer(src, roi, 0);
1103  filter.set_dst_buffer(dst, roi);
1104  filter.apply();
1105 }
1106 
1107 
1108 /** Applies a median filter.
1109  * @param src The source YUV buffer.
1110  * @param dst The destination YUV buffer.
1111  * @param width The width.
1112  * @param height The height.
1113  * @param i The median parameter. */
1114 void
1115 MirrorCalibTool::apply_median(unsigned char* src,
1116  unsigned char* dst,
1117  int width,
1118  int height,
1119  int i)
1120 {
1121  ROI* roi = ROI::full_image(width, height);
1122  FilterMedian filter(i);
1123  filter.set_src_buffer(src, roi, 0);
1124  filter.set_dst_buffer(dst, roi);
1125  filter.apply();
1126 }
1127 
1128 
1129 /** Applies a minimum filter.
1130  * @param src The source YUV buffer.
1131  * @param dst The destination YUV buffer.
1132  * @param width The width.
1133  * @param height The height. */
1134 void
1135 MirrorCalibTool::apply_min(unsigned char* src,
1136  unsigned char* dst,
1137  int width,
1138  int height)
1139 {
1140  ROI* roi = ROI::full_image(width, height);
1141  FilterMin filter;
1142  filter.set_src_buffer(src, roi, 0);
1143  filter.set_dst_buffer(dst, roi);
1144  filter.apply();
1145 }
1146 
1147 
1148 /** Applies a disjunction filter.
1149  * @param src1 The first source YUV buffer.
1150  * @param src2 The second source YUV buffer.
1151  * @param dst The destination YUV buffer.
1152  * @param width The width.
1153  * @param height The height. */
1154 void
1155 MirrorCalibTool::apply_or(unsigned char* src1,
1156  unsigned char* src2,
1157  unsigned char* dst,
1158  int width,
1159  int height)
1160 {
1161  ROI* roi = ROI::full_image(width, height);
1162  FilterOr filter;
1163  filter.set_src_buffer(src1, roi, 0);
1164  filter.set_src_buffer(src2, roi, 1);
1165  filter.set_dst_buffer(dst, roi);
1166  filter.apply();
1167 }
1168 
1169 
1170 /** Sets all pixels to black or white (i.e. maximum contrast).
1171  * @param buf The YUV buffer.
1172  * @param buflen Buffer's length. */
1173 void
1174 MirrorCalibTool::make_contrast(unsigned char* buf,
1175  size_t buflen)
1176 {
1177  for (unsigned int i = 0; i < buflen/2; i++) {
1178  buf[i] = (buf[i] >= 75) ? BRIGHT : DARK;
1179  }
1180 }
1181 
1182 
1183 /** Sets all chrominance values to 128.
1184  * @param buf The YUV buffer.
1185  * @param buflen Buffer's length. */
1186 void
1187 MirrorCalibTool::make_grayscale(unsigned char* buf,
1188  size_t buflen)
1189 {
1190  memset(buf + buflen/2, 128, buflen - buflen/2);
1191 }
1192 
1193 
1194 /** Get description of next step.
1195  * @return string that describes what's done in next_step().
1196  */
1197 const char*
1199 {
1200  switch (state_.step) {
1201  case SHARPENING: return "Sharpen";
1202  case EDGE_DETECTION: return "Edge detection";
1203  case COMBINATION: return "Combining images";
1204  case CENTERING: return "Finding center point";
1205  case PRE_MARKING: return "First marking";
1206  case FINAL_MARKING: return "Final marking";
1207  case DONE: return "Direction done";
1208  default: return "Invalid state";
1209  }
1210 }
1211 
1212 
1213 /** Finds the first marks. This is the first step in finding marks.
1214  * @return mark list
1215  */
1216 MirrorCalibTool::MarkList
1217 MirrorCalibTool::premark(const StepResult& prev,
1218  const unsigned char* yuv_mask,
1219  StepResult& result,
1220  PolarAngle phi,
1221  const PixelPoint& center)
1222 {
1223  const ConvexPolygon empty_polygon;
1224  return premark(empty_polygon, prev, yuv_mask, result, phi, center);
1225 }
1226 
1227 
1228 /** Finds the first marks. This is the first step in finding marks.
1229  * @return mark list
1230  */
1231 MirrorCalibTool::MarkList
1232 MirrorCalibTool::premark(const ConvexPolygon& polygon,
1233  const StepResult& prev,
1234  const unsigned char* yuv_mask,
1235  StepResult& result,
1236  PolarAngle phi,
1237  const PixelPoint& center)
1238 {
1239  const CartesianImage prev_img(prev, phi, center, yuv_mask);
1240  CartesianImage res_img(result, phi, center, yuv_mask);
1241  int width = MIN_WIDTH_OF_BIGGEST_LINE;
1242  MarkList premarks;
1243  for (PolarRadius length = 0; length < prev_img.max_radius(); length++)
1244  {
1245  const CartesianPoint p(0.0, length);
1246  if (polygon.contains(prev_img, p) && prev_img.is_line(p, width)) {
1247  premarks.push_back(length);
1248  res_img.highlight_line(p, width);
1249  }
1250  if (length % 25 == 0) {
1251  width *= (1.0f - APPROX_LINE_WIDTH_LOSS);
1252  }
1253  }
1254  return premarks;
1255 }
1256 
1257 
1258 /** Searches for holes between the pre-marks. Helper for mark().
1259  * @return holes
1260  */
1261 MirrorCalibTool::HoleList
1262 MirrorCalibTool::search_holes(const MarkList& premarks)
1263 {
1264  HoleList holes;
1265  PolarRadius prev_radius = -1;
1266  for (MarkList::const_iterator it = premarks.begin();
1267  it != premarks.end(); it++)
1268  {
1269  PolarRadius radius = *it;
1270  if (prev_radius != -1 && prev_radius + 1 < radius) {
1271  Hole hole(holes.size(), prev_radius, radius-1);
1272  holes.push_back(hole);
1273  }
1274  prev_radius = radius;
1275  }
1276  return holes;
1277 }
1278 
1279 
1280 /** Removes all but the n biggest holes (size depends on their position).
1281  * Helper for mark(). */
1282 MirrorCalibTool::HoleList
1283 MirrorCalibTool::filter_biggest_holes(const HoleList& holes,
1284  unsigned int n)
1285 {
1286 #ifdef FILTER_HOLES
1287  HoleList biggest = holes;
1288 #ifdef FILTER_MINI_HOLES
1289 restart: // XXX ugly :-)
1290  for (HoleList::iterator it = biggest.begin(); it != biggest.end(); it++)
1291  {
1292  if (it->size() <= FALSE_POSITIVE_HOLE_SIZE) {
1293  biggest.erase(it);
1294  goto restart;
1295  }
1296  }
1297 #endif
1298 
1299  HoleList filtered;
1300  for (unsigned int from = 0; from < biggest.size(); from++)
1301  {
1302  unsigned int to;
1303  for (to = from + 1; to < biggest.size(); to++) {
1304  if ((to - from + 1) > n) {
1305  to = from + n;
1306  break;
1307  }
1308  if (biggest[to - 1].size() < biggest[to].size()) {
1309  break;
1310  }
1311  }
1312  to--; // in all three break cases, to must be decremented
1313  if (to - from + 1 > filtered.size()) {
1314  filtered.clear();
1315  for (unsigned int j = from; j <= to; j++) {
1316  filtered.push_back(biggest[j]);
1317  }
1318  }
1319  }
1320  return filtered;
1321 #else
1322  HoleList biggest;
1323  for (HoleList::const_iterator it = holes.begin(); it != holes.end(); ++it)
1324  {
1325  const Hole& hole = *it;
1326 #ifdef FILTER_MINI_HOLES
1327  if (hole.size() < FALSE_POSITIVE_HOLE_SIZE) {
1328  // very small holes are usually false-positives
1329  continue;
1330  }
1331 #endif
1332  if (biggest.size() == 1 && hole.size() > biggest.front().size()) {
1333  // often the first determined hole is a part of the robot
1334  //biggest.erase(biggest.begin());
1335  }
1336  biggest.push_back(hole);
1337  if (biggest.size() == n) {
1338  break;
1339  }
1340  }
1341  return biggest;
1342 #endif
1343 }
1344 
1345 
1346 /** Calculates the position of the marks between holes. Helper for mark(). */
1347 MirrorCalibTool::MarkList
1348 MirrorCalibTool::determine_marks(const HoleList& holes)
1349 {
1350  HoleList biggest = filter_biggest_holes(holes, MARK_COUNT - 1);
1351  std::cout << "Filtered Holes: " << biggest.size() << std::endl;
1352  MarkList marks;
1353  for (HoleList::const_iterator prev, iter = biggest.begin();
1354  iter != biggest.end(); iter++) {
1355  const Hole& hole = *iter;
1356  if (iter == biggest.begin()) {
1357  const PolarRadius length = hole.from_length;
1358  marks.push_back(length);
1359  } else {
1360  const PolarRadius length = (hole.from_length + prev->to_length) / 2;
1361  marks.push_back(length);
1362  }
1363  if (iter+1 == biggest.end()) {
1364  const PolarRadius length = hole.to_length;
1365  marks.push_back(length);
1366  }
1367  prev = iter;
1368  }
1369  return marks;
1370 }
1371 
1372 
1373 /** Sets marks between all holes. The last step of marking. */
1374 MirrorCalibTool::MarkList
1375 MirrorCalibTool::mark(const MarkList& premarks,
1376  const unsigned char* yuv_mask,
1377  StepResult& result,
1378  PolarAngle phi,
1379  const PixelPoint& center)
1380 {
1381  std::cout << "Premarked places: " << premarks.size() << std::endl;
1382  HoleList holes = search_holes(premarks);
1383  std::cout << "Found Holes: " << holes.size() << std::endl;
1384  MarkList marks = determine_marks(holes);
1385  std::cout << "Found Marks: " << marks.size() << std::endl;
1386 
1387  CartesianImage res_img(result, phi, center, yuv_mask);
1388  for (MarkList::const_iterator iter = marks.begin();
1389  iter != marks.end(); iter++)
1390  {
1391  const PolarAngle angle = 0.0;
1392  const PolarRadius radius = *iter;
1393  res_img.highlight_point(CartesianPoint(angle, radius));
1394  std::cout << "Highlighting Mark at " << *iter << std::endl;
1395  }
1396  return marks;
1397 }
1398 
1399 
1400 void
1401 MirrorCalibTool::goto_next_state()
1402 {
1403  const Image& src_img = source_images_[state_.image_index];
1404  switch (state_.step) {
1405  case SHARPENING:
1406  state_.step = EDGE_DETECTION;
1407  break;
1408  case EDGE_DETECTION:
1409  if (src_img.results().size() <= ORIENTATION_COUNT) {
1410  state_.step = EDGE_DETECTION;
1411  } else {
1412  state_.step = COMBINATION;
1413  }
1414  break;
1415  case COMBINATION:
1416  if (src_img.results().size() < 2 * ORIENTATION_COUNT) {
1417  state_.step = COMBINATION;
1418  } else if (state_.image_index + 1 < source_images_.size()) {
1419  state_.step = SHARPENING;
1420  state_.image_index++;
1421  } else {
1422  state_.step = PRE_MARKING;
1423  state_.centering_done = false;
1424  state_.image_index = 0;
1425  }
1426  break;
1427  case CENTERING:
1428  state_.step = PRE_MARKING;
1429  state_.centering_done = true;
1430  state_.image_index = 0;
1431  break;
1432  case PRE_MARKING:
1433  state_.step = FINAL_MARKING;
1434  break;
1435  case FINAL_MARKING:
1436  if (state_.image_index + 1 < source_images_.size()) {
1437  state_.step = PRE_MARKING;
1438  state_.image_index++;
1439 #ifdef RECALCULATE_CENTER_POINT
1440  } else if (!state_.centering_done) {
1441  state_.step = CENTERING;
1442  state_.image_index = 0;
1443 #endif
1444  } else {
1445  state_.step = DONE;
1446  state_.image_index = 0;
1447  }
1448  break;
1449  case DONE:
1450  state_.step = DONE;
1451  state_.image_index = (state_.image_index + 1) % source_images_.size();
1452  break;
1453  default:
1454  throw fawkes::Exception("Invalid Calibration State");
1455  }
1456  printf("Next step: %s\n", get_state_description());
1457 }
1458 
1459 
1460 /** Performs one step in the calibration process.
1461  * In each step, a filter is applied to the image. The resulting image of
1462  * the n-th step is stored in the stepResults-vector on the (n-1)-th position.
1463  */
1464 void
1466 {
1467  printf("Performing step for image %u / %u, %s\n",
1468  (unsigned int)state_.image_index + 1,
1469  (unsigned int)source_images_.size(),
1471  if (state_.image_index >= source_images_.size()) {
1472  return;
1473  }
1474  Image& src_img = source_images_[state_.image_index];
1475  StepResult result(src_img.buflen(), src_img.width(), src_img.height());
1476  switch (state_.step) {
1477  case SHARPENING:
1478  {
1479  apply_sharpen(src_img.yuv_buffer(), result.yuv_buffer(),
1480  src_img.width(), src_img.height());
1481  make_grayscale(result.yuv_buffer(), result.buflen());
1482  }
1483  src_img.add_result(result);
1484  set_last_yuv_buffer(result.yuv_buffer());
1485  goto_next_state();
1486  return;
1487  case EDGE_DETECTION:
1488  {
1489  StepResult& prev = src_img.result(0);
1490  const int offset = (src_img.results().size() - 1) % ORIENTATION_COUNT;
1491  const orientation_t ori = static_cast<orientation_t>(ORI_DEG_0+offset);
1492  apply_sobel(prev.yuv_buffer(), result.yuv_buffer(),
1493  prev.width(), prev.height(), ori);
1494  make_grayscale(result.yuv_buffer(), result.buflen());
1495  make_contrast(result.yuv_buffer(), result.buflen());
1496  }
1497  printf("Edge detection in %u\n", (unsigned) src_img.results().size());
1498  src_img.add_result(result);
1499  set_last_yuv_buffer(result.yuv_buffer());
1500  assert(!memcmp(result.yuv_buffer(),
1502  result.buflen()));
1503  goto_next_state();
1504  return;
1505  case COMBINATION:
1506  {
1507  const int index1 = src_img.results().size() - ORIENTATION_COUNT == 1
1508  ? src_img.results().size() - ORIENTATION_COUNT
1509  : src_img.results().size() - 1;
1510  const int index2 = src_img.results().size() - ORIENTATION_COUNT + 1;
1511  assert(index1 != index2);
1512  printf("ORing: %d = or(%d, %d)\n", (unsigned) src_img.results().size(),
1513  index1, index2);
1514  StepResult& prev1 = src_img.result(index1);
1515  StepResult& prev2 = src_img.result(index2);
1516  assert(&prev1 != &prev2);
1517  assert(prev1.width() == prev2.width());
1518  assert(prev1.height() == prev2.height());
1519  apply_or(prev1.yuv_buffer(), prev2.yuv_buffer(), result.yuv_buffer(),
1520  prev1.width(), prev1.height());
1521  make_grayscale(result.yuv_buffer(), result.buflen());
1522  /*assert(memcmp(prev1.yuv_buffer(),
1523  prev2.yuv_buffer(),
1524  result.buflen()));
1525  assert(memcmp(result.yuv_buffer(),
1526  prev1.yuv_buffer(),
1527  result.buflen()));
1528  assert(memcmp(result.yuv_buffer(),
1529  prev1.yuv_buffer(),
1530  result.buflen()));*/
1531  }
1532  src_img.add_result(result);
1533  set_last_yuv_buffer(result.yuv_buffer());
1534  assert(!memcmp(result.yuv_buffer(),
1536  result.buflen()));
1537  goto_next_state();
1538  return;
1539  case CENTERING:
1540  {
1541  const StepResult& orig = src_img.result(0);
1542  memcpy(result.yuv_buffer(), orig.yuv_buffer(), result.buflen());
1543  const PixelPoint center = calculate_center(source_images_);
1544  img_center_x_ = center.x;
1545  img_center_y_ = center.y;
1546  std::cout << "Found center (" << center.x << ", "<< center.y << ")"
1547  << std::endl;
1548  CartesianImage img(result, relativeOrientationToImageRotation(0.0),
1549  center);
1550  img.highlight_point(CartesianPoint(0, 0));
1551  }
1552  src_img.add_result(result);
1553  set_last_yuv_buffer(result.yuv_buffer());
1554  goto_next_state();
1555  return;
1556  case PRE_MARKING:
1557  {
1558  const StepResult& prev = src_img.result(2 * ORIENTATION_COUNT - 1);
1559  memcpy(result.yuv_buffer(), prev.yuv_buffer(), result.buflen());
1560  const unsigned char* mask = img_yuv_mask_;
1561  const PixelPoint center(img_center_x_, img_center_y_);
1562  MarkList premarks = premark(prev, mask, result, src_img.ori(), center);
1563  src_img.set_premarks(premarks);
1564  apply_sharpen(src_img.yuv_buffer(), result.yuv_buffer(),
1565  src_img.width(), src_img.height());
1566  }
1567  src_img.add_result(result);
1568  set_last_yuv_buffer(result.yuv_buffer());
1569  goto_next_state();
1570  return;
1571  case FINAL_MARKING:
1572  {
1573  const StepResult& orig = src_img.result(0);
1574  memcpy(result.yuv_buffer(), orig.yuv_buffer(), result.buflen());
1575  const unsigned char* mask = img_yuv_mask_;
1576  const PixelPoint center(img_center_x_, img_center_y_);
1577  const MarkList marks = mark(src_img.premarks(), mask, result,
1578  src_img.ori(), center);
1579  src_img.set_marks(marks);
1580  const PolarAngle ori = src_img.ori();
1581  std::cout << "Marking done for orientation "
1582  << rad2deg(ori)
1583  << " = "
1584  << rad2deg(imageRotationToRelativeOrientation(ori))
1585  << std::endl;
1586  }
1587  src_img.add_result(result);
1588  set_last_yuv_buffer(result.yuv_buffer());
1589  goto_next_state();
1590  return;
1591  case DONE:
1592  {
1593  for (ImageList::iterator it = source_images_.begin();
1594  it != source_images_.end(); it++)
1595  {
1596  // cleanup: except the first result (whose yuv_buffer is needed)
1597  // no results are needed any more
1598  StepResultList results = it->results();
1599  results.erase(results.begin() + 1, results.end());
1600 
1601  const Image& src_img = *it;
1602  const PolarAngle ori = src_img.ori();
1603  MarkList marks = src_img.marks();
1604  mark_map_[imageRotationToRelativeOrientation(ori)] = marks;
1605  }
1606 
1607  const StepResult& prev = src_img.result(0);
1608  const PixelPoint center(img_center_x_, img_center_y_);
1609  CartesianImage img(result, relativeOrientationToImageRotation(0.0),
1610  center);
1611  memcpy(result.yuv_buffer(), prev.yuv_buffer(), result.buflen());
1612  img.highlight_pixel(center);
1613  img.highlight_point(CartesianPoint(0, 0)); // should be same as center
1614  for (MarkMap::const_iterator map_iter = mark_map_.begin();
1615  map_iter != mark_map_.end(); map_iter++)
1616  {
1617  const PolarAngle phi = map_iter->first;
1618  const MarkList& list = map_iter->second;
1619  printf("%3.f: ", rad2deg(phi));
1620  for (MarkList::const_iterator list_iter = list.begin();
1621  list_iter != list.end(); list_iter++)
1622  {
1623  const PolarAngle angle = phi;
1624  const PolarRadius radius = *list_iter;
1625  img.highlight_point(CartesianPoint(angle, radius));
1626  PixelPoint pp = img.to_pixel(CartesianPoint(angle, radius));
1627  printf("%3d (%d, %d)", radius, pp.x, pp.y);
1628  }
1629  printf("\n");
1630  }
1631  }
1632  src_img.add_result(result);
1633  set_last_yuv_buffer(result.yuv_buffer());
1634  goto_next_state();
1635  return;
1636  }
1637 }
1638 
1639 
1640 /**
1641  * Returns the center point determined by the mean of the first marks found in
1642  * the different directions.
1643  * The angle of the returned cartesian point is relative to the robots
1644  * orientation (and thus must be rotated by 90 degrees counter-clock-wise
1645  * to be displayed correctly).
1646  * @return the estimated center point relative to the robots orientation
1647  */
1649 MirrorCalibTool::calculate_center(const ImageList& images)
1650 {
1651  int x = 0;
1652  int y = 0;
1653  for (ImageList::const_iterator it = images.begin();
1654  it != images.end(); it++)
1655  {
1656  const Image& image = *it;
1657  const PolarRadius radius = image.marks().at(0);
1658  const unsigned char* null_buf = 0;
1659  const CartesianImage cart_image(null_buf, image.width(), image.height(),
1660  image.ori());
1661  const CartesianPoint point(0.0, radius);
1662  const PixelPoint pixel = cart_image.to_pixel(point);
1663  x += pixel.x;
1664  y += pixel.y;
1665  }
1666  return PixelPoint(x / images.size(), y / images.size());
1667 
1668  /*
1669  return PixelPoint(500, 500);
1670  int x = 0;
1671  int y = 0;
1672  int count = 0;
1673  for (MarkMap::const_iterator it = mark_map.begin();
1674  it != mark_map.end(); it++) {
1675  PolarAngle angle = it->first;
1676  PolarRadius dist = *(it->second.begin());
1677  CartesianPoint p(angle, dist);
1678  x += p.x;
1679  y += p.y;
1680  ++count;
1681  }
1682  CartesianPoint center(x/count, y/count);
1683  return center;
1684  */
1685 }
1686 
1687 
1688 /**
1689  * Searches the two angles in the MarkMap mark_map that are nearest to angle.
1690  * @param angle The reference angle to which the nearest marks are searched.
1691  * @param mark_map The mark map.
1692  * @return The two angles nearest to angle contained in mark_map (and therefore
1693  * keys to MarkLists in mark_map).
1694  */
1695 MirrorCalibTool::PolarAnglePair
1696 MirrorCalibTool::find_nearest_neighbors(PolarAngle angle,
1697  const MarkMap& mark_map)
1698 {
1699  typedef std::vector<PolarAngle> AngleList;
1700  AngleList diffs;
1701  for (MarkMap::const_iterator it = mark_map.begin();
1702  it != mark_map.end(); it++)
1703  {
1704  const PolarAngle mark_angle = it->first;
1705  const PolarAngle diff = normalize_mirror_rad(mark_angle - angle);
1706  diffs.push_back(diff);
1707 #if 0
1708  std::cout << "Finding nearest neighbors: "
1709  << "ref="<<angle<<"="<<rad2deg(angle)<<" "
1710  << "to="<<mark_angle<<"="<<rad2deg(mark_angle)<<" "
1711  << "diff="<<diff<<"="<<rad2deg(diff) << std::endl;
1712 #endif
1713  }
1714  bool min1_init = false;
1715  AngleList::size_type min1_index = 0;
1716  bool min2_init = false;
1717  AngleList::size_type min2_index = 0;
1718  for (int i = 0; static_cast<AngleList::size_type>(i) < diffs.size(); i++) {
1719  if (!min1_init || fabs(diffs[min1_index]) >= fabs(diffs[i])) {
1720  min2_index = min1_index;
1721  min2_init = min1_init;
1722  min1_index = i;
1723  min1_init = true;
1724  } else if (!min2_init || fabs(diffs[min2_index]) >= fabs(diffs[i])) {
1725  min2_index = i;
1726  min2_init = true;
1727  }
1728  }
1729  if (!min1_init) {
1730  throw fawkes::Exception("First minimum diff. angle not found");
1731  }
1732  if (!min2_init) {
1733  throw fawkes::Exception("Second minimum diff. angle not found");
1734  }
1735  PolarAngle min1 = 0.0;
1736  PolarAngle min2 = 0.0;
1737  AngleList::size_type i = 0;
1738  for (MarkMap::const_iterator it = mark_map.begin();
1739  it != mark_map.end(); it++)
1740  {
1741  if (i == min1_index) {
1742  min1 = it->first;
1743  } else if (i == min2_index) {
1744  min2 = it->first;
1745  }
1746  i++;
1747  }
1748 #if 0
1749  std::cout << "Found nearest neighbor 1: "
1750  << "ref="<<angle<<"="<<rad2deg(angle)<<" "
1751  << "to="<<min1<<"="<<rad2deg(min1) <<" "
1752  << "index="<<min1_index << std::endl;
1753  std::cout << "Found nearest neighbor 2: "
1754  << "ref="<<angle<<"="<<rad2deg(angle)<<" "
1755  << "to="<<min2<<"="<<rad2deg(min2) <<" "
1756  << "index="<<min2_index << std::endl;
1757 #endif
1758  return PolarAnglePair(min1, min2);
1759 }
1760 
1761 
1762 /**
1763  * Calculates the real distance to the n-th mark.
1764  * @param n The index of the mark, starting at 0.
1765  * @return The distance in centimeters.
1766  */
1767 MirrorCalibTool::RealDistance
1768 MirrorCalibTool::calculate_real_distance(int n)
1769 {
1770  return static_cast<int>(CARPET_DISTANCE_FROM_ROBOT_CENTER +
1771  static_cast<float>(n + 1) * MARK_DISTANCE);
1772 }
1773 
1774 
1775 MirrorCalibTool::RealDistance
1776 MirrorCalibTool::interpolate(PolarRadius radius,
1777  const MarkList& marks)
1778 {
1779  if (marks.size() < 2) {
1780  throw fawkes::Exception("Not enough marks for interpolation");
1781  }
1782  for (MarkList::size_type i = 0; i < marks.size(); i++)
1783  {
1784  // linear interpolation, x0, x1 = 'Stuetzstellen', f0, f1 = 'Stuetzwerte'
1785  if (i == 0 && radius < marks[i]) {
1786  const PolarRadius x0 = 0;
1787  const PolarRadius x1 = marks[i];
1788  const PolarRadius x = radius;
1789  const RealDistance f0 = 0;
1790  const RealDistance f1 = calculate_real_distance(i);
1791 #if 0
1792  std::cout << "A_Interpolate " << x << " between "
1793  << x0 << "->" << f0 << " "
1794  << x1 << "->" << f1 << std::endl;
1795 #endif
1796  return static_cast<RealDistance>(static_cast<double>(f0) +
1797  static_cast<double>(x - x0) *
1798  static_cast<double>(f1 - f0) /
1799  static_cast<double>(x1 - x0));
1800  } else if (i > 0 && marks[i-1] <= radius &&
1801  (radius < marks[i] || i+1 == marks.size())) {
1802  const PolarRadius x0 = marks[i-1];
1803  const PolarRadius x1 = marks[i];
1804  const PolarRadius x = radius;
1805  const RealDistance f0 = calculate_real_distance(i-1);
1806  const RealDistance f1 = calculate_real_distance(i);
1807 #if 0
1808  std::cout << "B_Interpolate " << x << " between "
1809  << x0 << "->" << f0 << " "
1810  << x1 << "->" << f1 << std::endl;
1811 #endif
1812  return static_cast<RealDistance>(static_cast<double>(f0) +
1813  static_cast<double>(x - x0) *
1814  static_cast<double>(f1 - f0) /
1815  static_cast<double>(x1 - x0));
1816  }
1817  }
1818  throw fawkes::Exception("Interpolation Error");
1819 }
1820 
1821 
1822 /**
1823  * Generates a new bulb based on the given data.
1824  * @param width The width of the image.
1825  * @param height The height of the image.
1826  * @param center The center pixel point of the image.
1827  * @param mark_map The map of marks where the key is the orientation of the
1828  * marks and the value is a sequence of distances to the marks.
1829  */
1830 Bulb
1831 MirrorCalibTool::generate(int width,
1832  int height,
1833  const PixelPoint& center,
1834  const MarkMap& mark_map)
1835 {
1836  const unsigned char* null_img_buf = 0;
1837  CartesianImage img(null_img_buf, width, height,
1838  relativeOrientationToImageRotation(0.0), center);
1839  Bulb bulb(width, height);
1840  bulb.setCenter(center.x, center.y);
1841  bulb.setOrientation(0.0);
1842  for (int x = 0; x < width; x++)
1843  {
1844  for (int y = 0; y < height; y++)
1845  {
1846  const PixelPoint pp(x, y);
1847  const CartesianPoint cp = img.to_cartesian(pp);
1848  const PolarAngle ori_to_robot = cp.atan();
1849  const PolarAnglePair nearest_neighbors =
1850  find_nearest_neighbors(ori_to_robot, mark_map);
1851  const PolarAngle diff1 =
1852  fabs(normalize_mirror_rad(nearest_neighbors.first - ori_to_robot));
1853  const PolarAngle diff2 =
1854  fabs(normalize_mirror_rad(nearest_neighbors.second - ori_to_robot));
1855  const PolarAngle norm = diff1 + diff2;
1856  assert(norm != 0.0);
1857  const double weight1 = 1.0 - diff1 / norm;
1858  const double weight2 = 1.0 - diff2 / norm;
1859 #if 0
1860  std::cout << "PixelPoint("<< x <<", "<< y <<")"<< std::endl;
1861  std::cout << "CartesianPoint("<< cp.x <<", "<< cp.y <<")"<< std::endl;
1862  std::cout << "Radius, Angle: " << cp.length() << " "
1863  << ori_to_robot << " = "
1864  << rad2deg(ori_to_robot) << std::endl;
1865  std::cout << "Neighbor 1: "
1866  << normalize_rad(nearest_neighbors.first) << " = "
1867  << rad2deg(normalize_rad(nearest_neighbors.first))
1868  << std::endl;
1869  std::cout << "Neighbor 2: "
1870  << normalize_rad(nearest_neighbors.second) << " = "
1871  << rad2deg(normalize_rad(nearest_neighbors.second))
1872  << std::endl;
1873  std::cout << "Diff 1: " << diff1 << " = " << rad2deg(diff1) << std::endl;
1874  std::cout << "Diff 2: " << diff2 << " = " << rad2deg(diff2) << std::endl;
1875  std::cout << "Norm Factor: " << norm << " = " << rad2deg(norm)
1876  << std::endl;
1877  std::cout << "Weight 1: " << weight1 << " = " << rad2deg(weight1)
1878  << std::endl;
1879  std::cout << "Weight 2: " << weight2 << " = " << rad2deg(weight2)
1880  << std::endl;
1881 #endif
1882  assert(0.0 <= weight1 && weight1 <= 1.0);
1883  assert(0.0 <= weight2 && weight2 <= 1.0);
1884  assert(1.0 - 10e-5 < weight1 + weight2 && weight1 + weight2 < 1.0 + 10e-5);
1885  const MarkList& marks1 = mark_map.at(nearest_neighbors.first);
1886  const MarkList& marks2 = mark_map.at(nearest_neighbors.second);
1887  const RealDistance dist1 = interpolate(cp.length(), marks1);
1888  const RealDistance dist2 = interpolate(cp.length(), marks2);
1889 #if 0
1890  std::cout << "Real 1 " << dist1 << std::endl;
1891  std::cout << "Real 2 " << dist2 << std::endl;
1892 #endif
1893  const RealDistance weighted_mean_dist = dist1 * weight1 + dist2 * weight2;
1894  const float world_dist_in_meters = weighted_mean_dist / 100.0f;
1895  const float world_phi_rel_to_robot = -1.0f * ori_to_robot;
1896  // world_phi_rel_to_robot must be multiplied with -1 because the image is
1897  // mirrored: what's on the right in the image, is on the left of the robot
1898  // in reality
1899 #if 0
1900  std::cout << "Dist 1: " << dist1 << std::endl;
1901  std::cout << "Dist 2: " << dist2 << std::endl;
1902  std::cout << "World Dist: " << world_dist_in_meters << std::endl;
1903  std::cout << "World Phi: " << ori_to_robot << " = "
1904  << rad2deg(ori_to_robot) << std::endl;
1905 #endif
1906  if (world_dist_in_meters > 0) {
1907  bulb.setWorldPoint(x, y, world_dist_in_meters, world_phi_rel_to_robot);
1908  }
1909  }
1910  }
1911  return bulb;
1912 }
1913 
1914 
1915 void
1916 MirrorCalibTool::set_last_yuv_buffer(const unsigned char* last_buf)
1917 {
1918  last_yuv_buffer_ = last_buf;
1919 }
1920 
1921 
1922 /** Get last created YUV buffer.
1923  * This is the result of the most recent step. Memory management is
1924  * done by MirrorCalibTool.
1925  * @return most recent result's YUV buffer
1926  */
1927 const unsigned char*
1929 {
1930  return last_yuv_buffer_;
1931 }
1932 
1933 
1934 /** Get the assumed distance and orientation of a pixel point.
1935  * @param x The X coordinate of the pixel that is to be evaluated.
1936  * @param y The Y coordinate of the pixel that is to be evaluated.
1937  * @param dist_ret The distance in the real world to the pixel.
1938  * @param ori_ret The orientation in the real world to the pixel.
1939  */
1940 void
1941 MirrorCalibTool::eval(unsigned int x, unsigned int y,
1942  float* dist_ret, float* ori_ret)
1943 {
1944  if (!bulb_) {
1945  printf("No bulb loaded, cannot evaluate.\n");
1946  return;
1947  }
1948  polar_coord_2d_t coord;
1949  coord = bulb_->getWorldPointRelative(x, y);
1950 
1951  *dist_ret = coord.r;
1952  *ori_ret = coord.phi;
1953 }
1954 
1955 
1956 /** Loads a calibration file.
1957  * @param filename Name of the file containing the calibration data.
1958  */
1959 void
1960 MirrorCalibTool::load(const char* filename)
1961 {
1962  bulb_ = new Bulb(filename);
1963 }
1964 
1965 
1966 /** Saves calibration data to a file.
1967  * @param filename The name of the file.
1968  */
1969 void
1970 MirrorCalibTool::save(const char* filename)
1971 {
1972  if (state_.step == DONE) {
1973  const Image& src_img = source_images_[state_.image_index];
1974  const PixelPoint center(img_center_x_, img_center_y_);
1975  Bulb bulb = generate(src_img.width(), src_img.height(), center, mark_map_);
1976  bulb.save(filename);
1977  } else {
1978  std::cout << "Can't save in the middle of the calibration" << std::endl;
1979  }
1980 }
1981 
1982 
1983 /** Draws a line from the image center in the given angle.
1984  * @param yuv_buffer The in/out parameter for the image.
1985  * @param angle_deg Angle of line in degrees.
1986  * @param center_x X-coordinate of center point in image pixels.
1987  * @param center_y Y-coordinate of center point in image pixels.
1988  * @param width Width of image.
1989  * @param height Height of image.
1990  */
1991 void
1992 MirrorCalibTool::draw_line(unsigned char* yuv_buffer,
1993  double angle_deg,
1994  int center_x,
1995  int center_y,
1996  int width,
1997  int height)
1998 {
1999  const PolarAngle angle = normalize_rad(deg2rad(angle_deg));
2000  CartesianImage img(yuv_buffer, width, height,
2001  relativeOrientationToImageRotation(0.0),
2002  PixelPoint(center_x, center_y));
2003  for (PolarRadius length = 0; length < img.max_radius(); length++)
2004  {
2005  const CartesianPoint p(angle, length);
2006  if (img.contains(p)) {
2007  img.set_color(p, MARK_LUMA, MARK_CHROMINANCE);
2008  }
2009  }
2010 }
2011 
2012 
2013 /** Draws a crosshair with the lines in the directions of the keys of
2014  * the mark map.
2015  * @param yuv_buffer The in/out parameter for the image.
2016  */
2017 void
2018 MirrorCalibTool::draw_mark_lines(unsigned char* yuv_buffer)
2019 {
2020  for (ImageList::const_iterator it = source_images_.begin();
2021  it != source_images_.end(); it++)
2022  {
2023  const Image& src_img = *it;
2024  CartesianImage img(yuv_buffer, src_img.width(), src_img.height(),
2025  src_img.ori(), PixelPoint(img_center_x_, img_center_y_));
2026  for (PolarRadius length = 0; length < img.max_radius(); length++)
2027  {
2028  const PolarAngle angle = 0.0;
2029  const CartesianPoint p(angle, length);
2030  if (img.contains(p)) {
2031  img.set_color(p, MARK_LUMA, MARK_CHROMINANCE);
2032  }
2033  }
2034  }
2035 }
2036 
2037 
2038 /** Draws a crosshair in the YUV-buffer. The crosshair consists of three lines
2039  * from the origin (0, 0) with the angles 0 degree, 120 degrees, 240 degrees.
2040  * @param yuv_buffer The in/out parameter for the image.
2041  * @param center_x X-coordinate of center point in image pixels.
2042  * @param center_y Y-coordinate of center point in image pixels.
2043  * @param width Width of image.
2044  * @param height Height of image.
2045  */
2046 void
2047 MirrorCalibTool::draw_crosshair(unsigned char* yuv_buffer,
2048  int center_x,
2049  int center_y,
2050  int width,
2051  int height)
2052 {
2053  const PolarAngle POSITIONS[] = { normalize_rad(deg2rad( 0.0f)),
2054  normalize_rad(deg2rad(-120.0f)),
2055  normalize_rad(deg2rad( 120.0f)) };
2056  const int POSITION_COUNT = sizeof POSITIONS / sizeof(double);
2057  CartesianImage img(yuv_buffer, width, height,
2058  relativeOrientationToImageRotation(0.0),
2059  PixelPoint(center_x, center_y));
2060  for (int i = 0; i < POSITION_COUNT; i++)
2061  {
2062  const PolarAngle angle = POSITIONS[i];
2063  for (PolarRadius length = 0; length < img.max_radius(); length++)
2064  {
2065  const CartesianPoint p(angle, length);
2066  if (img.contains(p)) {
2067  img.set_color(p, MARK_LUMA, MARK_CHROMINANCE);
2068  }
2069  }
2070  }
2071 }
2072 
2073 }
2074 
PixelPoint to_pixel(const CartesianPoint &p) const
Converts a cartesian point to a pixel point.
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_x() const
Center X accessor.
Definition: mirror_calib.h:76
unsigned char V
V component.
Definition: yuv.h:62
PolarAngle ori() const
Angle of marks wrt X axis.
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 char * get_state_description() const
Get description of next step.
PNM file reader.
Definition: pnm.h:36
const unsigned char * get_last_yuv_buffer() const
Get last created YUV buffer.
int height() const
Height of YUV buffer.
int width() const
Width of YUV buffer.
virtual void apply()
Apply the filter.
Definition: or.cpp:58
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.
float normalize_rad(float angle_rad)
Normalize angle in radian between 0 (inclusive) and 2*PI (exclusive).
Definition: angle.h:93
const unsigned char * buf() const
Buffer.
Fawkes library namespace.
CartesianPoint rotate(PolarAngle rotate_phi) const
Rotates the vector to the point counter-clockwise and returns the vector to the point.
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.
CartesianImage(const unsigned char *buf, int width, int height, PolarAngle phi, const PixelPoint &center)
Constructor.
const PolarAngle phi() const
Phi angle.
PolarRadius from_length
Beginning of the hole.
A hole is a sequence of pixels between two lines.
bool contains(const CartesianPoint &p) const
Indicates whether the image contains a cartesian point.
bool contains(const PixelPoint &p) const
Indicates whether the image contains a pixel point.
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.
int max_x() const
Get Maximum cartesian X coordinate of the image.
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.
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 draw_mark_lines(unsigned char *yuv_buffer)
Draws a crosshair with the lines in the directions of the keys of the mark map.
virtual void apply()
Apply the filter.
Definition: sobel.cpp:122
int center_y() const
Center Y accessor.
Definition: mirror_calib.h:79
unsigned char Y
Y component.
Definition: yuv.h:60
virtual colorspace_t colorspace()
Get colorspace from the just read image.
Definition: pnm.cpp:144
virtual void apply()
Apply the filter.
Definition: median.cpp:59
const unsigned char * yuv_buffer() const
YUV buffer.
PolarRadius length() const
Length of the vector the point.
Hole(int index, PolarRadius from_length, PolarRadius to_length)
Constructor.
void add_result(const StepResult &r)
Appends a result.
PolarAngle atan() const
Atan(y, x) of the point.
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:85
virtual void apply()
Apply the filter.
Definition: sharpen.cpp:56
static ROI * full_image(unsigned int width, unsigned int height)
Get full image ROI for given size.
Definition: roi.cpp:597
MarkList & marks()
The (final) marks.
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.
const unsigned char * yuv_buffer() const
YUV buffer.
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.
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 * mask() const
Mask.
const StepResultList & results() const
List of results.
virtual void apply()
Apply the filter.
Definition: min.cpp:50
PixelPoint(int x, int y)
Constructor.
PolarRadius max_radius() const
Maximum polar radius of the image.
unsigned char * yuv_buffer()
YUV buffer.
virtual unsigned int pixel_height()
Get height of read image in pixels.
Definition: pnm.cpp:156
CartesianImage(const unsigned char *buf, int width, int height, PolarAngle phi)
Constructor.
PolarRadius size() const
The size of the hole in pixels.
int height() const
YUV buffer&#39;s height.
void abort()
Aborts the calibration process.
CartesianPoint to_cartesian(const PixelPoint &p) const
Converts a pixel point to a cartesian point.
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.
CartesianImage(const StepResult &res, PolarAngle phi, const unsigned char *mask=0)
Constructor.
float bright_fraction(const CartesianPoint &from, const CartesianPoint &to) const
Get relative amount of bright pixels.
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.
CartesianImage(const StepResult &res, PolarAngle phi, PixelPoint center, const unsigned char *mask=0)
Constructor.
static bool contains(Point_map points, Point_2 point, std::string &name, float near_threshold)
Check if a point is already contained in a map.
Definition: voronoi.cpp:113
float r
distance
Definition: types.h:86
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.
const StepResult & result(int i) const
Returns the i-th result.
Minimum filter.
Definition: min.h:34
unsigned char U
U component.
Definition: yuv.h:61
void set_marks(const MarkList &marks)
The (final) marks.
const MarkList & marks() const
The (final) marks.
size_t buflen() const
YUV buffer&#39;s length.
PolarRadius to_length
Ending of the hole.
size_t buflen() const
YUV buffer&#39;s length.
void set_premarks(const MarkList &premarks)
The premarks.
YUV pixel.
Definition: yuv.h:59
float phi
angle
Definition: types.h:87
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
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.
void save(const char *filename)
Save LUT from file.
Definition: bulb.cpp:287
int width() const
YUV buffer&#39;s width.
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.
bool is_line(const CartesianPoint &p, int length) const
Indicates whether at pixel point p there is a highlighted line.
int max_y() const
Get Maximum cartesian Y coordinate of the image.
const MarkList & premarks()
The premarks.
const int height() const
Height of buffer.
virtual void set_buffer(unsigned char *yuv422planar_buffer)
Set buffer that the read image should be written to.
Definition: pnm.cpp:138
PixelPoint rotate(PolarAngle rotate_phi) const
Rotates the vector to the point counter-clockwise and returns the vector to the point.
virtual void set_dst_buffer(unsigned char *buf, ROI *roi)
Set the destination buffer.
Definition: filter.cpp:134
CartesianPoint(int x, int y)
Constructor.
const int width() const
Width of buffer.
Sobel filter.
Definition: sobel.h:37
unsigned char * yuv_buffer()
YUV buffer.