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 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;
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++) {
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);
714 MirrorCalibTool::ConvexPolygon::ConvexPolygon()
725 MirrorCalibTool::ConvexPolygon::contains(
const CartesianImage& img,
741 MirrorCalibTool::ConvexPolygon::contains(
const PixelPoint& r)
const 743 for (std::vector<PixelPoint>::size_type i = 1; i <= size(); i++) {
746 double val = (q.
x - p.
x) * (r.
y - p.
y) - (r.
x - p.
x) * (q.
y - p.
y);
767 PolarRadius from_length,
768 PolarRadius to_length)
770 from_length(from_length),
778 inline PolarRadius
size()
const {
return to_length - from_length; }
792 Image(
const unsigned char* yuv_buffer,
797 : yuv_buffer_(new unsigned char[buflen]),
802 refcount_(new unsigned int)
804 memcpy(yuv_buffer_, yuv_buffer, buflen);
811 : yuv_buffer_(copy.yuv_buffer_),
812 buflen_(copy.buflen_),
814 height_(copy.height_),
816 results_(copy.results_),
817 premarks_(copy.premarks_),
819 refcount_(copy.refcount_)
831 if (--*refcount_ == 0) {
832 delete[] yuv_buffer_;
835 yuv_buffer_ = copy.yuv_buffer_;
836 buflen_ = copy.buflen_;
837 width_ = copy.width_;
838 height_ = copy.height_;
840 results_ = copy.results_;
841 premarks_ = copy.premarks_;
842 marks_ = copy.marks_;
843 refcount_ = copy.refcount_;
852 if (--*refcount_ == 0) {
853 delete[] yuv_buffer_;
865 inline const unsigned char*
yuv_buffer()
const {
return yuv_buffer_; }
869 inline size_t buflen()
const {
return buflen_; }
873 inline int width()
const {
return width_; }
877 inline int height()
const {
return height_; }
881 inline PolarAngle
ori()
const {
return ori_; }
885 inline StepResultList&
results() {
return results_; }
889 inline const StepResultList&
results()
const {
return results_; }
893 inline const MarkList&
premarks() {
return premarks_; }
897 inline MarkList&
marks() {
return marks_; }
901 inline const MarkList&
marks()
const {
return marks_; }
918 inline void set_premarks(
const MarkList& premarks) { premarks_ = premarks; }
921 inline void set_marks(
const MarkList& marks) { marks_ = marks; }
924 unsigned char* yuv_buffer_;
929 StepResultList results_;
932 unsigned int* refcount_;
937 MirrorCalibTool::MirrorCalibTool()
938 : img_yuv_buffer_(0),
942 state_(CalibrationState())
951 if (img_yuv_buffer_) {
952 delete[] img_yuv_buffer_;
955 delete[] img_yuv_mask_;
978 MirrorCalibTool::PolarAngle
979 MirrorCalibTool::relativeOrientationToImageRotation(PolarAngle ori)
992 MirrorCalibTool::PolarAngle
993 MirrorCalibTool::imageRotationToRelativeOrientation(PolarAngle ori)
1007 if (img_yuv_mask_) {
1008 delete[] img_yuv_mask_;
1011 size_t size = colorspace_buffer_size(reader.
colorspace(),
1014 img_yuv_mask_ =
new unsigned char[size];
1015 if (img_center_x_ == -1) {
1018 if (img_center_y_ == -1) {
1043 ori = relativeOrientationToImageRotation(ori);
1044 Image src_img(yuv_buffer, buflen, width, height, ori);
1045 if (img_center_x_ == -1) {
1046 img_center_x_ = width / 2;
1048 if (img_center_y_ == -1) {
1049 img_center_y_ = height / 2;
1051 source_images_.push_back(src_img);
1061 state_ = CalibrationState();
1062 source_images_.clear();
1075 MirrorCalibTool::apply_sobel(
unsigned char* src,
1095 MirrorCalibTool::apply_sharpen(
unsigned char* src,
1115 MirrorCalibTool::apply_median(
unsigned char* src,
1135 MirrorCalibTool::apply_min(
unsigned char* src,
1155 MirrorCalibTool::apply_or(
unsigned char* src1,
1156 unsigned char* src2,
1174 MirrorCalibTool::make_contrast(
unsigned char* buf,
1177 for (
unsigned int i = 0; i < buflen/2; i++) {
1178 buf[i] = (buf[i] >= 75) ? BRIGHT : DARK;
1187 MirrorCalibTool::make_grayscale(
unsigned char* buf,
1190 memset(buf + buflen/2, 128, buflen - buflen/2);
1200 switch (state_.step) {
1201 case SHARPENING:
return "Sharpen";
1202 case EDGE_DETECTION:
return "Edge detection";
1203 case COMBINATION:
return "Combining images";
1204 case CENTERING:
return "Finding center point";
1205 case PRE_MARKING:
return "First marking";
1206 case FINAL_MARKING:
return "Final marking";
1207 case DONE:
return "Direction done";
1208 default:
return "Invalid state";
1216 MirrorCalibTool::MarkList
1217 MirrorCalibTool::premark(
const StepResult& prev,
1218 const unsigned char* yuv_mask,
1223 const ConvexPolygon empty_polygon;
1224 return premark(empty_polygon, prev, yuv_mask, result, phi, center);
1231 MirrorCalibTool::MarkList
1232 MirrorCalibTool::premark(
const ConvexPolygon& polygon,
1234 const unsigned char* yuv_mask,
1241 int width = MIN_WIDTH_OF_BIGGEST_LINE;
1243 for (PolarRadius length = 0; length < prev_img.
max_radius(); length++)
1246 if (polygon.contains(prev_img, p) && prev_img.
is_line(p, width)) {
1247 premarks.push_back(length);
1250 if (length % 25 == 0) {
1251 width *= (1.0f - APPROX_LINE_WIDTH_LOSS);
1261 MirrorCalibTool::HoleList
1262 MirrorCalibTool::search_holes(
const MarkList& premarks)
1265 PolarRadius prev_radius = -1;
1266 for (MarkList::const_iterator it = premarks.begin();
1267 it != premarks.end(); it++)
1269 PolarRadius radius = *it;
1270 if (prev_radius != -1 && prev_radius + 1 < radius) {
1271 Hole hole(holes.size(), prev_radius, radius-1);
1272 holes.push_back(hole);
1274 prev_radius = radius;
1282 MirrorCalibTool::HoleList
1283 MirrorCalibTool::filter_biggest_holes(
const HoleList& holes,
1287 HoleList biggest = holes;
1288 #ifdef FILTER_MINI_HOLES 1290 for (HoleList::iterator it = biggest.begin(); it != biggest.end(); it++)
1292 if (it->size() <= FALSE_POSITIVE_HOLE_SIZE) {
1300 for (
unsigned int from = 0; from < biggest.size(); from++)
1303 for (to = from + 1; to < biggest.size(); to++) {
1304 if ((to - from + 1) > n) {
1308 if (biggest[to - 1].size() < biggest[to].size()) {
1313 if (to - from + 1 > filtered.size()) {
1315 for (
unsigned int j = from; j <= to; j++) {
1316 filtered.push_back(biggest[j]);
1323 for (HoleList::const_iterator it = holes.begin(); it != holes.end(); ++it)
1325 const Hole& hole = *it;
1326 #ifdef FILTER_MINI_HOLES 1327 if (hole.
size() < FALSE_POSITIVE_HOLE_SIZE) {
1332 if (biggest.size() == 1 && hole.
size() > biggest.front().size()) {
1336 biggest.push_back(hole);
1337 if (biggest.size() == n) {
1347 MirrorCalibTool::MarkList
1348 MirrorCalibTool::determine_marks(
const HoleList& holes)
1350 HoleList biggest = filter_biggest_holes(holes, MARK_COUNT - 1);
1351 std::cout <<
"Filtered Holes: " << biggest.size() << std::endl;
1353 for (HoleList::const_iterator prev, iter = biggest.begin();
1354 iter != biggest.end(); iter++) {
1355 const Hole& hole = *iter;
1356 if (iter == biggest.begin()) {
1358 marks.push_back(length);
1360 const PolarRadius length = (hole.
from_length + prev->to_length) / 2;
1361 marks.push_back(length);
1363 if (iter+1 == biggest.end()) {
1364 const PolarRadius length = hole.
to_length;
1365 marks.push_back(length);
1374 MirrorCalibTool::MarkList
1375 MirrorCalibTool::mark(
const MarkList& premarks,
1376 const unsigned char* yuv_mask,
1381 std::cout <<
"Premarked places: " << premarks.size() << std::endl;
1382 HoleList holes = search_holes(premarks);
1383 std::cout <<
"Found Holes: " << holes.size() << std::endl;
1384 MarkList marks = determine_marks(holes);
1385 std::cout <<
"Found Marks: " << marks.size() << std::endl;
1388 for (MarkList::const_iterator iter = marks.begin();
1389 iter != marks.end(); iter++)
1391 const PolarAngle angle = 0.0;
1392 const PolarRadius radius = *iter;
1394 std::cout <<
"Highlighting Mark at " << *iter << std::endl;
1401 MirrorCalibTool::goto_next_state()
1403 const Image& src_img = source_images_[state_.image_index];
1404 switch (state_.step) {
1406 state_.step = EDGE_DETECTION;
1408 case EDGE_DETECTION:
1409 if (src_img.
results().size() <= ORIENTATION_COUNT) {
1410 state_.step = EDGE_DETECTION;
1412 state_.step = COMBINATION;
1416 if (src_img.
results().size() < 2 * ORIENTATION_COUNT) {
1417 state_.step = COMBINATION;
1418 }
else if (state_.image_index + 1 < source_images_.size()) {
1419 state_.step = SHARPENING;
1420 state_.image_index++;
1422 state_.step = PRE_MARKING;
1423 state_.centering_done =
false;
1424 state_.image_index = 0;
1428 state_.step = PRE_MARKING;
1429 state_.centering_done =
true;
1430 state_.image_index = 0;
1433 state_.step = FINAL_MARKING;
1436 if (state_.image_index + 1 < source_images_.size()) {
1437 state_.step = PRE_MARKING;
1438 state_.image_index++;
1439 #ifdef RECALCULATE_CENTER_POINT 1440 }
else if (!state_.centering_done) {
1441 state_.step = CENTERING;
1442 state_.image_index = 0;
1446 state_.image_index = 0;
1451 state_.image_index = (state_.image_index + 1) % source_images_.size();
1467 printf(
"Performing step for image %u / %u, %s\n",
1468 (
unsigned int)state_.image_index + 1,
1469 (
unsigned int)source_images_.size(),
1471 if (state_.image_index >= source_images_.size()) {
1474 Image& src_img = source_images_[state_.image_index];
1476 switch (state_.step) {
1487 case EDGE_DETECTION:
1490 const int offset = (src_img.
results().size() - 1) % ORIENTATION_COUNT;
1491 const orientation_t ori =
static_cast<orientation_t
>(ORI_DEG_0+offset);
1497 printf(
"Edge detection in %u\n", (
unsigned) src_img.
results().size());
1507 const int index1 = src_img.
results().size() - ORIENTATION_COUNT == 1
1508 ? src_img.
results().size() - ORIENTATION_COUNT
1509 : src_img.
results().size() - 1;
1510 const int index2 = src_img.
results().size() - ORIENTATION_COUNT + 1;
1511 assert(index1 != index2);
1512 printf(
"ORing: %d = or(%d, %d)\n", (
unsigned) src_img.
results().size(),
1516 assert(&prev1 != &prev2);
1517 assert(prev1.width() == prev2.
width());
1518 assert(prev1.height() == prev2.
height());
1520 prev1.width(), prev1.height());
1543 const PixelPoint center = calculate_center(source_images_);
1544 img_center_x_ = center.x;
1545 img_center_y_ = center.y;
1546 std::cout <<
"Found center (" << center.x <<
", "<< center.y <<
")" 1548 CartesianImage img(result, relativeOrientationToImageRotation(0.0),
1560 const unsigned char* mask = img_yuv_mask_;
1561 const PixelPoint center(img_center_x_, img_center_y_);
1562 MarkList premarks = premark(prev, mask, result, src_img.
ori(), center);
1575 const unsigned char* mask = img_yuv_mask_;
1576 const PixelPoint center(img_center_x_, img_center_y_);
1577 const MarkList marks = mark(src_img.
premarks(), mask, result,
1578 src_img.
ori(), center);
1580 const PolarAngle ori = src_img.
ori();
1581 std::cout <<
"Marking done for orientation " 1584 <<
rad2deg(imageRotationToRelativeOrientation(ori))
1593 for (ImageList::iterator it = source_images_.begin();
1594 it != source_images_.end(); it++)
1598 StepResultList results = it->results();
1599 results.erase(results.begin() + 1, results.end());
1601 const Image& src_img = *it;
1602 const PolarAngle ori = src_img.
ori();
1603 MarkList marks = src_img.
marks();
1604 mark_map_[imageRotationToRelativeOrientation(ori)] = marks;
1608 const PixelPoint center(img_center_x_, img_center_y_);
1609 CartesianImage img(result, relativeOrientationToImageRotation(0.0),
1614 for (MarkMap::const_iterator map_iter = mark_map_.begin();
1615 map_iter != mark_map_.end(); map_iter++)
1617 const PolarAngle phi = map_iter->first;
1618 const MarkList& list = map_iter->second;
1619 printf(
"%3.f: ",
rad2deg(phi));
1620 for (MarkList::const_iterator list_iter = list.begin();
1621 list_iter != list.end(); list_iter++)
1623 const PolarAngle angle = phi;
1624 const PolarRadius radius = *list_iter;
1627 printf(
"%3d (%d, %d)", radius, pp.
x, pp.
y);
1649 MirrorCalibTool::calculate_center(
const ImageList& images)
1653 for (ImageList::const_iterator it = images.begin();
1654 it != images.end(); it++)
1656 const Image& image = *it;
1657 const PolarRadius radius = image.
marks().at(0);
1658 const unsigned char* null_buf = 0;
1662 const PixelPoint pixel = cart_image.to_pixel(point);
1666 return PixelPoint(x / images.size(), y / images.size());
1695 MirrorCalibTool::PolarAnglePair
1696 MirrorCalibTool::find_nearest_neighbors(PolarAngle angle,
1697 const MarkMap& mark_map)
1699 typedef std::vector<PolarAngle> AngleList;
1701 for (MarkMap::const_iterator it = mark_map.begin();
1702 it != mark_map.end(); it++)
1704 const PolarAngle mark_angle = it->first;
1706 diffs.push_back(diff);
1708 std::cout <<
"Finding nearest neighbors: " 1709 <<
"ref="<<angle<<
"="<<
rad2deg(angle)<<
" " 1710 <<
"to="<<mark_angle<<
"="<<
rad2deg(mark_angle)<<
" " 1711 <<
"diff="<<diff<<
"="<<
rad2deg(diff) << std::endl;
1714 bool min1_init =
false;
1715 AngleList::size_type min1_index = 0;
1716 bool min2_init =
false;
1717 AngleList::size_type min2_index = 0;
1718 for (
int i = 0;
static_cast<AngleList::size_type
>(i) < diffs.size(); i++) {
1719 if (!min1_init || fabs(diffs[min1_index]) >= fabs(diffs[i])) {
1720 min2_index = min1_index;
1721 min2_init = min1_init;
1724 }
else if (!min2_init || fabs(diffs[min2_index]) >= fabs(diffs[i])) {
1735 PolarAngle min1 = 0.0;
1736 PolarAngle min2 = 0.0;
1737 AngleList::size_type i = 0;
1738 for (MarkMap::const_iterator it = mark_map.begin();
1739 it != mark_map.end(); it++)
1741 if (i == min1_index) {
1743 }
else if (i == min2_index) {
1749 std::cout <<
"Found nearest neighbor 1: " 1750 <<
"ref="<<angle<<
"="<<
rad2deg(angle)<<
" " 1751 <<
"to="<<min1<<
"="<<
rad2deg(min1) <<
" " 1752 <<
"index="<<min1_index << std::endl;
1753 std::cout <<
"Found nearest neighbor 2: " 1754 <<
"ref="<<angle<<
"="<<
rad2deg(angle)<<
" " 1755 <<
"to="<<min2<<
"="<<
rad2deg(min2) <<
" " 1756 <<
"index="<<min2_index << std::endl;
1758 return PolarAnglePair(min1, min2);
1767 MirrorCalibTool::RealDistance
1768 MirrorCalibTool::calculate_real_distance(
int n)
1770 return static_cast<int>(CARPET_DISTANCE_FROM_ROBOT_CENTER +
1771 static_cast<float>(n + 1) * MARK_DISTANCE);
1775 MirrorCalibTool::RealDistance
1776 MirrorCalibTool::interpolate(PolarRadius radius,
1777 const MarkList& marks)
1779 if (marks.size() < 2) {
1782 for (MarkList::size_type i = 0; i < marks.size(); i++)
1785 if (i == 0 && radius < marks[i]) {
1786 const PolarRadius x0 = 0;
1787 const PolarRadius x1 = marks[i];
1788 const PolarRadius x = radius;
1789 const RealDistance f0 = 0;
1790 const RealDistance f1 = calculate_real_distance(i);
1792 std::cout <<
"A_Interpolate " << x <<
" between " 1793 << x0 <<
"->" << f0 <<
" " 1794 << x1 <<
"->" << f1 << std::endl;
1796 return static_cast<RealDistance
>(
static_cast<double>(f0) +
1797 static_cast<double>(x - x0) *
1798 static_cast<double>(f1 - f0) /
1799 static_cast<double>(x1 - x0));
1800 }
else if (i > 0 && marks[i-1] <= radius &&
1801 (radius < marks[i] || i+1 == marks.size())) {
1802 const PolarRadius x0 = marks[i-1];
1803 const PolarRadius x1 = marks[i];
1804 const PolarRadius x = radius;
1805 const RealDistance f0 = calculate_real_distance(i-1);
1806 const RealDistance f1 = calculate_real_distance(i);
1808 std::cout <<
"B_Interpolate " << x <<
" between " 1809 << x0 <<
"->" << f0 <<
" " 1810 << x1 <<
"->" << f1 << std::endl;
1812 return static_cast<RealDistance
>(
static_cast<double>(f0) +
1813 static_cast<double>(x - x0) *
1814 static_cast<double>(f1 - f0) /
1815 static_cast<double>(x1 - x0));
1831 MirrorCalibTool::generate(
int width,
1834 const MarkMap& mark_map)
1836 const unsigned char* null_img_buf = 0;
1838 relativeOrientationToImageRotation(0.0), center);
1839 Bulb bulb(width, height);
1842 for (
int x = 0; x < width; x++)
1844 for (
int y = 0; y < height; y++)
1848 const PolarAngle ori_to_robot = cp.
atan();
1849 const PolarAnglePair nearest_neighbors =
1850 find_nearest_neighbors(ori_to_robot, mark_map);
1851 const PolarAngle diff1 =
1853 const PolarAngle diff2 =
1855 const PolarAngle norm = diff1 + diff2;
1856 assert(norm != 0.0);
1857 const double weight1 = 1.0 - diff1 / norm;
1858 const double weight2 = 1.0 - diff2 / norm;
1860 std::cout <<
"PixelPoint("<< x <<
", "<< y <<
")"<< std::endl;
1861 std::cout <<
"CartesianPoint("<< cp.
x <<
", "<< cp.
y <<
")"<< std::endl;
1862 std::cout <<
"Radius, Angle: " << cp.
length() <<
" " 1863 << ori_to_robot <<
" = " 1864 <<
rad2deg(ori_to_robot) << std::endl;
1865 std::cout <<
"Neighbor 1: " 1869 std::cout <<
"Neighbor 2: " 1873 std::cout <<
"Diff 1: " << diff1 <<
" = " <<
rad2deg(diff1) << std::endl;
1874 std::cout <<
"Diff 2: " << diff2 <<
" = " <<
rad2deg(diff2) << std::endl;
1875 std::cout <<
"Norm Factor: " << norm <<
" = " <<
rad2deg(norm)
1877 std::cout <<
"Weight 1: " << weight1 <<
" = " <<
rad2deg(weight1)
1879 std::cout <<
"Weight 2: " << weight2 <<
" = " <<
rad2deg(weight2)
1882 assert(0.0 <= weight1 && weight1 <= 1.0);
1883 assert(0.0 <= weight2 && weight2 <= 1.0);
1884 assert(1.0 - 10e-5 < weight1 + weight2 && weight1 + weight2 < 1.0 + 10e-5);
1885 const MarkList& marks1 = mark_map.at(nearest_neighbors.first);
1886 const MarkList& marks2 = mark_map.at(nearest_neighbors.second);
1887 const RealDistance dist1 = interpolate(cp.
length(), marks1);
1888 const RealDistance dist2 = interpolate(cp.
length(), marks2);
1890 std::cout <<
"Real 1 " << dist1 << std::endl;
1891 std::cout <<
"Real 2 " << dist2 << std::endl;
1893 const RealDistance weighted_mean_dist = dist1 * weight1 + dist2 * weight2;
1894 const float world_dist_in_meters = weighted_mean_dist / 100.0f;
1895 const float world_phi_rel_to_robot = -1.0f * ori_to_robot;
1900 std::cout <<
"Dist 1: " << dist1 << std::endl;
1901 std::cout <<
"Dist 2: " << dist2 << std::endl;
1902 std::cout <<
"World Dist: " << world_dist_in_meters << std::endl;
1903 std::cout <<
"World Phi: " << ori_to_robot <<
" = " 1904 <<
rad2deg(ori_to_robot) << std::endl;
1906 if (world_dist_in_meters > 0) {
1907 bulb.
setWorldPoint(x, y, world_dist_in_meters, world_phi_rel_to_robot);
1916 MirrorCalibTool::set_last_yuv_buffer(
const unsigned char* last_buf)
1918 last_yuv_buffer_ = last_buf;
1927 const unsigned char*
1930 return last_yuv_buffer_;
1942 float* dist_ret,
float* ori_ret)
1945 printf(
"No bulb loaded, cannot evaluate.\n");
1951 *dist_ret = coord.
r;
1952 *ori_ret = coord.
phi;
1962 bulb_ =
new Bulb(filename);
1972 if (state_.step == DONE) {
1973 const Image& src_img = source_images_[state_.image_index];
1974 const PixelPoint center(img_center_x_, img_center_y_);
1975 Bulb bulb = generate(src_img.
width(), src_img.
height(), center, mark_map_);
1976 bulb.
save(filename);
1978 std::cout <<
"Can't save in the middle of the calibration" << std::endl;
2001 relativeOrientationToImageRotation(0.0),
2003 for (PolarRadius length = 0; length < img.
max_radius(); length++)
2007 img.
set_color(p, MARK_LUMA, MARK_CHROMINANCE);
2020 for (ImageList::const_iterator it = source_images_.begin();
2021 it != source_images_.end(); it++)
2023 const Image& src_img = *it;
2026 for (PolarRadius length = 0; length < img.
max_radius(); length++)
2028 const PolarAngle angle = 0.0;
2031 img.
set_color(p, MARK_LUMA, MARK_CHROMINANCE);
2056 const int POSITION_COUNT =
sizeof POSITIONS /
sizeof(double);
2058 relativeOrientationToImageRotation(0.0),
2060 for (
int i = 0; i < POSITION_COUNT; i++)
2062 const PolarAngle angle = POSITIONS[i];
2063 for (PolarRadius length = 0; length < img.
max_radius(); length++)
2067 img.
set_color(p, MARK_LUMA, MARK_CHROMINANCE);
unsigned char V
V component.
virtual void apply()
Apply the filter.
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).
Fawkes library namespace.
virtual fawkes::polar_coord_2d_t getWorldPointRelative(unsigned int image_x, unsigned int image_y) const
Get relative coordinate based on image coordinates.
virtual void apply()
Apply the filter.
unsigned char Y
Y component.
virtual colorspace_t colorspace()
Get colorspace from the just read image.
virtual void apply()
Apply the filter.
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 void apply()
Apply the filter.
virtual unsigned int pixel_height()
Get height of read image in pixels.
Bulb mirror lookup table.
virtual void setOrientation(float angle)
Set orientation of the omni-camera device.
float rad2deg(float rad)
Convert an angle given in radians to degrees.
static bool contains(Point_map points, Point_2 point, std::string &name, float near_threshold)
Check if a point is already contained in a map.
unsigned char U
U component.
virtual unsigned int pixel_width()
Get width of read image in pixels.
virtual void setCenter(unsigned int image_x, unsigned int image_y)
Set center of omni-camera to given image pixel.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
void save(const char *filename)
Save LUT from file.
virtual void setWorldPoint(unsigned int image_x, unsigned int image_y, float world_r, float world_phi)
Set a world point mapping.
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.