diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h
index 89b6185..31564f2 100644
--- a/sourceCode/SG_baseAlgo_Export.h
+++ b/sourceCode/SG_baseAlgo_Export.h
@@ -302,12 +302,12 @@ SG_APISHARED_EXPORT void wd_getLineCorerFeature(
///
/// 提取激光线上的与XY平面垂直的特征（垂直段）
///
-SG_APISHARED_EXPORT void wd_getXYVertialFeature_peakCornerMethod(
+SG_APISHARED_EXPORT void wd_getXYVertialFeature_dirAngleMethod(
std::vector< SVzNL3DPosition>& lineData,
int lineIdx,
const double peakChkWin,
const SSG_cornerParam cornerPara,
- std::vector& xyVerticalSegs //环
+ std::vector& xyVerticalFlags //环
);
/// 提取激光线上的拐点特征。是在PSM， LVTypeFeature, BQ等拐点算法的基础上的版本。
@@ -795,9 +795,9 @@ SG_APISHARED_EXPORT Plane robustFitPlane(
SG_APISHARED_EXPORT Plane ransacFitPlane(
const std::vector& points,
std::vector& out_inliers,
- float dist_thresh = 0.5f, // 内点距离阈值
+ float dist_thresh = 0.3f, // 内点距离阈值
int max_iter = 1000, // 最大迭代
- int stop_no_improve = 150 // 连续多少次无提升就提前退出
+ int stop_no_improve = 250 // 连续多少次无提升就提前退出
);
//计算一个平面调平参数。
diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp
index 808d6ab..c73ee10 100644
--- a/sourceCode/SG_lineFeature.cpp
+++ b/sourceCode/SG_lineFeature.cpp
@@ -5713,109 +5713,33 @@ void wd_getLineCorerFeature(
/// （4）判断拐角是否为跳变
///
///
-void wd_getXYVertialFeature_peakCornerMethod(
+void wd_getXYVertialFeature_dirAngleMethod(
std::vector< SVzNL3DPosition>& lineData,
int lineIdx,
const double peakChkWin,
const SSG_cornerParam cornerPara,
- std::vector& xyVerticalSegs
+ std::vector& xyVerticalFlags
)
{
- double arcPerPointCornerMinValue = 5; //arc上每个点的转角最小值
- double arcTotalCornerMinValue = 30; //整个Arc的转角最小值
+ if (lineIdx == 1062)
+ int kkk = 1;
+ double minVerticalAngle = 75.0; //arc上每个点的转角最小值
+
+ xyVerticalFlags.resize(lineData.size());
//计算前向角和后向角
std::vector< SSG_pntDirAngle> ptDirAngles;
wd_computeDirAngle_wholeLine(lineData, cornerPara, ptDirAngles);
- double cornerMergeScale = cornerPara.scale;
- //提取backwardAngle极值（较早实现函数可以使用此函数进行代码优化）
- std::vector< SSG_pntDirAngle> backwardAnglePeakP;
- std::vector< SSG_pntDirAngle> backwardAnglePeakM;
- wd_searchBackwardAnglePeaks(
- ptDirAngles,
- lineData,
- cornerPara.cornerTh,
- cornerMergeScale,
- backwardAnglePeakP,
- backwardAnglePeakM
- );
-
- //提取forwardAngle极值（较早实现函数可以使用此函数进行代码优化）
- std::vector< SSG_pntDirAngle> forwardAnglePeakP;
- std::vector< SSG_pntDirAngle> forwardAnglePeakM;
- wd_searchForwardAnglePeaks(
- ptDirAngles,
- lineData,
- cornerPara.cornerTh,
- cornerMergeScale,
- forwardAnglePeakP,
- forwardAnglePeakM
- );
-
- //匹配forwardAnglePeakM和backwardAnglePeakM, forwardAnglePeakM在前，backwardAnglePeakM在后
- for (int i = 0; i < (int)forwardAnglePeakM.size(); i++)
+ for (int i = 0; i < (int)ptDirAngles.size(); i++)
{
- for (int j = 0; j < (int)backwardAnglePeakM.size(); j++)
- {
- if (backwardAnglePeakM[j].pntIdx > forwardAnglePeakM[i].pntIdx)
- {
- int pntIdx_1 = forwardAnglePeakM[i].pntIdx;
- int pntIdx_2 = backwardAnglePeakM[j].pntIdx;
- //计算两者的角度
- double diffY = abs(lineData[pntIdx_2].pt3D.y - lineData[pntIdx_1].pt3D.y);
- if (diffY < cornerMergeScale)
- {
- double tanValue = (lineData[pntIdx_2].pt3D.z - lineData[pntIdx_1].pt3D.z) / diffY;
- double angle = atan(tanValue) * 180.0 / PI;
- if (abs(angle) > cornerPara.cornerTh)
- {
- SSG_RUN a_seg;
- if(lineData[pntIdx_1].pt3D.z < lineData[pntIdx_2].pt3D.z)
- a_seg.value = 1;
- else
- a_seg.value = -1;
- a_seg.start = pntIdx_1;
- a_seg.len = pntIdx_2 - pntIdx_1 +1;
- xyVerticalSegs.push_back(a_seg);
- }
- }
- break;
- }
- }
- }
+ if (ptDirAngles[i].type < 0)
+ continue;
- //匹配forwardAnglePeakP和backwardAnglePeakP, forwardAnglePeakP在前，backwardAnglePeakP在后
- for (int i = 0; i < (int)forwardAnglePeakP.size(); i++)
- {
- for (int j = 0; j < (int)backwardAnglePeakP.size(); j++)
- {
- if (backwardAnglePeakP[j].pntIdx > forwardAnglePeakP[i].pntIdx)
- {
- int pntIdx_1 = forwardAnglePeakP[i].pntIdx;
- int pntIdx_2 = backwardAnglePeakP[j].pntIdx;
- //计算两者的角度
- double diffY = abs(lineData[pntIdx_2].pt3D.y - lineData[pntIdx_1].pt3D.y);
- if (diffY < cornerMergeScale)
- {
- double tanValue = (lineData[pntIdx_2].pt3D.z - lineData[pntIdx_1].pt3D.z) / diffY;
- double angle = atan(tanValue) * 180.0 / PI;
- if (abs(angle) > cornerPara.cornerTh)
- {
- SSG_RUN a_seg;
- if (lineData[pntIdx_1].pt3D.z < lineData[pntIdx_2].pt3D.z)
- a_seg.value = 1;
- else
- a_seg.value = -1;
- a_seg.start = pntIdx_1;
- a_seg.len = pntIdx_2 - pntIdx_1 + 1;
- xyVerticalSegs.push_back(a_seg);
- }
- }
- break;
- }
- }
+ if (((ptDirAngles[i].backwardAngle < -minVerticalAngle) && (ptDirAngles[i].forwardAngle < -minVerticalAngle)) ||
+ ((ptDirAngles[i].backwardAngle > minVerticalAngle) && (ptDirAngles[i].forwardAngle > minVerticalAngle)))
+ xyVerticalFlags[i] = 1;
}
-
+ return;
}
/// 提取激光线上的拐点特征。是在PSM， LVTypeFeature, BQ等拐点算法的基础上的版本。
diff --git a/sourceCode/rodAndBarDetection.cpp b/sourceCode/rodAndBarDetection.cpp
index b0bd92e..f1fb12b 100644
--- a/sourceCode/rodAndBarDetection.cpp
+++ b/sourceCode/rodAndBarDetection.cpp
@@ -26,7 +26,8 @@
//version 1.3.3 : 筑裕钢筋焊缝提取算法改进中间版本
//version 1.3.4 : 新的定位盘中心测量功能算法优化
//version 1.3.5 : 新的定位盘中心测量功能占将float运算改成double ,测试PC和3588差异
-std::string m_strVersion = "1.3.5";
+//version 1.3.6 : 新的定位盘中心测量功能：优化聚类前的垂直点去除算法，保证聚类结果正确
+std::string m_strVersion = "1.3.6";
const char* wd_rodAndBarDetectionVersion(void)
{
return m_strVersion.c_str();
@@ -2031,51 +2032,101 @@ SSX_platePoseInfo sx_getLocationPlatePose_new(
flags[i].resize(linePtNum);
std::fill(flags[i].begin(), flags[i].end(), 0);
}
- std::vector> zVertivalSegs;
+ std::vector> zVertivalFlags;
for (int line = 0; line < lineNum; line++)
{
if (line == 700)
int kkk = 1;
- std::vector line_verticalSegs;
- wd_getXYVertialFeature_peakCornerMethod(
+ std::vector line_verticalFlags;
+ wd_getXYVertialFeature_dirAngleMethod(
scanLines[line],
line,
cornerPara.scale,
cornerPara,
- line_verticalSegs
+ line_verticalFlags
);
- zVertivalSegs.push_back(line_verticalSegs);
+ zVertivalFlags.push_back(line_verticalFlags);
- for (int i = 0; i < (int)line_verticalSegs.size(); i++)
+ for (int i = 0; i < (int)line_verticalFlags.size(); i++)
{
- int idx_0 = line_verticalSegs[i].start;
- for (int j = 0; j < line_verticalSegs[i].len; j++)
- flags[line][idx_0 + j] = 1;
+ if(line_verticalFlags[i] > 0)
+ flags[line][i] = 1;
}
}
- std::vector> zVertivalSegs_h;
+ std::vector> zVertivalFlags_h;
for (int line = 0; line < linePtNum; line++)
{
if (line == 1177)
int kkk = 1;
- std::vector line_verticalSegs;
- wd_getXYVertialFeature_peakCornerMethod(
+ std::vector line_verticalFlags;
+ wd_getXYVertialFeature_dirAngleMethod(
scanLines_h[line],
line,
cornerPara.scale,
cornerPara,
- line_verticalSegs
+ line_verticalFlags
);
- zVertivalSegs_h.push_back(line_verticalSegs);
+ zVertivalFlags_h.push_back(line_verticalFlags);
- for (int i = 0; i < (int)line_verticalSegs.size(); i++)
+ for (int i = 0; i < (int)line_verticalFlags.size(); i++)
{
- int idx_0 = line_verticalSegs[i].start;
- for (int j = 0; j < line_verticalSegs[i].len; j++)
- flags[idx_0 + j][line] = 1;
+ if(line_verticalFlags[i] > 0)
+ flags[i][line] = 1;
}
}
+ for (int line = 0; line < lineNum; line++)
+ {
+ for (int j = 0; j < linePtNum; j++)
+ {
+ if (flags[line][j] > 0)
+ {
+ scanLines[line][j].pt3D.z = 0;
+ scanLines_h[j][line].pt3D.z = 0;
+ }
+ }
+ }
+ //迭代一次
+ SSG_lineSegParam lineSegPara;
+ lineSegPara.distScale = 5.0;
+ lineSegPara.segGapTh_y = 5.0;
+ lineSegPara.segGapTh_z = 5.0;
+ const int minSegLen = 5;
+ for (int line = 0; line < lineNum; line++)
+ {
+ std::vector segs;
+ wd_getLineDataIntervals(
+ scanLines[line],
+ lineSegPara,
+ segs);
+ for (int i = 0; i < (int)segs.size(); i++)
+ {
+ if (segs[i].len <= minSegLen)
+ {
+ int idx0 = segs[i].start;
+ for (int j = 0; j < segs[i].len; j++)
+ flags[line][idx0 + j] = 1;
+ }
+ }
+ }
+ for (int line = 0; line < linePtNum; line++)
+ {
+ std::vector segs;
+ wd_getLineDataIntervals(
+ scanLines_h[line],
+ lineSegPara,
+ segs);
+ for (int i = 0; i < (int)segs.size(); i++)
+ {
+ if (segs[i].len <= minSegLen)
+ {
+ int idx0 = segs[i].start;
+ for (int j = 0; j < segs[i].len; j++)
+ flags[idx0 + j][line] = 1;
+ }
+ }
+ }
+
//标注
for (int line = 0; line < lineNum; line++)
{