BQ_workpieceCornerExtraction ver 1.4.0
博清工件定位纠偏改进。算法上计算轮廓点的前向和后向角，进而计算拐角，识别直角结构，并补缺角。
This commit is contained in:
parent
1b0df92eaa
commit
098caff171
@ -42,7 +42,7 @@
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<PlatformToolset>v143</PlatformToolset>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
|
||||
|
||||
@ -42,7 +42,7 @@
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
|
||||
<ConfigurationType>DynamicLibrary</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<PlatformToolset>v143</PlatformToolset>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
|
||||
|
||||
@ -42,7 +42,7 @@
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
|
||||
<ConfigurationType>DynamicLibrary</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<PlatformToolset>v143</PlatformToolset>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
|
||||
|
||||
@ -773,23 +773,65 @@ void _searchPlusCornerPeaks(
|
||||
}
|
||||
}
|
||||
|
||||
//判断两个有序点之间是否是一条直线
|
||||
bool checkIsLine(
|
||||
SSG_dirCornerAngle& pt1, SSG_dirCornerAngle& pt2,
|
||||
std::vector<SWD_polarPt>& polarPoints,
|
||||
double angleTh, double cutLineDeviationTh)
|
||||
{
|
||||
int ringBuffSize = (int)polarPoints.size();
|
||||
//偏离度判断
|
||||
//计算两点连线
|
||||
double a, b, c;
|
||||
int idx1 = pt1.forward_pntIdx; //避开一个尺度
|
||||
int idx2 = pt2.backward_pntIdx; //避开一个尺度
|
||||
compute2ptLine_2(
|
||||
polarPoints[idx1].x, polarPoints[idx1].y,
|
||||
polarPoints[idx2].x, polarPoints[idx2].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 = idx2 - idx1;
|
||||
if (number < 0)
|
||||
number += ringBuffSize;
|
||||
for (int m = 0; m < number; m++)
|
||||
{
|
||||
int idx = (m + idx1) % ringBuffSize;
|
||||
double dist = abs(a * polarPoints[idx].x + b * polarPoints[idx].y + c);
|
||||
if (maxDist < dist)
|
||||
maxDist = dist;
|
||||
}
|
||||
if (maxDist < cutLineDeviationTh) //判断为直线
|
||||
return true;
|
||||
else
|
||||
return false;
|
||||
}
|
||||
|
||||
//根据轮廓的角点计算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)
|
||||
void _computeBranchesFromCornerPeaks(
|
||||
std::vector<SWD_polarPt>& polarPoints,
|
||||
std::vector< SSG_dirCornerAngle>& cornerPlusPeaks,
|
||||
std::vector<SWD_branchInfo>& branches,
|
||||
SSX_BQworkpiecePara& workpieceParam,
|
||||
#if _OUTPUT_DEBUG_DATA
|
||||
std::vector<SSX_debugInfo>& branchDebugData,
|
||||
#endif
|
||||
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)
|
||||
if (cornerPlusPeaks[i].pntIdx < 0)
|
||||
continue;
|
||||
|
||||
SSG_dirCornerAngle& corner_1 = cornerPlusPeaks[i];
|
||||
@ -803,38 +845,182 @@ void _computeBranchesFromCornerPeaks(std::vector<SWD_polarPt>& polarPoints, std:
|
||||
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;
|
||||
//判断两个有序点之间是否是一条直线
|
||||
bool isLine = checkIsLine( corner_1, corner_2, polarPoints, angleTh, cutLineDeviationTh);
|
||||
if (true == isLine) //判断为截角
|
||||
{
|
||||
//拟合计算交点
|
||||
//直线1
|
||||
std::vector<SVzNL3DPoint> linePts_1;
|
||||
int startIdx = corner_1.backward_pntIdx;
|
||||
SWD_polarPt startPt = polarPoints[startIdx];
|
||||
for (int m = 0; m < ringBuffSize; m++)
|
||||
{
|
||||
int idx = startIdx - m;
|
||||
if (idx < 0)
|
||||
idx += ringBuffSize;
|
||||
|
||||
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++;
|
||||
double dist = sqrt(pow(startPt.x - polarPoints[idx].x, 2) + pow(startPt.y - polarPoints[idx].y, 2));
|
||||
if (dist > workpieceParam.lineLen)
|
||||
break;
|
||||
SVzNL3DPoint a_pt = { polarPoints[idx].x , polarPoints[idx].y, polarPoints[idx].z };
|
||||
linePts_1.push_back(a_pt);
|
||||
}
|
||||
double _a1 = 0, _b1 = 0, _c1 = 0;
|
||||
lineFitting_abc(linePts_1, &_a1, &_b1, &_c1);
|
||||
//直线2
|
||||
std::vector<SVzNL3DPoint> linePts_2;
|
||||
startIdx = corner_2.forward_pntIdx;
|
||||
startPt = polarPoints[startIdx];
|
||||
for (int m = 0; m < ringBuffSize; m++)
|
||||
{
|
||||
int idx = (startIdx + m) % ringBuffSize;
|
||||
double dist = sqrt(pow(startPt.x - polarPoints[idx].x, 2) + pow(startPt.y - polarPoints[idx].y, 2));
|
||||
if (dist > workpieceParam.lineLen)
|
||||
break;
|
||||
SVzNL3DPoint a_pt = { polarPoints[idx].x , polarPoints[idx].y, polarPoints[idx].z };
|
||||
linePts_2.push_back(a_pt);
|
||||
}
|
||||
double _a2 = 0, _b2 = 0, _c2 = 0;
|
||||
lineFitting_abc(linePts_2, &_a2, &_b2, &_c2);
|
||||
//求交点
|
||||
SVzNL3DPoint crossPt = computeLineCrossPt_abs(_a1, _b1, _c1, _a2, _b2, _c2);
|
||||
linePts_1.insert(linePts_1.end(), linePts_2.begin(), linePts_2.end());
|
||||
crossPt.z = computeMeanZ(linePts_1);
|
||||
corner_1.point.x = crossPt.x;
|
||||
corner_1.point.y = crossPt.y;
|
||||
corner_1.point.z = crossPt.z;
|
||||
corner_2.pntIdx = -1;
|
||||
//更新corner
|
||||
corner_1.corner = sumCorner;
|
||||
corner_1.forward_pntIdx = corner_2.forward_pntIdx;
|
||||
}
|
||||
}
|
||||
}
|
||||
//迭代一次，对于循环Buff, 需要重新检查首尾结合部
|
||||
|
||||
|
||||
//滤除错误的Corner, 并删除
|
||||
for (int i = size_peaks-1; i >= 0; i--)
|
||||
{
|
||||
SSG_dirCornerAngle& a_corner = cornerPlusPeaks[i];
|
||||
if (a_corner.pntIdx < 0)
|
||||
{
|
||||
cornerPlusPeaks.erase(cornerPlusPeaks.begin() + i);
|
||||
}
|
||||
else
|
||||
{
|
||||
double angleDiff = abs(a_corner.corner - branchCornerAngle);
|
||||
if (angleDiff > angleTh)
|
||||
{
|
||||
cornerPlusPeaks.erase(cornerPlusPeaks.begin() + i);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//寻找branch: branch的特征：相邻之间为直线
|
||||
size_peaks = (int)cornerPlusPeaks.size();
|
||||
int branchID = 1;
|
||||
for (int i = 0; i < size_peaks; i++)
|
||||
{
|
||||
if (cornerPlusPeaks[i].pntIdx < 0)
|
||||
continue;
|
||||
|
||||
SSG_dirCornerAngle& corner_1 = cornerPlusPeaks[i];
|
||||
int tmpIdx = (i + 1) % size_peaks;
|
||||
SSG_dirCornerAngle& corner_2 = cornerPlusPeaks[tmpIdx];
|
||||
//判断两个有序点之间是否是一条直线
|
||||
bool isBranch = checkIsLine(corner_1, corner_2, polarPoints, angleTh, cutLineDeviationTh);
|
||||
if (true == isBranch) //判断为branch
|
||||
{
|
||||
//拟合三条直线
|
||||
// 直线0：两个角点之间的直线
|
||||
std::vector<SVzNL3DPoint> linePts_0;
|
||||
int ptSize = corner_2.backward_pntIdx - corner_1.forward_pntIdx;
|
||||
if (ptSize < 0)
|
||||
ptSize += ringBuffSize;
|
||||
for (int m = 0; m <= ptSize; m++)
|
||||
{
|
||||
int idx = (corner_1.forward_pntIdx + m) % ringBuffSize;
|
||||
SVzNL3DPoint a_pt = { polarPoints[idx].x , polarPoints[idx].y, polarPoints[idx].z };
|
||||
linePts_0.push_back(a_pt);
|
||||
}
|
||||
double _a0 = 0, _b0 = 0, _c0 = 0;
|
||||
lineFitting_abc(linePts_0, &_a0, &_b0, &_c0);
|
||||
//直线1
|
||||
std::vector<SVzNL3DPoint> linePts_1;
|
||||
int startIdx = corner_1.backward_pntIdx;
|
||||
SWD_polarPt startPt = polarPoints[startIdx];
|
||||
for (int m = 0; m < ringBuffSize; m++)
|
||||
{
|
||||
int idx = startIdx - m;
|
||||
if (idx < 0)
|
||||
idx += ringBuffSize;
|
||||
|
||||
double dist = sqrt(pow(startPt.x - polarPoints[idx].x, 2) + pow(startPt.y - polarPoints[idx].y, 2));
|
||||
if (dist > workpieceParam.lineLen)
|
||||
break;
|
||||
SVzNL3DPoint a_pt = { polarPoints[idx].x , polarPoints[idx].y, polarPoints[idx].z };
|
||||
linePts_1.push_back(a_pt);
|
||||
}
|
||||
double _a1 = 0, _b1 = 0, _c1 = 0;
|
||||
lineFitting_abc(linePts_1, &_a1, &_b1, &_c1);
|
||||
//直线2
|
||||
std::vector<SVzNL3DPoint> linePts_2;
|
||||
startIdx = corner_2.forward_pntIdx;
|
||||
startPt = polarPoints[startIdx];
|
||||
for (int m = 0; m < ringBuffSize; m++)
|
||||
{
|
||||
int idx = (startIdx + m) % ringBuffSize;
|
||||
double dist = sqrt(pow(startPt.x - polarPoints[idx].x, 2) + pow(startPt.y - polarPoints[idx].y, 2));
|
||||
if (dist > workpieceParam.lineLen)
|
||||
break;
|
||||
SVzNL3DPoint a_pt = { polarPoints[idx].x , polarPoints[idx].y, polarPoints[idx].z };
|
||||
linePts_2.push_back(a_pt);
|
||||
}
|
||||
double _a2 = 0, _b2 = 0, _c2 = 0;
|
||||
lineFitting_abc(linePts_2, &_a2, &_b2, &_c2);
|
||||
|
||||
#if _OUTPUT_DEBUG_DATA
|
||||
SSX_debugInfo a_branchDebug;
|
||||
a_branchDebug.rgnIdx = branchID++;
|
||||
a_branchDebug.edge_size = (int)linePts_0.size();
|
||||
a_branchDebug.edgeLink1_size = (int)linePts_1.size();
|
||||
a_branchDebug.edgeLink2_size = (int)linePts_2.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_2 = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * a_branchDebug.edgeLink2_size);
|
||||
|
||||
for (int m = 0; m < a_branchDebug.edge_size; m++)
|
||||
a_branchDebug.edge[m] = linePts_0[m];
|
||||
for (int m = 0; m < a_branchDebug.edgeLink1_size; m++)
|
||||
a_branchDebug.edgeLink_1[m] = linePts_1[m];
|
||||
for (int m = 0; m < a_branchDebug.edgeLink2_size; m++)
|
||||
a_branchDebug.edgeLink_2[m] = linePts_2[m];
|
||||
|
||||
branchDebugData.push_back(a_branchDebug);
|
||||
#endif
|
||||
|
||||
linePts_1.insert(linePts_1.end(), linePts_0.begin(), linePts_0.end());
|
||||
linePts_2.insert(linePts_2.end(), linePts_0.begin(), linePts_0.end());
|
||||
//计算branch信息
|
||||
SWD_branchInfo a_branchInfo;
|
||||
a_branchInfo.corner[0] = computeLineCrossPt_abs(_a0, _b0, _c0, _a1, _b1, _c1);
|
||||
a_branchInfo.corner[0].z = computeMeanZ(linePts_1);
|
||||
a_branchInfo.corner[2] = computeLineCrossPt_abs(_a0, _b0, _c0, _a2, _b2, _c2);
|
||||
a_branchInfo.corner[2].z = computeMeanZ(linePts_2);
|
||||
a_branchInfo.corner[1].x = (a_branchInfo.corner[0].x + a_branchInfo.corner[2].x) / 2;
|
||||
a_branchInfo.corner[1].y = (a_branchInfo.corner[0].y + a_branchInfo.corner[2].y) / 2;
|
||||
a_branchInfo.corner[1].z = computeMeanZ(linePts_0);
|
||||
//
|
||||
a_branchInfo.angle = corner_1.point.angle;
|
||||
a_branchInfo.line_a = _b0;
|
||||
a_branchInfo.line_b = -_a0;
|
||||
a_branchInfo.line_c = -(a_branchInfo.line_a * a_branchInfo.corner[1].x + a_branchInfo.line_b * a_branchInfo.corner[1].y);
|
||||
branches.push_back(a_branchInfo);
|
||||
|
||||
corner_1.pntIdx = -1;
|
||||
corner_2.pntIdx = -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
@ -1135,8 +1321,19 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
std::vector< SSG_dirCornerAngle> cornerPlusPeaks;
|
||||
_searchPlusCornerPeaks(dirCornerAngles, cornerPlusPeaks);
|
||||
//使用R过滤，判断是否缺角
|
||||
double branchCornerAngle = 90;
|
||||
double angleTh = 10;
|
||||
double cutLineDeviationTh = 5.0;
|
||||
std::vector<SWD_branchInfo> branches;
|
||||
_computeBranchesFromCornerPeaks(cornerPlusPeaks, branches);
|
||||
_computeBranchesFromCornerPeaks(
|
||||
polarPoints,
|
||||
cornerPlusPeaks,
|
||||
branches,
|
||||
workpieceParam,
|
||||
#if _OUTPUT_DEBUG_DATA
|
||||
debug_contours,
|
||||
#endif
|
||||
branchCornerAngle, angleTh, cutLineDeviationTh);
|
||||
|
||||
//提取R极值点
|
||||
double minR = -1, maxR = -1; //计算最小和最大的R，用以区分有没有分支。minR和maxR相差小时，为圆形或8角形，没有分支
|
||||
@ -1174,9 +1371,6 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
polarRPeakPts.push_back(polarPoints[pi]);
|
||||
}
|
||||
|
||||
double ratio_MaxMin = maxR / minR;
|
||||
bool hasBranch = ratio_MaxMin < 1.25 ? false : true;
|
||||
|
||||
std::vector<SWD_polarPt> validPolarRPeakPts;
|
||||
std::vector<SWD_polarPeakInfo> polarPeakInfo;
|
||||
int pkId = 0;
|
||||
@ -1251,192 +1445,19 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
int workpieceType = -1;
|
||||
bool partialScan = false; //当相机视野不能覆盖工件全部时，partialScan为true
|
||||
std::vector< SWD_branchInfo> branchInfo;
|
||||
if (true == hasBranch)
|
||||
if (branches.size() > 0) //有分支（branch)
|
||||
{
|
||||
//计算分支，90度范围内为同一分支。根据分支数量确定工件类型
|
||||
int pkSize = (int)validPolarRPeakPts.size();
|
||||
for (int m = 0; m < pkSize; m++)
|
||||
{
|
||||
if (validPolarRPeakPts[m].cptIndex < 0)
|
||||
continue;
|
||||
|
||||
//if (polarPeakInfo[m].cornerDir == 2) //逆时针。
|
||||
{
|
||||
int nxtIdx = (m + 1)%pkSize;
|
||||
|
||||
bool isSameBranch = checkSameBranch(validPolarRPeakPts[m], validPolarRPeakPts[nxtIdx],
|
||||
polarPoints, center_x, center_y);
|
||||
double angleDiff = computeAngleDiff(validPolarRPeakPts[nxtIdx].angle, validPolarRPeakPts[m].angle);
|
||||
if (angleDiff < 0)
|
||||
angleDiff += 360;
|
||||
if ( (angleDiff < 90) && (true == isSameBranch)) //为同一branch，合并，只保留一个
|
||||
{
|
||||
validPolarRPeakPts[nxtIdx].pkId = validPolarRPeakPts[m].pkId; //pair
|
||||
validPolarRPeakPts[nxtIdx].cptIndex = -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<std::vector<SWD_polarPt>> branchPeaks; //每个branch最多两个极值点
|
||||
branchPeaks.resize(pkId);
|
||||
std::vector<std::vector<SWD_polarPeakInfo>> branchPeakInfo;
|
||||
branchPeakInfo.resize(pkId);
|
||||
for (int m = 0; m < pkSize; m++)
|
||||
{
|
||||
if (validPolarRPeakPts[m].cptIndex < 0)
|
||||
validPolarRPeakPts[m].cptIndex = polarPeakInfo[m].cptIndex; //恢复
|
||||
int pkId = validPolarRPeakPts[m].pkId;
|
||||
branchPeaks[pkId].push_back(validPolarRPeakPts[m]);
|
||||
branchPeakInfo[pkId].push_back(polarPeakInfo[m]);
|
||||
}
|
||||
for (int m = pkSize - 1; m >= 0; m--)
|
||||
{
|
||||
if (branchPeaks[m].size() == 0)
|
||||
{
|
||||
branchPeaks.erase(branchPeaks.begin() + m);
|
||||
branchPeakInfo.erase(branchPeakInfo.begin() + m);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
int branchNum = (int)branchPeaks.size();
|
||||
int branchNum = (int)branches.size();
|
||||
if (branchNum == 2)
|
||||
{
|
||||
workpieceType = 3; //节点3
|
||||
|
||||
SVzNL3DPoint pt1 = { polarPoints[minRPos].x, polarPoints[minRPos].y, 0 };
|
||||
SVzNL3DPoint pt2 = { center_x, center_y, 0 };
|
||||
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 if (branchNum == 3)
|
||||
workpieceType = 2; //节点2
|
||||
else if (branchNum == 4)
|
||||
workpieceType = 1; //节点1
|
||||
else
|
||||
workpieceType = 0;
|
||||
|
||||
//计算各个branch的信息
|
||||
for (int branchIdx = 0; branchIdx < branchNum; branchIdx++)
|
||||
{
|
||||
std::vector<SWD_polarPt>& a_branch = branchPeaks[branchIdx];
|
||||
std::vector<SWD_polarPeakInfo>& a_branchInfo = branchPeakInfo[branchIdx];
|
||||
//取固定长度垂直边
|
||||
SWD_polarPt branchCorner;
|
||||
SWD_polarPeakInfo branchCornerInfo;
|
||||
if (a_branch.size() == 2) //取角度最接近90度的
|
||||
{
|
||||
double diff1 = abs(a_branchInfo[0].cornerAngle - 90);
|
||||
double diff2 = abs(a_branchInfo[1].cornerAngle - 90);
|
||||
branchCorner = diff1 < diff2 ? a_branch[0]: a_branch[1];
|
||||
branchCornerInfo = diff1 < diff2 ? a_branchInfo[0] : a_branchInfo[1];
|
||||
}
|
||||
else
|
||||
{
|
||||
branchCorner = a_branch[0];
|
||||
branchCornerInfo = a_branchInfo[0];
|
||||
}
|
||||
|
||||
SWD_branchInfo resultBranchInfo;
|
||||
std::vector<SVzNL3DPoint> branchLinePts; //用于显示和debug
|
||||
std::vector< SVzNL3DPoint> branchEdgePt1;
|
||||
std::vector< SVzNL3DPoint> branchEdgePt2;
|
||||
int opOK = _getBranchInfo(
|
||||
validStartLine, //开始扫描边界
|
||||
validEndLine, //结束扫描边界
|
||||
partialScan, //当相机视野不能覆盖工件全部时，partialScan为true,
|
||||
polarPoints,
|
||||
branchCorner,
|
||||
branchCornerInfo,
|
||||
&resultBranchInfo,
|
||||
branchLinePts,
|
||||
branchEdgePt1,
|
||||
branchEdgePt2);
|
||||
if(opOK < 0)
|
||||
{
|
||||
*errCode = SX_ERR_ZERO_CONTOUR_PT;
|
||||
return workpieceCorners;
|
||||
}
|
||||
branchInfo.push_back(resultBranchInfo);
|
||||
#if _OUTPUT_DEBUG_DATA
|
||||
SSX_debugInfo a_branchDebug;
|
||||
a_branchDebug.rgnIdx = (int)branchInfo.size();
|
||||
a_branchDebug.edge_size = (int)branchEdgePt1.size();
|
||||
a_branchDebug.edgeLink1_size = (int)branchLinePts.size();
|
||||
a_branchDebug.edgeLink2_size = (int)branchEdgePt2.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_2 = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * a_branchDebug.edgeLink2_size);
|
||||
|
||||
for(int m = 0; m < a_branchDebug.edge_size; m ++)
|
||||
a_branchDebug.edge[m] = branchEdgePt1[m];
|
||||
for (int m = 0; m < a_branchDebug.edgeLink1_size; m++)
|
||||
a_branchDebug.edgeLink_1[m] = branchLinePts[m];
|
||||
for (int m = 0; m < a_branchDebug.edgeLink2_size; m++)
|
||||
a_branchDebug.edgeLink_2[m] = branchEdgePt2[m];
|
||||
debug_contours.push_back(a_branchDebug);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
|
||||
branchInfo.insert(branchInfo.end(), branches.begin(), branches.end());
|
||||
|
||||
workpieceCorners.workpieceType = workpieceType;
|
||||
if (workpieceType == 1) //4个branch
|
||||
{
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user