MicroEdgeCorrectionTable.cpp
Go to the documentation of this file.
00001 #include "EdgeDetection/MicroEdgeCorrectionTable.h"
00002 
00003 #include "EdgeDetection/Interpolation.h"
00004 
00005 #include <cmath>
00006 
00007 namespace EdgeDetection
00008 {
00009         Edge MicroEdgeCorrectionTable::Correct(Edge measuredEdge)
00010         {
00011                 double offsetIndex = Interpolation::InterpolateReverse(area.GetLeft(), area.GetRight(), measuredEdge.GetOffset()) * (offsetCount - 1);
00012                 int offsetIndexLow = floor(offsetIndex);
00013                 int offsetIndexHigh = ceil(offsetIndex);
00014                 double offsetFraction = offsetIndex - offsetIndexLow;
00015 
00016                 double angleIndex = Interpolation::InterpolateReverse(area.GetTop(), area.GetBottom(), measuredEdge.GetAngle()) * (angleCount - 1);
00017                 int angleIndexLow = floor(angleIndex);
00018                 int angleIndexHigh = ceil(angleIndex);
00019                 double angleFraction = angleIndex - angleIndexLow;
00020 
00021                 Edge leftTop = entries[offsetIndexLow * angleCount + angleIndexLow];
00022                 Edge rightTop = entries[offsetIndexHigh * angleCount + angleIndexLow];
00023                 Edge leftBottom = entries[offsetIndexLow * angleCount + angleIndexHigh];
00024                 Edge rightBottom = entries[offsetIndexHigh * angleCount + angleIndexHigh];
00025 
00026                 Edge offsetTop = Edge::Interpolate(leftTop, rightTop, offsetFraction);
00027                 Edge offsetBottom = Edge::Interpolate(leftBottom, rightBottom, offsetFraction);
00028                 Edge offsetAngle = Edge::Interpolate(offsetTop, offsetBottom, angleFraction);
00029 
00030                 // Debug outputs
00031 /*
00032                 cout << "OffsetFraction: " << offsetFraction << ", AngleFraction: " << angleFraction << endl;
00033                 cout << "Indices: [" << offsetIndexLow << ", " << offsetIndexHigh << "], [" << angleIndexLow << ", " << angleIndexHigh << "]" << endl;
00034                 cout << "LeftTop: [" << leftTop.GetOffset() << ", " << leftTop.GetAngle() << "], RightTop: [" << rightTop.GetOffset() << ", " << rightTop.GetAngle() << "], LeftBottom: [" << leftBottom.GetOffset() << ", " << leftBottom.GetAngle() << "], RightBottom: [" << rightBottom.GetOffset() << ", " << rightBottom.GetAngle() << "]" << endl;
00035                 cout << "OffsetTop: [" << offsetTop.GetOffset() << ", " << offsetTop.GetAngle() << "], OffsetBottom: [" << offsetBottom.GetOffset() << ", " << offsetBottom.GetAngle() << "], OffsetAngle: [" << offsetAngle.GetOffset() << ", " << offsetAngle.GetAngle() << "]" << endl;
00036 */
00037 
00038                 return offsetAngle;
00039         }
00040 
00041         void MicroEdgeCorrectionTable::Initialize(Rectangle area, int offsetCount, int angleCount)
00042         {
00043                 if (area.GetLeft() < -1 || area.GetRight() > +1 || area.GetTop() < -M_PI || area.GetBottom() > +M_PI) throw "The parameter 'area' was out of range.";
00044                 if (offsetCount <= 0) throw "The parameter 'offsetCount' was out of range.";
00045                 if (angleCount <= 0) throw "The parameter 'angleCount' was out of range.";
00046 
00047                 this->area = area;
00048                 this->offsetCount = offsetCount;
00049                 this->angleCount = angleCount;
00050                 this->entries = vector<Edge>();
00051         }
00052         void MicroEdgeCorrectionTable::PopulateTable(MicroEdgeMap* microEdgeMap)
00053         {
00054                 for (int offsetIndex = 0; offsetIndex < offsetCount; offsetIndex++)
00055                         for (double angleIndex = 0; angleIndex < angleCount; angleIndex++)
00056                         {
00057                                 double offset = GetOffsetOffset() + offsetIndex * GetOffsetStep();
00058                                 double angle = GetAngleOffset() + angleIndex * GetAngleStep();
00059 
00060                                 Edge measuredEdge = Edge(offset, angle);
00061                                 Edge realEdge = microEdgeMap->FindRealEdge(measuredEdge);
00062 
00063                                 entries.push_back(realEdge);
00064                         }
00065         }
00066 
00067         istream* MicroEdgeCorrectionTable::Serialize(MicroEdgeCorrectionTable* microEdgeCorrectionTable)
00068         {
00069                 stringstream* data = new stringstream(stringstream::in | stringstream::out | stringstream::binary);
00070 
00071                 double areaLeft = microEdgeCorrectionTable->area.GetLeft();
00072                 double areaRight = microEdgeCorrectionTable->area.GetRight();
00073                 double areaTop = microEdgeCorrectionTable->area.GetTop();
00074                 double areaBottom = microEdgeCorrectionTable->area.GetBottom();
00075                 int offsetCount = microEdgeCorrectionTable->offsetCount;
00076                 int angleCount = microEdgeCorrectionTable->angleCount;
00077 
00078                 data->write((const char*)&areaLeft, sizeof(double));
00079                 data->write((const char*)&areaRight, sizeof(double));
00080                 data->write((const char*)&areaTop, sizeof(double));
00081                 data->write((const char*)&areaBottom, sizeof(double));
00082                 data->write((const char*)&offsetCount, sizeof(int));
00083                 data->write((const char*)&angleCount, sizeof(int));
00084         
00085                 for (vector<Edge>::iterator edge = microEdgeCorrectionTable->entries.begin(); edge < microEdgeCorrectionTable->entries.end(); edge++)
00086                 {
00087                         double offset = edge->GetOffset();
00088                         double angle = edge->GetAngle();
00089 
00090                         data->write((const char*)&offset, sizeof(double));
00091                         data->write((const char*)&angle, sizeof(double));
00092                 }
00093 
00094                 return data;
00095         }
00096         MicroEdgeCorrectionTable* MicroEdgeCorrectionTable::Deserialize(istream* data)
00097         {
00098                 double areaLeft;
00099                 double areaRight;
00100                 double areaTop;
00101                 double areaBottom;
00102                 int offsetCount;
00103                 int angleCount;
00104 
00105                 data->read((char*)&areaLeft, sizeof(double));
00106                 data->read((char*)&areaRight, sizeof(double));
00107                 data->read((char*)&areaTop, sizeof(double));
00108                 data->read((char*)&areaBottom, sizeof(double));
00109                 data->read((char*)&offsetCount, sizeof(int));
00110                 data->read((char*)&angleCount, sizeof(int));
00111 
00112                 MicroEdgeCorrectionTable* microEdgeCorrectionTable = new MicroEdgeCorrectionTable(Rectangle(areaLeft, areaRight, areaTop, areaBottom), offsetCount, angleCount);
00113         
00114                 while (!data->eof())
00115                 {
00116                         double offset;
00117                         double angle;
00118                 
00119                         data->read((char*)&offset, sizeof(double));
00120                         data->read((char*)&angle, sizeof(double));
00121                 
00122                         microEdgeCorrectionTable->entries.push_back(Edge(offset, angle));
00123                 }
00124         
00125                 return microEdgeCorrectionTable;
00126         }
00127 };


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