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

rodAndBarDetection version 1.2.5 :

Browse Source 
添加筑裕钢筋焊缝提取API
This commit is contained in:
jerryzeng 2026-04-28 00:57:13 +08:00
parent 2757c09682
commit c14a77e5a2
8 changed files with 1377 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

101
rodAndBarDetection_test/rodAndBarDetection_test.cpp
View File

@ -780,16 +780,22 @@ void _outputRGBDScan_RGBD_weldSeam(
int centerFlag = pt3D->nPointIdx >> 4;
if (centerFlag > 0)
{
rgb = { 180, 0, 0 };
size = 2;
if (centerFlag <= 2)
{
rgb = { 180, 0, 0 };
size = 2;
}
else if(centerFlag == 4)
{
rgb = { 0, 250, 0 };
size = 4;
}
}
else
{
rgb = objColor[pt3D->nPointIdx % 8];
size = 2;
}
}
else //if (pt3D->nPointIdx == 0)
{
@ -807,52 +813,75 @@ void _outputRGBDScan_RGBD_weldSeam(
if (objNum > 0)
{
sw << "Line_" << lineIdx << "_0_" << objNum << std::endl;
size = 12;
std::vector<SVzNL3DPoint> weldPoints;
for (int i = 0; i < objNum; i++)
{
if (i == 0)
rgb = { 250, 255, 0 };
if (weldSeamInfo[i].weldType == KeWD_WELD_POINT)
weldPoints.push_back(weldSeamInfo[i].center);
else
rgb = { 250, 0, 0 };
float x = (float)weldSeamInfo[i].center.x;
float y = (float)weldSeamInfo[i].center.y;
float z = (float)weldSeamInfo[i].center.z;
{
weldPoints.push_back(weldSeamInfo[i].startPt);
weldPoints.push_back(weldSeamInfo[i].center);
weldPoints.push_back(weldSeamInfo[i].endPt);
}
}
sw << "Line_" << lineIdx << "_0_" << (int)weldPoints.size() << std::endl;
size = 15;
for (int i = 0; i < (int)weldPoints.size(); i++)
{
rgb = { 250, 0, 0 };
float x = (float)weldPoints[i].x;
float y = (float)weldPoints[i].y;
float z = (float)weldPoints[i].z;
sw << "{" << x << "," << y << "," << z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
}
//输出法向
size = 8;
double len = 60;
size = 4;
double len1 = 20;
double len2 = 20;
lineIdx = 0;
for (int i = 0; i < objNum; i++)
{
if (i == 0)
rgb = { 250, 255, 0 };
SVzNL3DPoint pt0, pt1;
if (weldSeamInfo[i].weldType == KeWD_WELD_POINT)
{
pt0 = weldSeamInfo[i].center;
pt1 = { weldSeamInfo[i].center.x + len1 * weldSeamInfo[i].axialDir.x,
weldSeamInfo[i].center.y + len1 * weldSeamInfo[i].axialDir.y,
weldSeamInfo[i].center.z + len1 * weldSeamInfo[i].axialDir.z };
}
else
rgb = { 250, 0, 0 };
SVzNL3DPoint pt0 = { weldSeamInfo[i].center.x, weldSeamInfo[i].center.y, weldSeamInfo[i].center.z };
SVzNL3DPoint pt2 = { weldSeamInfo[i].center.x + len * weldSeamInfo[i].normalDir.x,
weldSeamInfo[i].center.y + len * weldSeamInfo[i].normalDir.y,
weldSeamInfo[i].center.z + len * weldSeamInfo[i].normalDir.z };
{
pt0 = weldSeamInfo[i].startPt;
pt1 = weldSeamInfo[i].endPt;
}
SVzNL3DPoint pt2 = { weldSeamInfo[i].center.x, weldSeamInfo[i].center.y, weldSeamInfo[i].center.z };
SVzNL3DPoint pt3 = { weldSeamInfo[i].center.x + len2 * weldSeamInfo[i].normalDir.x,
weldSeamInfo[i].center.y + len2 * weldSeamInfo[i].normalDir.y,
weldSeamInfo[i].center.z + len2 * weldSeamInfo[i].normalDir.z };
//显示轴向量
sw << "Poly_" << lineIdx << "_2" << std::endl;
sw << "{" << (float)weldSeamInfo[i].startPt.x << "," << (float)weldSeamInfo[i].startPt.y << "," << (float)weldSeamInfo[i].startPt.z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
sw << "{" << (float)weldSeamInfo[i].endPt.x << "," << (float)weldSeamInfo[i].endPt.y << "," << (float)weldSeamInfo[i].endPt.z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
lineIdx++;
//显示法向量
rgb = { 0, 0, 250 };
sw << "Poly_" << lineIdx << "_2" << std::endl;
sw << "{" << (float)pt0.x << "," << (float)pt0.y << "," << (float)pt0.z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
sw << "{" << (float)pt1.x << "," << (float)pt1.y << "," << (float)pt1.z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
lineIdx++;
//显示法向量
rgb = { 250, 0, 0 };
sw << "Poly_" << lineIdx << "_2" << std::endl;
sw << "{" << (float)pt2.x << "," << (float)pt2.y << "," << (float)pt2.z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
sw << "{" << (float)pt3.x << "," << (float)pt3.y << "," << (float)pt3.z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
lineIdx++;
}
lineIdx++;
@ -1156,18 +1185,19 @@ void rodWeldSeamPosition_test(void)
cornerParam.jumpCornerTh_2 = 60;
SSG_outlierFilterParam filterParam;
filterParam.continuityTh = 5.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声
filterParam.outlierTh = 5;
filterParam.continuityTh = 4.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声
filterParam.outlierTh = 4;
SSG_treeGrowParam growParam;
growParam.maxLineSkipNum = 5;
growParam.yDeviation_max = 10.0;
growParam.yDeviation_max = 5.0;
growParam.maxSkipDistance = 20.0;
growParam.zDeviation_max = 10.0;//
growParam.zDeviation_max = 3.0;//
growParam.minLTypeTreeLen = 50; //mm, 螺杆长度
growParam.minVTypeTreeLen = 50; //mm
bool isHorizonScan = true; //true:激光线平行槽道；false:激光线垂直槽道
double weldSeanRange = 100; //焊缝距钢筋交叉点的范围（最大值）
int errCode = 0;
std::vector<SSX_weldSeamInfo> weldSeamInfo;
sx_rebarWeldSeamPositioning(
@ -1177,6 +1207,7 @@ void rodWeldSeamPosition_test(void)
filterParam,
growParam,
rodParam,
weldSeanRange,
weldSeamInfo,
&errCode);
long t2 = (long)GetTickCount64();
@ -1192,7 +1223,7 @@ void rodWeldSeamPosition_test(void)
int main()
{
#if 1 //螺杆定位测试
#if 0 //螺杆定位测试
#if 0
screwTest();
#else

3
sourceCode/SG_errCode.h
View File

@ -47,3 +47,6 @@
//配天工件定位
#define SX_ERR_UNKNOWN_PLATE_DIR -2601
//蓼淘보룹땍貫
#define SX_ERR_NO_HIGHEST_ROD -2701

2
sourceCode/SG_featureGrow.cpp
View File

@ -843,7 +843,7 @@ void wd_getRodArcFeatureGrowingTrees(
{
for (int i = 0, i_max = (int)all_lineFeatures.size(); i < i_max; i++)
{
if (i == 424)
if (i == 1147)
int kkk = 1;
std::vector<SWD_rodArcFeature>& a_lineFeatures = all_lineFeatures[i];
for (int j = 0, j_max = (int)a_lineFeatures.size(); j < j_max; j++)

116
sourceCode/SG_lineFeature.cpp
View File

@ -3824,10 +3824,64 @@ bool compareByPtIdx(const SSG_basicFeature1D& a, const SSG_basicFeature1D& b) {
return a.jumpPos2D.y < b.jumpPos2D.y;
}
//根据距离连续性分段
void wd_lineDataSegment_dist(
std::vector< SWD3DPointPostion>& lineData,
std::vector<SSG_RUN>& segs,
const double maxDistTh,
const int minSegSize
)
{
int dataSize = (int)lineData.size();
int runIdx = 1;
SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx
double pre_z = 0;
double pre_y = 0;
for (int i = 0; i < dataSize; i++)
{
if (lineData[i].point.z > 1e-4)
{
if (a_run.len < 0)
{
a_run.start = i;
a_run.len = 1;
a_run.value = i;
}
else
{
double dist = sqrt(pow(lineData[i].point.y - pre_y, 2) + pow(lineData[i].point.z - pre_z, 2));
if (dist < maxDistTh)
{
a_run.len = i - a_run.start + 1;
a_run.value = i;
}
else
{
a_run.value = runIdx;
runIdx++;
if(a_run.len >= minSegSize)
segs.push_back(a_run);
a_run.start = i;
a_run.len = 1;
a_run.value = i;
}
}
pre_y = lineData[i].point.y;
pre_z = lineData[i].point.z;
}
}
if (a_run.len > 0)
{
if(a_run.len >= minSegSize)
segs.push_back(a_run);
}
}
//根据Z连续性分段
void wd_lineDataSegment_zDist(
std::vector< SVzNL3DPosition>& lineData,
std::vector< SVzNL3DPosition>& vldPts,
std::vector< SVzNL3DPosition>& vldPts,
std::vector<SSG_RUN_EX>& segs,
std::vector<int>& backIndexing,
const SSG_cornerParam cornerPara
@ -4218,6 +4272,7 @@ bool _chkRodArcFeature(
{
//pkIdx可能会有波动
double realPkCorner = abs(ptDirAngles[pkIdx].forwardAngle + ptDirAngles[pkIdx].backwardAngle);
int validCorner = ptDirAngles[pkIdx].type;
int realPkIdx = pkIdx;
//向前搜索
for (int i = pkIdx - 1; i >= 0; i--)
@ -4225,12 +4280,21 @@ bool _chkRodArcFeature(
if (lineData[i].pt3D.z < 1e-4)
continue;
if (ptDirAngles[i].type < 0)
continue;
double dist = sqrt(pow(lineData[pkIdx].pt3D.y - lineData[i].pt3D.y, 2) + pow(lineData[pkIdx].pt3D.z - lineData[i].pt3D.z, 2));
if (dist > searchWin)
break;
double flatAngle = abs(ptDirAngles[i].forwardAngle + ptDirAngles[i].backwardAngle);
if (realPkCorner > flatAngle)
if( validCorner < 0)
{
realPkCorner = flatAngle;
realPkIdx = i;
validCorner = 0;
}
else if (realPkCorner > flatAngle)
{
realPkCorner = flatAngle;
realPkIdx = i;
@ -4242,12 +4306,21 @@ bool _chkRodArcFeature(
if (lineData[i].pt3D.z < 1e-4)
continue;
if (ptDirAngles[i].type < 0)
continue;
double dist = sqrt(pow(lineData[pkIdx].pt3D.y - lineData[i].pt3D.y, 2) + pow(lineData[pkIdx].pt3D.z - lineData[i].pt3D.z, 2));
if (dist > searchWin)
break;
double flatAngle = abs(ptDirAngles[i].forwardAngle + ptDirAngles[i].backwardAngle);
if (realPkCorner > flatAngle)
if (validCorner < 0)
{
realPkCorner = flatAngle;
realPkIdx = i;
validCorner = 0;
}
else if (realPkCorner > flatAngle)
{
realPkCorner = flatAngle;
realPkIdx = i;
@ -4316,7 +4389,7 @@ void wd_getRodArcFeature_peakCornerMethod(
)
{
double arcPerPointCornerMinValue = 5; //arc上每个点的转角最小值
double arcTotalCornerMinValue = 45; //Õû¸öArcµÄת½Ç×îСֵ
double arcTotalCornerMinValue = 30; //整个Arc的转角最小值
//计算前向角和后向角
std::vector< SSG_pntDirAngle> ptDirAngles;
wd_computeDirAngle_wholeLine( lineData, cornerPara, ptDirAngles);
@ -4356,10 +4429,10 @@ void wd_getRodArcFeature_peakCornerMethod(
return;
}
//提取corner极值（较早实现函数可以使用此函数进行代码优化）
void _searchCornerPeaks(
void wd_searchCornerPeaks(
std::vector< SSG_pntDirAngle>& corners,
std::vector< SVzNL3DPosition>& vldPts,
const SSG_cornerParam cornerPara,
const double minCornerTh,
double cornerMergeScale,
std::vector< SSG_pntDirAngle>& cornerPeakP,
std::vector< SSG_pntDirAngle>& cornerPeakM
@ -4379,20 +4452,19 @@ void _searchCornerPeaks(
if (i == 275)
int kkk = 1;
SSG_pntDirAngle* curr_data = &corners[i];
if (curr_data->pntIdx < 0)
continue;
if (curr_data->pntIdx < 0)
{
if (i == i_max - 1) //最后一个
{
if (1 == _state) //上升
{
cornerPk_P.push_back(corners[eEdgePtIdx]);
if(corners[eEdgePtIdx].corner > 0)
cornerPk_P.push_back(corners[eEdgePtIdx]);
}
else if (2 == _state) //下降
{
cornerPk_M.push_back(corners[eEdgePtIdx]);
if(corners[eEdgePtIdx].corner < 0)
cornerPk_M.push_back(corners[eEdgePtIdx]);
}
}
continue;
@ -4451,7 +4523,7 @@ void _searchCornerPeaks(
double square_distTh = cornerMergeScale * cornerMergeScale; //2倍的cornerScale。
for (int i = 0, i_max = (int)cornerPk_P.size(); i < i_max; i++)
{
if (cornerPk_P[i].corner < cornerPara.cornerTh)
if (cornerPk_P[i].corner < minCornerTh)
continue;
bool isPeak = true;
@ -4494,7 +4566,7 @@ void _searchCornerPeaks(
for (int i = 0, i_max = (int)cornerPk_M.size(); i < i_max; i++)
{
if (abs(cornerPk_M[i].corner) < cornerPara.cornerTh)
if (abs(cornerPk_M[i].corner) < minCornerTh)
continue;
bool isPeak = true;
@ -4534,8 +4606,6 @@ void _searchCornerPeaks(
if (true == isPeak)
cornerPeakM.push_back(cornerPk_M[i]);
}
}
/// <summary>
@ -4703,10 +4773,10 @@ void wd_getLineCorerFeature(
std::vector< SSG_pntDirAngle> cornerPeakM;
double cornerMergeScale = cornerPara.scale * 2;
//提取corner极值
_searchCornerPeaks(
wd_searchCornerPeaks(
corners,
vldPts,
cornerPara,
cornerPara.cornerTh,
cornerMergeScale,
cornerPeakP,
cornerPeakM
@ -4899,10 +4969,10 @@ void wd_getLineCorerFeature_accelerate(
std::vector< SSG_pntDirAngle> cornerPeakM;
double cornerMergeScale = cornerPara.scale * 2;
//提取corner极值
_searchCornerPeaks(
wd_searchCornerPeaks(
corners,
lineData,
cornerPara,
cornerPara.cornerTh,
cornerMergeScale,
cornerPeakP,
cornerPeakM
@ -5645,10 +5715,10 @@ void sg_getLineZJumpFeature_cornerMethod(
std::vector< SSG_pntDirAngle> cornerPeakP;
std::vector< SSG_pntDirAngle> cornerPeakM;
double cornerMergeScale = cornerPara.scale * 2;
_searchCornerPeaks(
wd_searchCornerPeaks(
corners,
vldPts,
cornerPara,
cornerPara.cornerTh,
cornerMergeScale,
cornerPeakP,
cornerPeakM
@ -5884,10 +5954,10 @@ void sg_getLineLJumpFeature_cornerMethod(
std::vector< SSG_pntDirAngle> cornerPeakP;
std::vector< SSG_pntDirAngle> cornerPeakM;
double cornerMergeScale = cornerPara.scale;
_searchCornerPeaks(
wd_searchCornerPeaks(
corners,
vldPts,
cornerPara,
cornerPara.cornerTh,
cornerMergeScale,
cornerPeakP,
cornerPeakM

50
sourceCode/dataFitting.cpp
View File

@ -71,6 +71,56 @@ void lineFitting_abc(std::vector< SVzNL3DPoint>& inliers, double* _a, double* _b
return;
}
//拟合成通用直线方程ax+by+c=0，包括垂直
void indexingPtLineFitting_abc(std::vector< SWD3DPointPostion>& inliers, double* _a, double* _b, double* _c)
{
//判断是否为垂直
int dataSize = (int)inliers.size();
if (dataSize < 2)
return;
double deltaX = abs(inliers[0].point.x - inliers[dataSize - 1].point.x);
double deltaY = abs(inliers[0].point.y - inliers[dataSize - 1].point.y);
std::vector< SVzNL3DPoint> fittingData;
if (deltaX < deltaY)
{
//x=ky+b 拟合
for (int i = 0; i < dataSize; i++)
{
SVzNL3DPoint a_fitPt;
a_fitPt.x = inliers[i].point.y;
a_fitPt.y = inliers[i].point.x;
a_fitPt.z = inliers[i].point.z;
fittingData.push_back(a_fitPt);
}
double k = 0, b = 0;
lineFitting(fittingData, &k, &b);
//ax+by+c
*_a = 1.0;
*_b = -k;
*_c = -b;
}
else
{
for (int i = 0; i < dataSize; i++)
{
SVzNL3DPoint a_fitPt;
a_fitPt.x = inliers[i].point.x;
a_fitPt.y = inliers[i].point.y;
a_fitPt.z = inliers[i].point.z;
fittingData.push_back(a_fitPt);
}
//y = kx+b拟合
double k = 0, b = 0;
lineFitting(fittingData, &k, &b);
//ax+by+c
*_a = k;
*_b = -1;
*_c = b;
}
return;
}
//Ô²×îС¶þ³ËÄâºÏ
double fitCircleByLeastSquare(
const std::vector<SVzNL3DPoint>& pointArray,

1160
sourceCode/rodAndBarDetection.cpp
View File

File diff suppressed because it is too large Load Diff

13
sourceCode/rodAndBarDetection_Export.h
View File

@ -36,12 +36,24 @@ typedef struct
SVzNL3DPoint endPt;
}SSX_rodPositionInfo;
typedef enum
{
KeWD_WELD_UNKNOWN = 0,
KeWD_WELD_POINT, //焊点
KeWD_WELD_SEAM, //焊缝
KeWD_WELD_SEAM_GAP, //焊缝，有缝隙
}EWD_weldType;
typedef struct
{
EWD_weldType weldType;
SVzNL3DPoint startPt;
SVzNL3DPoint endPt;
SVzNL3DPoint center;
SVzNL3DPoint axialDir;
SVzNL3DPoint normalDir; //法向量
std::vector< SVzNL3DPoint> centerPts;
std::vector< SVzNL3DPoint> edgePts_0;
std::vector< SVzNL3DPoint> edgePts_1;
}SSX_weldSeamInfo;
//读版本号
@ -97,5 +109,6 @@ SG_APISHARED_EXPORT void sx_rebarWeldSeamPositioning(
const SSG_outlierFilterParam filterParam,
const SSG_treeGrowParam growParam,
const SSX_rodParam rodParam,
const double weldSeanRange, //焊缝距钢筋交叉点的范围（最大值）
std::vector<SSX_weldSeamInfo>& weldSeamInfo,
int* errCode);

4
workpieceHolePositioning_test/workpieceHolePositioning_test.cpp
View File

@ -566,7 +566,7 @@ void TuoPuFa_holePosition_test(void)
};
SVzNLRange fileIdx[TPF_TEST_GROUP] = {
{6,6}, {1, 16}, {18,18}
{6,6}, {1, 16}, {1,17}
};
const char* ver = wd_workpieceHolePositioningVersion();
@ -734,7 +734,7 @@ void TuoPuFa_holePosition_test(void)
#endif
#if TEST_COMPUTE_HOLE
for (int grp = 2; grp <= 2; grp++)
for (int grp = 1; grp <= 2; grp++)
{
SSG_planeCalibPara groundCalibPara;
//初始化成单位阵