diff --git a/sourceCode/SG_bagPositioning.cpp b/sourceCode/SG_bagPositioning.cpp index c52dd4b..5fd70c6 100644 --- a/sourceCode/SG_bagPositioning.cpp +++ b/sourceCode/SG_bagPositioning.cpp @@ -2386,6 +2386,12 @@ void sg_getBagPosition( if (hLine == 116) int kkk = 1; std::vector 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 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]); diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index 32c2087..c07da9c 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -35,6 +35,13 @@ SG_APISHARED_EXPORT void sg_lineDataSmoothing( std::vector& input, int smoothWin, std::vector& output); +//分段平滑，这样不会影响分段端点 +SG_APISHARED_EXPORT void sg_lineSegSmoothing( + std::vector& input, + double seg_y_deltaTh, //分段的Y间隔。大于此间隔，为新的分段 + double seg_z_deltaTh,//分段的Z间隔。大于此间隔，为新的分段 + int smoothWin, + std::vector& output); //VZ_APISHARED_EXPORT void sg_getLineMeanVar(); SG_APISHARED_EXPORT void sg_getLineLVFeature( diff --git a/sourceCode/SG_baseDataType.h b/sourceCode/SG_baseDataType.h index 5cc63de..6721679 100644 --- a/sourceCode/SG_baseDataType.h +++ b/sourceCode/SG_baseDataType.h @@ -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时跳变 diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index 72933bd..23d9ca2 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -62,7 +62,10 @@ SSG_meanVar _computeMeanVar(double* data, int size) return a_meanVar; } -void sg_lineDataSmoothing(std::vector& input, int smoothWin, std::vector& output) +void sg_lineDataSmoothing( + std::vector& input, + int smoothWin, + std::vector& output) { output.resize(input.size()); // 预分配足够的空间 std::copy(input.begin(), input.end(), output.begin()); @@ -91,6 +94,57 @@ void sg_lineDataSmoothing(std::vector& input, int smoothWin, st return; } +//对扫描线按分段进行平滑 +void sg_lineSegSmoothing( + std::vector& input, + double seg_y_deltaTh, //分段的Y间隔。大于此间隔，为新的分段 + double seg_z_deltaTh,//分段的Z间隔。大于此间隔，为新的分段 + int smoothWin, + std::vector& output) +{ + output.resize(input.size()); // 预分配足够的空间 + std::copy(input.begin(), input.end(), output.begin()); + + int dataSize = input.size(); + //计算分段 + std::vector> segs; + std::vector 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 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& filerData, std::vector& noisePts) { @@ -1094,21 +1148,40 @@ void sg_getLineCornerFeature( line_features->lineIdx = lineIdx; if (lineIdx == 538) int kkk = 1; - //去除零点 std::vector< SVzNL3DPosition> vldPts; std::vector 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;