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

博清定位算法修改备份

Browse Source
This commit is contained in:
jerryzeng 2026-03-10 16:57:56 +08:00
parent 73cee324c7
commit 1b0df92eaa
9 changed files with 695 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp
View File

@ -649,7 +649,7 @@ void _outputRGBDScanLapWeld_RGBD(
#define CONVERT_TO_GRID 0 #define CONVERT_TO_GRID 0
#define TEST_COMPUTE_CALIB_PARA 0 #define TEST_COMPUTE_CALIB_PARA 0
#define TEST_COMPUTE_CORNER 1 #define TEST_COMPUTE_CORNER 1
#define TEST_GROUP 8 #define TEST_GROUP 9
int main() int main()
{ {
const char* dataPath[TEST_GROUP] = { const char* dataPath[TEST_GROUP] = {
@ -661,10 +661,11 @@ int main()
"F:/ShangGu/项目/冠钦_博清科技/数据/工件点云/", //5 "F:/ShangGu/项目/冠钦_博清科技/数据/工件点云/", //5
"F:/ShangGu/项目/冠钦_博清科技/数据/顶板样式/", //6 "F:/ShangGu/项目/冠钦_博清科技/数据/顶板样式/", //6
"F:/ShangGu/项目/冠钦_博清科技/定位纠偏/现场数据/0107/", //7 "F:/ShangGu/项目/冠钦_博清科技/定位纠偏/现场数据/0107/", //7
"F:/ShangGu/项目/冠钦_博清科技/定位纠偏/现场数据/LaserDetectResult/", //8
}; };
SVzNLRange fileIdx[TEST_GROUP] = { SVzNLRange fileIdx[TEST_GROUP] = {
{1,3}, {6,8}, {6,8}, {6,8}, {6,8}, {9,11}, {2,4}, {1,4} {1,3}, {6,8}, {6,8}, {6,8}, {6,8}, {9,11}, {2,4}, {1,4}, {1,10}
}; };
#if CONVERT_TO_GRID #if CONVERT_TO_GRID
@ -690,8 +691,9 @@ int main()
#endif #endif
#if TEST_COMPUTE_CALIB_PARA #if TEST_COMPUTE_CALIB_PARA
int cvt_grp = 8;
char _calib_datafile[256]; char _calib_datafile[256];
sprintf_s(_calib_datafile, "%sLaserline_1.txt", dataPath[7]); sprintf_s(_calib_datafile, "%s1.txt", dataPath[cvt_grp]);
int lineNum = 0; int lineNum = 0;
float lineV = 0.0f; float lineV = 0.0f;
int dataCalib = 0; int dataCalib = 0;
@ -727,10 +729,10 @@ int main()
} }
// //
char calibFile[250]; char calibFile[250];
sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[7]); sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[cvt_grp]);
_outputCalibPara(calibFile, calibPara); _outputCalibPara(calibFile, calibPara);
char _out_file[256]; char _out_file[256];
sprintf_s(_out_file, "%sscanData_ground_1_calib.txt", dataPath[7]); sprintf_s(_out_file, "%sscanData_ground_1_calib.txt", dataPath[cvt_grp]);
int headNullLines = 0; int headNullLines = 0;
_outputScanDataFile_vector(_out_file, scanData, false, &headNullLines); _outputScanDataFile_vector(_out_file, scanData, false, &headNullLines);
printf("%s: calib done!\n", _calib_datafile); printf("%s: calib done!\n", _calib_datafile);
@ -741,7 +743,7 @@ int main()
const char* ver = wd_BQWorkpieceCornerVersion(); const char* ver = wd_BQWorkpieceCornerVersion();
printf("ver:%s\n", ver); printf("ver:%s\n", ver);
for (int grp = 7; grp <= 7; grp++) for (int grp = 8; grp <= 8; grp++)
{ {
SSG_planeCalibPara poseCalibPara; SSG_planeCalibPara poseCalibPara;
//初始化成单位阵 //初始化成单位阵
@ -763,7 +765,7 @@ int main()
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
{ {
//fidx =4; fidx =7;
char _scan_file[256]; char _scan_file[256];
if(0 == grp) if(0 == grp)
sprintf_s(_scan_file, "%sscanData_%d_grid.txt", dataPath[grp], fidx); sprintf_s(_scan_file, "%sscanData_%d_grid.txt", dataPath[grp], fidx);
@ -771,6 +773,8 @@ int main()
sprintf_s(_scan_file, "%s节点%d.txt", dataPath[grp], fidx); sprintf_s(_scan_file, "%s节点%d.txt", dataPath[grp], fidx);
else if (7 == grp) else if (7 == grp)
sprintf_s(_scan_file, "%sLaserline_%d.txt", dataPath[grp], fidx); sprintf_s(_scan_file, "%sLaserline_%d.txt", dataPath[grp], fidx);
else if (8 == grp)
sprintf_s(_scan_file, "%s%d.txt", dataPath[grp], fidx);
else else
sprintf_s(_scan_file, "%s%d_LazerData_Hi229229.txt", dataPath[grp], fidx); sprintf_s(_scan_file, "%s%d_LazerData_Hi229229.txt", dataPath[grp], fidx);
std::vector<std::vector< SVzNL3DPosition>> scanLines; std::vector<std::vector< SVzNL3DPosition>> scanLines;
@ -800,7 +804,7 @@ int main()
} }
#if 0 #if 0
char _out_file[256]; char _out_file[256];
sprintf_s(_out_file, "%sscanData_%d_calib.txt", dataPath[grp], fidx); sprintf_s(_out_file, "%s_%d_leveling.txt", dataPath[grp], fidx);
int headNullLines = 0; int headNullLines = 0;
_outputScanDataFile_vector(_out_file, scanLines, false, &headNullLines); _outputScanDataFile_vector(_out_file, scanLines, false, &headNullLines);
#endif #endif

185
YouJiang_sheetPositioning_test/YouJiang_sheetPositioning_test.cpp
View File

@ -929,7 +929,7 @@ SSG_planeCalibPara _readCalibPara(char* fileName)
#define TEST_COMPUTE_CALIB_PARA 0 #define TEST_COMPUTE_CALIB_PARA 0
#define TEST_COMPUTE_GRASP_POS 1 #define TEST_COMPUTE_GRASP_POS 1
#define TEST_GROUP 1 #define TEST_GROUP 1
int main() void sheetPosition_test(void)
{ {
const char* dataPath[TEST_GROUP] = { const char* dataPath[TEST_GROUP] = {
"F:/ShangGu/项目/冠钦项目/温州优匠工品薄片抓取/温州优匠工品薄片数据/", //0 "F:/ShangGu/项目/冠钦项目/温州优匠工品薄片抓取/温州优匠工品薄片数据/", //0
@ -1097,5 +1097,188 @@ int main()
#endif #endif
printf("all done!\n"); printf("all done!\n");
return;
} }
void sheetKeyHeighth_test(void)
{
const char* dataPath[TEST_GROUP] = {
"F:/ShangGu/项目/吕宁项目/测高/", //0
};
SVzNLRange fileIdx[TEST_GROUP] = {
{1,6} };
#if TEST_COMPUTE_CALIB_PARA
int cvt_grp = 0;
char _calib_datafile[256];
sprintf_s(_calib_datafile, "%s样品_grid_5.txt", dataPath[cvt_grp]);
std::vector<std::vector< SVzNL3DPosition>> scanLines;
wdReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanLines);
if (scanLines.size() > 0)
{
//getROIdata
std::vector<std::vector< SVzNL3DPosition>> roiScanLines;
SSG_ROIRectD roi2D = { 237.0, 340.0, -60, 6.0 };
getROIdata(roi2D, scanLines, roiScanLines);
char calibFile[250];
sprintf_s(calibFile, "%sgroundData_ROI.txt", dataPath[cvt_grp]);
_outputScanDataFile(calibFile, roiScanLines, 0, 0, 0);
SSG_planeCalibPara calibPara = wd_sheetPosition_getBaseCalibPara(roiScanLines);
//结果进行验证
int lineNum = (int)scanLines.size();
for (int i = 0; i < lineNum; i++)
{
if (i == 14)
int kkk = 1;
//行处理
//调平，去除地面
wd_sheetPosition_lineDataR(scanLines[i], calibPara.planeCalib, -1);// calibPara.planeHeight);
}
//
sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[cvt_grp]);
_outputCalibPara(calibFile, calibPara);
char _out_file[256];
sprintf_s(_out_file, "%sground_grid_calib_verify.txt", dataPath[cvt_grp]);
_outputScanDataFile(_out_file, scanLines, 0, 0, 0);
printf("%s: calib done!\n", _calib_datafile);
#if 1
for (int fidx = fileIdx[cvt_grp].nMin; fidx <= fileIdx[cvt_grp].nMax; fidx++)
{
char _scan_file[256];
sprintf_s(_scan_file, "%s样品_grid_%d.txt", dataPath[cvt_grp], fidx);
std::vector<std::vector< SVzNL3DPosition>> dataLines;
vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, dataLines);
if (dataLines.size() == 0)
continue;
lineNum = (int)dataLines.size();
for (int i = 0; i < lineNum; i++)
{
if (i == 14)
int kkk = 1;
//行处理
//调平，去除地面
wd_sheetPosition_lineDataR(dataLines[i], calibPara.planeCalib, -1);// calibPara.planeHeight);
}
sprintf_s(_out_file, "%s样品_grid_%d_leveling.txt", dataPath[cvt_grp], fidx);
_outputScanDataFile(_out_file, dataLines, 0, 0, 0);
printf("%s: leveling done!\n", _out_file);
}
#endif
}
#endif
#if TEST_COMPUTE_GRASP_POS
int endGroup = TEST_GROUP - 1;
for (int grp = 0; grp <= endGroup; grp++)
{
SSG_planeCalibPara poseCalibPara;
//初始化成单位阵
poseCalibPara.planeCalib[0] = 1.0;
poseCalibPara.planeCalib[1] = 0.0;
poseCalibPara.planeCalib[2] = 0.0;
poseCalibPara.planeCalib[3] = 0.0;
poseCalibPara.planeCalib[4] = 1.0;
poseCalibPara.planeCalib[5] = 0.0;
poseCalibPara.planeCalib[6] = 0.0;
poseCalibPara.planeCalib[7] = 0.0;
poseCalibPara.planeCalib[8] = 1.0;
poseCalibPara.planeHeight = 645.0;
for (int i = 0; i < 9; i++)
poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i];
char calibFile[250];
#if 1
if (grp == 0)
{
sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]);
poseCalibPara = _readCalibPara(calibFile);
}
#endif
SWD_sheetKeyParam sheetKeyParam;
sheetKeyParam.keySize = {8.0, 6.0}; //薄片按键大小
sheetKeyParam.sheetSize = {41.0,11.0}; //薄片大小
sheetKeyParam.keyToEdgeDistance = 8.0; //薄片按键距离最近的边的距离
sheetKeyParam.sheetThickness = 0.4; //薄片厚度0.4mm
sheetKeyParam.nominalKeyHeight = 0.5;
SWD_sheetAlgoParam algoParam;
memset(&algoParam, 0, sizeof(SWD_sheetAlgoParam));
algoParam.cornerParam.cornerTh = 40; //45度角
algoParam.cornerParam.scale = sheetKeyParam.sheetThickness*1.25; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
algoParam.cornerParam.minEndingGap = 5;
algoParam.cornerParam.minEndingGap_z = sheetKeyParam.sheetThickness * 0.5;
algoParam.cornerParam.jumpCornerTh_1 = 15; //水平角度，小于此角度视为水平
algoParam.cornerParam.jumpCornerTh_2 = 30;
SSG_outlierFilterParam filterParam;
algoParam.filterParam.continuityTh = 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声
algoParam.filterParam.outlierTh = 5;
SSG_treeGrowParam growParam;
algoParam.growParam.maxLineSkipNum = 5;
algoParam.growParam.yDeviation_max = 3.0;
algoParam.growParam.maxSkipDistance = 3.0;
algoParam.growParam.zDeviation_max = 3.0;// algoParam.bagParam.bagH / 2; //袋子高度1/2
algoParam.growParam.minLTypeTreeLen = 100; //mm
algoParam.growParam.minVTypeTreeLen = 100; //mm
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
{
SWD_gripState stateMachine;
memset(&stateMachine, 0, sizeof(SWD_gripState));
//fidx = 4;
int lineNum = 0;
float lineV = 0.0f;
int dataCalib = 0;
int maxTimeStamp = 0;
int clockPerSecond = 0;
char _scan_file[256];
sprintf_s(_scan_file, "%s样品_grid_%d.txt", dataPath[grp], fidx);
std::vector<std::vector< SVzNL3DPosition>> scanLines;
vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
if (scanLines.size() == 0)
continue;
long t1 = GetTickCount64();
int errCode = 0;
std::vector<SWD_sheetGrasper> resultObjPositions;
wd_computeSheetKeyHeigth(
scanLines,
sheetKeyParam,
poseCalibPara,
algoParam,
&stateMachine, //操作状态机。指示当前状态
&errCode,
resultObjPositions);
long t2 = GetTickCount64();
char _dbg_file[256];
#if 1
sprintf_s(_dbg_file, "%sresult\\样品_grid_%d_result.txt", dataPath[grp], fidx);
_outputScanDataFile_RGBD_obj(_dbg_file, scanLines, 0, 0, 0, resultObjPositions);
sprintf_s(_dbg_file, "%sresult\\样品_grid_%d_result_img.png", dataPath[grp], fidx);
cv::String imgName(_dbg_file);
double rpy[3] = { -30, 15, 0 }; //{ 0,-45, 0 }; //
//_genXOYProjectionImage(imgName, laser3DPoints, lineNum, rpy);
#endif
printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1));
}
}
#endif
printf("all done!\n");
}
int main()
{
#if 0
sheetPosition_test();
#else
sheetKeyHeighth_test();
#endif
return 0;
}

