1407 lines
45 KiB
C++
1407 lines
45 KiB
C++
#include <vector>
|
||
#include "SG_baseDataType.h"
|
||
#include "SG_baseAlgo_Export.h"
|
||
#include "BQ_workpieceCornerExtraction_Export.h"
|
||
#include <opencv2/opencv.hpp>
|
||
#include <limits>
|
||
|
||
//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";
|
||
const char* wd_BQWorkpieceCornerVersion(void)
|
||
{
|
||
return m_strVersion.c_str();
|
||
}
|
||
|
||
//计算一个平面调平参数。
|
||
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
|
||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||
SSG_planeCalibPara sx_BQ_getBaseCalibPara(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines)
|
||
{
|
||
return sg_getPlaneCalibPara2(scanLines);
|
||
}
|
||
|
||
//相机姿态调平，并去除地面
|
||
void sx_BQ_lineDataR(
|
||
std::vector< SVzNL3DPosition>& a_line,
|
||
const double* camPoseR,
|
||
double groundH)
|
||
{
|
||
lineDataRT_vector(a_line, camPoseR, groundH);
|
||
}
|
||
|
||
SVzNL3DPoint _translatePoint(SVzNL3DPoint point, double rMatrix[9])
|
||
{
|
||
SVzNL3DPoint result;
|
||
double x = point.x * rMatrix[0] + point.y * rMatrix[1] + point.z * rMatrix[2];
|
||
double y = point.x * rMatrix[3] + point.y * rMatrix[4] + point.z * rMatrix[5];
|
||
double z = point.x * rMatrix[6] + point.y * rMatrix[7] + point.z * rMatrix[8];
|
||
result.x = x;
|
||
result.y = y;
|
||
result.z = z;
|
||
return result;
|
||
}
|
||
|
||
//获取生长树的ROI
|
||
void sg_getTreeROI(SSG_featureTree* a_tree)
|
||
{
|
||
if (a_tree->treeNodes.size() == 0)
|
||
{
|
||
a_tree->roi.left = 0;
|
||
a_tree->roi.right = 0;
|
||
a_tree->roi.top = 0;
|
||
a_tree->roi.bottom = 0;
|
||
}
|
||
else
|
||
{
|
||
a_tree->roi.left = a_tree->treeNodes[0].jumpPos.x;
|
||
a_tree->roi.right = a_tree->treeNodes[0].jumpPos.x;
|
||
a_tree->roi.top = a_tree->treeNodes[0].jumpPos.y;
|
||
a_tree->roi.bottom = a_tree->treeNodes[0].jumpPos.y;
|
||
for (int i = 1, i_max = a_tree->treeNodes.size(); i < i_max; i++)
|
||
{
|
||
if (a_tree->roi.left > a_tree->treeNodes[i].jumpPos.x)
|
||
a_tree->roi.left = a_tree->treeNodes[i].jumpPos.x;
|
||
if (a_tree->roi.right < a_tree->treeNodes[i].jumpPos.x)
|
||
a_tree->roi.right = a_tree->treeNodes[i].jumpPos.x;
|
||
if (a_tree->roi.top > a_tree->treeNodes[i].jumpPos.y)
|
||
a_tree->roi.top = a_tree->treeNodes[i].jumpPos.y;
|
||
if (a_tree->roi.bottom < a_tree->treeNodes[i].jumpPos.y)
|
||
a_tree->roi.bottom = a_tree->treeNodes[i].jumpPos.y;
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
void _getEdgeContour(SSG_featureTree* a_tree, std::vector<SVzNL3DPoint>& contour, std::vector< std::vector<SVzNL3DPosition>>& scanLines, bool isVScan)
|
||
{
|
||
for (int j = 0, j_max = (int)a_tree->treeNodes.size(); j < j_max; j++)
|
||
{
|
||
SSG_basicFeature1D* a_feature = &a_tree->treeNodes[j];
|
||
SVzNL3DPoint a_pt;
|
||
if (true == isVScan)
|
||
a_pt = scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D;
|
||
else
|
||
a_pt = scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D;
|
||
if (a_pt.z > 1e-4)//虚假目标过滤后点会置0
|
||
{
|
||
contour.push_back(a_pt);
|
||
}
|
||
}
|
||
}
|
||
|
||
int _getPointClosestContour(std::vector<SSG_featureTree> trees, bool isVscanTrees, SVzNL3DPoint seedPt, std::vector< std::vector<SVzNL3DPosition>>& scanLines, bool fromHead)
|
||
{
|
||
double minDist = -1.0;
|
||
int idx = -1;
|
||
for (int i = 0, i_max = (int)trees.size(); i < i_max; i++)
|
||
{
|
||
SSG_basicFeature1D a_feature;
|
||
if (true == fromHead)
|
||
a_feature = trees[i].treeNodes[0];
|
||
else
|
||
a_feature = trees[i].treeNodes.back();
|
||
|
||
SVzNL3DPoint a_pt;
|
||
if (true == isVscanTrees)
|
||
a_pt = scanLines[a_feature.jumpPos2D.x][a_feature.jumpPos2D.y].pt3D;
|
||
else
|
||
a_pt = scanLines[a_feature.jumpPos2D.y][a_feature.jumpPos2D.x].pt3D;
|
||
|
||
double dist = sqrt(pow(a_pt.x - seedPt.x, 2) + pow(a_pt.y - seedPt.y, 2));
|
||
if (minDist < 0)
|
||
{
|
||
minDist = dist;
|
||
idx = i;
|
||
}
|
||
else
|
||
{
|
||
if(dist < minDist)
|
||
{
|
||
minDist = dist;
|
||
idx = i;
|
||
}
|
||
}
|
||
}
|
||
return idx;
|
||
}
|
||
|
||
void _getEdgeLinkingContour(SSG_featureTree* a_tree, bool isVScanTree, SVzNL3DPoint seedPt, std::vector<SVzNL3DPoint>& contour, std::vector< std::vector<SVzNL3DPosition>>& scanLines, bool fromHead, double lineLen)
|
||
{
|
||
for (int i = 0, i_max = (int)a_tree->treeNodes.size(); i < i_max; i++)
|
||
{
|
||
int idx = i;
|
||
if (false == fromHead)
|
||
idx = i_max - 1 - i;
|
||
SSG_basicFeature1D* a_feature = &a_tree->treeNodes[idx];
|
||
|
||
SVzNL3DPoint a_pt;
|
||
if (true == isVScanTree)
|
||
a_pt = scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D;
|
||
else
|
||
a_pt = scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D;
|
||
if (a_pt.z > 1e-4)//虚假目标过滤后点会置0
|
||
{
|
||
double dist = sqrt(pow(a_pt.x - seedPt.x, 2) + pow(a_pt.y - seedPt.y, 2));
|
||
if (dist > lineLen)
|
||
break;
|
||
contour.push_back(a_pt);
|
||
}
|
||
}
|
||
}
|
||
|
||
typedef struct
|
||
{
|
||
int rgnIdx;
|
||
std::vector<SVzNL3DPoint> edge;
|
||
SVzNL3DPoint edge_ends[2];
|
||
std::vector<SVzNL3DPoint> edgeLink_1;
|
||
SVzNL3DPoint edge_link1_ends[2];
|
||
std::vector<SVzNL3DPoint> edgeLink_2;
|
||
SVzNL3DPoint edge_link2_ends[2];
|
||
}SSX_featureContour;
|
||
|
||
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
|
||
{
|
||
int cptIndex;
|
||
int L1_ptIndex;
|
||
int L2_ptIndex;
|
||
double cornerAngle;
|
||
int cornerDir;
|
||
}SWD_polarPeakInfo;
|
||
|
||
typedef struct
|
||
{
|
||
double angle;
|
||
SVzNL3DPoint corner[3];
|
||
double line_a, line_b, line_c; //边的垂线方程 ax+by+c = 0
|
||
}SWD_branchInfo;
|
||
|
||
//逆时针旋转时 θ > 0 ；顺时针旋转时 θ < 0
|
||
cv::Point2f _rotate2D(cv::Point2f pt, double sinTheta, double cosTheta)
|
||
{
|
||
return (cv::Point2f((float)(pt.x * cosTheta - pt.y * sinTheta), (float)(pt.x * sinTheta + pt.y * cosTheta)));
|
||
}
|
||
|
||
bool compareByAngle(const SWD_polarPt& a, const SWD_polarPt& b) {
|
||
return a.angle < b.angle;
|
||
}
|
||
|
||
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)
|
||
ptNum++;
|
||
}
|
||
return ptNum;
|
||
}
|
||
|
||
//计算分支信息
|
||
int _getBranchInfo(
|
||
int validStartLine, //开始扫描边界
|
||
int validEndLine, //结束扫描边界
|
||
std::vector<SWD_polarPt>& polarPoints,
|
||
SWD_polarPt branchCorner,
|
||
SWD_polarPeakInfo branchCornerInfo,
|
||
SWD_branchInfo* resultBranchInfo,
|
||
std::vector<SVzNL3DPoint>& LinePts,
|
||
std::vector< SVzNL3DPoint>& edgePt1,
|
||
std::vector< SVzNL3DPoint>& edgePt2
|
||
)
|
||
{
|
||
int contourPtSize = (int)polarPoints.size();
|
||
std::vector<SWD_polarPt> branchContourPts;
|
||
|
||
int cornerWin = 2;
|
||
int LineDir, contourDir, startIdx, endingIdx;
|
||
if (branchCornerInfo.cornerDir == 2)//逆时针
|
||
{
|
||
startIdx = branchCorner.cptIndex - cornerWin; //此处将corner周围的点不计算在内
|
||
if (startIdx < 0)
|
||
startIdx += contourPtSize;
|
||
LineDir = -1;
|
||
contourDir = 1;
|
||
endingIdx = branchCornerInfo.L1_ptIndex;
|
||
}
|
||
else //顺时针
|
||
{
|
||
startIdx = (branchCorner.cptIndex + cornerWin) % contourPtSize;
|
||
LineDir = 1;
|
||
contourDir = -1;
|
||
endingIdx = branchCornerInfo.L2_ptIndex;
|
||
}
|
||
|
||
int ptIdx = startIdx;
|
||
while (1)
|
||
{
|
||
SVzNL3DPoint a_pt = { polarPoints[ptIdx].x, polarPoints[ptIdx].y, polarPoints[ptIdx].z };
|
||
LinePts.push_back(a_pt);
|
||
if (ptIdx == endingIdx)
|
||
break;
|
||
ptIdx += LineDir;
|
||
if (ptIdx < 0)
|
||
ptIdx += contourPtSize;
|
||
else
|
||
ptIdx = ptIdx % contourPtSize;
|
||
}
|
||
if (LinePts.size() < 5)
|
||
return -1;
|
||
|
||
//当直角由于扫描边界被切成一个上边很小的梯形时，检查出的角点可能不是真正的角点，需要进行检查
|
||
//迭代计算真正的角点：取corner两边5个点，若有边界，检查
|
||
int chkWin = 5;
|
||
bool toRefine = false;
|
||
for (int i = -chkWin; i <= chkWin; i++)
|
||
{
|
||
int idx = branchCorner.cptIndex + i;
|
||
if (idx < 0)
|
||
idx += contourPtSize;
|
||
else
|
||
idx = idx % contourPtSize;
|
||
if ((polarPoints[idx].lineIdx == validStartLine) || (polarPoints[idx].lineIdx == validEndLine))
|
||
{
|
||
toRefine = true;
|
||
break;
|
||
}
|
||
}
|
||
if (true == toRefine)
|
||
{
|
||
SVzNL3DPoint pt1 = LinePts[0];
|
||
SVzNL3DPoint pt2 = LinePts.back();
|
||
double aa, bb, cc;
|
||
compute2ptLine( pt1, pt2, &aa, &bb, &cc);
|
||
//计算真正的角点
|
||
double maxH = 0;
|
||
int maxHPos = 0;
|
||
for (int i = 0; i < (int)LinePts.size(); 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)//逆时针
|
||
{
|
||
startIdx = branchCorner.cptIndex - cornerWin; //此处将corner周围的点不计算在内
|
||
if (startIdx < 0)
|
||
startIdx += contourPtSize;
|
||
}
|
||
else //顺时针
|
||
startIdx = (branchCorner.cptIndex + cornerWin) % contourPtSize;
|
||
|
||
LinePts.clear();
|
||
ptIdx = startIdx;
|
||
while (1)
|
||
{
|
||
SVzNL3DPoint a_pt = { polarPoints[ptIdx].x, polarPoints[ptIdx].y, polarPoints[ptIdx].z };
|
||
LinePts.push_back(a_pt);
|
||
if (ptIdx == endingIdx)
|
||
break;
|
||
ptIdx += LineDir;
|
||
if (ptIdx < 0)
|
||
ptIdx += contourPtSize;
|
||
else
|
||
ptIdx = ptIdx % contourPtSize;
|
||
}
|
||
if (LinePts.size() < 5)
|
||
return -1;
|
||
}
|
||
|
||
//拟合直线: ax+by+c = 0
|
||
double _a = 0, _b = 0, _c = 0;
|
||
lineFitting_abc(LinePts, &_a, &_b, &_c);
|
||
SVzNL3DPoint endingPt0 = LinePts.back();
|
||
//计算端点垂足
|
||
SVzNL2DPointD footPt = sx_getFootPoint_abc(endingPt0.x, endingPt0.y, _a, _b, _c);
|
||
//提取其它轮廓点
|
||
for (int i = cornerWin; i < contourPtSize; i++) //避开角点周围
|
||
{
|
||
int ptIdx = i * contourDir + branchCorner.cptIndex;
|
||
if (ptIdx < 0)
|
||
ptIdx += contourPtSize;
|
||
else
|
||
ptIdx = ptIdx % contourPtSize;
|
||
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)
|
||
break;
|
||
branchContourPts.push_back(a_pt);
|
||
}
|
||
//检查轮廓点中有没有边界点。如果有边界点，需要进行推理
|
||
int contourPtNum = (int)branchContourPts.size();
|
||
bool hasSidePt = false;
|
||
for (int m = 0; m < contourPtNum; m++)
|
||
{
|
||
if ((branchContourPts[m].lineIdx == validStartLine) ||
|
||
(branchContourPts[m].lineIdx == validEndLine))
|
||
{
|
||
branchContourPts[m].z = -1.0; //label
|
||
hasSidePt = true;
|
||
}
|
||
}
|
||
//分开branch的另两段
|
||
if (false == hasSidePt)
|
||
{
|
||
double maxDist = -1;
|
||
int maxPos = -1;
|
||
//找到拐点:计算所有点与端点的距离，以最大值作为拐点
|
||
for (int m = 0; m < contourPtNum; m++)
|
||
{
|
||
double dist = sqrt(pow(branchContourPts[m].x - endingPt0.x, 2) +
|
||
pow(branchContourPts[m].y - endingPt0.y, 2));
|
||
if (maxDist < 0)
|
||
{
|
||
maxDist = dist;
|
||
maxPos = m;
|
||
}
|
||
else
|
||
{
|
||
if (maxDist < dist)
|
||
{
|
||
maxDist = dist;
|
||
maxPos = m;
|
||
}
|
||
}
|
||
}
|
||
//分隔两段
|
||
int start0 = 0;
|
||
int end0 = maxPos - cornerWin;
|
||
|
||
for (int m = start0; m <= end0; m++)
|
||
{
|
||
SVzNL3DPoint a_pt = { branchContourPts[m].x, branchContourPts[m].y, branchContourPts[m].z };
|
||
edgePt1.push_back(a_pt);
|
||
}
|
||
|
||
int start1 = maxPos + cornerWin;
|
||
int end1 = contourPtNum - 1;
|
||
|
||
for (int m = start1; m <= end1; m++)
|
||
{
|
||
SVzNL3DPoint a_pt = { branchContourPts[m].x, branchContourPts[m].y, branchContourPts[m].z };
|
||
edgePt2.push_back(a_pt);
|
||
}
|
||
|
||
//拟合求交点
|
||
double edge1_a, edge1_b, edge1_c;
|
||
lineFitting_abc(edgePt1, &edge1_a, &edge1_b, &edge1_c);
|
||
double edge2_a, edge2_b, edge2_c;
|
||
lineFitting_abc(edgePt2, &edge2_a, &edge2_b, &edge2_c);
|
||
|
||
//计算交点
|
||
SWD_branchInfo a_branchInfo;
|
||
a_branchInfo.corner[0] = computeLineCrossPt_abs(_a, _b, _c, edge1_a, edge1_b, edge1_c);
|
||
a_branchInfo.corner[0].z = computeMeanZ(edgePt1);
|
||
a_branchInfo.corner[2] = computeLineCrossPt_abs(edge1_a, edge1_b, edge1_c, edge2_a, edge2_b, edge2_c);
|
||
a_branchInfo.corner[2].z = a_branchInfo.corner[0].z;
|
||
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 = 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;
|
||
*resultBranchInfo = a_branchInfo;
|
||
}
|
||
else
|
||
{
|
||
//将边界去除，剩下的两段进行处理
|
||
std::vector< SVzNL3DPoint> edgePt1;
|
||
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))
|
||
{
|
||
break;
|
||
}
|
||
edgePt1.push_back(a_pt);
|
||
}
|
||
std::vector< SVzNL3DPoint> edgePt2;
|
||
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))
|
||
break;
|
||
edgePt2.insert(edgePt2.begin(), a_pt);
|
||
}
|
||
//计算第一段在直线上的垂足，取垂足的质心。过质心的垂线
|
||
int edge1PtNum = (int)edgePt1.size();
|
||
int edge2PtNum = (int)edgePt2.size();
|
||
if ((edge1PtNum == 0) || (edge2PtNum == 0))
|
||
{
|
||
return -1;
|
||
}
|
||
SVzNL2DPointD foot = { 0.0,0.0 };
|
||
for (int i = 0; i < edge1PtNum; i++)
|
||
{
|
||
SVzNL2DPointD a_foot = sx_getFootPoint_abc(edgePt1[i].x, edgePt1[i].y, _a, _b, _c);
|
||
foot.x += a_foot.x;
|
||
foot.y += a_foot.y;
|
||
}
|
||
foot.x = foot.x / edge1PtNum;
|
||
foot.y = foot.y / edge1PtNum;
|
||
double edge1_a, edge1_b, edge1_c;
|
||
edge1_a = _b;
|
||
edge1_b = -_a;
|
||
edge1_c = _a * foot.y - _b * foot.x;
|
||
//计算第二段在直线上的垂足，取垂足的质心。过质心的垂线
|
||
foot.x = 0;
|
||
foot.y = 0;
|
||
for (int i = 0; i < edge2PtNum; i++)
|
||
{
|
||
SVzNL2DPointD a_foot = sx_getFootPoint_abc(edgePt2[i].x, edgePt2[i].y, edge1_a, edge1_b, edge1_c);
|
||
foot.x += a_foot.x;
|
||
foot.y += a_foot.y;
|
||
}
|
||
foot.x = foot.x / edge2PtNum;
|
||
foot.y = foot.y / edge2PtNum;
|
||
double edge2_a, edge2_b, edge2_c;
|
||
edge2_a = _a;
|
||
edge2_b = _b;
|
||
edge2_c = -_a * foot.x - _b * foot.y;
|
||
//计算交点
|
||
SWD_branchInfo a_branchInfo;
|
||
a_branchInfo.corner[0] = computeLineCrossPt_abs(_a, _b, _c, edge1_a, edge1_b, edge1_c);
|
||
a_branchInfo.corner[0].z = computeMeanZ(edgePt1);
|
||
a_branchInfo.corner[2] = computeLineCrossPt_abs(edge1_a, edge1_b, edge1_c, edge2_a, edge2_b, edge2_c);
|
||
a_branchInfo.corner[2].z = a_branchInfo.corner[0].z;
|
||
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 = a_branchInfo.corner[0].z;
|
||
//
|
||
a_branchInfo.angle = branchCorner.angle;
|
||
a_branchInfo.line_a = _a;
|
||
a_branchInfo.line_b = _b;
|
||
a_branchInfo.line_c = -(_a * a_branchInfo.corner[1].x + _b * a_branchInfo.corner[1].y);
|
||
*resultBranchInfo = a_branchInfo;
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
|
||
SVzNL3DPoint computeEdgeCross(double angle, std::vector<SWD_polarPt>& polarPoints, double a, double b, double c)
|
||
{
|
||
double minDist = -1;
|
||
int angleIdx = -1;
|
||
int ptSize = (int)polarPoints.size();
|
||
for (int i = 0; i < ptSize; i++)
|
||
{
|
||
double diff = computeAngleDiff(angle, polarPoints[i].angle);
|
||
if (diff < 30) //搜索60度范围
|
||
{
|
||
double H = computePtDistToLine(polarPoints[i].x, polarPoints[i].y, a, b, c);
|
||
if (minDist < 0)
|
||
{
|
||
minDist = H;
|
||
angleIdx = i;
|
||
}
|
||
else
|
||
{
|
||
if (minDist > H)
|
||
{
|
||
minDist = H;
|
||
angleIdx = i;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
SVzNL3DPoint a_pt = { polarPoints[angleIdx].x, polarPoints[angleIdx].y, polarPoints[angleIdx].z };
|
||
return a_pt;
|
||
}
|
||
|
||
SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
const SSG_cornerParam cornerPara,
|
||
const SSG_outlierFilterParam filterParam,
|
||
SSG_treeGrowParam growParam,
|
||
SSG_planeCalibPara groundCalibPara,
|
||
SSX_BQworkpiecePara workpieceParam,
|
||
#if _OUTPUT_DEBUG_DATA
|
||
std::vector<SSX_debugInfo>& debug_contours,
|
||
#endif
|
||
int* errCode)
|
||
{
|
||
*errCode = 0;
|
||
SSX_BQworkpieceResult workpieceCorners;
|
||
memset(&workpieceCorners, 0, sizeof(SSX_BQworkpieceResult));
|
||
int lineNum = (int)scanLines.size();
|
||
if (lineNum == 0)
|
||
{
|
||
*errCode = SG_ERR_3D_DATA_NULL;
|
||
return workpieceCorners;
|
||
}
|
||
|
||
//将开始和结束的空白扫描线去除，获得扫描边界
|
||
int validStartLine = -1;
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
int linePtNum = _counterLinePtNum(scanLines[i]);
|
||
if (linePtNum > 0)
|
||
{
|
||
validStartLine = i;
|
||
break;
|
||
}
|
||
}
|
||
int validEndLine = -1;
|
||
for (int i = lineNum - 1; i >= 0; i--)
|
||
{
|
||
int linePtNum = _counterLinePtNum(scanLines[i]);
|
||
if (linePtNum > 0)
|
||
{
|
||
validEndLine = i;
|
||
break;
|
||
}
|
||
}
|
||
if ( (validStartLine < 0) || (validEndLine < 0))
|
||
{
|
||
*errCode = SG_ERR_3D_DATA_NULL;
|
||
return workpieceCorners;
|
||
}
|
||
|
||
int linePtNum = (int)scanLines[0].size();
|
||
bool isGridData = true;
|
||
|
||
//自适应各种旋转角度
|
||
{
|
||
//垂直跳变特征提取
|
||
std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_v_raw;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
if (line == 250)
|
||
int kkk = 1;
|
||
|
||
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
|
||
if (linePtNum != (int)lineData.size())
|
||
isGridData = false;
|
||
|
||
//滤波，滤除异常点
|
||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
|
||
|
||
std::vector<SSG_basicFeature1D> line_features;
|
||
int dataSize = (int)lineData.size();
|
||
sg_getLineCornerFeature_BQ(
|
||
&lineData[0],
|
||
dataSize,
|
||
line,
|
||
groundCalibPara.planeHeight,
|
||
cornerPara, //scale通常取bagH的1/4
|
||
line_features);
|
||
jumpFeatures_v_raw.push_back(line_features);
|
||
}
|
||
|
||
if (false == isGridData)//数据不是网格格式
|
||
{
|
||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||
return workpieceCorners;
|
||
}
|
||
|
||
//生成水平扫描
|
||
std::vector<std::vector<SVzNL3DPosition>> hLines_raw;
|
||
hLines_raw.resize(linePtNum);
|
||
for (int i = 0; i < linePtNum; i++)
|
||
hLines_raw[i].resize(lineNum);
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||
hLines_raw[j][line] = scanLines[line][j];
|
||
hLines_raw[j][line].pt3D.x = scanLines[line][j].pt3D.y;
|
||
hLines_raw[j][line].pt3D.y = scanLines[line][j].pt3D.x;
|
||
}
|
||
}
|
||
//水平arc特征提取
|
||
std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_h_raw;
|
||
int lineNum_h_raw = (int)hLines_raw.size();
|
||
for (int line = 0; line < lineNum_h_raw; line++)
|
||
{
|
||
if (line == 416)
|
||
int kkk = 1;
|
||
std::vector<SVzNL3DPosition>& lineData = hLines_raw[line];
|
||
//滤波，滤除异常点
|
||
int ptNum = (int)lineData.size();
|
||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, filterParam);
|
||
|
||
std::vector<SSG_basicFeature1D> line_features;
|
||
int dataSize = (int)lineData.size();
|
||
sg_getLineCornerFeature_BQ(
|
||
&hLines_raw[line][0],
|
||
dataSize,
|
||
line,
|
||
groundCalibPara.planeHeight,
|
||
cornerPara, //scale通常取bagH的1/4
|
||
line_features);
|
||
jumpFeatures_h_raw.push_back(line_features);
|
||
}
|
||
|
||
//特征生长，用于滤除噪点
|
||
//垂直方向特征生长（激光线方向）
|
||
std::vector<SSG_featureTree> v_trees;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
bool isLastLine = false;
|
||
if (line == lineNum - 1)
|
||
isLastLine = true;
|
||
std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_v_raw[line];
|
||
if (a_lineJumpFeature.size() > 0)
|
||
int kkk = 1;
|
||
if (line == 202)
|
||
int kkk = 1;
|
||
sg_lineFeaturesGrowing(
|
||
line,
|
||
isLastLine,
|
||
a_lineJumpFeature,
|
||
v_trees,
|
||
growParam);
|
||
}
|
||
|
||
//水平方向特征生长（扫描运动方向）
|
||
std::vector<SSG_featureTree> h_trees;
|
||
for (int line = 0; line < lineNum_h_raw; line++)
|
||
{
|
||
if (line == 650)
|
||
int kkk = 1;
|
||
bool isLastLine = false;
|
||
if (line == lineNum_h_raw - 1)
|
||
isLastLine = true;
|
||
std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_h_raw[line];
|
||
sg_lineFeaturesGrowing(
|
||
line,
|
||
isLastLine,
|
||
a_lineJumpFeature,
|
||
h_trees,
|
||
growParam);
|
||
}
|
||
|
||
std::vector<SWD_polarPt> polarPoints;
|
||
#if 0
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_v_raw[line];
|
||
for (int pi = 0, pi_max = (int)a_lineJumpFeature.size(); pi < pi_max; pi++)
|
||
{
|
||
int lineIdx = a_lineJumpFeature[pi].jumpPos2D.x;
|
||
int ptIdx = a_lineJumpFeature[pi].jumpPos2D.y;
|
||
if (scanLines[lineIdx][ptIdx].nPointIdx >= 0)
|
||
{
|
||
SWD_polarPt a_polarPt;
|
||
a_polarPt.lineIdx = lineIdx;
|
||
a_polarPt.ptIdx = ptIdx;
|
||
a_polarPt.R = 0;
|
||
a_polarPt.angle = 0;
|
||
a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x;
|
||
a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y;
|
||
a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z;
|
||
polarPoints.push_back(a_polarPt);
|
||
scanLines[lineIdx][ptIdx].nPointIdx = -1;
|
||
}
|
||
}
|
||
}
|
||
for (int line = 0; line < lineNum_h_raw; line++)
|
||
{
|
||
std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_h_raw[line];
|
||
for (int pi = 0, pi_max = (int)a_lineJumpFeature.size(); pi < pi_max; pi++)
|
||
{
|
||
int lineIdx = a_lineJumpFeature[pi].jumpPos2D.y;
|
||
int ptIdx = a_lineJumpFeature[pi].jumpPos2D.x;
|
||
if (scanLines[lineIdx][ptIdx].nPointIdx >= 0)
|
||
{
|
||
SWD_polarPt a_polarPt;
|
||
a_polarPt.lineIdx = lineIdx;
|
||
a_polarPt.ptIdx = ptIdx;
|
||
a_polarPt.R = 0;
|
||
a_polarPt.angle = 0;
|
||
a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x;
|
||
a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y;
|
||
a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z;
|
||
polarPoints.push_back(a_polarPt);
|
||
scanLines[lineIdx][ptIdx].nPointIdx = -1;
|
||
}
|
||
}
|
||
}
|
||
#else
|
||
for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++)
|
||
{
|
||
SSG_featureTree* a_vTree = &v_trees[i];
|
||
//在原始点云上标记，同时有Mask上标记
|
||
for (int j = 0, j_max = (int)a_vTree->treeNodes.size(); j < j_max; j++)
|
||
{
|
||
int lineIdx = a_vTree->treeNodes[j].jumpPos2D.x;
|
||
int ptIdx = a_vTree->treeNodes[j].jumpPos2D.y;
|
||
if (scanLines[lineIdx][ptIdx].nPointIdx >= 0)
|
||
{
|
||
SWD_polarPt a_polarPt;
|
||
a_polarPt.lineIdx = lineIdx;
|
||
a_polarPt.ptIdx = ptIdx;
|
||
a_polarPt.R = 0;
|
||
a_polarPt.angle = 0;
|
||
a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x;
|
||
a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y;
|
||
a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z;
|
||
polarPoints.push_back(a_polarPt);
|
||
scanLines[lineIdx][ptIdx].nPointIdx = -1;
|
||
}
|
||
}
|
||
}
|
||
for (int i = 0, i_max = (int)h_trees.size(); i < i_max; i++)
|
||
{
|
||
SSG_featureTree* a_hTree = &h_trees[i];
|
||
//在原始点云上标记，同时有Mask上标记
|
||
for (int j = 0, j_max = (int)a_hTree->treeNodes.size(); j < j_max; j++)
|
||
{
|
||
int lineIdx = a_hTree->treeNodes[j].jumpPos2D.y;
|
||
int ptIdx = a_hTree->treeNodes[j].jumpPos2D.x;
|
||
if (scanLines[lineIdx][ptIdx].nPointIdx >= 0)
|
||
{
|
||
SWD_polarPt a_polarPt;
|
||
a_polarPt.lineIdx = lineIdx;
|
||
a_polarPt.ptIdx = ptIdx;
|
||
a_polarPt.cptIndex = -1;
|
||
a_polarPt.R = 0;
|
||
a_polarPt.angle = 0;
|
||
a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x;
|
||
a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y;
|
||
a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z;
|
||
polarPoints.push_back(a_polarPt);
|
||
scanLines[lineIdx][ptIdx].nPointIdx = -1;
|
||
}
|
||
}
|
||
}
|
||
|
||
#endif
|
||
//计算几何中心
|
||
int contourPtSize = (int)polarPoints.size();
|
||
if (contourPtSize == 0)
|
||
{
|
||
*errCode = SX_ERR_ZERO_CONTOUR_PT;
|
||
return workpieceCorners;
|
||
}
|
||
double center_x = 0;
|
||
double center_y = 0;
|
||
for (int pi = 0; pi < contourPtSize; pi++)
|
||
{
|
||
center_x += polarPoints[pi].x;
|
||
center_y += polarPoints[pi].y;
|
||
}
|
||
center_x = center_x / (double)contourPtSize;
|
||
center_y = center_y / (double)contourPtSize;
|
||
//计算极坐标的R和Theta
|
||
for (int pi = 0; pi < contourPtSize; pi++)
|
||
{
|
||
double angle = atan2(polarPoints[pi].y - center_y, polarPoints[pi].x - center_x);
|
||
angle = (angle / PI) * 180 +180.0;
|
||
double R = sqrt(pow(polarPoints[pi].y - center_y, 2) + pow(polarPoints[pi].x - center_x, 2));
|
||
polarPoints[pi].R = R;
|
||
polarPoints[pi].angle = angle;
|
||
}
|
||
//按角度大小排序
|
||
std::sort(polarPoints.begin(), polarPoints.end(), compareByAngle);
|
||
for (int pi = 0; pi < contourPtSize; pi++)
|
||
polarPoints[pi].cptIndex = pi; // index
|
||
|
||
//提取R极值点
|
||
double minR = -1, maxR = -1; //计算最小和最大的R，用以区分有没有分支。minR和maxR相差小时，为圆形或8角形，没有分支
|
||
std::vector<SWD_polarPt> polarRPeakPts;
|
||
int winSize = contourPtSize / 36; //+-10度范围
|
||
if (winSize < 5)
|
||
winSize = 5;
|
||
for (int pi = 0; pi < contourPtSize; pi++)
|
||
{
|
||
double currR = polarPoints[pi].R;
|
||
if (minR < 0)
|
||
{
|
||
minR = currR;
|
||
maxR = currR;
|
||
}
|
||
else
|
||
{
|
||
minR = minR > currR ? currR : minR;
|
||
maxR = maxR < currR ? currR : maxR;
|
||
}
|
||
bool isPeak = true;
|
||
for (int k = -winSize; k <= winSize; k++)
|
||
{
|
||
int idx = (pi + k + contourPtSize) % contourPtSize; //筒形结构
|
||
if (polarPoints[idx].R > currR)
|
||
{
|
||
isPeak = false;
|
||
break;
|
||
}
|
||
}
|
||
if (true == isPeak)
|
||
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;
|
||
//过滤圆弧段的极值：由于重心偏移，圆弧段也会形成极值。根据极值两边L=直线段长度构成的张角判断
|
||
for (int i = 0, i_max = (int)polarRPeakPts.size(); i < i_max; i++)
|
||
{
|
||
int ptidx = polarRPeakPts[i].cptIndex;
|
||
double px, py, pz;
|
||
px = polarRPeakPts[i].x;
|
||
py = polarRPeakPts[i].y;
|
||
|
||
int LL1 = -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)
|
||
{
|
||
LL1 = idx;
|
||
break;
|
||
}
|
||
}
|
||
|
||
int LL2 = -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)
|
||
{
|
||
LL2 = idx;
|
||
break;
|
||
}
|
||
}
|
||
|
||
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 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; //顺时针
|
||
|
||
polarRPeakPts[i].cptIndex = ptidx;
|
||
polarRPeakPts[i].pkId = pkId;
|
||
pkId++;
|
||
validPolarRPeakPts.push_back(polarRPeakPts[i]);
|
||
polarPeakInfo.push_back(a_pkInfo);
|
||
}
|
||
}
|
||
}
|
||
|
||
int workpieceType = -1;
|
||
std::vector< SWD_branchInfo> branchInfo;
|
||
if (true == hasBranch)
|
||
{
|
||
//计算分支，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;
|
||
double angleDiff = validPolarRPeakPts[nxtIdx].angle - validPolarRPeakPts[m].angle;
|
||
if (angleDiff < 0)
|
||
angleDiff += 360;
|
||
if (angleDiff < 90) //为同一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();
|
||
if (branchNum == 2)
|
||
workpieceType = 3; //节点3
|
||
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, //结束扫描边界
|
||
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);
|
||
#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++)
|
||
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);
|
||
}
|
||
workpieceCorners.workpieceType = workpieceType;
|
||
if (workpieceType == 1) //4个branch
|
||
{
|
||
for (int m = 0; m < 3; m++)
|
||
workpieceCorners.corner_1[m] = branchInfo[0].corner[m];
|
||
for (int m = 0; m < 3; m++)
|
||
workpieceCorners.corner_2[m] = branchInfo[1].corner[m];
|
||
for (int m = 0; m < 3; m++)
|
||
workpieceCorners.corner_3[m] = branchInfo[2].corner[m];
|
||
for (int m = 0; m < 3; m++)
|
||
workpieceCorners.corner_4[m] = branchInfo[3].corner[m];
|
||
//计算剩余信息
|
||
double line1_a, line1_b, line1_c;
|
||
compute2ptLine(
|
||
workpieceCorners.corner_1[1], workpieceCorners.corner_3[1],
|
||
&line1_a, &line1_b, &line1_c);
|
||
double line2_a, line2_b, line2_c;
|
||
compute2ptLine(
|
||
workpieceCorners.corner_2[1], workpieceCorners.corner_4[1],
|
||
&line2_a, &line2_b, &line2_c);
|
||
workpieceCorners.center = computeLineCrossPt_abs(
|
||
line1_a, line1_b, line1_c,
|
||
line2_a, line2_b, line2_c);
|
||
workpieceCorners.center.z = (workpieceCorners.corner_1[1].z + workpieceCorners.corner_2[1].z +
|
||
workpieceCorners.corner_3[1].z + workpieceCorners.corner_4[1].z) / 4;
|
||
//line1旋转45度方向
|
||
double r45_line1_a, r45_line1_b, r45_line1_c;
|
||
rotateLine45Deg(
|
||
line1_a, line1_b, line1_c,
|
||
workpieceCorners.center.x, workpieceCorners.center.y,
|
||
&r45_line1_a, &r45_line1_b, &r45_line1_c);
|
||
|
||
double r45_line2_a, r45_line2_b, r45_line2_c;
|
||
rotateLine45Deg(
|
||
line2_a, line2_b, line2_c,
|
||
workpieceCorners.center.x, workpieceCorners.center.y,
|
||
&r45_line2_a, &r45_line2_b, &r45_line2_c);
|
||
|
||
double angle1 = getLineAngle(r45_line1_a, r45_line1_b, r45_line1_c);
|
||
double angle2 = getLineAngle(r45_line2_a, r45_line2_b, r45_line2_c);
|
||
if (angle1 < angle2)
|
||
{
|
||
SVzNL3DPoint a_cross = computeEdgeCross(angle1, polarPoints, r45_line1_a, r45_line1_b, r45_line1_c);
|
||
workpieceCorners.len45_B1 = compute2DLen(workpieceCorners.center, a_cross);
|
||
a_cross = computeEdgeCross((angle1+180), polarPoints, r45_line1_a, r45_line1_b, r45_line1_c);
|
||
workpieceCorners.len225_A2 = compute2DLen(workpieceCorners.center, a_cross);
|
||
a_cross = computeEdgeCross(angle2, polarPoints, r45_line2_a, r45_line2_b, r45_line2_c);
|
||
workpieceCorners.len135_A1 = compute2DLen(workpieceCorners.center, a_cross);
|
||
a_cross = computeEdgeCross((angle2+180), polarPoints, r45_line2_a, r45_line2_b, r45_line2_c);
|
||
workpieceCorners.len315_B2 = compute2DLen(workpieceCorners.center, a_cross);
|
||
}
|
||
else
|
||
{
|
||
SVzNL3DPoint a_cross = computeEdgeCross(angle2, polarPoints, r45_line2_a, r45_line2_b, r45_line2_c);
|
||
workpieceCorners.len45_B1 = compute2DLen(workpieceCorners.center, a_cross);
|
||
a_cross = computeEdgeCross((angle2 + 180), polarPoints, r45_line2_a, r45_line2_b, r45_line2_c);
|
||
workpieceCorners.len225_A2 = compute2DLen(workpieceCorners.center, a_cross);
|
||
a_cross = computeEdgeCross(angle1, polarPoints, r45_line1_a, r45_line1_b, r45_line1_c);
|
||
workpieceCorners.len135_A1 = compute2DLen(workpieceCorners.center, a_cross);
|
||
a_cross = computeEdgeCross((angle1 + 180), polarPoints, r45_line1_a, r45_line1_b, r45_line1_c);
|
||
workpieceCorners.len315_B2 = compute2DLen(workpieceCorners.center, a_cross);
|
||
}
|
||
}
|
||
else if (workpieceType == 2) //3 个branch
|
||
{
|
||
int startIdx = 0;
|
||
for (int i = 0; i < 3; i++)
|
||
{
|
||
int nxtIdx = (i + 1) % 3;
|
||
double diff = computeAngleDiff(branchInfo[i].angle, branchInfo[nxtIdx].angle);
|
||
if (diff > 135)
|
||
{
|
||
startIdx = nxtIdx;
|
||
break;
|
||
}
|
||
}
|
||
|
||
double crossDir;
|
||
for (int m = 0; m < 3; m++)
|
||
workpieceCorners.corner_1[m] = branchInfo[startIdx].corner[m];
|
||
startIdx = (startIdx + 1) % 3;
|
||
crossDir = branchInfo[startIdx].angle;
|
||
for (int m = 0; m < 3; m++)
|
||
workpieceCorners.corner_2[m] = branchInfo[startIdx].corner[m];
|
||
startIdx = (startIdx + 1) % 3;
|
||
for (int m = 0; m < 3; m++)
|
||
workpieceCorners.corner_3[m] = branchInfo[startIdx].corner[m];
|
||
|
||
//计算剩余信息
|
||
workpieceCorners.center.x = (workpieceCorners.corner_1[1].x + workpieceCorners.corner_3[1].x) / 2;
|
||
workpieceCorners.center.y = (workpieceCorners.corner_1[1].y + workpieceCorners.corner_3[1].y) / 2;
|
||
workpieceCorners.center.z = (workpieceCorners.corner_1[1].z + workpieceCorners.corner_3[1].z) / 2;
|
||
|
||
double line_a, line_b, line_c;
|
||
compute2ptLine(
|
||
workpieceCorners.corner_2[1], workpieceCorners.center,
|
||
&line_a, &line_b, &line_c);
|
||
|
||
workpieceCorners.len45_B1 = compute2DLen(workpieceCorners.corner_1[1], workpieceCorners.center);
|
||
workpieceCorners.len135_A1 = compute2DLen(workpieceCorners.corner_2[1], workpieceCorners.center);
|
||
workpieceCorners.len225_A2 = compute2DLen(workpieceCorners.corner_3[1], workpieceCorners.center);
|
||
SVzNL3DPoint a_cross = computeEdgeCross((crossDir+180), polarPoints, line_a, line_b, line_c);
|
||
workpieceCorners.len315_B2 = compute2DLen(a_cross, workpieceCorners.center);
|
||
}
|
||
else
|
||
{
|
||
for (int m = 0; m < 3; m++)
|
||
workpieceCorners.corner_1[m] = branchInfo[0].corner[m];
|
||
for (int m = 0; m < 3; m++)
|
||
workpieceCorners.corner_2[m] = branchInfo[1].corner[m];
|
||
SVzNL3DPoint center = computeLineCrossPt_abs(
|
||
branchInfo[0].line_a, branchInfo[0].line_b, branchInfo[0].line_c,
|
||
branchInfo[1].line_a, branchInfo[1].line_b, branchInfo[1].line_c);
|
||
workpieceCorners.center.x = center.x;
|
||
workpieceCorners.center.y = center.y;
|
||
workpieceCorners.center.z = (workpieceCorners.corner_1[1].z + workpieceCorners.corner_2[1].z) / 2;
|
||
workpieceCorners.len45_B1 = compute2DLen(workpieceCorners.corner_1[1], workpieceCorners.center);
|
||
workpieceCorners.len135_A1 = compute2DLen(workpieceCorners.corner_2[1], workpieceCorners.center);
|
||
SVzNL3DPoint a_cross = computeEdgeCross(
|
||
(branchInfo[0].angle + 180), polarPoints,
|
||
branchInfo[0].line_a, branchInfo[0].line_b, branchInfo[0].line_c);
|
||
workpieceCorners.len225_A2 = compute2DLen(a_cross, workpieceCorners.center);
|
||
a_cross = computeEdgeCross(
|
||
(branchInfo[1].angle + 180), polarPoints,
|
||
branchInfo[1].line_a, branchInfo[1].line_b, branchInfo[1].line_c);
|
||
workpieceCorners.len315_B2 = compute2DLen(a_cross, workpieceCorners.center);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
workpieceType = 4; //节点4
|
||
//检查没有在边界的Corner
|
||
int polarPkNum = (int)validPolarRPeakPts.size();
|
||
for (int pki = 0; pki < polarPkNum; pki++)
|
||
{
|
||
//检查是否在扫描边界
|
||
bool isSide = false;
|
||
int cptIdx = validPolarRPeakPts[pki].cptIndex;
|
||
SVzNL3DPoint cpt = { validPolarRPeakPts[pki].x, validPolarRPeakPts[pki].y, validPolarRPeakPts[pki].z };
|
||
std::vector<SVzNL3DPoint> edgePt_1;
|
||
for (int i = 0; i < contourPtSize; i++)
|
||
{
|
||
int idx = (i + cptIdx)%contourPtSize;
|
||
SVzNL3DPoint a_pt = { polarPoints[idx].x, polarPoints[idx].y, polarPoints[idx].z };
|
||
double len = sqrt(pow(cpt.x - a_pt.x, 2) + pow(cpt.y - a_pt.y, 2));
|
||
if (len > workpieceParam.lineLen)
|
||
break;
|
||
else
|
||
{
|
||
if ((polarPoints[idx].lineIdx == validStartLine) ||
|
||
(polarPoints[idx].lineIdx == validEndLine))
|
||
isSide = true;
|
||
else
|
||
edgePt_1.push_back(a_pt);
|
||
}
|
||
}
|
||
|
||
std::vector<SVzNL3DPoint> edgePt_2;
|
||
for (int i = 0; i < contourPtSize; i++)
|
||
{
|
||
int idx = cptIdx - i;
|
||
if (idx < 0)
|
||
idx += contourPtSize;
|
||
|
||
SVzNL3DPoint a_pt = { polarPoints[idx].x, polarPoints[idx].y, polarPoints[idx].z };
|
||
double len = sqrt(pow(cpt.x - a_pt.x, 2) + pow(cpt.y - a_pt.y, 2));
|
||
if (len > workpieceParam.lineLen)
|
||
break;
|
||
else
|
||
{
|
||
if ((polarPoints[idx].lineIdx == validStartLine) ||
|
||
(polarPoints[idx].lineIdx == validEndLine))
|
||
isSide = true;
|
||
else
|
||
edgePt_2.push_back(a_pt);
|
||
}
|
||
}
|
||
if( (edgePt_1.size() < 10) || (edgePt_2.size() < 10))
|
||
{
|
||
*errCode = SX_ERR_ZERO_CONTOUR_PT;
|
||
return workpieceCorners;
|
||
}
|
||
|
||
if (false == isSide)
|
||
{
|
||
//拟合计算交点，修正
|
||
double edge1_a = 0, edge1_b = 0, edge1_c = 0;
|
||
lineFitting_abc(edgePt_1, &edge1_a, &edge1_b, &edge1_c);
|
||
double edge2_a = 0, edge2_b = 0, edge2_c = 0;
|
||
lineFitting_abc(edgePt_2, &edge2_a, &edge2_b, &edge2_c);
|
||
//计算交点
|
||
SVzNL3DPoint crossPt = computeLineCrossPt_abs(
|
||
edge1_a, edge1_b, edge1_c, edge2_a, edge2_b, edge2_c);
|
||
validPolarRPeakPts[pki].x = crossPt.x;
|
||
validPolarRPeakPts[pki].y = crossPt.y;
|
||
}
|
||
}
|
||
if (polarPkNum != 8)
|
||
{
|
||
*errCode = SX_ERR_INVLID_RPEAK_NUM;
|
||
return workpieceCorners;
|
||
}
|
||
//取互相垂直的两对
|
||
//（1）取相邻corner距离最小的两段
|
||
std::vector<double> sideLens;
|
||
for (int i = 0; i < polarPkNum; i++)
|
||
{
|
||
int nxtIdx = (i + 1) % polarPkNum;
|
||
double len = sqrt(pow(validPolarRPeakPts[i].x - validPolarRPeakPts[nxtIdx].x, 2) +
|
||
pow(validPolarRPeakPts[i].y - validPolarRPeakPts[nxtIdx].y, 2));
|
||
sideLens.push_back(len);
|
||
}
|
||
double minLen1 = -1;
|
||
int minLen1_idx = -1;
|
||
for (int i = 0; i < polarPkNum; i++)
|
||
{
|
||
if (minLen1 < 0)
|
||
{
|
||
minLen1 = sideLens[i];
|
||
minLen1_idx = i;
|
||
}
|
||
else
|
||
{
|
||
if(minLen1 > sideLens[i])
|
||
{
|
||
minLen1 = sideLens[i];
|
||
minLen1_idx = i;
|
||
}
|
||
}
|
||
}
|
||
double minLen2 = -1;
|
||
int minLen2_idx = -1;
|
||
for (int i = 0; i < polarPkNum; i++)
|
||
{
|
||
if (i == minLen1_idx)
|
||
continue;
|
||
|
||
if (minLen2 < 0)
|
||
{
|
||
minLen2 = sideLens[i];
|
||
minLen2_idx = i;
|
||
}
|
||
else
|
||
{
|
||
if (minLen2 > sideLens[i])
|
||
{
|
||
minLen2 = sideLens[i];
|
||
minLen2_idx = i;
|
||
}
|
||
}
|
||
}
|
||
if( (minLen1_idx < 0) || (minLen2_idx < 0))
|
||
{
|
||
*errCode = SX_ERR_INVLID_RPEAK_NUM;
|
||
return workpieceCorners;
|
||
}
|
||
|
||
workpieceCorners.workpieceType = workpieceType;
|
||
//计算工件信息
|
||
int tmpIdx = (minLen1_idx + 1) % polarPkNum;
|
||
workpieceCorners.corner_1[0] = { validPolarRPeakPts[minLen1_idx].x, validPolarRPeakPts[minLen1_idx].y, validPolarRPeakPts[minLen1_idx].z };
|
||
workpieceCorners.corner_1[2] = { validPolarRPeakPts[tmpIdx].x, validPolarRPeakPts[tmpIdx].y, validPolarRPeakPts[tmpIdx].z };
|
||
workpieceCorners.corner_1[1].x = (workpieceCorners.corner_1[0].x + workpieceCorners.corner_1[2].x) / 2;
|
||
workpieceCorners.corner_1[1].y = (workpieceCorners.corner_1[0].y + workpieceCorners.corner_1[2].y) / 2;
|
||
workpieceCorners.corner_1[1].z = (workpieceCorners.corner_1[0].z + workpieceCorners.corner_1[2].z) / 2;
|
||
//顺序的两个corner为下一段
|
||
tmpIdx = (tmpIdx +1) % polarPkNum;
|
||
int tmpIdx1 = (tmpIdx + 1) % polarPkNum;
|
||
workpieceCorners.corner_2[0] = { validPolarRPeakPts[tmpIdx].x, validPolarRPeakPts[tmpIdx].y, validPolarRPeakPts[tmpIdx].z };
|
||
workpieceCorners.corner_2[2] = { validPolarRPeakPts[tmpIdx1].x, validPolarRPeakPts[tmpIdx1].y, validPolarRPeakPts[tmpIdx1].z };
|
||
workpieceCorners.corner_2[1].x = (workpieceCorners.corner_2[0].x + workpieceCorners.corner_2[2].x) / 2;
|
||
workpieceCorners.corner_2[1].y = (workpieceCorners.corner_2[0].y + workpieceCorners.corner_2[2].y) / 2;
|
||
workpieceCorners.corner_2[1].z = (workpieceCorners.corner_2[0].z + workpieceCorners.corner_2[2].z) / 2;
|
||
//第二个最短的两个corner为第3个corner
|
||
tmpIdx = (minLen2_idx + 1) % polarPkNum;
|
||
workpieceCorners.corner_3[0] = { validPolarRPeakPts[minLen2_idx].x, validPolarRPeakPts[minLen2_idx].y, validPolarRPeakPts[minLen2_idx].z };
|
||
workpieceCorners.corner_3[2] = { validPolarRPeakPts[tmpIdx].x, validPolarRPeakPts[tmpIdx].y, validPolarRPeakPts[tmpIdx].z };
|
||
workpieceCorners.corner_3[1].x = (workpieceCorners.corner_3[0].x + workpieceCorners.corner_3[2].x) / 2;
|
||
workpieceCorners.corner_3[1].y = (workpieceCorners.corner_3[0].y + workpieceCorners.corner_3[2].y) / 2;
|
||
workpieceCorners.corner_3[1].z = (workpieceCorners.corner_3[0].z + workpieceCorners.corner_3[2].z) / 2;
|
||
//顺序的两个corner为下一段
|
||
tmpIdx = (tmpIdx + 1) % polarPkNum;
|
||
tmpIdx1 = (tmpIdx + 1) % polarPkNum;
|
||
workpieceCorners.corner_4[0] = { validPolarRPeakPts[tmpIdx].x, validPolarRPeakPts[tmpIdx].y, validPolarRPeakPts[tmpIdx].z };
|
||
workpieceCorners.corner_4[2] = { validPolarRPeakPts[tmpIdx1].x, validPolarRPeakPts[tmpIdx1].y, validPolarRPeakPts[tmpIdx1].z };
|
||
workpieceCorners.corner_4[1].x = (workpieceCorners.corner_4[0].x + workpieceCorners.corner_4[2].x) / 2;
|
||
workpieceCorners.corner_4[1].y = (workpieceCorners.corner_4[0].y + workpieceCorners.corner_4[2].y) / 2;
|
||
workpieceCorners.corner_4[1].z = (workpieceCorners.corner_4[0].z + workpieceCorners.corner_4[2].z) / 2;
|
||
//计算剩余信息
|
||
double line1_a, line1_b, line1_c;
|
||
compute2ptLine(
|
||
workpieceCorners.corner_1[1], workpieceCorners.corner_3[1],
|
||
&line1_a, &line1_b, &line1_c);
|
||
double line2_a, line2_b, line2_c;
|
||
compute2ptLine(
|
||
workpieceCorners.corner_2[1], workpieceCorners.corner_4[1],
|
||
&line2_a, &line2_b, &line2_c);
|
||
workpieceCorners.center = computeLineCrossPt_abs(
|
||
line1_a, line1_b, line1_c,
|
||
line2_a, line2_b, line2_c);
|
||
workpieceCorners.center.z = (workpieceCorners.corner_1[1].z + workpieceCorners.corner_2[1].z +
|
||
workpieceCorners.corner_3[1].z + workpieceCorners.corner_4[1].z) / 4;
|
||
workpieceCorners.len135_A1 = compute2DLen(workpieceCorners.corner_1[1], workpieceCorners.center);
|
||
workpieceCorners.len225_A2 = compute2DLen(workpieceCorners.corner_2[1], workpieceCorners.center);
|
||
workpieceCorners.len315_B2 = compute2DLen(workpieceCorners.corner_3[1], workpieceCorners.center);
|
||
workpieceCorners.len45_B1 = compute2DLen(workpieceCorners.corner_4[1], workpieceCorners.center);
|
||
}
|
||
}
|
||
|
||
#if 1
|
||
//将数据重新投射回原来的坐标系，以保持手眼标定结果正确
|
||
for (int i = 0; i < lineNum; i++)
|
||
sx_BQ_lineDataR(scanLines[i], groundCalibPara.invRMatrix, -1);
|
||
//将检测结果重新投射回原来的坐标系
|
||
SVzNL3DPoint rawObj;
|
||
for (int i = 0; i < 3; i++)
|
||
{
|
||
rawObj = _translatePoint(workpieceCorners.corner_1[i], groundCalibPara.invRMatrix);
|
||
workpieceCorners.corner_1[i] = rawObj;
|
||
rawObj = _translatePoint(workpieceCorners.corner_2[i], groundCalibPara.invRMatrix);
|
||
workpieceCorners.corner_2[i] = rawObj;
|
||
rawObj = _translatePoint(workpieceCorners.corner_3[i], groundCalibPara.invRMatrix);
|
||
workpieceCorners.corner_3[i] = rawObj;
|
||
rawObj = _translatePoint(workpieceCorners.corner_4[i], groundCalibPara.invRMatrix);
|
||
workpieceCorners.corner_4[i] = rawObj;
|
||
}
|
||
rawObj = _translatePoint(workpieceCorners.center, groundCalibPara.invRMatrix);
|
||
workpieceCorners.center = rawObj;
|
||
#endif
|
||
|
||
#if _OUTPUT_DEBUG_DATA
|
||
for (int i = 0; i < (int)debug_contours.size(); i++)
|
||
{
|
||
SSX_debugInfo& a_branchDebug = debug_contours[i];
|
||
for (int m = 0; m < a_branchDebug.edge_size; m++)
|
||
{
|
||
rawObj = _translatePoint(a_branchDebug.edge[m], groundCalibPara.invRMatrix);
|
||
a_branchDebug.edge[m] = rawObj;
|
||
}
|
||
for (int m = 0; m < a_branchDebug.edgeLink1_size; m++)
|
||
{
|
||
rawObj = _translatePoint(a_branchDebug.edgeLink_1[m], groundCalibPara.invRMatrix);
|
||
a_branchDebug.edgeLink_1[m] = rawObj;
|
||
}
|
||
for (int m = 0; m < a_branchDebug.edgeLink2_size; m++)
|
||
{
|
||
rawObj = _translatePoint(a_branchDebug.edgeLink_2[m], groundCalibPara.invRMatrix);
|
||
a_branchDebug.edgeLink_2[m] = rawObj;
|
||
}
|
||
}
|
||
#endif
|
||
return workpieceCorners;
|
||
} |