jerryzeng/algoLib
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
algoLib/sourceCode/SX_lapWeldDetection.cpp

586 lines
17 KiB
C++
Raw Normal View History

搭接焊缝算法初次提交
2025-09-10 23:59:05 +08:00
#include <vector>
#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include "SX_lapWeldDetection_Export.h"
#include <opencv2/opencv.hpp>
#include <limits>
SX_lapWeldDetection version 1.0.0 :焊缝（平缝）识别检测算法初始发布
2026-08-13 16:22:03 +08:00
//version 1.0.0 :<3A><><EFBFBD>죨ƽ<ECA3A8>죩ʶ<ECA3A9><CAB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㷨<EFBFBD><E3B7A8>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>
std::string m_strVersion = "weldSeamAlgo_1.0.0";
const char* wd_weldSeamAlgoVersion(void)
{
return m_strVersion.c_str();
}
搭接焊缝算法初次提交
2025-09-10 23:59:05 +08:00
//<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>п<EFBFBD><D0BF><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD>Ͳο<CDB2><CEBF><EFBFBD>ƽƽ<C6BD>棬<EFBFBD><E6A3AC><EFBFBD><EFBFBD><EFBFBD>ߵ<EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD>ƽ
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD>淨<EFBFBD><E6B7A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>
SSG_planeCalibPara sx_getBaseCalibPara(
std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
return sg_getPlaneCalibPara2(scanLines);
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void sx_lineDataR(
std::vector< SVzNL3DPosition>& a_line,
const double* camPoseR,
double groundH)
{
lineDataRT_vector(a_line, camPoseR, groundH);
}
搭接焊缝检测算法提交
2025-09-11 19:37:22 +08:00
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;
}
//<2F>Ӻ<EFBFBD><D3BA><EFBFBD><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>յ<EFBFBD><D5B5><EFBFBD>Ϣ
void getWeldPoint(std::vector<SVzNL3DPoint>& a_weld_contour, int midPtNum, std::vector<SVzNL3DPoint>& a_weld)
{
if (a_weld_contour.size() == 0)
return;
//<2F><><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1>
double k, b;
lineFitting(a_weld_contour, &k, &b);
//Ѱ<><D1B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>յ<EFBFBD>
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);
}
SX_lapWeldDetection version 1.0.0 :焊缝（平缝）识别检测算法初始发布
2026-08-13 16:22:03 +08:00
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 _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;
}
搭接焊缝算法初次提交
2025-09-10 23:59:05 +08:00
//<2F><>ȡ<EFBFBD><C8A1><EFBFBD>Ӻ<EFBFBD><D3BA><EFBFBD>
void sx_getLapWeldPostion(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SSG_cornerParam cornerPara,
SSG_treeGrowParam growParam,
SSX_lapWeldParam lapWeldParam,
搭接焊缝检测算法提交
2025-09-11 19:37:22 +08:00
SSG_planeCalibPara groundCalibPara,
搭接焊缝算法初次提交
2025-09-10 23:59:05 +08:00
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;
搭接焊缝检测算法提交
2025-09-11 19:37:22 +08:00
//<2F><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ
搭接焊缝算法初次提交
2025-09-10 23:59:05 +08:00
std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_v;
搭接焊缝检测算法提交
2025-09-11 19:37:22 +08:00
if ((keSX_ScanMode_V == lapWeldParam.scanMode) || (keSX_ScanMode_Both == lapWeldParam.scanMode))
搭接焊缝算法初次提交
2025-09-10 23:59:05 +08:00
{
搭接焊缝检测算法提交
2025-09-11 19:37:22 +08:00
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();
YouJiang_sheetPositioning v1.0.0 初始版本
2026-03-01 21:16:05 +08:00
sg_getLineZJumpFeature_cornerMethod(
搭接焊缝检测算法提交
2025-09-11 19:37:22 +08:00
&scanLines[line][0],
dataSize,
line,
YouJiang_sheetPositioning v1.0.0 初始版本
2026-03-01 21:16:05 +08:00
cornerPara,
false, //ȡ<><C8A1><EFBFBD><EFBFBD>
搭接焊缝检测算法提交
2025-09-11 19:37:22 +08:00
a_line_features);
//<2F>˳<EFBFBD><CBB3><EFBFBD><EFBFBD><EFBFBD>
std::vector<SSG_basicFeature1D> vld_features;
for (int i = 0, i_max = a_line_features.size(); i < i_max; i++)
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>γɵ<CEB3><C9B5><EFBFBD><EFBFBD><EFBFBD>ȥ<EFBFBD><C8A5>
if (a_line_features[i].featureValue < (groundCalibPara.planeHeight - 0.25))
{
vld_features.push_back(a_line_features[i]);
}
}
jumpFeatures_v.push_back(vld_features);
}
搭接焊缝算法初次提交
2025-09-10 23:59:05 +08:00
}
if (false == isGridData)//<2F><><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ
{
*errCode = SG_ERR_NOT_GRID_FORMAT;
return;
}
//<2F><><EFBFBD><EFBFBD>ˮƽɨ<C6BD><C9A8>
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; //<2F><>ԭʼ<D4AD><CABC><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>ʹ<EFBFBD>ã<EFBFBD>
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<72><63><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ
std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_h;
int lineNum_h = (int)hLines.size();
搭接焊缝检测算法提交
2025-09-11 19:37:22 +08:00
if ((keSX_ScanMode_H == lapWeldParam.scanMode) || (keSX_ScanMode_Both == lapWeldParam.scanMode))
搭接焊缝算法初次提交
2025-09-10 23:59:05 +08:00
{
搭接焊缝检测算法提交
2025-09-11 19:37:22 +08:00
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();
YouJiang_sheetPositioning v1.0.0 初始版本
2026-03-01 21:16:05 +08:00
sg_getLineZJumpFeature_cornerMethod(
搭接焊缝检测算法提交
2025-09-11 19:37:22 +08:00
&hLines[line][0],
dataSize,
line,
YouJiang_sheetPositioning v1.0.0 初始版本
2026-03-01 21:16:05 +08:00
cornerPara,
false,//ȡ<><C8A1><EFBFBD><EFBFBD>
搭接焊缝检测算法提交
2025-09-11 19:37:22 +08:00
a_line_features);
//<2F>˳<EFBFBD><CBB3><EFBFBD><EFBFBD><EFBFBD>
std::vector<SSG_basicFeature1D> vld_features;
for (int i = 0, i_max = a_line_features.size(); i < i_max; i++)
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>γɵ<CEB3><C9B5><EFBFBD><EFBFBD><EFBFBD>ȥ<EFBFBD><C8A5>
if (a_line_features[i].featureValue < (groundCalibPara.planeHeight - 0.25))
{
vld_features.push_back(a_line_features[i]);
}
}
jumpFeatures_h.push_back(vld_features);
}
搭接焊缝算法初次提交
2025-09-10 23:59:05 +08:00
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߷<EFBFBD><DFB7><EFBFBD><EFBFBD><EFBFBD>
std::vector<SSG_featureTree> v_trees;
搭接焊缝检测算法提交
2025-09-11 19:37:22 +08:00
if ((keSX_ScanMode_V == lapWeldParam.scanMode) || (keSX_ScanMode_Both == lapWeldParam.scanMode))
搭接焊缝算法初次提交
2025-09-10 23:59:05 +08:00
{
搭接焊缝检测算法提交
2025-09-11 19:37:22 +08:00
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);
}
搭接焊缝算法初次提交
2025-09-10 23:59:05 +08:00
}
//ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
std::vector<SSG_featureTree> h_trees;
搭接焊缝检测算法提交
2025-09-11 19:37:22 +08:00
if ((keSX_ScanMode_H == lapWeldParam.scanMode) || (keSX_ScanMode_Both == lapWeldParam.scanMode))
搭接焊缝算法初次提交
2025-09-10 23:59:05 +08:00
{
搭接焊缝检测算法提交
2025-09-11 19:37:22 +08:00
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);
}
搭接焊缝算法初次提交
2025-09-10 23:59:05 +08:00
}
//tree<65><65>Ϣ
std::vector<SSG_treeInfo> allTreesInfo; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߽<EFBFBD>
SSG_treeInfo a_nullTree;
memset(&a_nullTree, 0, sizeof(SSG_treeInfo));
allTreesInfo.push_back(a_nullTree); //<2F><><EFBFBD>ִ洢λ<E6B4A2><CEBB><EFBFBD><EFBFBD>treeIdx<64><78>ͬλ<CDAC>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD>ǣ<EFBFBD><C7A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>б<EFBFBD>ע
int hvTreeIdx = 1;
for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++)
{
SSG_featureTree* a_vTree = &v_trees[i];
//<2F><>¼Tree<65><65><EFBFBD><EFBFBD>Ϣ
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);
搭接焊缝检测算法提交
2025-09-11 19:37:22 +08:00
std::vector<SVzNL3DPoint> a_weld_contour;
搭接焊缝算法初次提交
2025-09-10 23:59:05 +08:00
//<2F><>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD><CFB1>ǣ<EFBFBD>ͬʱ<CDAC><CAB1>Mask<73>ϱ<EFBFBD><CFB1><EFBFBD>
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)//<2F><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>˺<EFBFBD><CBBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>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;
}
搭接焊缝检测算法提交
2025-09-11 19:37:22 +08:00
a_weld_contour.push_back(scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D);
搭接焊缝算法初次提交
2025-09-10 23:59:05 +08:00
}
}
搭接焊缝检测算法提交
2025-09-11 19:37:22 +08:00
std::vector<SVzNL3DPoint> a_weld;
getWeldPoint(a_weld_contour, lapWeldParam.weldRefPoints, a_weld);
if (a_weld.size() > 0)
objOps.push_back(a_weld);
搭接焊缝算法初次提交
2025-09-10 23:59:05 +08:00
hvTreeIdx++;
}
int hTreeStart = hvTreeIdx;
////<2F><>ע:ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD>
for (int i = 0, i_max = (int)h_trees.size(); i < i_max; i++)
{
SSG_featureTree* a_hTree = &h_trees[i];
//<2F><>¼Tree<65><65><EFBFBD><EFBFBD>Ϣ
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; //ˮƽɨ<C6BD><C9A8>xy<78>ǽ<EFBFBD><C7BD><EFBFBD><EFBFBD><EFBFBD>
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);
搭接焊缝检测算法提交
2025-09-11 19:37:22 +08:00
std::vector<SVzNL3DPoint> a_weld_contour;
搭接焊缝算法初次提交
2025-09-10 23:59:05 +08:00
//<2F><>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD><CFB1>ǣ<EFBFBD>ͬʱ<CDAC><CAB1>Mask<73>ϱ<EFBFBD><CFB1><EFBFBD>
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)//<2F><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>˺<EFBFBD><CBBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>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;
}
搭接焊缝检测算法提交
2025-09-11 19:37:22 +08:00
a_weld_contour.push_back(scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D);
搭接焊缝算法初次提交
2025-09-10 23:59:05 +08:00
}
}
搭接焊缝检测算法提交
2025-09-11 19:37:22 +08:00
std::vector<SVzNL3DPoint> a_weld;
getWeldPoint(a_weld_contour, lapWeldParam.weldRefPoints, a_weld);
if (a_weld.size() > 0)
objOps.push_back(a_weld);
搭接焊缝算法初次提交
2025-09-10 23:59:05 +08:00
hvTreeIdx++;
}
int hvTreeSize = hvTreeIdx;
搭接焊缝检测算法提交
2025-09-11 19:37:22 +08:00
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ͷ<EFBFBD><CDB6><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD>Ա<EFBFBD><D4B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>۱궨<DBB1><EAB6A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȷ
for (int i = 0; i < lineNum; i++)
sx_lineDataR(scanLines[i], groundCalibPara.invRMatrix, -1);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ͷ<EFBFBD><CDB6><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ
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;
}
}
搭接焊缝算法初次提交
2025-09-10 23:59:05 +08:00
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
SX_lapWeldDetection version 1.0.0 :焊缝（平缝）识别检测算法初始发布
2026-08-13 16:22:03 +08:00
}
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;
}
bool _compareByXValue(const SVzNL3DPoint& a, const SVzNL3DPoint& b) {
return a.x < b.x;
}
//<2F><><EFBFBD>㺸<EFBFBD><E3BAB8>λ<EFBFBD><CEBB>
void BQ_getWeldSeamPose(
std::vector<SSG_gapFeatureTree>& trees,
int weldRefPoints, //<2F><><EFBFBD><EFBFBD><EFBFBD>м<EFBFBD><D0BC>ο<EFBFBD><CEBF><EFBFBD>λ<EFBFBD><CEBB>
std::vector<SVzNL3DPoint>& weldOps, //<2F><><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
std::vector<SVzNL3DPosition>& weldRawPoints, //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾ<EFBFBD><CABE>debug
int* errCode)
{
*errCode = 0;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EEB3A4>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; //<2F>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־
//<2F><><EFBFBD><EFBFBD><EEB3A4>treeΪ<65><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>кϲ<D0BA>
std::vector<SSG_gapFeatureTree> weldSeam;
weldSeam.push_back(trees[bestId]);
while (1)
{
int oldSize = (int)weldSeam.size();
SVzNLRange lineRange = _getTreesLineRange(weldSeam);
//<2F><><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1>
for (int i = 0; i < (int)trees.size(); i++)
{
if (trees[i].treeState > 0)
continue;
//<2F><><EFBFBD><EFBFBD><EFBFBD>ܷ񱻺ϲ<F1B1BBBA>
}
if (oldSize == (int)weldSeam.size())
break;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾ<EFBFBD><CABE><EFBFBD><EFBFBD>
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];
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);
}
}
int nodeSize = 0;
for(int i = 0; i < (int)weldSeam.size(); i ++)
nodeSize += weldSeam[i].treeNodes.size();
if (nodeSize < 10)
{
*errCode = SX_ERR_FITTING_POINTS_LESS;
return;
}
//<2F><><EFBFBD>ɽ<EFBFBD><C9BD><EFBFBD><EFBFBD><EFBFBD>Ϣ
//<2F>е<EFBFBD><D0B5>ο<EFBFBD><CEBF><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>ü<EFBFBD><C3BC>㷽<EFBFBD><E3B7BD><EFBFBD><EFBFBD><EFBFBD>Բο<D4B2><CEBF><EFBFBD><EFBFBD><EFBFBD>XΪ<58><CEAA>׼<EFBFBD><D7BC><EFBFBD><EFBFBD><EFBFBD>Ҹ<EFBFBD>ȡ<EFBFBD>γ<EFBFBD><CEB3><EFBFBD>һ<EFBFBD>룬<EFBFBD><EBA3AC><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD>ϣ<EFBFBD><CFA3><EFBFBD><EFBFBD><EFBFBD>X<EFBFBD><58><EFBFBD><EFBFBD>Y<EFBFBD><59><EFBFBD><EFBFBD>
int refSegNum = weldRefPoints + 1;
refSegNum = refSegNum * 2; //<2F>Զγ<D4B6>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
std::vector<std::vector<SVzNL3DPoint>> refSegments;
std::vector<std::vector<double>> refSegZ;
refSegments.resize(refSegNum);
refSegZ.resize(refSegNum);
SVzNLRangeD xRange = _getTreesXRange(weldSeam);
double segLen = (xRange.max - xRange.min) / refSegNum;
for (int i = 0; i < (int)weldSeam.size(); i++)
{
for (int j = 0; j < (int)weldSeam[i].treeNodes.size(); j++)
{
double x = weldSeam[i].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(weldSeam[i].treeNodes[j].gapPeak.pt3D);
double valleyZ = (weldSeam[i].treeNodes[j].gapPt_0.pt3D.z + weldSeam[i].treeNodes[j].gapPt_1.pt3D.z) / 2;
refSegZ[segIdx].push_back(valleyZ);
}
}
//<2F>ֶΰ<D6B6>X<EFBFBD><58>С<EFBFBD><D0A1><EFBFBD><EFBFBD>
for (int i = 0; i < refSegNum; i++)
{
if (refSegments[i].size() > 0)
std::sort(refSegments[i].begin(), refSegments[i].end(), _compareByXValue);
}
//<2F>ֶ<EFBFBD><D6B6><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1>
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<double> fittingPtZ;
if (segIdx_0 >= 0)
{
fittingPoints.insert(fittingPoints.end(), refSegments[segIdx_0].begin(), refSegments[segIdx_0].end()); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
fittingPtZ.insert(fittingPtZ.end(), refSegZ[segIdx_0].begin(), refSegZ[segIdx_0].end());
}
if (segIdx_1 < refSegNum)
{
fittingPoints.insert(fittingPoints.end(), refSegments[segIdx_1].begin(), refSegments[segIdx_1].end()); //<2F>Ұ<EFBFBD><D2B0><EFBFBD>
fittingPtZ.insert(fittingPtZ.end(), refSegZ[segIdx_1].begin(), refSegZ[segIdx_1].end());
}
while (fittingPoints.size() < 10) //<2F><>֤<EFBFBD>㹻<EFBFBD><E3B9BB><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>չ
int oldSize = (int)fittingPoints.size();
segIdx_0--;
if (segIdx_0 >= 0)
{
fittingPoints.insert(fittingPoints.end(), refSegments[segIdx_0].begin(), refSegments[segIdx_0].end());
fittingPtZ.insert(fittingPtZ.end(), refSegZ[segIdx_0].begin(), refSegZ[segIdx_0].end());
}
segIdx_1++;
if (segIdx_1 < refSegNum)
{
fittingPoints.insert(fittingPoints.end(), refSegments[segIdx_1].begin(), refSegments[segIdx_1].end());
fittingPtZ.insert(fittingPtZ.end(), refSegZ[segIdx_1].begin(), refSegZ[segIdx_1].end());
}
if (oldSize == (int)fittingPoints.size())
break;
}
//ֱ<><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double _a, _b, _c;
lineFitting_abc(fittingPoints, &_a, &_b, &_c);
double meanZ = _getMeanZ(fittingPtZ);
double x = segLen * i * 2.0 + xRange.min;
double y = -(_a * x + _c) / _b;
SVzNL3DPoint a_pt;
a_pt.x = x;
a_pt.y = y;
a_pt.z = meanZ;
weldOps.push_back(a_pt);
}
return;
搭接焊缝算法初次提交
2025-09-10 23:59:05 +08:00
}
Reference in New Issue Copy Permalink