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
00034 float det = m00*m11 - m01*m10;
00035
00036
00037 if (fabs(det) < 1e-10)
00038 return std::pair<float,float>(-1,0);
00039
00040
00041 float i00 = m11/det;
00042
00043 float i01 = -m01/det;
00044
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
00081 float phi = 0.5f*std::atan2(-2*Cxy,(Cyy-Cxx));
00082
00083 std::pair<float,float> pts = std::pair<float,float>(Ex,Ey);
00084
00085
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
00102
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 }