diff --git a/baseAlgorithm/baseAlgorithm.vcxproj b/baseAlgorithm/baseAlgorithm.vcxproj
index ec19a93..1df092a 100644
--- a/baseAlgorithm/baseAlgorithm.vcxproj
+++ b/baseAlgorithm/baseAlgorithm.vcxproj
@@ -164,11 +164,13 @@
+
+
/bigobj %(AdditionalOptions)
/bigobj %(AdditionalOptions)
diff --git a/lapWeldDetection_test/lapWeldDetection_test.cpp b/lapWeldDetection_test/lapWeldDetection_test.cpp
index 7f861a3..771fd2d 100644
--- a/lapWeldDetection_test/lapWeldDetection_test.cpp
+++ b/lapWeldDetection_test/lapWeldDetection_test.cpp
@@ -7,6 +7,7 @@
#include
#include "direct.h"
#include
+#include "SG_baseAlgo_Export.h"
#include "SX_lapWeldDetection_Export.h"
#include
#include
@@ -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>& 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>& 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> 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> 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> scanLines;
+ vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
+ _removeZeroLines_XYZ(scanLines);
+ if (scanLines.size() == 0)
+ continue;
+
+ //统计时间
+ long t1 = (long)GetTickCount64();//统计时间
+
+ std::vector trees;
+ for (int i = 0; i < (int)scanLines.size(); i++)
+ {
+ std::vector& 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 weldOps;
+ std::vector 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> 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;
+}
diff --git a/lapWeldDetection_test/lapWeldDetection_test.vcxproj b/lapWeldDetection_test/lapWeldDetection_test.vcxproj
index 0665d83..16e1c3e 100644
--- a/lapWeldDetection_test/lapWeldDetection_test.vcxproj
+++ b/lapWeldDetection_test/lapWeldDetection_test.vcxproj
@@ -126,7 +126,7 @@
Console
true
- opencv_world320d.lib;lapWeldDetection.lib;%(AdditionalDependencies)
+ opencv_world320d.lib;baseAlgorithm.lib;lapWeldDetection.lib;%(AdditionalDependencies)
..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories)
@@ -146,7 +146,7 @@
true
true
true
- opencv_world320.lib;lapWeldDetection.lib;%(AdditionalDependencies)
+ opencv_world320.lib;baseAlgorithm.lib;lapWeldDetection.lib;%(AdditionalDependencies)
..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories)
diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h
index 7eb5109..5b5f255 100644
--- a/sourceCode/SG_baseAlgo_Export.h
+++ b/sourceCode/SG_baseAlgo_Export.h
@@ -1117,3 +1117,11 @@ SG_APISHARED_EXPORT void WD_getHoleInfo(
std::vector& segTrees_h,
std::vector& validObjects
);
+
+/* ֧դݣҪդݣ */
+//ȡɨϵVValley
+SG_APISHARED_EXPORT void wd_getLineVFlaws(
+ std::vector& a_line,
+ const int lineIdx,
+ const SWD_valleyParam valleyParam,
+ std::vector< SSG_basicFeatureGap>& valleys);
diff --git a/sourceCode/SG_baseDataType.h b/sourceCode/SG_baseDataType.h
index 0c3155d..a2c6028 100644
--- a/sourceCode/SG_baseDataType.h
+++ b/sourceCode/SG_baseDataType.h
@@ -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; //LSlope¶ȼĴڳ
diff --git a/sourceCode/SG_errCode.h b/sourceCode/SG_errCode.h
index 25af7df..082325b 100644
--- a/sourceCode/SG_errCode.h
+++ b/sourceCode/SG_errCode.h
@@ -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
diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp
index 64ee904..a377d51 100644
--- a/sourceCode/SG_lineFeature.cpp
+++ b/sourceCode/SG_lineFeature.cpp
@@ -1,5 +1,7 @@
#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
+#include "WD_convolveGauss.h"
+
#include
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& data, int dataSize, std::vector& 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;
+
+ //µijֵ
+ 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;
+ //µijֵ
+ 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& a_line,
+ const int lineIdx, const int startId, const int endId,
+ std::vector& deriv_1_peaks,
+ std::vector& deriv_2_peaks,
+ std::vector& deriv_1,
+ std::vector& deriv_2,
+ std::vector& 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);
+ }
+ }
+ }
+ }
+
+ }
+ }
+ }
+}
+/* ֧դݣҪդݣ */
+//ȡɨϵVValley
+void wd_getLineVFlaws(
+ std::vector& 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 deriv_1_narrow;
+ std::vector deriv_2_narrow;
+ //gaussһͶ
+ wd_convolveGauss_laserLine(a_line, startId, endId, deriv_1_narrow, deriv_2_narrow, chkWidth_narrow, narrow_scale);
+
+ //
+ //peak: ؼͰݼͬʱҪʹ
+ std::vector deriv_1_peaks;
+ _peakDetect(deriv_1_narrow, (int)a_line.size(), deriv_1_peaks);
+ std::vector 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);
+
+}
+
diff --git a/sourceCode/SX_lapWeldDetection.cpp b/sourceCode/SX_lapWeldDetection.cpp
index 86d7cfe..d0f2135 100644
--- a/sourceCode/SX_lapWeldDetection.cpp
+++ b/sourceCode/SX_lapWeldDetection.cpp
@@ -5,6 +5,12 @@
#include
#include
+//version 1.0.0 :죨ƽ죩ʶ㷨ʼ
+std::string m_strVersion = "weldSeamAlgo_1.0.0";
+const char* wd_weldSeamAlgoVersion(void)
+{
+ return m_strVersion.c_str();
+}
//һƽƽ
//пһƽͲοƽƽ棬ߵƽеƽ
//תΪƽƽ淨ΪֱIJ
@@ -88,6 +94,64 @@ void getWeldPoint(std::vector& a_weld_contour, int midPtNum, std::
a_weld.push_back(pt_1);
}
+SVzNLRange _getTreesLineRange(std::vector& 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& 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>& scanLines,
@@ -355,4 +419,168 @@ void sx_getLapWeldPostion(
}
//
+}
+
+double _getMeanZ(std::vector& 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& trees,
+ int weldRefPoints, //мολ
+ std::vector& weldOps, //λ
+ std::vector& 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 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ΪҸȡγһ룬ֱϣXY
+ int refSegNum = weldRefPoints + 1;
+ refSegNum = refSegNum * 2; //Զγһ
+ std::vector> refSegments;
+ std::vector> 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 fittingPoints;
+ std::vector 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;
}
\ No newline at end of file
diff --git a/sourceCode/SX_lapWeldDetection_Export.h b/sourceCode/SX_lapWeldDetection_Export.h
index 55673c5..6ac913a 100644
--- a/sourceCode/SX_lapWeldDetection_Export.h
+++ b/sourceCode/SX_lapWeldDetection_Export.h
@@ -17,6 +17,8 @@ typedef struct
ESX_WeldScanMode scanMode;
}SSX_lapWeldParam;
+SG_APISHARED_EXPORT const char* wd_weldSeamAlgoVersion(void);
+
//һƽƽ
//пһƽͲοƽƽ棬ߵƽеƽ
//תΪƽƽ淨ΪֱIJ
@@ -39,3 +41,10 @@ SG_APISHARED_EXPORT void sx_getLapWeldPostion(
std::vector>& objOps,
int* errCode);
+//㺸λ
+SG_APISHARED_EXPORT void BQ_getWeldSeamPose(
+ std::vector& trees,
+ int weldRefPoints, //мολ
+ std::vector& weldOps, //λ
+ std::vector& weldRawPoints, //ʾdebug
+ int* errCode);
\ No newline at end of file
diff --git a/sourceCode/dataFitting.cpp b/sourceCode/dataFitting.cpp
index 8fcda05..df4a176 100644
--- a/sourceCode/dataFitting.cpp
+++ b/sourceCode/dataFitting.cpp
@@ -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;
}