MacroEdgeDetector.cpp
Go to the documentation of this file.
00001 #include "EdgeDetection/MacroEdgeDetector.h"
00002 
00003 #include "EdgeDetection/Vector2.h"
00004 #include "EdgeDetection/Edge.h"
00005 
00006 #include <vector>
00007 
00008 namespace EdgeDetection
00009 {
00010         Line MacroEdgeDetector::DetectEdge(Image* image, BoundedLine estimatedEdge)
00011         {
00012                 // Debug outputs
00013                 //cout << "Refining (" << estimatedEdge.GetStart().GetX() << ", " << estimatedEdge.GetStart().GetY() << ") -> (" << estimatedEdge.GetEnd().GetX() << ", " << estimatedEdge.GetEnd().GetY() << ")" << endl;
00014 
00015                 if (estimatedEdge.GetLength() < 3 * microEdgeDetector->GetSize()) throw "The estimated edge cannot be shorter than three times the window size.";
00016 
00017                 vector<Vector2> edgePoints;
00018 
00019                 double stepFraction = microEdgeDetector->GetSize() / estimatedEdge.GetLength();
00020                 double start = 0 + stepFraction;
00021                 double end = 1 - stepFraction;
00022 
00023                 for (double fraction = start; fraction <= end; fraction += stepFraction)
00024                 {
00025                         Vector2 position = estimatedEdge.GetPosition(fraction);
00026 
00027                         int windowLeft = Round(position.GetX() + 0.5 - 0.5 * microEdgeDetector->GetSize());
00028                         int windowRight = Round(position.GetX() + 0.5 + 0.5 * microEdgeDetector->GetSize());
00029                         int windowTop = Round(position.GetY() + 0.5 - 0.5 * microEdgeDetector->GetSize());
00030                         int windowBottom = Round(position.GetY() + 0.5 + 0.5 * microEdgeDetector->GetSize());
00031 
00032                         Image* windowRegion = image->GetRegion(windowLeft, windowRight, windowTop, windowBottom);
00033 
00034                         Edge measuredEdge = microEdgeDetector->DetectEdge(windowRegion);
00035                         Edge correctedEdge = microEdgeCorrectionTable->Correct(measuredEdge);
00036 
00037                         delete windowRegion;
00038 
00039                         Vector2 windowCenter = Vector2(0.5 * ((windowLeft - 0.5) + (windowRight - 0.5)), 0.5 * ((windowTop - 0.5) + (windowBottom - 0.5)));
00040                         Vector2 edgeCenter = Vector2::Add(windowCenter, Vector2::Multiply(0.5 * microEdgeDetector->GetSize(), correctedEdge.GetCenter()));
00041                         Vector2 correctedEdgeCenter = pointCorrector->CorrectPoint(edgeCenter);
00042 
00043                         // Debug outputs
00044 //                      cout << "(" << edgeCenter.GetX() << ", " << edgeCenter.GetY() << ") -> (" << correctedEdgeCenter.GetX() << ", " << correctedEdgeCenter.GetY() << ")" << endl;
00045 
00046                         edgePoints.push_back(correctedEdgeCenter);
00047 
00048                         // Debug outputs
00049 /*
00050                         cout << "Position: (" << position.GetX() << ", " << position.GetY() << ")" << endl;
00051                         cout << "Window: [" << windowLeft << ", " << windowRight << ", " << windowTop << ", " << windowBottom << "]" << endl;
00052                         cout << "Window Center: (" << windowCenter.GetX() << ", " << windowCenter.GetY() << ")" << endl;
00053                         cout << "Edge Center: (" << correctedEdge.GetCenter().GetX() << ", " << correctedEdge.GetCenter().GetY() << ")" << endl;
00054                         cout << "Measured: [" << measuredEdge.GetOffset() << ", " << measuredEdge.GetAngle() << "], Corrected: [" << correctedEdge.GetOffset() << ", " << correctedEdge.GetAngle() << "], Center: (" << edgeCenter.GetX() << ", " << edgeCenter.GetY() << ")" << endl;
00055 */
00056                 }
00057 
00058                 double averageX = 0;
00059                 for (vector<Vector2>::iterator edgePoint = edgePoints.begin(); edgePoint < edgePoints.end(); edgePoint++) averageX += edgePoint->GetX();
00060                 averageX /= edgePoints.size();
00061 
00062                 double averageY = 0;
00063                 for (vector<Vector2>::iterator edgePoint = edgePoints.begin(); edgePoint < edgePoints.end(); edgePoint++) averageY += edgePoint->GetY();
00064                 averageY /= edgePoints.size();
00065 
00066                 double momentXX = 0;
00067                 for (vector<Vector2>::iterator edgePoint = edgePoints.begin(); edgePoint < edgePoints.end(); edgePoint++) momentXX += (edgePoint->GetX() - averageX) * (edgePoint->GetX() - averageX);
00068                 momentXX /= edgePoints.size() - 1;
00069 
00070                 double momentYY = 0;
00071                 for (vector<Vector2>::iterator edgePoint = edgePoints.begin(); edgePoint < edgePoints.end(); edgePoint++) momentYY += (edgePoint->GetY() - averageY) * (edgePoint->GetY() - averageY);
00072                 momentYY /= edgePoints.size() - 1;
00073 
00074                 double momentXY = 0;
00075                 for (vector<Vector2>::iterator edgePoint = edgePoints.begin(); edgePoint < edgePoints.end(); edgePoint++) momentXY += (edgePoint->GetX() - averageX) * (edgePoint->GetY() - averageY);
00076                 momentXY /= edgePoints.size() - 1;
00077 
00078                 double a = momentXX - momentYY;
00079                 double b = momentXY + momentXY;
00080                 double c = a * a + b * b;
00081 
00082                 double factorX = - b * averageX + a * averageY - sqrt(c) * averageY;
00083                 double factorY = + b * averageY + a * averageX + sqrt(c) * averageX;
00084                 double offset = b * averageX * averageX - 2 * a * averageX * averageY - b * averageY * averageY;
00085 
00086                 // Debug outputs
00087 /*
00088                 Line estimatedLine = Line::Normalize(estimatedEdge);
00089                 Line fittedLine = Line::Normalize(Line(factorX, factorY, offset));
00090 
00091                 cout << "Estimated line: (" << estimatedLine.GetFactorX() << ") * x + (" << estimatedLine.GetFactorY() << ") * y + (" << estimatedLine.GetOffset() << ") = 0" << endl;
00092                 cout << "Fitted line: (" << fittedLine.GetFactorX() << ") * x + (" << fittedLine.GetFactorY() << ") * y + (" << fittedLine.GetOffset() << ") = 0" << endl;
00093 
00094                 double estimatedError = 0;
00095                 int estimatedAbove = 0;
00096                 int estimatedBelow = 0;
00097                 for (vector<Vector2>::iterator edgePoint = edgePoints.begin(); edgePoint < edgePoints.end(); edgePoint++)
00098                 {
00099                         estimatedError += abs(estimatedLine.GetSide(*edgePoint));
00100                         if (estimatedLine.GetSide(*edgePoint) >= 0) estimatedAbove++;
00101                         if (estimatedLine.GetSide(*edgePoint) < 0) estimatedBelow++;
00102                 }
00103                 double fittedError = 0;
00104                 int fittedAbove = 0;
00105                 int fittedBelow = 0;
00106                 for (vector<Vector2>::iterator edgePoint = edgePoints.begin(); edgePoint < edgePoints.end(); edgePoint++)
00107                 {
00108                         fittedError += abs(fittedLine.GetSide(*edgePoint));
00109                         if (fittedLine.GetSide(*edgePoint) >= 0) fittedAbove++;
00110                         if (fittedLine.GetSide(*edgePoint) < 0) fittedBelow++;
00111                 }
00112 
00113                 cout << "Estimated error: " << estimatedError << ", Estimated distribution: " << estimatedAbove << "/" << estimatedBelow << ", Fitted error: " << fittedError << ", Fitted distribution: " << fittedAbove << "/" << fittedBelow << endl;
00114 
00115                 return fittedLine;
00116 */
00117 
00118                 return Line(factorX, factorY, offset);
00119         }
00120 };


aruco_pose
Author(s): Julian Brunner
autogenerated on Mon Oct 6 2014 08:32:33