3dpcp/.svn/pristine/cb/cbf567ed06ca5921f1245475cddf406024731d50.svn-base

213 lines
6.9 KiB
Text
Raw Normal View History

2012-09-16 12:33:11 +00:00
/*
* clusterboundingbox implementation
*
* Copyright (C) Li Wei, Li Ming
*
* Released under the GPL version 3.
*
*/
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <algorithm>
#include "veloslam/debugview.h"
#include "veloslam/clusterboundingbox.h"
bool operator<(const BesiegePoint & s1,const BesiegePoint & s2)
{
return s1.angle < s2.angle;
}
BoundingBox::BoundingBox(void)
{
calCandidateBox=false;
}
BoundingBox::~BoundingBox(void)
{
}
void BoundingBox::Initial()
{
calCandidateBox=false;
allCandBox.clear();
minBoundingBox.clear();
}
void BoundingBox::CalCoordsAftRotation(double x,double y,double *newX,double *newY,double theta)
{
*newY=y*cosf(theta*M_PI/180)-x*sinf(theta*M_PI/180);
*newX=x*cosf(theta*M_PI/180)+y*sinf(theta*M_PI/180);
}
double BoundingBox::CalDirectionTwoPoints(double XPos1,double YPos1,double XPos2,double YPos2)
{
double delt_x=XPos2-XPos1;
double delt_y=YPos2-YPos1;
double angle;
double length=sqrt(delt_x*delt_x+delt_y*delt_y*1.0);
if(delt_x<0)
angle=360-acos(delt_y/length)*180/M_PI;
else
angle=acos(delt_y/length)*180/M_PI;
return angle;
}
void BoundingBox:: CalAllBoundingBox(cluster &gluClusterData)
{
int angleInterval=2;
int minCircumferenceIndex;
double minCircumference=10000000000;;
OrientedBoundingBox tempBoundingBox;
Initial();
for(int i=0;i<90;i+=angleInterval)
{
tempBoundingBox.newMaxXPointX=-1000000;
tempBoundingBox.newMaxYPointY=-1000000;
tempBoundingBox.newMinXPointX=1000000;
tempBoundingBox.newMinYPointY=1000000;
for (int j = 0; j <gluClusterData.size();j++)
{
cell* pcellObj= gluClusterData[j]->pCell;
for(int k=0;k<pcellObj->size();k++)
{
double newX,newY;
CalCoordsAftRotation((*pcellObj)[k]->x,(*pcellObj)[k]->y,&newX,&newY,i);
if(tempBoundingBox.newMaxXPointX<newX)
{
tempBoundingBox.newMaxXPointX=newX;
tempBoundingBox.newMaxXPointY=newY;
tempBoundingBox.boundingPointID[0].cellIndex=j;
tempBoundingBox.boundingPointID[0].pointIndexInCell=k;
}
if(tempBoundingBox.newMinXPointX>newX)
{
tempBoundingBox.newMinXPointX=newX;
tempBoundingBox.newMinXPointY=newY;
tempBoundingBox.boundingPointID[1].cellIndex=j;
tempBoundingBox.boundingPointID[1].pointIndexInCell=k;
}
if(tempBoundingBox.newMaxYPointY<newY)
{
tempBoundingBox.newMaxYPointY=newY;
tempBoundingBox.newMaxYPointX=newX;
tempBoundingBox.boundingPointID[2].cellIndex=j;
tempBoundingBox.boundingPointID[2].pointIndexInCell=k;
}
if(tempBoundingBox.newMinYPointY>newY)
{
tempBoundingBox.newMinYPointY=newY;
tempBoundingBox.newMinYPointX=newX;
tempBoundingBox.boundingPointID[3].cellIndex=j;
tempBoundingBox.boundingPointID[3].pointIndexInCell=k;
}
}
}
tempBoundingBox.length=tempBoundingBox.newMaxXPointX-tempBoundingBox.newMinXPointX;
tempBoundingBox.width=tempBoundingBox.newMaxYPointY-tempBoundingBox.newMinYPointY;
tempBoundingBox.area=(tempBoundingBox.newMaxXPointX-tempBoundingBox.newMinXPointX)*(tempBoundingBox.newMaxYPointY-tempBoundingBox.newMinYPointY);
tempBoundingBox.circumference=((tempBoundingBox.newMaxXPointX-tempBoundingBox.newMinXPointX)+(tempBoundingBox.newMaxYPointY-tempBoundingBox.newMinYPointY))*2;
tempBoundingBox.angle=i;
/**<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>ó<EFBFBD><C3B3>Ľ<EFBFBD><C4BD><EFBFBD><EFBFBD><EFBFBD>Y<EFBFBD><59><EFBFBD>ϱ<EFBFBD>X<EFBFBD><EFBFBD><E1B3A4>ͳһΪX<CEAA><58>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>**/
if(tempBoundingBox.newMaxXPointX-tempBoundingBox.newMinXPointX<
tempBoundingBox.newMaxYPointY- tempBoundingBox.newMinYPointY)
{
OrientedBoundingBox temp=tempBoundingBox;
tempBoundingBox.angle=temp.angle+90;
tempBoundingBox.newMaxXPointX=temp.newMaxYPointY;
tempBoundingBox.newMaxXPointY=-temp.newMaxYPointX;
tempBoundingBox.newMaxYPointX=temp.newMinXPointY;
tempBoundingBox.newMaxYPointY=-temp.newMinXPointX;
tempBoundingBox.newMinXPointX=temp.newMinYPointY;
tempBoundingBox.newMinXPointY=-temp.newMinYPointX;
tempBoundingBox.newMinYPointX=temp.newMaxXPointY;
tempBoundingBox.newMinYPointY=-temp.newMaxXPointX;
}
allCandBox.push_back(tempBoundingBox);
}
}
void BoundingBox::CalBestRectangleBox(cluster &gluClusterData,clusterFeature &glu)
{
int minCircumferenceIndex=0;
int bestBoxIndex;
if(!calCandidateBox)
CalAllBoundingBox(gluClusterData);
double minCircumference=allCandBox[0].circumference;
for(int i=0;i<allCandBox.size();i++)
{
if(allCandBox[i].circumference<minCircumference)
{
minCircumference=allCandBox[i].circumference;
minCircumferenceIndex=i;
}
}
bestBoxIndex=minCircumferenceIndex;
double clusterVex[4][2]={allCandBox[bestBoxIndex].newMaxXPointX,allCandBox[bestBoxIndex].newMaxYPointY,
allCandBox[bestBoxIndex].newMinXPointX,allCandBox[bestBoxIndex].newMaxYPointY,
allCandBox[bestBoxIndex].newMinXPointX,allCandBox[bestBoxIndex].newMinYPointY,
allCandBox[bestBoxIndex].newMaxXPointX,allCandBox[bestBoxIndex].newMinYPointY,
};
for(int i=0;i<4;i++)
{
CalCoordsAftRotation(clusterVex[i][0],clusterVex[i][1],
&glu.boxVex[i][0],&glu.boxVex[i][1],360-allCandBox[bestBoxIndex].angle);
}
glu.length=allCandBox[bestBoxIndex].length;
glu.width=allCandBox[bestBoxIndex].width;
glu.boxDirection=allCandBox[bestBoxIndex].angle;
Draw_Inclined_Cube_GL_RGB(glu.boxVex,glu.min_z,glu.max_z,0.0,0.0,1.0,3);
}
void BoundingBox::CalMinBoundingBox(cluster &gluClusterData,clusterFeature &glu)
{
if(!calCandidateBox)
CalAllBoundingBox(gluClusterData);
double minBisegeAverageX=0;
double minBisegeAverageY=0;
for(int i=0;i<allCandBox.size();i++)
{
for(int j=0;j<4;j++)
{
int cellIndex=allCandBox[i].boundingPointID[j].cellIndex;
int pointIndexInCell=allCandBox[i].boundingPointID[j].pointIndexInCell;
cell* pcellObj= gluClusterData[cellIndex]->pCell;
BesiegePoint tempPoint;
tempPoint.x=(*pcellObj)[pointIndexInCell]->x;
tempPoint.y=(*pcellObj)[pointIndexInCell]->y;
minBisegeAverageX+=tempPoint.x;
minBisegeAverageY+=tempPoint.y;
minBoundingBox.push_back(tempPoint);
}
}
minBisegeAverageX/=minBoundingBox.size();
minBisegeAverageY/=minBoundingBox.size();
for(int i=0;i<minBoundingBox.size();i++)
{
minBoundingBox[i].angle=CalDirectionTwoPoints(minBisegeAverageX,minBisegeAverageY,minBoundingBox[i].x,minBoundingBox[i].y);
}
sort(minBoundingBox.begin(),minBoundingBox.end());
for(int i=0;i<minBoundingBox.size()-1;i++)
Draw_Line_GL_RGB(minBoundingBox[i].x,minBoundingBox[i].y,-225,minBoundingBox[i+1].x,
minBoundingBox[i+1].y, -225,1,0,0,3);
Draw_Line_GL_RGB(minBoundingBox[0].x,minBoundingBox[0].y,-225,
minBoundingBox[minBoundingBox.size()-1].x,minBoundingBox[minBoundingBox.size()-1].y,-225,1,0,0,3);
}