Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

GLine2D.cc

Go to the documentation of this file.
00001 #include "GLine2D.h"
00002 
00003 namespace AprilTags {
00004 
00005 GLine2D::GLine2D() 
00006   : dx(0), dy(0), p(0,0), didNormalizeSlope(false), didNormalizeP(false) {}
00007 
00008 GLine2D::GLine2D(float slope, float b) 
00009   : dx(1), dy(slope), p(0,b), didNormalizeSlope(false), didNormalizeP(false){}
00010 
00011 GLine2D::GLine2D(float dX, float dY, const std::pair<float,float>& pt) 
00012   : dx(dX), dy(dY), p(pt), didNormalizeSlope(false), didNormalizeP(false) {}
00013 
00014 GLine2D::GLine2D(const std::pair<float,float>& p1, const std::pair<float,float>& p2)
00015   : dx(p2.first - p1.first), dy(p2.second - p1.second), p(p1), didNormalizeSlope(false), didNormalizeP(false) {}
00016 
00017 float GLine2D::getLineCoordinate(const std::pair<float,float>& pt) {
00018   normalizeSlope();
00019   return pt.first*dx + pt.second*dy;
00020 }
00021 
00022 std::pair<float,float> GLine2D::getPointOfCoordinate(float coord) {
00023   normalizeP();
00024   return std::pair<float,float>(p.first + coord*dx, p.second + coord*dy);
00025 }
00026 
00027 std::pair<float,float> GLine2D::intersectionWith(const GLine2D& line) const {
00028   float m00 = dx;
00029   float m01 = -line.getDx();
00030   float m10 = dy;
00031   float m11 = -line.getDy();
00032 
00033   // determinant of 'm'
00034   float det = m00*m11 - m01*m10;
00035 
00036   // parallel lines? if so, return (-1,0).
00037   if (fabs(det) < 1e-10)
00038     return std::pair<float,float>(-1,0);
00039 
00040   // inverse of 'm'
00041   float i00 = m11/det;
00042   // float i11 = m00/det;
00043   float i01 = -m01/det;
00044   // float i10 = -m10/det;
00045 
00046   float b00 = line.getFirst() - p.first;
00047   float b10 = line.getSecond() - p.second;
00048 
00049   float x00 = i00*b00 + i01*b10;
00050 
00051   return std::pair<float,float>(dx*x00+p.first, dy*x00+p.second);
00052 }
00053 
00054 GLine2D GLine2D::lsqFitXYW(const std::vector<XYWeight>& xyweights) {
00055   float Cxx=0, Cyy=0, Cxy=0, Ex=0, Ey=0, mXX=0, mYY=0, mXY=0, mX=0, mY=0;
00056   float n=0;
00057 
00058   int idx = 0;
00059   for (unsigned int i = 0; i < xyweights.size(); i++) {
00060     float x = xyweights[i].x;
00061     float y = xyweights[i].y;
00062     float alpha = xyweights[i].weight;
00063 
00064     mY  += y*alpha;
00065     mX  += x*alpha;
00066     mYY += y*y*alpha;
00067     mXX += x*x*alpha;
00068     mXY += x*y*alpha;
00069     n   += alpha;
00070 
00071     idx++;
00072   }
00073   
00074   Ex  = mX/n;
00075   Ey  = mY/n;
00076   Cxx = mXX/n - MathUtil::square(mX/n);
00077   Cyy = mYY/n - MathUtil::square(mY/n);
00078   Cxy = mXY/n - (mX/n)*(mY/n);
00079 
00080   // find dominant direction via SVD
00081   float phi = 0.5f*std::atan2(-2*Cxy,(Cyy-Cxx));
00082   // float rho = Ex*cos(phi) + Ey*sin(phi); //why is this needed if he never uses it?
00083   std::pair<float,float> pts = std::pair<float,float>(Ex,Ey);
00084 
00085   // compute line parameters
00086   return GLine2D(-std::sin(phi), std::cos(phi), pts);
00087 }
00088 
00089 void GLine2D::normalizeSlope() {
00090   if ( !didNormalizeSlope ) {
00091     float mag = std::sqrt(dx*dx+dy*dy);
00092     dx /= mag;
00093     dy /= mag;
00094     didNormalizeSlope=true;
00095   }
00096 }
00097 
00098 void GLine2D::normalizeP() {
00099   if ( !didNormalizeP ) {
00100     normalizeSlope();
00101     // we already have a point (P) on the line, and we know the line vector U
00102     // and its perpendicular vector V: so, P'=P.*V *V
00103     float dotprod = -dy*p.first + dx*p.second;
00104     p = std::pair<float,float>(-dy*dotprod, dx*dotprod);
00105     didNormalizeP = true;
00106   }
00107 }
00108 
00109 } // namespace

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