Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

Edge.cc

Go to the documentation of this file.
00001 #include "Vision/AprilTags/Edge.h"
00002 #include "Vision/AprilTags/FloatImage.h"
00003 #include "Vision/AprilTags/MathUtil.h"
00004 #include "Vision/AprilTags/UnionFindSimple.h"
00005 
00006 namespace AprilTags {
00007 
00008 float const Edge::minMag = 0.004f;
00009 float const Edge::maxEdgeCost = 30.f * float(M_PI) / 180.f;
00010 int const Edge::WEIGHT_SCALE = 100;
00011 float const Edge::thetaThresh = 100;
00012 float const Edge::magThresh = 1200;
00013 
00014 short int Edge::edgeCost(float  theta0, float theta1, float mag1) {
00015   if (mag1 < minMag)  // mag0 was checked by the main routine so no need to recheck here
00016     return -1;
00017 
00018   const float thetaErr = std::abs(MathUtil::mod2pi(theta1 - theta0));
00019   if (thetaErr > maxEdgeCost)
00020     return -1;
00021 
00022   const float normErr = thetaErr / maxEdgeCost;
00023   return (short int) (normErr*WEIGHT_SCALE);
00024 }
00025 
00026 void Edge::calcEdges(float theta0, int x, int y,
00027          const FloatImage& theta, const FloatImage& mag,
00028          std::vector<Edge> &edges, size_t &nEdges) {
00029   int width = theta.getWidth();
00030   int thisPixel = y*width+x;
00031 
00032   // horizontal edge
00033   short int cost1 = edgeCost(theta0, theta.get(x+1,y), mag.get(x+1,y));
00034   if (cost1 >= 0) {
00035     edges[nEdges].cost = cost1;
00036     edges[nEdges].pixelIdxA = thisPixel;
00037     edges[nEdges].pixelIdxB = y*width+x+1;
00038     ++nEdges;
00039   }
00040 
00041   // vertical edge
00042   short int cost2 = edgeCost(theta0, theta.get(x, y+1), mag.get(x,y+1));
00043   if (cost2 >= 0) {
00044     edges[nEdges].cost = cost2;
00045     edges[nEdges].pixelIdxA = thisPixel;
00046     edges[nEdges].pixelIdxB = (y+1)*width+x;
00047     ++nEdges;
00048   }
00049   
00050   // downward diagonal edge
00051   short int cost3 = edgeCost(theta0, theta.get(x+1, y+1), mag.get(x+1,y+1));
00052   if (cost3 >= 0) {
00053     edges[nEdges].cost = cost3;
00054     edges[nEdges].pixelIdxA = thisPixel;
00055     edges[nEdges].pixelIdxB = (y+1)*width+x+1;
00056     ++nEdges;
00057   }
00058 
00059   // updward diagonal edge
00060   short int cost4 = (x == 0) ? -1 : edgeCost(theta0, theta.get(x-1, y+1), mag.get(x-1,y+1));
00061   if (cost4 >= 0) {
00062     edges[nEdges].cost = cost4;
00063     edges[nEdges].pixelIdxA = thisPixel;
00064     edges[nEdges].pixelIdxB = (y+1)*width+x-1;
00065     ++nEdges;
00066   }
00067 }
00068 
00069 void Edge::mergeEdges(std::vector<Edge> &edges, UnionFindSimple &uf,
00070           float tmin[], float tmax[], float mmin[], float mmax[]) {
00071   for (size_t i = 0; i < edges.size(); i++) {
00072     int ida = edges[i].pixelIdxA;
00073     int idb = edges[i].pixelIdxB;
00074 
00075     ida = uf.getRepresentative(ida);
00076     idb = uf.getRepresentative(idb);
00077       
00078     if (ida == idb)
00079       continue;
00080 
00081     int sza = uf.getSetSize(ida);
00082     int szb = uf.getSetSize(idb);
00083 
00084     float tmina = tmin[ida], tmaxa = tmax[ida];
00085     float tminb = tmin[idb], tmaxb = tmax[idb];
00086 
00087     float costa = (tmaxa-tmina);
00088     float costb = (tmaxb-tminb);
00089 
00090     // bshift will be a multiple of 2pi that aligns the spans of 'b' with 'a'
00091     // so that we can properly take the union of them.
00092     float bshift = MathUtil::mod2pi((tmina+tmaxa)/2, (tminb+tmaxb)/2) - (tminb+tmaxb)/2;
00093 
00094     float tminab = min(tmina, tminb + bshift);
00095     float tmaxab = max(tmaxa, tmaxb + bshift);
00096 
00097     if (tmaxab-tminab > 2*(float)M_PI) // corner case that's probably not too useful to handle correctly, oh well.
00098       tmaxab = tminab + 2*(float)M_PI;
00099 
00100     float mminab = min(mmin[ida], mmin[idb]);
00101     float mmaxab = max(mmax[ida], mmax[idb]);
00102 
00103     // merge these two clusters?
00104     float costab = (tmaxab - tminab);
00105     if (costab <= (min(costa, costb) + Edge::thetaThresh/(sza+szb)) &&
00106   (mmaxab-mminab) <= min(mmax[ida]-mmin[ida], mmax[idb]-mmin[idb]) + Edge::magThresh/(sza+szb)) {
00107   
00108       int idab = uf.connectNodes(ida, idb);
00109   
00110       tmin[idab] = tminab;
00111       tmax[idab] = tmaxab;
00112   
00113       mmin[idab] = mminab;
00114       mmax[idab] = mmaxab;
00115     }
00116   }
00117 }
00118 
00119 } // namespace

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