SX_lapWeldDetection
version 1.0.0 :焊缝（平缝）识别检测算法初始发布
This commit is contained in:
parent
8354c4163c
commit
f4a25aae11
@ -164,11 +164,13 @@
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemGroup>
|
||||
<ClInclude Include="..\sourceCode\WD_convolveGauss.h" />
|
||||
<ClInclude Include="..\sourceCode\SG_baseAlgo_Export.h" />
|
||||
<ClInclude Include="..\sourceCode\SG_baseDataType.h" />
|
||||
<ClInclude Include="..\sourceCode\SG_errCode.h" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ClCompile Include="..\sourceCode\WD_convolveGauss.cpp" />
|
||||
<ClCompile Include="..\sourceCode\dataFitting.cpp">
|
||||
<AdditionalOptions Condition="'$(Configuration)|$(Platform)'=='Release|x64'">/bigobj %(AdditionalOptions)</AdditionalOptions>
|
||||
<AdditionalOptions Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">/bigobj %(AdditionalOptions)</AdditionalOptions>
|
||||
|
||||
@ -7,6 +7,7 @@
|
||||
#include <VZNL_Types.h>
|
||||
#include "direct.h"
|
||||
#include <string>
|
||||
#include "SG_baseAlgo_Export.h"
|
||||
#include "SX_lapWeldDetection_Export.h"
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <Windows.h>
|
||||
@ -206,7 +207,7 @@ void _outputRGBDScanLapWeld_RGBD(
|
||||
}
|
||||
else if (LINE_FEATURE_L_JUMP_L2H == featureType_v)
|
||||
{
|
||||
rgb = objColor[7];
|
||||
rgb = { 0, 250, 0 };
|
||||
size = 5;
|
||||
}
|
||||
else if (LINE_FEATURE_L_JUMP_H2L == featureType_h)
|
||||
@ -271,26 +272,36 @@ void _outputRGBDScanLapWeld_RGBD(
|
||||
double exLen = 5.0;
|
||||
rgb = objColor[0];
|
||||
size = 3;
|
||||
int polyIndx = 0;
|
||||
for (int i = 0; i < weldNum; i++)
|
||||
{
|
||||
SVzNL3DPoint pt0 = weldOPs[i][0];
|
||||
SVzNL3DPoint pt1 = weldOPs[i].back();
|
||||
double len = sqrt(pow(pt0.x - pt1.x, 2) + pow(pt0.y - pt1.y, 2));
|
||||
double k = exLen / len;
|
||||
sw << "Poly_" << i << "_2" << std::endl;
|
||||
double x = -k * (pt1.x - pt0.x) + pt0.x;
|
||||
double y = -k * (pt1.y - pt0.y) + pt0.y;
|
||||
double z = -k * (pt1.z - pt0.z) + pt0.z;
|
||||
sw << "{" << (float)x << "," << (float)y << "," << (float)z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
for (int j = 0; j < (int)weldOPs[i].size()-1; j++)
|
||||
{
|
||||
SVzNL3DPoint pt0 = weldOPs[i][j];
|
||||
SVzNL3DPoint pt1 = weldOPs[i][j +1];
|
||||
double len = sqrt(pow(pt0.x - pt1.x, 2) + pow(pt0.y - pt1.y, 2));
|
||||
double k1 = exLen / len;
|
||||
if (j > 0)
|
||||
k1 = 0;
|
||||
sw << "Poly_" << polyIndx << "_2" << std::endl;
|
||||
polyIndx++;
|
||||
double x = -k1 * (pt1.x - pt0.x) + pt0.x;
|
||||
double y = -k1 * (pt1.y - pt0.y) + pt0.y;
|
||||
double z = -k1 * (pt1.z - pt0.z) + pt0.z;
|
||||
sw << "{" << (float)x << "," << (float)y << "," << (float)z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
|
||||
x = -k * (pt0.x - pt1.x) + pt1.x;
|
||||
y = -k * (pt0.y - pt1.y) + pt1.y;
|
||||
z = -k * (pt0.z - pt1.z) + pt1.z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
double k2 = exLen / len;
|
||||
if (j < (int)weldOPs[i].size() - 2)
|
||||
k2 = 0;
|
||||
x = -k2 * (pt0.x - pt1.x) + pt1.x;
|
||||
y = -k2 * (pt0.y - pt1.y) + pt1.y;
|
||||
z = -k2 * (pt0.z - pt1.z) + pt1.z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
sw.close();
|
||||
@ -696,11 +707,48 @@ void _convertToGridData(std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
return;
|
||||
}
|
||||
|
||||
int counterLinePtNum_XYZ(std::vector< SVzNL3DPosition>& lineData)
|
||||
{
|
||||
int num = 0;
|
||||
for (int i = 0; i < (int)lineData.size(); i++)
|
||||
{
|
||||
if (lineData[i].pt3D.z > 1e-4)
|
||||
num++;
|
||||
}
|
||||
return num;
|
||||
}
|
||||
|
||||
void _removeZeroLines_XYZ(std::vector<std::vector< SVzNL3DPosition>>& scanData)
|
||||
{
|
||||
int lineNum = (int)scanData.size();
|
||||
int firstLine = -1;
|
||||
int lastLine = 0;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
int num = counterLinePtNum_XYZ(scanData[line]);
|
||||
if ((num > 0) && (firstLine < 0))
|
||||
firstLine = line;
|
||||
if (num > 0)
|
||||
lastLine = line;
|
||||
}
|
||||
if (firstLine < 0)
|
||||
{
|
||||
scanData.clear();
|
||||
return;
|
||||
}
|
||||
|
||||
if ((lastLine < (lineNum - 1)) && (lastLine > 0))
|
||||
scanData.erase(scanData.begin() + lastLine + 1, scanData.end());
|
||||
|
||||
scanData.erase(scanData.begin(), scanData.begin() + firstLine);
|
||||
return;
|
||||
}
|
||||
|
||||
#define CONVERT_TO_GRID 0
|
||||
#define TEST_COMPUTE_CALIB_PARA 0
|
||||
#define TEST_COMPUTE_POSITION 1
|
||||
#define TEST_GROUP 2
|
||||
int main()
|
||||
void lapWeldTest()
|
||||
{
|
||||
const char* dataPath[TEST_GROUP] = {
|
||||
"F:\\ShangGu\\项目\\钢板搭接焊缝检测\\数据\\", //0
|
||||
@ -866,13 +914,239 @@ int main()
|
||||
#endif
|
||||
}
|
||||
|
||||
// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单
|
||||
// 调试程序: F5 或调试 >“开始调试”菜单
|
||||
#define TEST_BQ_CALIB_PARA 0
|
||||
#define TEST_BQ_WELD_SEAM 1
|
||||
#define BQ_TEST_GROUP 1
|
||||
void BQ_thinWeldSeamTest()
|
||||
{
|
||||
const char* dataPath[BQ_TEST_GROUP] = {
|
||||
"F:/ShangGu/项目/冠钦_博清科技/焊缝识别/数据/", //0
|
||||
};
|
||||
|
||||
// 入门使用技巧:
|
||||
// 1. 使用解决方案资源管理器窗口添加/管理文件
|
||||
// 2. 使用团队资源管理器窗口连接到源代码管理
|
||||
// 3. 使用输出窗口查看生成输出和其他消息
|
||||
// 4. 使用错误列表窗口查看错误
|
||||
// 5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目
|
||||
// 6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件
|
||||
SVzNLRange fileIdx[BQ_TEST_GROUP] = {
|
||||
{1,4},
|
||||
};
|
||||
|
||||
|
||||
const char* ver = wd_weldSeamAlgoVersion();
|
||||
printf("ver :%s\n", ver);
|
||||
|
||||
#if TEST_BQ_CALIB_PARA
|
||||
char _calib_datafile[256];
|
||||
sprintf_s(_calib_datafile, "F:/ShangGu/项目/冠钦_博清科技/焊缝识别/数据/1_LaserData_Jl26C077.txt");
|
||||
int lineNum = 0;
|
||||
float lineV = 0.0f;
|
||||
int dataCalib = 0;
|
||||
int maxTimeStamp = 0;
|
||||
int clockPerSecond = 0;
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanData;
|
||||
vzReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanData);
|
||||
_removeZeroLines_XYZ(scanData);
|
||||
|
||||
//高度过滤，保留顶板
|
||||
double hTh = 490.0;
|
||||
for (int line = 0; line < (int)scanData.size(); line++)
|
||||
{
|
||||
for (int j = 0; j < (int)scanData[line].size(); j++)
|
||||
{
|
||||
if (scanData[line][j].pt3D.z > hTh)
|
||||
scanData[line][j].pt3D = { 0.0, 0.0, 0.0 };
|
||||
}
|
||||
}
|
||||
|
||||
lineNum = (int)scanData.size();
|
||||
if (scanData.size() > 0)
|
||||
{
|
||||
SSG_planeCalibPara calibPara = sx_getBaseCalibPara(scanData);
|
||||
//结果进行验证
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{
|
||||
if (i == 14)
|
||||
int kkk = 1;
|
||||
//行处理
|
||||
//调平，去除地面
|
||||
sx_lineDataR(scanData[i], calibPara.planeCalib, -1); // calibPara.planeHeight);
|
||||
}
|
||||
char calibFile[250];
|
||||
sprintf_s(calibFile, "F:/ShangGu/项目/冠钦_博清科技/焊缝识别/数据/ground_calib_para.txt");
|
||||
_outputCalibPara(calibFile, calibPara);
|
||||
char _out_file[256];
|
||||
sprintf_s(_out_file, "F:/ShangGu/项目/冠钦_博清科技/焊缝识别/数据/scanData_ground_1_calib.txt");
|
||||
int headNullLines = 0;
|
||||
_outputScanDataFile_vector(_out_file, scanData, false, &headNullLines);
|
||||
printf("%s: calib done!\n", _calib_datafile);
|
||||
#if 1
|
||||
for (int grp = 0; grp < 1; grp++)
|
||||
{
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
char _scan_file[256];
|
||||
sprintf_s(_scan_file, "%s%d_LaserData_Jl26C077.txt", dataPath[grp], fidx);
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanLines;
|
||||
vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
|
||||
_removeZeroLines_XYZ(scanLines);
|
||||
if (scanLines.size() == 0)
|
||||
continue;
|
||||
lineNum = (int)scanLines.size();
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
sx_lineDataR(scanLines[i], calibPara.planeCalib, -1);//调平，去除地面
|
||||
sprintf_s(_scan_file, "%s%d_ground_calibrated.txt", dataPath[grp], fidx);
|
||||
int headNullLines = 0;
|
||||
_outputScanDataFile_vector(_scan_file, scanLines, false, &headNullLines);
|
||||
printf("%s: calib done!\n", _scan_file);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
}
|
||||
#endif
|
||||
|
||||
#if TEST_BQ_WELD_SEAM
|
||||
//for (int group = 0; group < TEST_GROUPS; group++)
|
||||
for (int grp = 0; grp < BQ_TEST_GROUP; grp++)
|
||||
{
|
||||
SSG_planeCalibPara groundCalibPara;
|
||||
//初始化成单位阵
|
||||
groundCalibPara.planeCalib[0] = 1.0;
|
||||
groundCalibPara.planeCalib[1] = 0.0;
|
||||
groundCalibPara.planeCalib[2] = 0.0;
|
||||
groundCalibPara.planeCalib[3] = 0.0;
|
||||
groundCalibPara.planeCalib[4] = 1.0;
|
||||
groundCalibPara.planeCalib[5] = 0.0;
|
||||
groundCalibPara.planeCalib[6] = 0.0;
|
||||
groundCalibPara.planeCalib[7] = 0.0;
|
||||
groundCalibPara.planeCalib[8] = 1.0;
|
||||
groundCalibPara.planeHeight = -1.0;
|
||||
for (int i = 0; i < 9; i++)
|
||||
groundCalibPara.invRMatrix[i] = groundCalibPara.planeCalib[i];
|
||||
char calibFile[250];
|
||||
sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]);
|
||||
groundCalibPara = _readCalibPara(calibFile);
|
||||
|
||||
double para_chkWidth_wide = 2.5f;
|
||||
|
||||
SSG_cornerParam cornerPara;
|
||||
SVzNLRangeD gapParam;
|
||||
|
||||
SWD_valleyParam valleyParam;
|
||||
valleyParam.chkWin_width = 2.5; //mm, 检测2.5mm宽的V型
|
||||
valleyParam.maxWidth = 20.0; //最大宽度20mm，用于过滤虚假信号
|
||||
valleyParam.minHeight = 1.5;
|
||||
|
||||
SSG_treeGrowParam growParam;
|
||||
growParam.maxLineSkipNum = 50;
|
||||
growParam.yDeviation_max = 1.0;
|
||||
growParam.maxSkipDistance = 30.0;
|
||||
growParam.zDeviation_max = 5.0;//
|
||||
growParam.minLTypeTreeLen = 50; //mm,
|
||||
growParam.minVTypeTreeLen = 50; //mm
|
||||
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
char _scan_file[256];
|
||||
sprintf_s(_scan_file, "%s%d_LaserData_Jl26C077.txt", dataPath[grp], fidx);
|
||||
printf("process: %s......\n", _scan_file);
|
||||
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanLines;
|
||||
vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
|
||||
_removeZeroLines_XYZ(scanLines);
|
||||
if (scanLines.size() == 0)
|
||||
continue;
|
||||
|
||||
//统计时间
|
||||
long t1 = (long)GetTickCount64();//统计时间
|
||||
|
||||
std::vector<SSG_gapFeatureTree> trees;
|
||||
for (int i = 0; i < (int)scanLines.size(); i++)
|
||||
{
|
||||
std::vector<SVzNL3DPosition>& a_line = scanLines[i];
|
||||
std::vector< SSG_basicFeatureGap> line_valleys;
|
||||
sx_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1);//调平，去除地面
|
||||
#if 0
|
||||
sg_getLineGapFeature(
|
||||
a_line, //扫描线
|
||||
i, //当前扫描线序号
|
||||
cornerPara,
|
||||
gapParam,
|
||||
line_valleys);
|
||||
#else
|
||||
wd_getLineVFlaws(
|
||||
a_line,
|
||||
i,
|
||||
valleyParam,
|
||||
line_valleys);
|
||||
|
||||
#endif
|
||||
bool isLastLine = i == ((int)scanLines.size() - 1) ? true : false;
|
||||
sg_lineGapsGrowing(
|
||||
i,
|
||||
isLastLine,
|
||||
line_valleys,
|
||||
trees,
|
||||
growParam);
|
||||
}
|
||||
|
||||
int weldRefPoints = 3;
|
||||
int errCode = 0;
|
||||
std::vector<SVzNL3DPoint> weldOps;
|
||||
std::vector<SVzNL3DPosition> weldRawPoints;
|
||||
BQ_getWeldSeamPose(
|
||||
trees,
|
||||
weldRefPoints, //焊缝中间参考点位置
|
||||
weldOps, //焊缝位置
|
||||
weldRawPoints,
|
||||
&errCode);
|
||||
|
||||
long t2 = (long)GetTickCount64();
|
||||
printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1));
|
||||
|
||||
//生成debug信息
|
||||
for (int line = 0; line < (int)scanLines.size(); line++)
|
||||
{
|
||||
for (int j = 0; j < (int)scanLines[line].size(); j++)
|
||||
scanLines[line][j].nPointIdx = 0;
|
||||
}
|
||||
for (int i = 0; i < (int)trees.size(); i++)
|
||||
{
|
||||
for (int j = 0; j < (int)trees[i].treeNodes.size(); j++)
|
||||
{
|
||||
SSG_basicFeatureGap& a_node = trees[i].treeNodes[j];
|
||||
scanLines[a_node.lineIdx][a_node.gapPeak.nPointIdx].nPointIdx = 2;
|
||||
}
|
||||
}
|
||||
for (int i = 0; i < (int)weldRawPoints.size(); i++)
|
||||
{
|
||||
int lineIdx = weldRawPoints[i].nPointIdx >> 16;
|
||||
int ptIdx = weldRawPoints[i].nPointIdx & 0xffff;
|
||||
scanLines[lineIdx][ptIdx].nPointIdx = 1;
|
||||
}
|
||||
|
||||
char outFileName[256];
|
||||
//输出测试结果
|
||||
sprintf_s(outFileName, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx);
|
||||
std::vector< std::vector<SVzNL3DPoint>> multiSeam;
|
||||
multiSeam.push_back(weldOps);
|
||||
_outputRGBDScanLapWeld_RGBD(outFileName, scanLines, multiSeam, true);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
typedef enum
|
||||
{
|
||||
keSG_焊缝定位_搭接焊缝 = 0,
|
||||
keSG_焊缝定位_长直细焊缝,
|
||||
}ESG_testMode;
|
||||
|
||||
int main()
|
||||
{
|
||||
//ESG_testMode testMode = keSG_焊缝定位_搭接焊缝;
|
||||
ESG_testMode testMode = keSG_焊缝定位_长直细焊缝;
|
||||
|
||||
if (keSG_焊缝定位_搭接焊缝 == testMode)
|
||||
lapWeldTest();
|
||||
else if (keSG_焊缝定位_长直细焊缝 == testMode)
|
||||
BQ_thinWeldSeamTest();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -126,7 +126,7 @@
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<AdditionalDependencies>opencv_world320d.lib;lapWeldDetection.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||
<AdditionalDependencies>opencv_world320d.lib;baseAlgorithm.lib;lapWeldDetection.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
@ -146,7 +146,7 @@
|
||||
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||
<OptimizeReferences>true</OptimizeReferences>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<AdditionalDependencies>opencv_world320.lib;lapWeldDetection.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||
<AdditionalDependencies>opencv_world320.lib;baseAlgorithm.lib;lapWeldDetection.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
|
||||
@ -1117,3 +1117,11 @@ SG_APISHARED_EXPORT void WD_getHoleInfo(
|
||||
std::vector<SWD_segFeatureTree>& segTrees_h,
|
||||
std::vector<SSG_intPair>& validObjects
|
||||
);
|
||||
|
||||
/* 支持栅格数据（要求输入栅格数据） */
|
||||
//提取扫描线上的V型Valley
|
||||
SG_APISHARED_EXPORT void wd_getLineVFlaws(
|
||||
std::vector<SVzNL3DPosition>& a_line,
|
||||
const int lineIdx,
|
||||
const SWD_valleyParam valleyParam,
|
||||
std::vector< SSG_basicFeatureGap>& valleys);
|
||||
|
||||
@ -214,6 +214,7 @@ typedef struct
|
||||
double gap_depth;
|
||||
SVzNL3DPosition gapPt_0; //gap的端点
|
||||
SVzNL3DPosition gapPt_1; //gap的端点
|
||||
SVzNL3DPosition gapPeak;
|
||||
SSG_ROIRectD roi;
|
||||
}SSG_basicFeatureGap;
|
||||
|
||||
@ -250,6 +251,13 @@ typedef struct
|
||||
double outlierTh;
|
||||
}SSG_outlierFilterParam;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double chkWin_width;
|
||||
double maxWidth;
|
||||
double minHeight;
|
||||
}SWD_valleyParam;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double LSlopeZWin; //对L型Slope进行坡度计算的窗口长度
|
||||
|
||||
@ -13,6 +13,7 @@
|
||||
#define SX_ERR_ZERO_OBJECTS -1010
|
||||
#define SX_ERR_NULL_GROUND_PLANE -1011
|
||||
#define SX_ERR_ZERO_2D_OBJECTS -1012
|
||||
#define SX_ERR_FITTING_POINTS_LESS -1013
|
||||
|
||||
//BQ_workpiece
|
||||
#define SX_ERR_INVLD_VTREE_NUM -2001
|
||||
@ -30,6 +31,7 @@
|
||||
#define SX_ERR_ZERO_OBJ_BTMLAYER -2103
|
||||
#define SX_ERR_GET_INVALID_PALTE -2104 //몰겼瓊혤댄轎
|
||||
#define SX_ERR_BIN_EDGE_ERROR -2105
|
||||
#define SX_ERR_BIN_SLICE_ZEROPOINTS -2106
|
||||
|
||||
//뀔관
|
||||
#define SX_BAG_TRAY_EMPTY -2201
|
||||
|
||||
@ -1,5 +1,7 @@
|
||||
#include "SG_baseDataType.h"
|
||||
#include "SG_baseAlgo_Export.h"
|
||||
#include "WD_convolveGauss.h"
|
||||
|
||||
#include <vector>
|
||||
|
||||
const double EPS = 1e-8;
|
||||
@ -9526,3 +9528,307 @@ if (i == 98)
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
enum STATES { null = 0, UP, DOWN };
|
||||
struct posAttri
|
||||
{
|
||||
int peakPos;
|
||||
STATES peakState;
|
||||
};
|
||||
//peak检测
|
||||
void _peakDetect(std::vector<double>& data, int dataSize, std::vector<posAttri>& peakPosition)
|
||||
{
|
||||
if (dataSize == 0)
|
||||
return;
|
||||
|
||||
static float algoPara_threshold_unchange = 0.001;
|
||||
|
||||
//检测Peak
|
||||
float preData = data[0];
|
||||
|
||||
STATES a_state = null;
|
||||
int peakPos = 0;
|
||||
|
||||
for (int i = 1; i < dataSize; i++)
|
||||
{
|
||||
if (i == 115)
|
||||
int kkk = 1;
|
||||
|
||||
float currData = data[i];
|
||||
float delta = fabsf(currData - preData);
|
||||
switch (a_state)
|
||||
{
|
||||
case null:
|
||||
if (delta > algoPara_threshold_unchange)
|
||||
{
|
||||
if (currData > preData)
|
||||
a_state = UP;
|
||||
else
|
||||
a_state = DOWN;
|
||||
}
|
||||
peakPos = i;
|
||||
preData = currData;
|
||||
break;
|
||||
case UP:
|
||||
if (currData > preData) //记录最大值
|
||||
{
|
||||
peakPos = i;
|
||||
preData = currData;
|
||||
}
|
||||
if ((delta > algoPara_threshold_unchange) && (currData < preData))//检查拐点
|
||||
{
|
||||
//保存极值位置
|
||||
posAttri a_pos;
|
||||
a_pos.peakPos = peakPos;
|
||||
a_pos.peakState = a_state;
|
||||
peakPosition.push_back(a_pos);
|
||||
a_state = DOWN;
|
||||
|
||||
//新的初值
|
||||
peakPos = i;
|
||||
preData = currData;
|
||||
}
|
||||
break;
|
||||
case DOWN:
|
||||
if (currData < preData) //记录最小值
|
||||
{
|
||||
peakPos = i;
|
||||
preData = currData;
|
||||
}
|
||||
|
||||
if ((delta > algoPara_threshold_unchange) && (currData > preData)) //检查拐点
|
||||
{
|
||||
//保存极值位置
|
||||
posAttri a_pos;
|
||||
a_pos.peakPos = peakPos;
|
||||
a_pos.peakState = a_state;
|
||||
peakPosition.push_back(a_pos);
|
||||
|
||||
a_state = UP;
|
||||
//新的初值
|
||||
peakPos = i;
|
||||
preData = currData;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
//检查凹陷
|
||||
//使用窄窗口
|
||||
//凹陷：二阶导为负极值，在极值的两边有一正一负的一阶导极值。
|
||||
float _computeHeight(SVzNL3DPoint& startPt, SVzNL3DPoint& endPt, SVzNL3DPoint& valleyPt)
|
||||
{
|
||||
float _a = endPt.z - startPt.z;
|
||||
float _b = -(endPt.y - startPt.y);
|
||||
float _c = -_a * startPt.y - _b * startPt.z;
|
||||
float height = fabs(_a * valleyPt.y + _b * valleyPt.z + _c) / sqrt(pow(_a, 2) + pow(_b, 2));
|
||||
return height;
|
||||
}
|
||||
/* startId, endId: 有效点的起止位置。对于栅格数据格式，开始的空点和结束的空点都不是有效点 */
|
||||
void _valleyDetect(
|
||||
std::vector<SVzNL3DPosition>& a_line,
|
||||
const int lineIdx, const int startId, const int endId,
|
||||
std::vector<posAttri>& deriv_1_peaks,
|
||||
std::vector<posAttri>& deriv_2_peaks,
|
||||
std::vector<double>& deriv_1,
|
||||
std::vector<double>& deriv_2,
|
||||
std::vector<SSG_basicFeatureGap>& valleys,
|
||||
const SWD_valleyParam valleyParam)
|
||||
{
|
||||
std::vector< posAttri> buff;
|
||||
buff.resize(a_line.size());
|
||||
for (int i = 0; i < deriv_1_peaks.size(); i++)
|
||||
buff[deriv_1_peaks[i].peakPos] = deriv_1_peaks[i];
|
||||
|
||||
double y_start = a_line[startId].pt3D.y;
|
||||
double y_end = a_line[endId].pt3D.y;
|
||||
|
||||
//逐个peak检测：正peak
|
||||
for (int i = 0; i < deriv_2_peaks.size(); i++)
|
||||
{
|
||||
if (i == 153)
|
||||
int kkk = 1;
|
||||
posAttri a_pos = deriv_2_peaks[i];
|
||||
if ((a_pos.peakState == DOWN) && (deriv_2[a_pos.peakPos] < 0))
|
||||
{
|
||||
//检查两侧
|
||||
int preEdge = -1;
|
||||
for (int j = a_pos.peakPos - 1; j > 0; j--)
|
||||
{
|
||||
if (buff[j].peakState == DOWN)
|
||||
break;
|
||||
else if (buff[j].peakState == UP)
|
||||
{
|
||||
if (deriv_1[j] > 0)
|
||||
preEdge = j;
|
||||
break;
|
||||
}
|
||||
}
|
||||
int postEdge = -1;
|
||||
for (int j = a_pos.peakPos + 1; j < (int)a_line.size(); j++)
|
||||
{
|
||||
if (buff[j].peakState == UP)
|
||||
break;
|
||||
else if (buff[j].peakState == DOWN)
|
||||
{
|
||||
if (deriv_1[j] < 0)
|
||||
postEdge = j;
|
||||
break;
|
||||
}
|
||||
}
|
||||
//
|
||||
if ((preEdge >= 0) && (postEdge >= 0)) //初步符合凹陷特点
|
||||
{
|
||||
float edge_value_pre = deriv_1[preEdge];
|
||||
float edge_value_post = deriv_1[postEdge];
|
||||
//检测对称性
|
||||
float min_value, max_value;
|
||||
if (edge_value_pre < (-edge_value_post))
|
||||
{
|
||||
min_value = edge_value_pre;
|
||||
max_value = -edge_value_post;
|
||||
}
|
||||
else
|
||||
{
|
||||
max_value = edge_value_pre;
|
||||
min_value = -edge_value_post;
|
||||
}
|
||||
if (min_value > max_value / 10) //2023.05.02: 放宽。当凹陷在斜面上时，不再是maxValue/2的理想情况
|
||||
{
|
||||
//检查凹陷宽度深度，滤除噪声
|
||||
float th_pre = edge_value_pre * 0.5;
|
||||
int edge_pos_pre = -1;
|
||||
for (int j = preEdge; j >= 0; j--)
|
||||
{
|
||||
if ( (deriv_1[j] <= th_pre) && (a_line[j].nPointIdx >= 0)) //非填充点
|
||||
{
|
||||
edge_pos_pre = j;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
float th_post = edge_value_post * 0.5;
|
||||
int edge_pos_post = -1;
|
||||
for (int j = postEdge; j < (int)a_line.size(); j++)
|
||||
{
|
||||
if ((deriv_1[j] >= th_post) && (a_line[j].nPointIdx >= 0)) //非填充点
|
||||
{
|
||||
edge_pos_post = j;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
//检测宽度
|
||||
if ((edge_pos_pre >= 0) && (edge_pos_post >= 0))
|
||||
{
|
||||
//float width = a_line->m_p3DPositiont[edge_pos_post].pt3D.y - a_line->m_p3DPositiont[edge_pos_pre].pt3D.y;
|
||||
float width = a_line[postEdge].pt3D.y - a_line[preEdge].pt3D.y;
|
||||
if (width < valleyParam.maxWidth)
|
||||
{
|
||||
float height = _computeHeight(a_line[edge_pos_pre].pt3D,
|
||||
a_line[edge_pos_post].pt3D,
|
||||
a_line[a_pos.peakPos].pt3D);
|
||||
if (height > valleyParam.minHeight)
|
||||
{
|
||||
//精确确定peak点
|
||||
int realPeakPos = preEdge;
|
||||
for (int m = preEdge; m <= postEdge; m++)
|
||||
{
|
||||
if ((a_line[realPeakPos].pt3D.z > a_line[m].pt3D.z) && (a_line[m].nPointIdx >= 0))
|
||||
realPeakPos = m;
|
||||
}
|
||||
|
||||
SSG_basicFeatureGap a_valley;
|
||||
memset(&a_valley, 0, sizeof(SSG_basicFeatureGap));
|
||||
a_valley.gapPeak.nPointIdx = realPeakPos;
|
||||
a_valley.gapPeak.pt3D = a_line[realPeakPos].pt3D;
|
||||
a_valley.gapPt_0.nPointIdx = edge_pos_pre;
|
||||
a_valley.gapPt_0.pt3D = a_line[edge_pos_pre].pt3D;
|
||||
a_valley.gapPt_1.nPointIdx = edge_pos_post;
|
||||
a_valley.gapPt_1.pt3D = a_line[edge_pos_post].pt3D;
|
||||
a_valley.lineIdx = lineIdx;
|
||||
a_valley.gap_start = a_line[preEdge].pt3D.y;
|
||||
a_valley.gap_width = width;
|
||||
a_valley.gap_depth = height;
|
||||
valleys.push_back(a_valley);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
/* 支持栅格数据（要求输入栅格数据） */
|
||||
//提取扫描线上的V型Valley
|
||||
void wd_getLineVFlaws(
|
||||
std::vector<SVzNL3DPosition>& a_line,
|
||||
const int lineIdx,
|
||||
const SWD_valleyParam valleyParam,
|
||||
std::vector< SSG_basicFeatureGap>& valleys)
|
||||
{
|
||||
//处理栅格数据（填补空洞，统计有效点
|
||||
int startId = -1, endId = 0;
|
||||
for (int i = 0; i < (int)a_line.size(); i++)
|
||||
{
|
||||
a_line[i].nPointIdx = 0;
|
||||
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;
|
||||
|
||||
//填充孔洞
|
||||
double preZ = a_line[startId].pt3D.z;
|
||||
for (int i = startId + 1; i <= endId; i++)
|
||||
{
|
||||
|
||||
if (a_line[i].pt3D.z < 1e-4)
|
||||
a_line[i].pt3D.z = preZ;
|
||||
preZ = a_line[i].pt3D.z;
|
||||
}
|
||||
|
||||
//单窗口
|
||||
float total_deltaY = a_line[endId].pt3D.y - a_line[startId].pt3D.y;
|
||||
float scale = total_deltaY / validLinePtNum;
|
||||
#if 0
|
||||
//scale = scale / 1.5;
|
||||
float* deriv_1_wide = (float*)malloc(sizeof(float) * a_line->m_3DPointCount);
|
||||
memset(deriv_1_wide, 0, sizeof(float) * a_line->m_3DPointCount);
|
||||
float* deriv_2_wide = (float*)malloc(sizeof(float) * a_line->m_3DPointCount);
|
||||
memset(deriv_2_wide, 0, sizeof(float) * a_line->m_3DPointCount);
|
||||
//计算gauss一阶和二阶导
|
||||
_convolveGauss_laserLine(a_line, startId, endId, deriv_1_wide, deriv_2_wide, chkWidth_wide, scale);
|
||||
#endif
|
||||
|
||||
//单窗口：窄窗口
|
||||
double chkWidth_narrow = valleyParam.chkWin_width / 2;
|
||||
float narrow_scale = scale;
|
||||
std::vector<double> deriv_1_narrow;
|
||||
std::vector<double> deriv_2_narrow;
|
||||
//计算gauss一阶和二阶导
|
||||
wd_convolveGauss_laserLine(a_line, startId, endId, deriv_1_narrow, deriv_2_narrow, chkWidth_narrow, narrow_scale);
|
||||
|
||||
//
|
||||
//peak检测: 边沿检测和凹陷检测同时需要使用
|
||||
std::vector<posAttri> deriv_1_peaks;
|
||||
_peakDetect(deriv_1_narrow, (int)a_line.size(), deriv_1_peaks);
|
||||
std::vector<posAttri> deriv_2_peaks;
|
||||
_peakDetect(deriv_2_narrow, (int)a_line.size(), deriv_2_peaks);
|
||||
|
||||
//检查凹陷
|
||||
//使用窄窗口
|
||||
//凹陷：二阶导为极值，在极值的两边有一负一正的一阶导极值。
|
||||
//float innerPara_max_valley_width = 20.0; //内部参数。控制最大凹陷检测宽度，用于过滤虚假凹陷
|
||||
_valleyDetect(a_line, lineIdx, startId, endId, deriv_1_peaks, deriv_2_peaks, deriv_1_narrow, deriv_2_narrow, valleys, valleyParam);
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -5,6 +5,12 @@
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <limits>
|
||||
|
||||
//version 1.0.0 :焊缝（平缝）识别检测算法初始发布
|
||||
std::string m_strVersion = "weldSeamAlgo_1.0.0";
|
||||
const char* wd_weldSeamAlgoVersion(void)
|
||||
{
|
||||
return m_strVersion.c_str();
|
||||
}
|
||||
//计算一个平面调平参数。
|
||||
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
|
||||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||||
@ -88,6 +94,64 @@ void getWeldPoint(std::vector<SVzNL3DPoint>& a_weld_contour, int midPtNum, std::
|
||||
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 _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,
|
||||
@ -355,4 +419,168 @@ void sx_getLapWeldPostion(
|
||||
}
|
||||
|
||||
//输出结果
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
//计算焊缝位置
|
||||
void BQ_getWeldSeamPose(
|
||||
std::vector<SSG_gapFeatureTree>& trees,
|
||||
int weldRefPoints, //焊缝中间参考点位置
|
||||
std::vector<SVzNL3DPoint>& weldOps, //焊缝位置
|
||||
std::vector<SVzNL3DPosition>& weldRawPoints, //用于显示和debug
|
||||
int* errCode)
|
||||
{
|
||||
*errCode = 0;
|
||||
|
||||
//保留最长的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;
|
||||
}
|
||||
//生成显示数据
|
||||
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;
|
||||
}
|
||||
|
||||
//生成结果信息
|
||||
//中点参考点的位置计算方法：以参考点的X为基准，左右各取段长的一半，进行直线拟合，根据X计算Y坐标
|
||||
int refSegNum = weldRefPoints + 1;
|
||||
refSegNum = refSegNum * 2; //以段长一半进行区分
|
||||
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);
|
||||
}
|
||||
}
|
||||
//分段按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<double> fittingPtZ;
|
||||
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());
|
||||
}
|
||||
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());
|
||||
}
|
||||
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());
|
||||
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;
|
||||
}
|
||||
//直线拟合
|
||||
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;
|
||||
}
|
||||
@ -17,6 +17,8 @@ typedef struct
|
||||
ESX_WeldScanMode scanMode;
|
||||
}SSX_lapWeldParam;
|
||||
|
||||
SG_APISHARED_EXPORT const char* wd_weldSeamAlgoVersion(void);
|
||||
|
||||
//计算一个平面调平参数。
|
||||
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
|
||||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||||
@ -39,3 +41,10 @@ SG_APISHARED_EXPORT void sx_getLapWeldPostion(
|
||||
std::vector<std::vector<SVzNL3DPoint>>& objOps,
|
||||
int* errCode);
|
||||
|
||||
//计算焊缝位置
|
||||
SG_APISHARED_EXPORT void BQ_getWeldSeamPose(
|
||||
std::vector<SSG_gapFeatureTree>& trees,
|
||||
int weldRefPoints, //焊缝中间参考点位置
|
||||
std::vector<SVzNL3DPoint>& weldOps, //焊缝位置
|
||||
std::vector<SVzNL3DPosition>& weldRawPoints, //用于显示和debug
|
||||
int* errCode);
|
||||
@ -179,11 +179,15 @@ double fitCircleByLeastSquare(
|
||||
double e;
|
||||
double r2 = radius * radius;
|
||||
for (int pId = 0; pId < N; ++pId) {
|
||||
e = pow(pointArray[pId].x - center.x, 2) + pow(pointArray[pId].y - center.y, 2) - r2;
|
||||
if (e > err) {
|
||||
e = abs(pow(pointArray[pId].x - center.x, 2) + pow(pointArray[pId].y - center.y, 2) - r2);
|
||||
#if 0 if (e > err) {
|
||||
err = e;
|
||||
}
|
||||
#else
|
||||
err += e;
|
||||
#endif
|
||||
}
|
||||
err = sqrt(err / N);
|
||||
return err;
|
||||
}
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user