ROS中激光雷達數據處理之特征提取 C++源碼 - ELEOK

// 進行多邊形擬合： Points : 輪廓上的點 n -- 輪廓點數目 Eps -- 擬合精度
// 返回值：若該輪廓段需要分段，則返回分段點在該輪廓點列中的索引，否則，返回 0 表示不需要分段
// 這里是整個算法計算復雜性最大的一個地方
// 為了提高程序運行效率，對點到直線的距離計算進行改進：
// 多邊形擬合中的直線是由點列中的點決定的
// 為了計算點到直線的距離，
// 采用坐標系旋轉，將直線旋轉到x軸方向，這樣點到直線的距離即為各個點
// 在坐標旋轉后的y值的絕對值
// 同時，坐標旋轉矩陣在該次運算中為定值，只需一次計算，不需要多次的開方或三角計算
int OpenRadar::PolyContourFit( int* X, int* Y, int n , float Eps ) // 根據輪廓點，用多邊形擬合該輪廓點
{
double dis = sqrt((double)(((X[0] - X[n - 1])*(X[0] - X[n - 1])) +
((Y[0] - Y[n - 1])* (Y[0] - Y[n - 1]))));
double cosTheta = (X[n- 1] - X[0]) / dis;
double sinTheta = - ( Y[n- 1] - Y[0] )/dis;
double MaxDis = 0;
int i ;
int MaxDisInd = -1;
double dbDis;
for(i = 1 ; i < n - 1 ; i++)
{
// 進行坐標旋轉，求旋轉后的點到x軸的距離
dbDis = abs( (Y[i] - Y[0]) * cosTheta + (X[i] - X[0])* sinTheta);
if( dbDis > MaxDis)
{
MaxDis = dbDis;
MaxDisInd = i;
}
}
if(MaxDis > Eps)
{
return MaxDisInd;
// cout << "Line 1 : " << endl;
// cout << "Start :" << Points[0].x << " " << Points[0].y << " --- " << Points[MaxDisInd].x << " " << Points[MaxDisInd].y << endl;
// cout << "角度： "<<180 * atan2(Points[0].y - Points[MaxDisInd].y , Points[0].x - Points[MaxDisInd].x ) / 3.1415926;
// cout << "Line 2 :" << endl;
// cout << "Start :" << Points[MaxDisInd].x << " " << Points[MaxDisInd].y << " --- " << Points[n - 1].x << " " << Points[n - 1].y << endl;
// cout << "角度： "<< 180 * atan2(Points[n - 1].y - Points[MaxDisInd].y , Points[n - 1].x - Points[MaxDisInd].x ) / 3.1415926;
}
// else{
// cout << "Line 1 : " << endl;
// cout << "Start :" << Points[0].x << " " << Points[0].y << " --- " << Points[n - 1].x << " " << Points[n - 1].y << endl;
// cout << "角度： "<<180 * atan2(Points[n - 1].y - Points[0].y , Points[n - 1].x - Points[0].x ) / 3.1415926;
// }
return 0;
}
//將折線拆成兩段

復制代碼

CV_IMPLEMENT_QSORT( IntQSort, int, cmp_pts ) // 該宏利用聲明并定義函數IntQSort用于快速排序
int W[MAX_FITPOINTS_CNT];// =(int * )malloc(sizeof(int) * Cnt);// 權值系數
int WeightedFit(int X[] , int Y[] , int Cnt , LinePara * EstLinePara)
{
// 加權最小二乘法
// Cnt: 數據點計數
// EstLinePara : 直線擬合的估計值，可以利用最小二乘法計算得到
// 利用最小二乘進行估計
int * Tmp;
int FlagFlip = 0;// 是否對X,Y進行翻轉過
//FitPara(X , Y , Cnt , EstLinePara , NULL);
//if(abs(EstLinePara->a) > 1 || EstLinePara->a == NAN || EstLinePara->a == -NAN)
if( abs(X[0] - X[Cnt - 1]) < abs(Y[0] - Y[Cnt - 1]) )
{
// 該段直線為斜率大于1
// 沿45度線進行翻轉
// 將 X 和 Y 進行翻轉
Tmp = X;
X = Y;
Y = Tmp;
FlagFlip = 1; // 翻轉
}
int i = 0;
if(W == NULL)
return -1;
// 迭代20次
for(i = 0 ; i < 20 ; i++)
{
// 計算權值
CalW(X , Y ,Cnt , EstLinePara , W );
FitPara(X,Y,Cnt,EstLinePara,W);// 根據權值擬合參數
}
//free(W);
// EstLinePara->Dis = abs(EstLinePara->b)/(sqrt(EstLinePara->a * EstLinePara->a + EstLinePara->b * EstLinePara->b));
if(FlagFlip == 0)
{
// 未翻轉
EstLinePara->Rho = atan(EstLinePara->a);
}
else
{
// 翻轉過
if(abs(EstLinePara->a) < 0.00001)
{
EstLinePara->a = 100000;
}
else
{
EstLinePara->a =1.0/ EstLinePara->a;
}
EstLinePara->b = - EstLinePara->b * (EstLinePara->a);
EstLinePara->Rho = atan(EstLinePara->a);
}
//X Y若翻轉過，再翻轉回去
if(FlagFlip == 1)
{
// 該段直線為斜率大于1
// 沿45度線進行翻轉
// 將 X 和 Y 進行翻轉
Tmp = X;
X = Y;
Y = Tmp;
}

復制代碼

国产免费AV|泡泡玛特欧洲总部将设在伦敦|中文天堂网www新版资源在线|一本久道综合在线中文|国精产品一二三产区的使用方法|香蕉鱼在线观看|www.27eee

ELEOK