866 lines
25 KiB
C++
866 lines
25 KiB
C++
#include <vector>
|
||
#include "SG_baseDataType.h"
|
||
#include "SG_baseAlgo_Export.h"
|
||
#include "SX_lapWeldDetection_Export.h"
|
||
#include <opencv2/opencv.hpp>
|
||
#include <limits>
|
||
|
||
//version 1.0.0 :焊缝（平缝）识别检测算法初始发布
|
||
//version 1.1.0 :焊缝（平缝）识别修正回归测试中发现的bug
|
||
//version 1.2.0 :焊缝（平缝）识别添加博清模拟数据中的凸起焊缝，通过参数选择识别类型（不支持自动识别）
|
||
//version 1.2.1 :焊缝（平缝）添加起止点定位，去除首尾引弧板
|
||
std::string m_strVersion = "weldSeamAlgo_1.2.1";
|
||
|
||
const char* wd_weldSeamAlgoVersion(void)
|
||
{
|
||
return m_strVersion.c_str();
|
||
}
|
||
//计算一个平面调平参数。
|
||
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
|
||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||
SSG_planeCalibPara sx_getBaseCalibPara(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines)
|
||
{
|
||
return sg_getPlaneCalibPara2(scanLines);
|
||
}
|
||
|
||
//相机姿态调平，并去除地面
|
||
void sx_lineDataR(
|
||
std::vector< SVzNL3DPosition>& a_line,
|
||
const double* camPoseR,
|
||
double groundH)
|
||
{
|
||
lineDataRT_vector(a_line, camPoseR, groundH);
|
||
}
|
||
|
||
SVzNL2DPoint getFootPoint(double x0, double y0, double k, double b)
|
||
{
|
||
double A = k;
|
||
double B = -1;
|
||
double C = b;
|
||
SVzNL2DPoint foot;
|
||
foot.x = (B * B * x0 - A * B * y0 - A * C) / (A * A + B * B);
|
||
foot.y = (-A * B * x0 + A * A * y0 - B * C) / (A * A + B * B);
|
||
return foot;
|
||
}
|
||
|
||
double getWinMeanZ(std::vector<SVzNL3DPoint>& points, int startIdx, int win)
|
||
{
|
||
int size = (int)points.size();
|
||
double sumZ = 0;
|
||
double sumSize = 0;
|
||
for (int i = 0; i < win; i++)
|
||
{
|
||
int idx = i + startIdx;
|
||
if (idx < size)
|
||
{
|
||
if (points[idx].z > 1e-4)
|
||
{
|
||
sumZ += points[idx].z;
|
||
sumSize++;
|
||
}
|
||
}
|
||
}
|
||
if (sumSize > 0)
|
||
sumZ = sumZ / (double)sumSize;
|
||
return sumZ;
|
||
}
|
||
//从焊缝的扫描点获取焊缝的起点和终点信息
|
||
void getWeldPoint(std::vector<SVzNL3DPoint>& a_weld_contour, int midPtNum, std::vector<SVzNL3DPoint>& a_weld)
|
||
{
|
||
if (a_weld_contour.size() == 0)
|
||
return;
|
||
|
||
//拟合直线
|
||
double k, b;
|
||
lineFitting(a_weld_contour, &k, &b);
|
||
//寻找起点和终点
|
||
SVzNL3DPoint startPt = a_weld_contour[0];
|
||
SVzNL3DPoint endPt = a_weld_contour.back();
|
||
SVzNL2DPoint foot_s = getFootPoint(startPt.x, startPt.y, k, b);
|
||
SVzNL2DPoint foot_e = getFootPoint(endPt.x, endPt.y, k, b);
|
||
double meanZ_s = getWinMeanZ(a_weld_contour, 0, 5);
|
||
double meanZ_e = getWinMeanZ(a_weld_contour, (int)a_weld_contour.size()-1-5, 5);
|
||
|
||
SVzNL3DPoint pt_0 = { foot_s.x, foot_s.y, meanZ_s };
|
||
SVzNL3DPoint pt_1 = { foot_e.x, foot_e.y, meanZ_e };
|
||
a_weld.push_back(pt_0);
|
||
double ratio = 1.0 / ((double)midPtNum + 1.0);
|
||
|
||
for (int i = 1; i <= midPtNum; i++)
|
||
{
|
||
SVzNL3DPoint a_pt;
|
||
a_pt.x = (double)i * ratio * (pt_1.x - pt_0.x) + pt_0.x;
|
||
a_pt.y = (double)i * ratio * (pt_1.y - pt_0.y) + pt_0.y;
|
||
a_pt.z = (double)i * ratio * (pt_1.z - pt_0.z) + pt_0.z;
|
||
a_weld.push_back(a_pt);
|
||
}
|
||
a_weld.push_back(pt_1);
|
||
}
|
||
|
||
SVzNLRange _getTreesLineRange(std::vector<SSG_gapFeatureTree>& trees)
|
||
{
|
||
SVzNLRange a_rng = { 0, 0 };
|
||
if (trees.size() == 0)
|
||
return a_rng;
|
||
|
||
if (trees[0].sLineIdx < trees[0].eLineIdx)
|
||
{
|
||
a_rng.nMin = trees[0].sLineIdx;
|
||
a_rng.nMax = trees[0].eLineIdx;
|
||
}
|
||
else
|
||
{
|
||
a_rng.nMax = trees[0].sLineIdx;
|
||
a_rng.nMin = trees[0].eLineIdx;
|
||
}
|
||
|
||
for (int i = 1; i < (int)trees.size(); i++)
|
||
{
|
||
a_rng.nMin = a_rng.nMin > trees[i].sLineIdx ? trees[i].sLineIdx : a_rng.nMin;
|
||
a_rng.nMin = a_rng.nMin > trees[i].eLineIdx ? trees[i].eLineIdx : a_rng.nMin;
|
||
a_rng.nMax = a_rng.nMax < trees[i].sLineIdx ? trees[i].sLineIdx : a_rng.nMax;
|
||
a_rng.nMax = a_rng.nMax < trees[i].eLineIdx ? trees[i].eLineIdx : a_rng.nMax;
|
||
}
|
||
return a_rng;
|
||
}
|
||
|
||
SVzNLRangeD _getOneTreeXRange(SSG_gapFeatureTree& a_tree)
|
||
{
|
||
SVzNLRangeD a_rng = { 0.0, 0.0 };
|
||
|
||
SSG_basicFeatureGap nodeFirst = a_tree.treeNodes[0];
|
||
SSG_basicFeatureGap nodeLast = a_tree.treeNodes.back();
|
||
|
||
if (nodeFirst.gapPeak.pt3D.x < nodeLast.gapPeak.pt3D.x)
|
||
{
|
||
a_rng.min = nodeFirst.gapPeak.pt3D.x;
|
||
a_rng.max = nodeLast.gapPeak.pt3D.x;
|
||
}
|
||
else
|
||
{
|
||
a_rng.max = nodeFirst.gapPeak.pt3D.x;
|
||
a_rng.min = nodeLast.gapPeak.pt3D.x;
|
||
}
|
||
return a_rng;
|
||
}
|
||
|
||
SVzNLRangeD _getTreesXRange(std::vector<SSG_gapFeatureTree>& trees)
|
||
{
|
||
SVzNLRangeD a_rng = { 0.0, 0.0 };
|
||
if (trees.size() == 0)
|
||
return a_rng;
|
||
|
||
SSG_basicFeatureGap nodeFirst = trees[0].treeNodes[0];
|
||
SSG_basicFeatureGap nodeLast = trees[0].treeNodes.back();
|
||
|
||
if (nodeFirst.gapPeak.pt3D.x < nodeLast.gapPeak.pt3D.x)
|
||
{
|
||
a_rng.min = nodeFirst.gapPeak.pt3D.x;
|
||
a_rng.max = nodeLast.gapPeak.pt3D.x;
|
||
}
|
||
else
|
||
{
|
||
a_rng.max = nodeFirst.gapPeak.pt3D.x;
|
||
a_rng.min = nodeLast.gapPeak.pt3D.x;
|
||
}
|
||
for (int i = 1; i < (int)trees.size(); i++)
|
||
{
|
||
nodeFirst = trees[i].treeNodes[0];
|
||
nodeLast = trees[i].treeNodes.back();
|
||
a_rng.min = a_rng.min > nodeFirst.gapPeak.pt3D.x ? nodeFirst.gapPeak.pt3D.x : a_rng.min;
|
||
a_rng.min = a_rng.min > nodeLast.gapPeak.pt3D.x ? nodeLast.gapPeak.pt3D.x : a_rng.min;
|
||
a_rng.max = a_rng.max < nodeFirst.gapPeak.pt3D.x ? nodeFirst.gapPeak.pt3D.x : a_rng.max;
|
||
a_rng.max = a_rng.max < nodeLast.gapPeak.pt3D.x ? nodeLast.gapPeak.pt3D.x : a_rng.max;
|
||
}
|
||
return a_rng;
|
||
}
|
||
|
||
//提取搭接焊缝
|
||
void sx_getLapWeldPostion(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
const SSG_cornerParam cornerPara,
|
||
SSG_treeGrowParam growParam,
|
||
SSX_lapWeldParam lapWeldParam,
|
||
SSG_planeCalibPara groundCalibPara,
|
||
std::vector<std::vector<SVzNL3DPoint>>& objOps,
|
||
int* errCode)
|
||
{
|
||
*errCode = 0;
|
||
int lineNum = (int)scanLines.size();
|
||
if (lineNum == 0)
|
||
{
|
||
*errCode = SG_ERR_3D_DATA_NULL;
|
||
return;
|
||
}
|
||
|
||
int linePtNum = (int)scanLines[0].size();
|
||
bool isGridData = true;
|
||
//垂直跳变特征提取
|
||
std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_v;
|
||
if ((keSX_ScanMode_V == lapWeldParam.scanMode) || (keSX_ScanMode_Both == lapWeldParam.scanMode))
|
||
{
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
if (line == 400)
|
||
int kkk = 1;
|
||
|
||
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
|
||
if (linePtNum != (int)lineData.size())
|
||
isGridData = false;
|
||
|
||
std::vector<SSG_basicFeature1D> a_line_features;
|
||
int dataSize = (int)lineData.size();
|
||
sg_getLineZJumpFeature_cornerMethod(
|
||
&scanLines[line][0],
|
||
dataSize,
|
||
line,
|
||
cornerPara,
|
||
false, //取下沿
|
||
a_line_features);
|
||
//滤除地面
|
||
std::vector<SSG_basicFeature1D> vld_features;
|
||
for (int i = 0, i_max = a_line_features.size(); i < i_max; i++)
|
||
{
|
||
//将与地面形成的跳变去除
|
||
if (a_line_features[i].featureValue < (groundCalibPara.planeHeight - 0.25))
|
||
{
|
||
vld_features.push_back(a_line_features[i]);
|
||
}
|
||
}
|
||
jumpFeatures_v.push_back(vld_features);
|
||
}
|
||
}
|
||
|
||
if (false == isGridData)//数据不是网格格式
|
||
{
|
||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||
return;
|
||
}
|
||
|
||
//生成水平扫描
|
||
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();
|
||
if ((keSX_ScanMode_H == lapWeldParam.scanMode) || (keSX_ScanMode_Both == lapWeldParam.scanMode))
|
||
{
|
||
for (int line = 0; line < lineNum_h; line++)
|
||
{
|
||
std::vector<SVzNL3DPosition>& lineData = hLines[line];
|
||
|
||
std::vector<SSG_basicFeature1D> a_line_features;
|
||
int dataSize = (int)lineData.size();
|
||
sg_getLineZJumpFeature_cornerMethod(
|
||
&hLines[line][0],
|
||
dataSize,
|
||
line,
|
||
cornerPara,
|
||
false,//取下沿
|
||
a_line_features);
|
||
|
||
//滤除地面
|
||
std::vector<SSG_basicFeature1D> vld_features;
|
||
for (int i = 0, i_max = a_line_features.size(); i < i_max; i++)
|
||
{
|
||
//将与地面形成的跳变去除
|
||
if (a_line_features[i].featureValue < (groundCalibPara.planeHeight - 0.25))
|
||
{
|
||
vld_features.push_back(a_line_features[i]);
|
||
}
|
||
}
|
||
jumpFeatures_h.push_back(vld_features);
|
||
}
|
||
}
|
||
|
||
//特征生长
|
||
//垂直方向特征生长（激光线方向）
|
||
std::vector<SSG_featureTree> v_trees;
|
||
if ((keSX_ScanMode_V == lapWeldParam.scanMode) || (keSX_ScanMode_Both == lapWeldParam.scanMode))
|
||
{
|
||
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 == 400)
|
||
int kkk = 1;
|
||
sg_lineFeaturesGrowing(
|
||
line,
|
||
isLastLine,
|
||
a_lineJumpFeature,
|
||
v_trees,
|
||
growParam);
|
||
}
|
||
}
|
||
//水平方向特征生长（扫描运动方向）
|
||
std::vector<SSG_featureTree> h_trees;
|
||
if ((keSX_ScanMode_H == lapWeldParam.scanMode) || (keSX_ScanMode_Both == lapWeldParam.scanMode))
|
||
{
|
||
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];
|
||
|
||
//记录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);
|
||
}
|
||
}
|
||
std::vector<SVzNL3DPoint> a_weld;
|
||
getWeldPoint(a_weld_contour, lapWeldParam.weldRefPoints, a_weld);
|
||
if (a_weld.size() > 0)
|
||
objOps.push_back(a_weld);
|
||
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];
|
||
//记录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);
|
||
}
|
||
}
|
||
std::vector<SVzNL3DPoint> a_weld;
|
||
getWeldPoint(a_weld_contour, lapWeldParam.weldRefPoints, a_weld);
|
||
if (a_weld.size() > 0)
|
||
objOps.push_back(a_weld);
|
||
|
||
hvTreeIdx++;
|
||
}
|
||
int hvTreeSize = hvTreeIdx;
|
||
|
||
//将数据重新投射回原来的坐标系，以保持手眼标定结果正确
|
||
for (int i = 0; i < lineNum; i++)
|
||
sx_lineDataR(scanLines[i], groundCalibPara.invRMatrix, -1);
|
||
//将检测结果重新投射回原来的坐标系
|
||
for (int i = 0, i_max = (int)objOps.size(); i < i_max; i++)
|
||
{
|
||
std::vector<SVzNL3DPoint>& a_weld = objOps[i];
|
||
for (int j = 0, j_max = a_weld.size(); j < j_max; j++)
|
||
{
|
||
double x = a_weld[j].x * groundCalibPara.invRMatrix[0] +
|
||
a_weld[j].y * groundCalibPara.invRMatrix[1] +
|
||
a_weld[j].z * groundCalibPara.invRMatrix[2];
|
||
double y = a_weld[j].x * groundCalibPara.invRMatrix[3] +
|
||
a_weld[j].y * groundCalibPara.invRMatrix[4] +
|
||
a_weld[j].z * groundCalibPara.invRMatrix[5];
|
||
double z = a_weld[j].x * groundCalibPara.invRMatrix[6] +
|
||
a_weld[j].y * groundCalibPara.invRMatrix[7] +
|
||
a_weld[j].z * groundCalibPara.invRMatrix[8];
|
||
a_weld[j].x = x;
|
||
a_weld[j].y = y;
|
||
a_weld[j].z = z;
|
||
}
|
||
}
|
||
|
||
//输出结果
|
||
}
|
||
|
||
double _getMeanZ(std::vector<double>& ptZ)
|
||
{
|
||
double meanZ = 0;
|
||
int num = (int)ptZ.size();
|
||
for (int i = 0; i < num; i++)
|
||
meanZ += ptZ[i];
|
||
|
||
meanZ = meanZ / num;
|
||
return meanZ;
|
||
}
|
||
|
||
double _getNearestZ(std::vector<SVzNL3DPoint>& pts, double refx)
|
||
{
|
||
if (pts.size() == 0)
|
||
return 0;
|
||
|
||
double minDist = abs(pts[0].x - refx);
|
||
double bestZ = pts[0].z;
|
||
for (int i = 1; i < (int)pts.size(); i++)
|
||
{
|
||
double xDist = abs(pts[i].x = refx);
|
||
if (minDist > xDist)
|
||
{
|
||
minDist = xDist;
|
||
bestZ = pts[i].z;
|
||
}
|
||
}
|
||
return bestZ;
|
||
}
|
||
|
||
bool _compareByXValue(const SVzNL3DPoint& a, const SVzNL3DPoint& b) {
|
||
return a.x < b.x;
|
||
}
|
||
|
||
bool _compareByLineIdx(const SSG_gapFeatureTree& a, const SSG_gapFeatureTree& b) {
|
||
return a.sLineIdx < b.sLineIdx;
|
||
}
|
||
|
||
//检查焊板的起始位置
|
||
void BQ_checkWeldingStartLine(
|
||
std::vector<SVzNL3DPosition>& a_line,
|
||
const int lineIdx,
|
||
const double innerPara_lineDeviTh, //扫描线是直线的判断门限：点到起点终点构成直线的距离的最大值
|
||
const double innerPara_endingZDeviTh, //连续面扫描线端点的Z偏离最大门限
|
||
const double innerPara_endingDistTh, //连线面扫描线端点距离最大门限
|
||
SWD_weldSegment& weldStartLine)
|
||
{
|
||
if ( (lineIdx == 1504) || (lineIdx == 1039) || (lineIdx == 1984))
|
||
int kkk = 1;
|
||
|
||
//计算端点直线与所有点的距离，过滤掉曲线
|
||
int startId = -1, endId = 0;
|
||
for (int i = 0; i < (int)a_line.size(); i++)
|
||
{
|
||
if (a_line[i].pt3D.z > 1e-4)
|
||
{
|
||
if (startId < 0)
|
||
startId = i;
|
||
endId = i;
|
||
}
|
||
else
|
||
a_line[i].nPointIdx = -1; //空点标记
|
||
}
|
||
int validLinePtNum = endId - startId + 1;
|
||
if ((startId < 0) || (validLinePtNum < 10)) //空行
|
||
return;
|
||
|
||
//计算过2点的直线方程,不使用结构体
|
||
double lineYZ_a, lineYZ_b, lineYZ_c;
|
||
compute2ptLine_2(
|
||
a_line[startId].pt3D.y, a_line[startId].pt3D.z,
|
||
a_line[endId].pt3D.y, a_line[endId].pt3D.z,
|
||
&lineYZ_a, &lineYZ_b, &lineYZ_c);
|
||
|
||
//计算线上点到直线的最大距离：焊接的特点是两段。根据最高点将线段分为两段
|
||
double maxDist = 0;
|
||
int peakPtIdx = -1;
|
||
for (int i = 0; i < (int)a_line.size(); i++)
|
||
{
|
||
if (a_line[i].pt3D.z > 1e-4)
|
||
{
|
||
double dist = computePtDistToLine(a_line[i].pt3D.y, a_line[i].pt3D.z, lineYZ_a, lineYZ_b, lineYZ_c);
|
||
if (maxDist < dist)
|
||
{
|
||
maxDist = dist;
|
||
peakPtIdx = i;
|
||
}
|
||
}
|
||
}
|
||
double seg1_a, seg1_b, seg1_c;
|
||
compute2ptLine_2(
|
||
a_line[startId].pt3D.y, a_line[startId].pt3D.z,
|
||
a_line[peakPtIdx].pt3D.y, a_line[peakPtIdx].pt3D.z,
|
||
&seg1_a, &seg1_b, &seg1_c);
|
||
double seg2_a, seg2_b, seg2_c;
|
||
compute2ptLine_2(
|
||
a_line[peakPtIdx].pt3D.y, a_line[peakPtIdx].pt3D.z,
|
||
a_line[endId].pt3D.y, a_line[endId].pt3D.z,
|
||
&seg2_a, &seg2_b, &seg2_c);
|
||
|
||
//计算线上点到直线的距离
|
||
int invldPtNum = 0;
|
||
int vldPtNum = 0;
|
||
double meanZ = 0;
|
||
for (int i = 0; i < (int)a_line.size(); i++)
|
||
{
|
||
if (a_line[i].pt3D.z > 1e-4)
|
||
{
|
||
double dist;
|
||
if(i < peakPtIdx)
|
||
dist = computePtDistToLine(a_line[i].pt3D.y, a_line[i].pt3D.z, seg1_a, seg1_b, seg1_c);
|
||
else
|
||
dist = computePtDistToLine(a_line[i].pt3D.y, a_line[i].pt3D.z, seg2_a, seg2_b, seg2_c);
|
||
if(dist > innerPara_lineDeviTh)
|
||
invldPtNum++;
|
||
else
|
||
{
|
||
vldPtNum ++;
|
||
meanZ += a_line[i].pt3D.z;
|
||
}
|
||
}
|
||
}
|
||
|
||
if ( (invldPtNum > 10) || (vldPtNum < 10)) //滤噪
|
||
return;
|
||
|
||
meanZ = meanZ / vldPtNum;
|
||
|
||
SWD_segFeature a_seg;
|
||
a_seg.lineIdx = lineIdx;
|
||
a_seg.startPt = a_line[startId].pt3D;
|
||
a_seg.startPtIdx = startId;
|
||
a_seg.endPt = a_line[endId].pt3D;
|
||
a_seg.endPtIdx = endId;
|
||
a_seg.featureValue = meanZ;
|
||
if (weldStartLine.startSeg.lineIdx < 0)
|
||
{
|
||
weldStartLine.startSeg = a_seg;
|
||
weldStartLine.bufCounter = 1;
|
||
weldStartLine.curSegBuff[0] = a_seg;
|
||
}
|
||
else
|
||
{
|
||
bool validFlag = false;
|
||
for (int m = 0; m < weldStartLine.bufCounter; m++)
|
||
{
|
||
|
||
double zdiff1 = abs(a_seg.startPt.z - weldStartLine.curSegBuff[m].startPt.z);
|
||
double zdiff2 = abs(a_seg.endPt.z - weldStartLine.curSegBuff[m].endPt.z);
|
||
double dist1_YZ = sqrt(pow(a_seg.startPt.y - weldStartLine.curSegBuff[m].startPt.y, 2) + pow(a_seg.startPt.z - weldStartLine.curSegBuff[m].startPt.z, 2));
|
||
double dist2_YZ = sqrt(pow(a_seg.endPt.y - weldStartLine.curSegBuff[m].endPt.y, 2) + pow(a_seg.endPt.z - weldStartLine.curSegBuff[m].endPt.z, 2));
|
||
if ((zdiff1 < innerPara_endingZDeviTh) && (zdiff2 < innerPara_endingZDeviTh) &&
|
||
(dist1_YZ < innerPara_endingDistTh) && (dist2_YZ < innerPara_endingDistTh))
|
||
{
|
||
validFlag = true;
|
||
break;
|
||
}
|
||
}
|
||
if(true == validFlag)
|
||
{
|
||
if (weldStartLine.bufCounter < 5)
|
||
weldStartLine.bufCounter++;
|
||
|
||
weldStartLine.curSegBuff[4] = weldStartLine.curSegBuff[3];
|
||
weldStartLine.curSegBuff[3] = weldStartLine.curSegBuff[2];
|
||
weldStartLine.curSegBuff[2] = weldStartLine.curSegBuff[1];
|
||
weldStartLine.curSegBuff[1] = weldStartLine.curSegBuff[0];
|
||
weldStartLine.curSegBuff[0] = a_seg;
|
||
}
|
||
else
|
||
{
|
||
double zDiff = abs(weldStartLine.curSegBuff[0].featureValue - a_seg.featureValue);
|
||
double yLen_old = weldStartLine.curSegBuff[0].endPt.y - weldStartLine.curSegBuff[0].startPt.y;
|
||
double yLen_curr = a_seg.endPt.y - a_seg.startPt.y;
|
||
double lenDiff = abs(yLen_old - yLen_curr);
|
||
if (zDiff < innerPara_endingZDeviTh)
|
||
{
|
||
if (lenDiff < innerPara_endingDistTh)
|
||
{
|
||
if (weldStartLine.bufCounter < 5)
|
||
weldStartLine.bufCounter++;
|
||
|
||
weldStartLine.curSegBuff[4] = weldStartLine.curSegBuff[3];
|
||
weldStartLine.curSegBuff[3] = weldStartLine.curSegBuff[2];
|
||
weldStartLine.curSegBuff[2] = weldStartLine.curSegBuff[1];
|
||
weldStartLine.curSegBuff[1] = weldStartLine.curSegBuff[0];
|
||
weldStartLine.curSegBuff[0] = a_seg;
|
||
}
|
||
else if(yLen_old < yLen_curr)
|
||
{
|
||
weldStartLine.startSeg = a_seg;
|
||
weldStartLine.bufCounter = 1;
|
||
weldStartLine.curSegBuff[0] = a_seg;
|
||
}
|
||
}
|
||
else if (weldStartLine.curSegBuff[0].featureValue > a_seg.featureValue)
|
||
{
|
||
weldStartLine.startSeg = a_seg;
|
||
weldStartLine.bufCounter = 1;
|
||
weldStartLine.curSegBuff[0] = a_seg;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
//计算焊缝位置
|
||
void BQ_getWeldSeamPose(
|
||
std::vector<SSG_gapFeatureTree>& trees,
|
||
const SWD_segFeature& weldingStartLine,
|
||
const SWD_segFeature& weldingEndLine,
|
||
int weldRefPoints, //焊缝中间参考点位置
|
||
std::vector<SVzNL3DPoint>& weldOps, //焊缝位置
|
||
std::vector<SVzNL3DPosition>& weldRawPoints, //用于显示和debug
|
||
int* errCode)
|
||
{
|
||
*errCode = 0;
|
||
|
||
if (trees.size() == 0)
|
||
{
|
||
*errCode = SX_ERR_ZERO_OBJECTS;
|
||
return;
|
||
}
|
||
|
||
//保留最长的tree
|
||
int bestId = 0;
|
||
trees[0].treeState = 0;
|
||
for (int i = 1; i < (int)trees.size(); i++)
|
||
{
|
||
trees[i].treeState = 0;
|
||
if (trees[bestId].treeNodes.size() < trees[i].treeNodes.size())
|
||
bestId = i;
|
||
}
|
||
trees[bestId].treeState = 1; //已经被处理标志
|
||
|
||
//以最长的tree为基础，进行合并
|
||
std::vector<SSG_gapFeatureTree> weldSeam;
|
||
weldSeam.push_back(trees[bestId]);
|
||
while (1)
|
||
{
|
||
int oldSize = (int)weldSeam.size();
|
||
SVzNLRange lineRange = _getTreesLineRange(weldSeam);
|
||
//拟合直线
|
||
|
||
|
||
for (int i = 0; i < (int)trees.size(); i++)
|
||
{
|
||
if (trees[i].treeState > 0)
|
||
continue;
|
||
|
||
//检查能否被合并
|
||
|
||
|
||
}
|
||
if (oldSize == (int)weldSeam.size())
|
||
break;
|
||
}
|
||
|
||
//version 1.2.1 :确定起止点
|
||
//按照lineIdx排序
|
||
std::sort(weldSeam.begin(), weldSeam.end(), _compareByLineIdx);
|
||
const double parallelChkDistance = 15.0; //使用距离焊缝上下各15mm的平行线判断引弧板位置（判断连续性）
|
||
|
||
//根据weldingStartLine和weldingEndLine重新重新
|
||
SSG_gapFeatureTree validWeldSeam;
|
||
validWeldSeam.sLineIdx = -1;
|
||
validWeldSeam.eLineIdx = -1;
|
||
for (int i = 0; i < (int)weldSeam.size(); i++)
|
||
{
|
||
for (int j = 0; j < (int)weldSeam[i].treeNodes.size(); j++)
|
||
{
|
||
SSG_basicFeatureGap& a_node = weldSeam[i].treeNodes[j];
|
||
if ( (a_node.lineIdx >= weldingStartLine.lineIdx) && (a_node.lineIdx <= weldingEndLine.lineIdx))
|
||
{
|
||
validWeldSeam.treeNodes.push_back(a_node);
|
||
if (validWeldSeam.sLineIdx < 0)
|
||
{
|
||
validWeldSeam.sLineIdx = a_node.lineIdx;
|
||
validWeldSeam.eLineIdx = a_node.lineIdx;
|
||
}
|
||
else
|
||
validWeldSeam.eLineIdx = a_node.lineIdx;
|
||
}
|
||
}
|
||
}
|
||
|
||
//生成显示数据
|
||
for (int j = 0; j < (int)validWeldSeam.treeNodes.size(); j++)
|
||
{
|
||
SSG_basicFeatureGap& a_node = validWeldSeam.treeNodes[j];
|
||
SVzNL3DPosition a_debugPt;
|
||
a_debugPt.nPointIdx = (a_node.lineIdx << 16) | a_node.gapPeak.nPointIdx;
|
||
a_debugPt.pt3D = a_node.gapPeak.pt3D;
|
||
weldRawPoints.push_back(a_debugPt);
|
||
}
|
||
|
||
if (validWeldSeam.treeNodes.size() < 10)
|
||
{
|
||
*errCode = SX_ERR_FITTING_POINTS_LESS;
|
||
return;
|
||
}
|
||
|
||
//生成结果信息
|
||
//中点参考点的位置计算方法：以参考点的X为基准，左右各取段长的一半，进行直线拟合，根据X计算Y坐标
|
||
int refSegNum = weldRefPoints + 1;
|
||
refSegNum = refSegNum * 2; //以段长一半进行区分
|
||
std::vector<std::vector<SVzNL3DPoint>> refSegments;
|
||
std::vector<std::vector<SVzNL3DPoint>> refSegValleyPt;
|
||
refSegments.resize(refSegNum);
|
||
refSegValleyPt.resize(refSegNum);
|
||
//SVzNLRangeD xRange = _getTreesXRange(weldSeam);
|
||
SVzNLRangeD xRange = _getOneTreeXRange(validWeldSeam);
|
||
double segLen = (xRange.max - xRange.min) / refSegNum;
|
||
|
||
for (int j = 0; j < (int)validWeldSeam.treeNodes.size(); j++)
|
||
{
|
||
double x = validWeldSeam.treeNodes[j].gapPeak.pt3D.x;
|
||
double xoffset = x - xRange.min;
|
||
if (xoffset < 0)
|
||
xoffset = 0;
|
||
xoffset = xoffset / segLen;
|
||
int segIdx = (int)xoffset;
|
||
if (segIdx >= refSegNum)
|
||
segIdx = refSegNum - 1;
|
||
refSegments[segIdx].push_back(validWeldSeam.treeNodes[j].gapPeak.pt3D);
|
||
double valleyZ = (validWeldSeam.treeNodes[j].gapPt_0.pt3D.z + validWeldSeam.treeNodes[j].gapPt_1.pt3D.z) / 2;
|
||
double valleyX = (validWeldSeam.treeNodes[j].gapPt_0.pt3D.x + validWeldSeam.treeNodes[j].gapPt_1.pt3D.x) / 2;
|
||
double valleyY = (validWeldSeam.treeNodes[j].gapPt_0.pt3D.y + validWeldSeam.treeNodes[j].gapPt_1.pt3D.y) / 2;
|
||
SVzNL3DPoint a_valley = { valleyX , valleyY, valleyZ };
|
||
refSegValleyPt[segIdx].push_back(a_valley);
|
||
}
|
||
|
||
//分段按X大小排序
|
||
for (int i = 0; i < refSegNum; i++)
|
||
{
|
||
if (refSegments[i].size() > 0)
|
||
std::sort(refSegments[i].begin(), refSegments[i].end(), _compareByXValue);
|
||
}
|
||
//分段拟合直线
|
||
for (int i = 0; i <= weldRefPoints+1; i++)
|
||
{
|
||
int segIdx_0 = i * 2 - 1;
|
||
int segIdx_1 = i * 2;
|
||
std::vector<SVzNL3DPoint> fittingPoints;
|
||
std::vector<SVzNL3DPoint> fittingValleyPt;
|
||
if (segIdx_0 >= 0)
|
||
{
|
||
fittingPoints.insert(fittingPoints.end(), refSegments[segIdx_0].begin(), refSegments[segIdx_0].end()); //左半段
|
||
fittingValleyPt.insert(fittingValleyPt.end(), refSegValleyPt[segIdx_0].begin(), refSegValleyPt[segIdx_0].end());
|
||
}
|
||
if (segIdx_1 < refSegNum)
|
||
{
|
||
fittingPoints.insert(fittingPoints.end(), refSegments[segIdx_1].begin(), refSegments[segIdx_1].end()); //右半段
|
||
fittingValleyPt.insert(fittingValleyPt.end(), refSegValleyPt[segIdx_1].begin(), refSegValleyPt[segIdx_1].end());
|
||
}
|
||
while (fittingPoints.size() < 10) //保证足够的拟合点
|
||
{
|
||
//向两边扩展
|
||
int oldSize = (int)fittingPoints.size();
|
||
segIdx_0--;
|
||
if (segIdx_0 >= 0)
|
||
{
|
||
fittingPoints.insert(fittingPoints.end(), refSegments[segIdx_0].begin(), refSegments[segIdx_0].end());
|
||
fittingValleyPt.insert(fittingValleyPt.end(), refSegValleyPt[segIdx_0].begin(), refSegValleyPt[segIdx_0].end());
|
||
}
|
||
segIdx_1++;
|
||
if (segIdx_1 < refSegNum)
|
||
{
|
||
fittingPoints.insert(fittingPoints.end(), refSegments[segIdx_1].begin(), refSegments[segIdx_1].end());
|
||
fittingValleyPt.insert(fittingValleyPt.end(), refSegValleyPt[segIdx_1].begin(), refSegValleyPt[segIdx_1].end());
|
||
}
|
||
if (oldSize == (int)fittingPoints.size())
|
||
break;
|
||
}
|
||
//直线拟合
|
||
double _a, _b, _c;
|
||
lineFitting_abc(fittingPoints, &_a, &_b, &_c);
|
||
double x,y;
|
||
if (i == 0)
|
||
{
|
||
//计算开始扫描线方程
|
||
double startLineXY_a, startLineXY_b, startLineXY_c;
|
||
compute2ptLine_2(
|
||
weldingStartLine.startPt.x, weldingStartLine.startPt.y,
|
||
weldingStartLine.endPt.x, weldingStartLine.endPt.y,
|
||
&startLineXY_a, &startLineXY_b, &startLineXY_c);
|
||
//计算两条直线交点
|
||
SVzNL3DPoint crossPt = computeLineCrossPt_abs(startLineXY_a, startLineXY_b, startLineXY_c, _a, _b, _c);
|
||
x = crossPt.x;
|
||
y = crossPt.y;
|
||
}
|
||
else if (i == (weldRefPoints + 1))
|
||
{
|
||
//计算开始扫描线方程
|
||
double endLineXY_a, endLineXY_b, endLineXY_c;
|
||
compute2ptLine_2(
|
||
weldingEndLine.startPt.x, weldingEndLine.startPt.y,
|
||
weldingEndLine.endPt.x, weldingEndLine.endPt.y,
|
||
&endLineXY_a, &endLineXY_b, &endLineXY_c);
|
||
//计算两条直线交点
|
||
SVzNL3DPoint crossPt = computeLineCrossPt_abs(endLineXY_a, endLineXY_b, endLineXY_c, _a, _b, _c);
|
||
x = crossPt.x;
|
||
y = crossPt.y;
|
||
}
|
||
else
|
||
{
|
||
x = segLen * i * 2.0 + xRange.min;
|
||
y = -(_a * x + _c) / _b;
|
||
}
|
||
double nearestZ = _getNearestZ(fittingValleyPt, x); //_getMeanZ(fittingPtZ);
|
||
SVzNL3DPoint a_pt;
|
||
a_pt.x = x;
|
||
a_pt.y = y;
|
||
a_pt.z = nearestZ;
|
||
weldOps.push_back(a_pt);
|
||
}
|
||
|
||
|
||
return;
|
||
} |