24 #include "mirror_calib.h"
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>
53 #define FILTER_MINI_HOLES
57 namespace firevision {
91 const unsigned int ORIENTATION_COUNT = (ORI_DEG_360 - ORI_DEG_0);
92 const unsigned int MARK_COUNT = 6;
93 const float MARK_DISTANCE = 29.7;
94 const float CARPET_DISTANCE_FROM_ROBOT_CENTER = 5.0;
95 const unsigned char DARK = 0;
96 const unsigned char BRIGHT = 255;
97 const unsigned char MARK_LUMA = 128;
98 const unsigned char MARK_CHROMINANCE = 255;
99 const int MIN_WIDTH_OF_BIGGEST_LINE = 50;
100 const float APPROX_LINE_WIDTH_LOSS = 0.20f;
101 const float MIN_BRIGHT_FRACTION = 0.20f;
102 const int HIGHLIGHT_RADIUS = 2;
103 const int FALSE_POSITIVE_HOLE_SIZE = 5;
113 unsigned char *buffer_;
117 unsigned int * refcount_;
125 : buffer_(new unsigned char[buflen]),
129 refcount_(new unsigned int)
137 : buffer_(copy.buffer_),
138 buflen_(copy.buflen_),
140 height_(copy.height_),
141 refcount_(copy.refcount_)
153 if (--*refcount_ == 0) {
157 buffer_ = copy.buffer_;
158 refcount_ = copy.refcount_;
159 buflen_ = copy.buflen_;
160 width_ = copy.width_;
161 height_ = copy.height_;
170 if (--*refcount_ == 0) {
178 inline unsigned char *
186 inline const unsigned char *
244 return static_cast<PolarRadius>(sqrt(x * x + y * y));
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);
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);
340 unsigned char * buf_;
344 const PolarAngle phi_;
345 const unsigned char *mask_;
357 const unsigned char *mask = 0)
358 : buf_(const_cast<unsigned char *>(res.yuv_buffer())),
360 height_(res.height()),
372 : buf_(const_cast<unsigned char *>(res.yuv_buffer())),
374 height_(res.height()),
375 center_(
PixelPoint(res.width() / 2, res.height() / 2)),
393 : buf_(const_cast<unsigned char *>(buf)),
414 : buf_(buf), width_(width), height_(height), center_(center), phi_(phi), mask_(0)
424 : buf_(const_cast<unsigned char *>(buf)),
451 inline unsigned char *
459 inline const unsigned char *
467 inline const unsigned char *
513 return PixelPoint(center().x + rp.
x, center().y - rp.
y);
534 return 0 <= p.
x && p.
x <= width() - 1 && 0 <= p.
y && p.
y <= height() - 1;
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);
578 return std::max(center().x, width() - center().x);
586 return std::max(center().y, height() - center().y);
594 return static_cast<PolarRadius>(sqrt(max_x() * max_x() + max_y() * max_y()));
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;
618 set_color(to_pixel(p), luma, chrominance);
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;
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++) {
639 if (get(p) == BRIGHT) {
646 return static_cast<float>(static_cast<double>(bright_count) / static_cast<double>(pixel_count));
657 for (
int y_offset = 0; y_offset <= 1; y_offset++) {
661 if (bright_fraction(from, to) >= MIN_BRIGHT_FRACTION) {
686 draw_line(to_cartesian(p), to_cartesian(q));
696 for (PolarRadius length = 0; length <= distVec.
length(); length++) {
700 set_color(linePoint, MARK_LUMA, MARK_CHROMINANCE);
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++) {
716 set_color(pp, MARK_LUMA, MARK_CHROMINANCE);
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++) {
733 set_color(hp, MARK_LUMA, MARK_CHROMINANCE);
746 MirrorCalibTool::ConvexPolygon::ConvexPolygon()
757 MirrorCalibTool::ConvexPolygon::contains(
const CartesianImage &img,
const CartesianPoint &r)
const
771 MirrorCalibTool::ConvexPolygon::contains(
const PixelPoint &r)
const
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);
796 Hole(
int index, PolarRadius from_length, PolarRadius to_length)
797 : index(index), from_length(from_length), to_length(to_length)
807 return to_length - from_length;
821 Image(
const unsigned char *yuv_buffer,
size_t buflen,
int width,
int height, PolarAngle ori)
822 : yuv_buffer_(new unsigned char[buflen]),
827 refcount_(new unsigned int)
829 memcpy(yuv_buffer_, yuv_buffer, buflen);
836 : yuv_buffer_(copy.yuv_buffer_),
837 buflen_(copy.buflen_),
839 height_(copy.height_),
841 results_(copy.results_),
842 premarks_(copy.premarks_),
844 refcount_(copy.refcount_)
857 if (--*refcount_ == 0) {
858 delete[] yuv_buffer_;
861 yuv_buffer_ = copy.yuv_buffer_;
862 buflen_ = copy.buflen_;
863 width_ = copy.width_;
864 height_ = copy.height_;
866 results_ = copy.results_;
867 premarks_ = copy.premarks_;
868 marks_ = copy.marks_;
869 refcount_ = copy.refcount_;
878 if (--*refcount_ == 0) {
879 delete[] yuv_buffer_;
887 inline unsigned char *
895 inline const unsigned char *
935 inline StepResultList &
943 inline const StepResultList &
951 inline const MarkList &
967 inline const MarkList &
978 results_.push_back(r);
1003 premarks_ = premarks;
1014 unsigned char *yuv_buffer_;
1019 StepResultList results_;
1022 unsigned int * refcount_;
1026 MirrorCalibTool::MirrorCalibTool()
1027 : img_yuv_buffer_(0),
1031 state_(CalibrationState()),
1039 if (img_yuv_buffer_) {
1040 delete[] img_yuv_buffer_;
1042 if (img_yuv_mask_) {
1043 delete[] img_yuv_mask_;
1065 MirrorCalibTool::PolarAngle
1066 MirrorCalibTool::relativeOrientationToImageRotation(PolarAngle ori)
1078 MirrorCalibTool::PolarAngle
1079 MirrorCalibTool::imageRotationToRelativeOrientation(PolarAngle ori)
1092 if (img_yuv_mask_) {
1093 delete[] img_yuv_mask_;
1098 img_yuv_mask_ =
new unsigned char[size];
1099 if (img_center_x_ == -1) {
1102 if (img_center_y_ == -1) {
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;
1131 if (img_center_y_ == -1) {
1132 img_center_y_ = height / 2;
1134 source_images_.push_back(src_img);
1143 state_ = CalibrationState();
1144 source_images_.clear();
1156 MirrorCalibTool::apply_sobel(
unsigned char *src,
1175 MirrorCalibTool::apply_sharpen(
unsigned char *src,
unsigned char *dst,
int width,
int height)
1178 FilterSharpen filter;
1180 filter.set_dst_buffer(dst, roi);
1191 MirrorCalibTool::apply_median(
unsigned char *src,
unsigned char *dst,
int width,
int height,
int i)
1194 FilterMedian filter(i);
1195 filter.set_src_buffer(src, roi, 0);
1196 filter.set_dst_buffer(dst, roi);
1206 MirrorCalibTool::apply_min(
unsigned char *src,
unsigned char *dst,
int width,
int height)
1210 filter.set_src_buffer(src, roi, 0);
1211 filter.set_dst_buffer(dst, roi);
1222 MirrorCalibTool::apply_or(
unsigned char *src1,
1223 unsigned char *src2,
1230 filter.set_src_buffer(src1, roi, 0);
1231 filter.set_src_buffer(src2, roi, 1);
1232 filter.set_dst_buffer(dst, roi);
1240 MirrorCalibTool::make_contrast(
unsigned char *buf,
size_t buflen)
1242 for (
unsigned int i = 0; i < buflen / 2; i++) {
1243 buf[i] = (buf[i] >= 75) ? BRIGHT : DARK;
1251 MirrorCalibTool::make_grayscale(
unsigned char *buf,
size_t buflen)
1253 memset(buf + buflen / 2, 128, buflen - buflen / 2);
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";
1277 MirrorCalibTool::MarkList
1278 MirrorCalibTool::premark(
const StepResult & prev,
1279 const unsigned char *yuv_mask,
1280 StepResult & result,
1282 const PixelPoint & center)
1284 const ConvexPolygon empty_polygon;
1285 return premark(empty_polygon, prev, yuv_mask, result, phi, center);
1291 MirrorCalibTool::MarkList
1292 MirrorCalibTool::premark(
const ConvexPolygon &polygon,
1293 const StepResult & prev,
1294 const unsigned char *yuv_mask,
1295 StepResult & result,
1297 const PixelPoint & center)
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;
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);
1309 if (length % 25 == 0) {
1310 width *= (1.0f - APPROX_LINE_WIDTH_LOSS);
1319 MirrorCalibTool::HoleList
1320 MirrorCalibTool::search_holes(
const MarkList &premarks)
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);
1330 prev_radius = radius;
1337 MirrorCalibTool::HoleList
1338 MirrorCalibTool::filter_biggest_holes(
const HoleList &holes,
unsigned int n)
1341 HoleList biggest = holes;
1342 # ifdef FILTER_MINI_HOLES
1344 for (HoleList::iterator it = biggest.begin(); it != biggest.end(); it++) {
1345 if (it->size() <= FALSE_POSITIVE_HOLE_SIZE) {
1353 for (
unsigned int from = 0; from < biggest.size(); from++) {
1355 for (to = from + 1; to < biggest.size(); to++) {
1356 if ((to - from + 1) > n) {
1360 if (biggest[to - 1].size() < biggest[to].size()) {
1365 if (to - from + 1 > filtered.size()) {
1367 for (
unsigned int j = from; j <= to; j++) {
1368 filtered.push_back(biggest[j]);
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) {
1383 if (biggest.size() == 1 && hole.size() > biggest.front().size()) {
1387 biggest.push_back(hole);
1388 if (biggest.size() == n) {
1397 MirrorCalibTool::MarkList
1398 MirrorCalibTool::determine_marks(
const HoleList &holes)
1400 HoleList biggest = filter_biggest_holes(holes, MARK_COUNT - 1);
1401 std::cout <<
"Filtered Holes: " << biggest.size() << std::endl;
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);
1409 const PolarRadius length = (hole.from_length + prev->to_length) / 2;
1410 marks.push_back(length);
1412 if (iter + 1 == biggest.end()) {
1413 const PolarRadius length = hole.to_length;
1414 marks.push_back(length);
1422 MirrorCalibTool::MarkList
1423 MirrorCalibTool::mark(
const MarkList & premarks,
1424 const unsigned char *yuv_mask,
1425 StepResult & result,
1427 const PixelPoint & center)
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;
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;
1446 MirrorCalibTool::goto_next_state()
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;
1455 state_.step = 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++;
1465 state_.step = PRE_MARKING;
1466 state_.centering_done =
false;
1467 state_.image_index = 0;
1471 state_.step = PRE_MARKING;
1472 state_.centering_done =
true;
1473 state_.image_index = 0;
1475 case PRE_MARKING: state_.step = FINAL_MARKING;
break;
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;
1487 state_.image_index = 0;
1492 state_.image_index = (state_.image_index + 1) % source_images_.size();
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()) {
1513 Image & src_img = source_images_[state_.image_index];
1515 switch (state_.step) {
1518 make_grayscale(result.yuv_buffer(), result.buflen());
1521 set_last_yuv_buffer(result.yuv_buffer());
1524 case EDGE_DETECTION: {
1526 const int offset = (src_img.
results().size() - 1) % ORIENTATION_COUNT;
1527 const orientation_t ori = static_cast<orientation_t>(ORI_DEG_0 + offset);
1529 make_grayscale(result.yuv_buffer(), result.buflen());
1530 make_contrast(result.yuv_buffer(), result.buflen());
1532 printf(
"Edge detection in %u\n", (
unsigned)src_img.
results().size());
1534 set_last_yuv_buffer(result.yuv_buffer());
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);
1547 assert(&prev1 != &prev2);
1548 assert(prev1.width() == prev2.
width());
1549 assert(prev1.height() == prev2.
height());
1551 prev1.yuv_buffer(), prev2.
yuv_buffer(), result.yuv_buffer(), prev1.width(), prev1.height());
1552 make_grayscale(result.yuv_buffer(), result.buflen());
1564 set_last_yuv_buffer(result.yuv_buffer());
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);
1579 set_last_yuv_buffer(result.yuv_buffer());
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);
1592 set_last_yuv_buffer(result.yuv_buffer());
1595 case FINAL_MARKING: {
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);
1602 const PolarAngle ori = src_img.
ori();
1603 std::cout <<
"Marking done for orientation " <<
rad2deg(ori) <<
" = "
1604 <<
rad2deg(imageRotationToRelativeOrientation(ori)) << std::endl;
1607 set_last_yuv_buffer(result.yuv_buffer());
1611 for (ImageList::iterator it = source_images_.begin(); it != source_images_.end(); it++) {
1614 StepResultList results = it->results();
1615 results.erase(results.begin() + 1, results.end());
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;
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());
1629 for (MarkMap::const_iterator map_iter = mark_map_.begin(); map_iter != mark_map_.end();
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();
1636 const PolarAngle angle = phi;
1637 const PolarRadius radius = *list_iter;
1640 printf(
"%3d (%d, %d)", radius, pp.
x, pp.
y);
1646 set_last_yuv_buffer(result.yuv_buffer());
1661 MirrorCalibTool::calculate_center(
const ImageList &images)
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);
1675 return PixelPoint(x / images.size(), y / images.size());
1703 MirrorCalibTool::PolarAnglePair
1704 MirrorCalibTool::find_nearest_neighbors(PolarAngle angle,
const MarkMap &mark_map)
1706 typedef std::vector<PolarAngle> AngleList;
1708 for (MarkMap::const_iterator it = mark_map.begin(); it != mark_map.end(); it++) {
1709 const PolarAngle mark_angle = it->first;
1711 diffs.push_back(diff);
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;
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;
1729 }
else if (!min2_init || fabs(diffs[min2_index]) >= fabs(diffs[i])) {
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) {
1746 }
else if (i == min2_index) {
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;
1761 return PolarAnglePair(min1, min2);
1769 MirrorCalibTool::RealDistance
1770 MirrorCalibTool::calculate_real_distance(
int n)
1772 return static_cast<int>(CARPET_DISTANCE_FROM_ROBOT_CENTER
1773 + static_cast<float>(n + 1) * MARK_DISTANCE);
1776 MirrorCalibTool::RealDistance
1777 MirrorCalibTool::interpolate(PolarRadius radius,
const MarkList &marks)
1779 if (marks.size() < 2) {
1782 for (MarkList::size_type i = 0; i < marks.size(); i++) {
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);
1791 std::cout <<
"A_Interpolate " << x <<
" between "
1792 << x0 <<
"->" << f0 <<
" "
1793 << x1 <<
"->" << f1 << std::endl;
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);
1805 std::cout <<
"B_Interpolate " << x <<
" between "
1806 << x0 <<
"->" << f0 <<
" "
1807 << x1 <<
"->" << f1 << std::endl;
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));
1826 MirrorCalibTool::generate(
int width,
int height,
const PixelPoint ¢er,
const MarkMap &mark_map)
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);
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;
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: "
1855 std::cout <<
"Neighbor 2: "
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)
1863 std::cout <<
"Weight 1: " << weight1 <<
" = " <<
rad2deg(weight1)
1865 std::cout <<
"Weight 2: " << weight2 <<
" = " <<
rad2deg(weight2)
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);
1876 std::cout <<
"Real 1 " << dist1 << std::endl;
1877 std::cout <<
"Real 2 " << dist2 << std::endl;
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;
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;
1892 if (world_dist_in_meters > 0) {
1893 bulb.setWorldPoint(x, y, world_dist_in_meters, world_phi_rel_to_robot);
1901 MirrorCalibTool::set_last_yuv_buffer(
const unsigned char *last_buf)
1903 last_yuv_buffer_ = last_buf;
1911 const unsigned char *
1914 return last_yuv_buffer_;
1927 printf(
"No bulb loaded, cannot evaluate.\n");
1933 *dist_ret = coord.
r;
1934 *ori_ret = coord.
phi;
1943 bulb_ =
new Bulb(filename);
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);
1958 std::cout <<
"Can't save in the middle of the calibration" << std::endl;
1982 relativeOrientationToImageRotation(0.0),
1984 for (PolarRadius length = 0; length < img.
max_radius(); length++) {
1987 img.
set_color(p, MARK_LUMA, MARK_CHROMINANCE);
1999 for (ImageList::const_iterator it = source_images_.begin(); it != source_images_.end(); it++) {
2000 const Image & src_img = *it;
2006 for (PolarRadius length = 0; length < img.max_radius(); length++) {
2007 const PolarAngle angle = 0.0;
2009 if (img.contains(p)) {
2010 img.set_color(p, MARK_LUMA, MARK_CHROMINANCE);
2034 const int POSITION_COUNT =
sizeof POSITIONS /
sizeof(double);
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++) {
2045 img.
set_color(p, MARK_LUMA, MARK_CHROMINANCE);