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

sx_BQ_getWorkpieceCorners version 1.2.1

Browse Source 
- 修正了1.2.0版本问题
This commit is contained in:
jerryzeng 2025-12-11 04:56:17 +08:00
parent 6c5adfc7d5
commit 335ee0a92f
4 changed files with 210 additions and 99 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

104
BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp
View File

@ -530,7 +530,7 @@ void _outputRGBDScanLapWeld_RGBD(
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
} }
if ((debugData.size() > 0) && (workpieceCorner.workpieceType > 0)) if (workpieceCorner.workpieceType > 0)
{ {
std::vector< SWD_3DPointPair> lines; std::vector< SWD_3DPointPair> lines;
SWD_3DPointPair a_line; SWD_3DPointPair a_line;
@ -567,52 +567,54 @@ void _outputRGBDScanLapWeld_RGBD(
lines.push_back(a_line); lines.push_back(a_line);
} }
int linePtNum = 0; if (debugData.size() > 0)
for (int i = 0; i < (int)debugData.size(); i++)
linePtNum += debugData[i].edge_size + debugData[i].edgeLink1_size + debugData[i].edgeLink2_size;
sw << "Line_" << lineNum << "_0_" << linePtNum + 1 << std::endl;
lineNum++;
rgb = { 255, 0, 0 };
size = 3;
for (int i = 0; i < (int)debugData.size(); i++)
{ {
for (int j = 0; j < debugData[i].edge_size; j++) int linePtNum = 0;
{ for (int i = 0; i < (int)debugData.size(); i++)
float x = (float)debugData[i].edge[j].x; linePtNum += debugData[i].edge_size + debugData[i].edgeLink1_size + debugData[i].edgeLink2_size;
float y = (float)debugData[i].edge[j].y; sw << "Line_" << lineNum << "_0_" << linePtNum + 1 << std::endl;
float z = (float)debugData[i].edge[j].z; lineNum++;
sw << "{" << x << "," << y << "," << z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
}
for (int j = 0; j < debugData[i].edgeLink1_size; j++)
{
float x = (float)debugData[i].edgeLink_1[j].x;
float y = (float)debugData[i].edgeLink_1[j].y;
float z = (float)debugData[i].edgeLink_1[j].z;
sw << "{" << x << "," << y << "," << z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
}
for (int j = 0; j < debugData[i].edgeLink2_size; j++)
{
float x = (float)debugData[i].edgeLink_2[j].x;
float y = (float)debugData[i].edgeLink_2[j].y;
float z = (float)debugData[i].edgeLink_2[j].z;
sw << "{" << x << "," << y << "," << z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
}
}
//加一个点，用于跳过显示工具bug
float x = (float)debugData[0].edge[0].x;
float y = (float)debugData[0].edge[0].y;
float z = (float)debugData[0].edge[0].z;
sw << "{" << x << "," << y << "," << z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
rgb = { 255, 0, 0 };
size = 3;
for (int i = 0; i < (int)debugData.size(); i++)
{
for (int j = 0; j < debugData[i].edge_size; j++)
{
float x = (float)debugData[i].edge[j].x;
float y = (float)debugData[i].edge[j].y;
float z = (float)debugData[i].edge[j].z;
sw << "{" << x << "," << y << "," << z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
}
for (int j = 0; j < debugData[i].edgeLink1_size; j++)
{
float x = (float)debugData[i].edgeLink_1[j].x;
float y = (float)debugData[i].edgeLink_1[j].y;
float z = (float)debugData[i].edgeLink_1[j].z;
sw << "{" << x << "," << y << "," << z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
}
for (int j = 0; j < debugData[i].edgeLink2_size; j++)
{
float x = (float)debugData[i].edgeLink_2[j].x;
float y = (float)debugData[i].edgeLink_2[j].y;
float z = (float)debugData[i].edgeLink_2[j].z;
sw << "{" << x << "," << y << "," << z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
}
}
//加一个点，用于跳过显示工具bug
float x = (float)debugData[0].edge[0].x;
float y = (float)debugData[0].edge[0].y;
float z = (float)debugData[0].edge[0].z;
sw << "{" << x << "," << y << "," << z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
}
//显示拟合直线 //显示拟合直线
rgb = { 255, 0, 0 }; rgb = { 255, 0, 0 };
size = 3; size = 3;
@ -738,7 +740,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 = 6; grp <= 6; grp++) for (int grp = 5; grp <= 6; grp++)
{ {
SSG_planeCalibPara poseCalibPara; SSG_planeCalibPara poseCalibPara;
//初始化成单位阵 //初始化成单位阵
@ -760,7 +762,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 =1; //fidx =4;
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);
@ -788,6 +790,8 @@ int main()
{ {
if (2 == fidx) if (2 == fidx)
cuttingZ = 1760.0; cuttingZ = 1760.0;
else if(3 == fidx)
cuttingZ = 1780.0;
} }
sx_BQ_lineDataR(scanLines[i], poseCalibPara.planeCalib, cuttingZ); sx_BQ_lineDataR(scanLines[i], poseCalibPara.planeCalib, cuttingZ);
} }
@ -816,7 +820,7 @@ int main()
growParam.minLTypeTreeLen = 100; //mm growParam.minLTypeTreeLen = 100; //mm
growParam.minVTypeTreeLen = 100; //mm growParam.minVTypeTreeLen = 100; //mm
SSX_BQworkpiecePara workpieceParam; SSX_BQworkpiecePara workpieceParam;
workpieceParam.lineLen = 180.0; //直线段长度 workpieceParam.lineLen = 160.0; //直线段长度
int errCode = 0; int errCode = 0;
std::vector<SSX_debugInfo> debug_conturs; std::vector<SSX_debugInfo> debug_conturs;
SSX_BQworkpieceResult workpieceCorner = sx_BQ_getWorkpieceCorners( SSX_BQworkpieceResult workpieceCorner = sx_BQ_getWorkpieceCorners(
@ -834,11 +838,7 @@ int main()
printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1)); printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1));
//输出测试结果 //输出测试结果
sprintf_s(_scan_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx); sprintf_s(_scan_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx);
#if _OUTPUT_DEBUG_DATA
_outputRGBDScanLapWeld_RGBD(_scan_file, scanLines, workpieceCorner, false, debug_conturs); _outputRGBDScanLapWeld_RGBD(_scan_file, scanLines, workpieceCorner, false, debug_conturs);
#else
_outputRGBDScanLapWeld_RGBD(_scan_file, scanLines, workpieceCorner, true, NULL);
#endif
sprintf_s(calibFile, "%sresult\\LaserLine%d_corner_info.txt", dataPath[grp], fidx); sprintf_s(calibFile, "%sresult\\LaserLine%d_corner_info.txt", dataPath[grp], fidx);
_outputCornerInfo(calibFile, workpieceCorner); _outputCornerInfo(calibFile, workpieceCorner);
} }

