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

rodAndBarDetection version 1.4.0 :

Browse Source 
矩森棒材抓取算法改进：（1）修正了两个棒材被识别成一根的问题（2）增强了对噪点的容忍度，增强了鲁棒性
This commit is contained in:
jerryzeng 2026-06-24 21:13:49 +08:00
parent 29c25997fd
commit 693471b311
6 changed files with 601 additions and 109 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

77
rodAndBarDetection_test/rodAndBarDetection_test.cpp
View File

@ -973,33 +973,36 @@ void _outputRGBDScan_RGBD_weldSeam(
sw.close();
}
#define SCREW_TEST_GROUP 10
#define SCREW_TEST_GROUP 12
void screwTest(void)
{
const char* dataPath[SCREW_TEST_GROUP] = {
"F:/ShangGu/项目/冠钦项目/螺杆测量/数据/模拟数据/", //0
"F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/螺杆点云2/上方两根/", //1
"F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/螺杆点云3/", //2
"F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/螺杆点云4/位置1/", //3
"F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/螺杆点云4/位置2/", //4
"F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/螺杆点云4/位置2未动螺杆拧进去100mm左右/", //5
"F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/螺杆点云4/位置2向前100mm螺杆拧进去100mm左右/", //6
"F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/螺杆点云20260621/另一根螺杆/", //7
"F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/螺杆点云20260621/拍照点1/", //8
"F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/螺杆点云20260621/拍照点1xyz移动10mm/", //9
"F:/ShangGu/项目/冠钦项目/配天螺杆测量/数据/模拟数据/", //0
"F:/ShangGu/项目/冠钦项目/配天螺杆测量/配天现场点云/螺杆点云2/上方两根/", //1
"F:/ShangGu/项目/冠钦项目/配天螺杆测量/配天现场点云/螺杆点云3/", //2
"F:/ShangGu/项目/冠钦项目/配天螺杆测量/配天现场点云/螺杆点云4/位置1/", //3
"F:/ShangGu/项目/冠钦项目/配天螺杆测量/配天现场点云/螺杆点云4/位置2/", //4
"F:/ShangGu/项目/冠钦项目/配天螺杆测量/配天现场点云/螺杆点云4/位置2未动螺杆拧进去100mm左右/", //5
"F:/ShangGu/项目/冠钦项目/配天螺杆测量/配天现场点云/螺杆点云4/位置2向前100mm螺杆拧进去100mm左右/", //6
"F:/ShangGu/项目/冠钦项目/配天螺杆测量/配天现场点云/螺杆点云20260621/另一根螺杆/", //7
"F:/ShangGu/项目/冠钦项目/配天螺杆测量/配天现场点云/螺杆点云20260621/拍照点1/", //8
"F:/ShangGu/项目/冠钦项目/配天螺杆测量/配天现场点云/螺杆点云20260621/拍照点1xyz移动10mm/", //9
"F:/ShangGu/项目/冠钦项目/配天螺杆测量/配天现场点云/异常点云0615/", //10
"F:/ShangGu/项目/冠钦项目/配天螺杆测量/配天现场点云/异常点云0618/", //11
};
SVzNLRange fileIdx[SCREW_TEST_GROUP] = {
{1,4},{1,30},{1,11},
{1,20}, {1,20}, {1,5}, {1,21},
{1,10}, {1,10}, {1,10}
{1,10}, {1,10}, {1,10},
{1,9},{1,30}
};
const char* ver = wd_rodAndBarDetectionVersion();
printf("ver:%s\n", ver);
for (int grp = 3; grp <= 6; grp++)
for (int grp = 11; grp <= 11; grp++)
{
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
{
@ -1010,9 +1013,12 @@ void screwTest(void)
sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx);
else if( (grp>= 7)&&(grp <= 9))
sprintf_s(_scan_file, "%sLaserline_%d.txt", dataPath[grp], fidx);
else if(10 == grp)
sprintf_s(_scan_file, "%s%d_LaserData.txt", dataPath[grp], fidx);
else
sprintf_s(_scan_file, "%s%d_LaserData_Jl26C299.txt", dataPath[grp], fidx);
std::vector<std::vector< SVzNL3DPosition>> scanLines;
wdReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
@ -1026,13 +1032,13 @@ void screwTest(void)
if (grp == 0)
rodDiameter = 10.0;
else
rodDiameter = 28.0; //现场螺杆直径28mm
rodDiameter = 32.0; //现场螺杆直径28mm
//double rodDiameter = 10.0;
SSG_cornerParam cornerParam;
cornerParam.cornerTh = 30; //45度角
cornerParam.scale = rodDiameter/4; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
cornerParam.scale = rodDiameter / 4; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
cornerParam.minEndingGap = 20; // algoParam.bagParam.bagW / 4;
cornerParam.minEndingGap_z = 5.0;
cornerParam.jumpCornerTh_1 = 15; //水平角度，小于此角度视为水平
@ -1089,7 +1095,7 @@ void screwTest(void)
void locatingPlateTest(void)
{
const char* dataPath[LOCATING_PALTE_TEST_GROUP] = {
"F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/定位盘点云/", //0
"F:/ShangGu/项目/冠钦项目/配天螺杆测量/配天现场点云/定位盘点云/", //0
};
SVzNLRange fileIdx[LOCATING_PALTE_TEST_GROUP] = {
@ -1147,8 +1153,8 @@ void locatingPlateTest(void)
void newLocatingPlateTest(void)
{
const char* dataPath[NEW_LOCATING_PALTE_TEST_GROUP] = {
"F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/定位盘点云20260521/", //0
"F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/定位盘点云20260603/", //1
"F:/ShangGu/项目/冠钦项目/配天螺杆测量/配天现场点云/定位盘点云20260521/", //0
"F:/ShangGu/项目/冠钦项目/配天螺杆测量/配天现场点云/定位盘点云20260603/", //1
};
SVzNLRange fileIdx[NEW_LOCATING_PALTE_TEST_GROUP] = {
@ -1309,7 +1315,7 @@ void _convertToGridData_XYZRGB(std::vector<std::vector< SVzNLPointXYZRGBA>>& sca
return;
}
#define ROD_POSITION_TEST_GROUP 5
#define ROD_POSITION_TEST_GROUP 6
#define TEST_CONVERT_TO_GRID 0
#define TEST_COMPUTE_CALIB_PARA 0
#define TEST_COMPUTE_ROD_POSITION 1
@ -1348,7 +1354,7 @@ void rodPositionTest(void)
printf("all converted done!\n", _scan_src_file);
#endif
#if TEST_COMPUTE_CALIB_PARA
const char* calibDataPath = "F:/ShangGu/项目/冠钦项目/矩森棒材抓取/空料框及内剩一根棒材/调平/位置1/";
const char* calibDataPath = "F:/ShangGu/项目/冠钦项目/矩森棒材抓取/异常点云/";
char _calib_datafile[256];
sprintf_s(_calib_datafile, "%sGroundData_1.txt", calibDataPath);
int lineNum = 0;
@ -1413,17 +1419,19 @@ void rodPositionTest(void)
"F:/ShangGu/项目/冠钦项目/胶布圆棒抓取/模拟测试数据/", //3
"F:/ShangGu/项目/冠钦项目/胶布圆棒抓取/模拟测试数据2/", //4
"F:/ShangGu/项目/冠钦项目/矩森棒材抓取/异常点云/", //5
};
SVzNLRange fileIdx[ROD_POSITION_TEST_GROUP] = {
{1,8}, {1,3}, {1,31},
{1,5},{1,11}
{1,5},{1,11},{1,26}
};
const char* ver = wd_rodAndBarDetectionVersion();
printf("ver:%s\n", ver);
for (int grp = 0; grp <= 2; grp++)
for (int grp = 5; grp <= 5; grp++)
{
SSG_planeCalibPara poseCalibPara;
//初始化成单位阵
@ -1451,10 +1459,12 @@ void rodPositionTest(void)
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
{
//fidx =5;
//fidx =16;
char _scan_file[256];
if( (3 == grp) ||(4 == grp))
sprintf_s(_scan_file, "%s%d_LaserData_Hi229156.txt", dataPath[grp], fidx);
else if(5 == grp)
sprintf_s(_scan_file, "%s%d_LaserData_Ik256066.txt", dataPath[grp], fidx);
else
sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx);
@ -1472,6 +1482,11 @@ void rodPositionTest(void)
{
rodParam = rodParas[fidx];
}
else if (grp == 5)
{
rodParam.diameter = 96.0; //圆棒直径
rodParam.len = 440;
}
else
{
rodParam.diameter = 68.0; //圆棒直径
@ -1706,6 +1721,13 @@ void rodWeldSeamPosition_test(void)
weldSeamRange.min = 0;
weldSeamRange.max = 80;
}
EWD_weldingCategory weldCategory;
if ((grp == 0) || (grp == 1))
weldCategory = KeWD_WELD_CATEGPRY_I;
else
weldCategory = KeWD_WELD_CATEGPRY_II;
int errCode = 0;
std::vector<SSX_weldSeamInfo> weldSeamInfo;
sx_rebarWeldSeamPositioning(
@ -1716,6 +1738,7 @@ void rodWeldSeamPosition_test(void)
growParam,
rodParam,
weldSeamRange,
weldCategory,
weldSeamInfo,
&errCode);
long t2 = (long)GetTickCount64();
@ -1738,16 +1761,16 @@ typedef enum
keSG_测试_配天螺杆定位 = 0,
keSG_测试_配天定位盘定位,
keSG_测试_配天新定位盘定位,
keSG_测试_棒材抓取,
keSG_测试_矩森棒材抓取,
keSG_测试_筑裕钢筋焊缝定位,
} ESG_testMode;
int main()
{
ESG_testMode testMode = keSG_测试_配天螺杆定位;
//ESG_testMode testMode = keSG_测试_配天螺杆定位;
//ESG_testMode testMode = keSG_测试_配天定位盘定位;
//ESG_testMode testMode = keSG_测试_配天新定位盘定位;
//ESG_testMode testMode = keSG_测试_棒材抓取;
ESG_testMode testMode = keSG_测试_矩森棒材抓取;
//ESG_testMode testMode = keSG_测试_筑裕钢筋焊缝定位;
if(keSG_测试_配天螺杆定位 == testMode)
@ -1756,7 +1779,7 @@ int main()
locatingPlateTest();
else if (keSG_测试_配天新定位盘定位 == testMode)
newLocatingPlateTest();
else if(keSG_测试_棒材抓取 == testMode)
else if(keSG_测试_矩森棒材抓取 == testMode)
rodPositionTest();
else if(keSG_测试_筑裕钢筋焊缝定位 == testMode)
rodWeldSeamPosition_test();

26
sourceCode/SG_baseAlgo_Export.h
View File

@ -250,7 +250,7 @@ SG_APISHARED_EXPORT void wd_getRodArcFeature_segmentPeakCornerMethod(
std::vector< SVzNL3DPosition>& lineData,
int lineIdx,
const double maxDistTh,
const int minSegSize,
const double minSegSize,
const double peakChkWin,
const SSG_cornerParam cornerPara,
std::vector<SWD_rodArcFeature>& line_rodArcs //环
@ -311,6 +311,18 @@ SG_APISHARED_EXPORT void wd_getXYVertialFeature_dirAngleMethod(
const SSG_cornerParam cornerPara,
std::vector<int>& xyVerticalFlags //环
);
/// <summary>
/// 按段提取激光线上的与XY平面垂直的特征（垂直段）
/// </summary>
SG_APISHARED_EXPORT void wd_getXYVertialFeature_perSeg_dirAngleMethod(
std::vector< SVzNL3DPosition>& lineData,
int lineIdx,
const double maxDistTh,
const double minSegSize,
const SSG_cornerParam cornerPara,
std::vector<int>& xyVerticalFlags
);
/// <summary>
/// 提取激光线上的与XY平面水平的特征（水平段）
/// </summary>
@ -433,6 +445,18 @@ SG_APISHARED_EXPORT void sg_getLineLocalPeaks_2(
std::vector< SSG_basicFeature1D>& localZMax,
std::vector< SSG_basicFeature1D>& localZMin);
/// <summary>
/// 按段提取激光线上的极值点（极大值点和极小值点）
///
/// </summary>
SG_APISHARED_EXPORT void sg_getLineLocalPeaks_perSegment(
std::vector<SVzNL3DPosition>& lineData,
std::vector<SSG_RUN>& segs,
int lineIdx,
const double scaleWin,
std::vector< SSG_basicFeature1D>& localZMax,
std::vector< SSG_basicFeature1D>& localZMin);
SG_APISHARED_EXPORT void sg_getFlatLineLocalPeaks_vector(
std::vector<SVzNL3DPosition>& lineData,
int lineIdx,

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 == 770)
if (i == 630)
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++)

290
sourceCode/SG_lineFeature.cpp
View File

@ -839,6 +839,210 @@ void sg_getLineLocalPeaks_2(
return;
}
/// <summary>
/// 按段提取激光线上的极值点（极大值点和极小值点）
///
/// </summary>
void sg_getLineLocalPeaks_perSegment(
std::vector<SVzNL3DPosition>& lineData,
std::vector<SSG_RUN>& segs,
int lineIdx,
const double scaleWin,
std::vector< SSG_basicFeature1D>& localZMax,
std::vector< SSG_basicFeature1D>& localZMin)
{
int dataSize = (int)lineData.size();
if (dataSize < 2)
return;
//生成Mask
std::vector<int> segMask;
segMask.resize(lineData.size());
std::fill(segMask.begin(), segMask.end(), -1);
for (int i = 0; i < (int)segs.size(); i++)
{
int sIdx = segs[i].start;
int eIdx = segs[i].start + segs[i].len - 1;
for (int j = sIdx; j <= eIdx; j++)
segMask[j] = i;
}
int _state = 0;
int pre_i = -1;
int sEdgePtIdx = -1;
int eEdgePtIdx = -1;
SVzNL3DPosition* pre_data = NULL;
std::vector< SVzNL3DPosition> zTop;
std::vector< SVzNL3DPosition> zBtm;
for (int i = 0; i < dataSize; i++)
{
if (i == 141)
int kkk = 1;
lineData[i].nPointIdx = i;
SVzNL3DPosition* curr_data = &lineData[i];
if (curr_data->pt3D.z < 1e-4)
{
if (i == dataSize - 1) //最后一个
{
if (1 == _state) //上升
{
zTop.push_back(lineData[eEdgePtIdx]);
}
else if (2 == _state) //下降
{
zBtm.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) //下降
{
zTop.push_back(*pre_data);
_state = 2;
}
break;
case 2: //下降
if (z_diff > 0) // 上升
{
zBtm.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 = (int)zTop.size(); i < i_max; i++)
{
bool isPeak = true;
int curr_ptIdx = zTop[i].nPointIdx;
int curr_segIdx = segMask[curr_ptIdx];
//向前搜索
for (int j = i - 1; j >= 0; j--)
{
int chk_ptIdx = zTop[j].nPointIdx;
int chk_segIdx = segMask[chk_ptIdx];
double dist = pow(zTop[i].pt3D.y - zTop[j].pt3D.y, 2) + pow(zTop[i].pt3D.z - zTop[j].pt3D.z, 2);
if ((dist > square_distTh)||(chk_segIdx != curr_segIdx)) //超出尺度窗口
break;
if (zTop[i].pt3D.z < zTop[j].pt3D.z)
{
isPeak = false;
break;
}
}
//向后搜索
if (true == isPeak)
{
for (int j = i + 1; j < i_max; j++)
{
int chk_ptIdx = zTop[j].nPointIdx;
int chk_segIdx = segMask[chk_ptIdx];
double dist = pow(zTop[i].pt3D.y - zTop[j].pt3D.y, 2) + pow(zTop[i].pt3D.z - zTop[j].pt3D.z, 2);
if ((dist > square_distTh) || (chk_segIdx != curr_segIdx)) //超出尺度窗口
break;
if (zTop[i].pt3D.z < zTop[j].pt3D.z)
{
isPeak = false;
break;
}
}
}
if (true == isPeak)
{
SSG_basicFeature1D a_feature;
a_feature.featureType = LINE_FEATURE_PEAK_TOP;
a_feature.jumpPos = zTop[i].pt3D;
a_feature.jumpPos2D = { lineIdx, zTop[i].nPointIdx };
localZMax.push_back(a_feature);
}
}
//极大值点（谷底）
for (int i = 0, i_max = (int)zBtm.size(); i < i_max; i++)
{
bool isPeak = true;
int curr_ptIdx = zBtm[i].nPointIdx;
int curr_segIdx = segMask[curr_ptIdx];
//向前搜索
for (int j = i - 1; j >= 0; j--)
{
int chk_ptIdx = zBtm[j].nPointIdx;
int chk_segIdx = segMask[chk_ptIdx];
double dist = pow(zBtm[i].pt3D.y - zBtm[j].pt3D.y, 2) + pow(zBtm[i].pt3D.z - zBtm[j].pt3D.z, 2);
if ((dist > square_distTh) || (chk_segIdx != curr_segIdx)) //超出尺度窗口
break;
if (zBtm[i].pt3D.z > zBtm[j].pt3D.z)
{
isPeak = false;
break;
}
}
//向后搜索
if (true == isPeak)
{
for (int j = i + 1; j < i_max; j++)
{
int chk_ptIdx = zBtm[j].nPointIdx;
int chk_segIdx = segMask[chk_ptIdx];
double dist = pow(zBtm[i].pt3D.y - zBtm[j].pt3D.y, 2); pow(zBtm[i].pt3D.z - zBtm[j].pt3D.z, 2);
if ((dist > square_distTh) || (chk_segIdx != curr_segIdx)) //超出尺度窗口
break;
if (zBtm[i].pt3D.z > zBtm[j].pt3D.z)
{
isPeak = false;
break;
}
}
}
if (true == isPeak)
{
SSG_basicFeature1D a_feature;
a_feature.featureType = LINE_FEATURE_PEAK_BOTTOM;
a_feature.jumpPos = zBtm[i].pt3D;
a_feature.jumpPos2D = { lineIdx, zBtm[i].nPointIdx };
localZMin.push_back(a_feature);
}
}
return;
}
/// <summary>
/// 获取扫描线中的斜坡，并记录斜坡中的最大的相邻点之间的跳变。
/// 同时记录Ending。当没有中间袋子，只有左右袋子时，会有两组Ending。Ending之间的最小间距设置为袋子宽度的1/4
@ -4936,7 +5140,8 @@ void wd_getRodArcFeature_segmentPeakCornerMethod(
//搜索z极值。
std::vector< SSG_basicFeature1D> localZMax;
std::vector< SSG_basicFeature1D> localZMin;
sg_getLineLocalPeaks_2(lineData, lineIdx, peakChkWin, localZMax, localZMin);
//按段提取z极值
sg_getLineLocalPeaks_perSegment(lineData, segs, lineIdx, peakChkWin, localZMax, localZMin);
//逐个极小值点判断
int peakNum = (int)localZMin.size();
@ -5736,6 +5941,89 @@ void wd_getXYVertialFeature_dirAngleMethod(
return;
}
/// <summary>
/// 按段提取激光线上的与XY平面垂直的特征（垂直段）
/// </summary>
void wd_getXYVertialFeature_perSeg_dirAngleMethod(
std::vector< SVzNL3DPosition>& lineData,
int lineIdx,
const double maxDistTh,
const double minSegSize,
const SSG_cornerParam cornerPara,
std::vector<int>& xyVerticalFlags
)
{
if (lineIdx == 562)
int kkk = 1;
double minVerticalAngle = cornerPara.cornerTh; //arc上每个点的转角最大值
xyVerticalFlags.resize(lineData.size());
std::fill(xyVerticalFlags.begin(), xyVerticalFlags.end(), 0);
//根据z连续性分段
std::vector<SSG_RUN> segs;
wd_lineDataSegment_dist_2(
lineData,
segs,
maxDistTh,
minSegSize
);
//计算前向角和后向角
std::vector< SSG_pntDirAngle> ptDirAngles;
_computeDirAngle_perSeg_2(lineData, segs, cornerPara, ptDirAngles);
for (int i = 0; i < (int)ptDirAngles.size(); i++)
{
if ((ptDirAngles[i].type < 0) || (ptDirAngles[i].pntIdx < 0))
continue;
if (((ptDirAngles[i].backwardAngle < -minVerticalAngle) && (ptDirAngles[i].forwardAngle < -minVerticalAngle)) ||
((ptDirAngles[i].backwardAngle > minVerticalAngle) && (ptDirAngles[i].forwardAngle > minVerticalAngle)))
xyVerticalFlags[i] = 1;
}
//检查边缘点
for (int i = 1; i < (int)ptDirAngles.size() - 1; i++)
{
if ((xyVerticalFlags[i - 1] == 0) && (xyVerticalFlags[i] == 1))
{
int sIdx = ptDirAngles[i].backwardPntIdx;
bool isEdge = true;
for (int j = sIdx; j < i; j++)
{
if (ptDirAngles[j].pntIdx >= 0)
{
isEdge = false;
break;
}
}
if (true == isEdge)
{
for (int j = sIdx; j < i; j++)
xyVerticalFlags[j] = 1;
}
}
if ((xyVerticalFlags[i + 1] == 0) && (xyVerticalFlags[i] == 1))
{
int eIdx = ptDirAngles[i].forwardPntIdx;
bool isEdge = true;
for (int j = i + 1; j <= eIdx; j++)
{
if (ptDirAngles[j].pntIdx >= 0)
{
isEdge = false;
break;
}
}
if (true == isEdge)
{
for (int j = i + 1; j <= eIdx; j++)
xyVerticalFlags[j] = 1;
}
}
}
return;
}
/// <summary>
/// 提取激光线上的与XY平面水平的特征（水平段）
/// seg端点：z距离大于门限

307
sourceCode/rodAndBarDetection.cpp
View File

@ -29,7 +29,8 @@
//version 1.3.6 : 新的定位盘中心测量功能：优化聚类前的垂直点去除算法，保证聚类结果正确
//version 1.3.7 : 新的定位盘中心测量功能：进一步优化了内部参数，优化了垂直点去除效果
//version 1.3.8 : 新的螺杆定位算法，使用PCA方法确定螺杆轴向
std::string m_strVersion = "1.3.8";
//version 1.4.0 : 矩森棒材抓取算法改进：（1）修正了两个棒材被识别成一根的问题（2）增强了对噪点的容忍度，增强了鲁棒性
std::string m_strVersion = "RodAndBarDetection_1.4.0";
const char* wd_rodAndBarDetectionVersion(void)
{
return m_strVersion.c_str();
@ -241,12 +242,67 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9])
}
void rodAarcFeatueDetection(
void rodArcFeatueDetection(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SSG_cornerParam cornerPara,
const SSG_outlierFilterParam filterParam,
const double segment_maxDistTh,
const double segment_minSegSze,
const double rodDiameter,
std::vector<std::vector<SWD_rodArcFeature>>& arcFeatures)
{
int lineNum = (int)scanLines.size();
int linePtNum = (int)scanLines[0].size();
for (int line = 0; line < lineNum; line++)
{
if (line == 780)
int kkk = 1;
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
// Filter outliers from line data
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
// Extract rod arc features
std::vector<SWD_rodArcFeature> line_rodArcs;
#if 0
double arcTotalCornerMinValue = 22.5; //整个Arc的转角最小值, 360/16
wd_getRodArcFeature_YZCurvatureMethod(
lineData,
line,
rodDiameter / 4,
rodDiameter / 4,
rodDiameter / 4,
rodDiameter,
arcTotalCornerMinValue,
line_rodArcs //环
);
#else
wd_getRodArcFeature_segmentPeakCornerMethod(
lineData,
line,
segment_maxDistTh,
segment_minSegSze,
rodDiameter / 2,
cornerPara,
line_rodArcs //环
);
#endif
//wd_getRodArcFeature_peakCornerMethod(lineData, line, rodDiameter / 2, cornerPara, line_rodArcs);
arcFeatures.push_back(line_rodArcs);
}
return;
}
#if 0
//提取扫描线上Arc和V形特征，其中接近90度内凹和外凸的特征。用于钢筋焊接
void lineArcAndVFeatueDetection(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SSG_cornerParam cornerPara,
const SSG_outlierFilterParam filterParam,
const double rodDiameter,
std::vector<std::vector<SWD_rodArcFeature>>& arcFeatures)
std::vector<std::vector<SWD_rodArcFeature>>& arcFeatures,
std::vector<std::vector< SWD_rodArcFeature>>& convexVFeature,
std::vector<std::vector< SWD_rodArcFeature>>& concaveVFeature
)
{
int lineNum = (int)scanLines.size();
int linePtNum = (int)scanLines[0].size();
@ -276,7 +332,7 @@ void rodAarcFeatueDetection(
wd_getRodArcFeature_segmentPeakCornerMethod(
lineData,
line,
rodDiameter/4,
rodDiameter / 4,
rodDiameter / 8,
rodDiameter / 2,
cornerPara,
@ -288,6 +344,7 @@ void rodAarcFeatueDetection(
}
return;
}
#endif
//所有点计算点云ROI: vecotr格式, （不过滤z小于0的点）
SVzNL3DRangeD _getPointCloudROI(std::vector<SWD3DPointPostion>& scanData)
@ -467,10 +524,14 @@ SVzNL3DRangeD _getPointCloudROI(std::vector<SWD3DPointPostion>& scanData)
}
std::vector<std::vector<SWD_rodArcFeature>> arcFeatures;
rodAarcFeatueDetection(
const double segment_maxDistTh = rodDiameter / 4;
const double segment_minSegSize = rodDiameter / 8;
rodArcFeatueDetection(
data_lines,
cornerPara,
filterParam,
segment_maxDistTh,
segment_minSegSize,
rodDiameter,
arcFeatures);
@ -3193,11 +3254,12 @@ SVzNL3DPoint _exchangeXY(SVzNL3DPoint pt)
void _computeRodInfo(
SWD_rodArcFeatureTree& a_objTree,
int startIdx,
int endIdx,
bool treeIsHorizon,
std::vector< SVzNL3DPoint>& fittingPoints,
SSX_rodPositionInfo& a_objRod)
{
int nodeSize = a_objTree.treeNodes.size();
//拟合
double _a, _b, _c;
lineFitting_abc(fittingPoints, &_a, &_b, &_c);
@ -3231,7 +3293,7 @@ void _computeRodInfo(
_c = _c / tmpData;
SVzNL3DPoint realStart, realEnd;
bool foundStart = false;
for (int j = 0; j < nodeSize; j++)
for (int j = startIdx; j < endIdx; j++)
{
SVzNL3DPoint a_pt = a_objTree.treeNodes[j].peakPt;
if (true == treeIsHorizon)
@ -3246,13 +3308,13 @@ void _computeRodInfo(
}
if (false == foundStart)
{
realStart = a_objTree.treeNodes[0].peakPt;
realStart = a_objTree.treeNodes[startIdx].peakPt;
if (true == treeIsHorizon)
realStart = _exchangeXY(realStart);
}
bool foundEnd = false;
for (int j = nodeSize - 1; j >= 0; j--)
for (int j = endIdx; j >= startIdx; j--)
{
SVzNL3DPoint a_pt = a_objTree.treeNodes[j].peakPt;
if (true == treeIsHorizon)
@ -3267,7 +3329,7 @@ void _computeRodInfo(
}
if (false == foundEnd)
{
realEnd = a_objTree.treeNodes[nodeSize - 1].peakPt;
realEnd = a_objTree.treeNodes[endIdx].peakPt;
if (true == treeIsHorizon)
realEnd = _exchangeXY(realEnd);
}
@ -3277,7 +3339,7 @@ void _computeRodInfo(
double dist_s = sqrt(pow(foot_s.x - foot1.x, 2) + pow(foot_s.y - foot1.y, 2));
double dist_e = sqrt(pow(foot_e.x - foot1.x, 2) + pow(foot_e.y - foot1.y, 2));
//生成目标信息
;
a_objRod.startPt = { foot_s.x, foot_s.y, -(dist_s / len) * deltaZ + fittingPoints[0].z };
a_objRod.endPt = { foot_e.x, foot_e.y, (dist_e / len) * deltaZ + fittingPoints[0].z };
a_objRod.center = { (a_objRod.startPt.x + a_objRod.endPt.x) / 2,
@ -3377,16 +3439,19 @@ void sx_rodPositioning(
}
}
//内部参数
const double segment_maxDistTh = 5.0;
const double segment_minSegSize = rodParam.diameter / 8;
//在垂直方向上分别提取ARC特征，并进行特征生长
std::vector<std::vector<SWD_rodArcFeature>> arcFeatures_v;
rodAarcFeatueDetection( scanLines, cornerPara, filterParam, rodParam.diameter, arcFeatures_v);
rodArcFeatueDetection( scanLines, cornerPara, filterParam, segment_maxDistTh, segment_minSegSize, rodParam.diameter, arcFeatures_v);
//特征生长
std::vector<SWD_rodArcFeatureTree> rodArcTrees_v;
wd_getRodArcFeatureGrowingTrees(arcFeatures_v, rodArcTrees_v, growParam);
//水平方向
std::vector<std::vector<SWD_rodArcFeature>> arcFeatures_h;
rodAarcFeatueDetection(hLines_raw, cornerPara, filterParam, rodParam.diameter, arcFeatures_h);
rodArcFeatueDetection(hLines_raw, cornerPara, filterParam, segment_maxDistTh, segment_minSegSize, rodParam.diameter, arcFeatures_h);
//特征生长
std::vector<SWD_rodArcFeatureTree> rodArcTrees_h;
wd_getRodArcFeatureGrowingTrees(arcFeatures_h, rodArcTrees_h, growParam);
@ -3441,32 +3506,65 @@ void sx_rodPositioning(
int nodeSize = rodArcTrees_v[i].treeNodes.size();
SVzNL3DPoint startCenter = rodArcTrees_v[i].treeNodes[0].peakPt;
SVzNL3DPoint endCenter = rodArcTrees_v[i].treeNodes[nodeSize-1].peakPt;
double len = sqrt(pow(startCenter.x - endCenter.x, 2) +
pow(startCenter.y - endCenter.y, 2) +
pow(startCenter.z - endCenter.z, 2));
double lenDiff = abs(len - rodParam.len);
double lenDiff2 = abs(len - 2 * rodParam.len);
if ( (lenDiff < rodParam.len* 0.15) || (lenDiff2 < rodParam.len * 0.15)) //validObj
double sumLen = sqrt(pow(startCenter.x - endCenter.x, 2) +
pow(startCenter.y - endCenter.y, 2) +
pow(startCenter.z - endCenter.z, 2));
double len_ratio = sumLen / rodParam.len;
int len_mod = (int)(len_ratio + 0.5);
double len_diff = (double)len_mod - len_ratio;
if (len_diff < 0.15)
{
//在XY平面内直线拟合
//为了防止端部影响，跳过端面数据
std::vector<SVzNL3DPoint> fittingPoints;
for (int j = 0; j < nodeSize; j++)
double preSegLen = sumLen / (double)len_mod;
std::vector<SVzNLRange> segEndings;
segEndings.resize(len_mod);
int preIdx = -1;
for (int m = 0; m < len_mod; m++)
{
double dist1 = sqrt(pow(rodArcTrees_v[i].treeNodes[j].peakPt.x - startCenter.x, 2) +
pow(rodArcTrees_v[i].treeNodes[j].peakPt.y - startCenter.y, 2) +
pow(rodArcTrees_v[i].treeNodes[j].peakPt.z - startCenter.z, 2));
double dist2 = sqrt(pow(rodArcTrees_v[i].treeNodes[j].peakPt.x - endCenter.x, 2) +
pow(rodArcTrees_v[i].treeNodes[j].peakPt.y - endCenter.y, 2) +
pow(rodArcTrees_v[i].treeNodes[j].peakPt.z - endCenter.z, 2));
if ((dist1 > rodParam.diameter / 2) && (dist2 > rodParam.diameter / 2))
fittingPoints.push_back(rodArcTrees_v[i].treeNodes[j].peakPt);
int distIdx = preIdx + 1;
startCenter = rodArcTrees_v[i].treeNodes[distIdx].peakPt;
SVzNLRange a_segEnding = { distIdx , distIdx };
double currSegLen = 0;
while (distIdx < nodeSize)
{
currSegLen = sqrt(pow(rodArcTrees_v[i].treeNodes[distIdx].peakPt.x - startCenter.x, 2) +
pow(rodArcTrees_v[i].treeNodes[distIdx].peakPt.y - startCenter.y, 2) +
pow(rodArcTrees_v[i].treeNodes[distIdx].peakPt.z - startCenter.z, 2));
if (currSegLen > preSegLen)
break;
else
a_segEnding.nMax = distIdx;
distIdx++;
}
segEndings[m] = a_segEnding;
preIdx = a_segEnding.nMax;
}
for (int m = 0; m < len_mod; m++)
{
//在XY平面内直线拟合
//为了防止端部影响，跳过端面数据
std::vector<SVzNL3DPoint> fittingPoints;
for (int j = segEndings[m].nMin; j <= segEndings[m].nMax; j++)
{
int sIdx = segEndings[m].nMin;
int eIdx = segEndings[m].nMax;
startCenter = rodArcTrees_v[i].treeNodes[sIdx].peakPt;
endCenter = rodArcTrees_v[i].treeNodes[eIdx].peakPt;
double dist1 = sqrt(pow(rodArcTrees_v[i].treeNodes[j].peakPt.x - startCenter.x, 2) +
pow(rodArcTrees_v[i].treeNodes[j].peakPt.y - startCenter.y, 2) +
pow(rodArcTrees_v[i].treeNodes[j].peakPt.z - startCenter.z, 2));
double dist2 = sqrt(pow(rodArcTrees_v[i].treeNodes[j].peakPt.x - endCenter.x, 2) +
pow(rodArcTrees_v[i].treeNodes[j].peakPt.y - endCenter.y, 2) +
pow(rodArcTrees_v[i].treeNodes[j].peakPt.z - endCenter.z, 2));
if ((dist1 > rodParam.diameter / 2) && (dist2 > rodParam.diameter / 2))
fittingPoints.push_back(rodArcTrees_v[i].treeNodes[j].peakPt);
}
if (fittingPoints.size() < 3)
continue;
SSX_rodPositionInfo a_objRod;
_computeRodInfo(rodArcTrees_v[i], segEndings[m].nMin, segEndings[m].nMax, false, fittingPoints, a_objRod);
rodInfo.push_back(a_objRod);
}
if (fittingPoints.size() < 3)
continue;
SSX_rodPositionInfo a_objRod;
_computeRodInfo(rodArcTrees_v[i], false, fittingPoints, a_objRod);
rodInfo.push_back(a_objRod);
}
}
//水平目标
@ -3478,32 +3576,67 @@ void sx_rodPositioning(
SVzNL3DPoint endCenter = rodArcTrees_h[i].treeNodes[nodeSize - 1].peakPt;
endCenter = _exchangeXY(endCenter);
double len = sqrt(pow(startCenter.x - endCenter.x, 2) +
double sumLen = sqrt(pow(startCenter.x - endCenter.x, 2) +
pow(startCenter.y - endCenter.y, 2) +
pow(startCenter.z - endCenter.z, 2));
double lenDiff = abs(len - rodParam.len);
if (lenDiff < rodParam.len * 0.15) //validObj
double len_ratio = sumLen / rodParam.len;
int len_mod = (int)(len_ratio + 0.5);
double len_diff = (double)len_mod - len_ratio;
if (len_diff < 0.15)
{
//在XY平面内直线拟合
//为了防止端部影响，跳过端面数据
std::vector<SVzNL3DPoint> fittingPoints;
for (int j = 0; j < nodeSize; j++)
double preSegLen = sumLen / (double)len_mod;
std::vector<SVzNLRange> segEndings;
segEndings.resize(len_mod);
int preIdx = -1;
for (int m = 0; m < len_mod; m++)
{
SVzNL3DPoint a_pt = _exchangeXY(rodArcTrees_h[i].treeNodes[j].peakPt);
double dist1 = sqrt(pow(a_pt.x - startCenter.x, 2) + pow(a_pt.y - startCenter.y, 2) + pow(a_pt.z - startCenter.z, 2));
double dist2 = sqrt(pow(a_pt.x - endCenter.x, 2) + pow(a_pt.y - endCenter.y, 2) + pow(a_pt.z - endCenter.z, 2));
if ((dist1 > rodParam.diameter / 2) && (dist2 > rodParam.diameter / 2))
fittingPoints.push_back(a_pt);
int distIdx = preIdx + 1;
startCenter = _exchangeXY(rodArcTrees_h[i].treeNodes[distIdx].peakPt);
SVzNLRange a_segEnding = { distIdx , distIdx };
double currSegLen = 0;
while (distIdx < nodeSize)
{
SVzNL3DPoint a_pt = _exchangeXY(rodArcTrees_h[i].treeNodes[distIdx].peakPt);
currSegLen = sqrt(pow(a_pt.x - startCenter.x, 2) +
pow(a_pt.y - startCenter.y, 2) +
pow(a_pt.z - startCenter.z, 2));
if (currSegLen > preSegLen)
break;
else
a_segEnding.nMax = distIdx;
distIdx++;
}
segEndings[m] = a_segEnding;
preIdx = a_segEnding.nMax;
}
if (fittingPoints.size() < 3)
continue;
for (int m = 0; m < len_mod; m++)
{
//在XY平面内直线拟合
//为了防止端部影响，跳过端面数据
int sIdx = segEndings[m].nMin;
int eIdx = segEndings[m].nMax;
startCenter = _exchangeXY(rodArcTrees_h[i].treeNodes[sIdx].peakPt);
endCenter = _exchangeXY(rodArcTrees_h[i].treeNodes[eIdx].peakPt);
SSX_rodPositionInfo a_objRod;
_computeRodInfo(rodArcTrees_h[i], true, fittingPoints, a_objRod);
//检查是否与垂直检测目标重叠
bool isExist = checkObjEixst(a_objRod, rodInfo, rodParam);
if(false == isExist)
rodInfo.push_back(a_objRod);
std::vector<SVzNL3DPoint> fittingPoints;
for (int j = segEndings[m].nMin; j < segEndings[m].nMax; j++)
{
SVzNL3DPoint a_pt = _exchangeXY(rodArcTrees_h[i].treeNodes[j].peakPt);
double dist1 = sqrt(pow(a_pt.x - startCenter.x, 2) + pow(a_pt.y - startCenter.y, 2) + pow(a_pt.z - startCenter.z, 2));
double dist2 = sqrt(pow(a_pt.x - endCenter.x, 2) + pow(a_pt.y - endCenter.y, 2) + pow(a_pt.z - endCenter.z, 2));
if ((dist1 > rodParam.diameter / 2) && (dist2 > rodParam.diameter / 2))
fittingPoints.push_back(a_pt);
}
if (fittingPoints.size() < 3)
continue;
SSX_rodPositionInfo a_objRod;
_computeRodInfo(rodArcTrees_h[i], segEndings[m].nMin, segEndings[m].nMax, true, fittingPoints, a_objRod);
//检查是否与垂直检测目标重叠
bool isExist = checkObjEixst(a_objRod, rodInfo, rodParam);
if (false == isExist)
rodInfo.push_back(a_objRod);
}
}
}
//（2）遮挡判断
@ -4630,6 +4763,7 @@ void sx_rebarWeldSeamPositioning(
const SSG_treeGrowParam growParam,
const SSX_rodParam rodParam,
const SVzNLRangeD weldSeamRange, //焊缝距钢筋交叉点的范围（最小值和最大值）
EWD_weldingCategory weldCategory,
std::vector<SSX_weldSeamInfo>& weldSeamInfo,
int* errCode)
{
@ -4708,13 +4842,24 @@ void sx_rebarWeldSeamPositioning(
}
#endif
//在垂直方向上分别提取ARC特征，并进行特征生长
//在垂直方向上分别提取ARC特征，并进行特征生长 const double segment_maxDistTh = rodDiameter / 4;
const double segment_maxDistTh = rodParam.diameter / 4;
const double segment_minSegSize = rodParam.diameter / 8;
std::vector<std::vector<SWD_rodArcFeature>> arcFeatures_v;
rodAarcFeatueDetection(scanLines, cornerPara, filterParam, rodParam.diameter, arcFeatures_v);
#if 1
rodArcFeatueDetection(scanLines, cornerPara, filterParam, segment_maxDistTh, segment_minSegSize, rodParam.diameter, arcFeatures_v);
#else
lineArcAndWeldFeatueDetection(scanLines, cornerPara, filterParam, rodParam.diameter, arcFeatures_v);
#endif
//特征生长
std::vector<SWD_rodArcFeatureTree> allRodArcTrees_v;
wd_getRodArcFeatureGrowingTrees(arcFeatures_v, allRodArcTrees_v, growParam);
//提取内V和外V特征(按段进行)
//根据水平度过滤目标（已经调平）
double maxTanValue = tan(10.0 * PI / 180);
std::vector<SWD_rodArcFeatureTree> rodArcTrees_v;
@ -4730,7 +4875,7 @@ void sx_rebarWeldSeamPositioning(
}
//水平方向
std::vector<std::vector<SWD_rodArcFeature>> arcFeatures_h;
rodAarcFeatueDetection(hLines_raw, cornerPara, filterParam, rodParam.diameter, arcFeatures_h);
rodArcFeatueDetection(hLines_raw, cornerPara, filterParam, segment_maxDistTh, segment_minSegSize, rodParam.diameter, arcFeatures_h);
//特征生长
std::vector<SWD_rodArcFeatureTree> allRodArcTrees_h;
wd_getRodArcFeatureGrowingTrees(arcFeatures_h, allRodArcTrees_h, growParam);
@ -4844,6 +4989,7 @@ void sx_rebarWeldSeamPositioning(
else
vRodType = 1;
//沿交点向两侧寻找
//内部参数
double validCrossRebar_distTh = rodParam.diameter * 1.5;
@ -4979,7 +5125,6 @@ void sx_rebarWeldSeamPositioning(
SWD3DPointPostion nearestLinePt_topRebar = _computeNearestPoint(highestRod, crossPt, &posIdx);
crossPoints.push_back(nearestLinePt_topRebar);
//计算垂直向上的向量，用于计算焊缝方向
SVzNL3DPoint vec_axial = { highestRod.line_end.point.x - highestRod.line_start.point.x,
highestRod.line_end.point.y - highestRod.line_start.point.y,
@ -4993,29 +5138,33 @@ void sx_rebarWeldSeamPositioning(
a_vectorPosition.nPointIdx = posIdx;
a_vectorPosition.pt3D = v;
vectorPositions.push_back(a_vectorPosition);
//计算下面钢筋的焊缝
//SWD3DPointPostion nearestLinePt_btmRebar = _computeNearestPoint(validVRod[i], crossPt);
std::vector<SSX_weldSeamInfo> a_rebarWeldSeam;
_computeRebarWeldSeam(
validVRod[i],
v,
true,
crossPt,
-1,
a_rebarWeldSeam,
scanLines,
rodParam,
weldPointCornerParam,
weldSeamRange //焊缝距钢筋交叉点的范围（最大值）
);
weldSeamInfo.insert(weldSeamInfo.end(), a_rebarWeldSeam.begin(), a_rebarWeldSeam.end());
if (KeWD_WELD_CATEGPRY_I == weldCategory)
{
//计算下面钢筋的焊缝
//SWD3DPointPostion nearestLinePt_btmRebar = _computeNearestPoint(validVRod[i], crossPt);
std::vector<SSX_weldSeamInfo> a_rebarWeldSeam;
_computeRebarWeldSeam(
validVRod[i],
v,
true,
crossPt,
-1,
a_rebarWeldSeam,
scanLines,
rodParam,
weldPointCornerParam,
weldSeamRange //焊缝距钢筋交叉点的范围（最大值）
);
weldSeamInfo.insert(weldSeamInfo.end(), a_rebarWeldSeam.begin(), a_rebarWeldSeam.end());
}
}
for (int i = 0; i < (int)crossPoints.size(); i++)
for (int i = 0; i < (int)vectorPositions.size(); i++)
{
if (vectorPositions[i].nPointIdx < 0)
continue;
for (int j = i + 1; j < (int)crossPoints.size(); j++)
for (int j = i + 1; j < (int)vectorPositions.size(); j++)
{
if (vectorPositions[j].nPointIdx < 0)
continue;
@ -5025,7 +5174,7 @@ void sx_rebarWeldSeamPositioning(
vectorPositions[j].nPointIdx = -1;
}
}
for (int i = 0; i < (int)crossPoints.size(); i++)
for (int i = 0; i < (int)vectorPositions.size(); i++)
{
if (vectorPositions[i].nPointIdx < 0)
continue;

8
sourceCode/rodAndBarDetection_Export.h
View File

@ -36,6 +36,13 @@ typedef struct
SVzNL3DPoint endPt;
}SSX_rodPositionInfo;
typedef enum
{
KeWD_WELD_CATEGORY_UNKNOWN = 0,
KeWD_WELD_CATEGPRY_I, //焊接类型1：每根钢筋都有支点
KeWD_WELD_CATEGPRY_II, //焊接类型2：交叉钢筋上面的钢筋有支点
}EWD_weldingCategory;
typedef enum
{
KeWD_WELD_UNKNOWN = 0,
@ -128,5 +135,6 @@ SG_APISHARED_EXPORT void sx_rebarWeldSeamPositioning(
const SSG_treeGrowParam growParam,
const SSX_rodParam rodParam,
const SVzNLRangeD weldSeamRange, //焊缝距钢筋交叉点的范围（最小值和最大值）
EWD_weldingCategory weldCategory,
std::vector<SSX_weldSeamInfo>& weldSeamInfo,
int* errCode);