00001 #include <Eigen/Dense>
00002
00003 #include "AprilTags/FloatImage.h"
00004 #include "AprilTags/MathUtil.h"
00005 #include "AprilTags/GLine2D.h"
00006 #include "AprilTags/Quad.h"
00007 #include "AprilTags/Segment.h"
00008
00009 namespace AprilTags {
00010
00011 const float Quad::maxQuadAspectRatio = 32;
00012
00013 Quad::Quad(const std::vector< std::pair<float,float> >& p, const std::pair<float,float>& opticalCenter)
00014 : quadPoints(p), segments(), observedPerimeter(), homography(opticalCenter) {
00015 #ifdef STABLE_H
00016 std::vector< std::pair<float,float> > srcPts;
00017 srcPts.push_back(std::make_pair(-1, -1));
00018 srcPts.push_back(std::make_pair( 1, -1));
00019 srcPts.push_back(std::make_pair( 1, 1));
00020 srcPts.push_back(std::make_pair(-1, 1));
00021 homography.setCorrespondences(srcPts, p);
00022 #else
00023 homography.addCorrespondence(-1, -1, quadPoints[0].first, quadPoints[0].second);
00024 homography.addCorrespondence( 1, -1, quadPoints[1].first, quadPoints[1].second);
00025 homography.addCorrespondence( 1, 1, quadPoints[2].first, quadPoints[2].second);
00026 homography.addCorrespondence(-1, 1, quadPoints[3].first, quadPoints[3].second);
00027 #endif
00028
00029 #ifdef INTERPOLATE
00030 p0 = Eigen::Vector2f(p[0].first, p[0].second);
00031 p3 = Eigen::Vector2f(p[3].first, p[3].second);
00032 p01 = (Eigen::Vector2f(p[1].first, p[1].second) - p0);
00033 p32 = (Eigen::Vector2f(p[2].first, p[2].second) - p3);
00034 #endif
00035 }
00036
00037 std::pair<float,float> Quad::interpolate(float x, float y) {
00038 #ifdef INTERPOLATE
00039 Eigen::Vector2f r1 = p0 + p01 * (x+1.)/2.;
00040 Eigen::Vector2f r2 = p3 + p32 * (x+1.)/2.;
00041 Eigen::Vector2f r = r1 + (r2-r1) * (y+1)/2;
00042 return std::pair<float,float>(r(0), r(1));
00043 #else
00044 return homography.project(x,y);
00045 #endif
00046 }
00047
00048 std::pair<float,float> Quad::interpolate01(float x, float y) {
00049 return interpolate(2*x-1, 2*y-1);
00050 }
00051
00052 void Quad::search(const FloatImage& fImage, std::vector<Segment*>& path,
00053 Segment& parent, int depth, std::vector<Quad>& quads,
00054 const std::pair<float,float>& opticalCenter) {
00055
00056
00057 if (depth == 4) {
00058
00059
00060
00061 if (path[4] == path[0]) {
00062
00063 std::vector< std::pair<float,float> > p(4);
00064 float calculatedPerimeter = 0;
00065 bool bad = false;
00066 for (int i = 0; i < 4; i++) {
00067
00068
00069 GLine2D linea(std::make_pair(path[i]->getX0(),path[i]->getY0()),
00070 std::make_pair(path[i]->getX1(),path[i]->getY1()));
00071 GLine2D lineb(std::make_pair(path[i+1]->getX0(),path[i+1]->getY0()),
00072 std::make_pair(path[i+1]->getX1(),path[i+1]->getY1()));
00073
00074 p[i] = linea.intersectionWith(lineb);
00075 calculatedPerimeter += path[i]->getLength();
00076
00077
00078 if (p[i].first == -1)
00079 bad = true;
00080 }
00081
00082
00083
00084 if (!bad) {
00085 float t0 = std::atan2(p[1].second-p[0].second, p[1].first-p[0].first);
00086 float t1 = std::atan2(p[2].second-p[1].second, p[2].first-p[1].first);
00087 float t2 = std::atan2(p[3].second-p[2].second, p[3].first-p[2].first);
00088 float t3 = std::atan2(p[0].second-p[3].second, p[0].first-p[3].first);
00089
00090
00091
00092 float ttheta = MathUtil::mod2pi(t1-t0) + MathUtil::mod2pi(t2-t1) +
00093 MathUtil::mod2pi(t3-t2) + MathUtil::mod2pi(t0-t3);
00094
00095
00096
00097 if (ttheta < -7 || ttheta > -5)
00098 bad = true;
00099 }
00100
00101 if (!bad) {
00102 float d0 = MathUtil::distance2D(p[0], p[1]);
00103 float d1 = MathUtil::distance2D(p[1], p[2]);
00104 float d2 = MathUtil::distance2D(p[2], p[3]);
00105 float d3 = MathUtil::distance2D(p[3], p[0]);
00106 float d4 = MathUtil::distance2D(p[0], p[2]);
00107 float d5 = MathUtil::distance2D(p[1], p[3]);
00108
00109
00110 if (d0 < Quad::minimumEdgeLength || d1 < Quad::minimumEdgeLength || d2 < Quad::minimumEdgeLength ||
00111 d3 < Quad::minimumEdgeLength || d4 < Quad::minimumEdgeLength || d5 < Quad::minimumEdgeLength) {
00112 bad = true;
00113
00114 }
00115
00116
00117 float dmax = max(max(d0,d1), max(d2,d3));
00118 float dmin = min(min(d0,d1), min(d2,d3));
00119
00120 if (dmax > dmin*Quad::maxQuadAspectRatio) {
00121 bad = true;
00122
00123 }
00124 }
00125
00126 if (!bad) {
00127 Quad q(p, opticalCenter);
00128 q.segments=path;
00129 q.observedPerimeter = calculatedPerimeter;
00130 quads.push_back(q);
00131 }
00132 }
00133 return;
00134 }
00135
00136
00137
00138
00139
00140 for (unsigned int i = 0; i < parent.children.size(); i++) {
00141 Segment &child = *parent.children[i];
00142
00143
00144
00145
00146
00147
00148
00149
00150 if ( child.getTheta() > path[0]->getTheta() ) {
00151
00152 continue;
00153 }
00154 path[depth+1] = &child;
00155 search(fImage, path, child, depth+1, quads, opticalCenter);
00156 }
00157 }
00158
00159 }