194
sourceCode/BQ_workpieceCornerExtraction.cpp
View File

@ -7,7 +7,8 @@
//version 1.1.0 : base version release to customer, output corner coordinate //version 1.1.0 : base version release to customer, output corner coordinate
//version 1.2.0 : add position length output //version 1.2.0 : add position length output
std::string m_strVersion = "1.2.0"; //version 1.2.1 : fix bugs for ver1.2.0
std::string m_strVersion = "1.2.1";
const char* wd_BQWorkpieceCornerVersion(void) const char* wd_BQWorkpieceCornerVersion(void)
{ {
return m_strVersion.c_str(); return m_strVersion.c_str();
@ -207,7 +208,9 @@ int _counterLinePtNum(std::vector<SVzNL3DPosition>& lineData)
int ptNum = 0; int ptNum = 0;
for (int i = 0, i_max = (int)lineData.size(); i < i_max; i++) for (int i = 0, i_max = (int)lineData.size(); i < i_max; i++)
{ {
if (lineData[i].pt3D.z > 1e-4) if ( (abs(lineData[i].pt3D.z) > 1e-4) ||
(abs(lineData[i].pt3D.x > 1e-4)) ||
(abs(lineData[i].pt3D.y > 1e-4)))
ptNum++; ptNum++;
} }
return ptNum; return ptNum;
@ -217,6 +220,7 @@ int _counterLinePtNum(std::vector<SVzNL3DPosition>& lineData)
int _getBranchInfo( int _getBranchInfo(
int validStartLine, //开始扫描边界 int validStartLine, //开始扫描边界
int validEndLine, //结束扫描边界 int validEndLine, //结束扫描边界
bool partialScan, //当相机视野不能覆盖工件全部时，partialScan为true,
std::vector<SWD_polarPt>& polarPoints, std::vector<SWD_polarPt>& polarPoints,
SWD_polarPt branchCorner, SWD_polarPt branchCorner,
SWD_polarPeakInfo branchCornerInfo, SWD_polarPeakInfo branchCornerInfo,
@ -229,6 +233,10 @@ int _getBranchInfo(
int contourPtSize = (int)polarPoints.size(); int contourPtSize = (int)polarPoints.size();
std::vector<SWD_polarPt> branchContourPts; std::vector<SWD_polarPt> branchContourPts;
int validStartLineWin = validStartLine + 3;
int validEndLineWin = validEndLine - 3;
if (validEndLineWin < 0)
validEndLineWin = 0;
int cornerWin = 2; int cornerWin = 2;
int LineDir, contourDir, startIdx, endingIdx; int LineDir, contourDir, startIdx, endingIdx;
if (branchCornerInfo.cornerDir == 2)//逆时针 if (branchCornerInfo.cornerDir == 2)//逆时针
@ -275,7 +283,8 @@ int _getBranchInfo(
idx += contourPtSize; idx += contourPtSize;
else else
idx = idx % contourPtSize; idx = idx % contourPtSize;
if ((polarPoints[idx].lineIdx == validStartLine) || (polarPoints[idx].lineIdx == validEndLine)) if ((partialScan == true) &&
((polarPoints[idx].lineIdx == validStartLine) || (polarPoints[idx].lineIdx == validEndLine)))
{ {
toRefine = true; toRefine = true;
break; break;
@ -299,12 +308,31 @@ int _getBranchInfo(
maxHPos = i; maxHPos = i;
} }
} }
//迭代一次
int recEnding = maxHPos * 2;
if (recEnding >= LinePts.size())
recEnding = (int)LinePts.size() - 1;
pt1 = LinePts[0];
pt2 = LinePts[recEnding];
compute2ptLine(pt1, pt2, &aa, &bb, &cc);
//计算真正的角点
maxH = 0;
maxHPos = 0;
for (int i = 0; i < recEnding; i++)
{
double H = computePtDistToLine(LinePts[i].x, LinePts[i].y, aa, bb, cc);
if (maxH < H)
{
maxH = H;
maxHPos = i;
}
}
int cptIdx = startIdx + LineDir * maxHPos; int cptIdx = startIdx + LineDir * maxHPos;
if (cptIdx < 0) if (cptIdx < 0)
cptIdx += contourPtSize; cptIdx += contourPtSize;
else else
cptIdx = cptIdx % contourPtSize; cptIdx = cptIdx % contourPtSize;
branchCorner = polarPoints[cptIdx]; branchCorner = polarPoints[cptIdx];
if (branchCornerInfo.cornerDir == 2)//逆时针 if (branchCornerInfo.cornerDir == 2)//逆时针
{ {
@ -340,6 +368,7 @@ int _getBranchInfo(
//计算端点垂足 //计算端点垂足
SVzNL2DPointD footPt = sx_getFootPoint_abc(endingPt0.x, endingPt0.y, _a, _b, _c); SVzNL2DPointD footPt = sx_getFootPoint_abc(endingPt0.x, endingPt0.y, _a, _b, _c);
//提取其它轮廓点 //提取其它轮廓点
double minDist = -1;
for (int i = cornerWin; i < contourPtSize; i++) //避开角点周围 for (int i = cornerWin; i < contourPtSize; i++) //避开角点周围
{ {
int ptIdx = i * contourDir + branchCorner.cptIndex; int ptIdx = i * contourDir + branchCorner.cptIndex;
@ -350,7 +379,12 @@ int _getBranchInfo(
SWD_polarPt a_pt = polarPoints[ptIdx]; SWD_polarPt a_pt = polarPoints[ptIdx];
SVzNL2DPointD contourFoot = sx_getFootPoint_abc(a_pt.x, a_pt.y, _a, _b, _c); SVzNL2DPointD contourFoot = sx_getFootPoint_abc(a_pt.x, a_pt.y, _a, _b, _c);
double dist = sqrt(pow(footPt.x - contourFoot.x, 2) + pow(footPt.y - contourFoot.y, 2)); double dist = sqrt(pow(footPt.x - contourFoot.x, 2) + pow(footPt.y - contourFoot.y, 2));
if (dist < 1.0) if (minDist < 0)
minDist = dist;
else
minDist = minDist > dist ? dist : minDist;
if ((dist < 10) &&(dist > (minDist + 1.0)))
break; break;
branchContourPts.push_back(a_pt); branchContourPts.push_back(a_pt);
} }
@ -359,8 +393,9 @@ int _getBranchInfo(
bool hasSidePt = false; bool hasSidePt = false;
for (int m = 0; m < contourPtNum; m++) for (int m = 0; m < contourPtNum; m++)
{ {
if ((branchContourPts[m].lineIdx == validStartLine) || if ((partialScan == true) &&
(branchContourPts[m].lineIdx == validEndLine)) ((branchContourPts[m].lineIdx == validStartLine) ||
(branchContourPts[m].lineIdx == validEndLine)))
{ {
branchContourPts[m].z = -1.0; //label branchContourPts[m].z = -1.0; //label
hasSidePt = true; hasSidePt = true;
@ -426,9 +461,10 @@ int _getBranchInfo(
a_branchInfo.corner[1].z = a_branchInfo.corner[0].z; a_branchInfo.corner[1].z = a_branchInfo.corner[0].z;
// //
a_branchInfo.angle = branchCorner.angle; a_branchInfo.angle = branchCorner.angle;
a_branchInfo.line_a = _a;
a_branchInfo.line_a = _b; a_branchInfo.line_a = edge1_b;
a_branchInfo.line_a = _c; a_branchInfo.line_b = -edge1_a;
a_branchInfo.line_c = -(a_branchInfo.line_a * a_branchInfo.corner[1].x + a_branchInfo.line_b * a_branchInfo.corner[1].y);
*resultBranchInfo = a_branchInfo; *resultBranchInfo = a_branchInfo;
} }
else else
@ -438,7 +474,8 @@ int _getBranchInfo(
for (int i = 0; i < contourPtNum; i++) for (int i = 0; i < contourPtNum; i++)
{ {
SVzNL3DPoint a_pt = { branchContourPts[i].x, branchContourPts[i].y, branchContourPts[i].z }; SVzNL3DPoint a_pt = { branchContourPts[i].x, branchContourPts[i].y, branchContourPts[i].z };
if ((branchContourPts[i].lineIdx == validStartLine)|| (branchContourPts[i].lineIdx == validEndLine)) if ((partialScan == true) &&
((branchContourPts[i].lineIdx <= validStartLineWin)|| (branchContourPts[i].lineIdx >= validEndLineWin)))
{ {
break; break;
} }
@ -448,7 +485,8 @@ int _getBranchInfo(
for (int i = contourPtNum - 1; i >= 0; i--) for (int i = contourPtNum - 1; i >= 0; i--)
{ {
SVzNL3DPoint a_pt = { branchContourPts[i].x, branchContourPts[i].y, branchContourPts[i].z }; SVzNL3DPoint a_pt = { branchContourPts[i].x, branchContourPts[i].y, branchContourPts[i].z };
if ((branchContourPts[i].lineIdx == validStartLine) || (branchContourPts[i].lineIdx == validEndLine)) if ((partialScan == true) &&
((branchContourPts[i].lineIdx <= validStartLineWin) || (branchContourPts[i].lineIdx >= validEndLineWin)))
break; break;
edgePt2.insert(edgePt2.begin(), a_pt); edgePt2.insert(edgePt2.begin(), a_pt);
} }
@ -829,6 +867,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
//提取R极值点 //提取R极值点
double minR = -1, maxR = -1; //计算最小和最大的R，用以区分有没有分支。minR和maxR相差小时，为圆形或8角形，没有分支 double minR = -1, maxR = -1; //计算最小和最大的R，用以区分有没有分支。minR和maxR相差小时，为圆形或8角形，没有分支
int minRPos = -1;
std::vector<SWD_polarPt> polarRPeakPts; std::vector<SWD_polarPt> polarRPeakPts;
int winSize = contourPtSize / 36; //+-10度范围 int winSize = contourPtSize / 36; //+-10度范围
if (winSize < 5) if (winSize < 5)
@ -840,9 +879,11 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
{ {
minR = currR; minR = currR;
maxR = currR; maxR = currR;
minRPos = pi;
} }
else else
{ {
minRPos = minR > currR ? pi : minRPos;
minR = minR > currR ? currR : minR; minR = minR > currR ? currR : minR;
maxR = maxR < currR ? currR : maxR; maxR = maxR < currR ? currR : maxR;
} }
@ -867,6 +908,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
std::vector<SWD_polarPeakInfo> polarPeakInfo; std::vector<SWD_polarPeakInfo> polarPeakInfo;
int pkId = 0; int pkId = 0;
//过滤圆弧段的极值：由于重心偏移，圆弧段也会形成极值。根据极值两边L=直线段长度构成的张角判断 //过滤圆弧段的极值：由于重心偏移，圆弧段也会形成极值。根据极值两边L=直线段长度构成的张角判断
double arcAngleChkLen = 100; //检测圆弧张角的尺度
for (int i = 0, i_max = (int)polarRPeakPts.size(); i < i_max; i++) for (int i = 0, i_max = (int)polarRPeakPts.size(); i < i_max; i++)
{ {
int ptidx = polarRPeakPts[i].cptIndex; int ptidx = polarRPeakPts[i].cptIndex;
@ -875,13 +917,16 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
py = polarRPeakPts[i].y; py = polarRPeakPts[i].y;
int LL1 = -1; int LL1 = -1;
int halfLL1 = -1;
for (int j = ptidx - 1; j > -contourPtSize; j--) for (int j = ptidx - 1; j > -contourPtSize; j--)
{ {
int idx = (j + contourPtSize) % contourPtSize; //筒形结构 int idx = (j + contourPtSize) % contourPtSize; //筒形结构
double cx = polarPoints[idx].x; double cx = polarPoints[idx].x;
double cy = polarPoints[idx].y; double cy = polarPoints[idx].y;
double len = sqrt(pow(px - cx, 2) + pow(py - cy, 2)); double len = sqrt(pow(px - cx, 2) + pow(py - cy, 2));
if (len > workpieceParam.lineLen) if (len < arcAngleChkLen)
halfLL1 = idx;
if (len > (workpieceParam.lineLen))
{ {
LL1 = idx; LL1 = idx;
break; break;
@ -889,13 +934,16 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
} }
int LL2 = -1; int LL2 = -1;
int halfLL2 = -1;
for (int j = ptidx + 1; j < contourPtSize*2; j++) for (int j = ptidx + 1; j < contourPtSize*2; j++)
{ {
int idx = j % contourPtSize; //筒形结构 int idx = j % contourPtSize; //筒形结构
double cx = polarPoints[idx].x; double cx = polarPoints[idx].x;
double cy = polarPoints[idx].y; double cy = polarPoints[idx].y;
double len = sqrt(pow(px - cx, 2) + pow(py - cy, 2)); double len = sqrt(pow(px - cx, 2) + pow(py - cy, 2));
if (len > workpieceParam.lineLen) if (len < arcAngleChkLen)
halfLL2 = idx;
if (len > (workpieceParam.lineLen))
{ {
LL2 = idx; LL2 = idx;
break; break;
@ -904,31 +952,20 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
if ((LL1 >= 0) && (LL2 >= 0)) if ((LL1 >= 0) && (LL2 >= 0))
{ {
double len1 = sqrt(pow(px - polarPoints[LL1].x, 2) + pow(py - polarPoints[LL1].y, 2)); double len1 = sqrt(pow(px - polarPoints[halfLL1].x, 2) + pow(py - polarPoints[halfLL1].y, 2));
double len2 = sqrt(pow(px - polarPoints[LL2].x, 2) + pow(py - polarPoints[LL2].y, 2)); double len2 = sqrt(pow(px - polarPoints[halfLL2].x, 2) + pow(py - polarPoints[halfLL2].y, 2));
double len3 = sqrt(pow(polarPoints[LL1].x - polarPoints[LL2].x, 2) + double len3 = sqrt(pow(polarPoints[halfLL1].x - polarPoints[halfLL2].x, 2) +
pow(polarPoints[LL1].y - polarPoints[LL2].y, 2)); pow(polarPoints[halfLL1].y - polarPoints[halfLL2].y, 2));
double cosTheta = (len1 * len1 + len2 * len2 - len3 * len3) / (2 * len1 * len2); double cosTheta = (len1 * len1 + len2 * len2 - len3 * len3) / (2 * len1 * len2);
double theta = acos(cosTheta) * 180.0 / PI; double theta = acos(cosTheta) * 180.0 / PI;
if (theta < 150) if (theta < 150)
{ {
double L1_angle = polarPoints[ptidx].angle - polarPoints[LL1].angle;
if (L1_angle < 0)
L1_angle += 360;
double L2_angle = polarPoints[LL2].angle - polarPoints[ptidx].angle;
if (L2_angle < 0)
L2_angle += 360;
SWD_polarPeakInfo a_pkInfo; SWD_polarPeakInfo a_pkInfo;
a_pkInfo.cptIndex = ptidx; a_pkInfo.cptIndex = ptidx;
a_pkInfo.L1_ptIndex = LL1; a_pkInfo.L1_ptIndex = LL1;
a_pkInfo.L2_ptIndex = LL2; a_pkInfo.L2_ptIndex = LL2;
a_pkInfo.cornerAngle = theta; a_pkInfo.cornerAngle = theta;
if(L1_angle < L2_angle) a_pkInfo.cornerDir = 0;
a_pkInfo.cornerDir = 2; //逆时针
else
a_pkInfo.cornerDir = 1; //顺时针
polarRPeakPts[i].cptIndex = ptidx; polarRPeakPts[i].cptIndex = ptidx;
polarRPeakPts[i].pkId = pkId; polarRPeakPts[i].pkId = pkId;
pkId++; pkId++;
@ -939,6 +976,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
} }
int workpieceType = -1; int workpieceType = -1;
bool partialScan = false; //当相机视野不能覆盖工件全部时，partialScan为true
std::vector< SWD_branchInfo> branchInfo; std::vector< SWD_branchInfo> branchInfo;
if (true == hasBranch) if (true == hasBranch)
{ {
@ -949,10 +987,10 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
if (validPolarRPeakPts[m].cptIndex < 0) if (validPolarRPeakPts[m].cptIndex < 0)
continue; continue;
if (polarPeakInfo[m].cornerDir == 2) //逆时针。 //if (polarPeakInfo[m].cornerDir == 2) //逆时针。
{ {
int nxtIdx = (m + 1)%pkSize; int nxtIdx = (m + 1)%pkSize;
double angleDiff = validPolarRPeakPts[nxtIdx].angle - validPolarRPeakPts[m].angle; double angleDiff = computeAngleDiff(validPolarRPeakPts[nxtIdx].angle, validPolarRPeakPts[m].angle);
if (angleDiff < 0) if (angleDiff < 0)
angleDiff += 360; angleDiff += 360;
if (angleDiff < 90) //为同一branch，合并，只保留一个 if (angleDiff < 90) //为同一branch，合并，只保留一个
@ -985,11 +1023,74 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
} }
int branchNum = (int)branchPeaks.size(); int branchNum = (int)branchPeaks.size();
if (branchNum == 2) if (branchNum == 2)
{
workpieceType = 3; //节点3 workpieceType = 3; //节点3
else if (branchNum == 3)
workpieceType = 2; //节点2 SVzNL3DPoint pt1 = { polarPoints[minRPos].x, polarPoints[minRPos].y, 0 };
else if (branchNum == 4) SVzNL3DPoint pt2 = { center_x, center_y, 0 };
workpieceType = 1; //节点1 double aux_a, aux_b, aux_c;
compute2ptLine(pt1, pt2, &aux_a, &aux_b, &aux_c);
double counterAngle = polarPoints[minRPos].angle + 180;
if (counterAngle > 360)
counterAngle = counterAngle - 360;
SVzNL3DPoint a_cross = computeEdgeCross(counterAngle, polarPoints, aux_a, aux_b, aux_c);
double refine_cx = (pt1.x + a_cross.x) / 2;
double refine_cy = (pt1.y + a_cross.y) / 2;
//确定cornerDir
for (int m = 0; m < branchNum; m++)
{
for (int n = 0; n < branchPeaks[m].size(); n++)
{
int cptIdx = branchPeakInfo[m][n].cptIndex;
int LL1 = branchPeakInfo[m][n].L1_ptIndex;
int LL2 = branchPeakInfo[m][n].L2_ptIndex;
double angle = atan2(polarPoints[cptIdx].y - refine_cy, polarPoints[cptIdx].x - refine_cx);
angle = (angle / PI) * 180 + 180.0;
double LL1_angle = atan2(polarPoints[LL1].y - refine_cy, polarPoints[LL1].x - refine_cx);
LL1_angle = (LL1_angle / PI) * 180 + 180.0;
double LL2_angle = atan2(polarPoints[LL2].y - refine_cy, polarPoints[LL2].x - refine_cx);
LL2_angle = (LL2_angle / PI) * 180 + 180.0;
double L1_angle = computeAngleDiff(angle, LL1_angle);
double L2_angle = computeAngleDiff(LL2_angle, angle);
if (L1_angle < L2_angle)
branchPeakInfo[m][n].cornerDir = 2; //逆时针
else
branchPeakInfo[m][n].cornerDir = 1; //顺时针
}
}
}
else if ((branchNum == 3) || (branchNum == 4))
{
if (branchNum == 3)
{
partialScan = true;
workpieceType = 2; //节点2
}
else
workpieceType = 1; //节点1
//确定cornerDir
for (int m = 0; m < branchNum; m++)
{
for(int n = 0; n < branchPeaks[m].size(); n++)
{
int cptIdx = branchPeakInfo[m][n].cptIndex;
int LL1 = branchPeakInfo[m][n].L1_ptIndex;
int LL2 = branchPeakInfo[m][n].L2_ptIndex;
double L1_angle = polarPoints[cptIdx].angle - polarPoints[LL1].angle;
if (L1_angle < 0)
L1_angle += 360;
double L2_angle = polarPoints[LL2].angle - polarPoints[cptIdx].angle;
if (L2_angle < 0)
L2_angle += 360;
if (L1_angle < L2_angle)
branchPeakInfo[m][n].cornerDir = 2; //逆时针
else
branchPeakInfo[m][n].cornerDir = 1; //顺时针
}
}
}
else else
workpieceType = 0; workpieceType = 0;
@ -1021,6 +1122,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
int opOK = _getBranchInfo( int opOK = _getBranchInfo(
validStartLine, //开始扫描边界 validStartLine, //开始扫描边界
validEndLine, //结束扫描边界 validEndLine, //结束扫描边界
partialScan, //当相机视野不能覆盖工件全部时，partialScan为true,
polarPoints, polarPoints,
branchCorner, branchCorner,
branchCornerInfo, branchCornerInfo,
@ -1043,7 +1145,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
a_branchDebug.edge = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * a_branchDebug.edge_size); a_branchDebug.edge = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * a_branchDebug.edge_size);
a_branchDebug.edgeLink_1 = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * a_branchDebug.edgeLink1_size); a_branchDebug.edgeLink_1 = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * a_branchDebug.edgeLink1_size);
a_branchDebug.edgeLink_2 = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * a_branchDebug.edgeLink2_size); a_branchDebug.edgeLink_2 = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * a_branchDebug.edgeLink2_size);
#endif
for(int m = 0; m < a_branchDebug.edge_size; m ++) for(int m = 0; m < a_branchDebug.edge_size; m ++)
a_branchDebug.edge[m] = branchEdgePt1[m]; a_branchDebug.edge[m] = branchEdgePt1[m];
for (int m = 0; m < a_branchDebug.edgeLink1_size; m++) for (int m = 0; m < a_branchDebug.edgeLink1_size; m++)
@ -1051,6 +1153,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
for (int m = 0; m < a_branchDebug.edgeLink2_size; m++) for (int m = 0; m < a_branchDebug.edgeLink2_size; m++)
a_branchDebug.edgeLink_2[m] = branchEdgePt2[m]; a_branchDebug.edgeLink_2[m] = branchEdgePt2[m];
debug_contours.push_back(a_branchDebug); debug_contours.push_back(a_branchDebug);
#endif
} }
workpieceCorners.workpieceType = workpieceType; workpieceCorners.workpieceType = workpieceType;
if (workpieceType == 1) //4个branch if (workpieceType == 1) //4个branch
@ -1183,11 +1286,16 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
else else
{ {
workpieceType = 4; //节点4 workpieceType = 4; //节点4
partialScan = true;
//检查没有在边界的Corner //检查没有在边界的Corner
int polarPkNum = (int)validPolarRPeakPts.size(); int polarPkNum = (int)validPolarRPeakPts.size();
for (int pki = 0; pki < polarPkNum; pki++) for (int pki = 0; pki < polarPkNum; pki++)
{ {
//检查是否在扫描边界 //检查是否在扫描边界
int validStartLineWin = validStartLine + 3;
int validEndLineWin = validEndLine - 3;
if (validEndLineWin < 0)
validEndLineWin = 0;
bool isSide = false; bool isSide = false;
int cptIdx = validPolarRPeakPts[pki].cptIndex; int cptIdx = validPolarRPeakPts[pki].cptIndex;
SVzNL3DPoint cpt = { validPolarRPeakPts[pki].x, validPolarRPeakPts[pki].y, validPolarRPeakPts[pki].z }; SVzNL3DPoint cpt = { validPolarRPeakPts[pki].x, validPolarRPeakPts[pki].y, validPolarRPeakPts[pki].z };
@ -1201,8 +1309,9 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
break; break;
else else
{ {
if ((polarPoints[idx].lineIdx == validStartLine) || if ( (partialScan == true) &&
(polarPoints[idx].lineIdx == validEndLine)) ((polarPoints[idx].lineIdx <= validStartLineWin) ||
(polarPoints[idx].lineIdx >= validEndLineWin)))
isSide = true; isSide = true;
else else
edgePt_1.push_back(a_pt); edgePt_1.push_back(a_pt);
@ -1222,8 +1331,9 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
break; break;
else else
{ {
if ((polarPoints[idx].lineIdx == validStartLine) || if ((partialScan == true) &&
(polarPoints[idx].lineIdx == validEndLine)) ((polarPoints[idx].lineIdx <= validStartLineWin) ||
(polarPoints[idx].lineIdx >= validEndLineWin)))
isSide = true; isSide = true;
else else
edgePt_2.push_back(a_pt); edgePt_2.push_back(a_pt);
@ -1235,7 +1345,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
return workpieceCorners; return workpieceCorners;
} }
if (false == isSide) if (true == isSide)
{ {
//拟合计算交点，修正 //拟合计算交点，修正
double edge1_a = 0, edge1_b = 0, edge1_c = 0; double edge1_a = 0, edge1_b = 0, edge1_c = 0;

2
sourceCode/SG_baseAlgo_Export.h
View File

@ -369,7 +369,7 @@ SG_APISHARED_EXPORT void rotateLine45Deg(
double* r_a, double* r_b, double* r_c); double* r_a, double* r_b, double* r_c);
//计算直线角度 //计算直线角度
SG_APISHARED_EXPORT double getLineAngle(double _a, double _b, double _c); SG_APISHARED_EXPORT double getLineAngle(const double _a, const double _b, const double _c);
//计算两点的2D距离 //计算两点的2D距离
SG_APISHARED_EXPORT double compute2DLen(SVzNL3DPoint pt1, SVzNL3DPoint pt2); SG_APISHARED_EXPORT double compute2DLen(SVzNL3DPoint pt1, SVzNL3DPoint pt2);

9
sourceCode/SG_baseFunc.cpp
View File

@ -341,15 +341,16 @@ void rotateLine45Deg(
return; return;
} }
double getLineAngle(double _a, double _b, double _c) double getLineAngle(const double _a, const double _b, const double _c)
{ {
if (_a = 0) if (_a == 0)
return 0; return 0;
else if (_b = 0) else if (_b == 0)
return 90; return 90;
else else
{ {
double theta = atan(-_a / _b) + PI / 2; double k = _a / _b;
double theta = atan(-k) + PI / 2;
theta = (theta * 180.0) / PI; theta = (theta * 180.0) / PI;
return theta; return theta;
} }