|
|
/*
|
|
|
* 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);
|
|
|
|
|
|
}
|