pgiitu/BTech-Project-Robot-Localization- — Issue #1: Slope Comparison in GenHypothesis
Author: dapurv5
Created: 2012-03-07 03:20:51+00:00
State: open

list HypothesisGenerator::GenHypothesis(){
list hyps;

EdgeIterator mapIter = mapP.edges_begin();
EdgeIterator visIter = visP.edges_begin();

EdgeIterator mapIterCpy = mapP.edges_begin();
EdgeIterator visIterCpy = visP.edges_begin();

Segment mapEdge = *(mapIter);
Segment visEdge = *(visIter);

for(int i = 0; i < mapP.size(); i++)
{
visIter = visP.edges_begin();
visIterCpy = visP.edges_begin();
for(int j = 0; j < visP.size(); j++)
{
if(IsMatch(mapEdge, visEdge)) //SHOULDN'T SLOPE BE ALSO SAME HERE????????
{
Vector T(visEdge.point(0), mapEdge.point(0));
Vector invT(mapEdge.point(0), visEdge.point(0));

Transformation translate(CGAL::TRANSLATION, T);
Transformation invTranslate(CGAL::TRANSLATION, invT);

TranslatePolygon(translate, visP);

robotPos = translate(robotPos);

if(!IsInList(hyps,robotPos))
{
Polygon visibilityPolygon=pUtil.CalcVisibilityPolygon(mapP,robotPos);
if(pUtil.doPolygonsMatch(visibilityPolygon,visP))
{
hyps.push_back(robotPos);
}
}

TranslatePolygon(invTranslate, visP);
robotPos = invTranslate(robotPos);
}

visEdge = GetNextEdge(visIter, visP);
GetNextEdge(visIterCpy, visP);
}
mapEdge = GetNextEdge(mapIter, mapP);
GetNextEdge(mapIterCpy, mapP);
}
return hyps;
}

bool HypothesisGenerator::IsMatch(Segment mapEdge, Segment visEdge){
return (pUtil.EqualsValue(length(mapEdge),length(visEdge)) && pUtil.EqualsValue(orient(mapEdge),orient(visEdge)));
//	return (length(mapEdge) == length(visEdge) && orient(mapEdge) == orient(visEdge) );
}