Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

Quad.cc

Go to the documentation of this file.
00001 #include "Vision/AprilTags/FloatImage.h"
00002 #include "Vision/AprilTags/MathUtil.h"
00003 #include "Vision/AprilTags/GLine2D.h"
00004 #include "Vision/AprilTags/Quad.h"
00005 #include "Vision/AprilTags/Segment.h"
00006 
00007 namespace AprilTags {
00008   
00009 const float Quad::maxQuadAspectRatio = 32;
00010 
00011 Quad::Quad(const std::vector< std::pair<float,float> >& p, const std::pair<float,float>& opticalCenter)
00012   : quadPoints(p), segments(), observedPerimeter(), homography(opticalCenter)
00013 #ifdef QUAD_INTERPOLATE
00014   , p0(fmat::pack(p[0].first, p[0].second)), p3(fmat::pack(p[3].first, p[3].second)) ,p01(), p32()
00015 #endif
00016  {
00017 #ifdef QUAD_INTERPOLATE
00018   // borrowed from Michael Kaess' AprilTags library
00019   const fmat::Column<2,float> p1 = fmat::pack(p[1].first, p[1].second);
00020   const fmat::Column<2,float> p2 = fmat::pack(p[2].first, p[2].second);
00021   p01 = p1 - p0;
00022   p32 = p2 - p3;
00023 #endif
00024   // Keep homography even if interpolating, since we'll use this to
00025   // orient the AprilTag in the world map.  But the results might not
00026   // be very good because we're not using a good algorithm to find the
00027   // homography.  See Michael Kaess' version of Homography33 that
00028   // calls cv::findHomography for better results.
00029   homography.addCorrespondence(-1, -1, quadPoints[0].first, quadPoints[0].second);
00030   homography.addCorrespondence( 1, -1, quadPoints[1].first, quadPoints[1].second);
00031   homography.addCorrespondence( 1,  1, quadPoints[2].first, quadPoints[2].second);
00032   homography.addCorrespondence(-1,  1, quadPoints[3].first, quadPoints[3].second);
00033   homography.compute();
00034 }
00035 
00036 std::pair<float,float> Quad::interpolate(float x, float y) {
00037 #ifdef QUAD_INTERPOLATE
00038   // borrowed from Michael Kaess' AprilTags library
00039   fmat::Column<2,float> r1 = p0 + p01 * (x+1.0)/2.0;
00040   fmat::Column<2,float> r2 = p3 + p32 * (x+1.0)/2.0;
00041   fmat::Column<2,float> r = r1 + (r2-r1) * (y+1.0)/2.0;
00042   return std::pair<float,float>(r[0],r[1]);
00043 #else
00044   return homography.project(x,y);
00045 #endif
00046 }
00047 
00048 void Quad::search(const FloatImage& fImage, std::vector<Segment*>& path,
00049       Segment& parent, int depth, std::vector<Quad>& quads) {
00050   // cout << "Searching segment " << parent.getId() << ", depth=" << depth << ", #children=" << parent.children.size() << endl;
00051   // terminal depth occurs when we've found four segments.
00052   if (depth == 4) {
00053     // cout << "Entered terminal depth" << endl; // debug code
00054 
00055     // Is the first segment the same as the last segment (i.e., a loop?)
00056     if (path[4] == path[0]) {
00057       // the 4 corners of the quad as computed by the intersection of segments.
00058       std::vector< std::pair<float,float> > p(4);
00059       float calculatedPerimeter = 0;
00060       bool bad = false;
00061       for (int i = 0; i < 4; i++) {
00062   // compute intersections between all the lines. This will give us 
00063   // sub-pixel accuracy for the corners of the quad.
00064   GLine2D linea(std::make_pair(path[i]->getX0(),path[i]->getY0()),
00065           std::make_pair(path[i]->getX1(),path[i]->getY1()));
00066   GLine2D lineb(std::make_pair(path[i+1]->getX0(),path[i+1]->getY0()),
00067           std::make_pair(path[i+1]->getX1(),path[i+1]->getY1()));
00068 
00069   p[i] = linea.intersectionWith(lineb);
00070   calculatedPerimeter += path[i]->getLength();
00071 
00072   // no intersection? Occurs when the lines are almost parallel.
00073   if (p[i].first == -1)
00074     bad = true;
00075       }
00076       // cout << "bad = " << bad << endl;
00077       // eliminate quads that don't form a simply connected loop, i.e., those 
00078       // that form an hour glass, or wind the wrong way.
00079       if (!bad) {
00080     float t0 = std::atan2(p[1].second-p[0].second, p[1].first-p[0].first);
00081   float t1 = std::atan2(p[2].second-p[1].second, p[2].first-p[1].first);
00082   float t2 = std::atan2(p[3].second-p[2].second, p[3].first-p[2].first);
00083   float t3 = std::atan2(p[0].second-p[3].second, p[0].first-p[3].first);
00084 
00085   //  double ttheta = fmod(t1-t0, 2*M_PI) + fmod(t2-t1, 2*M_PI) +
00086   //    fmod(t3-t2, 2*M_PI) + fmod(t0-t3, 2*M_PI);
00087   float ttheta = MathUtil::mod2pi(t1-t0) + MathUtil::mod2pi(t2-t1) +
00088     MathUtil::mod2pi(t3-t2) + MathUtil::mod2pi(t0-t3);
00089   // cout << "ttheta=" << ttheta << endl;
00090   // the magic value is -2*PI. It should be exact, 
00091   // but we allow for (lots of) numeric imprecision.
00092   if (ttheta < -7 || ttheta > -5)
00093     bad = true;
00094       }
00095 
00096       if (!bad) {
00097   float d0 = MathUtil::distance2D(p[0], p[1]);
00098   float d1 = MathUtil::distance2D(p[1], p[2]);
00099   float d2 = MathUtil::distance2D(p[2], p[3]);
00100   float d3 = MathUtil::distance2D(p[3], p[0]);
00101   float d4 = MathUtil::distance2D(p[0], p[2]);
00102   float d5 = MathUtil::distance2D(p[1], p[3]);
00103 
00104   // check sizes
00105   if (d0 < Quad::minimumEdgeLength || d1 < Quad::minimumEdgeLength || d2 < Quad::minimumEdgeLength ||
00106       d3 < Quad::minimumEdgeLength || d4 < Quad::minimumEdgeLength || d5 < Quad::minimumEdgeLength) {
00107     bad = true;
00108     // cout << "tagsize too small" << endl;
00109   }
00110 
00111   // check aspect ratio
00112   float dmax = max(max(d0,d1), max(d2,d3));
00113   float dmin = min(min(d0,d1), min(d2,d3));
00114 
00115   if (dmax > dmin*Quad::maxQuadAspectRatio) {
00116     bad = true;
00117     // cout << "aspect ratio too extreme" << endl;
00118   }
00119       }
00120 
00121       if (!bad) {
00122   std::pair<float,float> opticalCenter(fImage.getWidth()/2, fImage.getHeight()/2);
00123   Quad q(p, opticalCenter);
00124   q.segments=path;
00125   q.observedPerimeter = calculatedPerimeter;
00126   quads.push_back(q);
00127       }
00128     }
00129     return;
00130   }
00131 
00132   //  if (depth >= 1) // debug code
00133   //cout << "depth: " << depth << endl;
00134 
00135   // Not terminal depth. Recurse on any children that obey the correct handedness.
00136   for (unsigned int i = 0; i < parent.children.size(); i++) {
00137     Segment &child = *parent.children[i];
00138     //    cout << "  Child " << child.getId() << ":  ";
00139     // (handedness was checked when we created the children)
00140     
00141     // we could rediscover each quad 4 times (starting from
00142     // each corner). If we had an arbitrary ordering over
00143     // points, we can eliminate the redundant detections by
00144     // requiring that the first corner have the lowest
00145     // value. We're arbitrarily going to use theta...
00146     if ( child.getTheta() > path[0]->getTheta() ) {
00147       // cout << "theta failed: " << child.getTheta() << " > " << path[0]->getTheta() << endl;
00148       continue;
00149     }
00150     path[depth+1] = &child;
00151     search(fImage, path, child, depth+1, quads);
00152   }
00153 }
00154 
00155 std::ostream& operator<<(std::ostream &os, const Quad &quad) {
00156   os << "Quad(";
00157   for (std::vector<std::pair<float,float> >::const_iterator it = quad.quadPoints.begin();
00158        it != quad.quadPoints.end(); it++) {
00159     if ( it != quad.quadPoints.begin() )
00160       os << ", ";
00161     os << "[" << int(round(it->first)) << "," << int(round(it->second)) << "]";
00162   }
00163   os << ")";
00164   return os;
00165 }
00166 
00167 } // namespace

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:49 2016 by Doxygen 1.6.3