2537 lines
65 KiB
C++
2537 lines
65 KiB
C++
#include "SG_baseDataType.h"
|
||
#include "SG_baseAlgo_Export.h"
|
||
#include <vector>
|
||
|
||
typedef struct
|
||
{
|
||
int flag;
|
||
int sPtIdx;
|
||
int ePtIdx;
|
||
SVzNL3DPosition startPt;
|
||
SVzNL3DPosition endPt;
|
||
}SSG_jump;
|
||
|
||
typedef struct
|
||
{
|
||
int pntIdx;
|
||
double forwardAngle; //前向角
|
||
double backwardAngle; //后向角
|
||
double corner; //拐角
|
||
double forwardDiffZ;
|
||
double backwardDiffZ;
|
||
}SSG_pntDirAngle;
|
||
|
||
typedef struct
|
||
{
|
||
int flag;
|
||
int startPtIdx;
|
||
int endPtIdx;
|
||
SVzNL3DPosition startPt;
|
||
SVzNL3DPosition endPt;
|
||
SSG_jump maxJump;
|
||
}SSG_slope;
|
||
|
||
typedef struct
|
||
{
|
||
int startPtIdx;
|
||
int endPtIdx;
|
||
double zDiff;
|
||
}SSG_zWin;
|
||
|
||
SSG_meanVar _computeMeanVar(double* data, int size)
|
||
{
|
||
double sum_x = 0;
|
||
double sum_square = 0;
|
||
int num = 0;
|
||
for (int i = 0; i < size; i++)
|
||
{
|
||
sum_x += data[i];
|
||
sum_square += data[i] * data[i];
|
||
num++;
|
||
}
|
||
SSG_meanVar a_meanVar = { 0, 0 };
|
||
if (num > 0)
|
||
{
|
||
a_meanVar.mean = sum_x / num;
|
||
a_meanVar.var = sum_square / num - a_meanVar.mean * a_meanVar.mean;
|
||
if (a_meanVar.var < 0)
|
||
a_meanVar.var = 0;
|
||
else
|
||
a_meanVar.var = sqrt(a_meanVar.var);
|
||
}
|
||
return a_meanVar;
|
||
}
|
||
|
||
void sg_lineDataSmoothing(std::vector<SVzNL3DPosition>& input, int smoothWin, std::vector<SVzNL3DPosition>& output)
|
||
{
|
||
output.resize(input.size()); // 预分配足够的空间
|
||
std::copy(input.begin(), input.end(), output.begin());
|
||
|
||
int size = input.size();
|
||
for (int i = 0; i < size; i++)
|
||
{
|
||
if (input[i].pt3D.z > 1e-4)
|
||
{
|
||
int n = 0;
|
||
double sum_z = 0;
|
||
for (int j = i-smoothWin / 2; j <= i+smoothWin / 2; j++)
|
||
{
|
||
if ((j >= 0) && (j < size))
|
||
{
|
||
if (input[j].pt3D.z > 1e-4)
|
||
{
|
||
sum_z += input[j].pt3D.z;
|
||
n++;
|
||
}
|
||
}
|
||
}
|
||
output[i].pt3D.z = sum_z / n;
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
//滤除离群点：z跳变门限方法
|
||
void sg_lineDataRemoveOutlier(SVzNL3DPosition* lineData, int dataSize, SSG_outlierFilterParam filterParam, std::vector<SVzNL3DPosition>& filerData, std::vector<int>& noisePts)
|
||
{
|
||
filerData.resize(dataSize);
|
||
|
||
std::vector< SSG_RUN> continueRuns;
|
||
SSG_RUN a_run = {0, -1, 0}; //startIdx, len, lastIdx
|
||
double pre_z = 0;
|
||
for (int i = 0; i < dataSize; i++)
|
||
{
|
||
if (i == 370)
|
||
int kkk = 1;
|
||
filerData[i] = lineData[i];
|
||
if (lineData[i].pt3D.z > 1e-4)
|
||
{
|
||
if (a_run.len < 0)
|
||
{
|
||
a_run.start = i;
|
||
a_run.len = 1;
|
||
a_run.value = i;
|
||
}
|
||
else
|
||
{
|
||
double z_diff = abs(lineData[i].pt3D.z - pre_z);
|
||
if (z_diff < filterParam.continuityTh)
|
||
{
|
||
a_run.len++;
|
||
a_run.value = i;
|
||
}
|
||
else
|
||
{
|
||
continueRuns.push_back(a_run);
|
||
a_run.start = i;
|
||
a_run.len = 1;
|
||
a_run.value = i;
|
||
}
|
||
}
|
||
pre_z = lineData[i].pt3D.z;
|
||
}
|
||
}
|
||
if(a_run.len > 0)
|
||
continueRuns.push_back(a_run);
|
||
|
||
for (int i = 0, i_max = continueRuns.size(); i < i_max; i++)
|
||
{
|
||
if (continueRuns[i].len < filterParam.outlierTh) //噪声
|
||
{
|
||
for (int j = continueRuns[i].start; j <= continueRuns[i].value; j++)
|
||
{
|
||
filerData[j].pt3D.z = 0;
|
||
noisePts.push_back(j);//记录序号，用于在原始数据上去除噪声
|
||
}
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
//滤除离群点：z跳变门限方法
|
||
void sg_lineDataRemoveOutlier_changeOriginData(SVzNL3DPosition* lineData, int dataSize, SSG_outlierFilterParam filterParam)
|
||
{
|
||
std::vector< SSG_RUN> continueRuns;
|
||
SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx
|
||
double pre_z = 0;
|
||
for (int i = 0; i < dataSize; i++)
|
||
{
|
||
if (i == 370)
|
||
int kkk = 1;
|
||
if (lineData[i].pt3D.z > 1e-4)
|
||
{
|
||
if (a_run.len < 0)
|
||
{
|
||
a_run.start = i;
|
||
a_run.len = 1;
|
||
a_run.value = i;
|
||
}
|
||
else
|
||
{
|
||
double z_diff = abs(lineData[i].pt3D.z - pre_z);
|
||
if (z_diff < filterParam.continuityTh)
|
||
{
|
||
a_run.len++;
|
||
a_run.value = i;
|
||
}
|
||
else
|
||
{
|
||
continueRuns.push_back(a_run);
|
||
a_run.start = i;
|
||
a_run.len = 1;
|
||
a_run.value = i;
|
||
}
|
||
}
|
||
pre_z = lineData[i].pt3D.z;
|
||
}
|
||
}
|
||
if (a_run.len > 0)
|
||
continueRuns.push_back(a_run);
|
||
|
||
for (int i = 0, i_max = continueRuns.size(); i < i_max; i++)
|
||
{
|
||
if (continueRuns[i].len < filterParam.outlierTh) //噪声
|
||
{
|
||
for (int j = continueRuns[i].start; j <= continueRuns[i].value; j++)
|
||
{
|
||
lineData[j].pt3D.z = 0;
|
||
}
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
//滤除离群点：点距离方法
|
||
void sg_lineDataRemoveOutlier_ptDistMethod(SVzNL3DPosition* lineData, int dataSize, SSG_outlierFilterParam filterParam, std::vector<SVzNL3DPosition>& filerData, std::vector<int>& noisePts)
|
||
{
|
||
filerData.resize(dataSize);
|
||
|
||
std::vector< SSG_RUN> continueRuns;
|
||
SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx
|
||
SVzNL3DPoint pre_pt = {0,0,0};
|
||
for (int i = 0; i < dataSize; i++)
|
||
{
|
||
if (i == 370)
|
||
int kkk = 1;
|
||
filerData[i] = lineData[i];
|
||
if (lineData[i].pt3D.z > 1e-4)
|
||
{
|
||
if (a_run.len < 0)
|
||
{
|
||
a_run.start = i;
|
||
a_run.len = 1;
|
||
a_run.value = i;
|
||
}
|
||
else
|
||
{
|
||
#if 1
|
||
double pt_dist = sqrt(pow(lineData[i].pt3D.x - pre_pt.x, 2) +
|
||
pow(lineData[i].pt3D.y - pre_pt.y, 2) +
|
||
pow(lineData[i].pt3D.z - pre_pt.z, 2));
|
||
if (pt_dist < filterParam.continuityTh)
|
||
{
|
||
a_run.len++;
|
||
a_run.value = i;
|
||
}
|
||
else
|
||
{
|
||
continueRuns.push_back(a_run);
|
||
a_run.start = i;
|
||
a_run.len = 1;
|
||
a_run.value = i;
|
||
}
|
||
#else
|
||
double pt_dy = abs(lineData[i].pt3D.y - pre_pt.y);
|
||
double pt_dz = abs(lineData[i].pt3D.z - pre_pt.z);
|
||
if (pt_dy * 3 > pt_dz)
|
||
{
|
||
a_run.len++;
|
||
a_run.value = i;
|
||
}
|
||
else
|
||
{
|
||
continueRuns.push_back(a_run);
|
||
a_run.start = i;
|
||
a_run.len = 1;
|
||
a_run.value = i;
|
||
}
|
||
#endif
|
||
|
||
}
|
||
pre_pt = lineData[i].pt3D;
|
||
}
|
||
}
|
||
if (a_run.len > 0)
|
||
continueRuns.push_back(a_run);
|
||
|
||
for (int i = 0, i_max = continueRuns.size(); i < i_max; i++)
|
||
{
|
||
if (continueRuns[i].len < filterParam.outlierTh) //噪声
|
||
{
|
||
for (int j = continueRuns[i].start; j <= continueRuns[i].value; j++)
|
||
{
|
||
filerData[j].pt3D.z = 0;
|
||
noisePts.push_back(j);//记录序号，用于在原始数据上去除噪声
|
||
}
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取扫描线中的斜坡，并记录斜坡中的最大的相邻点之间的跳变。
|
||
/// 同时记录Ending。当没有中间袋子，只有左右袋子时，会有两组Ending。Ending之间的最小间距设置为袋子宽度的1/4
|
||
/// </summary>
|
||
/// <param name="lineData">扫描线数据，兼容栅格数据格式</param>
|
||
/// <param name="dataSize">扫描线数据长度</param>
|
||
/// <param name="validSlopeH">有效斜坡长度。小于此长度的斜坡被视为噪声</param>
|
||
/// <param name="slopes">输出检测到的斜坡</param>
|
||
void _getSlopes(SVzNL3DPosition* lineData, int dataSize, const double validSlopeH, double minEndingGap,
|
||
std::vector< SSG_slope>& slopes, std::vector<SVzNLRange>& endings)
|
||
{
|
||
if (dataSize < 2)
|
||
return;
|
||
|
||
int _state = 0;
|
||
int pre_i = -1;
|
||
int sEdgePtIdx = -1;
|
||
int eEdgePtIdx = -1;
|
||
int nullPtNum = 0;
|
||
SVzNL3DPosition* pre_data = NULL;
|
||
SSG_slope a_slope = { 0, 0, 0, {0, {0,0,0}}, {0, {0,0,0}}, { 0, 0, 0, {0, {0,0,0}}, {0, {0,0,0}}} };
|
||
for (int i = 0; i < dataSize; i++)
|
||
{
|
||
if (i >= 317)
|
||
int kkk = 1;
|
||
SVzNL3DPosition* curr_data = &lineData[i];
|
||
if (curr_data->pt3D.z < 1e-4)
|
||
{
|
||
nullPtNum++;
|
||
if (i == dataSize - 1) //最后一个
|
||
{
|
||
if (sEdgePtIdx >= 0)
|
||
{
|
||
SVzNLRange a_range = { sEdgePtIdx , eEdgePtIdx };
|
||
endings.push_back(a_range);
|
||
}
|
||
}
|
||
continue;
|
||
}
|
||
|
||
if (NULL == pre_data)
|
||
{
|
||
sEdgePtIdx = i;
|
||
eEdgePtIdx = i;
|
||
pre_data = curr_data;
|
||
pre_i = i;
|
||
nullPtNum = 0;
|
||
continue;
|
||
}
|
||
|
||
if (i == dataSize - 1) //最后一个
|
||
{
|
||
if (sEdgePtIdx >= 0)
|
||
{
|
||
SVzNLRange a_range = { sEdgePtIdx , i };
|
||
endings.push_back(a_range);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if (nullPtNum > 0)
|
||
{
|
||
double y_diff = abs(curr_data->pt3D.y - pre_data->pt3D.y);
|
||
if ((y_diff > minEndingGap) && (sEdgePtIdx >= 0))
|
||
{
|
||
SVzNLRange a_range = { sEdgePtIdx , eEdgePtIdx };
|
||
endings.push_back(a_range);
|
||
sEdgePtIdx = i;
|
||
}
|
||
nullPtNum = 0;
|
||
}
|
||
}
|
||
eEdgePtIdx = i;
|
||
|
||
double z_diff = curr_data->pt3D.z - pre_data->pt3D.z;
|
||
switch (_state)
|
||
{
|
||
case 0: //初态
|
||
if (z_diff > 0) //下降
|
||
{
|
||
a_slope.startPtIdx = pre_i;
|
||
a_slope.endPtIdx = i;
|
||
a_slope.startPt = *pre_data;
|
||
a_slope.endPt = *curr_data;
|
||
a_slope.maxJump.sPtIdx = pre_i;
|
||
a_slope.maxJump.ePtIdx = i;
|
||
a_slope.maxJump.startPt = *pre_data;
|
||
a_slope.maxJump.endPt = *curr_data;
|
||
_state = 2;
|
||
}
|
||
else if (z_diff < 0) //上升
|
||
{
|
||
a_slope.startPtIdx = pre_i;
|
||
a_slope.endPtIdx = i;
|
||
a_slope.startPt = *pre_data;
|
||
a_slope.endPt = *curr_data;
|
||
a_slope.maxJump.sPtIdx = pre_i;
|
||
a_slope.maxJump.ePtIdx = i;
|
||
a_slope.maxJump.startPt = *pre_data;
|
||
a_slope.maxJump.endPt = *curr_data;
|
||
_state = 1;
|
||
}
|
||
break;
|
||
case 1: //上升
|
||
if (z_diff > 0) //下降
|
||
{
|
||
double height = a_slope.startPt.pt3D.z - a_slope.endPt.pt3D.z;
|
||
if (height >= validSlopeH)
|
||
{
|
||
a_slope.flag = 0;
|
||
a_slope.maxJump.flag = 0;
|
||
slopes.push_back(a_slope);
|
||
}
|
||
|
||
a_slope.startPtIdx = pre_i;
|
||
a_slope.endPtIdx = i;
|
||
a_slope.startPt = *pre_data;
|
||
a_slope.endPt = *curr_data;
|
||
a_slope.maxJump.sPtIdx = pre_i;
|
||
a_slope.maxJump.ePtIdx = i;
|
||
a_slope.maxJump.startPt = *pre_data;
|
||
a_slope.maxJump.endPt = *curr_data;
|
||
_state = 2;
|
||
}
|
||
else
|
||
{
|
||
a_slope.endPtIdx = i;
|
||
a_slope.endPt = *curr_data;
|
||
double maxDelta = a_slope.maxJump.startPt.pt3D.z - a_slope.maxJump.endPt.pt3D.z;
|
||
if (maxDelta < -z_diff)
|
||
{
|
||
a_slope.maxJump.sPtIdx = pre_i;
|
||
a_slope.maxJump.ePtIdx = i;
|
||
a_slope.maxJump.startPt = *pre_data;
|
||
a_slope.maxJump.endPt = *curr_data;
|
||
}
|
||
}
|
||
break;
|
||
case 2: //下降
|
||
if (z_diff < 0) // 上升
|
||
{
|
||
double height = a_slope.endPt.pt3D.z - a_slope.startPt.pt3D.z;
|
||
if (height >= validSlopeH)
|
||
{
|
||
a_slope.flag = 0;
|
||
a_slope.maxJump.flag = 0;
|
||
slopes.push_back(a_slope);
|
||
}
|
||
a_slope.startPtIdx = pre_i;
|
||
a_slope.endPtIdx = i;
|
||
a_slope.startPt = *pre_data;
|
||
a_slope.endPt = *curr_data;
|
||
a_slope.maxJump.sPtIdx = pre_i;
|
||
a_slope.maxJump.ePtIdx = i;
|
||
a_slope.maxJump.startPt = *pre_data;
|
||
a_slope.maxJump.endPt = *curr_data;
|
||
_state = 1;
|
||
}
|
||
else
|
||
{
|
||
a_slope.endPtIdx = i;
|
||
a_slope.endPt = *curr_data;
|
||
double maxDelta = a_slope.maxJump.endPt.pt3D.z - a_slope.maxJump.startPt.pt3D.z;
|
||
if (maxDelta < z_diff)
|
||
{
|
||
a_slope.maxJump.sPtIdx = pre_i;
|
||
a_slope.maxJump.ePtIdx = i;
|
||
a_slope.maxJump.startPt = *pre_data;
|
||
a_slope.maxJump.endPt = *curr_data;
|
||
}
|
||
}
|
||
break;
|
||
default:
|
||
_state = 0;
|
||
break;
|
||
}
|
||
pre_data = curr_data;
|
||
pre_i = i;
|
||
}
|
||
//最后一个
|
||
bool chkLast = true;
|
||
if (slopes.size() > 0)
|
||
{
|
||
if (slopes.back().startPtIdx == a_slope.startPtIdx)
|
||
chkLast = false;
|
||
}
|
||
if (true == chkLast)
|
||
{
|
||
if (_state == 1)//上升
|
||
{
|
||
double height = a_slope.startPt.pt3D.z - a_slope.endPt.pt3D.z;
|
||
if (height >= validSlopeH)
|
||
{
|
||
a_slope.flag = 0;
|
||
a_slope.maxJump.flag = 0;
|
||
slopes.push_back(a_slope);
|
||
}
|
||
}
|
||
else if (_state == 2)//下降
|
||
{
|
||
double height = a_slope.endPt.pt3D.z - a_slope.startPt.pt3D.z;
|
||
if (height >= validSlopeH)
|
||
{
|
||
a_slope.flag = 0;
|
||
a_slope.maxJump.flag = 0;
|
||
slopes.push_back(a_slope);
|
||
}
|
||
}
|
||
}
|
||
|
||
return;
|
||
}
|
||
|
||
SVzNL3DPosition _getZNearestPt(SVzNL3DPosition* lineData, int startIdx, int endIdx, double nominalZ)
|
||
{
|
||
SVzNL3DPosition rsltPt = { 0, {0,0,0} };
|
||
double diffZ_min = -1;
|
||
for (int i = startIdx; i <= endIdx; i++)
|
||
{
|
||
double diffZ = abs(nominalZ - lineData[i].pt3D.z);
|
||
if (diffZ_min < 0)
|
||
{
|
||
diffZ_min = diffZ;
|
||
rsltPt = lineData[i];
|
||
}
|
||
else
|
||
{
|
||
if (diffZ_min > diffZ)
|
||
{
|
||
diffZ_min = diffZ;
|
||
rsltPt = lineData[i];
|
||
}
|
||
}
|
||
}
|
||
return rsltPt;
|
||
}
|
||
bool _chkVFeature(SVzNL3DPosition* lineData, int dataSize, SSG_slope* down_slope, SSG_slope* up_slope, const SSG_VFeatureParam valleyPara)
|
||
{
|
||
bool isValidValley = false;
|
||
//寻找最低点
|
||
SVzNL3DPosition valleyPt;
|
||
//检测两个Slope是否在Z向有重叠
|
||
if ((down_slope->endPt.pt3D.z > up_slope->endPt.pt3D.z) &&
|
||
(up_slope->startPt.pt3D.z > down_slope->startPt.pt3D.z))
|
||
{
|
||
if (down_slope->endPt.pt3D.z >= up_slope->startPt.pt3D.z) //down_slope的EndPt是最低点
|
||
{
|
||
valleyPt = down_slope->endPt;//down_slope的EndPt是最低点
|
||
double chkZ_diff = abs(up_slope->startPt.pt3D.z - valleyPt.pt3D.z);
|
||
//在dwon_slope中寻找与up_slope的startPt的z高度最接近的点
|
||
SVzNL3DPosition counterPt = _getZNearestPt(lineData, down_slope->startPtIdx, down_slope->endPtIdx, up_slope->startPt.pt3D.z);
|
||
if (chkZ_diff < valleyPara.valleyMinH) //需要检测valleyMinH的宽度
|
||
{
|
||
SVzNL3DPosition minH_pt1 = _getZNearestPt(lineData, down_slope->startPtIdx, down_slope->endPtIdx, down_slope->endPt.pt3D.z - valleyPara.valleyMinH/2);
|
||
SVzNL3DPosition minH_pt2 = _getZNearestPt(lineData, up_slope->startPtIdx, up_slope->endPtIdx, down_slope->endPt.pt3D.z - valleyPara.valleyMinH/2);
|
||
double valleyW = abs(minH_pt1.pt3D.y - minH_pt2.pt3D.y);
|
||
if (valleyW < valleyPara.valleyMaxW)
|
||
isValidValley = true;
|
||
}
|
||
else//up_slope的最低点高于valleyMinH， 不需要检测valleyMinH的宽度
|
||
{
|
||
//检查Valley的宽高比
|
||
//在up_slope中寻找与down_slope的endPt的z高度最接近的点
|
||
double valleyW = abs(counterPt.pt3D.y - up_slope->startPt.pt3D.z);
|
||
if (valleyW < valleyPara.valleyMaxW)
|
||
isValidValley = true;
|
||
}
|
||
}
|
||
else //up_slope的startPt是最低点
|
||
{
|
||
valleyPt = up_slope->startPt;//up_slope的startPt是最低点
|
||
double chkZ_diff = abs(down_slope->endPt.pt3D.z - valleyPt.pt3D.z);
|
||
//在up_slope中寻找与down_slope的endPt的z高度最接近的点
|
||
SVzNL3DPosition counterPt = _getZNearestPt(lineData, up_slope->startPtIdx, up_slope->endPtIdx, down_slope->endPt.pt3D.z);
|
||
if (chkZ_diff < valleyPara.valleyMinH) //需要检测valleyMinH的宽度
|
||
{
|
||
SVzNL3DPosition minH_pt1 = _getZNearestPt(lineData, down_slope->startPtIdx, down_slope->endPtIdx, up_slope->startPt.pt3D.z - valleyPara.valleyMinH);
|
||
SVzNL3DPosition minH_pt2 = _getZNearestPt(lineData, up_slope->startPtIdx, up_slope->endPtIdx, up_slope->startPt.pt3D.z - valleyPara.valleyMinH);
|
||
double valleyW = abs(minH_pt1.pt3D.y - minH_pt2.pt3D.y);
|
||
if (valleyW < valleyPara.valleyMaxW)
|
||
isValidValley = true;
|
||
}
|
||
else//up_slope的最低点高于valleyMinH， 不需要检测valleyMinH的宽度
|
||
{
|
||
//检查Valley的宽高比
|
||
//在up_slope中寻找与down_slope的endPt的z高度最接近的点
|
||
double valleyW = abs(counterPt.pt3D.y - up_slope->startPt.pt3D.z);
|
||
if (valleyW < valleyPara.valleyMaxW)
|
||
isValidValley = true;
|
||
}
|
||
}
|
||
}
|
||
return isValidValley;
|
||
}
|
||
|
||
double _computerMaxSlopeK(SVzNL3DPosition* lineData, int dataSize, SSG_jump maxJump, double zWinLen)
|
||
{
|
||
//从maxJumPos向两端各延伸1cm，计算坡度
|
||
double z0 = maxJump.startPt.pt3D.z;
|
||
double z_start = z0;
|
||
double y_start = maxJump.startPt.pt3D.y;
|
||
double z1 = maxJump.endPt.pt3D.z;
|
||
double z_end = z1;
|
||
double y_end = maxJump.endPt.pt3D.y;
|
||
#if 1
|
||
if (maxJump.sPtIdx > 0)
|
||
{
|
||
y_start = lineData[maxJump.sPtIdx-1].pt3D.y;
|
||
z_start = lineData[maxJump.sPtIdx - 1].pt3D.z;
|
||
}
|
||
if (maxJump.ePtIdx < dataSize - 1)
|
||
{
|
||
y_end = lineData[maxJump.ePtIdx + 1].pt3D.y;
|
||
z_end = lineData[maxJump.ePtIdx + 1].pt3D.z;
|
||
}
|
||
#else
|
||
//在窗口内计算坡度以排除最大下降点本身z波动的影响
|
||
if (z0 < z1) //下降：对于L型斜坡，最大下降点应该在endPt端，再往下应该是平坦段，因此窗口要从startPt往前找。
|
||
{
|
||
double max_dz = 0;
|
||
for (int i = maxJump.sPtIdx; i >= 0; i--)
|
||
{
|
||
if (lineData[i].pt3D.z > 1e-4)
|
||
{
|
||
double dz = abs(z0 - lineData[i].pt3D.z);
|
||
if (max_dz < dz)
|
||
{
|
||
z_start = lineData[i].pt3D.z;
|
||
y_start = lineData[i].pt3D.y;
|
||
max_dz = dz;
|
||
}
|
||
if (dz > zWinLen)
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
else //上升：对于L型斜坡，最大下降点应该在startPt端，再往前应该是平坦段，因此窗口要从endPt往后找。
|
||
{
|
||
double max_dz = 0;
|
||
for (int i = maxJump.ePtIdx; i < dataSize; i++)
|
||
{
|
||
if (lineData[i].pt3D.z > 1e-4)
|
||
{
|
||
double dz = abs(z0 - lineData[i].pt3D.z);
|
||
if (max_dz < dz)
|
||
{
|
||
z_end = lineData[i].pt3D.z;
|
||
y_end = lineData[i].pt3D.y;
|
||
max_dz = dz;
|
||
}
|
||
if (dz > zWinLen)
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
#endif
|
||
return abs((z_start - z_end) / (y_start - y_end));
|
||
}
|
||
|
||
/// <summary>
|
||
/// 提取激光线上的L型跳变和V型跳变特征
|
||
/// nPointIdx被重新定义成Feature类型
|
||
/// 算法流程：
|
||
/// （1）提取扫描线中的Slope(超过门限高度的slope为有效slope）,局部最小值，以及局部Z跳变最大的点
|
||
/// （2）取窗口宽度内的最小值为有效最小值
|
||
/// （3）V:在最小值点两个Win宽度一半内检查有没有对称的Slope。
|
||
/// （4）L:在Slope中检测Z跳变最大的点，判断角度。
|
||
///
|
||
/// </summary>
|
||
void sg_getLineLVFeature(SVzNL3DPosition* lineData, int dataSize, int lineIdx, const SSG_slopeParam slopeParam, const SSG_VFeatureParam valleyPara, SSG_lineFeature* line_features)
|
||
{
|
||
std::vector< SSG_slope> slopes;
|
||
//提取扫描线中的Slope(超过门限高度的slope为有效slope）
|
||
std::vector<SVzNLRange> endings; //22024.12.16: 注意：当没有中间袋子，只有左右袋子时，有两组Ending。
|
||
_getSlopes(lineData, dataSize, slopeParam.validSlopeH, slopeParam.minEndingGap, slopes, endings);
|
||
|
||
//首先检查L型边界：本身具有足够坡度，与下一段形成明显坡度差
|
||
//再检查是否可以形成V型特征
|
||
for (int i = 0, i_max = slopes.size(); i < i_max; i++)
|
||
{
|
||
SSG_slope* a_slope = &slopes[i];
|
||
if (a_slope->flag > 0)
|
||
continue;
|
||
|
||
double curr_slopeH = a_slope->endPt.pt3D.z - a_slope->startPt.pt3D.z;
|
||
double curr_maxJumpH = a_slope->maxJump.endPt.pt3D.z - a_slope->maxJump.startPt.pt3D.z;
|
||
int curr_self_type = 0;
|
||
SSG_basicFeature1D self_slope;
|
||
memset(&self_slope, 0, sizeof(SSG_basicFeature1D));
|
||
if (abs(curr_maxJumpH) > slopeParam.minLJumpH)
|
||
{
|
||
if (curr_maxJumpH > 0) //下降
|
||
{
|
||
curr_self_type = LINE_FEATURE_L_JUMP_H2L;
|
||
self_slope.featureType = LINE_FEATURE_L_JUMP_H2L;
|
||
self_slope.jumpPos = a_slope->maxJump.startPt.pt3D;
|
||
self_slope.jumpPos2D = { lineIdx, a_slope->maxJump.sPtIdx };
|
||
}
|
||
else
|
||
{
|
||
curr_self_type = LINE_FEATURE_L_JUMP_L2H;
|
||
self_slope.featureType = LINE_FEATURE_L_JUMP_L2H;
|
||
self_slope.jumpPos = a_slope->maxJump.endPt.pt3D;
|
||
self_slope.jumpPos2D = { lineIdx,a_slope->maxJump.ePtIdx };
|
||
}
|
||
}
|
||
else if (abs(curr_slopeH) > slopeParam.minLJumpH)
|
||
{
|
||
//计算最大坡度
|
||
double maxSlopeK = _computerMaxSlopeK(lineData, dataSize, a_slope->maxJump, slopeParam.LSlopeZWin);
|
||
//abs(slopeH / (a_slope->endPt.pt3D.y - a_slope->startPt.pt3D.y));
|
||
if (maxSlopeK > 2) //对应63度，在此角度下，相邻点在1mm时，点的波动基本不会导致slope的反转
|
||
{
|
||
if (curr_slopeH > 0) //下降
|
||
{
|
||
bool nearEdgeFlag = false;
|
||
for (int m = 0; m < endings.size(); m++)
|
||
{
|
||
int gap_0 = abs(a_slope->endPtIdx - endings[m].nMin);
|
||
int gap_1 = abs(a_slope->endPtIdx - endings[m].nMax);
|
||
if ( (gap_0 <= 2) || (gap_1 <= 2))
|
||
{
|
||
nearEdgeFlag = true;
|
||
break;
|
||
}
|
||
}
|
||
if (false == nearEdgeFlag)
|
||
{
|
||
curr_self_type = LINE_FEATURE_L_SLOPE_H2L;
|
||
self_slope.featureType = LINE_FEATURE_L_SLOPE_H2L;
|
||
self_slope.jumpPos = a_slope->maxJump.endPt.pt3D; //a_slope->endPt.pt3D;
|
||
self_slope.jumpPos2D = { lineIdx, a_slope->maxJump.ePtIdx };//a_slope->endPtIdx };
|
||
}
|
||
}
|
||
else
|
||
{
|
||
bool nearEdgeFlag = false;
|
||
for (int m = 0; m < endings.size(); m++)
|
||
{
|
||
int gap_0 = abs(a_slope->startPtIdx - endings[m].nMin);
|
||
int gap_1 = abs(a_slope->startPtIdx - endings[m].nMax);
|
||
if ((gap_0 <= 2) || (gap_1 <= 2))
|
||
{
|
||
nearEdgeFlag = true;
|
||
break;
|
||
}
|
||
}
|
||
if (false == nearEdgeFlag)
|
||
{
|
||
curr_self_type = LINE_FEATURE_L_SLOPE_L2H;
|
||
self_slope.featureType = LINE_FEATURE_L_SLOPE_L2H;
|
||
self_slope.jumpPos = a_slope->maxJump.startPt.pt3D; //a_slope->startPt.pt3D;
|
||
self_slope.jumpPos2D = { lineIdx, a_slope->maxJump.sPtIdx }; //a_slope->startPtIdx };
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
//考虑下一个特征，迭代检查
|
||
int nxt_self_type = 0;
|
||
int complex_type = 0; //复合特征
|
||
SSG_basicFeature1D nxt_self_slope;
|
||
memset(&nxt_self_slope, 0, sizeof(SSG_basicFeature1D));
|
||
SSG_basicFeature1D a_valley;
|
||
memset(&a_valley, 0, sizeof(SSG_basicFeature1D));
|
||
if (i < i_max - 1) //检查能否与下一个配对
|
||
{
|
||
SSG_slope* nxt_slope = &slopes[i + 1];
|
||
double z_diff = a_slope->endPt.pt3D.z - a_slope->startPt.pt3D.z;
|
||
double nxt_z_diff = nxt_slope->endPt.pt3D.z - nxt_slope->startPt.pt3D.z;
|
||
double curr_dz = abs(z_diff);
|
||
double nxt_dz = abs(nxt_z_diff);
|
||
if ( (z_diff > 0) && (nxt_z_diff < 0) && (0 == nxt_slope->flag)) //下降slope
|
||
{
|
||
//检查是否可以配对
|
||
bool isValley = _chkVFeature(lineData, dataSize, a_slope, nxt_slope, valleyPara);
|
||
if (true == isValley) //&& (false == isLSlope))
|
||
{
|
||
a_slope->flag = 1;
|
||
nxt_slope->flag = 1;
|
||
complex_type = LINE_FEATURE_V_SLOPE;
|
||
a_valley.featureType = LINE_FEATURE_V_SLOPE;
|
||
if (a_slope->endPt.pt3D.z >= nxt_slope->startPt.pt3D.z)
|
||
{
|
||
a_valley.jumpPos = a_slope->endPt.pt3D;
|
||
a_valley.jumpPos2D = { lineIdx, a_slope->endPtIdx };
|
||
}
|
||
else
|
||
{
|
||
a_valley.jumpPos = nxt_slope->startPt.pt3D;
|
||
a_valley.jumpPos2D = { lineIdx, nxt_slope->startPtIdx };
|
||
}
|
||
}
|
||
}
|
||
if (complex_type > 0)//检查下一个slope的type
|
||
{
|
||
double nxt_slopeH = nxt_slope->endPt.pt3D.z - nxt_slope->startPt.pt3D.z;
|
||
double nxt_maxJumpH = nxt_slope->maxJump.endPt.pt3D.z - nxt_slope->maxJump.startPt.pt3D.z;
|
||
if (abs(nxt_maxJumpH) > slopeParam.minLJumpH)
|
||
{
|
||
if (nxt_maxJumpH > 0) //下降
|
||
{
|
||
nxt_self_type = LINE_FEATURE_L_JUMP_H2L;
|
||
nxt_self_slope.featureType = LINE_FEATURE_L_JUMP_H2L;
|
||
nxt_self_slope.jumpPos = nxt_slope->maxJump.startPt.pt3D;
|
||
nxt_self_slope.jumpPos2D = { lineIdx, nxt_slope->maxJump.sPtIdx };
|
||
}
|
||
else
|
||
{
|
||
nxt_self_type = LINE_FEATURE_L_JUMP_L2H;
|
||
nxt_self_slope.featureType = LINE_FEATURE_L_JUMP_L2H;
|
||
nxt_self_slope.jumpPos = nxt_slope->maxJump.endPt.pt3D;
|
||
nxt_self_slope.jumpPos2D = { lineIdx,nxt_slope->maxJump.ePtIdx };
|
||
}
|
||
}
|
||
else if (abs(nxt_slopeH) > slopeParam.minLJumpH)
|
||
{
|
||
//计算最大坡度
|
||
double nxt_maxSlopeK = _computerMaxSlopeK(lineData, dataSize, nxt_slope->maxJump, slopeParam.LSlopeZWin);
|
||
//abs(slopeH / (nxt_slope->endPt.pt3D.y - nxt_slope->startPt.pt3D.y));
|
||
if (nxt_maxSlopeK > 2) //对应63度，在此角度下，相邻点在1mm时，点的波动基本不会导致slope的反转
|
||
{
|
||
if (nxt_slopeH > 0) //下降
|
||
{
|
||
bool nearEdgeFlag = false;
|
||
for (int m = 0; m < endings.size(); m++)
|
||
{
|
||
int gap_0 = abs(nxt_slope->endPtIdx - endings[m].nMin);
|
||
int gap_1 = abs(nxt_slope->endPtIdx - endings[m].nMax);
|
||
if ((gap_0 <= 2) || (gap_1 <= 2))
|
||
{
|
||
nearEdgeFlag = true;
|
||
break;
|
||
}
|
||
}
|
||
if (false == nearEdgeFlag)
|
||
{
|
||
nxt_self_type = LINE_FEATURE_L_SLOPE_H2L;
|
||
nxt_self_slope.featureType = LINE_FEATURE_L_SLOPE_H2L;
|
||
nxt_self_slope.jumpPos = nxt_slope->maxJump.endPt.pt3D;//nxt_slope->endPt.pt3D;
|
||
nxt_self_slope.jumpPos2D = { lineIdx, nxt_slope->maxJump.ePtIdx };//nxt_slope->endPtIdx };
|
||
}
|
||
}
|
||
else
|
||
{
|
||
bool nearEdgeFlag = false;
|
||
for (int m = 0; m < endings.size(); m++)
|
||
{
|
||
int gap_0 = abs(nxt_slope->startPtIdx - endings[m].nMin);
|
||
int gap_1 = abs(nxt_slope->startPtIdx - endings[m].nMax);
|
||
if ((gap_0 <= 2) || (gap_1 <= 2))
|
||
{
|
||
nearEdgeFlag = true;
|
||
break;
|
||
}
|
||
}
|
||
if (false == nearEdgeFlag)
|
||
{
|
||
nxt_self_type = LINE_FEATURE_L_SLOPE_L2H;
|
||
nxt_self_slope.featureType = LINE_FEATURE_L_SLOPE_L2H;
|
||
nxt_self_slope.jumpPos = nxt_slope->maxJump.startPt.pt3D; //nxt_slope->startPt.pt3D;
|
||
nxt_self_slope.jumpPos2D = { lineIdx, nxt_slope->maxJump.sPtIdx }; //nxt_slope->startPtIdx };
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
}
|
||
}
|
||
|
||
//处理同时存在的情况
|
||
if (complex_type == 0)
|
||
{
|
||
if (curr_self_type > 0)
|
||
{
|
||
line_features->features.push_back(self_slope);
|
||
//标记
|
||
a_slope->flag = 1;
|
||
if ((LINE_FEATURE_L_JUMP_H2L == curr_self_type) || (LINE_FEATURE_L_JUMP_L2H == curr_self_type))
|
||
a_slope->maxJump.flag = 1;
|
||
}
|
||
if (nxt_self_type > 0)
|
||
{
|
||
line_features->features.push_back(nxt_self_slope);
|
||
//标记
|
||
slopes[i + 1].flag = 1;
|
||
if ((LINE_FEATURE_L_JUMP_H2L == nxt_self_type) || (LINE_FEATURE_L_JUMP_L2H == nxt_self_type))
|
||
slopes[i + 1].maxJump.flag = 1;
|
||
}
|
||
}
|
||
else //complex_type > 0
|
||
{
|
||
const double sameFeatureDistTh = 5.0;
|
||
const double slope_jump_th = 40.0;
|
||
SSG_slope* nxt_slope = &slopes[i + 1];
|
||
|
||
//标记
|
||
a_slope->flag = 1;
|
||
nxt_slope->flag = 1;
|
||
if ((curr_self_type == 0) && (nxt_self_type == 0))
|
||
{
|
||
line_features->features.push_back(a_valley);
|
||
}
|
||
else if ((curr_self_type > 0) && (nxt_self_type == 0))
|
||
{
|
||
SVzNL3DPoint valley_lowest = a_valley.jumpPos;
|
||
SVzNL3DPoint curr_slope_lowest;
|
||
if (LINE_FEATURE_L_JUMP_H2L == curr_self_type)
|
||
curr_slope_lowest = a_slope->maxJump.endPt.pt3D;
|
||
else if (LINE_FEATURE_L_JUMP_L2H == curr_self_type)
|
||
curr_slope_lowest = a_slope->maxJump.startPt.pt3D;
|
||
else
|
||
curr_slope_lowest = self_slope.jumpPos;
|
||
double valley_slope_dist_0 = abs(valley_lowest.y - curr_slope_lowest.y);
|
||
if (valley_slope_dist_0 > sameFeatureDistTh) //同时存在
|
||
{
|
||
line_features->features.push_back(self_slope);
|
||
line_features->features.push_back(a_valley);
|
||
}
|
||
else
|
||
{
|
||
//检查最高点高差。高差大于
|
||
double highest_diff = a_slope->startPt.pt3D.z - nxt_slope->endPt.pt3D.z;
|
||
if (abs(highest_diff) < slope_jump_th) //小于40mm(袋子高度的1/4)，视为valley; 反之，视为Slope
|
||
{
|
||
if ((LINE_FEATURE_L_JUMP_H2L == curr_self_type) || (LINE_FEATURE_L_JUMP_L2H == curr_self_type))//JUMP优先
|
||
line_features->features.push_back(self_slope);
|
||
else
|
||
line_features->features.push_back(a_valley);
|
||
}
|
||
else
|
||
line_features->features.push_back(self_slope);
|
||
}
|
||
}
|
||
else if ((curr_self_type == 0) && (nxt_self_type > 0))
|
||
{
|
||
SVzNL3DPoint valley_lowest = a_valley.jumpPos;
|
||
SVzNL3DPoint nxt_slope_lowest;
|
||
if (LINE_FEATURE_L_JUMP_H2L == nxt_self_type)
|
||
nxt_slope_lowest = nxt_slope->maxJump.endPt.pt3D;
|
||
else if (LINE_FEATURE_L_JUMP_L2H == nxt_self_type)
|
||
nxt_slope_lowest = nxt_slope->maxJump.startPt.pt3D;
|
||
else
|
||
nxt_slope_lowest = nxt_self_slope.jumpPos;
|
||
double valley_slope_dist_1 = abs(valley_lowest.y - nxt_slope_lowest.y);
|
||
if (valley_slope_dist_1 > sameFeatureDistTh) //同时存在
|
||
{
|
||
line_features->features.push_back(a_valley);
|
||
line_features->features.push_back(nxt_self_slope);
|
||
}
|
||
else // ((curr_self_type > 0) && (nxt_self_type > 0))
|
||
{
|
||
//检查最高点高差。高差大于
|
||
double highest_diff = a_slope->startPt.pt3D.z - nxt_slope->endPt.pt3D.z;
|
||
if (abs(highest_diff) < slope_jump_th) //小于40mm(袋子高度的1/4)，视为valley; 反之，视为Slope
|
||
{
|
||
if ((LINE_FEATURE_L_JUMP_H2L == nxt_self_type) || (LINE_FEATURE_L_JUMP_L2H == nxt_self_type)) //JUMP优先
|
||
line_features->features.push_back(nxt_self_slope);
|
||
else
|
||
line_features->features.push_back(a_valley);
|
||
}
|
||
else
|
||
line_features->features.push_back(nxt_self_slope);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
SVzNL3DPoint valley_lowest = a_valley.jumpPos;
|
||
SVzNL3DPoint curr_slope_lowest;
|
||
if (LINE_FEATURE_L_JUMP_H2L == curr_self_type)
|
||
curr_slope_lowest = a_slope->maxJump.endPt.pt3D;
|
||
else if (LINE_FEATURE_L_JUMP_L2H == curr_self_type)
|
||
curr_slope_lowest = a_slope->maxJump.startPt.pt3D;
|
||
else
|
||
curr_slope_lowest = self_slope.jumpPos;
|
||
double valley_slope_dist_0 = abs(valley_lowest.y - curr_slope_lowest.y);
|
||
|
||
SVzNL3DPoint nxt_slope_lowest;
|
||
if (LINE_FEATURE_L_JUMP_H2L == nxt_self_type)
|
||
nxt_slope_lowest = nxt_slope->maxJump.endPt.pt3D;
|
||
else if (LINE_FEATURE_L_JUMP_L2H == nxt_self_type)
|
||
nxt_slope_lowest = nxt_slope->maxJump.startPt.pt3D;
|
||
else
|
||
nxt_slope_lowest = nxt_self_slope.jumpPos;
|
||
double valley_slope_dist_1= abs(valley_lowest.y - nxt_slope_lowest.y);
|
||
|
||
if ( (valley_slope_dist_0 > sameFeatureDistTh) && (valley_slope_dist_1 > sameFeatureDistTh)) //同时存在
|
||
{
|
||
line_features->features.push_back(self_slope);
|
||
line_features->features.push_back(a_valley);
|
||
line_features->features.push_back(nxt_self_slope);
|
||
}
|
||
else if ((valley_slope_dist_0 > sameFeatureDistTh) && (valley_slope_dist_1 <= sameFeatureDistTh))
|
||
{
|
||
line_features->features.push_back(self_slope);
|
||
if((LINE_FEATURE_L_JUMP_H2L == nxt_self_type) || (LINE_FEATURE_L_JUMP_L2H == nxt_self_type)) //JUMP优先
|
||
line_features->features.push_back(nxt_self_slope);
|
||
else
|
||
line_features->features.push_back(a_valley);
|
||
}
|
||
else if ((valley_slope_dist_0 <= sameFeatureDistTh) && (valley_slope_dist_1 > sameFeatureDistTh))
|
||
{
|
||
if((LINE_FEATURE_L_JUMP_H2L == curr_self_type) || (LINE_FEATURE_L_JUMP_L2H == curr_self_type))//JUMP优先
|
||
line_features->features.push_back(self_slope);
|
||
else
|
||
line_features->features.push_back(a_valley);
|
||
line_features->features.push_back(nxt_self_slope);
|
||
}
|
||
else //合并为一个
|
||
{
|
||
//检查最高点高差。高差大于
|
||
double highest_diff = a_slope->startPt.pt3D.z - nxt_slope->endPt.pt3D.z;
|
||
if (abs(highest_diff) < slope_jump_th) //小于40mm(袋子高度的1/4)，视为valley; 反之，视为Slope
|
||
{
|
||
line_features->features.push_back(a_valley);
|
||
}
|
||
else
|
||
{
|
||
if (highest_diff > 0) //L2H slope
|
||
{
|
||
if (self_slope.jumpPos.z > nxt_self_slope.jumpPos.z)
|
||
{
|
||
self_slope.featureType = LINE_FEATURE_L_SLOPE_L2H;
|
||
line_features->features.push_back(self_slope);
|
||
}
|
||
else
|
||
{
|
||
nxt_self_slope.featureType = LINE_FEATURE_L_SLOPE_L2H;
|
||
line_features->features.push_back(nxt_self_slope);
|
||
}
|
||
}
|
||
else //H2L slope
|
||
{
|
||
if (self_slope.jumpPos.z > nxt_self_slope.jumpPos.z)
|
||
{
|
||
self_slope.featureType = LINE_FEATURE_L_SLOPE_H2L;
|
||
line_features->features.push_back(self_slope);
|
||
}
|
||
else
|
||
{
|
||
nxt_self_slope.featureType = LINE_FEATURE_L_SLOPE_H2L;
|
||
line_features->features.push_back(nxt_self_slope);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
//检查连续的JUMP是否可能合并
|
||
int i = line_features->features.size() - 2;
|
||
while(i >= 0)
|
||
{
|
||
if (i < ((int)line_features->features.size() - 1))
|
||
{
|
||
SSG_basicFeature1D* curr_feature = &line_features->features[i];
|
||
SSG_basicFeature1D* nxt_feature = &line_features->features[i + 1];
|
||
if ((curr_feature->featureType == nxt_feature->featureType) &&
|
||
((LINE_FEATURE_L_JUMP_H2L == curr_feature->featureType) ||
|
||
(LINE_FEATURE_L_JUMP_L2H == curr_feature->featureType)))
|
||
{
|
||
//计算两个的距离
|
||
double dist = abs(curr_feature->jumpPos.y - nxt_feature->jumpPos.y);
|
||
if (dist < 20) //距离小于20mm，合并
|
||
{
|
||
if (LINE_FEATURE_L_JUMP_L2H)
|
||
*curr_feature = *nxt_feature;
|
||
line_features->features.erase(line_features->features.begin() + i + 1);
|
||
}
|
||
}
|
||
}
|
||
i--;
|
||
}
|
||
|
||
//添加开始和结束边界
|
||
for (int i = 0; i < endings.size(); i++)
|
||
{
|
||
SSG_basicFeature1D an_edge;
|
||
memset(&an_edge, 0, sizeof(SSG_basicFeature1D));
|
||
an_edge.featureType = LINE_FEATURE_LINE_ENDING_0;
|
||
an_edge.jumpPos = lineData[endings[i].nMin].pt3D;
|
||
an_edge.jumpPos2D = { lineIdx, endings[i].nMin };
|
||
line_features->endings.push_back(an_edge);
|
||
//line_features.insert(line_features.begin(), an_edge); //头部
|
||
//尾部
|
||
an_edge.featureType = LINE_FEATURE_LINE_ENDING_1;
|
||
an_edge.jumpPos = lineData[endings[i].nMax].pt3D;
|
||
an_edge.jumpPos2D = { lineIdx, endings[i].nMax };
|
||
line_features->endings.push_back(an_edge);
|
||
}
|
||
|
||
return;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 提取激光线上的拐点特征
|
||
/// nPointIdx被重新定义成Feature类型
|
||
/// 算法流程：
|
||
/// （1）逐点计算前向角和后向角
|
||
/// （2）逐点计算拐角，顺时针为负，逆时针为正
|
||
/// （3）搜索正拐角的极大值。
|
||
/// （4）判断拐角是否为跳变
|
||
/// </summary>
|
||
void sg_getLineCornerFeature(
|
||
SVzNL3DPosition* lineData,
|
||
int dataSize,
|
||
int lineIdx,
|
||
const SSG_cornerParam cornerPara, //scale通常取bagH的1/4
|
||
SSG_lineFeature* line_features)
|
||
{
|
||
line_features->lineIdx = lineIdx;
|
||
if (lineIdx == 538)
|
||
int kkk = 1;
|
||
|
||
//去除零点
|
||
std::vector< SVzNL3DPosition> vldPts;
|
||
std::vector<SSG_RUN> segs;
|
||
int segStart = -1, segEnd = -1;
|
||
for (int i = 0; i < dataSize; i++)
|
||
{
|
||
SVzNL3DPosition a_pt = lineData[i];
|
||
a_pt.nPointIdx = i;
|
||
if (lineData[i].pt3D.z > 1e-4)
|
||
{
|
||
if (segStart < 0)
|
||
segStart = i;
|
||
segEnd = i;
|
||
vldPts.push_back(a_pt);
|
||
}
|
||
else
|
||
{
|
||
if (segStart >= 0)
|
||
{
|
||
SSG_RUN a_run;
|
||
a_run.start = segStart;
|
||
a_run.len = segEnd - segStart + 1;
|
||
a_run.value = 1;
|
||
segs.push_back(a_run);
|
||
segStart = -1;
|
||
segEnd = -1;
|
||
}
|
||
}
|
||
}
|
||
//last
|
||
if (segStart >= 0)
|
||
{
|
||
SSG_RUN a_run;
|
||
a_run.start = segStart;
|
||
a_run.len = segEnd - segStart + 1;
|
||
a_run.value = 1;
|
||
segs.push_back(a_run);
|
||
}
|
||
|
||
//计算前向角和后向角
|
||
std::vector< SSG_pntDirAngle> corners;
|
||
corners.resize(vldPts.size());
|
||
for (int i = 0, i_max = vldPts.size(); i < i_max; i++)
|
||
{
|
||
//前向寻找
|
||
int pre_i = -1;
|
||
for (int j = i - 1; j >= 0; j--)
|
||
{
|
||
double dist = sqrt( pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) +
|
||
pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2));
|
||
if (dist >= cornerPara.scale)
|
||
{
|
||
pre_i = j;
|
||
break;
|
||
}
|
||
}
|
||
//后向寻找
|
||
int post_i = -1;
|
||
for (int j = i + 1; j < i_max; j++)
|
||
{
|
||
double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) +
|
||
pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2));
|
||
if (dist >= cornerPara.scale)
|
||
{
|
||
post_i = j;
|
||
break;
|
||
}
|
||
}
|
||
//计算拐角
|
||
if ((pre_i < 0) || (post_i < 0))
|
||
{
|
||
corners[i].pntIdx = -1;
|
||
corners[i].forwardAngle = 0;
|
||
corners[i].backwardAngle = 0;
|
||
corners[i].corner = 0;
|
||
corners[i].forwardDiffZ = 0;
|
||
corners[i].backwardDiffZ = 0;
|
||
}
|
||
else
|
||
{
|
||
double tanValue_pre = (vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z) / abs(vldPts[i].pt3D.y - vldPts[pre_i].pt3D.y);
|
||
double tanValue_post = (vldPts[post_i].pt3D.z - vldPts[i].pt3D.z) / abs(vldPts[post_i].pt3D.y - vldPts[i].pt3D.y);
|
||
double forwardAngle = atan(tanValue_post) * 180.0 / PI;
|
||
double backwardAngle = atan(tanValue_pre) * 180.0 / PI;
|
||
corners[i].pntIdx = i;
|
||
corners[i].forwardAngle = forwardAngle;
|
||
corners[i].backwardAngle = backwardAngle;
|
||
corners[i].corner = -(forwardAngle - backwardAngle); //图像坐标系与正常坐标系y方向相反，所以有“-”号
|
||
corners[i].forwardDiffZ = vldPts[post_i].pt3D.z - vldPts[i].pt3D.z;
|
||
corners[i].backwardDiffZ = vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z;
|
||
}
|
||
}
|
||
|
||
//搜索拐角极值
|
||
int _state = 0;
|
||
int pre_i = -1;
|
||
int sEdgePtIdx = -1;
|
||
int eEdgePtIdx = -1;
|
||
SSG_pntDirAngle* pre_data = NULL;
|
||
std::vector< SSG_pntDirAngle> cornerPeakP;
|
||
std::vector< SSG_pntDirAngle> cornerPeakM;
|
||
for (int i = 0, i_max = vldPts.size(); i < i_max; i++)
|
||
{
|
||
if (i == 275)
|
||
int kkk = 1;
|
||
SSG_pntDirAngle* curr_data = &corners[i];
|
||
if (curr_data->pntIdx < 0)
|
||
{
|
||
if (i == i_max-1) //最后一个
|
||
{
|
||
if (1 == _state) //上升
|
||
{
|
||
cornerPeakP.push_back(corners[eEdgePtIdx]);
|
||
}
|
||
else if (2 == _state) //下降
|
||
{
|
||
cornerPeakM.push_back(corners[eEdgePtIdx]);
|
||
}
|
||
}
|
||
continue;
|
||
}
|
||
|
||
if (NULL == pre_data)
|
||
{
|
||
sEdgePtIdx = i;
|
||
eEdgePtIdx = i;
|
||
pre_data = curr_data;
|
||
pre_i = i;
|
||
continue;
|
||
}
|
||
|
||
eEdgePtIdx = i;
|
||
double cornerDiff = curr_data->corner - pre_data->corner;
|
||
switch (_state)
|
||
{
|
||
case 0: //初态
|
||
if (cornerDiff < 0) //下降
|
||
{
|
||
_state = 2;
|
||
}
|
||
else if (cornerDiff > 0) //上升
|
||
{
|
||
_state = 1;
|
||
}
|
||
break;
|
||
case 1: //上升
|
||
if (cornerDiff < 0) //下降
|
||
{
|
||
cornerPeakP.push_back(*pre_data);
|
||
_state = 2;
|
||
}
|
||
break;
|
||
case 2: //下降
|
||
if (cornerDiff > 0) // 上升
|
||
{
|
||
cornerPeakM.push_back(*pre_data);
|
||
_state = 1;
|
||
}
|
||
break;
|
||
default:
|
||
_state = 0;
|
||
break;
|
||
}
|
||
pre_data = curr_data;
|
||
pre_i = i;
|
||
}
|
||
//注意：最后一个不处理，为基座位置
|
||
|
||
//极小值点（峰顶）
|
||
//极值比较，在尺度窗口下寻找局部极值点
|
||
double square_distTh = 4 * cornerPara.scale * cornerPara.scale; //2倍的cornerScale。
|
||
for (int i = 0, i_max = cornerPeakP.size(); i < i_max; i++)
|
||
{
|
||
if (cornerPeakP[i].corner < cornerPara.cornerTh)
|
||
continue;
|
||
|
||
bool isPeak = true;
|
||
//向前搜索
|
||
int cornerPtIdx = cornerPeakP[i].pntIdx;
|
||
for (int j = i - 1; j >= 0; j--)
|
||
{
|
||
int prePtIdx = cornerPeakP[j].pntIdx;
|
||
double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[prePtIdx].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ;
|
||
if (dist > square_distTh) //超出尺度窗口
|
||
break;
|
||
|
||
if (cornerPeakP[i].corner < cornerPeakP[j].corner)
|
||
{
|
||
isPeak = false;
|
||
break;
|
||
}
|
||
}
|
||
//向后搜索
|
||
if (true == isPeak)
|
||
{
|
||
cornerPtIdx = cornerPeakP[i].pntIdx;
|
||
for (int j = i + 1; j < i_max; j++)
|
||
{
|
||
int postPtIdx = cornerPeakP[j].pntIdx;
|
||
double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2);
|
||
if (dist > square_distTh) //超出尺度窗口
|
||
break;
|
||
|
||
if (cornerPeakP[i].corner < cornerPeakP[j].corner)
|
||
{
|
||
isPeak = false;
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
if (true == isPeak)
|
||
{
|
||
SSG_basicFeature1D a_feature;
|
||
if( (cornerPeakP[i].backwardAngle > cornerPara.jumpCornerTh_1) && (cornerPeakP[i].forwardAngle > -cornerPara.jumpCornerTh_2))
|
||
a_feature.featureType = LINE_FEATURE_L_JUMP_H2L;
|
||
else if ((cornerPeakP[i].forwardAngle < -cornerPara.jumpCornerTh_1) && (cornerPeakP[i].backwardAngle < cornerPara.jumpCornerTh_2))
|
||
a_feature.featureType = LINE_FEATURE_L_JUMP_L2H;
|
||
else
|
||
a_feature.featureType = LINE_FEATURE_CORNER_V;
|
||
|
||
|
||
a_feature.jumpPos = vldPts[cornerPtIdx].pt3D;
|
||
a_feature.jumpPos2D = { lineIdx, vldPts[cornerPtIdx].nPointIdx };
|
||
line_features->features.push_back(a_feature);
|
||
}
|
||
}
|
||
|
||
//添加开始和结束边界
|
||
//检查seg是否需要合并；向后合并
|
||
for (int i = 0, i_max = segs.size(); i < i_max - 1; i++)
|
||
{
|
||
SSG_RUN* nxt_seg = &segs[i + 1];
|
||
SSG_RUN* curr_seg = &segs[i];
|
||
|
||
int idx_1 = curr_seg->start + curr_seg->len - 1;
|
||
int idx_2 = nxt_seg->start;
|
||
double y_diff = abs(lineData[idx_1].pt3D.y - lineData[idx_2].pt3D.y);
|
||
if (y_diff < cornerPara.minEndingGap) //合并
|
||
{
|
||
int idx_end = nxt_seg->start + nxt_seg->len - 1;
|
||
nxt_seg->start = curr_seg->start;
|
||
nxt_seg->len = idx_end - curr_seg->start + 1;
|
||
curr_seg->value = 0;
|
||
}
|
||
}
|
||
for (int i = 0, i_max = segs.size(); i < i_max; i++)
|
||
{
|
||
if (0 == segs[i].value) //被合并
|
||
continue;
|
||
|
||
int idx_1 = segs[i].start;
|
||
int idx_2 = segs[i].start + segs[i].len - 1;
|
||
|
||
SSG_basicFeature1D an_edge;
|
||
memset(&an_edge, 0, sizeof(SSG_basicFeature1D));
|
||
an_edge.featureType = LINE_FEATURE_LINE_ENDING_0;
|
||
an_edge.jumpPos = lineData[idx_1].pt3D;
|
||
an_edge.jumpPos2D = { lineIdx, idx_1 };
|
||
line_features->endings.push_back(an_edge);
|
||
//line_features.insert(line_features.begin(), an_edge); //头部
|
||
//尾部
|
||
an_edge.featureType = LINE_FEATURE_LINE_ENDING_1;
|
||
an_edge.jumpPos = lineData[idx_2].pt3D;
|
||
an_edge.jumpPos2D = { lineIdx, idx_2 };
|
||
line_features->endings.push_back(an_edge);
|
||
}
|
||
return;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 提取激光线上的极值点（极大值点和极小值点）
|
||
///
|
||
/// </summary>
|
||
void sg_getLineLocalPeaks(
|
||
SVzNL3DPosition* lineData,
|
||
int dataSize,
|
||
int lineIdx,
|
||
const double scaleWin,
|
||
std::vector< SSG_basicFeature1D>& localMax,
|
||
std::vector< SSG_basicFeature1D>& localMin)
|
||
{
|
||
if (dataSize < 2)
|
||
return;
|
||
|
||
int _state = 0;
|
||
int pre_i = -1;
|
||
int sEdgePtIdx = -1;
|
||
int eEdgePtIdx = -1;
|
||
SVzNL3DPosition* pre_data = NULL;
|
||
std::vector< SVzNL3DPosition> pkTop;
|
||
std::vector< SVzNL3DPosition> pkBtm;
|
||
for (int i = 0; i < dataSize; i++)
|
||
{
|
||
lineData[i].nPointIdx = i;
|
||
SVzNL3DPosition* curr_data = &lineData[i];
|
||
if (curr_data->pt3D.z < 1e-4)
|
||
{
|
||
if (i == dataSize - 1) //最后一个
|
||
{
|
||
if ( 1 == _state ) //上升
|
||
{
|
||
pkTop.push_back(lineData[eEdgePtIdx]);
|
||
}
|
||
else if (2 == _state) //下降
|
||
{
|
||
pkBtm.push_back(lineData[eEdgePtIdx]);
|
||
}
|
||
}
|
||
continue;
|
||
}
|
||
|
||
if (NULL == pre_data)
|
||
{
|
||
sEdgePtIdx = i;
|
||
eEdgePtIdx = i;
|
||
pre_data = curr_data;
|
||
pre_i = i;
|
||
continue;
|
||
}
|
||
|
||
eEdgePtIdx = i;
|
||
double z_diff = curr_data->pt3D.z - pre_data->pt3D.z;
|
||
switch (_state)
|
||
{
|
||
case 0: //初态
|
||
if (z_diff > 0) //下降
|
||
{
|
||
_state = 2;
|
||
}
|
||
else if (z_diff < 0) //上升
|
||
{
|
||
_state = 1;
|
||
}
|
||
break;
|
||
case 1: //上升
|
||
if (z_diff > 0) //下降
|
||
{
|
||
pkTop.push_back(*pre_data);
|
||
_state = 2;
|
||
}
|
||
break;
|
||
case 2: //下降
|
||
if (z_diff < 0) // 上升
|
||
{
|
||
pkBtm.push_back(*pre_data);
|
||
_state = 1;
|
||
}
|
||
break;
|
||
default:
|
||
_state = 0;
|
||
break;
|
||
}
|
||
pre_data = curr_data;
|
||
pre_i = i;
|
||
}
|
||
//注意：最后一个不处理，为基座位置
|
||
|
||
//极小值点（峰顶）
|
||
//极值比较，在尺度窗口下寻找局部极值点
|
||
double square_distTh = scaleWin * scaleWin;
|
||
for (int i = 0, i_max = pkTop.size(); i < i_max; i++)
|
||
{
|
||
bool isPeak = true;
|
||
//向前搜索
|
||
for (int j = i - 1; j >= 0; j--)
|
||
{
|
||
double dist = pow(pkTop[i].pt3D.y - pkTop[j].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ;
|
||
if (dist > square_distTh) //超出尺度窗口
|
||
break;
|
||
|
||
if (pkTop[i].pt3D.z > pkTop[j].pt3D.z)
|
||
{
|
||
isPeak = false;
|
||
break;
|
||
}
|
||
}
|
||
//向后搜索
|
||
if (true == isPeak)
|
||
{
|
||
for (int j = i + 1; j < i_max; j++)
|
||
{
|
||
double dist = pow(pkTop[i].pt3D.y - pkTop[j].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2);
|
||
if (dist > square_distTh) //超出尺度窗口
|
||
break;
|
||
|
||
if (pkTop[i].pt3D.z > pkTop[j].pt3D.z)
|
||
{
|
||
isPeak = false;
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
if (true == isPeak)
|
||
{
|
||
SSG_basicFeature1D a_feature;
|
||
a_feature.featureType = LINE_FEATURE_PEAK_TOP;
|
||
a_feature.jumpPos = pkTop[i].pt3D;
|
||
a_feature.jumpPos2D = { lineIdx, pkTop[i].nPointIdx };
|
||
localMin.push_back(a_feature);
|
||
}
|
||
}
|
||
//极大值点（谷底）
|
||
for (int i = 0, i_max = pkBtm.size(); i < i_max; i++)
|
||
{
|
||
bool isPeak = true;
|
||
//向前搜索
|
||
for (int j = i - 1; j >= 0; j--)
|
||
{
|
||
double dist = pow(pkBtm[i].pt3D.y - pkBtm[j].pt3D.y, 2); // +pow(pkBtm[i].pt3D.x - pkBtm[j].pt3D.x, 2);
|
||
if (dist > square_distTh) //超出尺度窗口
|
||
break;
|
||
|
||
if (pkBtm[i].pt3D.z < pkBtm[j].pt3D.z)
|
||
{
|
||
isPeak = false;
|
||
break;
|
||
}
|
||
}
|
||
//向后搜索
|
||
if (true == isPeak)
|
||
{
|
||
for (int j = i + 1; j < i_max; j++)
|
||
{
|
||
double dist = pow(pkBtm[i].pt3D.y - pkBtm[j].pt3D.y, 2); // +pow(pkBtm[i].pt3D.x - pkBtm[j].pt3D.x, 2);
|
||
if (dist > square_distTh) //超出尺度窗口
|
||
break;
|
||
|
||
if (pkBtm[i].pt3D.z < pkBtm[j].pt3D.z)
|
||
{
|
||
isPeak = false;
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
if (true == isPeak)
|
||
{
|
||
SSG_basicFeature1D a_feature;
|
||
a_feature.featureType = LINE_FEATURE_PEAK_BOTTOM;
|
||
a_feature.jumpPos = pkBtm[i].pt3D;
|
||
a_feature.jumpPos2D = { lineIdx, pkBtm[i].nPointIdx };
|
||
localMax.push_back(a_feature);
|
||
}
|
||
}
|
||
|
||
return;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 提取激光线上的下跳变点（Z值变大的跳变）
|
||
///
|
||
/// </summary>
|
||
void sg_getLineDownJumps(
|
||
SVzNL3DPosition* lineData,
|
||
int dataSize,
|
||
int lineIdx,
|
||
const double jumpTh,
|
||
std::vector< SSG_basicFeature1D>& downJumps)
|
||
{
|
||
if (dataSize < 2)
|
||
return;
|
||
|
||
int pre_i = -1;
|
||
SVzNL3DPosition* pre_data = NULL;
|
||
for (int i = 0; i < dataSize; i++)
|
||
{
|
||
if (i == 830)
|
||
int kkk = 1;
|
||
lineData[i].nPointIdx = i;
|
||
SVzNL3DPosition* curr_data = &lineData[i];
|
||
if (curr_data->pt3D.z < 1e-4)
|
||
{
|
||
continue;
|
||
}
|
||
|
||
if (NULL == pre_data)
|
||
{
|
||
pre_data = curr_data;
|
||
pre_i = i;
|
||
continue;
|
||
}
|
||
|
||
double z_diff = curr_data->pt3D.z - pre_data->pt3D.z;
|
||
if (z_diff > jumpTh)
|
||
{
|
||
SSG_basicFeature1D a_feature;
|
||
a_feature.featureType = LINE_FEATURE_L_JUMP_H2L;
|
||
a_feature.jumpPos = lineData[pre_i].pt3D;
|
||
a_feature.jumpPos2D = { lineIdx, lineData[pre_i].nPointIdx };
|
||
downJumps.push_back(a_feature);
|
||
}
|
||
pre_data = curr_data;
|
||
pre_i = i;
|
||
}
|
||
return;
|
||
}
|
||
|
||
int _getSegPeak(SVzNL3DPosition* lineData, int segStartIdx, int segEndIdx)
|
||
{
|
||
double pkZ = -1;
|
||
int pkZIdx = -1;
|
||
for (int i = segStartIdx; i <= segEndIdx; i++)
|
||
{
|
||
if (lineData[i].pt3D.z < 1e-4)
|
||
continue;
|
||
|
||
if (pkZ < 0)
|
||
{
|
||
pkZ = lineData[i].pt3D.z;
|
||
pkZIdx = i;
|
||
}
|
||
else
|
||
{
|
||
if (pkZ > lineData[i].pt3D.z)
|
||
{
|
||
pkZ = lineData[i].pt3D.z;
|
||
pkZIdx = i;
|
||
}
|
||
}
|
||
}
|
||
return pkZIdx;
|
||
}
|
||
|
||
int _getYNearestPtIdx(SVzNL3DPosition* lineData, int segStartIdx, int segEndIdx, double midY)
|
||
{
|
||
double minDiff = -1;
|
||
int pkZIdx = -1;
|
||
for (int i = segStartIdx; i <= segEndIdx; i++)
|
||
{
|
||
if (lineData[i].pt3D.z < 1e-4)
|
||
continue;
|
||
|
||
double yDiff = abs(lineData[i].pt3D.y - midY);
|
||
|
||
if (minDiff < 0)
|
||
{
|
||
minDiff = yDiff;
|
||
pkZIdx = i;
|
||
}
|
||
else
|
||
{
|
||
if (minDiff > yDiff)
|
||
{
|
||
minDiff = yDiff;
|
||
pkZIdx = i;
|
||
}
|
||
}
|
||
}
|
||
return pkZIdx;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 提取激光线上的圆柱形特征
|
||
/// nPointIdx被重新定义成Feature类型
|
||
/// 算法流程：
|
||
/// （1）逐点计算前向角和后向角
|
||
/// （2）逐点计算拐角，顺时针为负，逆时针为正
|
||
/// （3）搜索正拐角的极大值。
|
||
/// （4）判断拐角是否为跳变
|
||
/// </summary>
|
||
void sg_getLineUpperSemiCircleFeature(
|
||
SVzNL3DPosition* lineData,
|
||
int dataSize,
|
||
int lineIdx,
|
||
const double sieveDiameter,
|
||
const SSG_slopeParam slopePara,
|
||
std::vector< SSG_featureSemiCircle>& result_features,
|
||
cv::Mat& holeMask) //根据holeMask进行合并
|
||
{
|
||
if (lineIdx == 194)
|
||
int kkk = 1;
|
||
//去除零点
|
||
std::vector< SVzNL3DPosition> vldPts;
|
||
std::vector<SSG_RUN> segs;
|
||
int segStart = -1, segEnd = -1;
|
||
for (int i = 0; i < dataSize; i++)
|
||
{
|
||
SVzNL3DPosition a_pt = lineData[i];
|
||
a_pt.nPointIdx = i;
|
||
if (lineData[i].pt3D.z > 1e-4)
|
||
{
|
||
if (segStart < 0)
|
||
segStart = i;
|
||
segEnd = i;
|
||
vldPts.push_back(a_pt);
|
||
}
|
||
else
|
||
{
|
||
if (segStart >= 0)
|
||
{
|
||
SSG_RUN a_run;
|
||
a_run.start = segStart;
|
||
a_run.len = segEnd - segStart + 1;
|
||
a_run.value = 1;
|
||
segs.push_back(a_run);
|
||
segStart = -1;
|
||
segEnd = -1;
|
||
}
|
||
}
|
||
}
|
||
//last
|
||
if (segStart >= 0)
|
||
{
|
||
SSG_RUN a_run;
|
||
a_run.start = segStart;
|
||
a_run.len = segEnd - segStart + 1;
|
||
a_run.value = 1;
|
||
segs.push_back(a_run);
|
||
}
|
||
//检查seg是否需要合并；向后合并
|
||
for (int i = 0, i_max = segs.size(); i < i_max - 1; i++)
|
||
{
|
||
SSG_RUN* nxt_seg = &segs[i + 1];
|
||
SSG_RUN* curr_seg = &segs[i];
|
||
|
||
int idx_1 = curr_seg->start + curr_seg->len - 1;
|
||
int idx_2 = nxt_seg->start;
|
||
double y_diff = abs(lineData[idx_1].pt3D.y - lineData[idx_2].pt3D.y);
|
||
if (y_diff < slopePara.minEndingGap) //合并
|
||
{
|
||
int idx_end = nxt_seg->start + nxt_seg->len - 1;
|
||
nxt_seg->start = curr_seg->start;
|
||
nxt_seg->len = idx_end - curr_seg->start + 1;
|
||
curr_seg->value = 0;
|
||
}
|
||
}
|
||
|
||
//获取localPeak
|
||
std::vector< SSG_basicFeature1D> localMax;
|
||
std::vector< SSG_basicFeature1D> localMin;
|
||
std::vector<int> endingFlag;
|
||
endingFlag.resize(dataSize);
|
||
for (int i = 0; i < dataSize; i++)
|
||
endingFlag[i] = 0;
|
||
for (int i = 0,i_max=segs.size(); i < i_max; i++)
|
||
{
|
||
if (0 == segs[i].value) //被合并
|
||
continue;
|
||
|
||
int idx_1 = segs[i].start;
|
||
int idx_2 = segs[i].start + segs[i].len - 1;
|
||
endingFlag[idx_1] = 2;
|
||
endingFlag[idx_2] = 3;
|
||
|
||
std::vector< SSG_basicFeature1D> segMax;
|
||
std::vector< SSG_basicFeature1D> segMin;
|
||
sg_getLineLocalPeaks(
|
||
&lineData[idx_1],
|
||
segs[i].len,
|
||
lineIdx,
|
||
slopePara.LSlopeZWin,
|
||
segMax,
|
||
segMin);
|
||
for (int m = 0, m_max = segMax.size(); m < m_max; m++)
|
||
{
|
||
segMax[m].jumpPos2D.y += idx_1;
|
||
localMax.push_back(segMax[m]);
|
||
}
|
||
for (int m = 0, m_max = segMin.size(); m < m_max; m++)
|
||
{
|
||
segMin[m].jumpPos2D.y += idx_1;
|
||
localMin.push_back(segMin[m]);
|
||
}
|
||
}
|
||
|
||
//计算slope的Z变化
|
||
std::vector< SSG_zWin> slopes; //记录窗口长度内的z变化
|
||
slopes.resize(vldPts.size());
|
||
for (int i = 0, i_max = vldPts.size(); i < i_max; i++)
|
||
{
|
||
//后向寻找
|
||
int post_i = -1;
|
||
for (int j = i + 1; j < i_max; j++)
|
||
{
|
||
double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) +
|
||
pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2));
|
||
if (dist >= slopePara.LSlopeZWin)
|
||
{
|
||
post_i = j;
|
||
break;
|
||
}
|
||
}
|
||
//计算slope
|
||
if (post_i < 0)
|
||
{
|
||
slopes[i].startPtIdx = i;
|
||
slopes[i].endPtIdx = -1;
|
||
slopes[i].zDiff = 0;
|
||
}
|
||
else
|
||
{
|
||
slopes[i].startPtIdx = i;
|
||
slopes[i].endPtIdx = post_i;
|
||
slopes[i].zDiff = vldPts[post_i].pt3D.z - vldPts[i].pt3D.z;
|
||
}
|
||
}
|
||
|
||
//搜索zWin极值
|
||
int _state = 0;
|
||
int pre_i = -1;
|
||
int sEdgePtIdx = -1;
|
||
int eEdgePtIdx = -1;
|
||
SSG_zWin* pre_data = NULL;
|
||
std::vector< SSG_zWin> zWinPeakP; //正的zWin
|
||
std::vector< SSG_zWin> zWinPeakM; //负的zWin
|
||
SSG_zWin* maxPkPlus = NULL;
|
||
SSG_zWin* maxPkMinus = NULL;
|
||
for (int i = 0, i_max = vldPts.size(); i < i_max; i++)
|
||
{
|
||
if (i == 275)
|
||
int kkk = 1;
|
||
SSG_zWin* curr_data = &slopes[i];
|
||
if ( curr_data->endPtIdx < 0)
|
||
{
|
||
if ( (i == i_max - 1) && (pre_data != NULL)) //最后一个
|
||
{
|
||
if (1 == _state) //上升
|
||
{
|
||
if (pre_data->zDiff > 0)
|
||
zWinPeakP.push_back(slopes[eEdgePtIdx]);
|
||
}
|
||
else if (2 == _state) //下降
|
||
{
|
||
if (pre_data->zDiff < 0)
|
||
zWinPeakM.push_back(slopes[eEdgePtIdx]);
|
||
}
|
||
}
|
||
continue;
|
||
}
|
||
|
||
if (NULL == pre_data)
|
||
{
|
||
sEdgePtIdx = i;
|
||
eEdgePtIdx = i;
|
||
pre_data = curr_data;
|
||
pre_i = i;
|
||
continue;
|
||
}
|
||
|
||
eEdgePtIdx = i;
|
||
double zDiffDelta = curr_data->zDiff - pre_data->zDiff;
|
||
switch (_state)
|
||
{
|
||
case 0: //初态
|
||
if (zDiffDelta < 0) //下降
|
||
{
|
||
if (curr_data->zDiff < 0)
|
||
{
|
||
maxPkMinus = curr_data;
|
||
_state = 2;
|
||
}
|
||
}
|
||
else if (zDiffDelta > 0) //上升
|
||
{
|
||
if (curr_data->zDiff > 0)
|
||
{
|
||
maxPkPlus = curr_data;
|
||
_state = 1;
|
||
}
|
||
}
|
||
break;
|
||
case 1: //上升
|
||
if (zDiffDelta < 0) //下降
|
||
{
|
||
if(maxPkPlus)
|
||
zWinPeakP.push_back(*maxPkPlus);
|
||
maxPkPlus = NULL;
|
||
if (curr_data->zDiff < 0)
|
||
maxPkMinus = curr_data;
|
||
else
|
||
maxPkMinus = NULL;
|
||
_state = 2;
|
||
}
|
||
else
|
||
{
|
||
//记录最大slope
|
||
if (curr_data->zDiff > 0)
|
||
{
|
||
if (NULL == maxPkPlus)
|
||
maxPkPlus = curr_data;
|
||
else
|
||
{
|
||
if(maxPkPlus->zDiff < curr_data->zDiff)
|
||
maxPkPlus = curr_data;
|
||
}
|
||
}
|
||
}
|
||
break;
|
||
case 2: //下降
|
||
if (zDiffDelta > 0) // 上升
|
||
{
|
||
if (maxPkMinus)
|
||
zWinPeakM.push_back(*maxPkMinus);
|
||
maxPkMinus = NULL;
|
||
if (curr_data->zDiff > 0)
|
||
maxPkPlus = curr_data;
|
||
else
|
||
maxPkPlus = NULL;
|
||
_state = 1;
|
||
}
|
||
else
|
||
{
|
||
//记录最大slope
|
||
if (curr_data->zDiff < 0)
|
||
{
|
||
if (NULL == maxPkMinus)
|
||
maxPkMinus = curr_data;
|
||
else
|
||
{
|
||
if (maxPkMinus->zDiff > curr_data->zDiff)
|
||
maxPkMinus = curr_data;
|
||
}
|
||
}
|
||
}
|
||
break;
|
||
default:
|
||
_state = 0;
|
||
break;
|
||
}
|
||
pre_data = curr_data;
|
||
pre_i = i;
|
||
}
|
||
|
||
//极值比较，在尺度窗口下寻找局部极值点
|
||
std::vector<SSG_zWin> validZWinPeakP;
|
||
double square_distTh = 4 * slopePara.LSlopeZWin * slopePara.LSlopeZWin; //2倍的cornerScale。
|
||
for (int i = 0, i_max = zWinPeakP.size(); i < i_max; i++)
|
||
{
|
||
if (zWinPeakP[i].zDiff < slopePara.validSlopeH)
|
||
continue;
|
||
|
||
bool isPeak = true;
|
||
//向前搜索
|
||
int currPtIdx = zWinPeakP[i].startPtIdx;
|
||
for (int j = i - 1; j >= 0; j--)
|
||
{
|
||
int prePtIdx = zWinPeakP[j].startPtIdx;
|
||
double dist = pow(vldPts[currPtIdx].pt3D.y - vldPts[prePtIdx].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ;
|
||
if (dist > square_distTh) //超出尺度窗口
|
||
break;
|
||
|
||
if (zWinPeakP[i].zDiff < zWinPeakP[j].zDiff)
|
||
{
|
||
isPeak = false;
|
||
break;
|
||
}
|
||
}
|
||
//向后搜索
|
||
if (true == isPeak)
|
||
{
|
||
currPtIdx = zWinPeakP[i].startPtIdx;
|
||
for (int j = i + 1; j < i_max; j++)
|
||
{
|
||
int postPtIdx = zWinPeakP[j].startPtIdx;
|
||
double dist = pow(vldPts[currPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2);
|
||
if (dist > square_distTh) //超出尺度窗口
|
||
break;
|
||
|
||
if (zWinPeakP[i].zDiff < zWinPeakP[j].zDiff)
|
||
{
|
||
isPeak = false;
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
if (true == isPeak)
|
||
{
|
||
SSG_zWin vldPk;
|
||
vldPk.startPtIdx = vldPts[zWinPeakP[i].startPtIdx].nPointIdx;
|
||
vldPk.endPtIdx = vldPts[zWinPeakP[i].endPtIdx].nPointIdx;
|
||
vldPk.zDiff = zWinPeakP[i].zDiff;
|
||
validZWinPeakP.push_back(vldPk);
|
||
//endingFlag[vldPk.startPtIdx] = 4;
|
||
}
|
||
}
|
||
|
||
//极值比较，在尺度窗口下寻找局部极值点
|
||
std::vector<SSG_zWin> validZWinPeakM;
|
||
for (int i = 0, i_max = zWinPeakM.size(); i < i_max; i++)
|
||
{
|
||
if (zWinPeakM[i].zDiff > -slopePara.validSlopeH)
|
||
continue;
|
||
|
||
bool isPeak = true;
|
||
//向前搜索
|
||
int currPtIdx = zWinPeakM[i].startPtIdx;
|
||
for (int j = i - 1; j >= 0; j--)
|
||
{
|
||
int prePtIdx = zWinPeakM[j].startPtIdx;
|
||
double dist = pow(vldPts[currPtIdx].pt3D.y - vldPts[prePtIdx].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ;
|
||
if (dist > square_distTh) //超出尺度窗口
|
||
break;
|
||
|
||
if (zWinPeakM[i].zDiff > zWinPeakM[j].zDiff)
|
||
{
|
||
isPeak = false;
|
||
break;
|
||
}
|
||
}
|
||
//向后搜索
|
||
if (true == isPeak)
|
||
{
|
||
currPtIdx = zWinPeakM[i].startPtIdx;
|
||
for (int j = i + 1; j < i_max; j++)
|
||
{
|
||
int postPtIdx = zWinPeakM[j].startPtIdx;
|
||
double dist = pow(vldPts[currPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2);
|
||
if (dist > square_distTh) //超出尺度窗口
|
||
break;
|
||
|
||
if (zWinPeakM[i].zDiff > zWinPeakM[j].zDiff)
|
||
{
|
||
isPeak = false;
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
if (true == isPeak)
|
||
{
|
||
SSG_zWin vldPk;
|
||
vldPk.startPtIdx = vldPts[zWinPeakM[i].startPtIdx].nPointIdx;
|
||
vldPk.endPtIdx = vldPts[zWinPeakM[i].endPtIdx].nPointIdx;
|
||
vldPk.zDiff = zWinPeakM[i].zDiff;
|
||
validZWinPeakM.push_back(vldPk);
|
||
//endingFlag[vldPk.endPtIdx] = 5;
|
||
}
|
||
}
|
||
|
||
//对validCornerPeakP进行配对
|
||
std::vector< SSG_featureSemiCircle> line_features;
|
||
for (int i = 0, i_max = localMin.size(); i < i_max; i++)
|
||
{
|
||
int ptIdx = localMin[i].jumpPos2D.y;
|
||
if (endingFlag[ptIdx] > 0) //已经被处理
|
||
continue;
|
||
|
||
//向前搜索
|
||
int preIdx = -1;
|
||
for (int j = ptIdx - 1; j >= 0; j--)
|
||
{
|
||
if (endingFlag[j] > 0)
|
||
{
|
||
preIdx = j;
|
||
break;
|
||
}
|
||
}
|
||
//身后搜索
|
||
int postIdx = -1;
|
||
for (int j = ptIdx + 1; j < dataSize; j++)
|
||
{
|
||
if (endingFlag[j] > 0)
|
||
{
|
||
postIdx = j;
|
||
break;
|
||
}
|
||
}
|
||
if ((preIdx >= 0) && (postIdx >= 0))
|
||
{
|
||
int preFlag = endingFlag[preIdx];
|
||
int postFlag = endingFlag[postIdx];
|
||
if ((2 == preFlag ) && (3 == postFlag)) //段落情况，筛网悬空会出现这种情况
|
||
{
|
||
double yDist = abs(lineData[preIdx].pt3D.y - lineData[postIdx].pt3D.y);
|
||
if (yDist < sieveDiameter * 4) //valid
|
||
{
|
||
SSG_featureSemiCircle a_feature;
|
||
a_feature.flag = 0;
|
||
a_feature.startPtIdx = preIdx;
|
||
a_feature.endPtIdx = postIdx;
|
||
double midY = (lineData[preIdx].pt3D.y + lineData[postIdx].pt3D.y) / 2.0;
|
||
a_feature.width = yDist;
|
||
a_feature.lineIdx = lineIdx;
|
||
a_feature.midPtIdx = _getYNearestPtIdx(lineData, preIdx, postIdx, midY);
|
||
a_feature.midPt = lineData[a_feature.midPtIdx].pt3D;
|
||
if (a_feature.midPtIdx >= 0);
|
||
{
|
||
int pkIdx = _getSegPeak(lineData, preIdx, postIdx);
|
||
a_feature.pkHeight = lineData[pkIdx].pt3D.z;
|
||
line_features.push_back(a_feature);
|
||
for (int m = a_feature.startPtIdx; m <= a_feature.endPtIdx; m++)
|
||
{
|
||
if (0 == endingFlag[m])
|
||
endingFlag[m] = 1;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
else if ( ((5 == preFlag) && (4 == postFlag)) || //有底板情况，筛网放置在一个平面上会出现这种情况
|
||
((2 == preFlag) && (4 == postFlag)) ||
|
||
((5 == preFlag) && (3 == postFlag)))
|
||
{
|
||
double yDist = abs(lineData[preIdx].pt3D.y - lineData[postIdx].pt3D.y);
|
||
if (yDist < sieveDiameter * 4) //valid
|
||
{
|
||
SSG_featureSemiCircle a_feature;
|
||
a_feature.flag = 0;
|
||
a_feature.startPtIdx = preIdx;
|
||
a_feature.endPtIdx = postIdx;
|
||
a_feature.width = abs(lineData[preIdx].pt3D.y - lineData[postIdx].pt3D.y);
|
||
a_feature.lineIdx = lineIdx;
|
||
double midY = (lineData[preIdx].pt3D.y + lineData[postIdx].pt3D.y) / 2.0;
|
||
a_feature.midPtIdx = _getYNearestPtIdx(lineData, preIdx, postIdx, midY);
|
||
a_feature.midPt = lineData[a_feature.midPtIdx].pt3D;
|
||
if (a_feature.midPtIdx >= 0)
|
||
{
|
||
int pkPtIdx = _getSegPeak(lineData, preIdx, postIdx);
|
||
a_feature.pkHeight = lineData[pkPtIdx].pt3D.z;
|
||
line_features.push_back(a_feature);
|
||
for (int m = a_feature.startPtIdx; m <= a_feature.endPtIdx; m++)
|
||
{
|
||
if (0 == endingFlag[m])
|
||
endingFlag[m] = 1;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
//根据孔洞目标进行合并
|
||
for (int i = 0, i_max = line_features.size(); i < i_max; i++)
|
||
{
|
||
if (FEATURE_FLAG_INVALID == line_features[i].flag)
|
||
continue;
|
||
|
||
if (i < i_max - 1)
|
||
{
|
||
int idx1 = line_features[i].endPtIdx + 1;
|
||
int holeId1 = holeMask.at<int>(idx1, lineIdx);
|
||
int idx2 = line_features[i + 1].startPtIdx - 1;
|
||
int holeId2 = holeMask.at<int>(idx2, lineIdx);
|
||
if( (holeId1 > 0) && (holeId1 == holeId2))
|
||
{
|
||
//合并
|
||
line_features[i].endPtIdx = line_features[i + 1].endPtIdx;
|
||
double midY = (lineData[line_features[i].startPtIdx].pt3D.y + lineData[line_features[i].endPtIdx].pt3D.y) / 2;
|
||
line_features[i].midPtIdx = _getYNearestPtIdx(lineData, line_features[i].startPtIdx, line_features[i].endPtIdx, midY);
|
||
line_features[i].midPt = lineData[line_features[i].midPtIdx].pt3D;
|
||
line_features[i].width = abs(lineData[line_features[i].startPtIdx].pt3D.y - lineData[line_features[i].endPtIdx].pt3D.y);
|
||
int pkPtIdx = _getSegPeak(lineData, line_features[i].startPtIdx, line_features[i].endPtIdx);
|
||
line_features[i].pkHeight = lineData[pkPtIdx].pt3D.z;
|
||
result_features.push_back(line_features[i]);
|
||
line_features[i + 1].flag = FEATURE_FLAG_INVALID;
|
||
}
|
||
else
|
||
{
|
||
result_features.push_back(line_features[i]);
|
||
}
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
|
||
|
||
//计算检查长度时使用街区距离以避免太多的平方和开方运算
|
||
//如果z方向为Jump返回True。Jump定义Seed前一个点z的变化大于门限
|
||
bool _getPtPreMaxDelta(
|
||
SVzNL3DPosition* lineData,
|
||
int dataSize,
|
||
int seedIdx,
|
||
const double chkLen,
|
||
double* maxDeltaY,
|
||
double* maxDeltaZ,
|
||
int* endingIdx)
|
||
{
|
||
double seedY = lineData[seedIdx].pt3D.y;
|
||
double seedZ = lineData[seedIdx].pt3D.z;
|
||
*maxDeltaY = 0; //初始化成0
|
||
*maxDeltaZ = 0;
|
||
*endingIdx = -1;//初始化成无效值，表示没有足够长度
|
||
if (seedIdx == 0) //无法向前检查
|
||
return false;
|
||
|
||
double max_dy = -1;
|
||
double max_dz = -1;
|
||
for (int i = seedIdx - 1; i >= 0; i--)
|
||
{
|
||
SVzNL3DPoint* a_pt = &(lineData[i].pt3D);
|
||
if (a_pt->z > 1e-4)
|
||
{
|
||
double dy = abs(a_pt->y - seedY);
|
||
double dz = abs(a_pt->z - seedZ);
|
||
double dist = dy + dz;
|
||
|
||
if (dist > chkLen)
|
||
{
|
||
*endingIdx = i;
|
||
*maxDeltaY = max_dy;
|
||
*maxDeltaZ = max_dz;
|
||
if (i == seedIdx - 1)
|
||
return true;
|
||
else
|
||
return false;
|
||
}
|
||
if (max_dy < 0)
|
||
{
|
||
max_dy = dy;
|
||
max_dz = dz;
|
||
}
|
||
else
|
||
{
|
||
max_dy = max_dy < dy ? dy : max_dy;
|
||
max_dz = max_dz < dz ? dz : max_dz;
|
||
}
|
||
}
|
||
}
|
||
*endingIdx = -1;//没有足够长度
|
||
return false;
|
||
}
|
||
|
||
//计算检查长度时使用街区距离以避免太多的平方和开方运算
|
||
//如果z方向为Jump返回True。Jump定义Seed前一个点z的变化大于门限
|
||
bool _getPtPostMaxDelta(
|
||
SVzNL3DPosition* lineData,
|
||
int dataSize,
|
||
int seedIdx,
|
||
const double chkLen,
|
||
double* maxDeltaY,
|
||
double* maxDeltaZ,
|
||
int* endingIdx)
|
||
{
|
||
double seedY = lineData[seedIdx].pt3D.y;
|
||
double seedZ = lineData[seedIdx].pt3D.z;
|
||
*maxDeltaY = 0; //初始化成0
|
||
*maxDeltaZ = 0;
|
||
*endingIdx = -1;//初始化成无效值，表示没有足够长度
|
||
if (seedIdx == dataSize-1) //无法向后检查
|
||
return false;
|
||
|
||
|
||
double max_dy = -1;
|
||
double max_dz = -1;
|
||
for (int i = seedIdx + 1; i < dataSize; i++)
|
||
{
|
||
SVzNL3DPoint* a_pt = &(lineData[i].pt3D);
|
||
if (a_pt->z > 1e-4)
|
||
{
|
||
double dy = abs(a_pt->y - seedY);
|
||
double dz = abs(a_pt->z - seedZ);
|
||
double dist = dy + dz;
|
||
if (dist > chkLen)
|
||
{
|
||
*endingIdx = i;
|
||
*maxDeltaY = max_dy;
|
||
*maxDeltaZ = max_dz;
|
||
if (i == seedIdx - 1)
|
||
return true;
|
||
else
|
||
return false;
|
||
}
|
||
if (max_dy < 0)
|
||
{
|
||
max_dy = dy;
|
||
max_dz = dz;
|
||
}
|
||
else
|
||
{
|
||
max_dy = max_dy < dy ? dy : max_dy;
|
||
max_dz = max_dz < dz ? dz : max_dz;
|
||
}
|
||
}
|
||
}
|
||
*endingIdx = -1;//表示没有足够长度
|
||
return false;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 使用模板法提取直角特征
|
||
/// 水平向下直角特征：拐点左侧deltaZ在一个很小的范围内；拐点右侧deltaY在一个很小的范围内
|
||
/// </summary>
|
||
/// <param name="lineData"></param>
|
||
/// <param name="dataSize"></param>
|
||
/// <param name="lineIdx"></param>
|
||
/// <param name="slopeParam"></param>
|
||
/// <param name="valleyPara"></param>
|
||
/// <param name="line_features"></param>
|
||
void sg_getLineRigthAngleFeature(
|
||
SVzNL3DPosition* lineData,
|
||
int dataSize,
|
||
int lineIdx,
|
||
const SSG_lineRightAngleParam templatePara_HF,
|
||
const SSG_lineRightAngleParam templatePara_FH,
|
||
const SSG_lineRightAngleParam templatePara_HR,
|
||
const SSG_lineRightAngleParam templatePara_RH,
|
||
SSG_lineFeature* line_features)
|
||
{
|
||
double chkLen = templatePara_HF.H_len > templatePara_HF.V_len ? templatePara_HF.V_len : templatePara_HF.H_len;
|
||
chkLen = chkLen > templatePara_FH.H_len ? templatePara_FH.H_len : chkLen;
|
||
chkLen = chkLen > templatePara_FH.V_len ? templatePara_FH.V_len : chkLen;
|
||
chkLen = chkLen > templatePara_HR.H_len ? templatePara_HR.H_len : chkLen;
|
||
chkLen = chkLen > templatePara_HR.V_len ? templatePara_HR.V_len : chkLen;
|
||
chkLen = chkLen > templatePara_RH.H_len ? templatePara_RH.H_len : chkLen;
|
||
chkLen = chkLen > templatePara_RH.V_len ? templatePara_RH.V_len : chkLen;
|
||
|
||
double maxDelta = templatePara_HF.maxDelta < templatePara_FH.maxDelta ? templatePara_FH.maxDelta : templatePara_HF.maxDelta;
|
||
maxDelta = maxDelta < templatePara_HR.maxDelta ? templatePara_HR.maxDelta : maxDelta;
|
||
maxDelta = maxDelta < templatePara_RH.maxDelta ? templatePara_RH.maxDelta : maxDelta;
|
||
|
||
std::vector<int> types;
|
||
std::vector<int> jumpFlags;
|
||
types.resize(dataSize);
|
||
jumpFlags.resize(dataSize);
|
||
for (int i = 0; i < dataSize; i++)
|
||
{
|
||
if (i == 98)
|
||
int kkk = 1;
|
||
types[i] = 0;
|
||
jumpFlags[i] = 0;
|
||
double maxDeltaY_pre = 0;
|
||
double maxDeltaZ_pre = 0;
|
||
int endingIdx_pre = -1;
|
||
double maxDeltaY_post = 0;
|
||
double maxDeltaZ_post = 0;
|
||
int endingIdx_post = -1;
|
||
if (lineData[i].pt3D.z < 1e-4)
|
||
continue;
|
||
|
||
//首先以最小的检查长度进行直角判断
|
||
bool isJump_pre = _getPtPreMaxDelta(
|
||
lineData,
|
||
dataSize,
|
||
i,
|
||
chkLen,
|
||
&maxDeltaY_pre,
|
||
&maxDeltaZ_pre,
|
||
&endingIdx_pre);
|
||
|
||
bool isJump_post = _getPtPostMaxDelta(
|
||
lineData,
|
||
dataSize,
|
||
i,
|
||
chkLen,
|
||
&maxDeltaY_post,
|
||
&maxDeltaZ_post,
|
||
&endingIdx_post);
|
||
//共四种直角：水平-向下；向下-水平；水平-向上；向上-水平
|
||
if ((endingIdx_pre < 0) || (endingIdx_post < 0)) //没有足够长度
|
||
continue;
|
||
|
||
if (true == isJump_pre)
|
||
maxDeltaY_pre = 0; //实际可能会很大
|
||
if (true == isJump_post)
|
||
maxDeltaY_post = 0; //实际可能会很大
|
||
|
||
if ((maxDeltaZ_pre < maxDelta) && (maxDeltaY_post < maxDelta)) //可能为HX型（水平-垂直）
|
||
{
|
||
if (lineData[endingIdx_post].pt3D.z > lineData[i].pt3D.z) //下降
|
||
{
|
||
//使用templatePara_HX的参数重新进行迭代计算
|
||
isJump_pre = _getPtPreMaxDelta(
|
||
lineData,
|
||
dataSize,
|
||
i,
|
||
templatePara_HF.H_len,
|
||
&maxDeltaY_pre,
|
||
&maxDeltaZ_pre,
|
||
&endingIdx_pre);
|
||
isJump_post = _getPtPostMaxDelta(
|
||
lineData,
|
||
dataSize,
|
||
i,
|
||
templatePara_HF.V_len,
|
||
&maxDeltaY_post,
|
||
&maxDeltaZ_post,
|
||
&endingIdx_post);
|
||
|
||
if (true == isJump_pre)
|
||
maxDeltaY_pre = 0; //实际可能会很大
|
||
if (true == isJump_post)
|
||
maxDeltaY_post = 0; //实际可能会很大
|
||
if ((maxDeltaZ_pre < templatePara_HF.maxDelta) && (maxDeltaY_post < templatePara_HF.maxDelta))
|
||
{
|
||
if ((true == isJump_pre) || (true == isJump_post))
|
||
jumpFlags[i] = 1;
|
||
types[i] = LINE_FEATURE_RIGHT_ANGLE_HF;
|
||
}
|
||
}
|
||
else //上升
|
||
{
|
||
//使用templatePara_HX的参数重新进行迭代计算
|
||
isJump_pre = _getPtPreMaxDelta(
|
||
lineData,
|
||
dataSize,
|
||
i,
|
||
templatePara_HR.H_len,
|
||
&maxDeltaY_pre,
|
||
&maxDeltaZ_pre,
|
||
&endingIdx_pre);
|
||
isJump_post = _getPtPostMaxDelta(
|
||
lineData,
|
||
dataSize,
|
||
i,
|
||
templatePara_HR.V_len,
|
||
&maxDeltaY_post,
|
||
&maxDeltaZ_post,
|
||
&endingIdx_post);
|
||
|
||
if (true == isJump_pre)
|
||
maxDeltaY_pre = 0; //实际可能会很大
|
||
if (true == isJump_post)
|
||
maxDeltaY_post = 0; //实际可能会很大
|
||
if ((maxDeltaZ_pre < templatePara_HF.maxDelta) && (maxDeltaY_post < templatePara_HF.maxDelta))
|
||
{
|
||
if ((true == isJump_pre) || (true == isJump_post))
|
||
jumpFlags[i] = 1;
|
||
types[i] = LINE_FEATURE_RIGHT_ANGLE_HR;;
|
||
}
|
||
}
|
||
}
|
||
else if ((maxDeltaY_pre < maxDelta) && (maxDeltaZ_post < maxDelta)) //可能为XH型（垂直-水平）
|
||
{
|
||
if (lineData[i].pt3D.z > lineData[endingIdx_pre].pt3D.z) //下降
|
||
{
|
||
//使用templatePara_XH的参数重新进行迭代计算
|
||
isJump_pre = _getPtPreMaxDelta(
|
||
lineData,
|
||
dataSize,
|
||
i,
|
||
templatePara_FH.V_len,
|
||
&maxDeltaY_pre,
|
||
&maxDeltaZ_pre,
|
||
&endingIdx_pre);
|
||
isJump_post = _getPtPostMaxDelta(
|
||
lineData,
|
||
dataSize,
|
||
i,
|
||
templatePara_FH.H_len,
|
||
&maxDeltaY_post,
|
||
&maxDeltaZ_post,
|
||
&endingIdx_post);
|
||
|
||
if (true == isJump_pre)
|
||
maxDeltaY_pre = 0; //实际可能会很大
|
||
if (true == isJump_post)
|
||
maxDeltaY_post = 0; //实际可能会很大
|
||
if ((maxDeltaY_pre < templatePara_FH.maxDelta) && (maxDeltaZ_post < templatePara_FH.maxDelta))
|
||
{
|
||
if ((true == isJump_pre) || (true == isJump_post))
|
||
jumpFlags[i] = 1;
|
||
types[i] = LINE_FEATURE_RIGHT_ANGLE_FH;
|
||
}
|
||
}
|
||
else//上升
|
||
{
|
||
//使用templatePara_XH的参数重新进行迭代计算
|
||
isJump_pre = _getPtPreMaxDelta(
|
||
lineData,
|
||
dataSize,
|
||
i,
|
||
templatePara_RH.V_len,
|
||
&maxDeltaY_pre,
|
||
&maxDeltaZ_pre,
|
||
&endingIdx_pre);
|
||
isJump_post = _getPtPostMaxDelta(
|
||
lineData,
|
||
dataSize,
|
||
i,
|
||
templatePara_RH.H_len,
|
||
&maxDeltaY_post,
|
||
&maxDeltaZ_post,
|
||
&endingIdx_post);
|
||
|
||
if (true == isJump_pre)
|
||
maxDeltaY_pre = 0; //实际可能会很大
|
||
if (true == isJump_post)
|
||
maxDeltaY_post = 0; //实际可能会很大
|
||
if ((maxDeltaY_pre < templatePara_RH.maxDelta) && (maxDeltaZ_post < templatePara_RH.maxDelta))
|
||
{
|
||
if ((true == isJump_pre) || (true == isJump_post))
|
||
jumpFlags[i] = 1;
|
||
types[i] = LINE_FEATURE_RIGHT_ANGLE_RH;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
//对连续的同类型直角进行处理
|
||
int i = 0;
|
||
int pre_type = -1;
|
||
while(i < dataSize)
|
||
{
|
||
int sIdx = i;
|
||
int eIdx = i;
|
||
if (types[i] != pre_type)
|
||
{
|
||
int sIdx = i;
|
||
int eIdx = i;
|
||
int jumpPos = jumpFlags[i] == 1 ? i : -1; //跳变位置
|
||
for (int j = i; j < dataSize; j++)
|
||
{
|
||
if (types[j] != types[i])
|
||
break;
|
||
else
|
||
{
|
||
eIdx = j;
|
||
if (jumpFlags[j] == 1)
|
||
jumpPos = j;
|
||
}
|
||
}
|
||
if (types[i] > 0)
|
||
{
|
||
SSG_basicFeature1D a_rightAngle;
|
||
a_rightAngle.featureType = types[i];
|
||
if (jumpPos >= 0)
|
||
{
|
||
a_rightAngle.jumpPos2D = { lineIdx, jumpPos };
|
||
a_rightAngle.jumpPos = lineData[jumpPos].pt3D;
|
||
}
|
||
else
|
||
{
|
||
int pos = (sIdx + eIdx) / 2;//取中点
|
||
if (lineData[pos].pt3D.z < 1e-4)
|
||
{
|
||
int w = pos - sIdx;
|
||
for (int m = 1; m <= w; m++)
|
||
{
|
||
if (lineData[pos + m].pt3D.z > 1e-4)
|
||
{
|
||
pos = pos + m;
|
||
break;
|
||
}
|
||
else if (lineData[pos - m].pt3D.z > 1e-4)
|
||
{
|
||
pos = pos - m;
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
a_rightAngle.jumpPos2D = { lineIdx, pos };
|
||
a_rightAngle.jumpPos = lineData[pos].pt3D;
|
||
}
|
||
line_features->features.push_back(a_rightAngle);
|
||
}
|
||
pre_type = types[i];
|
||
i = eIdx + 1;
|
||
}
|
||
else
|
||
i++;
|
||
}
|
||
return;
|
||
}
|