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
55using namespace fawkes;
56
57namespace 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
90namespace {
91const unsigned int ORIENTATION_COUNT = (ORI_DEG_360 - ORI_DEG_0);
92const unsigned int MARK_COUNT = 6;
93const float MARK_DISTANCE = 29.7;
94const float CARPET_DISTANCE_FROM_ROBOT_CENTER = 5.0;
95const unsigned char DARK = 0;
96const unsigned char BRIGHT = 255;
97const unsigned char MARK_LUMA = 128;
98const unsigned char MARK_CHROMINANCE = 255;
99const int MIN_WIDTH_OF_BIGGEST_LINE = 50;
100const float APPROX_LINE_WIDTH_LOSS = 0.20f;
101const float MIN_BRIGHT_FRACTION = 0.20f;
102const int HIGHLIGHT_RADIUS = 2;
103const 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{
112private:
113 unsigned char *buffer_;
114 size_t buflen_;
115 int width_;
116 int height_;
117 unsigned int * refcount_;
118
119public:
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 */
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 &
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 *
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{
220public:
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{
274public:
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{
309public:
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. */
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{
339private:
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
347public:
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,
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 *
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
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 {
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. */
746MirrorCalibTool::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 */
756bool
757MirrorCalibTool::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 */
770bool
771MirrorCalibTool::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{
787public:
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)
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{
814public:
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 *
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
1013private:
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. */
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 */
1065MirrorCalibTool::PolarAngle
1066MirrorCalibTool::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 */
1078MirrorCalibTool::PolarAngle
1079MirrorCalibTool::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 */
1089void
1090MirrorCalibTool::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 */
1119void
1120MirrorCalibTool::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. */
1138void
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. */
1155void
1156MirrorCalibTool::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. */
1174void
1175MirrorCalibTool::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. */
1190void
1191MirrorCalibTool::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. */
1205void
1206MirrorCalibTool::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. */
1221void
1222MirrorCalibTool::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. */
1239void
1240MirrorCalibTool::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. */
1250void
1251MirrorCalibTool::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 */
1259const 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 */
1277MirrorCalibTool::MarkList
1278MirrorCalibTool::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 */
1291MirrorCalibTool::MarkList
1292MirrorCalibTool::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 */
1319MirrorCalibTool::HoleList
1320MirrorCalibTool::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(). */
1337MirrorCalibTool::HoleList
1338MirrorCalibTool::filter_biggest_holes(const HoleList &holes, unsigned int n)
1339{
1340#ifdef FILTER_HOLES
1341 HoleList biggest = holes;
1342# ifdef FILTER_MINI_HOLES
1343restart: // 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(). */
1397MirrorCalibTool::MarkList
1398MirrorCalibTool::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. */
1422MirrorCalibTool::MarkList
1423MirrorCalibTool::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
1445void
1446MirrorCalibTool::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 */
1503void
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);
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 */
1661MirrorCalibTool::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 */
1703MirrorCalibTool::PolarAnglePair
1704MirrorCalibTool::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 */
1769MirrorCalibTool::RealDistance
1770MirrorCalibTool::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
1776MirrorCalibTool::RealDistance
1777MirrorCalibTool::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 */
1825Bulb
1826MirrorCalibTool::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
1900void
1901MirrorCalibTool::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 */
1911const 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 */
1923void
1924MirrorCalibTool::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 */
1940void
1941MirrorCalibTool::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 */
1949void
1950MirrorCalibTool::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 */
1970void
1971MirrorCalibTool::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 */
1996void
1997MirrorCalibTool::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 */
2024void
2025MirrorCalibTool::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
Base class for exceptions in Fawkes.
Definition: exception.h:36
Bulb mirror lookup table.
Definition: bulb.h:37
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 save(const char *filename)
Save LUT from file.
Definition: bulb.cpp:297
Sobel filter.
Definition: sobel.h:35
virtual void apply()
Apply the filter.
Definition: sobel.cpp:158
virtual void set_dst_buffer(unsigned char *buf, ROI *roi)
Set the destination buffer.
Definition: filter.cpp:123
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
Wraps an image so that access to (0, 0) is mapped to the middle of the image and so on.
CartesianImage(const unsigned char *buf, int width, int height, PolarAngle phi)
Constructor.
float bright_fraction(const CartesianPoint &from, const CartesianPoint &to) const
Get relative amount of bright pixels.
void highlight_point(const CartesianPoint &p)
Highlights a point, i.e.
CartesianImage(const StepResult &res, PolarAngle phi, PixelPoint center, const unsigned char *mask=0)
Constructor.
PolarRadius max_radius() const
Maximum polar radius of the image.
const PixelPoint & center() const
Center of buffer.
int max_y() const
Get Maximum cartesian Y coordinate of the image.
void highlight_pixel(const PixelPoint &p)
Highlights a pixel, i.e.
PixelPoint to_pixel(const CartesianPoint &p) const
Converts a cartesian point to a pixel point.
void highlight_line(const CartesianPoint &p, int length)
Highlightes a line at pixel point p.
CartesianImage(unsigned char *buf, int width, int height, PolarAngle phi)
Constructor.
const unsigned char * mask() const
Mask.
bool contains(const CartesianPoint &p) const
Indicates whether the image contains a cartesian 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.
CartesianImage(const StepResult &res, PolarAngle phi, const unsigned char *mask=0)
Constructor.
bool contains(const PixelPoint &p) const
Indicates whether the image contains a pixel point.
CartesianImage(const unsigned char *buf, int width, int height, PolarAngle phi, const PixelPoint &center)
Constructor.
const unsigned char * buf() const
Buffer.
void draw_line(const PixelPoint &p, const PixelPoint &q)
Draws a line from p to q.
CartesianImage(unsigned char *buf, int width, int height, PolarAngle phi, const PixelPoint &center)
Constructor.
void set_color(const CartesianPoint &p, unsigned char luma, unsigned char chrominance)
Sets the luminance Y and chrominance U at a given cartesian point.
bool is_line(const CartesianPoint &p, int length) const
Indicates whether at pixel point p there is a highlighted line.
CartesianPoint to_cartesian(const PixelPoint &p) const
Converts a pixel point to a cartesian point.
void draw_line(const CartesianPoint &p, const CartesianPoint &q)
Draws a line from p to q.
int max_x() const
Get Maximum cartesian X coordinate of the image.
unsigned char get(const CartesianPoint &p) const
Get luminance at a given point.
A cartesian point is a 2d point which can have negative X and Y coords.
CartesianPoint rotate(PolarAngle rotate_phi) const
Rotates the vector to the point counter-clockwise and returns the vector to the point.
CartesianPoint(PolarAngle phi, PolarRadius length)
Constructor.
A hole is a sequence of pixels between two lines.
PolarRadius size() const
The size of the hole in pixels.
PolarRadius from_length
Beginning of the hole.
PolarRadius to_length
Ending of the hole.
Hole(int index, PolarRadius from_length, PolarRadius to_length)
Constructor.
int index
The index of the hole.
A container for a YUV-buffer etc.
const StepResult & result(int i) const
Returns the i-th result.
const MarkList & marks() const
The (final) marks.
void add_result(const StepResult &r)
Appends a result.
const unsigned char * yuv_buffer() const
YUV buffer.
int height() const
YUV buffer's height.
Image(const Image &copy)
Constructor.
StepResultList & results()
List of results.
Image & operator=(const Image &copy)
Assignment.
int width() const
YUV buffer's width.
void set_premarks(const MarkList &premarks)
The premarks.
PolarAngle ori() const
Angle of marks wrt X axis.
const MarkList & premarks()
The premarks.
unsigned char * yuv_buffer()
YUV buffer.
MarkList & marks()
The (final) marks.
const StepResultList & results() const
List of results.
StepResult & result(int i)
Returns the i-th result.
Image(const unsigned char *yuv_buffer, size_t buflen, int width, int height, PolarAngle ori)
Constructor.
void set_marks(const MarkList &marks)
The (final) marks.
size_t buflen() const
YUV buffer's length.
A pixel point is a 2d point with positive X and Y coords.
PixelPoint rotate(PolarAngle rotate_phi) const
Rotates the vector to the point counter-clockwise and returns the vector to the point.
PixelPoint(int x, int y)
Constructor.
PolarRadius length() const
Length of the vector the point.
Point(int x, int y)
Constructor.
PolarAngle atan() const
Atan(y, x) of the point.
Point(const Point &p)
Copy constructor.
Point & operator=(const Point &p)
Assignment operator.
The result of a step contains a YUV buffer.
int width() const
Width of YUV buffer.
StepResult & operator=(const StepResult &copy)
Assignment.
StepResult(size_t buflen, int width, int height)
Constructor.
size_t buflen() const
YUV buffer's length.
StepResult(const StepResult &copy)
Constructor.
const unsigned char * yuv_buffer() const
YUV buffer.
unsigned char * yuv_buffer()
YUV buffer.
int height() const
Height of YUV buffer.
void push_back(const unsigned char *yuv_buffer, size_t buflen, int width, int height, double ori)
Store image for calibration process.
void draw_mark_lines(unsigned char *yuv_buffer)
Draws a crosshair with the lines in the directions of the keys of the mark map.
const unsigned char * get_last_yuv_buffer() const
Get last created YUV buffer.
void save(const char *filename)
Saves calibration data to a file.
void eval(unsigned int x, unsigned int y, float *x_ret, float *y_ret)
Get the assumed distance and orientation of a pixel point.
void next_step()
Performs one step in the calibration process.
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 load(const char *filename)
Loads a calibration file.
void abort()
Aborts the calibration process.
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.
int center_y() const
Center Y accessor.
Definition: mirror_calib.h:85
int center_x() const
Center X accessor.
Definition: mirror_calib.h:78
void load_mask(const char *mask_file_name)
Loads a PNM mask file for the robot's bars.
const char * get_state_description() const
Get description of next step.
PNM file reader.
Definition: pnm.h:34
virtual void set_buffer(unsigned char *yuv422planar_buffer)
Set buffer that the read image should be written to.
Definition: pnm.cpp:124
virtual unsigned int pixel_width()
Get width of read image in pixels.
Definition: pnm.cpp:136
virtual unsigned int pixel_height()
Get height of read image in pixels.
Definition: pnm.cpp:142
virtual colorspace_t colorspace()
Get colorspace from the just read image.
Definition: pnm.cpp:130
virtual void read()
Read data from file.
Definition: pnm.cpp:148
Region of interest.
Definition: roi.h:55
static ROI * full_image(unsigned int width, unsigned int height)
Get full image ROI for given size.
Definition: roi.cpp:565
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
Definition: angle.h:72
float normalize_rad(float angle_rad)
Normalize angle in radian between 0 (inclusive) and 2*PI (exclusive).
Definition: angle.h:90
float rad2deg(float rad)
Convert an angle given in radians to degrees.
Definition: angle.h:46
Polar coordinates.
Definition: types.h:96
float phi
angle
Definition: types.h:98
float r
distance
Definition: types.h:97
YUV pixel.
Definition: yuv.h:58
unsigned char V
V component.
Definition: yuv.h:61
unsigned char U
U component.
Definition: yuv.h:60
unsigned char Y
Y component.
Definition: yuv.h:59