24 #include "mirror_calib.h"
26 #include <core/exception.h>
27 #include <utils/math/angle.h>
29 #include <fvutils/color/yuv.h>
30 #include <fvutils/readers/pnm.h>
32 #include <fvfilters/sobel.h>
33 #include <fvfilters/sharpen.h>
34 #include <fvfilters/median.h>
35 #include <fvfilters/or.h>
36 #include <fvfilters/laplace.h>
37 #include <fvfilters/min.h>
55 #define FILTER_MINI_HOLES
57 using namespace fawkes;
59 namespace firevision {
93 const unsigned int ORIENTATION_COUNT = (ORI_DEG_360 - ORI_DEG_0);
94 const unsigned int MARK_COUNT = 6;
95 const float MARK_DISTANCE = 29.7;
96 const float CARPET_DISTANCE_FROM_ROBOT_CENTER = 5.0;
97 const unsigned char DARK = 0;
98 const unsigned char BRIGHT = 255;
99 const unsigned char MARK_LUMA = 128;
100 const unsigned char MARK_CHROMINANCE = 255;
101 const int MIN_WIDTH_OF_BIGGEST_LINE = 50;
102 const float APPROX_LINE_WIDTH_LOSS = 0.20f;
103 const float MIN_BRIGHT_FRACTION = 0.20f;
104 const int HIGHLIGHT_RADIUS = 2;
105 const int FALSE_POSITIVE_HOLE_SIZE = 5;
115 unsigned char* buffer_;
119 unsigned int* refcount_;
127 : buffer_(new unsigned char[buflen]),
131 refcount_(new unsigned int)
139 : buffer_(copy.buffer_),
140 buflen_(copy.buflen_),
142 height_(copy.height_),
143 refcount_(copy.refcount_)
154 if (--*refcount_ == 0) {
158 buffer_ = copy.buffer_;
159 refcount_ = copy.refcount_;
160 buflen_ = copy.buflen_;
161 width_ = copy.width_;
162 height_ = copy.height_;
171 if (--*refcount_ == 0) {
183 inline const unsigned char*
yuv_buffer()
const {
return buffer_; }
187 inline size_t buflen()
const {
return buflen_; }
191 inline int width()
const {
return width_; }
195 inline int height()
const {
return height_; }
221 return static_cast<PolarRadius
>(sqrt(x*x + y*y));
256 :
Point(length * cos(phi),
275 const PolarRadius len = length();
276 const PolarAngle phi = atan() + rotate_phi;
277 const int x = len * cos(phi);
278 const int y = len * sin(phi);
305 const PolarRadius len = length();
306 const PolarAngle phi = atan() + rotate_phi;
307 const int x = len * cos(phi);
308 const int y = len * sin(phi);
326 const PolarAngle phi_;
327 const unsigned char* mask_;
339 const unsigned char* mask = 0)
340 : buf_(const_cast<unsigned char*>(res.yuv_buffer())),
342 height_(res.height()),
355 const unsigned char* mask = 0)
356 : buf_(const_cast<unsigned char*>(res.yuv_buffer())),
358 height_(res.height()),
359 center_(
PixelPoint(res.width()/2, res.height()/2)),
377 : buf_(const_cast<unsigned char*>(buf)),
416 : buf_(const_cast<unsigned char*>(buf)),
446 inline unsigned char*
buf() {
return buf_; }
450 inline const unsigned char*
mask()
const {
return mask_; }
454 inline const unsigned char*
buf()
const {
return buf_; }
462 inline const int width()
const {
return width_; }
466 inline const int height()
const {
return height_; }
470 inline const PolarAngle
phi()
const {
return phi_; }
480 return PixelPoint(center().x + rp.
x, center().y - rp.
y);
501 return 0 <= p.
x && p.
x <= width()-1 && 0 <= p.
y && p.
y <= height()-1;
511 return contains(to_pixel(p));
527 (YUV422_PLANAR_Y_AT(mask(), width(), pp.
x, pp.
y) != ignr.
Y &&
528 YUV422_PLANAR_U_AT(mask(), width(), height(), pp.
x, pp.
y) != ignr.
U &&
529 YUV422_PLANAR_V_AT(mask(), width(), height(), pp.
x, pp.
y) != ignr.
V)) {
530 return YUV422_PLANAR_Y_AT(buf(), width(), pp.
x, pp.
y);
542 inline int max_x()
const {
return std::max(center().x, width() - center().x); }
546 inline int max_y()
const {
return std::max(center().y, height() - center().y); }
551 return static_cast<PolarRadius
>(sqrt(max_x()*max_x() + max_y()*max_y()));
561 unsigned char chrominance)
566 YUV422_PLANAR_Y_AT(buf(), width(), p.
x, p.
y) = luma;
567 YUV422_PLANAR_U_AT(buf(), width(), height(), p.
x, p.
y) = chrominance;
577 unsigned char chrominance)
579 set_color(to_pixel(p), luma, chrominance);
591 const int from_x = from.
x < to.
x ? from.
x : to.
x;
592 const int to_x = from.
x > to.
x ? from.
x : to.
x;
593 const int from_y = from.
y < to.
y ? from.
y : to.
y;
594 const int to_y = from.
y > to.
y ? from.
y : to.
y;
596 int bright_count = 0;
597 for (
int x = from_x; x <= to_x; x++) {
598 for (
int y = from_y; y <= to_y; y++) {
601 if (
get(p) == BRIGHT) {
608 return static_cast<float>(
static_cast<double>(bright_count)
609 / static_cast<double>(pixel_count));
621 for (
int y_offset = 0; y_offset <= 1; y_offset++) {
625 if (bright_fraction(from, to) >= MIN_BRIGHT_FRACTION) {
652 draw_line(to_cartesian(p), to_cartesian(q));
663 for (PolarRadius length = 0; length <= distVec.
length(); length++) {
666 if (contains(linePoint)) {
667 set_color(linePoint, MARK_LUMA, MARK_CHROMINANCE);
678 const int R = HIGHLIGHT_RADIUS;
679 for (
int xx = p.
x-R; xx <= p.
x+R; xx++) {
680 for (
int yy = p.
y-R; yy <= p.
y+R; yy++) {
683 set_color(pp, MARK_LUMA, MARK_CHROMINANCE);
695 const int R = HIGHLIGHT_RADIUS;
696 for (
int xx = p.
x-R; xx <= p.
x+R; xx++) {
697 for (
int yy = p.
y-R; yy <= p.
y+R; yy++) {
700 set_color(hp, MARK_LUMA, MARK_CHROMINANCE);
720 MirrorCalibTool::ConvexPolygon::contains(
const CartesianImage& img,
721 const CartesianPoint& r)
const
723 return contains(img.to_pixel(r));
736 MirrorCalibTool::ConvexPolygon::contains(
const PixelPoint& r)
const
738 for (std::vector<PixelPoint>::size_type i = 1; i <= size(); i++) {
739 const PixelPoint& p = at(i-1);
740 const PixelPoint& q = at(i % size());
741 double val = (q.x - p.x) * (r.y - p.y) - (r.x - p.x) * (q.y - p.y);
762 PolarRadius from_length,
763 PolarRadius to_length)
765 from_length(from_length),
773 inline PolarRadius
size()
const {
return to_length - from_length; }
787 Image(
const unsigned char* yuv_buffer,
792 : yuv_buffer_(new unsigned char[buflen]),
797 refcount_(new unsigned int)
799 memcpy(yuv_buffer_, yuv_buffer, buflen);
806 : yuv_buffer_(copy.yuv_buffer_),
807 buflen_(copy.buflen_),
809 height_(copy.height_),
811 results_(copy.results_),
812 premarks_(copy.premarks_),
814 refcount_(copy.refcount_)
826 if (--*refcount_ == 0) {
827 delete[] yuv_buffer_;
830 yuv_buffer_ = copy.yuv_buffer_;
831 buflen_ = copy.buflen_;
832 width_ = copy.width_;
833 height_ = copy.height_;
835 results_ = copy.results_;
836 premarks_ = copy.premarks_;
837 marks_ = copy.marks_;
838 refcount_ = copy.refcount_;
847 if (--*refcount_ == 0) {
848 delete[] yuv_buffer_;
860 inline const unsigned char*
yuv_buffer()
const {
return yuv_buffer_; }
864 inline size_t buflen()
const {
return buflen_; }
868 inline int width()
const {
return width_; }
872 inline int height()
const {
return height_; }
876 inline PolarAngle
ori()
const {
return ori_; }
880 inline StepResultList&
results() {
return results_; }
884 inline const StepResultList&
results()
const {
return results_; }
888 inline const MarkList&
premarks() {
return premarks_; }
892 inline MarkList&
marks() {
return marks_; }
896 inline const MarkList&
marks()
const {
return marks_; }
913 inline void set_premarks(
const MarkList& premarks) { premarks_ = premarks; }
916 inline void set_marks(
const MarkList& marks) { marks_ = marks; }
919 unsigned char* yuv_buffer_;
924 StepResultList results_;
927 unsigned int* refcount_;
932 MirrorCalibTool::MirrorCalibTool()
933 : img_yuv_buffer_(0),
937 state_(CalibrationState())
946 if (img_yuv_buffer_) {
947 delete[] img_yuv_buffer_;
950 delete[] img_yuv_mask_;
973 MirrorCalibTool::PolarAngle
974 MirrorCalibTool::relativeOrientationToImageRotation(PolarAngle ori)
987 MirrorCalibTool::PolarAngle
988 MirrorCalibTool::imageRotationToRelativeOrientation(PolarAngle ori)
1002 if (img_yuv_mask_) {
1003 delete[] img_yuv_mask_;
1006 size_t size = colorspace_buffer_size(reader.
colorspace(),
1009 img_yuv_mask_ =
new unsigned char[size];
1010 if (img_center_x_ == -1) {
1013 if (img_center_y_ == -1) {
1038 ori = relativeOrientationToImageRotation(ori);
1039 Image src_img(yuv_buffer, buflen, width, height, ori);
1040 if (img_center_x_ == -1) {
1041 img_center_x_ = width / 2;
1043 if (img_center_y_ == -1) {
1044 img_center_y_ = height / 2;
1046 source_images_.push_back(src_img);
1056 state_ = CalibrationState();
1057 source_images_.clear();
1070 MirrorCalibTool::apply_sobel(
unsigned char* src,
1090 MirrorCalibTool::apply_sharpen(
unsigned char* src,
1096 FilterSharpen filter;
1098 filter.set_dst_buffer(dst, roi);
1110 MirrorCalibTool::apply_median(
unsigned char* src,
1117 FilterMedian filter(i);
1118 filter.set_src_buffer(src, roi, 0);
1119 filter.set_dst_buffer(dst, roi);
1130 MirrorCalibTool::apply_min(
unsigned char* src,
1137 filter.set_src_buffer(src, roi, 0);
1138 filter.set_dst_buffer(dst, roi);
1150 MirrorCalibTool::apply_or(
unsigned char* src1,
1151 unsigned char* src2,
1158 filter.set_src_buffer(src1, roi, 0);
1159 filter.set_src_buffer(src2, roi, 1);
1160 filter.set_dst_buffer(dst, roi);
1169 MirrorCalibTool::make_contrast(
unsigned char* buf,
1172 for (
unsigned int i = 0; i < buflen/2; i++) {
1173 buf[i] = (buf[i] >= 75) ? BRIGHT : DARK;
1182 MirrorCalibTool::make_grayscale(
unsigned char* buf,
1185 memset(buf + buflen/2, 128, buflen - buflen/2);
1195 switch (state_.step) {
1196 case SHARPENING:
return "Sharpen";
1197 case EDGE_DETECTION:
return "Edge detection";
1198 case COMBINATION:
return "Combining images";
1199 case CENTERING:
return "Finding center point";
1200 case PRE_MARKING:
return "First marking";
1201 case FINAL_MARKING:
return "Final marking";
1202 case DONE:
return "Direction done";
1203 default:
return "Invalid state";
1211 MirrorCalibTool::MarkList
1212 MirrorCalibTool::premark(
const StepResult& prev,
1213 const unsigned char* yuv_mask,
1216 const PixelPoint& center)
1218 const ConvexPolygon empty_polygon;
1219 return premark(empty_polygon, prev, yuv_mask, result, phi, center);
1226 MirrorCalibTool::MarkList
1227 MirrorCalibTool::premark(
const ConvexPolygon& polygon,
1228 const StepResult& prev,
1229 const unsigned char* yuv_mask,
1232 const PixelPoint& center)
1234 const CartesianImage prev_img(prev, phi, center, yuv_mask);
1235 CartesianImage res_img(result, phi, center, yuv_mask);
1236 int width = MIN_WIDTH_OF_BIGGEST_LINE;
1238 for (PolarRadius length = 0; length < prev_img.max_radius(); length++)
1240 const CartesianPoint p(0.0, length);
1241 if (polygon.contains(prev_img, p) && prev_img.is_line(p, width)) {
1242 premarks.push_back(length);
1243 res_img.highlight_line(p, width);
1245 if (length % 25 == 0) {
1246 width *= (1.0f - APPROX_LINE_WIDTH_LOSS);
1256 MirrorCalibTool::HoleList
1257 MirrorCalibTool::search_holes(
const MarkList& premarks)
1260 PolarRadius prev_radius = -1;
1261 for (MarkList::const_iterator it = premarks.begin();
1262 it != premarks.end(); it++)
1264 PolarRadius radius = *it;
1265 if (prev_radius != -1 && prev_radius + 1 < radius) {
1266 Hole hole(holes.size(), prev_radius, radius-1);
1267 holes.push_back(hole);
1269 prev_radius = radius;
1277 MirrorCalibTool::HoleList
1278 MirrorCalibTool::filter_biggest_holes(
const HoleList& holes,
1282 HoleList biggest = holes;
1283 #ifdef FILTER_MINI_HOLES
1285 for (HoleList::iterator it = biggest.begin(); it != biggest.end(); it++)
1287 if (it->size() <= FALSE_POSITIVE_HOLE_SIZE) {
1295 for (
unsigned int from = 0; from < biggest.size(); from++)
1298 for (to = from + 1; to < biggest.size(); to++) {
1299 if ((to - from + 1) > n) {
1303 if (biggest[to - 1].size() < biggest[to].size()) {
1308 if (to - from + 1 > filtered.size()) {
1310 for (
unsigned int j = from; j <= to; j++) {
1311 filtered.push_back(biggest[j]);
1318 for (HoleList::const_iterator it = holes.begin(); it != holes.end(); ++it)
1320 const Hole& hole = *it;
1321 #ifdef FILTER_MINI_HOLES
1322 if (hole.size() < FALSE_POSITIVE_HOLE_SIZE) {
1327 if (biggest.size() == 1 && hole.size() > biggest.front().size()) {
1331 biggest.push_back(hole);
1332 if (biggest.size() == n) {
1342 MirrorCalibTool::MarkList
1343 MirrorCalibTool::determine_marks(
const HoleList& holes)
1345 HoleList biggest = filter_biggest_holes(holes, MARK_COUNT - 1);
1346 std::cout <<
"Filtered Holes: " << biggest.size() << std::endl;
1348 for (HoleList::const_iterator prev, iter = biggest.begin();
1349 iter != biggest.end(); iter++) {
1350 const Hole& hole = *iter;
1351 if (iter == biggest.begin()) {
1352 const PolarRadius length = hole.from_length;
1353 marks.push_back(length);
1355 const PolarRadius length = (hole.from_length + prev->to_length) / 2;
1356 marks.push_back(length);
1358 if (iter+1 == biggest.end()) {
1359 const PolarRadius length = hole.to_length;
1360 marks.push_back(length);
1369 MirrorCalibTool::MarkList
1370 MirrorCalibTool::mark(
const MarkList& premarks,
1371 const unsigned char* yuv_mask,
1374 const PixelPoint& center)
1376 std::cout <<
"Premarked places: " << premarks.size() << std::endl;
1377 HoleList holes = search_holes(premarks);
1378 std::cout <<
"Found Holes: " << holes.size() << std::endl;
1379 MarkList marks = determine_marks(holes);
1380 std::cout <<
"Found Marks: " << marks.size() << std::endl;
1382 CartesianImage res_img(result, phi, center, yuv_mask);
1383 for (MarkList::const_iterator iter = marks.begin();
1384 iter != marks.end(); iter++)
1386 const PolarAngle angle = 0.0;
1387 const PolarRadius radius = *iter;
1388 res_img.highlight_point(CartesianPoint(angle, radius));
1389 std::cout <<
"Highlighting Mark at " << *iter << std::endl;
1396 MirrorCalibTool::goto_next_state()
1398 const Image& src_img = source_images_[state_.image_index];
1399 switch (state_.step) {
1401 state_.step = EDGE_DETECTION;
1403 case EDGE_DETECTION:
1404 if (src_img.results().size() <= ORIENTATION_COUNT) {
1405 state_.step = EDGE_DETECTION;
1407 state_.step = COMBINATION;
1411 if (src_img.results().size() < 2 * ORIENTATION_COUNT) {
1412 state_.step = COMBINATION;
1413 }
else if (state_.image_index + 1 < source_images_.size()) {
1414 state_.step = SHARPENING;
1415 state_.image_index++;
1417 state_.step = PRE_MARKING;
1418 state_.centering_done =
false;
1419 state_.image_index = 0;
1423 state_.step = PRE_MARKING;
1424 state_.centering_done =
true;
1425 state_.image_index = 0;
1428 state_.step = FINAL_MARKING;
1431 if (state_.image_index + 1 < source_images_.size()) {
1432 state_.step = PRE_MARKING;
1433 state_.image_index++;
1434 #ifdef RECALCULATE_CENTER_POINT
1435 }
else if (!state_.centering_done) {
1436 state_.step = CENTERING;
1437 state_.image_index = 0;
1441 state_.image_index = 0;
1446 state_.image_index = (state_.image_index + 1) % source_images_.size();
1462 printf(
"Performing step for image %u / %u, %s\n",
1463 (
unsigned int)state_.image_index + 1,
1464 (
unsigned int)source_images_.size(),
1466 if (state_.image_index >= source_images_.size()) {
1469 Image& src_img = source_images_[state_.image_index];
1471 switch (state_.step) {
1474 apply_sharpen(src_img.
yuv_buffer(), result.yuv_buffer(),
1476 make_grayscale(result.yuv_buffer(), result.buflen());
1479 set_last_yuv_buffer(result.yuv_buffer());
1482 case EDGE_DETECTION:
1485 const int offset = (src_img.
results().size() - 1) % ORIENTATION_COUNT;
1486 const orientation_t ori =
static_cast<orientation_t
>(ORI_DEG_0+offset);
1487 apply_sobel(prev.
yuv_buffer(), result.yuv_buffer(),
1489 make_grayscale(result.yuv_buffer(), result.buflen());
1490 make_contrast(result.yuv_buffer(), result.buflen());
1492 printf(
"Edge detection in %u\n", (
unsigned) src_img.
results().size());
1494 set_last_yuv_buffer(result.yuv_buffer());
1495 assert(!memcmp(result.yuv_buffer(),
1502 const int index1 = src_img.
results().size() - ORIENTATION_COUNT == 1
1503 ? src_img.
results().size() - ORIENTATION_COUNT
1504 : src_img.
results().size() - 1;
1505 const int index2 = src_img.
results().size() - ORIENTATION_COUNT + 1;
1506 assert(index1 != index2);
1507 printf(
"ORing: %d = or(%d, %d)\n", (
unsigned) src_img.
results().size(),
1511 assert(&prev1 != &prev2);
1512 assert(prev1.width() == prev2.
width());
1513 assert(prev1.height() == prev2.
height());
1514 apply_or(prev1.yuv_buffer(), prev2.
yuv_buffer(), result.yuv_buffer(),
1515 prev1.width(), prev1.height());
1516 make_grayscale(result.yuv_buffer(), result.buflen());
1517 assert(memcmp(prev1.yuv_buffer(),
1520 assert(memcmp(result.yuv_buffer(),
1523 assert(memcmp(result.yuv_buffer(),
1528 set_last_yuv_buffer(result.yuv_buffer());
1529 assert(!memcmp(result.yuv_buffer(),
1537 memcpy(result.yuv_buffer(), orig.
yuv_buffer(), result.buflen());
1538 const PixelPoint center = calculate_center(source_images_);
1539 img_center_x_ = center.x;
1540 img_center_y_ = center.y;
1541 std::cout <<
"Found center (" << center.x <<
", "<< center.y <<
")"
1543 CartesianImage img(result, relativeOrientationToImageRotation(0.0),
1548 set_last_yuv_buffer(result.yuv_buffer());
1554 memcpy(result.yuv_buffer(), prev.
yuv_buffer(), result.buflen());
1555 const unsigned char* mask = img_yuv_mask_;
1556 const PixelPoint center(img_center_x_, img_center_y_);
1557 MarkList premarks = premark(prev, mask, result, src_img.
ori(), center);
1559 apply_sharpen(src_img.
yuv_buffer(), result.yuv_buffer(),
1563 set_last_yuv_buffer(result.yuv_buffer());
1569 memcpy(result.yuv_buffer(), orig.
yuv_buffer(), result.buflen());
1570 const unsigned char* mask = img_yuv_mask_;
1571 const PixelPoint center(img_center_x_, img_center_y_);
1572 const MarkList marks = mark(src_img.
premarks(), mask, result,
1573 src_img.
ori(), center);
1575 const PolarAngle ori = src_img.
ori();
1576 std::cout <<
"Marking done for orientation "
1579 <<
rad2deg(imageRotationToRelativeOrientation(ori))
1583 set_last_yuv_buffer(result.yuv_buffer());
1588 for (ImageList::iterator it = source_images_.begin();
1589 it != source_images_.end(); it++)
1593 StepResultList results = it->results();
1594 results.erase(results.begin() + 1, results.end());
1596 const Image& src_img = *it;
1597 const PolarAngle ori = src_img.
ori();
1598 MarkList marks = src_img.
marks();
1599 mark_map_[imageRotationToRelativeOrientation(ori)] = marks;
1603 const PixelPoint center(img_center_x_, img_center_y_);
1604 CartesianImage img(result, relativeOrientationToImageRotation(0.0),
1606 memcpy(result.yuv_buffer(), prev.
yuv_buffer(), result.buflen());
1609 for (MarkMap::const_iterator map_iter = mark_map_.begin();
1610 map_iter != mark_map_.end(); map_iter++)
1612 const PolarAngle phi = map_iter->first;
1613 const MarkList& list = map_iter->second;
1614 printf(
"%3.f: ",
rad2deg(phi));
1615 for (MarkList::const_iterator list_iter = list.begin();
1616 list_iter != list.end(); list_iter++)
1618 const PolarAngle angle = phi;
1619 const PolarRadius radius = *list_iter;
1622 printf(
"%3d (%d, %d)", radius, pp.
x, pp.
y);
1628 set_last_yuv_buffer(result.yuv_buffer());
1644 MirrorCalibTool::calculate_center(
const ImageList& images)
1648 for (ImageList::const_iterator it = images.begin();
1649 it != images.end(); it++)
1651 const Image& image = *it;
1652 const PolarRadius radius = image.marks().at(0);
1653 const unsigned char* null_buf = 0;
1654 const CartesianImage cart_image(null_buf, image.width(), image.height(),
1656 const CartesianPoint point(0.0, radius);
1657 const PixelPoint pixel = cart_image.to_pixel(point);
1661 return PixelPoint(x / images.size(), y / images.size());
1690 MirrorCalibTool::PolarAnglePair
1691 MirrorCalibTool::find_nearest_neighbors(PolarAngle angle,
1692 const MarkMap& mark_map)
1694 typedef std::vector<PolarAngle> AngleList;
1696 for (MarkMap::const_iterator it = mark_map.begin();
1697 it != mark_map.end(); it++)
1699 const PolarAngle mark_angle = it->first;
1701 diffs.push_back(diff);
1703 std::cout <<
"Finding nearest neighbors: "
1704 <<
"ref="<<angle<<
"="<<
rad2deg(angle)<<
" "
1705 <<
"to="<<mark_angle<<
"="<<
rad2deg(mark_angle)<<
" "
1706 <<
"diff="<<diff<<
"="<<
rad2deg(diff) << std::endl;
1709 bool min1_init =
false;
1710 AngleList::size_type min1_index = 0;
1711 bool min2_init =
false;
1712 AngleList::size_type min2_index = 0;
1713 for (
int i = 0;
static_cast<AngleList::size_type
>(i) < diffs.size(); i++) {
1714 if (!min1_init || fabs(diffs[min1_index]) >= fabs(diffs[i])) {
1715 min2_index = min1_index;
1716 min2_init = min1_init;
1719 }
else if (!min2_init || fabs(diffs[min2_index]) >= fabs(diffs[i])) {
1730 PolarAngle min1 = 0.0;
1731 PolarAngle min2 = 0.0;
1732 AngleList::size_type i = 0;
1733 for (MarkMap::const_iterator it = mark_map.begin();
1734 it != mark_map.end(); it++)
1736 if (i == min1_index) {
1738 }
else if (i == min2_index) {
1744 std::cout <<
"Found nearest neighbor 1: "
1745 <<
"ref="<<angle<<
"="<<
rad2deg(angle)<<
" "
1746 <<
"to="<<min1<<
"="<<
rad2deg(min1) <<
" "
1747 <<
"index="<<min1_index << std::endl;
1748 std::cout <<
"Found nearest neighbor 2: "
1749 <<
"ref="<<angle<<
"="<<
rad2deg(angle)<<
" "
1750 <<
"to="<<min2<<
"="<<
rad2deg(min2) <<
" "
1751 <<
"index="<<min2_index << std::endl;
1753 return PolarAnglePair(min1, min2);
1762 MirrorCalibTool::RealDistance
1763 MirrorCalibTool::calculate_real_distance(
int n)
1765 return static_cast<int>(CARPET_DISTANCE_FROM_ROBOT_CENTER +
1766 static_cast<float>(n + 1) * MARK_DISTANCE);
1770 MirrorCalibTool::RealDistance
1771 MirrorCalibTool::interpolate(PolarRadius radius,
1772 const MarkList& marks)
1774 if (marks.size() < 2) {
1777 for (MarkList::size_type i = 0; i < marks.size(); i++)
1780 if (i == 0 && radius < marks[i]) {
1781 const PolarRadius x0 = 0;
1782 const PolarRadius x1 = marks[i];
1783 const PolarRadius x = radius;
1784 const RealDistance f0 = 0;
1785 const RealDistance f1 = calculate_real_distance(i);
1787 std::cout <<
"A_Interpolate " << x <<
" between "
1788 << x0 <<
"->" << f0 <<
" "
1789 << x1 <<
"->" << f1 << std::endl;
1791 return static_cast<RealDistance
>(
static_cast<double>(f0) +
1792 static_cast<double>(x - x0) *
1793 static_cast<double>(f1 - f0) /
1794 static_cast<double>(x1 - x0));
1795 }
else if (i > 0 && marks[i-1] <= radius &&
1796 (radius < marks[i] || i+1 == marks.size())) {
1797 const PolarRadius x0 = marks[i-1];
1798 const PolarRadius x1 = marks[i];
1799 const PolarRadius x = radius;
1800 const RealDistance f0 = calculate_real_distance(i-1);
1801 const RealDistance f1 = calculate_real_distance(i);
1803 std::cout <<
"B_Interpolate " << x <<
" between "
1804 << x0 <<
"->" << f0 <<
" "
1805 << x1 <<
"->" << f1 << std::endl;
1807 return static_cast<RealDistance
>(
static_cast<double>(f0) +
1808 static_cast<double>(x - x0) *
1809 static_cast<double>(f1 - f0) /
1810 static_cast<double>(x1 - x0));
1826 MirrorCalibTool::generate(
int width,
1828 const PixelPoint& center,
1829 const MarkMap& mark_map)
1831 const unsigned char* null_img_buf = 0;
1832 CartesianImage img(null_img_buf, width, height,
1833 relativeOrientationToImageRotation(0.0), center);
1834 Bulb bulb(width, height);
1835 bulb.setCenter(center.x, center.y);
1836 bulb.setOrientation(0.0);
1837 for (
int x = 0; x < width; x++)
1839 for (
int y = 0; y < height; y++)
1841 const PixelPoint pp(x, y);
1842 const CartesianPoint cp = img.to_cartesian(pp);
1843 const PolarAngle ori_to_robot = cp.atan();
1844 const PolarAnglePair nearest_neighbors =
1845 find_nearest_neighbors(ori_to_robot, mark_map);
1846 const PolarAngle diff1 =
1848 const PolarAngle diff2 =
1850 const PolarAngle norm = diff1 + diff2;
1851 assert(norm != 0.0);
1852 const double weight1 = 1.0 - diff1 / norm;
1853 const double weight2 = 1.0 - diff2 / norm;
1855 std::cout <<
"PixelPoint("<< x <<
", "<< y <<
")"<< std::endl;
1856 std::cout <<
"CartesianPoint("<< cp.x <<
", "<< cp.y <<
")"<< std::endl;
1857 std::cout <<
"Radius, Angle: " << cp.length() <<
" "
1858 << ori_to_robot <<
" = "
1859 <<
rad2deg(ori_to_robot) << std::endl;
1860 std::cout <<
"Neighbor 1: "
1864 std::cout <<
"Neighbor 2: "
1868 std::cout <<
"Diff 1: " << diff1 <<
" = " <<
rad2deg(diff1) << std::endl;
1869 std::cout <<
"Diff 2: " << diff2 <<
" = " <<
rad2deg(diff2) << std::endl;
1870 std::cout <<
"Norm Factor: " << norm <<
" = " <<
rad2deg(norm)
1872 std::cout <<
"Weight 1: " << weight1 <<
" = " <<
rad2deg(weight1)
1874 std::cout <<
"Weight 2: " << weight2 <<
" = " <<
rad2deg(weight2)
1877 assert(0.0 <= weight1 && weight1 <= 1.0);
1878 assert(0.0 <= weight2 && weight2 <= 1.0);
1879 assert(1.0 - 10e-5 < weight1 + weight2 && weight1 + weight2 < 1.0 + 10e-5);
1880 const MarkList& marks1 = mark_map.at(nearest_neighbors.first);
1881 const MarkList& marks2 = mark_map.at(nearest_neighbors.second);
1882 const RealDistance dist1 = interpolate(cp.length(), marks1);
1883 const RealDistance dist2 = interpolate(cp.length(), marks2);
1885 std::cout <<
"Real 1 " << dist1 << std::endl;
1886 std::cout <<
"Real 2 " << dist2 << std::endl;
1888 const RealDistance weighted_mean_dist = dist1 * weight1 + dist2 * weight2;
1889 const float world_dist_in_meters = weighted_mean_dist / 100.0f;
1890 const float world_phi_rel_to_robot = -1.0f * ori_to_robot;
1895 std::cout <<
"Dist 1: " << dist1 << std::endl;
1896 std::cout <<
"Dist 2: " << dist2 << std::endl;
1897 std::cout <<
"World Dist: " << world_dist_in_meters << std::endl;
1898 std::cout <<
"World Phi: " << ori_to_robot <<
" = "
1899 <<
rad2deg(ori_to_robot) << std::endl;
1901 if (world_dist_in_meters > 0) {
1902 bulb.setWorldPoint(x, y, world_dist_in_meters, world_phi_rel_to_robot);
1911 MirrorCalibTool::set_last_yuv_buffer(
const unsigned char* last_buf)
1913 last_yuv_buffer_ = last_buf;
1922 const unsigned char*
1925 return last_yuv_buffer_;
1937 float* dist_ret,
float* ori_ret)
1940 printf(
"No bulb loaded, cannot evaluate.\n");
1946 *dist_ret = coord.
r;
1947 *ori_ret = coord.
phi;
1957 bulb_ =
new Bulb(filename);
1967 if (state_.step == DONE) {
1968 const Image& src_img = source_images_[state_.image_index];
1969 const PixelPoint center(img_center_x_, img_center_y_);
1970 Bulb bulb = generate(src_img.
width(), src_img.
height(), center, mark_map_);
1971 bulb.
save(filename);
1973 std::cout <<
"Can't save in the middle of the calibration" << std::endl;
1996 relativeOrientationToImageRotation(0.0),
1998 for (PolarRadius length = 0; length < img.
max_radius(); length++)
2002 img.
set_color(p, MARK_LUMA, MARK_CHROMINANCE);
2015 for (ImageList::const_iterator it = source_images_.begin();
2016 it != source_images_.end(); it++)
2018 const Image& src_img = *it;
2021 for (PolarRadius length = 0; length < img.max_radius(); length++)
2023 const PolarAngle angle = 0.0;
2025 if (img.contains(p)) {
2026 img.set_color(p, MARK_LUMA, MARK_CHROMINANCE);
2051 const int POSITION_COUNT =
sizeof POSITIONS /
sizeof(double);
2053 relativeOrientationToImageRotation(0.0),
2055 for (
int i = 0; i < POSITION_COUNT; i++)
2057 const PolarAngle angle = POSITIONS[i];
2058 for (PolarRadius length = 0; length < img.
max_radius(); length++)
2062 img.
set_color(p, MARK_LUMA, MARK_CHROMINANCE);
unsigned char V
V component.
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.
float normalize_rad(float angle_rad)
Normalize angle in radian between 0 (inclusive) and 2*PI (exclusive).
DrawingManipulator * set_color(float r, float g, float b)
Creates a drawing manipulator which sets the given color.
virtual void apply()
Apply the filter.
unsigned char Y
Y component.
virtual colorspace_t colorspace()
Get colorspace from the just read image.
static ROI * full_image(unsigned int width, unsigned int height)
Get full image ROI for given size.
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
virtual void read()
Read data from file.
Base class for exceptions in Fawkes.
virtual unsigned int pixel_height()
Get height of read image in pixels.
virtual fawkes::polar_coord_2d_t getWorldPointRelative(unsigned int image_x, unsigned int image_y) const
Get relative coordinate based on image coordinates.
Bulb mirror lookup table.
float rad2deg(float rad)
Convert an angle given in radians to degrees.
unsigned char U
U component.
virtual unsigned int pixel_width()
Get width of read image in pixels.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
void save(const char *filename)
Save LUT from file.
virtual void set_buffer(unsigned char *yuv422planar_buffer)
Set buffer that the read image should be written to.
virtual void set_dst_buffer(unsigned char *buf, ROI *roi)
Set the destination buffer.