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