1
YouJiang_sheetPositioning_test/YouJiang_sheetPositioning_test.vcxproj
View File

@ -24,6 +24,7 @@
<ProjectGuid>{80cbf888-79f3-424f-a094-17edfd452e1e}</ProjectGuid> <ProjectGuid>{80cbf888-79f3-424f-a094-17edfd452e1e}</ProjectGuid>
<RootNamespace>YouJiangsheetPositioningtest</RootNamespace> <RootNamespace>YouJiangsheetPositioningtest</RootNamespace>
<WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion> <WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion>
<ProjectName>YouJiang_sheetPositioning_test</ProjectName>
</PropertyGroup> </PropertyGroup>
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" /> <Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration"> <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">

270
sourceCode/BQ_workpieceCornerExtraction.cpp
View File

@ -9,7 +9,8 @@
//version 1.2.0 : add position length output //version 1.2.0 : add position length output
//version 1.2.1 : fix bugs for ver1.2.0 //version 1.2.1 : fix bugs for ver1.2.0
//version 1.3.0 : 算法对同一brach判断添加了距离判断 //version 1.3.0 : 算法对同一brach判断添加了距离判断
std::string m_strVersion = "1.3.0"; //version 1.4.0 : 使用轮廓点拐角判断目标类型
std::string m_strVersion = "1.4.0";
const char* wd_BQWorkpieceCornerVersion(void) const char* wd_BQWorkpieceCornerVersion(void)
{ {
return m_strVersion.c_str(); return m_strVersion.c_str();
@ -585,6 +586,256 @@ bool checkSameBranch(SWD_polarPt& corner_1, SWD_polarPt& corner_2,
} }
//计算封闭序列（首尾相连）在XOY平面内前向角和后向角以及拐角。 前向角：序号+； 后向角：序号-
void _computeClosedPntListDirCorners(std::vector<SWD_polarPt>& polarPoints, double scale, std::vector< SSG_dirCornerAngle>& dirCornerAngles)
{
int pntSize = (int)polarPoints.size();
//std::fill(dirCornerAngles.begin(), dirCornerAngles.end(), SSG_dirCornerAngle{ 0, 0, 0, 0, 0, 0 });
dirCornerAngles.resize(pntSize);
for (int i = 0; i < pntSize; i++)
{
memset(&dirCornerAngles[i], 0, sizeof(SSG_dirCornerAngle));
SWD_polarPt& a_polarPt = polarPoints[i];
//前向寻找
int minus_i = -1;
for (int loop = i - 1; loop >= i-pntSize; loop--)
{
int j = loop >= 0 ? loop : (loop + pntSize);
double dist = sqrt(pow(polarPoints[i].x - polarPoints[j].x, 2) +
pow(polarPoints[i].y - polarPoints[j].y, 2));
if (dist >= scale)
{
minus_i = j;
break;
}
}
//后向寻找
int plus_i = -1;
for (int loop = i + 1; loop < i+pntSize; loop++)
{
int j = loop % pntSize;
double dist = sqrt(pow(polarPoints[i].x - polarPoints[j].x, 2) +
pow(polarPoints[i].y - polarPoints[j].y, 2));
if (dist >= scale)
{
plus_i = j;
break;
}
}
//计算拐角
if ((minus_i >= 0) && (plus_i >= 0))
{
double backwardAngle = atan2(polarPoints[i].y - polarPoints[minus_i].y, polarPoints[i].x - polarPoints[minus_i].x) * 180.0 / PI;
double forwardAngle = atan2(polarPoints[plus_i].y - polarPoints[i].y, polarPoints[plus_i].x - polarPoints[i].x) * 180.0 / PI;
dirCornerAngles[i].forwardAngle = forwardAngle;
dirCornerAngles[i].backwardAngle = backwardAngle;
double corner = forwardAngle - backwardAngle;
if (corner < -180)
corner += 360;
else if (corner > 180)
corner = corner - 360;
dirCornerAngles[i].corner = corner; //图像坐标系与正常坐标系y方向相反，所以有“-”号
dirCornerAngles[i].pntIdx = i;
dirCornerAngles[i].forward_pntIdx = plus_i;
dirCornerAngles[i].backward_pntIdx = minus_i;
dirCornerAngles[i].point = polarPoints[i];
}
}
}
//提取corner极值（较早实现函数可以使用此函数进行代码优化）
void _searchPlusCornerPeaks(
std::vector< SSG_dirCornerAngle>& corners,
std::vector< SSG_dirCornerAngle>& cornerPlusPeaks
)
{
std::vector<SSG_dirCornerAngle> peakCorners;
int cornerSize = (int)corners.size();
//搜索拐角极值
int _state = 0;
int pre_i = -1;
int sEdgePtIdx = -1;
int eEdgePtIdx = -1;
SSG_dirCornerAngle* pre_data = NULL;
for (int i = 0, i_max = cornerSize; i < i_max; i++)
{
if (i == 275)
int kkk = 1;
SSG_dirCornerAngle* curr_data = &corners[i];
if (NULL == pre_data)
{
sEdgePtIdx = i;
eEdgePtIdx = i;
pre_data = curr_data;
pre_i = i;
continue;
}
eEdgePtIdx = i;
double cornerDiff = curr_data->corner - pre_data->corner;
switch (_state)
{
case 0: //初态
if (cornerDiff < 0) //下降
{
_state = 2;
}
else if (cornerDiff > 0) //上升
{
_state = 1;
}
break;
case 1: //上升
if (cornerDiff < 0) //下降
{
if (pre_data->corner > 10) //使用10度作为门限，滤除点波动导致的角度变化
peakCorners.push_back(*pre_data);
_state = 2;
}
break;
case 2: //下降
if (cornerDiff > 0) // 上升
_state = 1;
break;
default:
_state = 0;
break;
}
pre_data = curr_data;
pre_i = i;
}
//注意：最后一个不处理，为基座位置
//极小值点（峰顶）
//极值比较，在尺度窗口下寻找局部极值点
for (int i = 0, i_max = (int)peakCorners.size(); i < i_max; i++)
{
bool isPeak = true;
SSG_dirCornerAngle& a_dirAngle = peakCorners[i];
int curr_dist1 = a_dirAngle.pntIdx - a_dirAngle.backward_pntIdx;
if (curr_dist1 < 0)
curr_dist1 += cornerSize;
int curr_dist2 = a_dirAngle.forward_pntIdx - a_dirAngle.pntIdx;
if (curr_dist2 < 0)
curr_dist2 += cornerSize;
//minus方向寻找
int minus_i = i;
while (1)
{
minus_i--;
if (minus_i < 0)
minus_i += i_max;
SSG_dirCornerAngle& minus_dirAngle = peakCorners[minus_i];
int pntDist_0 = a_dirAngle.pntIdx - minus_dirAngle.pntIdx;
if (pntDist_0 < 0)
pntDist_0 += cornerSize;
if (pntDist_0 > curr_dist1)
break;
if (a_dirAngle.corner < minus_dirAngle.corner)
{
isPeak = false;
break;
}
}
//plus方向寻找
int plus_i = i;
while (1)
{
plus_i++;
if (plus_i >= i_max)
plus_i = 0;
SSG_dirCornerAngle& plus_dirAngle = peakCorners[plus_i];
int pntDist_0 = plus_dirAngle.pntIdx - a_dirAngle.pntIdx;
if (pntDist_0 < 0)
pntDist_0 += cornerSize;
if (pntDist_0 > curr_dist2)
break;
if (a_dirAngle.corner < plus_dirAngle.corner)
{
isPeak = false;
break;
}
}
if (true == isPeak)
{
double corner_curr = a_dirAngle.corner;
double corner_1 = corners[a_dirAngle.forward_pntIdx].corner;
double corner_2 = corners[a_dirAngle.backward_pntIdx].corner;
double diff_1 = corner_curr - corner_1;
double diff_2 = corner_curr - corner_2;
if ((diff_1 > corner_curr / 2) && (diff_2 > corner_curr / 2))
cornerPlusPeaks.push_back(a_dirAngle);
}
}
}
//根据轮廓的角点计算Branch， 并判断是否缺角。缺角时通过拟合方式补全
// std::vector<SWD_polarPt>& polarPoints: 点的循环对列
//branchCornerAngle: 支持寻找不同角度的Branch，由branchCornerAngle指定。90度为直角。
//angleTh: 角度误差范围。比如名义为90的直角，实际加工会有误差。
//cutLineDeviationTh:以截角两点连线，截角上点到直线的最大偏离。保证截角是直线
void _computeBranchesFromCornerPeaks(std::vector<SWD_polarPt>& polarPoints, std::vector< SSG_dirCornerAngle>& cornerPlusPeaks, std::vector<SWD_branchInfo>& branches, double branchCornerAngle, double angleTh, double cutLineDeviationTh)
{
int ringBuffSize = (int)polarPoints.size();
//判断是否有缺角
int size_peaks = (int)cornerPlusPeaks.size();
std::vector<int> cutPairFlags;
cutPairFlags.resize(size_peaks);
std::fill(cutPairFlags.begin(), cutPairFlags.end(), 0);
int cutPairID = 1;
for (int i = 0; i < size_peaks; i++)
{
if (cutPairFlags[i] > 0)
continue;
SSG_dirCornerAngle& corner_1 = cornerPlusPeaks[i];
int tmpIdx = (i + 1) % size_peaks;
SSG_dirCornerAngle& corner_2 = cornerPlusPeaks[tmpIdx];
if (corner_2.pntIdx < 0)
continue;
double sumCorner = corner_1.corner + corner_2.corner;
double angleDiff = abs(sumCorner - branchCornerAngle);
if (angleDiff < angleTh)
{
//偏离度判断
//计算两点连线
double a, b, c;
compute2ptLine_2(
corner_1.point.x, corner_1.point.y,
corner_2.point.x, corner_2.point.y,
&a, &b, &c);
double tmp = sqrt(a * a + b * b);
a = a / tmp;
b = b / tmp;
c = c / tmp;
double maxDist = 0;
int number = corner_2.pntIdx - corner_1.pntIdx;
if (number < 0)
number += ringBuffSize;
for (int m = 0; m < number; m++)
{
int idx = (m + corner_1.pntIdx) % ringBuffSize;
double dist = abs(a * polarPoints[idx].x + b * polarPoints[m].y + c);
if (maxDist < dist)
maxDist = dist;
}
if (maxDist < cutLineDeviationTh) //判断为截角
{
cutPairFlags[i] = cutPairID;
cutPairFlags[i+1] = cutPairID;
cutPairID++;
}
}
}
//迭代一次，对于循环Buff, 需要重新检查首尾结合部
}
SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
std::vector< std::vector<SVzNL3DPosition>>& scanLines, std::vector< std::vector<SVzNL3DPosition>>& scanLines,
@ -876,6 +1127,17 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
for (int pi = 0; pi < contourPtSize; pi++) for (int pi = 0; pi < contourPtSize; pi++)
polarPoints[pi].cptIndex = pi; // index polarPoints[pi].cptIndex = pi; // index
//计算有序边缘点的角度变化 2026.03.06
std::vector< SSG_dirCornerAngle> dirCornerAngles;
double scale = 20.0;
_computeClosedPntListDirCorners(polarPoints, scale, dirCornerAngles);
//提取方向角拐点(正）极值
std::vector< SSG_dirCornerAngle> cornerPlusPeaks;
_searchPlusCornerPeaks(dirCornerAngles, cornerPlusPeaks);
//使用R过滤，判断是否缺角
std::vector<SWD_branchInfo> branches;
_computeBranchesFromCornerPeaks(cornerPlusPeaks, branches);
//提取R极值点 //提取R极值点
double minR = -1, maxR = -1; //计算最小和最大的R，用以区分有没有分支。minR和maxR相差小时，为圆形或8角形，没有分支 double minR = -1, maxR = -1; //计算最小和最大的R，用以区分有没有分支。minR和maxR相差小时，为圆形或8角形，没有分支
int minRPos = -1; int minRPos = -1;
@ -1035,6 +1297,9 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
branchPeakInfo.erase(branchPeakInfo.begin() + m); branchPeakInfo.erase(branchPeakInfo.begin() + m);
} }
} }
int branchNum = (int)branchPeaks.size(); int branchNum = (int)branchPeaks.size();
if (branchNum == 2) if (branchNum == 2)
{ {
@ -1169,6 +1434,9 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
debug_contours.push_back(a_branchDebug); debug_contours.push_back(a_branchDebug);
#endif #endif
} }
workpieceCorners.workpieceType = workpieceType; workpieceCorners.workpieceType = workpieceType;
if (workpieceType == 1) //4个branch if (workpieceType == 1) //4个branch
{ {

3
sourceCode/SG_baseAlgo_Export.h
View File

@ -512,7 +512,8 @@ SG_APISHARED_EXPORT bool leastSquareParabolaFitEigen(
//计算Z均值 //计算Z均值
SG_APISHARED_EXPORT double computeMeanZ(std::vector< SVzNL3DPoint>& pts); SG_APISHARED_EXPORT double computeMeanZ(std::vector< SVzNL3DPoint>& pts);
SG_APISHARED_EXPORT double computeROIMeanZ(std::vector<std::vector< SVzNL3DPosition>>& scanLines, SVzNLRect roi);
SG_APISHARED_EXPORT void computeROIZCutLabel(std::vector<std::vector< SVzNL3DPosition>>& scanLines, SVzNLRect roi, double cutZ, int labelID);
//计算角度差值，在0-180度范围 //计算角度差值，在0-180度范围
SG_APISHARED_EXPORT double computeAngleDiff(double theta1, double theta2); SG_APISHARED_EXPORT double computeAngleDiff(double theta1, double theta2);

45
sourceCode/SG_baseDataType.h
View File

@ -100,6 +100,12 @@ typedef struct
double bottom; double bottom;
}SSG_ROIRectD; }SSG_ROIRectD;
typedef struct
{
double width;
double height;
}SSG_size2D;
typedef struct typedef struct
{ {
SVzNL3DPoint center; SVzNL3DPoint center;
@ -526,6 +532,20 @@ typedef struct
double invRMatrix[9]; //旋转矩阵，回到原坐标系 double invRMatrix[9]; //旋转矩阵，回到原坐标系
}SSG_planeCalibPara; }SSG_planeCalibPara;
typedef struct
{
int pkId;
int lineIdx;
int ptIdx;
int cptIndex; //圆周扫描上的点序
//double cornerAngle; //以点为中心的两侧弦的夹角
double R;
double angle;
double x;
double y;
double z;
}SWD_polarPt;
typedef struct typedef struct
{ {
int pntIdx; int pntIdx;
@ -541,6 +561,17 @@ typedef struct
double backward_z; double backward_z;
}SSG_pntDirAngle; }SSG_pntDirAngle;
typedef struct
{
double forwardAngle; //前向角
double backwardAngle; //后向角
double corner; //拐角
int pntIdx;
int forward_pntIdx;
int backward_pntIdx;
SWD_polarPt point;
}SSG_dirCornerAngle;
// 图像数据结构 // 图像数据结构
typedef struct { typedef struct {
int width; int width;
@ -562,20 +593,6 @@ typedef struct
SVzNL3DPoint pt2; SVzNL3DPoint pt2;
}SWD_3DPointPair; }SWD_3DPointPair;
typedef struct
{
int pkId;
int lineIdx;
int ptIdx;
int cptIndex; //圆周扫描上的点序
//double cornerAngle; //以点为中心的两侧弦的夹角
double R;
double angle;
double x;
double y;
double z;
}SWD_polarPt;
typedef struct typedef struct
{ {
int cptIndex; int cptIndex;

34
sourceCode/SG_baseFunc.cpp
View File

@ -298,6 +298,40 @@ double computeMeanZ(std::vector< SVzNL3DPoint>& pts)
return 0; return 0;
} }
double computeROIMeanZ(std::vector<std::vector< SVzNL3DPosition>>& scanLines, SVzNLRect roi)
{
int vldNum = 0;
double sumZ = 0;
for (int line = roi.left; line <= roi.right; line++)
{
for (int i = roi.top; i <= roi.bottom; i++)
{
if (scanLines[line][i].pt3D.z > 1e-4)
{
sumZ += scanLines[line][i].pt3D.z;
vldNum++;
}
}
}
if (vldNum > 0)
return (sumZ / vldNum);
else
return 0;
}
void computeROIZCutLabel(std::vector<std::vector< SVzNL3DPosition>>& scanLines, SVzNLRect roi, double cutZ, int labelID)
{
for (int line = roi.left; line <= roi.right; line++)
{
for (int i = roi.top; i <= roi.bottom; i++)
{
if ( (scanLines[line][i].pt3D.z > 1e-4) && (scanLines[line][i].pt3D.z < cutZ))
scanLines[line][i].nPointIdx = labelID;
}
}
return;
}
SVzNL3DPoint computeLineCrossPt_abs(double a1, double b1, double c1, double a2, double b2, double c2) SVzNL3DPoint computeLineCrossPt_abs(double a1, double b1, double c1, double a2, double b2, double c2)
{ {
SVzNL3DPoint crossPt; SVzNL3DPoint crossPt;

153
sourceCode/YouJiang_sheetPositioning.cpp
View File

@ -30,16 +30,14 @@ void wd_sheetPosition_lineDataR(
lineDataRT_vector(a_line, camPoseR, groundH); lineDataRT_vector(a_line, camPoseR, groundH);
} }
//薄片定位 //提取边缘封闭目标的边缘特征，输出聚类簇
//使用语义模块匹配：（1）提取特征（2）基于特征匹配薄片 void wd_computeClosedEdgeClusters(
void wd_YouJiang_getSheetPosition(
std::vector< std::vector<SVzNL3DPosition>>& scanLines, std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SWD_sheetTemplateParam templateParam,
const SSG_planeCalibPara groundCalibPara, const SSG_planeCalibPara groundCalibPara,
const SWD_sheetAlgoParam algoParam, const SWD_sheetAlgoParam algoParam,
SWD_gripState* opState, //操作状态机。指示当前状态
int* errCode, int* errCode,
std::vector<SWD_sheetGrasper>& resultObjPositions) std::vector<std::vector< SVzNL2DPoint>>& clusters, //只记录位置
std::vector<SVzNL3DRangeD>& clustersRoi3D)
{ {
int lineNum = (int)scanLines.size(); int lineNum = (int)scanLines.size();
if (lineNum == 0) if (lineNum == 0)
@ -198,8 +196,6 @@ void wd_YouJiang_getSheetPosition(
//聚类 //聚类
//采用迭代思想，回归思路进行高效聚类 //采用迭代思想，回归思路进行高效聚类
std::vector<std::vector< SVzNL2DPoint>> clusters; //只记录位置
std::vector<SVzNL3DRangeD> clustersRoi3D;
int clusterID = 1; int clusterID = 1;
int clusterCheckWin = 3; int clusterCheckWin = 3;
for (int y = 0; y < lineNum_h_raw; y++) for (int y = 0; y < lineNum_h_raw; y++)
@ -236,8 +232,32 @@ void wd_YouJiang_getSheetPosition(
clusterID++; clusterID++;
} }
} }
int clusterNum = (int)clusters.size(); }
//薄片定位
//使用语义模块匹配：（1）提取特征（2）基于特征匹配薄片
void wd_YouJiang_getSheetPosition(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SWD_sheetTemplateParam templateParam,
const SSG_planeCalibPara groundCalibPara,
const SWD_sheetAlgoParam algoParam,
SWD_gripState* opState, //操作状态机。指示当前状态
int* errCode,
std::vector<SWD_sheetGrasper>& resultObjPositions)
{
std::vector<std::vector< SVzNL2DPoint>> clusters;//只记录位置
std::vector<SVzNL3DRangeD> clustersRoi3D;
//提取边缘封闭目标的边缘特征，输出聚类簇
wd_computeClosedEdgeClusters(
scanLines,
groundCalibPara,
algoParam,
errCode,
clusters, //只记录位置
clustersRoi3D);
int clusterNum = (int)clusters.size();
//投影和标注。投影的目的是用于模板匹配时的特征处理。以1mm为单位.记录clusterID //投影和标注。投影的目的是用于模板匹配时的特征处理。以1mm为单位.记录clusterID
SVzNL3DRangeD roi3D = sg_getScanDataROI_vector(scanLines); SVzNL3DRangeD roi3D = sg_getScanDataROI_vector(scanLines);
double projectionScale = 1.0; //以1mm为投影尺度 double projectionScale = 1.0; //以1mm为投影尺度
@ -250,12 +270,14 @@ void wd_YouJiang_getSheetPosition(
for (int i = 0; i < clusterNum; i++) for (int i = 0; i < clusterNum; i++)
{ {
std::vector< SVzNL2DPoint>& a_cluster = clusters[i]; std::vector< SVzNL2DPoint>& a_cluster = clusters[i];
for (int j = 0; j < (int)clusters.size(); j++) for (int j = 0; j < (int)a_cluster.size(); j++)
{ {
SVzNL2DPoint& a_pt = a_cluster[i]; SVzNL2DPoint& a_pt = a_cluster[j];
SVzNL3DPosition& a_pt3d = scanLines[a_pt.x][a_pt.y]; SVzNL3DPosition& a_pt3d = scanLines[a_pt.x][a_pt.y];
int px = (int)((a_pt3d.pt3D.x - roi3D.xRange.min) / projectionScale); int px = (int)((a_pt3d.pt3D.x - roi3D.xRange.min) / projectionScale);
int py = (int)((a_pt3d.pt3D.y - roi3D.yRange.min) / projectionScale); int py = (int)((a_pt3d.pt3D.y - roi3D.yRange.min) / projectionScale);
if ((px < 0) || (px >= projecction_xw) || (py < 0) || (py >= projecction_yh))
int kkk = 1;
projectionClusters[px][py] = i + 1; //clusterID = index + 1 projectionClusters[px][py] = i + 1; //clusterID = index + 1
} }
} }
@ -288,4 +310,113 @@ void wd_YouJiang_getSheetPosition(
}
typedef struct
{
cv::RotatedRect rect;
cv::Point2f vertices[4];
}_RectParam;
//薄片按键高度测量
void wd_computeSheetKeyHeigth(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SWD_sheetKeyParam sheetKeyParam,
const SSG_planeCalibPara groundCalibPara,
const SWD_sheetAlgoParam algoParam,
SWD_gripState* opState, //操作状态机。指示当前状态
int* errCode,
std::vector<SWD_sheetGrasper>& resultObjPositions)
{
std::vector<std::vector< SVzNL2DPoint>> clusters;//只记录位置
std::vector<SVzNL3DRangeD> clustersRoi3D;
//提取边缘封闭目标的边缘特征，输出聚类簇
wd_computeClosedEdgeClusters(
scanLines,
groundCalibPara,
algoParam,
errCode,
clusters, //只记录位置
clustersRoi3D);
int clusterNum = (int)clusters.size();
//聚类结果分析
//根据大小确定薄片
const double sameSizeTh_w = sheetKeyParam.sheetSize.width * 0.05;
const double sameSizeTh_h = sheetKeyParam.sheetSize.height * 0.05;
const double zAreaWin = 5;
std::vector<_RectParam> clustersRect;
clustersRect.resize(clusters.size());
#if 1
std::vector<int> validIndice;
for (int i = 0; i < clusterNum; i++)
{
std::vector< SVzNL2DPoint>& a_cluster = clusters[i];
if (a_cluster.size() < 4)
continue;
//计算最小外接矩
std::vector<cv::Point2f> points;
SVzNLRect roi = { -1,-1,-1,-1 };
for (int j = 0; j < (int)a_cluster.size(); j++)
{
SVzNL2DPoint& a_pt = a_cluster[j];
SVzNL3DPosition& a_pt3d = scanLines[a_pt.x][a_pt.y];
//生成ROI
if (roi.left < 0)
roi = { a_pt.x ,a_pt.x , a_pt.y, a_pt.y };
else
{
if (roi.left > a_pt.x)
roi.left = a_pt.x;
if (roi.right < a_pt.x)
roi.right = a_pt.x;
if (roi.top > a_pt.y)
roi.top = a_pt.y;
if (roi.bottom < a_pt.y)
roi.bottom = a_pt.y;
}
float x = (float)a_pt3d.pt3D.x;
float y = (float)a_pt3d.pt3D.y;
points.push_back(cv::Point2f(x, y));
}
if (points.size() == 0)
continue;
// 最小外接矩形
cv::RotatedRect rect = minAreaRect(points);
cv::Point2f vertices[4];
rect.points(vertices);
double width = rect.size.width; //投影的宽和高， 对应袋子的长和宽
double height = rect.size.height;
if (width < height)
{
double tmp = height;
height = width;
width = tmp;
}
int cx = (roi.left + roi.right) / 2;
int cy = (roi.top + roi.bottom) / 2;
//判别
double w_diff = abs(width - sheetKeyParam.sheetSize.width);
double h_diff = abs(height - sheetKeyParam.sheetSize.height);
if ((w_diff < sameSizeTh_w) && (h_diff < sameSizeTh_h))
{
//寻找到目标
//取中心点，以中心点附近区域计算Z
int cx = (roi.left + roi.right) / 2;
int cy = (roi.top + roi.bottom) / 2;
SVzNLRect meanZRoi = { cx - zAreaWin , cx + zAreaWin , cy - zAreaWin , cy + zAreaWin };
double meanZ = computeROIMeanZ(scanLines, meanZRoi);
//以Z值作为门限进行切割
double cutZ = meanZ - sheetKeyParam.nominalKeyHeight * 0.75;
int labelID = i + 100; //ID从100开始
computeROIZCutLabel(scanLines, roi, cutZ, labelID);
}
}
#endif
} }

19
sourceCode/YouJiang_sheetPositioning_Export.h
View File

@ -17,6 +17,15 @@ typedef struct
double centerBossHeight; //薄片中央凸起高度 double centerBossHeight; //薄片中央凸起高度
}SWD_sheetTemplateParam; }SWD_sheetTemplateParam;
typedef struct
{
SSG_size2D keySize; //薄片按键大小
SSG_size2D sheetSize; //薄片大小
double keyToEdgeDistance; //按键距最近的边的距离
double sheetThickness; //薄片厚度
double nominalKeyHeight; //名义按键高度
}SWD_sheetKeyParam;
typedef struct typedef struct
{ {
SSG_outlierFilterParam filterParam; SSG_outlierFilterParam filterParam;
@ -65,3 +74,13 @@ SG_APISHARED_EXPORT void wd_YouJiang_getSheetPosition(
SWD_gripState* opState, //操作状态机。指示当前状态 SWD_gripState* opState, //操作状态机。指示当前状态
int* errCode, int* errCode,
std::vector<SWD_sheetGrasper>& resultObjPositions); std::vector<SWD_sheetGrasper>& resultObjPositions);
//薄片按键高度测量
SG_APISHARED_EXPORT void wd_computeSheetKeyHeigth(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SWD_sheetKeyParam sheetKeyParam,
const SSG_planeCalibPara groundCalibPara,
const SWD_sheetAlgoParam algoParam,
SWD_gripState* opState, //操作状态机。指示当前状态
int* errCode,
std::vector<SWD_sheetGrasper>& resultObjPositions);