jerryzeng/algoLib
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

临时提交

Browse Source
This commit is contained in:
jerryzeng 2025-07-18 16:27:34 +08:00
parent 724a5a15e6
commit 7af730c44f
4 changed files with 99 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
sourceCode/SG_bagPositioning.cpp
View File

@ -2386,6 +2386,12 @@ void sg_getBagPosition(
if (hLine == 116)
int kkk = 1;
std::vector<SVzNL3DPosition> smoothData;
//sg_lineSegSmoothing(
// hLines[hLine],
// algoParam.cornerParam.minEndingGap, //分段的Y间隔。大于此间隔，为新的分段
// algoParam.cornerParam.minEndingGap_z,//分段的Z间隔。大于此间隔，为新的分段
// 5,
// smoothData);
sg_lineDataSmoothing(hLines[hLine], 5, smoothData);
SSG_lineFeature a_hLine_featrues;
a_hLine_featrues.lineIdx = hLine;
@ -3214,6 +3220,7 @@ if (col == 586)
sg_getLocalPeaks_distTransform(distTransform, dt_peaks, searchWin);
//获取Peaks
int invlidDistToEdge = 50; //距离边缘近的Peak是非法点。
double minPeakValue = algoParam.bagParam.bagW / 8;
std::vector<SSG_2DValueI> peaks;
for (int i = 0; i < dt_peaks.size(); i++)
{
@ -3223,7 +3230,8 @@ if (col == 586)
int y_diff_0 = dt_peaks[i].y;//距上边距离
int y_diff_1 = distTransform.rows - dt_peaks[i].y;//距下边距离
if ((x_diff_0 < invlidDistToEdge) || (x_diff_1 < invlidDistToEdge) ||
(y_diff_0 < invlidDistToEdge) || (y_diff_1 < invlidDistToEdge))
(y_diff_0 < invlidDistToEdge) || (y_diff_1 < invlidDistToEdge) ||
(dt_peaks[i].valueD < minPeakValue))
continue;
//在distTranformIndexing中回找坐标
@ -3251,7 +3259,7 @@ if (col == 586)
int peakRgnId = 1;
for (int i = 0, i_max = peaks.size(); i < i_max; i++)
{
if (i == 2)
if (i == 3)
int kkk = 1;
SVzNL3DPosition* pk_pt = &(laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y]);

7
sourceCode/SG_baseAlgo_Export.h
View File

@ -35,6 +35,13 @@ SG_APISHARED_EXPORT void sg_lineDataSmoothing(
std::vector<SVzNL3DPosition>& input,
int smoothWin,
std::vector<SVzNL3DPosition>& output);
//分段平滑，这样不会影响分段端点
SG_APISHARED_EXPORT void sg_lineSegSmoothing(
std::vector<SVzNL3DPosition>& input,
double seg_y_deltaTh, //分段的Y间隔。大于此间隔，为新的分段
double seg_z_deltaTh,//分段的Z间隔。大于此间隔，为新的分段
int smoothWin,
std::vector<SVzNL3DPosition>& output);
//VZ_APISHARED_EXPORT void sg_getLineMeanVar();
SG_APISHARED_EXPORT void sg_getLineLVFeature(

3
sourceCode/SG_baseDataType.h
View File

@ -148,7 +148,8 @@ typedef struct
typedef struct
{
double minEndingGap; //连续段门限。大于此门限，为不连续
double minEndingGap; //y方向连续段门限。大于此门限，为不连续
double minEndingGap_z; //z方向连续段门限。大于此门限，为不连续
double scale; //计算方向角的窗口比例尺
double cornerTh; //拐角门限，大于此门限，为有效拐点
double jumpCornerTh_1; //判断拐角是否为跳变的两个门限。当一个门限大于jumpCornerTh_1且另一个小于jumpCornerTh_2时跳变

84
sourceCode/SG_lineFeature.cpp
View File

@ -62,7 +62,10 @@ SSG_meanVar _computeMeanVar(double* data, int size)
return a_meanVar;
}
void sg_lineDataSmoothing(std::vector<SVzNL3DPosition>& input, int smoothWin, std::vector<SVzNL3DPosition>& output)
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());
@ -91,6 +94,57 @@ void sg_lineDataSmoothing(std::vector<SVzNL3DPosition>& input, int smoothWin, st
return;
}
//对扫描线按分段进行平滑
void sg_lineSegSmoothing(
std::vector<SVzNL3DPosition>& input,
double seg_y_deltaTh, //分段的Y间隔。大于此间隔，为新的分段
double seg_z_deltaTh,//分段的Z间隔。大于此间隔，为新的分段
int smoothWin,
std::vector<SVzNL3DPosition>& output)
{
output.resize(input.size()); // 预分配足够的空间
std::copy(input.begin(), input.end(), output.begin());
int dataSize = input.size();
//计算分段
std::vector<std::vector<SVzNL3DPosition>> segs;
std::vector<SVzNL3DPosition> a_seg;
for (int i = 0; i < dataSize; i++)
{
SVzNL3DPosition a_pt = input[i];
//seg判断
if (a_pt.pt3D.z > 1e-4)
{
if (a_seg.size() == 0)
a_seg.push_back(a_pt);
else //检查两点距离
{
SVzNL3DPosition pre_pt = a_seg.back();
double diff_z = abs(a_pt.pt3D.z - pre_pt.pt3D.z);
double diff_y = abs(a_pt.pt3D.y - pre_pt.pt3D.y);
if ((diff_y > seg_y_deltaTh) || (diff_z > seg_z_deltaTh))
{
segs.push_back(a_seg);
a_seg.clear();
a_seg.push_back(a_pt);
}
}
}
}
//last
if (a_seg.size() > 0)
segs.push_back(a_seg);
//分段平滑
for (int i = 0, i_max = segs.size(); i < i_max; i++)
{
std::vector<SVzNL3DPosition> seg_output;
sg_lineDataSmoothing(segs[i], smoothWin, seg_output);
output.insert(output.end(), seg_output.begin(), seg_output.end());
}
return;
}
//滤除离群点：z跳变门限方法
void sg_lineDataRemoveOutlier(SVzNL3DPosition* lineData, int dataSize, SSG_outlierFilterParam filterParam, std::vector<SVzNL3DPosition>& filerData, std::vector<int>& noisePts)
{
@ -1094,21 +1148,40 @@ void sg_getLineCornerFeature(
line_features->lineIdx = lineIdx;
if (lineIdx == 538)
int kkk = 1;
//去除零点
std::vector< SVzNL3DPosition> vldPts;
std::vector<SSG_RUN> segs;
//修改seg的定义。seg端点是两点间距离大于门限的点
int segStart = -1, segEnd = -1;
for (int i = 0; i < dataSize; i++)
{
if ( (lineIdx == 399)&&(i==568))
int kkk = 1;
SVzNL3DPosition a_pt = lineData[i];
a_pt.nPointIdx = i;
if (lineData[i].pt3D.z > 1e-4)
vldPts.push_back(a_pt);
//seg判断
if (lineData[i].pt3D.z > 1e-4)
{
if (segStart < 0)
segStart = i;
else //检查两点距离
{
SVzNL3DPosition pre_pt = lineData[i-1];
double diff_z = abs(a_pt.pt3D.z - pre_pt.pt3D.z);
if (diff_z > cornerPara.minEndingGap_z)
{
SSG_RUN a_run;
a_run.start = segStart;
a_run.len = segEnd - segStart + 1;
a_run.value = 1;
segs.push_back(a_run);
segStart = i;
}
}
segEnd = i;
vldPts.push_back(a_pt);
}
else
{
@ -1139,6 +1212,8 @@ void sg_getLineCornerFeature(
corners.resize(vldPts.size());
for (int i = 0, i_max = vldPts.size(); i < i_max; i++)
{
if ((lineIdx == 399) && (i == 419))
int kkk = 1;
//前向寻找
int pre_i = -1;
for (int j = i - 1; j >= 0; j--)
@ -1332,7 +1407,8 @@ void sg_getLineCornerFeature(
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) //ºÏ²¢
double z_diff = abs(lineData[idx_1].pt3D.z - lineData[idx_2].pt3D.z);
if ( (y_diff < cornerPara.minEndingGap) && (z_diff < cornerPara.minEndingGap_z)) //合并
{
int idx_end = nxt_seg->start + nxt_seg->len - 1;
nxt_seg->start = curr_seg->start;