sx_BQ_getWorkpieceCorners version 1.2.1
- 修正了1.2.0版本问题
This commit is contained in:
parent
6c5adfc7d5
commit
335ee0a92f
@ -530,7 +530,7 @@ void _outputRGBDScanLapWeld_RGBD(
|
||||
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;
|
||||
SWD_3DPointPair a_line;
|
||||
@ -567,6 +567,8 @@ void _outputRGBDScanLapWeld_RGBD(
|
||||
lines.push_back(a_line);
|
||||
}
|
||||
|
||||
if (debugData.size() > 0)
|
||||
{
|
||||
int linePtNum = 0;
|
||||
for (int i = 0; i < (int)debugData.size(); i++)
|
||||
linePtNum += debugData[i].edge_size + debugData[i].edgeLink1_size + debugData[i].edgeLink2_size;
|
||||
@ -612,7 +614,7 @@ void _outputRGBDScanLapWeld_RGBD(
|
||||
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;
|
||||
@ -738,7 +740,7 @@ int main()
|
||||
const char* ver = wd_BQWorkpieceCornerVersion();
|
||||
printf("ver:%s\n", ver);
|
||||
|
||||
for (int grp = 6; grp <= 6; grp++)
|
||||
for (int grp = 5; grp <= 6; grp++)
|
||||
{
|
||||
SSG_planeCalibPara poseCalibPara;
|
||||
//初始化成单位阵
|
||||
@ -760,7 +762,7 @@ int main()
|
||||
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
//fidx =1;
|
||||
//fidx =4;
|
||||
char _scan_file[256];
|
||||
if(0 == grp)
|
||||
sprintf_s(_scan_file, "%sscanData_%d_grid.txt", dataPath[grp], fidx);
|
||||
@ -788,6 +790,8 @@ int main()
|
||||
{
|
||||
if (2 == fidx)
|
||||
cuttingZ = 1760.0;
|
||||
else if(3 == fidx)
|
||||
cuttingZ = 1780.0;
|
||||
}
|
||||
sx_BQ_lineDataR(scanLines[i], poseCalibPara.planeCalib, cuttingZ);
|
||||
}
|
||||
@ -816,7 +820,7 @@ int main()
|
||||
growParam.minLTypeTreeLen = 100; //mm
|
||||
growParam.minVTypeTreeLen = 100; //mm
|
||||
SSX_BQworkpiecePara workpieceParam;
|
||||
workpieceParam.lineLen = 180.0; //直线段长度
|
||||
workpieceParam.lineLen = 160.0; //直线段长度
|
||||
int errCode = 0;
|
||||
std::vector<SSX_debugInfo> debug_conturs;
|
||||
SSX_BQworkpieceResult workpieceCorner = sx_BQ_getWorkpieceCorners(
|
||||
@ -834,11 +838,7 @@ int main()
|
||||
printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1));
|
||||
//输出测试结果
|
||||
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);
|
||||
#else
|
||||
_outputRGBDScanLapWeld_RGBD(_scan_file, scanLines, workpieceCorner, true, NULL);
|
||||
#endif
|
||||
sprintf_s(calibFile, "%sresult\\LaserLine%d_corner_info.txt", dataPath[grp], fidx);
|
||||
_outputCornerInfo(calibFile, workpieceCorner);
|
||||
}
|
||||
|
||||
@ -7,7 +7,8 @@
|
||||
|
||||
//version 1.1.0 : base version release to customer, output corner coordinate
|
||||
//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)
|
||||
{
|
||||
return m_strVersion.c_str();
|
||||
@ -207,7 +208,9 @@ int _counterLinePtNum(std::vector<SVzNL3DPosition>& lineData)
|
||||
int ptNum = 0;
|
||||
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++;
|
||||
}
|
||||
return ptNum;
|
||||
@ -217,6 +220,7 @@ int _counterLinePtNum(std::vector<SVzNL3DPosition>& lineData)
|
||||
int _getBranchInfo(
|
||||
int validStartLine, //开始扫描边界
|
||||
int validEndLine, //结束扫描边界
|
||||
bool partialScan, //当相机视野不能覆盖工件全部时，partialScan为true,
|
||||
std::vector<SWD_polarPt>& polarPoints,
|
||||
SWD_polarPt branchCorner,
|
||||
SWD_polarPeakInfo branchCornerInfo,
|
||||
@ -229,6 +233,10 @@ int _getBranchInfo(
|
||||
int contourPtSize = (int)polarPoints.size();
|
||||
std::vector<SWD_polarPt> branchContourPts;
|
||||
|
||||
int validStartLineWin = validStartLine + 3;
|
||||
int validEndLineWin = validEndLine - 3;
|
||||
if (validEndLineWin < 0)
|
||||
validEndLineWin = 0;
|
||||
int cornerWin = 2;
|
||||
int LineDir, contourDir, startIdx, endingIdx;
|
||||
if (branchCornerInfo.cornerDir == 2)//逆时针
|
||||
@ -275,7 +283,8 @@ int _getBranchInfo(
|
||||
idx += contourPtSize;
|
||||
else
|
||||
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;
|
||||
break;
|
||||
@ -299,12 +308,31 @@ int _getBranchInfo(
|
||||
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;
|
||||
if (cptIdx < 0)
|
||||
cptIdx += contourPtSize;
|
||||
else
|
||||
cptIdx = cptIdx % contourPtSize;
|
||||
|
||||
branchCorner = polarPoints[cptIdx];
|
||||
if (branchCornerInfo.cornerDir == 2)//逆时针
|
||||
{
|
||||
@ -340,6 +368,7 @@ int _getBranchInfo(
|
||||
//计算端点垂足
|
||||
SVzNL2DPointD footPt = sx_getFootPoint_abc(endingPt0.x, endingPt0.y, _a, _b, _c);
|
||||
//提取其它轮廓点
|
||||
double minDist = -1;
|
||||
for (int i = cornerWin; i < contourPtSize; i++) //避开角点周围
|
||||
{
|
||||
int ptIdx = i * contourDir + branchCorner.cptIndex;
|
||||
@ -350,7 +379,12 @@ int _getBranchInfo(
|
||||
SWD_polarPt a_pt = polarPoints[ptIdx];
|
||||
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));
|
||||
if (dist < 1.0)
|
||||
if (minDist < 0)
|
||||
minDist = dist;
|
||||
else
|
||||
minDist = minDist > dist ? dist : minDist;
|
||||
|
||||
if ((dist < 10) &&(dist > (minDist + 1.0)))
|
||||
break;
|
||||
branchContourPts.push_back(a_pt);
|
||||
}
|
||||
@ -359,8 +393,9 @@ int _getBranchInfo(
|
||||
bool hasSidePt = false;
|
||||
for (int m = 0; m < contourPtNum; m++)
|
||||
{
|
||||
if ((branchContourPts[m].lineIdx == validStartLine) ||
|
||||
(branchContourPts[m].lineIdx == validEndLine))
|
||||
if ((partialScan == true) &&
|
||||
((branchContourPts[m].lineIdx == validStartLine) ||
|
||||
(branchContourPts[m].lineIdx == validEndLine)))
|
||||
{
|
||||
branchContourPts[m].z = -1.0; //label
|
||||
hasSidePt = true;
|
||||
@ -426,9 +461,10 @@ int _getBranchInfo(
|
||||
a_branchInfo.corner[1].z = a_branchInfo.corner[0].z;
|
||||
//
|
||||
a_branchInfo.angle = branchCorner.angle;
|
||||
a_branchInfo.line_a = _a;
|
||||
a_branchInfo.line_a = _b;
|
||||
a_branchInfo.line_a = _c;
|
||||
|
||||
a_branchInfo.line_a = edge1_b;
|
||||
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;
|
||||
}
|
||||
else
|
||||
@ -438,7 +474,8 @@ int _getBranchInfo(
|
||||
for (int i = 0; i < contourPtNum; i++)
|
||||
{
|
||||
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;
|
||||
}
|
||||
@ -448,7 +485,8 @@ int _getBranchInfo(
|
||||
for (int i = contourPtNum - 1; i >= 0; i--)
|
||||
{
|
||||
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;
|
||||
edgePt2.insert(edgePt2.begin(), a_pt);
|
||||
}
|
||||
@ -829,6 +867,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
|
||||
//提取R极值点
|
||||
double minR = -1, maxR = -1; //计算最小和最大的R，用以区分有没有分支。minR和maxR相差小时，为圆形或8角形，没有分支
|
||||
int minRPos = -1;
|
||||
std::vector<SWD_polarPt> polarRPeakPts;
|
||||
int winSize = contourPtSize / 36; //+-10度范围
|
||||
if (winSize < 5)
|
||||
@ -840,9 +879,11 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
{
|
||||
minR = currR;
|
||||
maxR = currR;
|
||||
minRPos = pi;
|
||||
}
|
||||
else
|
||||
{
|
||||
minRPos = minR > currR ? pi : minRPos;
|
||||
minR = minR > currR ? currR : minR;
|
||||
maxR = maxR < currR ? currR : maxR;
|
||||
}
|
||||
@ -867,6 +908,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
std::vector<SWD_polarPeakInfo> polarPeakInfo;
|
||||
int pkId = 0;
|
||||
//过滤圆弧段的极值：由于重心偏移，圆弧段也会形成极值。根据极值两边L=直线段长度构成的张角判断
|
||||
double arcAngleChkLen = 100; //检测圆弧张角的尺度
|
||||
for (int i = 0, i_max = (int)polarRPeakPts.size(); i < i_max; i++)
|
||||
{
|
||||
int ptidx = polarRPeakPts[i].cptIndex;
|
||||
@ -875,13 +917,16 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
py = polarRPeakPts[i].y;
|
||||
|
||||
int LL1 = -1;
|
||||
int halfLL1 = -1;
|
||||
for (int j = ptidx - 1; j > -contourPtSize; j--)
|
||||
{
|
||||
int idx = (j + contourPtSize) % contourPtSize; //筒形结构
|
||||
double cx = polarPoints[idx].x;
|
||||
double cy = polarPoints[idx].y;
|
||||
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;
|
||||
break;
|
||||
@ -889,13 +934,16 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
}
|
||||
|
||||
int LL2 = -1;
|
||||
int halfLL2 = -1;
|
||||
for (int j = ptidx + 1; j < contourPtSize*2; j++)
|
||||
{
|
||||
int idx = j % contourPtSize; //筒形结构
|
||||
double cx = polarPoints[idx].x;
|
||||
double cy = polarPoints[idx].y;
|
||||
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;
|
||||
break;
|
||||
@ -904,31 +952,20 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
|
||||
if ((LL1 >= 0) && (LL2 >= 0))
|
||||
{
|
||||
double len1 = sqrt(pow(px - polarPoints[LL1].x, 2) + pow(py - polarPoints[LL1].y, 2));
|
||||
double len2 = sqrt(pow(px - polarPoints[LL2].x, 2) + pow(py - polarPoints[LL2].y, 2));
|
||||
double len3 = sqrt(pow(polarPoints[LL1].x - polarPoints[LL2].x, 2) +
|
||||
pow(polarPoints[LL1].y - polarPoints[LL2].y, 2));
|
||||
double len1 = sqrt(pow(px - polarPoints[halfLL1].x, 2) + pow(py - polarPoints[halfLL1].y, 2));
|
||||
double len2 = sqrt(pow(px - polarPoints[halfLL2].x, 2) + pow(py - polarPoints[halfLL2].y, 2));
|
||||
double len3 = sqrt(pow(polarPoints[halfLL1].x - polarPoints[halfLL2].x, 2) +
|
||||
pow(polarPoints[halfLL1].y - polarPoints[halfLL2].y, 2));
|
||||
double cosTheta = (len1 * len1 + len2 * len2 - len3 * len3) / (2 * len1 * len2);
|
||||
double theta = acos(cosTheta) * 180.0 / PI;
|
||||
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;
|
||||
a_pkInfo.cptIndex = ptidx;
|
||||
a_pkInfo.L1_ptIndex = LL1;
|
||||
a_pkInfo.L2_ptIndex = LL2;
|
||||
a_pkInfo.cornerAngle = theta;
|
||||
if(L1_angle < L2_angle)
|
||||
a_pkInfo.cornerDir = 2; //ÄæʱÕë
|
||||
else
|
||||
a_pkInfo.cornerDir = 1; //˳ʱÕë
|
||||
|
||||
a_pkInfo.cornerDir = 0;
|
||||
polarRPeakPts[i].cptIndex = ptidx;
|
||||
polarRPeakPts[i].pkId = pkId;
|
||||
pkId++;
|
||||
@ -939,6 +976,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
}
|
||||
|
||||
int workpieceType = -1;
|
||||
bool partialScan = false; //当相机视野不能覆盖工件全部时，partialScan为true
|
||||
std::vector< SWD_branchInfo> branchInfo;
|
||||
if (true == hasBranch)
|
||||
{
|
||||
@ -949,10 +987,10 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
if (validPolarRPeakPts[m].cptIndex < 0)
|
||||
continue;
|
||||
|
||||
if (polarPeakInfo[m].cornerDir == 2) //ÄæʱÕë¡£
|
||||
//if (polarPeakInfo[m].cornerDir == 2) //逆时针。
|
||||
{
|
||||
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)
|
||||
angleDiff += 360;
|
||||
if (angleDiff < 90) //为同一branch，合并，只保留一个
|
||||
@ -985,11 +1023,74 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
}
|
||||
int branchNum = (int)branchPeaks.size();
|
||||
if (branchNum == 2)
|
||||
{
|
||||
workpieceType = 3; //节点3
|
||||
else if (branchNum == 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 if (branchNum == 4)
|
||||
}
|
||||
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
|
||||
workpieceType = 0;
|
||||
|
||||
@ -1021,6 +1122,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
int opOK = _getBranchInfo(
|
||||
validStartLine, //开始扫描边界
|
||||
validEndLine, //结束扫描边界
|
||||
partialScan, //当相机视野不能覆盖工件全部时，partialScan为true,
|
||||
polarPoints,
|
||||
branchCorner,
|
||||
branchCornerInfo,
|
||||
@ -1043,7 +1145,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
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);
|
||||
#endif
|
||||
|
||||
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++)
|
||||
@ -1051,6 +1153,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
for (int m = 0; m < a_branchDebug.edgeLink2_size; m++)
|
||||
a_branchDebug.edgeLink_2[m] = branchEdgePt2[m];
|
||||
debug_contours.push_back(a_branchDebug);
|
||||
#endif
|
||||
}
|
||||
workpieceCorners.workpieceType = workpieceType;
|
||||
if (workpieceType == 1) //4个branch
|
||||
@ -1183,11 +1286,16 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
else
|
||||
{
|
||||
workpieceType = 4; //节点4
|
||||
partialScan = true;
|
||||
//检查没有在边界的Corner
|
||||
int polarPkNum = (int)validPolarRPeakPts.size();
|
||||
for (int pki = 0; pki < polarPkNum; pki++)
|
||||
{
|
||||
//检查是否在扫描边界
|
||||
int validStartLineWin = validStartLine + 3;
|
||||
int validEndLineWin = validEndLine - 3;
|
||||
if (validEndLineWin < 0)
|
||||
validEndLineWin = 0;
|
||||
bool isSide = false;
|
||||
int cptIdx = validPolarRPeakPts[pki].cptIndex;
|
||||
SVzNL3DPoint cpt = { validPolarRPeakPts[pki].x, validPolarRPeakPts[pki].y, validPolarRPeakPts[pki].z };
|
||||
@ -1201,8 +1309,9 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
break;
|
||||
else
|
||||
{
|
||||
if ((polarPoints[idx].lineIdx == validStartLine) ||
|
||||
(polarPoints[idx].lineIdx == validEndLine))
|
||||
if ( (partialScan == true) &&
|
||||
((polarPoints[idx].lineIdx <= validStartLineWin) ||
|
||||
(polarPoints[idx].lineIdx >= validEndLineWin)))
|
||||
isSide = true;
|
||||
else
|
||||
edgePt_1.push_back(a_pt);
|
||||
@ -1222,8 +1331,9 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
break;
|
||||
else
|
||||
{
|
||||
if ((polarPoints[idx].lineIdx == validStartLine) ||
|
||||
(polarPoints[idx].lineIdx == validEndLine))
|
||||
if ((partialScan == true) &&
|
||||
((polarPoints[idx].lineIdx <= validStartLineWin) ||
|
||||
(polarPoints[idx].lineIdx >= validEndLineWin)))
|
||||
isSide = true;
|
||||
else
|
||||
edgePt_2.push_back(a_pt);
|
||||
@ -1235,7 +1345,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
return workpieceCorners;
|
||||
}
|
||||
|
||||
if (false == isSide)
|
||||
if (true == isSide)
|
||||
{
|
||||
//拟合计算交点，修正
|
||||
double edge1_a = 0, edge1_b = 0, edge1_c = 0;
|
||||
|
||||
@ -369,7 +369,7 @@ SG_APISHARED_EXPORT void rotateLine45Deg(
|
||||
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距离
|
||||
SG_APISHARED_EXPORT double compute2DLen(SVzNL3DPoint pt1, SVzNL3DPoint pt2);
|
||||
|
||||
@ -341,15 +341,16 @@ void rotateLine45Deg(
|
||||
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;
|
||||
else if (_b = 0)
|
||||
else if (_b == 0)
|
||||
return 90;
|
||||
else
|
||||
{
|
||||
double theta = atan(-_a / _b) + PI / 2;
|
||||
double k = _a / _b;
|
||||
double theta = atan(-k) + PI / 2;
|
||||
theta = (theta * 180.0) / PI;
|
||||
return theta;
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user