731 lines
24 KiB
C++
731 lines
24 KiB
C++
#include <vector>
|
||
#include "SG_baseDataType.h"
|
||
#include "SG_baseAlgo_Export.h"
|
||
#include "BQ_workpieceCornerExtraction_Export.h"
|
||
#include <opencv2/opencv.hpp>
|
||
#include <limits>
|
||
|
||
//计算一个平面调平参数。
|
||
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
|
||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||
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;
|
||
|
||
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
|
||
SSX_debugInfo* debug_conturs,
|
||
#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 linePtNum = (int)scanLines[0].size();
|
||
bool isGridData = true;
|
||
|
||
//垂直跳变特征提取
|
||
std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_v;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
if (line == 202)
|
||
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.push_back(line_features);
|
||
}
|
||
|
||
if (false == isGridData)//数据不是网格格式
|
||
{
|
||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||
return workpieceCorners;
|
||
}
|
||
|
||
//生成水平扫描
|
||
std::vector<std::vector<SVzNL3DPosition>> hLines;
|
||
hLines.resize(linePtNum);
|
||
for (int i = 0; i < linePtNum; i++)
|
||
hLines[i].resize(lineNum);
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||
hLines[j][line] = scanLines[line][j];
|
||
hLines[j][line].pt3D.x = scanLines[line][j].pt3D.y;
|
||
hLines[j][line].pt3D.y = scanLines[line][j].pt3D.x;
|
||
}
|
||
}
|
||
//水平arc特征提取
|
||
std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_h;
|
||
int lineNum_h = (int)hLines.size();
|
||
for (int line = 0; line < lineNum_h; line++)
|
||
{
|
||
if (line == 416)
|
||
int kkk = 1;
|
||
std::vector<SVzNL3DPosition>& lineData = hLines[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[line][0],
|
||
dataSize,
|
||
line,
|
||
groundCalibPara.planeHeight,
|
||
cornerPara, //scale通常取bagH的1/4
|
||
line_features);
|
||
jumpFeatures_h.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[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; line++)
|
||
{
|
||
if (line == 650)
|
||
int kkk = 1;
|
||
bool isLastLine = false;
|
||
if (line == lineNum_h - 1)
|
||
isLastLine = true;
|
||
std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_h[line];
|
||
sg_lineFeaturesGrowing(
|
||
line,
|
||
isLastLine,
|
||
a_lineJumpFeature,
|
||
h_trees,
|
||
growParam);
|
||
}
|
||
|
||
//tree信息
|
||
std::vector<SSG_treeInfo> allTreesInfo; //不包含边界
|
||
SSG_treeInfo a_nullTree;
|
||
memset(&a_nullTree, 0, sizeof(SSG_treeInfo));
|
||
allTreesInfo.push_back(a_nullTree); //保持存储位置与treeIdx相同位置，方便索引
|
||
//标记，根据起点的生长树进行标注
|
||
int hvTreeIdx = 1;
|
||
for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++)
|
||
{
|
||
SSG_featureTree* a_vTree = &v_trees[i];
|
||
sg_getTreeROI(a_vTree);
|
||
//记录Tree的信息
|
||
SSG_treeInfo a_treeInfo;
|
||
a_treeInfo.vTreeFlag = 1;
|
||
a_treeInfo.treeIdx = hvTreeIdx;
|
||
a_treeInfo.treeType = a_vTree->treeType;
|
||
a_treeInfo.sLineIdx = a_vTree->sLineIdx;
|
||
a_treeInfo.eLineIdx = a_vTree->eLineIdx;
|
||
a_treeInfo.roi = a_vTree->roi;
|
||
allTreesInfo.push_back(a_treeInfo);
|
||
|
||
std::vector<SVzNL3DPoint> a_weld_contour;
|
||
//在原始点云上标记，同时有Mask上标记
|
||
for (int j = 0, j_max = (int)a_vTree->treeNodes.size(); j < j_max; j++)
|
||
{
|
||
SSG_basicFeature1D* a_feature = &a_vTree->treeNodes[j];
|
||
if (scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.z > 1e-4)//虚假目标过滤后点会置0
|
||
{
|
||
int existEdgeId = scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx >> 16;
|
||
if (existEdgeId == 0)
|
||
{
|
||
scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx = a_feature->featureType;
|
||
scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx &= 0xffff;
|
||
scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx += hvTreeIdx << 16;
|
||
}
|
||
a_weld_contour.push_back(scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D);
|
||
}
|
||
}
|
||
hvTreeIdx++;
|
||
}
|
||
int hTreeStart = hvTreeIdx;
|
||
////标注:水平特征
|
||
for (int i = 0, i_max = (int)h_trees.size(); i < i_max; i++)
|
||
{
|
||
SSG_featureTree* a_hTree = &h_trees[i];
|
||
sg_getTreeROI(a_hTree);
|
||
//记录Tree的信息
|
||
SSG_treeInfo a_treeInfo;
|
||
a_treeInfo.vTreeFlag = 0;
|
||
a_treeInfo.treeIdx = hvTreeIdx;
|
||
a_treeInfo.treeType = a_hTree->treeType;
|
||
a_treeInfo.sLineIdx = a_hTree->sLineIdx;
|
||
a_treeInfo.eLineIdx = a_hTree->eLineIdx;
|
||
a_treeInfo.roi.left = a_hTree->roi.top; //水平扫描xy是交换的
|
||
a_treeInfo.roi.right = a_hTree->roi.bottom;
|
||
a_treeInfo.roi.top = a_hTree->roi.left;
|
||
a_treeInfo.roi.bottom = a_hTree->roi.right;
|
||
allTreesInfo.push_back(a_treeInfo);
|
||
|
||
std::vector<SVzNL3DPoint> a_weld_contour;
|
||
//在原始点云上标记，同时有Mask上标记
|
||
for (int j = 0, j_max = (int)a_hTree->treeNodes.size(); j < j_max; j++)
|
||
{
|
||
SSG_basicFeature1D* a_feature = &a_hTree->treeNodes[j];
|
||
if (scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.z > 1e-4)//虚假目标过滤后点会置0
|
||
{
|
||
int existEdgeId = scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx >> 16;
|
||
if (existEdgeId == 0)
|
||
{
|
||
scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += a_feature->featureType << 4;
|
||
scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx &= 0xffff;
|
||
scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += hvTreeIdx << 16;
|
||
}
|
||
a_weld_contour.push_back(scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D);
|
||
}
|
||
}
|
||
hvTreeIdx++;
|
||
}
|
||
int hvTreeSize = hvTreeIdx;
|
||
|
||
if(v_trees.size() < 2)
|
||
{
|
||
*errCode = SX_ERR_INVLD_VTREE_NUM;
|
||
return workpieceCorners;
|
||
}
|
||
if (h_trees.size() < 2)
|
||
{
|
||
*errCode = SX_ERR_INVLD_HTREE_NUM;
|
||
return workpieceCorners;
|
||
}
|
||
|
||
//寻找vTree的最上和最下
|
||
int vTree_T = 0;
|
||
int vTree_B = 0;
|
||
for (int i = 1, i_max = (int)v_trees.size(); i < i_max; i++)
|
||
{
|
||
if (v_trees[i].roi.top < v_trees[vTree_T].roi.top)
|
||
vTree_T = i;
|
||
if (v_trees[i].roi.bottom > v_trees[vTree_B].roi.bottom)
|
||
vTree_B = i;
|
||
}
|
||
|
||
//寻找hTree的最左和最右
|
||
int hTree_L = 0;
|
||
int hTree_R = 0;
|
||
for (int i = 1, i_max = (int)h_trees.size(); i < i_max; i++)
|
||
{
|
||
//水平扫描xy是交换的，左右对应ROI的topBottom
|
||
if (h_trees[i].roi.top < h_trees[hTree_L].roi.top)
|
||
hTree_L = i;
|
||
if (h_trees[i].roi.bottom > h_trees[hTree_R].roi.bottom)
|
||
hTree_R = i;
|
||
}
|
||
|
||
SSX_featureContour region[4];
|
||
region[0].rgnIdx = 0; //Left
|
||
_getEdgeContour(&h_trees[hTree_L], region[0].edge, scanLines,false);
|
||
//寻找对应的两边
|
||
SVzNL3DPoint firstPt = region[0].edge[0];
|
||
SVzNL3DPoint lastPt = region[0].edge.back();
|
||
int idx0 = _getPointClosestContour(v_trees, true, firstPt, scanLines, true);
|
||
if (idx0 < 0)
|
||
{
|
||
*errCode = SX_ERR_INVLD_CLOSES_PT;
|
||
return;
|
||
}
|
||
_getEdgeLinkingContour(&v_trees[idx0], true, firstPt, region[0].edgeLink_1, scanLines, true, workpieceParam.lineLen);
|
||
int idx1 = _getPointClosestContour(v_trees, true, lastPt, scanLines, true);
|
||
if (idx1 < 0)
|
||
{
|
||
*errCode = SX_ERR_INVLD_CLOSES_PT;
|
||
return;
|
||
}
|
||
_getEdgeLinkingContour(&v_trees[idx1], true, lastPt, region[0].edgeLink_2, scanLines, true, workpieceParam.lineLen);
|
||
if ((region[0].edgeLink_1.size() < 5) || (region[0].edgeLink_2.size() < 5))
|
||
{
|
||
*errCode = SX_ERR_INVLD_EDGE_LINK_NUM;
|
||
return workpieceCorners;
|
||
}
|
||
|
||
region[1].rgnIdx = 1; //Top
|
||
_getEdgeContour(&v_trees[vTree_T], region[1].edge, scanLines, true);
|
||
//寻找对应的两边
|
||
firstPt = region[1].edge[0];
|
||
lastPt = region[1].edge.back();
|
||
idx0 = _getPointClosestContour(h_trees, false, firstPt, scanLines, true);
|
||
if (idx0 < 0)
|
||
{
|
||
*errCode = SX_ERR_INVLD_CLOSES_PT;
|
||
return;
|
||
}
|
||
_getEdgeLinkingContour(&h_trees[idx0], false, firstPt, region[1].edgeLink_1, scanLines, true, workpieceParam.lineLen);
|
||
idx1 = _getPointClosestContour(h_trees, false, lastPt, scanLines, true);
|
||
if (idx1 < 0)
|
||
{
|
||
*errCode = SX_ERR_INVLD_CLOSES_PT;
|
||
return;
|
||
}
|
||
_getEdgeLinkingContour(&h_trees[idx1], false, lastPt, region[1].edgeLink_2, scanLines, true, workpieceParam.lineLen);
|
||
if ((region[1].edgeLink_1.size() < 5) || (region[1].edgeLink_2.size() < 5))
|
||
{
|
||
*errCode = SX_ERR_INVLD_EDGE_LINK_NUM;
|
||
return workpieceCorners;
|
||
}
|
||
region[2].rgnIdx = 2; //Right
|
||
_getEdgeContour(&h_trees[hTree_R], region[2].edge, scanLines, false);
|
||
//寻找对应的两边
|
||
firstPt = region[2].edge[0];
|
||
lastPt = region[2].edge.back();
|
||
idx0 = _getPointClosestContour(v_trees, true, firstPt, scanLines, false);
|
||
if (idx0 < 0)
|
||
{
|
||
*errCode = SX_ERR_INVLD_CLOSES_PT;
|
||
return;
|
||
}
|
||
_getEdgeLinkingContour(&v_trees[idx0], true, firstPt, region[2].edgeLink_1, scanLines, false, workpieceParam.lineLen);
|
||
idx1 = _getPointClosestContour(v_trees, true, lastPt, scanLines, false);
|
||
if (idx1 < 0)
|
||
{
|
||
*errCode = SX_ERR_INVLD_CLOSES_PT;
|
||
return;
|
||
}
|
||
_getEdgeLinkingContour(&v_trees[idx1], true, lastPt, region[2].edgeLink_2, scanLines, false, workpieceParam.lineLen);
|
||
if ((region[2].edgeLink_1.size() < 5) || (region[2].edgeLink_2.size() < 5))
|
||
{
|
||
*errCode = SX_ERR_INVLD_EDGE_LINK_NUM;
|
||
return workpieceCorners;
|
||
}
|
||
region[3].rgnIdx = 3; //Bottom
|
||
_getEdgeContour(&v_trees[vTree_B], region[3].edge, scanLines, true);
|
||
//寻找对应的两边
|
||
firstPt = region[3].edge[0];
|
||
lastPt = region[3].edge.back();
|
||
idx0 = _getPointClosestContour(h_trees, false, firstPt, scanLines, true);
|
||
if (idx0 < 0)
|
||
{
|
||
*errCode = SX_ERR_INVLD_CLOSES_PT;
|
||
return;
|
||
}
|
||
_getEdgeLinkingContour(&h_trees[idx0], false, firstPt, region[3].edgeLink_1, scanLines, false, workpieceParam.lineLen);
|
||
idx1 = _getPointClosestContour(h_trees, false, lastPt, scanLines, true);
|
||
if (idx1 < 0)
|
||
{
|
||
*errCode = SX_ERR_INVLD_CLOSES_PT;
|
||
return;
|
||
}
|
||
_getEdgeLinkingContour(&h_trees[idx1], false, lastPt, region[3].edgeLink_2, scanLines, false, workpieceParam.lineLen);
|
||
if ((region[3].edgeLink_1.size() < 5) || (region[3].edgeLink_2.size() < 5))
|
||
{
|
||
*errCode = SX_ERR_INVLD_EDGE_LINK_NUM;
|
||
return workpieceCorners;
|
||
}
|
||
|
||
for (int i = 0; i < 4; i++)
|
||
{
|
||
if ((i == 0) || (i == 2))
|
||
{
|
||
//Left:防止垂直直线，使用x=ky+b
|
||
std::vector<SVzNL3DPoint> transPts;
|
||
for (int m = 0, m_max = (int)region[i].edge.size(); m < m_max; m++)
|
||
{
|
||
SVzNL3DPoint a_pt;
|
||
a_pt.x = region[i].edge[m].y;
|
||
a_pt.y = region[i].edge[m].x;
|
||
a_pt.z = region[i].edge[m].z;
|
||
transPts.push_back(a_pt);
|
||
}
|
||
//拟合测量
|
||
double edge_x_k, edge_x_b;
|
||
lineFitting(transPts, &edge_x_k, &edge_x_b);
|
||
//计算拟合直线端点
|
||
SVzNL3DPoint end_0 = transPts[0];
|
||
SVzNL3DPoint end_1 = transPts.back();
|
||
SVzNL2DPointD foot_0 = sx_getFootPoint(end_0.x, end_0.y, edge_x_k, edge_x_b);
|
||
SVzNL2DPointD foot_1 = sx_getFootPoint(end_1.x, end_1.y, edge_x_k, edge_x_b);
|
||
region[i].edge_ends[0] = { foot_0.y, foot_0.x, groundCalibPara.planeHeight };
|
||
region[i].edge_ends[1] = { foot_1.y, foot_1.x, groundCalibPara.planeHeight };
|
||
//两侧Linking直线使用 y=kx+b
|
||
double edge_link1_k, edge_link1_b;
|
||
lineFitting(region[i].edgeLink_1, &edge_link1_k, &edge_link1_b);
|
||
end_0 = region[i].edgeLink_1[0];
|
||
end_1 = region[i].edgeLink_1.back();
|
||
foot_0 = sx_getFootPoint(end_0.x, end_0.y, edge_link1_k, edge_link1_b);
|
||
foot_1 = sx_getFootPoint(end_1.x, end_1.y, edge_link1_k, edge_link1_b);
|
||
region[i].edge_link1_ends[0] = { foot_0.x, foot_0.y, groundCalibPara.planeHeight };
|
||
region[i].edge_link1_ends[1] = { foot_1.x, foot_1.y, groundCalibPara.planeHeight };
|
||
|
||
double edge_link2_k, edge_link2_b;
|
||
lineFitting(region[i].edgeLink_2, &edge_link2_k, &edge_link2_b);
|
||
end_0 = region[i].edgeLink_2[0];
|
||
end_1 = region[i].edgeLink_2.back();
|
||
foot_0 = sx_getFootPoint(end_0.x, end_0.y, edge_link2_k, edge_link2_b);
|
||
foot_1 = sx_getFootPoint(end_1.x, end_1.y, edge_link2_k, edge_link2_b);
|
||
region[i].edge_link2_ends[0] = { foot_0.x, foot_0.y, groundCalibPara.planeHeight };
|
||
region[i].edge_link2_ends[1] = { foot_1.x, foot_1.y, groundCalibPara.planeHeight };
|
||
|
||
//计算交点
|
||
end_0 = region[i].edge[0];
|
||
end_1 = region[i].edge.back();
|
||
SVzNL3DPoint crossPt[3];
|
||
crossPt[0].x = (edge_x_k * edge_link1_b + edge_x_b) / (1.0 - edge_x_k * edge_link1_k);
|
||
crossPt[0].y = edge_link1_k * crossPt[0].x + edge_link1_b;
|
||
crossPt[0].z = groundCalibPara.planeHeight;
|
||
crossPt[2].x = (edge_x_k * edge_link2_b + edge_x_b) / (1.0 - edge_x_k * edge_link2_k);
|
||
crossPt[2].y = edge_link2_k * crossPt[2].x + edge_link2_b;
|
||
crossPt[2].z = groundCalibPara.planeHeight;
|
||
crossPt[1].x = (crossPt[0].x + crossPt[2].x) / 2;
|
||
crossPt[1].y = (crossPt[0].y + crossPt[2].y) / 2;
|
||
crossPt[1].z = groundCalibPara.planeHeight;
|
||
if (i == 0)
|
||
{
|
||
for (int m = 0; m < 3; m++)
|
||
workpieceCorners.corner_L[m] = crossPt[m];
|
||
}
|
||
else
|
||
{
|
||
for (int m = 0; m < 3; m++)
|
||
workpieceCorners.corner_R[m] = crossPt[m];
|
||
}
|
||
}
|
||
else
|
||
{
|
||
|
||
//拟合测量
|
||
double edge_k, edge_b;
|
||
lineFitting(region[i].edge, &edge_k, &edge_b);
|
||
SVzNL3DPoint end_0 = region[i].edge[0];
|
||
SVzNL3DPoint end_1 = region[i].edge.back();
|
||
SVzNL2DPointD foot_0 = sx_getFootPoint(end_0.x, end_0.y, edge_k, edge_b);
|
||
SVzNL2DPointD foot_1 = sx_getFootPoint(end_1.x, end_1.y, edge_k, edge_b);
|
||
region[i].edge_ends[0] = { foot_0.x, foot_0.y, groundCalibPara.planeHeight };
|
||
region[i].edge_ends[1] = { foot_1.x, foot_1.y, groundCalibPara.planeHeight };
|
||
//防止垂直直线，使用x=ky+b
|
||
std::vector<SVzNL3DPoint> transPts_link1;
|
||
for (int m = 0, m_max = (int)region[i].edgeLink_1.size(); m < m_max; m++)
|
||
{
|
||
SVzNL3DPoint a_pt;
|
||
a_pt.x = region[i].edgeLink_1[m].y;
|
||
a_pt.y = region[i].edgeLink_1[m].x;
|
||
a_pt.z = region[i].edgeLink_1[m].z;
|
||
transPts_link1.push_back(a_pt);
|
||
}
|
||
double edge_link1_kx, edge_link1_bx;
|
||
lineFitting(transPts_link1, &edge_link1_kx, &edge_link1_bx);
|
||
//计算拟合直线端点
|
||
end_0 = transPts_link1[0];
|
||
end_1 = transPts_link1.back();
|
||
foot_0 = sx_getFootPoint(end_0.x, end_0.y, edge_link1_kx, edge_link1_bx);
|
||
foot_1 = sx_getFootPoint(end_1.x, end_1.y, edge_link1_kx, edge_link1_bx);
|
||
region[i].edge_link1_ends[0] = { foot_0.y, foot_0.x, groundCalibPara.planeHeight };
|
||
region[i].edge_link1_ends[1] = { foot_1.y, foot_1.x, groundCalibPara.planeHeight };
|
||
//两侧Linking直线使用 y=kx+b
|
||
std::vector<SVzNL3DPoint> transPts_link2;
|
||
for (int m = 0, m_max = (int)region[i].edgeLink_2.size(); m < m_max; m++)
|
||
{
|
||
SVzNL3DPoint a_pt;
|
||
a_pt.x = region[i].edgeLink_2[m].y;
|
||
a_pt.y = region[i].edgeLink_2[m].x;
|
||
a_pt.z = region[i].edgeLink_2[m].z;
|
||
transPts_link2.push_back(a_pt);
|
||
}
|
||
double edge_link2_kx, edge_link2_bx;
|
||
lineFitting(transPts_link2, &edge_link2_kx, &edge_link2_bx);
|
||
end_0 = transPts_link2[0];
|
||
end_1 = transPts_link2.back();
|
||
foot_0 = sx_getFootPoint(end_0.x, end_0.y, edge_link2_kx, edge_link2_bx);
|
||
foot_1 = sx_getFootPoint(end_1.x, end_1.y, edge_link2_kx, edge_link2_bx);
|
||
region[i].edge_link2_ends[0] = { foot_0.y, foot_0.x, groundCalibPara.planeHeight };
|
||
region[i].edge_link2_ends[1] = { foot_1.y, foot_1.x, groundCalibPara.planeHeight };
|
||
|
||
//计算交点
|
||
end_0 = region[i].edge[0];
|
||
end_1 = region[i].edge.back();
|
||
SVzNL3DPoint crossPt[3];
|
||
crossPt[0].x = (edge_link1_kx * edge_b + edge_link1_bx) / (1.0 - edge_link1_kx * edge_k);
|
||
crossPt[0].y = edge_k * crossPt[0].x + edge_b;
|
||
crossPt[0].z = groundCalibPara.planeHeight;
|
||
crossPt[2].x = (edge_link2_kx * edge_b + edge_link2_bx) / (1.0 - edge_link2_kx * edge_k);
|
||
crossPt[2].y = edge_k * crossPt[2].x + edge_b;
|
||
crossPt[2].z = groundCalibPara.planeHeight;
|
||
crossPt[1].x = (crossPt[0].x + crossPt[2].x) / 2;
|
||
crossPt[1].y = (crossPt[0].y + crossPt[2].y) / 2;
|
||
crossPt[1].z = groundCalibPara.planeHeight;
|
||
if (i == 1)
|
||
{
|
||
for (int m = 0; m < 3; m++)
|
||
workpieceCorners.corner_T[m] = crossPt[m];
|
||
}
|
||
else
|
||
{
|
||
for (int m = 0; m < 3; m++)
|
||
workpieceCorners.corner_B[m] = crossPt[m];
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
#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_L[i], groundCalibPara.invRMatrix);
|
||
workpieceCorners.corner_L[i] = rawObj;
|
||
rawObj = _translatePoint(workpieceCorners.corner_R[i], groundCalibPara.invRMatrix);
|
||
workpieceCorners.corner_R[i] = rawObj;
|
||
rawObj = _translatePoint(workpieceCorners.corner_T[i], groundCalibPara.invRMatrix);
|
||
workpieceCorners.corner_T[i] = rawObj;
|
||
rawObj = _translatePoint(workpieceCorners.corner_B[i], groundCalibPara.invRMatrix);
|
||
workpieceCorners.corner_B[i] = rawObj;
|
||
}
|
||
#if _OUTPUT_DEBUG_DATA
|
||
for (int i = 0; i < 4; i++)
|
||
{
|
||
rawObj = _translatePoint(region[i].edge_ends[0], groundCalibPara.invRMatrix);
|
||
region[i].edge_ends[0] = rawObj;
|
||
rawObj = _translatePoint(region[i].edge_ends[1], groundCalibPara.invRMatrix);
|
||
region[i].edge_ends[1] = rawObj;
|
||
rawObj = _translatePoint(region[i].edge_link1_ends[0], groundCalibPara.invRMatrix);
|
||
region[i].edge_link1_ends[0] = rawObj;
|
||
rawObj = _translatePoint(region[i].edge_link1_ends[1], groundCalibPara.invRMatrix);
|
||
region[i].edge_link1_ends[1] = rawObj;
|
||
rawObj = _translatePoint(region[i].edge_link2_ends[0], groundCalibPara.invRMatrix);
|
||
region[i].edge_link2_ends[0] = rawObj;
|
||
rawObj = _translatePoint(region[i].edge_link2_ends[1], groundCalibPara.invRMatrix);
|
||
region[i].edge_link2_ends[1] = rawObj;
|
||
for (int m = 0, m_max = (int)region[i].edge.size(); m < m_max; m++)
|
||
{
|
||
rawObj = _translatePoint(region[i].edge[m], groundCalibPara.invRMatrix);
|
||
region[i].edge[m] = rawObj;
|
||
}
|
||
for (int m = 0, m_max = (int)region[i].edgeLink_1.size(); m < m_max; m++)
|
||
{
|
||
rawObj = _translatePoint(region[i].edgeLink_1[m], groundCalibPara.invRMatrix);
|
||
region[i].edgeLink_1[m] = rawObj;;
|
||
}
|
||
for (int m = 0, m_max = (int)region[i].edgeLink_2.size(); m < m_max; m++)
|
||
{
|
||
rawObj = _translatePoint(region[i].edgeLink_2[m], groundCalibPara.invRMatrix);
|
||
region[i].edgeLink_2[m] = rawObj;;
|
||
}
|
||
}
|
||
#endif
|
||
#endif
|
||
|
||
#if _OUTPUT_DEBUG_DATA
|
||
if (debug_conturs)
|
||
{
|
||
for (int i = 0; i < 4; i++)
|
||
{
|
||
debug_conturs[i].rgnIdx = region[i].rgnIdx;
|
||
debug_conturs[i].edge_ends[0] = region[i].edge_ends[0];
|
||
debug_conturs[i].edge_ends[1] = region[i].edge_ends[1];
|
||
debug_conturs[i].edge_link1_ends[0] = region[i].edge_link1_ends[0];
|
||
debug_conturs[i].edge_link1_ends[1] = region[i].edge_link1_ends[1];
|
||
debug_conturs[i].edge_link2_ends[0] = region[i].edge_link2_ends[0];
|
||
debug_conturs[i].edge_link2_ends[1] = region[i].edge_link2_ends[1];
|
||
debug_conturs[i].edge_size = (int)region[i].edge.size();
|
||
debug_conturs[i].edge = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * (int)region[i].edge.size());
|
||
for (int m = 0, m_max = (int)region[i].edge.size(); m < m_max; m++)
|
||
debug_conturs[i].edge[m] = region[i].edge[m];
|
||
debug_conturs[i].edgeLink1_size = (int)region[i].edgeLink_1.size();
|
||
debug_conturs[i].edgeLink_1 = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * (int)region[i].edgeLink_1.size());
|
||
for (int m = 0, m_max = (int)region[i].edgeLink_1.size(); m < m_max; m++)
|
||
debug_conturs[i].edgeLink_1[m] = region[i].edgeLink_1[m];
|
||
debug_conturs[i].edgeLink2_size = (int)region[i].edgeLink_2.size();
|
||
debug_conturs[i].edgeLink_2 = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * (int)region[i].edgeLink_2.size());
|
||
for (int m = 0, m_max = (int)region[i].edgeLink_2.size(); m < m_max; m++)
|
||
debug_conturs[i].edgeLink_2[m] = region[i].edgeLink_2[m];
|
||
}
|
||
}
|
||
#endif
|
||
|
||
workpieceCorners.workpieceType = 1;
|
||
return workpieceCorners;
|
||
} |