AREXPORT std::map<int, ArLineFinderSegment *> *ArLineFinder::getLines(void)
{
// fill the laser readings into myPoints
fillPointsFromLaser();
// make lines out of myPoints into myLines
findLines();
// put the lines from myLines into combined lines
// this will recurse itself until there are no more
combineLines();
// now filter out the short lines
filterLines();
// combines the lines again
combineLines();
return myLines;
}
void CGraphFitting::mergeModels()
{
printf("Start Merging\n");
const int num=getValidNum();
int index=num-1;
for(int i=0;i<num-1;++i)
{
MyLine3D& mLine=getLine(i);
double diff=99999;
int curSel=i;
//printf("for line %d ,there are ",i);
for(int j=i+1;j<num-1;++j)
{
if(i==j) continue;
MyLine3D& nLine=getLine(j);
if(mLine.isSimilar(nLine))
{
double cur=mLine.getSimilarity(nLine);
if(diff>cur)
{
diff=cur;
curSel=j;
}
}
}
if(curSel!=i && diff<3)
{
MyLine3D line = combineLines(mLine,getLine(curSel));
if(line.isValid())
{
setModel(line,index++);
}
}
}
getLine(index).setValid(true);//background model
adjustModels();//readjust models
printf("Merge from %d to %d lines\n",num,getValidNum());
}
AREXPORT bool ArLineFinder::combineLines(void)
{
int start = 0;
int len = myLines->size();
// this is the min line distance
std::map<int, ArLineFinderSegment *> *newLines;
int numNewLines = 0;
int numNewMerges = 0;
ArLineFinderSegment *newLine;
newLines = new std::map<int, ArLineFinderSegment *>;
if (myPrinting)
ArLog::log(ArLog::Normal, "new iteration\n");
bool nextMerged = false;
for (start = 0; start < len; start++)
{
if (nextMerged)
{
nextMerged = false;
continue;
}
if (start + 1 == len)
{
if (myPrinting)
ArLog::log(ArLog::Normal, "inserted last one %g",
ArPose((*myLines)[start]->getX1(),
(*myLines)[start]->getY1()).findDistanceTo(
ArPose((*myLines)[start]->getX2(), (*myLines)[start]->getY2())));
(*newLines)[numNewLines] = new ArLineFinderSegment(*((*myLines)[start]));
numNewLines++;
continue;
}
newLine = averageSegments((*myLines)[start], (*myLines)[start+1]);
if (newLine != NULL)
{
if (myPrinting)
ArLog::log(ArLog::Normal, "merged %g %g to %g",
(*myLines)[start]->getLength(),
(*myLines)[start+1]->getLength(),
newLine->getLength());
(*newLines)[numNewLines] = newLine;
numNewLines++;
numNewMerges++;
nextMerged = true;
}
else
{
if (myPrinting)
ArLog::log(ArLog::Normal, "inserted anyways %g",
(*myLines)[start]->getLength());
(*newLines)[numNewLines] = new ArLineFinderSegment(*((*myLines)[start]));
numNewLines++;
}
}
// move the new lines over and delete the old ones
if (myLines != NULL && myLines->begin() != myLines->end())
{
ArUtil::deleteSetPairs(myLines->begin(), myLines->end());
delete myLines;
myLines = NULL;
}
else if (myLines != NULL)
{
delete myLines;
myLines = NULL;
}
myLines = newLines;
// if we didn't merge any just return
if (numNewMerges == 0)
return true;
// otherwise do it again
return combineLines();
}
