653 lines
12 KiB
C++
653 lines
12 KiB
C++
/*
|
||
* intersection_detection implementation
|
||
*
|
||
* Copyright (C) Chen Long, Li Wei, Li Ming, Andreas Nuechter,
|
||
*
|
||
* Released under the GPL version 3.
|
||
*
|
||
*/
|
||
|
||
#include "veloslam/intersection_detection.h"
|
||
#include "veloslam/veloscan.h"
|
||
#include <iostream>
|
||
#include <fstream>
|
||
#define DefaultColumnSize 360
|
||
|
||
svm_model *m = svm_load_model("SegIter.model");
|
||
svm_node *nod=new svm_node[361];
|
||
|
||
IntersectionDetection::IntersectionDetection()
|
||
{
|
||
slashWide=200;
|
||
slashMaxLength=3500;
|
||
slashAngleDefinition=1;
|
||
|
||
columnNum= 360;
|
||
cellSize= 50;
|
||
MaxRad=6000;
|
||
if(MaxRad%cellSize!=0)
|
||
MaxRad=(MaxRad/cellSize+1)*cellSize;
|
||
cellNum=MaxRad/cellSize;
|
||
MinRad=0;
|
||
|
||
// CalcRadAndTheta();
|
||
}
|
||
|
||
IntersectionDetection::~IntersectionDetection()
|
||
{
|
||
|
||
|
||
}
|
||
|
||
int IntersectionDetection::GetPointData()
|
||
{
|
||
for(unsigned int i = 0; i < Scan::allScans.size(); i++)
|
||
for (int j = 0; j < Scan::allScans[i]->get_points_red_size(); j++)
|
||
{
|
||
Point p;
|
||
p.x=Scan::allScans[i]->get_points_red()[j][0];
|
||
p.y=Scan::allScans[i]->get_points_red()[j][1];
|
||
p.z=Scan::allScans[i]->get_points_red()[j][2];
|
||
allPoints_AfterRegstn.push_back(p);
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
int IntersectionDetection::CalcRadAndTheta()
|
||
{
|
||
|
||
GetPointData();
|
||
int i,j;
|
||
int size= allPoints_AfterRegstn.size();
|
||
|
||
for(i=0; i< size; ++i)
|
||
{
|
||
allPoints_AfterRegstn[i].rad = sqrt( allPoints_AfterRegstn[i].x*allPoints_AfterRegstn[i].x
|
||
+ allPoints_AfterRegstn[i].z*allPoints_AfterRegstn[i].z );
|
||
allPoints_AfterRegstn[i].tan_theta = allPoints_AfterRegstn[i].z/allPoints_AfterRegstn[i].x;
|
||
}
|
||
return 0;
|
||
|
||
}
|
||
|
||
|
||
int IntersectionDetection::CalPointCellPos(double x,double y,double z ,int * column,int * row)
|
||
{
|
||
int i,j,count=0;
|
||
float flag;
|
||
int offset;
|
||
int sectionSize=columnNum/8;
|
||
float inc=(M_PI*2)/columnNum;
|
||
|
||
vector<float> tanv;
|
||
vector<float>::iterator result;
|
||
|
||
for(i=0; i<sectionSize; ++i)
|
||
{
|
||
tanv.push_back(tan(inc*i));
|
||
}
|
||
double rad=sqrt(x*x+z*z);
|
||
double tan_theta=z/x;
|
||
|
||
int diff;
|
||
if(x >0 && z>0 )
|
||
{
|
||
if(x >z)
|
||
{
|
||
flag=tan_theta;
|
||
result=upper_bound(tanv.begin(),tanv.end(),flag);
|
||
if(result==tanv.end())
|
||
{
|
||
offset=sectionSize-1;
|
||
}
|
||
else
|
||
{
|
||
offset=result-tanv.begin();
|
||
}
|
||
}
|
||
else
|
||
{
|
||
flag=1/tan_theta;
|
||
result=upper_bound(tanv.begin(),tanv.end(),flag);
|
||
if(result==tanv.end())
|
||
{
|
||
offset=sectionSize;
|
||
}
|
||
else
|
||
{
|
||
diff=result-tanv.begin();
|
||
offset=sectionSize*2-1-(diff);
|
||
}
|
||
|
||
}
|
||
}
|
||
|
||
else if(x<0 && z>0)
|
||
{
|
||
if(-x>z)
|
||
{
|
||
flag=-tan_theta;
|
||
result=upper_bound(tanv.begin(),tanv.end(),flag);
|
||
if(result==tanv.end())
|
||
{
|
||
offset=sectionSize*3;
|
||
}
|
||
else
|
||
{
|
||
offset=sectionSize*4-1-(result-tanv.begin());
|
||
}
|
||
|
||
}
|
||
else
|
||
{
|
||
flag=1/-tan_theta;
|
||
result=upper_bound(tanv.begin(),tanv.end(),flag);
|
||
if(result==tanv.end())
|
||
{
|
||
offset=sectionSize*3-1;
|
||
}
|
||
else
|
||
{
|
||
offset=sectionSize*2+(result-tanv.begin());
|
||
}
|
||
}
|
||
}
|
||
|
||
else if(x<0&&z<0)
|
||
{
|
||
if(-x>-z)
|
||
{
|
||
flag=tan_theta;
|
||
result=upper_bound(tanv.begin(),tanv.end(),flag);
|
||
if(result==tanv.end())
|
||
{
|
||
offset=sectionSize*5-1;
|
||
}
|
||
else
|
||
{
|
||
offset=sectionSize*4+(result-tanv.begin());
|
||
}
|
||
}
|
||
else
|
||
{
|
||
flag=1/tan_theta;
|
||
result=upper_bound(tanv.begin(),tanv.end(),flag);
|
||
if(result==tanv.end())
|
||
{
|
||
offset=sectionSize*5;
|
||
}
|
||
else
|
||
{
|
||
offset=sectionSize*6-1-(result-tanv.begin());
|
||
}
|
||
|
||
}
|
||
}
|
||
|
||
else if(x>0&&z<0)
|
||
{
|
||
if(x>-z)
|
||
{
|
||
flag=-tan_theta;
|
||
result=upper_bound(tanv.begin(),tanv.end(),flag);
|
||
if(result==tanv.end())
|
||
{
|
||
offset=sectionSize*7;
|
||
}
|
||
else
|
||
{
|
||
offset=sectionSize*8-1-(result-tanv.begin());
|
||
}
|
||
}
|
||
else
|
||
{
|
||
flag=1/-tan_theta;
|
||
result=upper_bound(tanv.begin(),tanv.end(),flag);
|
||
if(result==tanv.end())
|
||
{
|
||
offset=sectionSize*7-1;
|
||
}
|
||
else
|
||
{
|
||
offset=sectionSize*6+(result-tanv.begin());
|
||
}
|
||
}
|
||
}
|
||
|
||
int k= (int)((rad-MinRad)/(cellSize*1.0));
|
||
* column=offset;
|
||
* row=k;
|
||
return 0;
|
||
}
|
||
|
||
|
||
int IntersectionDetection::CalCellMinDis()
|
||
{
|
||
for(int i=0;i<cellNum;i++)
|
||
{
|
||
float halfCellAngle=180.0/columnNum;
|
||
float minCellDis=2*( i*cellSize *sin(halfCellAngle));
|
||
|
||
if(minCellDis>cellSize)
|
||
minCellDis=cellSize;
|
||
minCellDisList.push_back(minCellDis);
|
||
}
|
||
minCellDisList[0]=minCellDisList[1];
|
||
|
||
return 0;
|
||
}
|
||
|
||
|
||
int IntersectionDetection::TransferToCellArray()
|
||
{
|
||
|
||
int i,j,count=0;
|
||
float flag;
|
||
int offset;
|
||
int sectionSize=columnNum/8;
|
||
|
||
|
||
cellArray_AfterRegstn.resize(columnNum);
|
||
for(i=0; i<columnNum; ++i)
|
||
cellArray_AfterRegstn[i].resize(cellNum);
|
||
|
||
float inc=(M_PI*2)/columnNum;
|
||
|
||
vector<float> tanv;
|
||
vector<float>::iterator result;
|
||
|
||
for(i=0; i<sectionSize; ++i)
|
||
{
|
||
tanv.push_back(tan(inc*i));
|
||
}
|
||
|
||
int diff;
|
||
for(i=0; i< allPoints_AfterRegstn.size(); ++i)
|
||
{
|
||
count++;
|
||
Point &pt= allPoints_AfterRegstn[i];
|
||
|
||
if(pt.rad <=MinRad || pt.rad>=MaxRad)
|
||
continue;
|
||
|
||
if(pt.x >0 && pt.z>0 )
|
||
{
|
||
if(pt.x > pt.z)
|
||
{
|
||
flag=pt.tan_theta;
|
||
result=upper_bound(tanv.begin(),tanv.end(),flag);
|
||
if(result==tanv.end())
|
||
{
|
||
offset=sectionSize-1;
|
||
}
|
||
else
|
||
{
|
||
offset=result-tanv.begin();
|
||
}
|
||
}
|
||
else
|
||
{
|
||
flag=1/pt.tan_theta;
|
||
result=upper_bound(tanv.begin(),tanv.end(),flag);
|
||
if(result==tanv.end())
|
||
{
|
||
offset=sectionSize;
|
||
}
|
||
else
|
||
{
|
||
diff=result-tanv.begin();
|
||
offset=sectionSize*2-1-(diff);
|
||
}
|
||
|
||
}
|
||
}
|
||
|
||
else if(pt.x<0 && pt.z>0)
|
||
{
|
||
if(-pt.x>pt.z)
|
||
{
|
||
flag=-pt.tan_theta;
|
||
result=upper_bound(tanv.begin(),tanv.end(),flag);
|
||
if(result==tanv.end())
|
||
{
|
||
offset=sectionSize*3;
|
||
}
|
||
else
|
||
{
|
||
offset=sectionSize*4-1-(result-tanv.begin());
|
||
}
|
||
|
||
}
|
||
else
|
||
{
|
||
flag=1/-pt.tan_theta;
|
||
result=upper_bound(tanv.begin(),tanv.end(),flag);
|
||
if(result==tanv.end())
|
||
{
|
||
offset=sectionSize*3-1;
|
||
}
|
||
else
|
||
{
|
||
offset=sectionSize*2+(result-tanv.begin());
|
||
}
|
||
}
|
||
}
|
||
|
||
else if(pt.x<0&&pt.z<0)
|
||
{
|
||
if(-pt.x>-pt.z)
|
||
{
|
||
flag=pt.tan_theta;
|
||
result=upper_bound(tanv.begin(),tanv.end(),flag);
|
||
if(result==tanv.end())
|
||
{
|
||
offset=sectionSize*5-1;
|
||
}
|
||
else
|
||
{
|
||
offset=sectionSize*4+(result-tanv.begin());
|
||
}
|
||
}
|
||
else
|
||
{
|
||
flag=1/pt.tan_theta;
|
||
result=upper_bound(tanv.begin(),tanv.end(),flag);
|
||
if(result==tanv.end())
|
||
{
|
||
offset=sectionSize*5;
|
||
}
|
||
else
|
||
{
|
||
offset=sectionSize*6-1-(result-tanv.begin());
|
||
}
|
||
|
||
}
|
||
}
|
||
|
||
else if(pt.x>0&&pt.z<0)
|
||
{
|
||
if(pt.x>-pt.z)
|
||
{
|
||
flag=-pt.tan_theta;
|
||
result=upper_bound(tanv.begin(),tanv.end(),flag);
|
||
if(result==tanv.end())
|
||
{
|
||
offset=sectionSize*7;
|
||
}
|
||
else
|
||
{
|
||
offset=sectionSize*8-1-(result-tanv.begin());
|
||
}
|
||
}
|
||
else
|
||
{
|
||
flag=1/-pt.tan_theta;
|
||
result=upper_bound(tanv.begin(),tanv.end(),flag);
|
||
if(result==tanv.end())
|
||
{
|
||
offset=sectionSize*7-1;
|
||
}
|
||
else
|
||
{
|
||
offset=sectionSize*6+(result-tanv.begin());
|
||
}
|
||
}
|
||
}
|
||
|
||
else
|
||
{
|
||
continue;
|
||
}
|
||
|
||
int k= (int)((pt.rad-MinRad)/(cellSize*1.0));
|
||
cellArray_AfterRegstn[offset][k].push_back(&pt);
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
|
||
int IntersectionDetection:: CalcCellFeature(cell& cellobj, cellFeature& f)
|
||
{
|
||
int outlier;
|
||
float lastMaxY;
|
||
f.size=cellobj.size();
|
||
f.cellType=0;
|
||
|
||
if(f.size==0)
|
||
{
|
||
f.cellType|=CELL_TYPE_INVALID;
|
||
return 0;
|
||
}
|
||
|
||
f.ave_x= f.ave_y = f.ave_z=0.0;
|
||
f.delta_y=0;
|
||
|
||
int i=0;
|
||
for(i=0; i<f.size; ++i)
|
||
{
|
||
f.ave_x+=cellobj[i]->x;
|
||
f.ave_z+=cellobj[i]->z;
|
||
f.ave_y+=cellobj[i]->y;
|
||
|
||
if(i==0)
|
||
{
|
||
outlier=0;
|
||
f.min_x=f.max_x=cellobj[i]->x;
|
||
f.min_z=f.max_z=cellobj[i]->z;
|
||
lastMaxY=f.min_y=f.max_y=cellobj[i]->y;
|
||
}
|
||
else
|
||
{
|
||
if(f.max_x<cellobj[i]->x) f.max_x=cellobj[i]->x;
|
||
|
||
if(f.min_x>cellobj[i]->x) f.min_x=cellobj[i]->x;
|
||
|
||
if(f.max_z<cellobj[i]->z) f.max_z=cellobj[i]->z;
|
||
|
||
if(f.min_z>cellobj[i]->z) f.min_z=cellobj[i]->z;
|
||
|
||
if(f.max_y<cellobj[i]->y)
|
||
{
|
||
lastMaxY=f.max_y;
|
||
f.max_y=cellobj[i]->y;
|
||
outlier=i;
|
||
}
|
||
|
||
if(f.min_y>cellobj[i]->y)
|
||
f.min_y=cellobj[i]->y;
|
||
}
|
||
}
|
||
|
||
if(f.size>1)
|
||
{
|
||
int y=f.ave_y-cellobj[outlier]->y;
|
||
y/=(f.size-1)*1.0;
|
||
|
||
if(cellobj[outlier]->y-y<50)
|
||
{
|
||
outlier=-1;
|
||
f.ave_y/=f.size*1.0;
|
||
}
|
||
else
|
||
{
|
||
f.max_y=lastMaxY;
|
||
f.ave_y=y;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
outlier=-1;
|
||
f.ave_y/=f.size*1.0;
|
||
}
|
||
|
||
f.ave_x/=f.size*1.0;
|
||
f.ave_z/=f.size*1.0;
|
||
|
||
for(i=0; i<f.size; ++i)
|
||
{
|
||
if(i==outlier)
|
||
continue;
|
||
f.delta_y+= absf(cellobj[i]->y - f.ave_y);
|
||
}
|
||
|
||
float threshold;
|
||
threshold=f.delta_y;
|
||
|
||
float GridThresholdGroundDetect =120;
|
||
if(threshold > GridThresholdGroundDetect)
|
||
f.cellType|=CELL_TYPE_ABOVE_DELTA_Y;
|
||
else
|
||
f.cellType|=CELL_TYPE_BELOW_DELTA_Y;
|
||
return 0;
|
||
}
|
||
|
||
|
||
int IntersectionDetection::CalcAllCellFeature()
|
||
{
|
||
int i,j;
|
||
|
||
if( cellArray_AfterRegstn.size()==0)
|
||
return -1;
|
||
|
||
if( cellFeatureArray_AfterRegstn.size()==0)
|
||
{
|
||
cellFeatureArray_AfterRegstn.resize(columnNum);
|
||
for(i=0; i<columnNum; ++i)
|
||
cellFeatureArray_AfterRegstn[i].resize(cellNum);
|
||
}
|
||
|
||
for(j=0; j <columnNum; j++)
|
||
{
|
||
cellColumn &column=cellArray_AfterRegstn[j];
|
||
cellNum=column.size();
|
||
for( i=0; i<cellNum; i++)
|
||
{
|
||
cell &cellObj=cellArray_AfterRegstn[j][i];
|
||
cellFeature &feature=cellFeatureArray_AfterRegstn[j][i];
|
||
|
||
feature.columnID=j;
|
||
feature.cellID=i;
|
||
|
||
feature.pCell=&cellObj;
|
||
CalcCellFeature(cellObj,feature);
|
||
|
||
// if( feature.delta_y > 120)
|
||
// cellFeatureArray_AfterRegstn[j][i].cellType |= CELL_TYPE_ABOVE_DELTA_Y;
|
||
}
|
||
}
|
||
return 0;
|
||
|
||
}
|
||
|
||
int IntersectionDetection::CalWideSlashEdge_For_RoadShape(float Angle,int startColumID,int startCellID,float maxLength,float wide)
|
||
{
|
||
float LengthX;
|
||
float LengthZ;
|
||
double sinAngle=sin(Angle*M_PI/180);
|
||
double cosAngle=cos(Angle*M_PI/180);
|
||
float columnAngle=360.0/columnNum;
|
||
|
||
float XPos=cellSize*(startCellID+0.5)*cos( columnAngle*(startColumID+0.5)*M_PI/180);
|
||
float ZPos=cellSize*(startCellID+0.5)*sin( columnAngle*(startColumID+0.5)*M_PI/180);
|
||
|
||
float halfWide=wide/2;
|
||
float vexPointXPos1,vexPointZPos1,vexPointXPos2,vexPointZPos2,closePointX,closePointZ;
|
||
float moveInterval=0;
|
||
float midPointX,midPointZ;
|
||
int pointColumn,pointRow;
|
||
|
||
IntersectionFeature temp;
|
||
temp.slashLength=maxLength;
|
||
temp.angle=Angle;
|
||
|
||
for(float k=0;k<maxLength;k+=moveInterval)
|
||
{
|
||
XPos+=sinAngle*moveInterval;
|
||
ZPos+=cosAngle*moveInterval;
|
||
vexPointXPos1=XPos+halfWide*cosAngle;
|
||
vexPointZPos1=ZPos-halfWide*sinAngle;
|
||
vexPointXPos2=XPos-halfWide*cosAngle;
|
||
vexPointZPos2=ZPos+halfWide*sinAngle;
|
||
|
||
if(vexPointXPos1*vexPointXPos1+vexPointZPos1*vexPointZPos1>
|
||
vexPointXPos2*vexPointXPos2+vexPointZPos2*vexPointZPos2)
|
||
{
|
||
closePointX=vexPointXPos2;
|
||
closePointZ=vexPointZPos2;
|
||
}
|
||
else
|
||
{
|
||
closePointX=vexPointXPos1;
|
||
closePointZ=vexPointZPos1;
|
||
}
|
||
CalPointCellPos(closePointX,0,closePointZ,&pointColumn,&pointRow);
|
||
moveInterval=minCellDisList[pointRow];
|
||
|
||
for(float i=0;i<=wide;i+=moveInterval)
|
||
{
|
||
midPointX=vexPointXPos1-i*cosAngle*moveInterval;
|
||
midPointZ=vexPointZPos1+i*sinAngle*moveInterval;
|
||
CalPointCellPos(midPointX,0,midPointZ,&pointColumn,&pointRow);
|
||
|
||
if(pointRow>cellNum-1)
|
||
{
|
||
temp.slashEndColumn=pointColumn;
|
||
temp.slashEndRow=cellNum-1;
|
||
goto labelCalSlashEdge_For_RoadShape;
|
||
}
|
||
cellFeature& cellobj=cellFeatureArray_AfterRegstn[pointColumn][pointRow];
|
||
if(cellobj.cellType & CELL_TYPE_ABOVE_DELTA_Y )
|
||
{
|
||
temp.slashEndColumn=pointColumn;
|
||
temp.slashEndRow=pointRow;
|
||
temp.slashLength=k;
|
||
goto labelCalSlashEdge_For_RoadShape;
|
||
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
labelCalSlashEdge_For_RoadShape:
|
||
temp.slashLength=temp.slashLength/100;//<2F><>λ<EFBFBD><CEBB>Ϊm
|
||
intersectionFeature.push_back(temp);
|
||
|
||
return 0;
|
||
}
|
||
|
||
|
||
int IntersectionDetection::DetectIntersection()
|
||
{
|
||
int startColumn=0;
|
||
int startRow=30;
|
||
|
||
for(float i=0;i<360;i+=slashAngleDefinition)
|
||
{
|
||
CalWideSlashEdge_For_RoadShape(i,startColumn,startRow,slashMaxLength,slashWide);
|
||
}
|
||
|
||
double labelSVM;
|
||
for(int i=0;i<360;i++)
|
||
{
|
||
nod[i].index=i+1;
|
||
nod[i].value=intersectionFeature[i].slashLength/slashMaxLength;
|
||
}
|
||
nod[360].index=-1;
|
||
labelSVM= svm_predict(m,nod);
|
||
|
||
ofstream output;
|
||
output.open("intersection.txt");
|
||
output<<"labelSVM:"<<labelSVM<<endl;
|
||
for(int j=0;j<360;j++)
|
||
output<<j<<":"<<" "<<nod[j].value;
|
||
output.close();
|
||
|
||
if(labelSVM>0.5)
|
||
cout<<"intersection"<<endl;
|
||
else
|
||
cout<<"segment"<<endl;
|
||
|
||
return 0;
|
||
}
|
||
|