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
firevision::MirrorCalibTool::CartesianPoint::CartesianPoint
CartesianPoint(PolarAngle phi, PolarRadius length)
Constructor.
Definition: mirror_calib.cpp:278
firevision::MirrorCalibTool::abort
void abort()
Aborts the calibration process.
Definition: mirror_calib.cpp:1139
firevision::PNMReader
Definition: pnm.h:39
firevision::MirrorCalibTool::CartesianImage::CartesianImage
CartesianImage(const unsigned char *buf, int width, int height, PolarAngle phi)
Constructor.
Definition: mirror_calib.cpp:423
firevision::MirrorCalibTool::CartesianImage::highlight_point
void highlight_point(const CartesianPoint &p)
Highlights a point, i.e.
Definition: mirror_calib.cpp:726
firevision::MirrorCalibTool::Image::results
const StepResultList & results() const
List of results.
Definition: mirror_calib.cpp:944
firevision::MirrorCalibTool::PixelPoint
A pixel point is a 2d point with positive X and Y coords.
Definition: mirror_calib.cpp:307
firevision::Bulb::getWorldPointRelative
virtual fawkes::polar_coord_2d_t getWorldPointRelative(unsigned int image_x, unsigned int image_y) const
Definition: bulb.cpp:373
firevision::Filter::set_src_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
firevision::MirrorCalibTool::next_step
void next_step()
Performs one step in the calibration process.
Definition: mirror_calib.cpp:1504
firevision::MirrorCalibTool::StepResult::yuv_buffer
unsigned char * yuv_buffer()
YUV buffer.
Definition: mirror_calib.cpp:179
firevision::MirrorCalibTool::Hole::from_length
PolarRadius from_length
Beginning of the hole.
Definition: mirror_calib.cpp:789
fawkes::polar_coord_2d_t::r
float r
distance
Definition: types.h:97
firevision::MirrorCalibTool::draw_line
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.
Definition: mirror_calib.cpp:1971
firevision::FilterSobel
Definition: sobel.h:39
firevision::MirrorCalibTool::StepResult::StepResult
StepResult(size_t buflen, int width, int height)
Constructor.
Definition: mirror_calib.cpp:124
firevision::MirrorCalibTool::Image::add_result
void add_result(const StepResult &r)
Appends a result.
Definition: mirror_calib.cpp:976
firevision::MirrorCalibTool::Hole::Hole
Hole(int index, PolarRadius from_length, PolarRadius to_length)
Constructor.
Definition: mirror_calib.cpp:796
firevision::MirrorCalibTool::push_back
void push_back(const unsigned char *yuv_buffer, size_t buflen, int width, int height, double ori)
Store image for calibration process.
Definition: mirror_calib.cpp:1120
firevision::MirrorCalibTool::Image::set_marks
void set_marks(const MarkList &marks)
The (final) marks.
Definition: mirror_calib.cpp:1008
firevision::MirrorCalibTool::Hole::size
PolarRadius size() const
The size of the hole in pixels.
Definition: mirror_calib.cpp:805
firevision::MirrorCalibTool::CartesianImage::mask
const unsigned char * mask() const
Mask.
Definition: mirror_calib.cpp:460
fawkes::polar_coord_2d_t
Polar coordinates.
Definition: types.h:95
firevision::MirrorCalibTool::Image::marks
MarkList & marks()
The (final) marks.
Definition: mirror_calib.cpp:960
firevision::PNMReader::pixel_height
virtual unsigned int pixel_height()
Definition: pnm.cpp:142
firevision::MirrorCalibTool::CartesianImage::bright_fraction
float bright_fraction(const CartesianPoint &from, const CartesianPoint &to) const
Get relative amount of bright pixels.
Definition: mirror_calib.cpp:627
firevision::MirrorCalibTool::Hole::to_length
PolarRadius to_length
Ending of the hole.
Definition: mirror_calib.cpp:790
firevision::MirrorCalibTool::Point
Abstract Point class.
Definition: mirror_calib.cpp:218
firevision::MirrorCalibTool::Point::Point
Point(const Point &p)
Copy constructor.
Definition: mirror_calib.cpp:234
firevision::MirrorCalibTool::Point::operator=
Point & operator=(const Point &p)
Assignment operator.
Definition: mirror_calib.cpp:261
firevision::MirrorCalibTool::CartesianImage::CartesianImage
CartesianImage(unsigned char *buf, int width, int height, PolarAngle phi, const PixelPoint &center)
Constructor.
Definition: mirror_calib.cpp:409
firevision::MirrorCalibTool::Point::length
PolarRadius length() const
Length of the vector the point.
Definition: mirror_calib.cpp:242
firevision::ROI
Definition: roi.h:60
firevision::MirrorCalibTool::CartesianImage::CartesianImage
CartesianImage(unsigned char *buf, int width, int height, PolarAngle phi)
Constructor.
Definition: mirror_calib.cpp:438
firevision::MirrorCalibTool::Point::atan
PolarAngle atan() const
Atan(y, x) of the point.
Definition: mirror_calib.cpp:251
firevision::YUV_t_struct
YUV pixel.
Definition: yuv.h:64
fawkes::contains
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
firevision::MirrorCalibTool::Image::operator=
Image & operator=(const Image &copy)
Assignment.
Definition: mirror_calib.cpp:854
firevision::Bulb::save
void save(const char *filename)
Save LUT from file.
Definition: bulb.cpp:297
firevision::MirrorCalibTool::StepResult::StepResult
StepResult(const StepResult &copy)
Constructor.
Definition: mirror_calib.cpp:136
firevision::MirrorCalibTool::CartesianImage::highlight_line
void highlight_line(const CartesianPoint &p, int length)
Highlightes a line at pixel point p.
Definition: mirror_calib.cpp:672
firevision::PNMReader::colorspace
virtual colorspace_t colorspace()
Definition: pnm.cpp:130
firevision::MirrorCalibTool::CartesianImage::CartesianImage
CartesianImage(const StepResult &res, PolarAngle phi, const unsigned char *mask=0)
Constructor.
Definition: mirror_calib.cpp:371
firevision::MirrorCalibTool::Image::results
StepResultList & results()
List of results.
Definition: mirror_calib.cpp:936
firevision::MirrorCalibTool::Image::yuv_buffer
const unsigned char * yuv_buffer() const
YUV buffer.
Definition: mirror_calib.cpp:896
firevision::Bulb
Definition: bulb.h:42
firevision::MirrorCalibTool::Image::buflen
size_t buflen() const
YUV buffer's length.
Definition: mirror_calib.cpp:904
firevision::MirrorCalibTool::Image::set_premarks
void set_premarks(const MarkList &premarks)
The premarks.
Definition: mirror_calib.cpp:1001
firevision::MirrorCalibTool::Image::premarks
const MarkList & premarks()
The premarks.
Definition: mirror_calib.cpp:952
firevision::Filter::set_dst_buffer
virtual void set_dst_buffer(unsigned char *buf, ROI *roi)
Set the destination buffer.
Definition: filter.cpp:123
firevision::MirrorCalibTool::Point::x
int x
X coordinate.
Definition: mirror_calib.cpp:221
firevision::MirrorCalibTool::~MirrorCalibTool
~MirrorCalibTool()
Destructor.
Definition: mirror_calib.cpp:1037
firevision::MirrorCalibTool::Image::~Image
~Image()
Destructor.
Definition: mirror_calib.cpp:876
firevision::MirrorCalibTool::CartesianPoint
A cartesian point is a 2d point which can have negative X and Y coords.
Definition: mirror_calib.cpp:272
firevision::MirrorCalibTool::CartesianImage::center
const PixelPoint & center() const
Center of buffer.
Definition: mirror_calib.cpp:476
firevision::PNMReader::read
virtual void read()
Definition: pnm.cpp:148
firevision::MirrorCalibTool::StepResult::~StepResult
~StepResult()
Destructor.
Definition: mirror_calib.cpp:168
fawkes::rad2deg
float rad2deg(float rad)
Convert an angle given in radians to degrees.
Definition: angle.h:52
firevision::MirrorCalibTool::save
void save(const char *filename)
Saves calibration data to a file.
Definition: mirror_calib.cpp:1950
firevision::PNMReader::set_buffer
virtual void set_buffer(unsigned char *yuv422planar_buffer)
Definition: pnm.cpp:124
firevision::MirrorCalibTool::StepResult::operator=
StepResult & operator=(const StepResult &copy)
Assignment.
Definition: mirror_calib.cpp:150
firevision::MirrorCalibTool::PixelPoint::rotate
PixelPoint rotate(PolarAngle rotate_phi) const
Rotates the vector to the point counter-clockwise and returns the vector to the point.
Definition: mirror_calib.cpp:322
firevision::MirrorCalibTool::Point::y
int y
Y coordinate.
Definition: mirror_calib.cpp:222
firevision::YUV_t_struct::Y
unsigned char Y
Y component.
Definition: yuv.h:66
fawkes
firevision::MirrorCalibTool::StepResult::width
int width() const
Width of YUV buffer.
Definition: mirror_calib.cpp:203
firevision::MirrorCalibTool::Image::result
StepResult & result(int i)
Returns the i-th result.
Definition: mirror_calib.cpp:985
firevision::MirrorCalibTool::Image::Image
Image(const unsigned char *yuv_buffer, size_t buflen, int width, int height, PolarAngle ori)
Constructor.
Definition: mirror_calib.cpp:821
firevision::MirrorCalibTool::CartesianImage::height
int height() const
Height of buffer.
Definition: mirror_calib.cpp:492
firevision::MirrorCalibTool::Image::width
int width() const
YUV buffer's width.
Definition: mirror_calib.cpp:912
firevision::MirrorCalibTool::StepResult::buflen
size_t buflen() const
YUV buffer's length.
Definition: mirror_calib.cpp:195
firevision::MirrorCalibTool::StepResult
The result of a step contains a YUV buffer.
Definition: mirror_calib.cpp:110
firevision::MirrorCalibTool::Image
A container for a YUV-buffer etc.
Definition: mirror_calib.cpp:812
firevision::MirrorCalibTool::CartesianImage::contains
bool contains(const CartesianPoint &p) const
Indicates whether the image contains a cartesian point.
Definition: mirror_calib.cpp:542
firevision::MirrorCalibTool::draw_mark_lines
void draw_mark_lines(unsigned char *yuv_buffer)
Draws a crosshair with the lines in the directions of the keys of the mark map.
Definition: mirror_calib.cpp:1997
fawkes::deg2rad
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:42
firevision::MirrorCalibTool::PixelPoint::PixelPoint
PixelPoint(int x, int y)
Constructor.
Definition: mirror_calib.cpp:313
firevision::MirrorCalibTool::CartesianImage::to_pixel
PixelPoint to_pixel(const CartesianPoint &p) const
Converts a cartesian point to a pixel point.
Definition: mirror_calib.cpp:510
firevision::MirrorCalibTool::CartesianImage::get
unsigned char get(const CartesianPoint &p) const
Get luminance at a given point.
Definition: mirror_calib.cpp:552
firevision::MirrorCalibTool::Image::marks
const MarkList & marks() const
The (final) marks.
Definition: mirror_calib.cpp:968
firevision::MirrorCalibTool::StepResult::height
int height() const
Height of YUV buffer.
Definition: mirror_calib.cpp:211
firevision::MirrorCalibTool::CartesianImage::draw_line
void draw_line(const CartesianPoint &p, const CartesianPoint &q)
Draws a line from p to q.
Definition: mirror_calib.cpp:693
firevision::MirrorCalibTool::CartesianImage::max_y
int max_y() const
Get Maximum cartesian Y coordinate of the image.
Definition: mirror_calib.cpp:584
firevision::MirrorCalibTool::Image::yuv_buffer
unsigned char * yuv_buffer()
YUV buffer.
Definition: mirror_calib.cpp:888
fawkes::normalize_rad
float normalize_rad(float angle_rad)
Normalize angle in radian between 0 (inclusive) and 2*PI (exclusive).
Definition: angle.h:96
firevision::MirrorCalibTool::CartesianImage::highlight_pixel
void highlight_pixel(const PixelPoint &p)
Highlights a pixel, i.e.
Definition: mirror_calib.cpp:709
firevision::MirrorCalibTool::load_mask
void load_mask(const char *mask_file_name)
Loads a PNM mask file for the robot's bars.
Definition: mirror_calib.cpp:1090
firevision::MirrorCalibTool::CartesianImage::buf
unsigned char * buf()
Get Buffer.
Definition: mirror_calib.cpp:452
firevision::MirrorCalibTool::get_last_yuv_buffer
const unsigned char * get_last_yuv_buffer() const
Get last created YUV buffer.
Definition: mirror_calib.cpp:1912
fawkes::normalize_mirror_rad
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
Definition: angle.h:78
firevision::MirrorCalibTool::CartesianPoint::CartesianPoint
CartesianPoint(int x, int y)
Constructor.
Definition: mirror_calib.cpp:285
firevision::MirrorCalibTool::CartesianPoint::rotate
CartesianPoint rotate(PolarAngle rotate_phi) const
Rotates the vector to the point counter-clockwise and returns the vector to the point.
Definition: mirror_calib.cpp:294
firevision::MirrorCalibTool::Image::Image
Image(const Image &copy)
Constructor.
Definition: mirror_calib.cpp:835
firevision::MirrorCalibTool::StepResult::yuv_buffer
const unsigned char * yuv_buffer() const
YUV buffer.
Definition: mirror_calib.cpp:187
firevision::MirrorCalibTool::center_y
int center_y() const
Center Y accessor.
Definition: mirror_calib.h:99
firevision::ROI::full_image
static ROI * full_image(unsigned int width, unsigned int height)
Get full image ROI for given size.
Definition: roi.cpp:565
firevision::MirrorCalibTool::CartesianImage::draw_line
void draw_line(const PixelPoint &p, const PixelPoint &q)
Draws a line from p to q.
Definition: mirror_calib.cpp:684
firevision::MirrorCalibTool::Image::result
const StepResult & result(int i) const
Returns the i-th result.
Definition: mirror_calib.cpp:994
firevision::MirrorCalibTool::CartesianImage::width
int width() const
Width of buffer.
Definition: mirror_calib.cpp:484
firevision::MirrorCalibTool::Hole::index
int index
The index of the hole.
Definition: mirror_calib.cpp:788
firevision::PNMReader::pixel_width
virtual unsigned int pixel_width()
Definition: pnm.cpp:136
firevision::MirrorCalibTool::CartesianImage::max_radius
PolarRadius max_radius() const
Maximum polar radius of the image.
Definition: mirror_calib.cpp:592
firevision::MirrorCalibTool::CartesianImage::CartesianImage
CartesianImage(const unsigned char *buf, int width, int height, PolarAngle phi, const PixelPoint &center)
Constructor.
Definition: mirror_calib.cpp:388
firevision::MirrorCalibTool::Image::height
int height() const
YUV buffer's height.
Definition: mirror_calib.cpp:920
firevision::MirrorCalibTool::CartesianImage::set_color
void set_color(const CartesianPoint &p, unsigned char luma, unsigned char chrominance)
Sets the luminance Y and chrominance U at a given cartesian point.
Definition: mirror_calib.cpp:616
firevision::MirrorCalibTool::load
void load(const char *filename)
Loads a calibration file.
Definition: mirror_calib.cpp:1941
firevision::FilterSobel::apply
virtual void apply()
Definition: sobel.cpp:166
firevision::MirrorCalibTool::get_state_description
const char * get_state_description() const
Get description of next step.
Definition: mirror_calib.cpp:1260
firevision::MirrorCalibTool::CartesianImage::set_color
void set_color(const PixelPoint &p, unsigned char luma, unsigned char chrominance)
Sets the luminance Y and chrominance U at a given pixel point.
Definition: mirror_calib.cpp:602
firevision::MirrorCalibTool::CartesianImage::buf
const unsigned char * buf() const
Buffer.
Definition: mirror_calib.cpp:468
firevision::MirrorCalibTool::CartesianImage::to_cartesian
CartesianPoint to_cartesian(const PixelPoint &p) const
Converts a pixel point to a cartesian point.
Definition: mirror_calib.cpp:521
firevision::MirrorCalibTool::draw_crosshair
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.
Definition: mirror_calib.cpp:2025
firevision::MirrorCalibTool::CartesianImage::is_line
bool is_line(const CartesianPoint &p, int length) const
Indicates whether at pixel point p there is a highlighted line.
Definition: mirror_calib.cpp:655
firevision::MirrorCalibTool::CartesianImage::contains
bool contains(const PixelPoint &p) const
Indicates whether the image contains a pixel point.
Definition: mirror_calib.cpp:532
fawkes::polar_coord_2d_t::phi
float phi
angle
Definition: types.h:98
firevision::MirrorCalibTool::CartesianImage::phi
PolarAngle phi() const
Phi angle.
Definition: mirror_calib.cpp:500
firevision::MirrorCalibTool::CartesianImage
Wraps an image so that access to (0, 0) is mapped to the middle of the image and so on.
Definition: mirror_calib.cpp:337
firevision::MirrorCalibTool::Image::ori
PolarAngle ori() const
Angle of marks wrt X axis.
Definition: mirror_calib.cpp:928
firevision::MirrorCalibTool::CartesianImage::max_x
int max_x() const
Get Maximum cartesian X coordinate of the image.
Definition: mirror_calib.cpp:576
firevision::MirrorCalibTool::eval
void eval(unsigned int x, unsigned int y, float *x_ret, float *y_ret)
Get the assumed distance and orientation of a pixel point.
Definition: mirror_calib.cpp:1924
firevision::MirrorCalibTool::CartesianImage::CartesianImage
CartesianImage(const StepResult &res, PolarAngle phi, PixelPoint center, const unsigned char *mask=0)
Constructor.
Definition: mirror_calib.cpp:354
firevision::MirrorCalibTool::center_x
int center_x() const
Center X accessor.
Definition: mirror_calib.h:92
firevision::YUV_t_struct::U
unsigned char U
U component.
Definition: yuv.h:67
firevision::MirrorCalibTool::Hole
A hole is a sequence of pixels between two lines.
Definition: mirror_calib.cpp:785
firevision::MirrorCalibTool::Point::Point
Point(int x, int y)
Constructor.
Definition: mirror_calib.cpp:227
firevision::YUV_t_struct::V
unsigned char V
V component.
Definition: yuv.h:68
fawkes::Exception
Definition: exception.h:41