搭接焊缝算法初次提交
This commit is contained in:
parent
be5dbbccfb
commit
f855ffd70b
@ -86,6 +86,27 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "glovePositioning_test", "gl
|
||||
{4060CE45-6235-4CA7-B64C-6E4E5CDDC34F} = {4060CE45-6235-4CA7-B64C-6E4E5CDDC34F}
|
||||
EndProjectSection
|
||||
EndProject
|
||||
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "bottleCapQualityCheck", "bottleCapQualityCheck\bottleCapQualityCheck.vcxproj", "{DE243631-2472-45B7-84DD-06C8B5DC1D5A}"
|
||||
ProjectSection(ProjectDependencies) = postProject
|
||||
{95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD}
|
||||
EndProjectSection
|
||||
EndProject
|
||||
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "bottleCapQualityCheck_test", "bottleCapQualityCheck_test\bottleCapQualityCheck_test.vcxproj", "{6093BECC-3FE0-45EC-8BDE-726DB8C67E84}"
|
||||
ProjectSection(ProjectDependencies) = postProject
|
||||
{DE243631-2472-45B7-84DD-06C8B5DC1D5A} = {DE243631-2472-45B7-84DD-06C8B5DC1D5A}
|
||||
EndProjectSection
|
||||
EndProject
|
||||
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "lapWeldDetection", "lapWeldDetection\lapWeldDetection.vcxproj", "{0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0}"
|
||||
ProjectSection(ProjectDependencies) = postProject
|
||||
{95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD}
|
||||
EndProjectSection
|
||||
EndProject
|
||||
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "lapWeldDetection_test", "lapWeldDetection_test\lapWeldDetection_test.vcxproj", "{BD65AC40-A2C9-4C8D-ADBA-ABA595C5F32D}"
|
||||
ProjectSection(ProjectDependencies) = postProject
|
||||
{95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD}
|
||||
{0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0} = {0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0}
|
||||
EndProjectSection
|
||||
EndProject
|
||||
Global
|
||||
GlobalSection(SolutionConfigurationPlatforms) = preSolution
|
||||
Debug|x64 = Debug|x64
|
||||
@ -230,6 +251,38 @@ Global
|
||||
{7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}.Release|x64.Build.0 = Release|x64
|
||||
{7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}.Release|x86.ActiveCfg = Release|Win32
|
||||
{7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}.Release|x86.Build.0 = Release|Win32
|
||||
{DE243631-2472-45B7-84DD-06C8B5DC1D5A}.Debug|x64.ActiveCfg = Debug|x64
|
||||
{DE243631-2472-45B7-84DD-06C8B5DC1D5A}.Debug|x64.Build.0 = Debug|x64
|
||||
{DE243631-2472-45B7-84DD-06C8B5DC1D5A}.Debug|x86.ActiveCfg = Debug|Win32
|
||||
{DE243631-2472-45B7-84DD-06C8B5DC1D5A}.Debug|x86.Build.0 = Debug|Win32
|
||||
{DE243631-2472-45B7-84DD-06C8B5DC1D5A}.Release|x64.ActiveCfg = Release|x64
|
||||
{DE243631-2472-45B7-84DD-06C8B5DC1D5A}.Release|x64.Build.0 = Release|x64
|
||||
{DE243631-2472-45B7-84DD-06C8B5DC1D5A}.Release|x86.ActiveCfg = Release|Win32
|
||||
{DE243631-2472-45B7-84DD-06C8B5DC1D5A}.Release|x86.Build.0 = Release|Win32
|
||||
{6093BECC-3FE0-45EC-8BDE-726DB8C67E84}.Debug|x64.ActiveCfg = Debug|x64
|
||||
{6093BECC-3FE0-45EC-8BDE-726DB8C67E84}.Debug|x64.Build.0 = Debug|x64
|
||||
{6093BECC-3FE0-45EC-8BDE-726DB8C67E84}.Debug|x86.ActiveCfg = Debug|Win32
|
||||
{6093BECC-3FE0-45EC-8BDE-726DB8C67E84}.Debug|x86.Build.0 = Debug|Win32
|
||||
{6093BECC-3FE0-45EC-8BDE-726DB8C67E84}.Release|x64.ActiveCfg = Release|x64
|
||||
{6093BECC-3FE0-45EC-8BDE-726DB8C67E84}.Release|x64.Build.0 = Release|x64
|
||||
{6093BECC-3FE0-45EC-8BDE-726DB8C67E84}.Release|x86.ActiveCfg = Release|Win32
|
||||
{6093BECC-3FE0-45EC-8BDE-726DB8C67E84}.Release|x86.Build.0 = Release|Win32
|
||||
{0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0}.Debug|x64.ActiveCfg = Debug|x64
|
||||
{0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0}.Debug|x64.Build.0 = Debug|x64
|
||||
{0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0}.Debug|x86.ActiveCfg = Debug|Win32
|
||||
{0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0}.Debug|x86.Build.0 = Debug|Win32
|
||||
{0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0}.Release|x64.ActiveCfg = Release|x64
|
||||
{0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0}.Release|x64.Build.0 = Release|x64
|
||||
{0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0}.Release|x86.ActiveCfg = Release|Win32
|
||||
{0C0B9B7A-9BF2-4157-BFFD-D12012E39FA0}.Release|x86.Build.0 = Release|Win32
|
||||
{BD65AC40-A2C9-4C8D-ADBA-ABA595C5F32D}.Debug|x64.ActiveCfg = Debug|x64
|
||||
{BD65AC40-A2C9-4C8D-ADBA-ABA595C5F32D}.Debug|x64.Build.0 = Debug|x64
|
||||
{BD65AC40-A2C9-4C8D-ADBA-ABA595C5F32D}.Debug|x86.ActiveCfg = Debug|Win32
|
||||
{BD65AC40-A2C9-4C8D-ADBA-ABA595C5F32D}.Debug|x86.Build.0 = Debug|Win32
|
||||
{BD65AC40-A2C9-4C8D-ADBA-ABA595C5F32D}.Release|x64.ActiveCfg = Release|x64
|
||||
{BD65AC40-A2C9-4C8D-ADBA-ABA595C5F32D}.Release|x64.Build.0 = Release|x64
|
||||
{BD65AC40-A2C9-4C8D-ADBA-ABA595C5F32D}.Release|x86.ActiveCfg = Release|Win32
|
||||
{BD65AC40-A2C9-4C8D-ADBA-ABA595C5F32D}.Release|x86.Build.0 = Release|Win32
|
||||
EndGlobalSection
|
||||
GlobalSection(SolutionProperties) = preSolution
|
||||
HideSolutionNode = FALSE
|
||||
|
||||
@ -77,7 +77,9 @@ SVzNLXYZRGBDLaserLine* vzReadLaserScanPointFromFile_XYZRGB(const char* fileName,
|
||||
int lineIdx = 0;
|
||||
int ptIdx = 0;
|
||||
int ptNum = 0;
|
||||
int pre_ptNum = -1;
|
||||
std::vector< SVzNLPointXYZRGBA> a_line;
|
||||
int vldLineIdx = 0;
|
||||
int vldPtNum = 0;
|
||||
unsigned int timeStamp = 0;
|
||||
while (getline(inputFile, linedata))
|
||||
@ -119,20 +121,25 @@ SVzNLXYZRGBDLaserLine* vzReadLaserScanPointFromFile_XYZRGB(const char* fileName,
|
||||
break;
|
||||
|
||||
int recvPtNum = (int)a_line.size();
|
||||
if ( (recvPtNum == ptNum) && ((vldPtNum > 0) || (false == removeNullLines)))
|
||||
if ( (recvPtNum == pre_ptNum) && ((vldPtNum > 0) || (false == removeNullLines)))
|
||||
{
|
||||
SVzNLPointXYZRGBA* p3DPoint = (SVzNLPointXYZRGBA*)malloc(sizeof(SVzNLPointXYZRGBA) * ptNum);
|
||||
_scanLines[lineIdx].nPointCnt = ptNum;
|
||||
_scanLines[lineIdx].nTimeStamp = timeStamp;
|
||||
_scanLines[lineIdx].p3DPoint = p3DPoint;
|
||||
for (int m = 0; m < ptNum; m++)
|
||||
SVzNLPointXYZRGBA* p3DPoint;
|
||||
if (pre_ptNum > 0)
|
||||
p3DPoint = (SVzNLPointXYZRGBA*)malloc(sizeof(SVzNLPointXYZRGBA) * pre_ptNum);
|
||||
else
|
||||
p3DPoint = NULL;
|
||||
_scanLines[vldLineIdx].nPointCnt = pre_ptNum;
|
||||
_scanLines[vldLineIdx].nTimeStamp = timeStamp;
|
||||
_scanLines[vldLineIdx].p3DPoint = p3DPoint;
|
||||
for (int m = 0; m < pre_ptNum; m++)
|
||||
p3DPoint[m] = a_line[m];
|
||||
lineIdx++;
|
||||
vldLineIdx++;
|
||||
}
|
||||
//new Line
|
||||
timeStamp = curr_timeStamp;
|
||||
vldPtNum = 0;
|
||||
a_line.clear();
|
||||
pre_ptNum = ptNum;
|
||||
}
|
||||
else if (0 == strncmp("{", linedata.c_str(), 1))
|
||||
{
|
||||
@ -167,19 +174,23 @@ SVzNLXYZRGBDLaserLine* vzReadLaserScanPointFromFile_XYZRGB(const char* fileName,
|
||||
}
|
||||
//last line
|
||||
int recvPtNum = (int)a_line.size();
|
||||
if ((recvPtNum == ptNum) && ((vldPtNum > 0) || (false == removeNullLines)))
|
||||
if ((recvPtNum == pre_ptNum) && ((vldPtNum > 0) || (false == removeNullLines)))
|
||||
{
|
||||
SVzNLPointXYZRGBA* p3DPoint = (SVzNLPointXYZRGBA*)malloc(sizeof(SVzNLPointXYZRGBA) * ptNum);
|
||||
_scanLines[lineIdx].nPointCnt = ptNum;
|
||||
_scanLines[lineIdx].nTimeStamp = timeStamp;
|
||||
_scanLines[lineIdx].p3DPoint = p3DPoint;
|
||||
for (int m = 0; m < ptNum; m++)
|
||||
SVzNLPointXYZRGBA* p3DPoint;
|
||||
if (pre_ptNum > 0)
|
||||
p3DPoint = (SVzNLPointXYZRGBA*)malloc(sizeof(SVzNLPointXYZRGBA) * pre_ptNum);
|
||||
else
|
||||
p3DPoint = NULL;
|
||||
_scanLines[vldLineIdx].nPointCnt = pre_ptNum;
|
||||
_scanLines[vldLineIdx].nTimeStamp = timeStamp;
|
||||
_scanLines[vldLineIdx].p3DPoint = p3DPoint;
|
||||
for (int m = 0; m < pre_ptNum; m++)
|
||||
p3DPoint[m] = a_line[m];
|
||||
lineIdx++;
|
||||
vldLineIdx++;
|
||||
}
|
||||
|
||||
if (scanLineNum)
|
||||
*scanLineNum = lineIdx;
|
||||
*scanLineNum = vldLineIdx;
|
||||
inputFile.close();
|
||||
return _scanLines;
|
||||
}
|
||||
@ -400,6 +411,133 @@ SVzNL3DLaserLine* vzReadLaserScanPointFromFile_XYZ(const char* fileName, int* sc
|
||||
return _scanLines;
|
||||
}
|
||||
|
||||
//从RGBD点云中读取XYZ信息
|
||||
SVzNL3DLaserLine* vzReadXYZPointFromFile_XYZRGB(const char* fileName, int* scanLineNum, float* scanV,
|
||||
int* dataCalib, int* scanMaxStamp, int* canClockUnit)
|
||||
{
|
||||
std::ifstream inputFile(fileName);
|
||||
std::string linedata;
|
||||
|
||||
if (inputFile.is_open() == false)
|
||||
return NULL;
|
||||
|
||||
SVzNL3DLaserLine* _scanLines = NULL;
|
||||
|
||||
int lines = 0;
|
||||
int dataElements = 4;
|
||||
int firstIndex = -1;
|
||||
|
||||
int dataFileVer = DATA_VER_OLD;
|
||||
std::getline(inputFile, linedata); //第一行
|
||||
int lineNum = 0;
|
||||
if (0 == strncmp("LineNum:", linedata.c_str(), 8))
|
||||
{
|
||||
dataFileVer = DATA_VER_NEW;
|
||||
sscanf_s(linedata.c_str(), "LineNum:%d", &lines);
|
||||
if (lines == 0)
|
||||
return NULL;
|
||||
lineNum = lines;
|
||||
_scanLines = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1));
|
||||
memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1));
|
||||
if (scanLineNum)
|
||||
*scanLineNum = lines;
|
||||
}
|
||||
else if (0 == strncmp("LineNum_", linedata.c_str(), 8))
|
||||
{
|
||||
dataFileVer = DATA_VER_OLD;
|
||||
sscanf_s(linedata.c_str(), "LineNum_%d", &lines);
|
||||
if (lines == 0)
|
||||
return NULL;
|
||||
lineNum = lines;
|
||||
_scanLines = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1));
|
||||
memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1));
|
||||
if (scanLineNum)
|
||||
*scanLineNum = lines;
|
||||
}
|
||||
if (_scanLines == NULL)
|
||||
return NULL;
|
||||
|
||||
int ptNum = 0;
|
||||
int lineIdx = -1;
|
||||
int ptIdx = 0;
|
||||
SVzNL3DPosition* p3DPoint = NULL;
|
||||
if (dataFileVer == DATA_VER_NEW)
|
||||
{
|
||||
while (getline(inputFile, linedata))
|
||||
{
|
||||
if (0 == strncmp("ScanSpeed:", linedata.c_str(), 10))
|
||||
{
|
||||
double lineV = 0;
|
||||
sscanf_s(linedata.c_str(), "ScanSpeed:%lf", &lineV);
|
||||
if (scanV)
|
||||
*scanV = (float)lineV;
|
||||
}
|
||||
else if (0 == strncmp("PointAdjust:", linedata.c_str(), 12))
|
||||
{
|
||||
int ptAdjusted = 0;
|
||||
sscanf_s(linedata.c_str(), "PointAdjust:%d", &ptAdjusted);
|
||||
if (dataCalib)
|
||||
*dataCalib = ptAdjusted;
|
||||
}
|
||||
else if (0 == strncmp("MaxTimeStamp:", linedata.c_str(), 13))
|
||||
{
|
||||
unsigned int maxTimeStamp = 0;
|
||||
unsigned int timePerStamp = 0;
|
||||
sscanf_s(linedata.c_str(), "MaxTimeStamp:%u_%u", &maxTimeStamp, &timePerStamp);
|
||||
if (scanMaxStamp)
|
||||
*scanMaxStamp = maxTimeStamp;
|
||||
if (canClockUnit)
|
||||
*canClockUnit = timePerStamp;
|
||||
}
|
||||
else if (0 == strncmp("Line_", linedata.c_str(), 5))
|
||||
{
|
||||
int lineIndex;
|
||||
unsigned int timeStamp;
|
||||
sscanf_s(linedata.c_str(), "Line_%d_%u_%d", &lineIndex, &timeStamp, &ptNum);
|
||||
if (firstIndex < 0)
|
||||
firstIndex = lineIndex;
|
||||
|
||||
lineIndex = lineIndex - firstIndex;
|
||||
if ((lineIndex < 0) || (lineIndex >= lines))
|
||||
break;
|
||||
|
||||
//new Line
|
||||
lineIdx++;
|
||||
if (ptNum > 0)
|
||||
{
|
||||
p3DPoint = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * ptNum);
|
||||
memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * ptNum);
|
||||
}
|
||||
else
|
||||
p3DPoint = NULL;
|
||||
_scanLines[lineIdx].nPositionCnt = 0;
|
||||
_scanLines[lineIdx].nTimeStamp = timeStamp;
|
||||
_scanLines[lineIdx].p3DPosition = p3DPoint;
|
||||
|
||||
}
|
||||
else if (0 == strncmp("{", linedata.c_str(), 1))
|
||||
{
|
||||
float X, Y, Z;
|
||||
float leftX, leftY;
|
||||
float rightX, rightY;
|
||||
float r, g, b;
|
||||
sscanf_s(linedata.c_str(), "{%f,%f,%f,%f,%f,%f }-{%f,%f}-{%f,%f}", &X, &Y, &Z, &r, &g, &b, &leftX, &leftY, &rightX, &rightY);
|
||||
int id = _scanLines[lineIdx].nPositionCnt;
|
||||
if (id < ptNum)
|
||||
{
|
||||
p3DPoint[id].pt3D.x = X;
|
||||
p3DPoint[id].pt3D.y = Y;
|
||||
p3DPoint[id].pt3D.z = Z;
|
||||
_scanLines[lineIdx].nPositionCnt = id + 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
inputFile.close();
|
||||
return _scanLines;
|
||||
}
|
||||
|
||||
SVzNL3DLaserLine* _convertToGridData_XYZRGB(SVzNLXYZRGBDLaserLine* laser3DPoints, int lineNum, double _F, double* camPoseR, int* outLineNum)
|
||||
{
|
||||
int min_y = 100000000;
|
||||
@ -1875,7 +2013,7 @@ void _genXOYProjectionImage(cv::String& fileName, SVzNL3DLaserLine* scanData, in
|
||||
else
|
||||
y_scale = x_scale;
|
||||
|
||||
int angleDrawLen = dirLen / x_scale;
|
||||
int angleDrawLen = dirLen;// / x_scale;
|
||||
_XOYprojection(img, scan_lines, objOps, x_scale, y_scale, x_range, y_range, true, angleDrawLen);
|
||||
|
||||
//旋转视角显示
|
||||
@ -2373,7 +2511,7 @@ void _outputScanDataFile_removeZeros(char* fileName, SVzNL3DLaserLine* scanData,
|
||||
#define TEST_COMPUTE_GRASP_POINT 1
|
||||
#define TEST_COMPUTE_CALIB_PARA 0
|
||||
|
||||
#define TEST_GROUP 22
|
||||
#define TEST_GROUP 27
|
||||
#define TEST_TOP_VIEW_GROUP (TEST_GROUP - 8)
|
||||
#define TEST_TOP_ORIEN_GROUP (TEST_GROUP - 6)
|
||||
int main()
|
||||
@ -2407,13 +2545,13 @@ int main()
|
||||
int dataCalib = 0;
|
||||
int maxTimeStamp = 0;
|
||||
int clockPerSecond = 0;
|
||||
sprintf_s(_scan_dir, "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向_2\\");
|
||||
char _scan_file[256];
|
||||
double _F = 1247.95;// 1231.2; //1729.0;; //f
|
||||
for (int i = 1; i <= 28; i++)
|
||||
sprintf_s(_scan_dir, "F:\\ShangGu\\编织袋数据\\纸箱拆跺点云\\");
|
||||
char _scan_src_file[256];
|
||||
double _F = 1802.16; //1247.95;// 1231.2; //1729.0;; //f
|
||||
for (int i = 5; i <= 22; i++)
|
||||
{
|
||||
sprintf_s(_scan_file, "%sLaserLine%d.txt", _scan_dir, i);
|
||||
SVzNLXYZRGBDLaserLine* laser3DPoints_RGBD = vzReadLaserScanPointFromFile_XYZRGB(_scan_file, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond);
|
||||
sprintf_s(_scan_src_file, "%sLaserLine%d.txt", _scan_dir, i);
|
||||
SVzNLXYZRGBDLaserLine* laser3DPoints_RGBD = vzReadLaserScanPointFromFile_XYZRGB(_scan_src_file, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond, true);
|
||||
if (laser3DPoints_RGBD == NULL)
|
||||
continue;
|
||||
|
||||
@ -2429,7 +2567,11 @@ int main()
|
||||
#endif
|
||||
int vldLineNum = 0;
|
||||
SVzNL3DLaserLine* gridData = _convertToGridData_XYZRGB(laser3DPoints_RGBD, lineNum, _F, camPoseR, &vldLineNum);
|
||||
|
||||
char _out_file[256];
|
||||
sprintf_s(_out_file, "%sLaserLine%d_grid.txt", _scan_dir, i);
|
||||
_outputScanDataFile_self(_out_file, gridData, vldLineNum,
|
||||
lineV, maxTimeStamp, clockPerSecond);
|
||||
#if 0
|
||||
//生成水平扫描数据
|
||||
int hLineNum = gridData[0].nPositionCnt;
|
||||
SVzNL3DLaserLine* hScanData = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * hLineNum);
|
||||
@ -2448,15 +2590,11 @@ int main()
|
||||
hScanData[hLine].p3DPosition[m].pt3D.z = gridData[m].p3DPosition[hLine].pt3D.z;
|
||||
}
|
||||
}
|
||||
char _out_file[256];
|
||||
sprintf_s(_out_file, "%sLaserLine%d_grid.txt", _scan_dir, i);
|
||||
_outputScanDataFile_self(_out_file, gridData, vldLineNum,
|
||||
lineV, maxTimeStamp, clockPerSecond);
|
||||
sprintf_s(_out_file, "%sLaserLine%d_hScanData.txt", _scan_dir, i);
|
||||
_outputScanDataFile_removeZeros(_out_file, hScanData, hLineNum,
|
||||
lineV, maxTimeStamp, clockPerSecond);
|
||||
|
||||
printf("%s: convert done!\n", _scan_file);
|
||||
#endif
|
||||
printf("%s: convert done!\n", _scan_src_file);
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -2512,20 +2650,27 @@ int main()
|
||||
"F:\\ShangGu\\编织袋数据\\点云12_盐袋\\", //11
|
||||
"F:\\ShangGu\\编织袋数据\\点云13_现场测试\\", //12
|
||||
"F:\\ShangGu\\编织袋数据\\山东现场\\", //13
|
||||
"F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\", //14
|
||||
"F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向_2\\", //15
|
||||
"F:\\ShangGu\\编织袋数据\\侧抓数据\\", //16
|
||||
"F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250419\\", //17
|
||||
"F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-1\\", //18
|
||||
"F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-2\\", //19
|
||||
"F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-3\\", //20
|
||||
"F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-4\\", //21
|
||||
"F:\\ShangGu\\编织袋数据\\拆包模拟环境\\", //14
|
||||
"F:\\ShangGu\\纸箱拆垛数据\\vizum数据\\", //15
|
||||
"F:\\ShangGu\\纸箱拆垛数据\\380×480mm纸箱RGB点云及2D图像\\", //16
|
||||
"F:\\ShangGu\\纸箱拆垛数据\\380×580mm纸箱RGB点云及2d图像\\", //17
|
||||
"F:\\ShangGu\\纸箱拆垛数据\\480×580mm纸箱rgb点云及2d图像\\", //18
|
||||
|
||||
"F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\", //19
|
||||
"F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向_2\\", //20
|
||||
"F:\\ShangGu\\编织袋数据\\侧抓数据\\", //21
|
||||
"F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250419\\", //22
|
||||
"F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-1\\", //23
|
||||
"F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-2\\", //24
|
||||
"F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-3\\", //25
|
||||
"F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-4\\", //26
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[TEST_GROUP] = {
|
||||
{0,176},{1,200},{1,166},{122,141},{1,65},
|
||||
{1,29},{108,135},{0,200}, {1,200}, {1,12},
|
||||
{2,4}, {1,5}, {1,1}, {1,3},
|
||||
{2,4}, {1,5}, {1,1}, {1,3}, {11,11},
|
||||
{5,22},{1, 15},{1,15}, {1, 15},
|
||||
{1,21},{1,28},
|
||||
{3,3}, {1,51}, {4,83}, {1,74}, {1,61}, {1,84}
|
||||
};
|
||||
@ -2549,7 +2694,7 @@ int main()
|
||||
|
||||
SG_bagPositionParam algoParam;
|
||||
int endGroup = TEST_GROUP - 1;
|
||||
for (int grp = 14; grp <= 15; grp++)
|
||||
for (int grp = 17; grp <= 18; grp++)
|
||||
{
|
||||
if (grp < 10)
|
||||
{
|
||||
@ -2587,7 +2732,67 @@ int main()
|
||||
algoParam.growParam.minLTypeTreeLen = 50.0; //mm
|
||||
algoParam.growParam.minVTypeTreeLen = 50.0; //mm
|
||||
}
|
||||
else if ( (grp >= 14) && (grp <= 15))
|
||||
else if (grp == 14)
|
||||
{
|
||||
algoParam.bagParam.bagL = 750; //袋子长65cm
|
||||
algoParam.bagParam.bagW = 420; //袋子宽40cm
|
||||
algoParam.bagParam.bagH = 150; //袋子高16cm
|
||||
algoParam.growParam.maxLineSkipNum = 5;
|
||||
algoParam.growParam.yDeviation_max = 20.0;
|
||||
algoParam.growParam.maxSkipDistance = 20.0;
|
||||
algoParam.growParam.zDeviation_max = 80;// algoParam.bagParam.bagH / 2; //袋子高度1/2
|
||||
algoParam.growParam.minLTypeTreeLen = 50.0; //mm
|
||||
algoParam.growParam.minVTypeTreeLen = 50.0; //mm
|
||||
}
|
||||
else if (grp == 15) //纸箱拆垛数据\vizum数据
|
||||
{
|
||||
algoParam.bagParam.bagL = 400; //袋子长65cm
|
||||
algoParam.bagParam.bagW = 255; //袋子宽40cm
|
||||
algoParam.bagParam.bagH = 300; //袋子高16cm
|
||||
algoParam.growParam.maxLineSkipNum = 5;
|
||||
algoParam.growParam.yDeviation_max = 20.0;
|
||||
algoParam.growParam.maxSkipDistance = 20.0;
|
||||
algoParam.growParam.zDeviation_max = 80;// algoParam.bagParam.bagH / 2; //袋子高度1/2
|
||||
algoParam.growParam.minLTypeTreeLen = 50.0; //mm
|
||||
algoParam.growParam.minVTypeTreeLen = 50.0; //mm
|
||||
}
|
||||
else if (grp == 16) //纸箱拆垛数据\380×480mm纸箱RGB点云及2D图像
|
||||
{
|
||||
algoParam.bagParam.bagL = 480; //袋子长
|
||||
algoParam.bagParam.bagW = 380; //袋子宽
|
||||
algoParam.bagParam.bagH = 580; //袋子高
|
||||
algoParam.growParam.maxLineSkipNum = 5;
|
||||
algoParam.growParam.yDeviation_max = 20.0;
|
||||
algoParam.growParam.maxSkipDistance = 20.0;
|
||||
algoParam.growParam.zDeviation_max = 80;// algoParam.bagParam.bagH / 2; //袋子高度1/2
|
||||
algoParam.growParam.minLTypeTreeLen = 50.0; //mm
|
||||
algoParam.growParam.minVTypeTreeLen = 50.0; //mm
|
||||
}
|
||||
else if (grp == 17) //纸箱拆垛数据\380×580mm纸箱RGB点云及2d图像
|
||||
{
|
||||
algoParam.bagParam.bagL = 580; //袋子长
|
||||
algoParam.bagParam.bagW = 380; //袋子宽
|
||||
algoParam.bagParam.bagH = 480; //袋子高
|
||||
algoParam.growParam.maxLineSkipNum = 5;
|
||||
algoParam.growParam.yDeviation_max = 20.0;
|
||||
algoParam.growParam.maxSkipDistance = 20.0;
|
||||
algoParam.growParam.zDeviation_max = 80;// algoParam.bagParam.bagH / 2; //袋子高度1/2
|
||||
algoParam.growParam.minLTypeTreeLen = 50.0; //mm
|
||||
algoParam.growParam.minVTypeTreeLen = 50.0; //mm
|
||||
}
|
||||
else if (grp == 18) //纸箱拆垛数据\480×580mm纸箱rgb点云及2d图像
|
||||
{
|
||||
algoParam.bagParam.bagL = 580; //袋子长
|
||||
algoParam.bagParam.bagW = 480; //袋子宽
|
||||
algoParam.bagParam.bagH = 380; //袋子高
|
||||
algoParam.growParam.maxLineSkipNum = 5;
|
||||
algoParam.growParam.yDeviation_max = 20.0;
|
||||
algoParam.growParam.maxSkipDistance = 20.0;
|
||||
algoParam.growParam.zDeviation_max = 80;// algoParam.bagParam.bagH / 2; //袋子高度1/2
|
||||
algoParam.growParam.minLTypeTreeLen = 50.0; //mm
|
||||
algoParam.growParam.minVTypeTreeLen = 50.0; //mm
|
||||
}
|
||||
else if ( (grp >= 19) && (grp <= 20))
|
||||
{
|
||||
algoParam.bagParam.bagL = 750; //袋子长65cm
|
||||
algoParam.bagParam.bagW = 450; //袋子宽40cm
|
||||
@ -2612,10 +2817,10 @@ int main()
|
||||
algoParam.growParam.minVTypeTreeLen = 50.0; //mm
|
||||
}
|
||||
#if BAG_ALGO_USE_CORNER_FEATURE
|
||||
algoParam.cornerParam.cornerTh = 30; //45度角
|
||||
algoParam.cornerParam.scale = 15; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
|
||||
algoParam.cornerParam.cornerTh = 45; //45度角
|
||||
algoParam.cornerParam.scale = 50; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
|
||||
algoParam.cornerParam.minEndingGap = 20;// algoParam.bagParam.bagW / 4;
|
||||
algoParam.cornerParam.minEndingGap_z = algoParam.bagParam.bagH / 4;
|
||||
algoParam.cornerParam.minEndingGap_z = 20; // algoParam.bagParam.bagH / 4;
|
||||
algoParam.cornerParam.jumpCornerTh_1 = 60;
|
||||
algoParam.cornerParam.jumpCornerTh_2 = 15;
|
||||
#else
|
||||
@ -2695,13 +2900,28 @@ int main()
|
||||
poseCalibPara.invRMatrix[8] = 0.999699;
|
||||
|
||||
}
|
||||
else if ( grp == 14)
|
||||
else if (grp == 14)
|
||||
{
|
||||
char calibFile[250];
|
||||
sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\拆包模拟环境\\ground_calib_para2.txt");
|
||||
poseCalibPara = _readCalibPara(calibFile);
|
||||
}
|
||||
else if ( (grp >= 15) && (grp <= 18)) //箱子
|
||||
{
|
||||
algoParam.cornerParam.cornerTh = 45; //45度角
|
||||
algoParam.cornerParam.scale = 10; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
|
||||
algoParam.cornerParam.minEndingGap = 5;// algoParam.bagParam.bagW / 4;
|
||||
algoParam.cornerParam.minEndingGap_z = 5; // algoParam.bagParam.bagH / 4;
|
||||
algoParam.cornerParam.jumpCornerTh_1 = 60;
|
||||
algoParam.cornerParam.jumpCornerTh_2 = 15;
|
||||
}
|
||||
else if ( grp == 19)
|
||||
{
|
||||
char calibFile[250];
|
||||
sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\ground_calib_para.txt");
|
||||
poseCalibPara = _readCalibPara(calibFile);
|
||||
}
|
||||
else if (grp == 15)
|
||||
else if (grp == 20)
|
||||
{
|
||||
char calibFile[250];
|
||||
sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向_2\\ground_calib_para.txt");
|
||||
@ -2717,11 +2937,24 @@ int main()
|
||||
int dataCalib = 0;
|
||||
int maxTimeStamp = 0;
|
||||
int clockPerSecond = 0;
|
||||
sprintf_s(_scan_file, "%sLaserLine%d_grid.txt", dataPath[grp], fidx);
|
||||
SVzNL3DLaserLine* laser3DPoints = vzReadLaserScanPointFromFile_XYZ(_scan_file, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond);
|
||||
SVzNL3DLaserLine* laser3DPoints = NULL;
|
||||
if ((grp >= 16) && (grp <= 18))
|
||||
{
|
||||
sprintf_s(_scan_file, "%s%d.txt", dataPath[grp], fidx);
|
||||
laser3DPoints = vzReadXYZPointFromFile_XYZRGB(_scan_file, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond);
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf_s(_scan_file, "%sLaserLine%d_grid.txt", dataPath[grp], fidx);
|
||||
laser3DPoints = vzReadLaserScanPointFromFile_XYZ(_scan_file, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond);
|
||||
}
|
||||
if (laser3DPoints == NULL)
|
||||
continue;
|
||||
|
||||
#if 0 //数据转存
|
||||
sprintf_s(_scan_file, "%sLaserLine%d_grid_noRGB.txt", dataPath[grp], fidx);
|
||||
_outputScanDataFile_self(_scan_file, laser3DPoints, lineNum,
|
||||
lineV, maxTimeStamp, clockPerSecond);
|
||||
#endif
|
||||
algoParam.filterParam.continuityTh = 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声
|
||||
algoParam.filterParam.outlierTh = 5;
|
||||
|
||||
@ -2749,7 +2982,11 @@ int main()
|
||||
//调平，去除地面
|
||||
sg_lineDataR(&laser3DPoints[i], poseCalibPara.planeCalib, poseCalibPara.planeHeight);
|
||||
}
|
||||
|
||||
#if 0 //数据转存
|
||||
sprintf_s(_scan_file, "%sLaserLine%d_grid_RTadjust.txt", dataPath[grp], fidx);
|
||||
_outputScanDataFile_self(_scan_file, laser3DPoints, lineNum,
|
||||
lineV, maxTimeStamp, clockPerSecond);
|
||||
#endif
|
||||
std::vector<SSG_peakRgnInfo> objOps;
|
||||
sg_getBagPosition(laser3DPoints, lineNum, algoParam, poseCalibPara, objOps);
|
||||
#endif
|
||||
@ -2764,7 +3001,7 @@ int main()
|
||||
sprintf_s(_dbg_file, "%sresult\\LaserLine%d_result_img.png", dataPath[grp], fidx);
|
||||
cv::String imgName(_dbg_file);
|
||||
double rpy[3] = { -30, 15, 0 }; //{ 0,-45, 0 }; //
|
||||
double angleDrawLen = algoParam.bagParam.bagL / 3;
|
||||
double angleDrawLen = algoParam.bagParam.bagL * 0.4;
|
||||
_genXOYProjectionImage(imgName, laser3DPoints, lineNum, objOps, rpy, angleDrawLen);
|
||||
#endif
|
||||
#if 0
|
||||
@ -2788,8 +3025,7 @@ int main()
|
||||
}
|
||||
else if (grp < TEST_TOP_ORIEN_GROUP)
|
||||
{
|
||||
double FBVldPtRatioTh;
|
||||
|
||||
fidx = 0;
|
||||
SSG_hsvCmpParam colorCmpParam; //色度和色饱和度比较门限，小于门限为同一颜色
|
||||
colorCmpParam.hueTh = 15.0;
|
||||
colorCmpParam.saturateTh = 120.0; //60
|
||||
@ -2835,7 +3071,7 @@ int main()
|
||||
int kkk = 1;
|
||||
//行处理
|
||||
//调平，去除地面
|
||||
sg_lineDataR_RGBD(&laser3DPoints[i], poseCalibPara.planeCalib, poseCalibPara.planeHeight);
|
||||
//sg_lineDataR_RGBD(&laser3DPoints[i], poseCalibPara.planeCalib, poseCalibPara.planeHeight);
|
||||
}
|
||||
#if 0
|
||||
char _out_file[256];
|
||||
|
||||
172
lapWeldDetection/lapWeldDetection.vcxproj
Normal file
172
lapWeldDetection/lapWeldDetection.vcxproj
Normal file
@ -0,0 +1,172 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||
<ItemGroup Label="ProjectConfigurations">
|
||||
<ProjectConfiguration Include="Debug|Win32">
|
||||
<Configuration>Debug</Configuration>
|
||||
<Platform>Win32</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Release|Win32">
|
||||
<Configuration>Release</Configuration>
|
||||
<Platform>Win32</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Debug|x64">
|
||||
<Configuration>Debug</Configuration>
|
||||
<Platform>x64</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Release|x64">
|
||||
<Configuration>Release</Configuration>
|
||||
<Platform>x64</Platform>
|
||||
</ProjectConfiguration>
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ClCompile Include="..\sourceCode\SX_lapWeldDetection.cpp" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ClInclude Include="..\sourceCode\SX_lapWeldDetection_Export.h" />
|
||||
</ItemGroup>
|
||||
<PropertyGroup Label="Globals">
|
||||
<VCProjectVersion>16.0</VCProjectVersion>
|
||||
<Keyword>Win32Proj</Keyword>
|
||||
<ProjectGuid>{0c0b9b7a-9bf2-4157-bffd-d12012e39fa0}</ProjectGuid>
|
||||
<RootNamespace>lapWeldDetection</RootNamespace>
|
||||
<WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion>
|
||||
</PropertyGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
|
||||
<ConfigurationType>DynamicLibrary</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
|
||||
<ConfigurationType>DynamicLibrary</ConfigurationType>
|
||||
<UseDebugLibraries>false</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
|
||||
<ConfigurationType>DynamicLibrary</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
|
||||
<ConfigurationType>DynamicLibrary</ConfigurationType>
|
||||
<UseDebugLibraries>false</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
|
||||
<ImportGroup Label="ExtensionSettings">
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="Shared">
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<PropertyGroup Label="UserMacros" />
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<LinkIncremental>true</LinkIncremental>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<LinkIncremental>false</LinkIncremental>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<LinkIncremental>true</LinkIncremental>
|
||||
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
|
||||
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<LinkIncremental>false</LinkIncremental>
|
||||
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
|
||||
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
|
||||
</PropertyGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>WIN32;_DEBUG;LAPWELDDETECTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
<PrecompiledHeader>Use</PrecompiledHeader>
|
||||
<PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Windows</SubSystem>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<EnableUAC>false</EnableUAC>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>WIN32;NDEBUG;LAPWELDDETECTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
<PrecompiledHeader>Use</PrecompiledHeader>
|
||||
<PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Windows</SubSystem>
|
||||
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||
<OptimizeReferences>true</OptimizeReferences>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<EnableUAC>false</EnableUAC>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>_DEBUG;LAPWELDDETECTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
<PrecompiledHeader>NotUsing</PrecompiledHeader>
|
||||
<PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
|
||||
<AdditionalIncludeDirectories>..\..\thirdParty\opencv\build\include;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Windows</SubSystem>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<EnableUAC>false</EnableUAC>
|
||||
<AdditionalDependencies>opencv_world480d.lib;baseAlgorithm.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||
<AdditionalLibraryDirectories>..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>NDEBUG;LAPWELDDETECTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
<PrecompiledHeader>NotUsing</PrecompiledHeader>
|
||||
<PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
|
||||
<AdditionalIncludeDirectories>..\..\thirdParty\opencv\build\include;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Windows</SubSystem>
|
||||
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||
<OptimizeReferences>true</OptimizeReferences>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<EnableUAC>false</EnableUAC>
|
||||
<AdditionalDependencies>opencv_world480.lib;baseAlgorithm.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||
<AdditionalLibraryDirectories>..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Release;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
|
||||
<ImportGroup Label="ExtensionTargets">
|
||||
</ImportGroup>
|
||||
</Project>
|
||||
829
lapWeldDetection_test/lapWeldDetection_test.cpp
Normal file
829
lapWeldDetection_test/lapWeldDetection_test.cpp
Normal file
@ -0,0 +1,829 @@
|
||||
// glovePositioning_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
|
||||
//
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
#include <vector>
|
||||
#include <stdio.h>
|
||||
#include <VZNL_Types.h>
|
||||
#include "direct.h"
|
||||
#include <string>
|
||||
#include "SX_lapWeldDetection_Export.h"
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <Windows.h>
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int r;
|
||||
int g;
|
||||
int b;
|
||||
}SG_color;
|
||||
|
||||
void vzReadLaserScanPointFromFile_plyTxt(const char* fileName, std::vector< SVzNL3DPoint>& scanData, bool exchangeXY)
|
||||
{
|
||||
std::ifstream inputFile(fileName);
|
||||
std::string linedata;
|
||||
|
||||
if (inputFile.is_open() == false)
|
||||
return;
|
||||
|
||||
while (std::getline(inputFile, linedata))
|
||||
{
|
||||
if (linedata.empty())
|
||||
continue;
|
||||
|
||||
double X, Y, Z;
|
||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &X, &Y, &Z);
|
||||
SVzNL3DPoint a_pt;
|
||||
if (true == exchangeXY)
|
||||
{
|
||||
a_pt.x = Y; //将扫描线调整为Y方向；扫描方向为X方向
|
||||
a_pt.y = X;
|
||||
}
|
||||
else
|
||||
{
|
||||
a_pt.x = X;
|
||||
a_pt.y = Y;
|
||||
}
|
||||
a_pt.z = Z;
|
||||
scanData.push_back(a_pt);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
void _outputScanDataFile_vector(char* fileName, std::vector<std::vector<SVzNL3DPosition>>& scanLines, bool removeZeros, int* headNullLines)
|
||||
{
|
||||
std::ofstream sw(fileName);
|
||||
int lineNum = (int)scanLines.size();
|
||||
if (lineNum == 0)
|
||||
return;
|
||||
|
||||
sw << "LineNum:" << lineNum << std::endl;
|
||||
sw << "DataType: 0" << std::endl;
|
||||
sw << "ScanSpeed: 0" << std::endl;
|
||||
sw << "PointAdjust: 1" << std::endl;
|
||||
sw << "MaxTimeStamp: 0_0" << std::endl;
|
||||
|
||||
int lineIdx = 0;
|
||||
int null_lines = 0;
|
||||
bool counterNull = true;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
int linePtNum = (int)scanLines[line].size();
|
||||
if (linePtNum == 0)
|
||||
continue;
|
||||
|
||||
if (true == removeZeros)
|
||||
{
|
||||
int vldPtNum = 0;
|
||||
for (int i = 0; i < linePtNum; i++)
|
||||
{
|
||||
if (scanLines[line][i].pt3D.z > 1e-4)
|
||||
vldPtNum++;
|
||||
}
|
||||
linePtNum = vldPtNum;
|
||||
}
|
||||
sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl;
|
||||
lineIdx++;
|
||||
bool isNull = true;
|
||||
for (int i = 0; i < linePtNum; i++)
|
||||
{
|
||||
SVzNL3DPoint* pt3D = &scanLines[line][i].pt3D;
|
||||
if ((pt3D->z > 1e-4) && (isNull == true))
|
||||
isNull = false;
|
||||
if ((true == removeZeros) && (pt3D->z < 1e-4))
|
||||
continue;
|
||||
float x = (float)pt3D->x;
|
||||
float y = (float)pt3D->y;
|
||||
float z = (float)pt3D->z;
|
||||
sw << "{ " << x << "," << y << "," << z << " }-";
|
||||
sw << "{0,0}-{0,0}" << std::endl;
|
||||
}
|
||||
if (true == counterNull)
|
||||
{
|
||||
if (true == isNull)
|
||||
null_lines++;
|
||||
else
|
||||
counterNull = false;
|
||||
}
|
||||
}
|
||||
*headNullLines = null_lines;
|
||||
sw.close();
|
||||
}
|
||||
|
||||
//输出水平扫描数据
|
||||
void _outputScanDataFile_vector_h(char* fileName, std::vector<std::vector<SVzNL3DPosition>>& scanLines)
|
||||
{
|
||||
std::ofstream sw(fileName);
|
||||
int lineNum = (int)scanLines[0].size();
|
||||
if (lineNum == 0)
|
||||
return;
|
||||
|
||||
sw << "LineNum:" << lineNum << std::endl;
|
||||
sw << "DataType: 0" << std::endl;
|
||||
sw << "ScanSpeed: 0" << std::endl;
|
||||
sw << "PointAdjust: 1" << std::endl;
|
||||
sw << "MaxTimeStamp: 0_0" << std::endl;
|
||||
|
||||
int linePtNum = (int)scanLines.size();
|
||||
int lineIdx = 0;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
|
||||
sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl;
|
||||
lineIdx++;
|
||||
bool isNull = true;
|
||||
for (int i = 0; i < linePtNum; i++)
|
||||
{
|
||||
SVzNL3DPoint* pt3D = &scanLines[i][line].pt3D;
|
||||
|
||||
float x = (float)pt3D->y;
|
||||
float y = (float)pt3D->x;
|
||||
float z = (float)pt3D->z;
|
||||
sw << "{ " << x << "," << y << "," << z << " }-";
|
||||
sw << "{0,0}-{0,0}" << std::endl;
|
||||
}
|
||||
}
|
||||
sw.close();
|
||||
}
|
||||
|
||||
void _outputRGBDScanLapWeld_RGBD(
|
||||
char* fileName,
|
||||
std::vector<std::vector<SVzNL3DPosition>>& scanLines,
|
||||
std::vector< std::vector<SVzNL3DPoint>> weldOPs)
|
||||
{
|
||||
int lineNum = (int)scanLines.size();
|
||||
std::ofstream sw(fileName);
|
||||
int realLines = lineNum;
|
||||
if (weldOPs.size() > 0)
|
||||
realLines++;
|
||||
|
||||
sw << "LineNum:" << realLines << std::endl;
|
||||
sw << "DataType: 0" << std::endl;
|
||||
sw << "ScanSpeed: 0" << std::endl;
|
||||
sw << "PointAdjust: 1" << std::endl;
|
||||
sw << "MaxTimeStamp: 0_0" << std::endl;
|
||||
|
||||
int maxLineIndex = 0;
|
||||
int max_stamp = 0;
|
||||
|
||||
SG_color rgb = { 0, 0, 0 };
|
||||
|
||||
SG_color objColor[8] = {
|
||||
{245,222,179},//淡黄色
|
||||
{210,105, 30},//巧克力色
|
||||
{240,230,140},//黄褐色
|
||||
{135,206,235},//天蓝色
|
||||
{250,235,215},//古董白
|
||||
{189,252,201},//薄荷色
|
||||
{221,160,221},//梅红色
|
||||
{188,143,143},//玫瑰红色
|
||||
};
|
||||
int size = 1;
|
||||
int lineIdx = 0;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
int linePtNum = (int)scanLines[line].size();
|
||||
if (linePtNum == 0)
|
||||
continue;
|
||||
|
||||
sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl;
|
||||
lineIdx++;
|
||||
for (int i = 0; i < linePtNum; i++)
|
||||
{
|
||||
SVzNL3DPosition* pt3D = &scanLines[line][i];
|
||||
int featureType_v = pt3D->nPointIdx & 0xffff;
|
||||
int featureType_h = featureType_v >> 4;
|
||||
featureType_v &= 0xff;
|
||||
if (LINE_FEATURE_PEAK_TOP == featureType_v)
|
||||
{
|
||||
rgb = { 255, 97, 0 };
|
||||
size = 5;
|
||||
}
|
||||
else if (LINE_FEATURE_PEAK_TOP == featureType_h)
|
||||
{
|
||||
rgb = { 97, 255, 0 };
|
||||
size = 5;
|
||||
}
|
||||
else
|
||||
{
|
||||
rgb = { 200, 200, 200 };
|
||||
size = 1;
|
||||
}
|
||||
float x = (float)pt3D->pt3D.x;
|
||||
float y = (float)pt3D->pt3D.y;
|
||||
float z = (float)pt3D->pt3D.z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
}
|
||||
}
|
||||
if (weldOPs.size() > 0)
|
||||
{
|
||||
int weldNum = (int)weldOPs.size();
|
||||
int linePtNum = 0;
|
||||
for (int i = 0; i < weldNum; i++)
|
||||
linePtNum += (int)weldOPs[i].size();
|
||||
sw << "Line_" << lineNum << "_0_" << linePtNum + 1 << std::endl;
|
||||
|
||||
rgb = { 0, 0, 255 };
|
||||
size = 25;
|
||||
for (int i = 0; i < weldNum; i++)
|
||||
{
|
||||
rgb = objColor[i % 8];
|
||||
for (int j = 0; j < (int)weldOPs[i].size(); j++)
|
||||
{
|
||||
float x = (float)weldOPs[i][j].x;
|
||||
float y = (float)weldOPs[i][j].y;
|
||||
float z = (float)weldOPs[i][j].z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
}
|
||||
}
|
||||
//加一个点，用于跳过显示工具bug
|
||||
rgb = objColor[0];
|
||||
float x = (float)weldOPs[0][0].x;
|
||||
float y = (float)weldOPs[0][0].y;
|
||||
float z = (float)weldOPs[0][0].z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
}
|
||||
sw.close();
|
||||
}
|
||||
|
||||
|
||||
void _outputScanDataFile_ptr(char* fileName, SVzNL3DLaserLine* scanData, int lineNum)
|
||||
{
|
||||
std::ofstream sw(fileName);
|
||||
sw << "LineNum:" << lineNum << std::endl;
|
||||
sw << "DataType: 0" << std::endl;
|
||||
sw << "ScanSpeed: 0" << std::endl;
|
||||
sw << "PointAdjust: 1" << std::endl;
|
||||
sw << "MaxTimeStamp: 0_0" << std::endl;
|
||||
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
sw << "Line_" << line << "_" << scanData[line].nTimeStamp << "_" << scanData[line].nPositionCnt << std::endl;
|
||||
for (int i = 0; i < scanData[line].nPositionCnt; i++)
|
||||
{
|
||||
SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i];
|
||||
float x = (float)pt3D->pt3D.x;
|
||||
float y = (float)pt3D->pt3D.y;
|
||||
float z = (float)pt3D->pt3D.z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}" << std::endl;
|
||||
}
|
||||
}
|
||||
sw.close();
|
||||
}
|
||||
|
||||
|
||||
#define DATA_VER_OLD 0
|
||||
#define DATA_VER_NEW 1
|
||||
#define DATA_VER_FROM_CUSTOM 2
|
||||
#define VZ_LASER_LINE_PT_MAX_NUM 4096
|
||||
SVzNL3DLaserLine* vzReadLaserScanPointFromFile_XYZ(const char* fileName, int* scanLineNum, float* scanV,
|
||||
int* dataCalib, int* scanMaxStamp, int* canClockUnit)
|
||||
{
|
||||
std::ifstream inputFile(fileName);
|
||||
std::string linedata;
|
||||
|
||||
if (inputFile.is_open() == false)
|
||||
return NULL;
|
||||
|
||||
SVzNL3DLaserLine* _scanLines = NULL;
|
||||
|
||||
int lines = 0;
|
||||
int dataElements = 4;
|
||||
int firstIndex = -1;
|
||||
|
||||
int dataFileVer = DATA_VER_OLD;
|
||||
std::getline(inputFile, linedata); //第一行
|
||||
int lineNum = 0;
|
||||
if (0 == strncmp("LineNum:", linedata.c_str(), 8))
|
||||
{
|
||||
dataFileVer = DATA_VER_NEW;
|
||||
sscanf_s(linedata.c_str(), "LineNum:%d", &lines);
|
||||
if (lines == 0)
|
||||
return NULL;
|
||||
lineNum = lines;
|
||||
_scanLines = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1));
|
||||
memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1));
|
||||
if (scanLineNum)
|
||||
*scanLineNum = lines;
|
||||
}
|
||||
else if (0 == strncmp("LineNum_", linedata.c_str(), 8))
|
||||
{
|
||||
dataFileVer = DATA_VER_OLD;
|
||||
sscanf_s(linedata.c_str(), "LineNum_%d", &lines);
|
||||
if (lines == 0)
|
||||
return NULL;
|
||||
lineNum = lines;
|
||||
_scanLines = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1));
|
||||
memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1));
|
||||
if (scanLineNum)
|
||||
*scanLineNum = lines;
|
||||
}
|
||||
if (_scanLines == NULL)
|
||||
return NULL;
|
||||
|
||||
int ptNum = 0;
|
||||
int lineIdx = -1;
|
||||
int ptIdx = 0;
|
||||
SVzNL3DPosition* p3DPoint = NULL;
|
||||
if (dataFileVer == DATA_VER_NEW)
|
||||
{
|
||||
while (getline(inputFile, linedata))
|
||||
{
|
||||
if (0 == strncmp("ScanSpeed:", linedata.c_str(), 10))
|
||||
{
|
||||
double lineV = 0;
|
||||
sscanf_s(linedata.c_str(), "ScanSpeed:%lf", &lineV);
|
||||
if (scanV)
|
||||
*scanV = (float)lineV;
|
||||
}
|
||||
else if (0 == strncmp("PointAdjust:", linedata.c_str(), 12))
|
||||
{
|
||||
int ptAdjusted = 0;
|
||||
sscanf_s(linedata.c_str(), "PointAdjust:%d", &ptAdjusted);
|
||||
if (dataCalib)
|
||||
*dataCalib = ptAdjusted;
|
||||
}
|
||||
else if (0 == strncmp("MaxTimeStamp:", linedata.c_str(), 13))
|
||||
{
|
||||
unsigned int maxTimeStamp = 0;
|
||||
unsigned int timePerStamp = 0;
|
||||
sscanf_s(linedata.c_str(), "MaxTimeStamp:%u_%u", &maxTimeStamp, &timePerStamp);
|
||||
if (scanMaxStamp)
|
||||
*scanMaxStamp = maxTimeStamp;
|
||||
if (canClockUnit)
|
||||
*canClockUnit = timePerStamp;
|
||||
}
|
||||
else if (0 == strncmp("Line_", linedata.c_str(), 5))
|
||||
{
|
||||
int lineIndex;
|
||||
unsigned int timeStamp;
|
||||
sscanf_s(linedata.c_str(), "Line_%d_%u_%d", &lineIndex, &timeStamp, &ptNum);
|
||||
if (firstIndex < 0)
|
||||
firstIndex = lineIndex;
|
||||
|
||||
lineIndex = lineIndex - firstIndex;
|
||||
if ((lineIndex < 0) || (lineIndex >= lines))
|
||||
break;
|
||||
|
||||
//new Line
|
||||
lineIdx++;
|
||||
if (ptNum > 0)
|
||||
{
|
||||
p3DPoint = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * ptNum);
|
||||
memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * ptNum);
|
||||
}
|
||||
else
|
||||
p3DPoint = NULL;
|
||||
_scanLines[lineIdx].nPositionCnt = 0;
|
||||
_scanLines[lineIdx].nTimeStamp = timeStamp;
|
||||
_scanLines[lineIdx].p3DPosition = p3DPoint;
|
||||
|
||||
}
|
||||
else if (0 == strncmp("{", linedata.c_str(), 1))
|
||||
{
|
||||
float X, Y, Z;
|
||||
int imageY = 0;
|
||||
float leftX, leftY;
|
||||
float rightX, rightY;
|
||||
sscanf_s(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY);
|
||||
int id = _scanLines[lineIdx].nPositionCnt;
|
||||
if (id < ptNum)
|
||||
{
|
||||
p3DPoint[id].pt3D.x = X;
|
||||
p3DPoint[id].pt3D.y = Y;
|
||||
p3DPoint[id].pt3D.z = Z;
|
||||
_scanLines[lineIdx].nPositionCnt = id + 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
else if (dataFileVer == DATA_VER_OLD)
|
||||
{
|
||||
while (getline(inputFile, linedata))
|
||||
{
|
||||
if (0 == strncmp("DataElements_", linedata.c_str(), 13))
|
||||
{
|
||||
sscanf_s(linedata.c_str(), "DataElements_%d", &dataElements);
|
||||
if ((dataElements != 3) && (dataElements != 4))
|
||||
break;
|
||||
}
|
||||
if (0 == strncmp("LineV_", linedata.c_str(), 6))
|
||||
{
|
||||
double lineV = 0;
|
||||
sscanf_s(linedata.c_str(), "LineV_%lf", &lineV);
|
||||
}
|
||||
else if (0 == strncmp("Line_", linedata.c_str(), 5))
|
||||
{
|
||||
int lineIndex;
|
||||
unsigned int timeStamp;
|
||||
sscanf_s(linedata.c_str(), "Line_%d_%u", &lineIndex, &timeStamp);
|
||||
#if 0
|
||||
if (scanLineListTail == NULL)
|
||||
firstIndex = lineIndex;
|
||||
#endif
|
||||
lineIndex = lineIndex - firstIndex;
|
||||
if ((lineIndex < 0) || (lineIndex >= lines))
|
||||
break;
|
||||
//new Line
|
||||
//new Line
|
||||
lineIdx++;
|
||||
p3DPoint = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * VZ_LASER_LINE_PT_MAX_NUM);
|
||||
memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * VZ_LASER_LINE_PT_MAX_NUM);
|
||||
_scanLines[lineIdx].nPositionCnt = 0;
|
||||
_scanLines[lineIdx].nTimeStamp = timeStamp;
|
||||
_scanLines[lineIdx].p3DPosition = p3DPoint;
|
||||
}
|
||||
else if (0 == strncmp("(", linedata.c_str(), 1))
|
||||
{
|
||||
float X, Y, Z;
|
||||
int imageY = 0;
|
||||
if (dataElements == 4)
|
||||
sscanf_s(linedata.c_str(), "(%f,%f,%f,%d)", &X, &Y, &Z, &imageY);
|
||||
else
|
||||
sscanf_s(linedata.c_str(), "(%f,%f,%f)", &X, &Y, &Z);
|
||||
int id = _scanLines[lineIdx].nPositionCnt;
|
||||
if (id < VZ_LASER_LINE_PT_MAX_NUM)
|
||||
{
|
||||
p3DPoint[id].pt3D.x = X;
|
||||
p3DPoint[id].pt3D.y = Y;
|
||||
p3DPoint[id].pt3D.z = Z;
|
||||
_scanLines[lineIdx].nPositionCnt = id + 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
inputFile.close();
|
||||
return _scanLines;
|
||||
}
|
||||
|
||||
void vzReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector<std::vector< SVzNL3DPosition>>& scanData)
|
||||
{
|
||||
std::ifstream inputFile(fileName);
|
||||
std::string linedata;
|
||||
|
||||
if (inputFile.is_open() == false)
|
||||
return;
|
||||
|
||||
std::vector< SVzNL3DPosition> a_line;
|
||||
int ptIdx = 0;
|
||||
while (getline(inputFile, linedata))
|
||||
{
|
||||
if (0 == strncmp("Line_", linedata.c_str(), 5))
|
||||
{
|
||||
int ptSize = (int)a_line.size();
|
||||
if (ptSize > 0)
|
||||
{
|
||||
scanData.push_back(a_line);
|
||||
}
|
||||
a_line.clear();
|
||||
ptIdx = 0;
|
||||
}
|
||||
else if (0 == strncmp("{", linedata.c_str(), 1))
|
||||
{
|
||||
float X, Y, Z;
|
||||
int imageY = 0;
|
||||
float leftX, leftY;
|
||||
float rightX, rightY;
|
||||
sscanf_s(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY);
|
||||
SVzNL3DPosition a_pt;
|
||||
a_pt.pt3D.x = X;
|
||||
a_pt.pt3D.y = Y;
|
||||
a_pt.pt3D.z = Z;
|
||||
a_pt.nPointIdx = ptIdx;
|
||||
ptIdx++;
|
||||
a_line.push_back(a_pt);
|
||||
}
|
||||
}
|
||||
//last line
|
||||
int ptSize = (int)a_line.size();
|
||||
if (ptSize > 0)
|
||||
{
|
||||
scanData.push_back(a_line);
|
||||
a_line.clear();
|
||||
}
|
||||
|
||||
inputFile.close();
|
||||
return;
|
||||
}
|
||||
|
||||
void _outputCalibPara(char* fileName, SSG_planeCalibPara calibPara)
|
||||
{
|
||||
std::ofstream sw(fileName);
|
||||
char dataStr[250];
|
||||
//调平矩阵
|
||||
sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[0], calibPara.planeCalib[1], calibPara.planeCalib[2]);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[3], calibPara.planeCalib[4], calibPara.planeCalib[5]);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[6], calibPara.planeCalib[7], calibPara.planeCalib[8]);
|
||||
sw << dataStr << std::endl;
|
||||
//地面高度
|
||||
sprintf_s(dataStr, 250, "%g", calibPara.planeHeight);
|
||||
sw << dataStr << std::endl;
|
||||
//反向旋转矩阵
|
||||
sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[0], calibPara.invRMatrix[1], calibPara.invRMatrix[2]);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[3], calibPara.invRMatrix[4], calibPara.invRMatrix[5]);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[6], calibPara.invRMatrix[7], calibPara.invRMatrix[8]);
|
||||
sw << dataStr << std::endl;
|
||||
|
||||
sw.close();
|
||||
}
|
||||
|
||||
SSG_planeCalibPara _readCalibPara(char* fileName)
|
||||
{
|
||||
//设置初始结果
|
||||
double initCalib[9] = {
|
||||
1.0, 0.0, 0.0,
|
||||
0.0, 1.0, 0.0,
|
||||
0.0, 0.0, 1.0 };
|
||||
SSG_planeCalibPara planePara;
|
||||
for (int i = 0; i < 9; i++)
|
||||
planePara.planeCalib[i] = initCalib[i];
|
||||
planePara.planeHeight = -1.0;
|
||||
for (int i = 0; i < 9; i++)
|
||||
planePara.invRMatrix[i] = initCalib[i];
|
||||
|
||||
std::ifstream inputFile(fileName);
|
||||
std::string linedata;
|
||||
|
||||
if (inputFile.is_open() == false)
|
||||
return planePara;
|
||||
|
||||
//调平矩阵
|
||||
std::getline(inputFile, linedata);
|
||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[0], &planePara.planeCalib[1], &planePara.planeCalib[2]);
|
||||
std::getline(inputFile, linedata);
|
||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[3], &planePara.planeCalib[4], &planePara.planeCalib[5]);
|
||||
std::getline(inputFile, linedata);
|
||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[6], &planePara.planeCalib[7], &planePara.planeCalib[8]);
|
||||
//地面高度
|
||||
std::getline(inputFile, linedata);
|
||||
sscanf_s(linedata.c_str(), "%lf", &planePara.planeHeight);
|
||||
//反向旋转矩阵
|
||||
std::getline(inputFile, linedata);
|
||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[0], &planePara.invRMatrix[1], &planePara.invRMatrix[2]);
|
||||
std::getline(inputFile, linedata);
|
||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[3], &planePara.invRMatrix[4], &planePara.invRMatrix[5]);
|
||||
std::getline(inputFile, linedata);
|
||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[6], &planePara.invRMatrix[7], &planePara.invRMatrix[8]);
|
||||
|
||||
inputFile.close();
|
||||
return planePara;
|
||||
}
|
||||
|
||||
void _getRoiClouds(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
int startLine,
|
||||
int endLine,
|
||||
int startPtIdx,
|
||||
int endPtIdx,
|
||||
std::vector< std::vector<SVzNL3DPosition>>& roiScanLines)
|
||||
{
|
||||
for (int i = startLine; i < endLine; i++)
|
||||
{
|
||||
if (i >= scanLines.size())
|
||||
break;
|
||||
|
||||
std::vector<SVzNL3DPosition> cut_line;
|
||||
std::vector<SVzNL3DPosition>& a_line = scanLines[i];
|
||||
for (int j = startPtIdx; j < endPtIdx; j++)
|
||||
{
|
||||
SVzNL3DPosition a_pt;
|
||||
if (j >= a_line.size())
|
||||
{
|
||||
a_pt.nPointIdx = 0;
|
||||
a_pt.pt3D = { 0,0,0 };
|
||||
}
|
||||
else
|
||||
a_pt = a_line[j];
|
||||
cut_line.push_back(a_pt);
|
||||
}
|
||||
roiScanLines.push_back(cut_line);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
void _convertToGridData(std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
std::vector< std::vector<SVzNL3DPosition>>& gridScanLines,
|
||||
double y_step)
|
||||
{
|
||||
double y_min = scanLines[0][0].pt3D.y;
|
||||
double y_max = y_min;
|
||||
int lineNum = (int)scanLines.size();
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
std::vector<SVzNL3DPosition>& a_line = scanLines[line];
|
||||
int ptNum = (int)a_line.size();
|
||||
for (int i = 0; i < ptNum; i++)
|
||||
{
|
||||
if (y_min > a_line[i].pt3D.y)
|
||||
y_min = a_line[i].pt3D.y;
|
||||
if (y_max < a_line[i].pt3D.y)
|
||||
y_max = a_line[i].pt3D.y;
|
||||
}
|
||||
}
|
||||
double half_step = y_step / 2;
|
||||
int gridPtNum = (int)((y_max - y_min + half_step) / y_step) + 1;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
std::vector<SVzNL3DPosition> grid_line;
|
||||
grid_line.resize(gridPtNum);
|
||||
for (int i = 0; i < gridPtNum; i++)
|
||||
grid_line[i] = { 0, {0.0, 0.0, 0.0} };
|
||||
|
||||
std::vector<SVzNL3DPosition>& a_line = scanLines[line];
|
||||
int ptNum = (int)a_line.size();
|
||||
for (int i = 0; i < ptNum; i++)
|
||||
{
|
||||
int ptIdx = (int)((a_line[i].pt3D.y + half_step - y_min) / y_step);
|
||||
grid_line[ptIdx] = a_line[i];
|
||||
}
|
||||
gridScanLines.push_back(grid_line);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
#define CONVERT_TO_GRID 0
|
||||
#define TEST_COMPUTE_CALIB_PARA 0
|
||||
#define TEST_COMPUTE_GLOVE_POSITION 1
|
||||
#define TEST_GROUP 1
|
||||
int main()
|
||||
{
|
||||
const char* dataPath[TEST_GROUP] = {
|
||||
"F:\\ShangGu\\项目\\钢板搭接焊缝检测\\数据\\", //0
|
||||
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[TEST_GROUP] = {
|
||||
{0,6}
|
||||
};
|
||||
|
||||
#if CONVERT_TO_GRID
|
||||
int convertGrp = 0;
|
||||
for (int fidx = fileIdx[convertGrp].nMin; fidx <= fileIdx[convertGrp].nMax; fidx++)
|
||||
{
|
||||
char _scan_file[256];
|
||||
sprintf_s(_scan_file, "%sglove_%d.txt", dataPath[convertGrp], fidx);
|
||||
std::vector< SVzNL3DPoint> scanData;
|
||||
bool exchangeXY = true;
|
||||
vzReadLaserScanPointFromFile_plyTxt(_scan_file, scanData, exchangeXY);
|
||||
//将数据恢复为按扫描线存储格式
|
||||
std::vector< std::vector<SVzNL3DPosition>> scanLines;
|
||||
wd_getScanLines(scanData, scanLines);
|
||||
|
||||
double y_step;
|
||||
if ((fidx == 4) || (fidx == 5))
|
||||
y_step = 0.106;
|
||||
else
|
||||
y_step = 0.091;
|
||||
std::vector< std::vector<SVzNL3DPosition>> grid_scanLines;
|
||||
_convertToGridData(scanLines, grid_scanLines, y_step);
|
||||
sprintf_s(_scan_file, "%sglove_%d_scanLine.txt", dataPath[convertGrp], fidx);
|
||||
int headNullLines = 0;
|
||||
_outputScanDataFile_vector(_scan_file, grid_scanLines, false, &headNullLines);
|
||||
printf("%s: head null lines = %d\n", _scan_file, headNullLines);
|
||||
#if 1
|
||||
sprintf_s(_scan_file, "%sglove_%d_scanLine_h.txt", dataPath[convertGrp], fidx);
|
||||
_outputScanDataFile_vector_h(_scan_file, grid_scanLines);
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
#if TEST_COMPUTE_CALIB_PARA
|
||||
char _calib_datafile[256];
|
||||
sprintf_s(_calib_datafile, "F:\\ShangGu\\项目\\钢板搭接焊缝检测\\数据\\scanData_ground_1.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);
|
||||
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);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if TEST_COMPUTE_GLOVE_POSITION
|
||||
for (int grp = 0; grp <= 0; grp++)
|
||||
{
|
||||
SSG_planeCalibPara poseCalibPara;
|
||||
//初始化成单位阵
|
||||
poseCalibPara.planeCalib[0] = 1.0;
|
||||
poseCalibPara.planeCalib[1] = 0.0;
|
||||
poseCalibPara.planeCalib[2] = 0.0;
|
||||
poseCalibPara.planeCalib[3] = 0.0;
|
||||
poseCalibPara.planeCalib[4] = 1.0;
|
||||
poseCalibPara.planeCalib[5] = 0.0;
|
||||
poseCalibPara.planeCalib[6] = 0.0;
|
||||
poseCalibPara.planeCalib[7] = 0.0;
|
||||
poseCalibPara.planeCalib[8] = 1.0;
|
||||
poseCalibPara.planeHeight = -1.0;
|
||||
for (int i = 0; i < 9; i++)
|
||||
poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i];
|
||||
char calibFile[250];
|
||||
sprintf_s(calibFile, "F:\\ShangGu\\项目\\钢板搭接焊缝检测\\数据\\ground_calib_para.txt");
|
||||
poseCalibPara = _readCalibPara(calibFile);
|
||||
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
//fidx =1;
|
||||
char _scan_file[256];
|
||||
sprintf_s(_scan_file, "%sscanData_%d.txt", dataPath[grp], fidx);
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanLines;
|
||||
vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
|
||||
|
||||
long t1 = (long)GetTickCount64();//统计时间
|
||||
|
||||
for (int i = 0, i_max = (int)scanLines.size(); i < i_max; i++)
|
||||
{
|
||||
if (i == 14)
|
||||
int kkk = 1;
|
||||
//行处理
|
||||
//调平，去除地面
|
||||
sx_lineDataR(scanLines[i], poseCalibPara.planeCalib, -1);// calibPara.planeHeight);
|
||||
}
|
||||
#if 0
|
||||
char _out_file[256];
|
||||
sprintf_s(_out_file, "%sscanData_%d_calib.txt", dataPath[grp], fidx);
|
||||
int headNullLines = 0;
|
||||
_outputScanDataFile_vector(_out_file, scanLines, false, &headNullLines);
|
||||
#endif
|
||||
SSG_treeGrowParam growParam;
|
||||
growParam.maxLineSkipNum = 5;
|
||||
growParam.yDeviation_max = 1.0;
|
||||
growParam.maxSkipDistance = 5.0;
|
||||
growParam.zDeviation_max = 2;// algoParam.bagParam.bagH / 2; //袋子高度1/2
|
||||
growParam.minLTypeTreeLen = 30.0; //mm
|
||||
growParam.minVTypeTreeLen = 30.0; //mm
|
||||
SSG_cornerParam cornerParam;
|
||||
cornerParam.cornerTh =30; //45度角
|
||||
cornerParam.scale = 4; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
|
||||
cornerParam.minEndingGap = 20;// algoParam.bagParam.bagW / 4;
|
||||
cornerParam.minEndingGap_z = 20; // algoParam.bagParam.bagH / 4;
|
||||
cornerParam.jumpCornerTh_1 = 10;
|
||||
cornerParam.jumpCornerTh_2 = 30;
|
||||
SSX_lapWeldParam lapWeldParam;
|
||||
lapWeldParam.lapHeight = 2.0;
|
||||
lapWeldParam.weldMinLen = 2.0;
|
||||
lapWeldParam.weldRefPoints = 2;
|
||||
int errCode = 0;
|
||||
std::vector<std::vector<SVzNL3DPoint>> weldOps;
|
||||
sx_getLapWeldPostion(
|
||||
scanLines,
|
||||
cornerParam,
|
||||
growParam,
|
||||
lapWeldParam,
|
||||
weldOps,
|
||||
&errCode);
|
||||
|
||||
long t2 = (long)GetTickCount64();
|
||||
printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1));
|
||||
//输出测试结果
|
||||
sprintf_s(_scan_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx);
|
||||
_outputRGBDScanLapWeld_RGBD(_scan_file, scanLines, weldOps);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单
|
||||
// 调试程序: F5 或调试 >“开始调试”菜单
|
||||
|
||||
// 入门使用技巧:
|
||||
// 1. 使用解决方案资源管理器窗口添加/管理文件
|
||||
// 2. 使用团队资源管理器窗口连接到源代码管理
|
||||
// 3. 使用输出窗口查看生成输出和其他消息
|
||||
// 4. 使用错误列表窗口查看错误
|
||||
// 5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目
|
||||
// 6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件
|
||||
157
lapWeldDetection_test/lapWeldDetection_test.vcxproj
Normal file
157
lapWeldDetection_test/lapWeldDetection_test.vcxproj
Normal file
@ -0,0 +1,157 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||
<ItemGroup Label="ProjectConfigurations">
|
||||
<ProjectConfiguration Include="Debug|Win32">
|
||||
<Configuration>Debug</Configuration>
|
||||
<Platform>Win32</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Release|Win32">
|
||||
<Configuration>Release</Configuration>
|
||||
<Platform>Win32</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Debug|x64">
|
||||
<Configuration>Debug</Configuration>
|
||||
<Platform>x64</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Release|x64">
|
||||
<Configuration>Release</Configuration>
|
||||
<Platform>x64</Platform>
|
||||
</ProjectConfiguration>
|
||||
</ItemGroup>
|
||||
<PropertyGroup Label="Globals">
|
||||
<VCProjectVersion>16.0</VCProjectVersion>
|
||||
<Keyword>Win32Proj</Keyword>
|
||||
<ProjectGuid>{bd65ac40-a2c9-4c8d-adba-aba595c5f32d}</ProjectGuid>
|
||||
<RootNamespace>lapWeldDetectiontest</RootNamespace>
|
||||
<WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion>
|
||||
</PropertyGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>false</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>false</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
|
||||
<ImportGroup Label="ExtensionSettings">
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="Shared">
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<PropertyGroup Label="UserMacros" />
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<LinkIncremental>true</LinkIncremental>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<LinkIncremental>false</LinkIncremental>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<LinkIncremental>true</LinkIncremental>
|
||||
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
|
||||
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<LinkIncremental>false</LinkIncremental>
|
||||
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
|
||||
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
|
||||
</PropertyGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||
<OptimizeReferences>true</OptimizeReferences>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
<AdditionalIncludeDirectories>..\..\thirdParty\opencv\build\include;</AdditionalIncludeDirectories>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<AdditionalDependencies>opencv_world480d.lib;lapWeldDetection.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||
<AdditionalLibraryDirectories>..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
<AdditionalIncludeDirectories>..\..\thirdParty\opencv\build\include;</AdditionalIncludeDirectories>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||
<OptimizeReferences>true</OptimizeReferences>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<AdditionalDependencies>opencv_world480.lib;lapWeldDetection.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||
<AdditionalLibraryDirectories>..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Release;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemGroup>
|
||||
<ClCompile Include="lapWeldDetection_test.cpp" />
|
||||
</ItemGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
|
||||
<ImportGroup Label="ExtensionTargets">
|
||||
</ImportGroup>
|
||||
</Project>
|
||||
@ -60,6 +60,22 @@ SG_APISHARED_EXPORT void sg_getLineCornerFeature(
|
||||
const SSG_cornerParam cornerPara, //scale通常取bagH的1/4
|
||||
SSG_lineFeature* line_features);
|
||||
|
||||
/// <summary>
|
||||
/// 提取激光线上的Jumping特征
|
||||
/// nPointIdx被重新定义成Feature类型
|
||||
/// 算法流程：
|
||||
/// （1）逐点计算前向角和后向角
|
||||
/// （2）逐点计算拐角，顺时针为负，逆时针为正
|
||||
/// （3）搜索正拐角的极大值。
|
||||
/// （4）判断拐角是否为跳变
|
||||
/// </summary>
|
||||
SG_APISHARED_EXPORT void sg_getLineJumpFeature_cornerMethod(
|
||||
SVzNL3DPosition* lineData,
|
||||
int dataSize,
|
||||
int lineIdx,
|
||||
const SSG_cornerParam cornerPara, //scale通常取bagH的1/4
|
||||
std::vector< SSG_basicFeature1D> jumpFeatures);
|
||||
|
||||
SG_APISHARED_EXPORT void wd_getLineGloveArcs(
|
||||
std::vector<SVzNL3DPosition>& lineData,
|
||||
int lineIdx,
|
||||
@ -278,6 +294,12 @@ SG_APISHARED_EXPORT SSG_planeCalibPara sg_getPlaneCalibPara(
|
||||
SVzNL3DLaserLine* laser3DPoints,
|
||||
int lineNum);
|
||||
|
||||
//计算一个平面调平参数。
|
||||
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
|
||||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||||
SG_APISHARED_EXPORT SSG_planeCalibPara sg_getPlaneCalibPara2(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines);
|
||||
|
||||
//计算一个平面调平参数。
|
||||
//以数据输入中ROI以内的点进行平面拟合，计算调平参数
|
||||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||||
|
||||
@ -402,6 +402,7 @@ typedef struct
|
||||
typedef struct
|
||||
{
|
||||
int pntIdx;
|
||||
int type;
|
||||
double forwardAngle; //Ç°Ďň˝Ç
|
||||
double backwardAngle; //şóĎň˝Ç
|
||||
double corner; //šŐ˝Ç
|
||||
@ -409,4 +410,6 @@ typedef struct
|
||||
double backwardDiffZ;
|
||||
double pre_stepDist;
|
||||
double post_stepDist;
|
||||
double forward_z;
|
||||
double backward_z;
|
||||
}SSG_pntDirAngle;
|
||||
|
||||
@ -1207,6 +1207,344 @@ SSG_planeCalibPara sg_getPlaneCalibPara(
|
||||
return planePara;
|
||||
}
|
||||
|
||||
//计算一个平面调平参数。
|
||||
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
|
||||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||||
SSG_planeCalibPara sg_getPlaneCalibPara2(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines)
|
||||
{
|
||||
//设置初始结果
|
||||
double initCalib[9] = {
|
||||
1.0, 0.0, 0.0,
|
||||
0.0, 1.0, 0.0,
|
||||
0.0, 0.0, 1.0 };
|
||||
SSG_planeCalibPara planePara;
|
||||
for (int i = 0; i < 9; i++)
|
||||
planePara.planeCalib[i] = initCalib[i];
|
||||
planePara.planeHeight = -1.0;
|
||||
|
||||
int lineNum = (int)scanLines.size();
|
||||
//统计z范围
|
||||
SVzNLRangeD zRange = { 0, -1 }; //< Z范围
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
int nPositionCnt = (int)scanLines[line].size();
|
||||
for (int i = 0; i < nPositionCnt; i++)
|
||||
{
|
||||
SVzNL3DPosition* pt3D = &scanLines[line][i];
|
||||
if (pt3D->pt3D.z < 1e-4)
|
||||
continue;
|
||||
//z
|
||||
if (zRange.max < zRange.min)
|
||||
{
|
||||
zRange.min = pt3D->pt3D.z;
|
||||
zRange.max = pt3D->pt3D.z;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (zRange.min > pt3D->pt3D.z)
|
||||
zRange.min = pt3D->pt3D.z;
|
||||
if (zRange.max < pt3D->pt3D.z)
|
||||
zRange.max = pt3D->pt3D.z;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//在Z方向进行统计，取第一个极值
|
||||
//以mm为单位，简化量化
|
||||
int zHistSize = (int)(zRange.max - zRange.min) + 1;
|
||||
if (zHistSize == 0)
|
||||
return planePara;
|
||||
|
||||
std::vector<int> zHist;
|
||||
zHist.resize(zHistSize);
|
||||
int totalPntSize = 0;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
int nPositionCnt = (int)scanLines[line].size();
|
||||
for (int i = 0; i < nPositionCnt; i++)
|
||||
{
|
||||
SVzNL3DPosition* pt3D = &scanLines[line][i];
|
||||
if (pt3D->pt3D.z < 1e-4)
|
||||
continue;
|
||||
|
||||
totalPntSize++;
|
||||
int histPos = (int)(pt3D->pt3D.z - zRange.min);
|
||||
zHist[histPos] ++;
|
||||
}
|
||||
}
|
||||
std::vector<int> zSumHist;
|
||||
zSumHist.resize(zHistSize);
|
||||
bool isSame = true;
|
||||
//以厘米为单位进行累加
|
||||
for (int i = 0; i < zHistSize; i++)
|
||||
{
|
||||
int sumValue = 0;
|
||||
for (int j = i - 5; j <= i + 5; j++)
|
||||
{
|
||||
if ((j >= 0) && (j < zHistSize))
|
||||
sumValue += zHist[j];
|
||||
}
|
||||
zSumHist[i] = sumValue;
|
||||
if (i > 0)
|
||||
{
|
||||
if (sumValue != zSumHist[i - 1])
|
||||
isSame = false;
|
||||
}
|
||||
}
|
||||
if (true == isSame)
|
||||
{
|
||||
//不进行累加（如果累加，累加值相等）
|
||||
for (int i = 0; i < zHistSize; i++)
|
||||
zSumHist[i] = zHist[i];
|
||||
}
|
||||
|
||||
//寻找极值
|
||||
int _state = 0;
|
||||
int pre_i = -1;
|
||||
int sEdgePtIdx = -1;
|
||||
int eEdgePtIdx = -1;
|
||||
int pre_data = -1;
|
||||
std::vector< SSG_intPair> pkTop;
|
||||
std::vector< SSG_intPair> pkBtm;
|
||||
std::vector<int> pkBtmBackIndexing;
|
||||
pkBtmBackIndexing.resize(zHistSize);
|
||||
for (int i = 0; i < zHistSize; i++)
|
||||
pkBtmBackIndexing[i] = -1;
|
||||
|
||||
|
||||
for (int i = 0; i < zHistSize; i++)
|
||||
{
|
||||
int curr_data = zSumHist[i];
|
||||
if (pre_data < 0)
|
||||
{
|
||||
sEdgePtIdx = i;
|
||||
eEdgePtIdx = i;
|
||||
pre_data = curr_data;
|
||||
pre_i = i;
|
||||
continue;
|
||||
}
|
||||
|
||||
eEdgePtIdx = i;
|
||||
double z_diff = curr_data - pre_data;
|
||||
switch (_state)
|
||||
{
|
||||
case 0: //初态
|
||||
if (z_diff < 0) //下降
|
||||
{
|
||||
_state = 2;
|
||||
}
|
||||
else if (z_diff > 0) //上升
|
||||
{
|
||||
_state = 1;
|
||||
}
|
||||
break;
|
||||
case 1: //上升
|
||||
if (z_diff < 0) //下降
|
||||
{
|
||||
pkTop.push_back({ pre_i, pre_data });
|
||||
_state = 2;
|
||||
}
|
||||
else if (i == (zHistSize - 1))
|
||||
pkTop.push_back({ i, curr_data });
|
||||
break;
|
||||
case 2: //下降
|
||||
if (z_diff > 0) // 上升
|
||||
{
|
||||
int pkBtmIdx = (int)pkBtm.size();
|
||||
pkBtmBackIndexing[pre_i] = pkBtmIdx;
|
||||
pkBtm.push_back({ pre_i, pre_data });
|
||||
_state = 1;
|
||||
}
|
||||
else if (i == (zHistSize - 1))
|
||||
{
|
||||
int pkBtmIdx = (int)pkBtm.size();
|
||||
pkBtmBackIndexing[i] = pkBtmIdx;
|
||||
pkBtm.push_back({ i, curr_data });
|
||||
}
|
||||
break;
|
||||
default:
|
||||
_state = 0;
|
||||
break;
|
||||
}
|
||||
pre_data = curr_data;
|
||||
pre_i = i;
|
||||
}
|
||||
//寻找第一个超过总点数1/3的极值点
|
||||
if (pkTop.size() < 1)
|
||||
return planePara;
|
||||
|
||||
int pntSizeTh = totalPntSize / 10;
|
||||
SSG_intPair* vldPeak = NULL;
|
||||
for (int i = 0, i_max = (int)pkTop.size(); i < i_max; i++)
|
||||
{
|
||||
if (pkTop[i].data_1 > pntSizeTh)
|
||||
{
|
||||
vldPeak = &pkTop[i];
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (NULL == vldPeak)
|
||||
return planePara;
|
||||
|
||||
//寻找开始和结束位置
|
||||
//向前向后寻找
|
||||
int preBtmIdx = -1;
|
||||
for (int j = vldPeak->data_0 - 1; j >= 0; j--)
|
||||
{
|
||||
if (pkBtmBackIndexing[j] >= 0)
|
||||
{
|
||||
int idx = pkBtmBackIndexing[j];
|
||||
if (pkBtm[idx].data_1 < (vldPeak->data_1 / 2))
|
||||
{
|
||||
preBtmIdx = j;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
int postBtmIdx = -1;
|
||||
for (int j = vldPeak->data_0 + 1; j < zHistSize; j++)
|
||||
{
|
||||
if (pkBtmBackIndexing[j] >= 0)
|
||||
{
|
||||
int idx = pkBtmBackIndexing[j];
|
||||
if (pkBtm[idx].data_1 < (vldPeak->data_1 / 2))
|
||||
{
|
||||
postBtmIdx = j;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
SVzNLRangeD topZRange;
|
||||
if (preBtmIdx < 0)
|
||||
topZRange.min = zRange.min;
|
||||
else
|
||||
topZRange.min = (float)preBtmIdx + zRange.min;
|
||||
if (postBtmIdx < 0)
|
||||
topZRange.max = zRange.max;
|
||||
else
|
||||
topZRange.max = (float)postBtmIdx + zRange.min;
|
||||
|
||||
//取数据
|
||||
std::vector<cv::Point3f> Points3ds;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
int nPositionCnt = (int)scanLines[line].size();
|
||||
for (int i = 0; i < nPositionCnt; i++)
|
||||
{
|
||||
SVzNL3DPosition* pt3D = &scanLines[line][i];
|
||||
if (pt3D->pt3D.z < 1e-4)
|
||||
continue;
|
||||
|
||||
if ((pt3D->pt3D.z >= topZRange.min) && (pt3D->pt3D.z <= topZRange.max))
|
||||
{
|
||||
cv::Point3f a_vldPt;
|
||||
a_vldPt.x = (float)pt3D->pt3D.x;
|
||||
a_vldPt.y = (float)pt3D->pt3D.y;
|
||||
a_vldPt.z = (float)pt3D->pt3D.z;
|
||||
Points3ds.push_back(a_vldPt);
|
||||
}
|
||||
}
|
||||
}
|
||||
//平面拟合
|
||||
std::vector<double> planceFunc;
|
||||
vzCaculateLaserPlane(Points3ds, planceFunc);
|
||||
|
||||
#if 1 //两个向量的旋转旋转，使用四元数法，
|
||||
Vector3 a = Vector3(planceFunc[0], planceFunc[1], planceFunc[2]);
|
||||
Vector3 b = Vector3(0, 0, -1.0);
|
||||
Quaternion quanPara = rotationBetweenVectors(a, b);
|
||||
|
||||
RotationMatrix rMatrix;
|
||||
quaternionToMatrix(quanPara, rMatrix.data);
|
||||
//计算反向旋转矩阵
|
||||
Quaternion invQuanPara = rotationBetweenVectors(b, a);
|
||||
RotationMatrix invMatrix;
|
||||
quaternionToMatrix(invQuanPara, invMatrix.data);
|
||||
#else //根据平面的法向量计算欧拉角，进而计算旋转矩阵
|
||||
//参数计算
|
||||
SSG_EulerAngles eulerPra = planeNormalToEuler(planceFunc[0], planceFunc[1], planceFunc[2]);
|
||||
//反射进行校正
|
||||
eulerPra.roll = eulerPra.roll;
|
||||
eulerPra.pitch = eulerPra.pitch;
|
||||
eulerPra.yaw = eulerPra.yaw;
|
||||
RotationMatrix rMatrix = eulerToRotationMatrix(eulerPra.yaw, eulerPra.pitch, eulerPra.roll);
|
||||
#endif
|
||||
|
||||
planePara.planeCalib[0] = rMatrix.data[0][0];
|
||||
planePara.planeCalib[1] = rMatrix.data[0][1];
|
||||
planePara.planeCalib[2] = rMatrix.data[0][2];
|
||||
planePara.planeCalib[3] = rMatrix.data[1][0];
|
||||
planePara.planeCalib[4] = rMatrix.data[1][1];
|
||||
planePara.planeCalib[5] = rMatrix.data[1][2];
|
||||
planePara.planeCalib[6] = rMatrix.data[2][0];
|
||||
planePara.planeCalib[7] = rMatrix.data[2][1];
|
||||
planePara.planeCalib[8] = rMatrix.data[2][2];
|
||||
|
||||
planePara.invRMatrix[0] = invMatrix.data[0][0];
|
||||
planePara.invRMatrix[1] = invMatrix.data[0][1];
|
||||
planePara.invRMatrix[2] = invMatrix.data[0][2];
|
||||
planePara.invRMatrix[3] = invMatrix.data[1][0];
|
||||
planePara.invRMatrix[4] = invMatrix.data[1][1];
|
||||
planePara.invRMatrix[5] = invMatrix.data[1][2];
|
||||
planePara.invRMatrix[6] = invMatrix.data[2][0];
|
||||
planePara.invRMatrix[7] = invMatrix.data[2][1];
|
||||
planePara.invRMatrix[8] = invMatrix.data[2][2];
|
||||
|
||||
#if 0 //test: 两个矩阵的乘积必须是单位阵
|
||||
double testMatrix[3][3];
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
for (int j = 0; j < 3; j++)
|
||||
{
|
||||
testMatrix[i][j] = 0;
|
||||
for (int m = 0; m < 3; m++)
|
||||
testMatrix[i][j] += invMatrix.data[i][m] * rMatrix.data[m][j];
|
||||
}
|
||||
}
|
||||
#endif
|
||||
//数据进行转换
|
||||
SVzNLRangeD calibZRange = { 0, -1 };
|
||||
topZRange = { 0, -1 };
|
||||
for (int i = 0, i_max = (int)Points3ds.size(); i < i_max; i++)
|
||||
{
|
||||
//z
|
||||
if (topZRange.max < topZRange.min)
|
||||
{
|
||||
topZRange.min = Points3ds[i].z;
|
||||
topZRange.max = Points3ds[i].z;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (topZRange.min > Points3ds[i].z)
|
||||
topZRange.min = Points3ds[i].z;
|
||||
if (topZRange.max < Points3ds[i].z)
|
||||
topZRange.max = Points3ds[i].z;
|
||||
}
|
||||
cv::Point3f a_calibPt;
|
||||
a_calibPt.x = (float)(Points3ds[i].x * planePara.planeCalib[0] + Points3ds[i].y * planePara.planeCalib[1] + Points3ds[i].z * planePara.planeCalib[2]);
|
||||
a_calibPt.y = (float)(Points3ds[i].x * planePara.planeCalib[3] + Points3ds[i].y * planePara.planeCalib[4] + Points3ds[i].z * planePara.planeCalib[5]);
|
||||
a_calibPt.z = (float)(Points3ds[i].x * planePara.planeCalib[6] + Points3ds[i].y * planePara.planeCalib[7] + Points3ds[i].z * planePara.planeCalib[8]);
|
||||
//z
|
||||
if (calibZRange.max < calibZRange.min)
|
||||
{
|
||||
calibZRange.min = a_calibPt.z;
|
||||
calibZRange.max = a_calibPt.z;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (calibZRange.min > a_calibPt.z)
|
||||
calibZRange.min = a_calibPt.z;
|
||||
if (calibZRange.max < a_calibPt.z)
|
||||
calibZRange.max = a_calibPt.z;
|
||||
}
|
||||
}
|
||||
planePara.planeHeight = calibZRange.min;
|
||||
|
||||
return planePara;
|
||||
}
|
||||
|
||||
//计算一个平面调平参数。
|
||||
//以数据输入中ROI以内的点进行平面拟合，计算调平参数
|
||||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||||
|
||||
@ -1438,6 +1438,377 @@ void sg_getLineCornerFeature(
|
||||
return;
|
||||
}
|
||||
|
||||
bool compareByIdx(const SSG_pntDirAngle& a, const SSG_pntDirAngle& b) {
|
||||
return a.pntIdx < b.pntIdx;
|
||||
}
|
||||
/// <summary>
|
||||
/// 提取激光线上的Jumping特征
|
||||
/// nPointIdx被重新定义成Feature类型
|
||||
/// 算法流程：
|
||||
/// （1）逐点计算前向角和后向角
|
||||
/// （2）逐点计算拐角，顺时针为负，逆时针为正
|
||||
/// （3）搜索正拐角的极大值。
|
||||
/// （4）判断拐角是否为跳变
|
||||
/// </summary>
|
||||
void sg_getLineJumpFeature_cornerMethod(
|
||||
SVzNL3DPosition* lineData,
|
||||
int dataSize,
|
||||
int lineIdx,
|
||||
const SSG_cornerParam cornerPara, //scale通常取bagH的1/4
|
||||
std::vector< SSG_basicFeature1D> jumpFeatures)
|
||||
{
|
||||
//去除零点
|
||||
std::vector< SVzNL3DPosition> vldPts;
|
||||
std::vector<SSG_RUN> segs;
|
||||
//修改seg的定义。seg端点是两点间距离大于门限的点
|
||||
int segStart = -1, segEnd = -1;
|
||||
for (int i = 0; i < dataSize; i++)
|
||||
{
|
||||
SVzNL3DPosition a_pt = lineData[i];
|
||||
a_pt.nPointIdx = i;
|
||||
if (lineData[i].pt3D.z > 1e-4)
|
||||
vldPts.push_back(a_pt);
|
||||
|
||||
//seg判断
|
||||
if (lineData[i].pt3D.z > 1e-4)
|
||||
{
|
||||
if (segStart < 0)
|
||||
segStart = i;
|
||||
else //检查两点距离
|
||||
{
|
||||
SVzNL3DPosition pre_pt = lineData[i - 1];
|
||||
double diff_z = abs(a_pt.pt3D.z - pre_pt.pt3D.z);
|
||||
if (diff_z > cornerPara.minEndingGap_z)
|
||||
{
|
||||
SSG_RUN a_run;
|
||||
a_run.start = segStart;
|
||||
a_run.len = segEnd - segStart + 1;
|
||||
a_run.value = 1;
|
||||
segs.push_back(a_run);
|
||||
segStart = i;
|
||||
}
|
||||
}
|
||||
segEnd = i;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (segStart >= 0)
|
||||
{
|
||||
SSG_RUN a_run;
|
||||
a_run.start = segStart;
|
||||
a_run.len = segEnd - segStart + 1;
|
||||
a_run.value = 1;
|
||||
segs.push_back(a_run);
|
||||
segStart = -1;
|
||||
segEnd = -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
//last
|
||||
if (segStart >= 0)
|
||||
{
|
||||
SSG_RUN a_run;
|
||||
a_run.start = segStart;
|
||||
a_run.len = segEnd - segStart + 1;
|
||||
a_run.value = 1;
|
||||
segs.push_back(a_run);
|
||||
}
|
||||
|
||||
//计算前向角和后向角
|
||||
std::vector< SSG_pntDirAngle> corners;
|
||||
corners.resize(vldPts.size());
|
||||
for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++)
|
||||
{
|
||||
if (i == 875)
|
||||
int kkk = 1;
|
||||
|
||||
//前向寻找
|
||||
int pre_i = -1;
|
||||
for (int j = i - 1; j >= 0; j--)
|
||||
{
|
||||
double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) +
|
||||
pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2));
|
||||
if (dist >= cornerPara.scale)
|
||||
{
|
||||
pre_i = j;
|
||||
break;
|
||||
}
|
||||
}
|
||||
//后向寻找
|
||||
int post_i = -1;
|
||||
for (int j = i + 1; j < i_max; j++)
|
||||
{
|
||||
double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) +
|
||||
pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2));
|
||||
if (dist >= cornerPara.scale)
|
||||
{
|
||||
post_i = j;
|
||||
break;
|
||||
}
|
||||
}
|
||||
//计算拐角
|
||||
if ((pre_i < 0) || (post_i < 0))
|
||||
{
|
||||
corners[i].pntIdx = -1;
|
||||
corners[i].forwardAngle = 0;
|
||||
corners[i].backwardAngle = 0;
|
||||
corners[i].corner = 0;
|
||||
corners[i].forwardDiffZ = 0;
|
||||
corners[i].backwardDiffZ = 0;
|
||||
corners[i].forward_z = 0;
|
||||
corners[i].backward_z = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
double tanValue_pre = (vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z) / abs(vldPts[i].pt3D.y - vldPts[pre_i].pt3D.y);
|
||||
double tanValue_post = (vldPts[post_i].pt3D.z - vldPts[i].pt3D.z) / abs(vldPts[post_i].pt3D.y - vldPts[i].pt3D.y);
|
||||
double forwardAngle = atan(tanValue_post) * 180.0 / PI; //沿序号增长方向
|
||||
double backwardAngle = atan(tanValue_pre) * 180.0 / PI; //沿序号减小方向
|
||||
corners[i].pntIdx = i;
|
||||
corners[i].forwardAngle = forwardAngle;
|
||||
corners[i].backwardAngle = backwardAngle;
|
||||
corners[i].corner = -(forwardAngle - backwardAngle); //图像坐标系与正常坐标系y方向相反，所以有“-”号
|
||||
corners[i].forwardDiffZ = vldPts[post_i].pt3D.z - vldPts[i].pt3D.z;
|
||||
corners[i].backwardDiffZ = vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z;
|
||||
corners[i].forward_z = vldPts[post_i].pt3D.z; //沿序号增长方向
|
||||
corners[i].backward_z = vldPts[pre_i].pt3D.z; //沿序号减小方向
|
||||
}
|
||||
}
|
||||
|
||||
//搜索拐角极值
|
||||
int _state = 0;
|
||||
int pre_i = -1;
|
||||
int sEdgePtIdx = -1;
|
||||
int eEdgePtIdx = -1;
|
||||
SSG_pntDirAngle* pre_data = NULL;
|
||||
std::vector< SSG_pntDirAngle> cornerPeakP;
|
||||
std::vector< SSG_pntDirAngle> cornerPeakM;
|
||||
for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++)
|
||||
{
|
||||
if (i == 275)
|
||||
int kkk = 1;
|
||||
SSG_pntDirAngle* curr_data = &corners[i];
|
||||
if (curr_data->pntIdx < 0)
|
||||
{
|
||||
if (i == i_max - 1) //最后一个
|
||||
{
|
||||
if (1 == _state) //上升
|
||||
{
|
||||
cornerPeakP.push_back(corners[eEdgePtIdx]);
|
||||
}
|
||||
else if (2 == _state) //下降
|
||||
{
|
||||
cornerPeakM.push_back(corners[eEdgePtIdx]);
|
||||
}
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
if (NULL == pre_data)
|
||||
{
|
||||
sEdgePtIdx = i;
|
||||
eEdgePtIdx = i;
|
||||
pre_data = curr_data;
|
||||
pre_i = i;
|
||||
continue;
|
||||
}
|
||||
|
||||
eEdgePtIdx = i;
|
||||
double cornerDiff = curr_data->corner - pre_data->corner;
|
||||
switch (_state)
|
||||
{
|
||||
case 0: //初态
|
||||
if (cornerDiff < 0) //下降
|
||||
{
|
||||
_state = 2;
|
||||
}
|
||||
else if (cornerDiff > 0) //上升
|
||||
{
|
||||
_state = 1;
|
||||
}
|
||||
break;
|
||||
case 1: //上升
|
||||
if (cornerDiff < 0) //下降
|
||||
{
|
||||
cornerPeakP.push_back(*pre_data);
|
||||
_state = 2;
|
||||
}
|
||||
break;
|
||||
case 2: //下降
|
||||
if (cornerDiff > 0) // 上升
|
||||
{
|
||||
cornerPeakM.push_back(*pre_data);
|
||||
_state = 1;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
_state = 0;
|
||||
break;
|
||||
}
|
||||
pre_data = curr_data;
|
||||
pre_i = i;
|
||||
}
|
||||
//注意：最后一个不处理，为基座位置
|
||||
|
||||
//极小值点（峰顶）
|
||||
//极值比较，在尺度窗口下寻找局部极值点
|
||||
std::vector< SSG_pntDirAngle> cornerFeatures;
|
||||
double square_distTh = 4 * cornerPara.scale * cornerPara.scale; //2倍的cornerScale。
|
||||
for (int i = 0, i_max = (int)cornerPeakP.size(); i < i_max; i++)
|
||||
{
|
||||
if (cornerPeakP[i].corner < cornerPara.cornerTh)
|
||||
continue;
|
||||
|
||||
bool isPeak = true;
|
||||
//向前搜索
|
||||
int cornerPtIdx = cornerPeakP[i].pntIdx;
|
||||
for (int j = i - 1; j >= 0; j--)
|
||||
{
|
||||
int prePtIdx = cornerPeakP[j].pntIdx;
|
||||
double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[prePtIdx].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ;
|
||||
if (dist > square_distTh) //超出尺度窗口
|
||||
break;
|
||||
|
||||
if (cornerPeakP[i].corner < cornerPeakP[j].corner)
|
||||
{
|
||||
isPeak = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
//向后搜索
|
||||
if (true == isPeak)
|
||||
{
|
||||
cornerPtIdx = cornerPeakP[i].pntIdx;
|
||||
for (int j = i + 1; j < i_max; j++)
|
||||
{
|
||||
int postPtIdx = cornerPeakP[j].pntIdx;
|
||||
double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2);
|
||||
if (dist > square_distTh) //超出尺度窗口
|
||||
break;
|
||||
|
||||
if (cornerPeakP[i].corner < cornerPeakP[j].corner)
|
||||
{
|
||||
isPeak = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (true == isPeak)
|
||||
{
|
||||
if ((abs(cornerPeakP[i].backwardAngle) < cornerPara.jumpCornerTh_1) && (abs(cornerPeakP[i].forwardAngle) > cornerPara.jumpCornerTh_2))
|
||||
{
|
||||
cornerPeakP[i].type = LINE_FEATURE_RIGHT_ANGLE_HR;
|
||||
cornerFeatures.push_back(cornerPeakP[i]);
|
||||
}
|
||||
else if ((abs(cornerPeakP[i].forwardAngle) < cornerPara.jumpCornerTh_1) && (abs(cornerPeakP[i].backwardAngle) > cornerPara.jumpCornerTh_2))
|
||||
{
|
||||
cornerPeakP[i].type = LINE_FEATURE_RIGHT_ANGLE_FH;
|
||||
cornerFeatures.push_back(cornerPeakP[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
for (int i = 0, i_max = (int)cornerPeakM.size(); i < i_max; i++)
|
||||
{
|
||||
if (abs(cornerPeakM[i].corner) < cornerPara.cornerTh)
|
||||
continue;
|
||||
|
||||
bool isPeak = true;
|
||||
//向前搜索
|
||||
int cornerPtIdx = cornerPeakM[i].pntIdx;
|
||||
for (int j = i - 1; j >= 0; j--)
|
||||
{
|
||||
int prePtIdx = cornerPeakM[j].pntIdx;
|
||||
double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[prePtIdx].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ;
|
||||
if (dist > square_distTh) //超出尺度窗口
|
||||
break;
|
||||
|
||||
if (abs(cornerPeakM[i].corner) < abs(cornerPeakM[j].corner))
|
||||
{
|
||||
isPeak = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
//向后搜索
|
||||
if (true == isPeak)
|
||||
{
|
||||
cornerPtIdx = cornerPeakM[i].pntIdx;
|
||||
for (int j = i + 1; j < i_max; j++)
|
||||
{
|
||||
int postPtIdx = cornerPeakM[j].pntIdx;
|
||||
double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2);
|
||||
if (dist > square_distTh) //超出尺度窗口
|
||||
break;
|
||||
|
||||
if (abs(cornerPeakM[i].corner) < abs(cornerPeakM[j].corner))
|
||||
{
|
||||
isPeak = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (true == isPeak)
|
||||
{
|
||||
if ((abs(cornerPeakM[i].backwardAngle) < cornerPara.jumpCornerTh_1) && (abs(cornerPeakM[i].forwardAngle) > cornerPara.jumpCornerTh_2))
|
||||
{
|
||||
cornerPeakM[i].type = LINE_FEATURE_RIGHT_ANGLE_HF;
|
||||
cornerFeatures.push_back(cornerPeakM[i]);
|
||||
}
|
||||
else if ((abs(cornerPeakM[i].forwardAngle) < cornerPara.jumpCornerTh_1) && (abs(cornerPeakM[i].backwardAngle) > cornerPara.jumpCornerTh_2))
|
||||
{
|
||||
cornerPeakM[i].type = LINE_FEATURE_RIGHT_ANGLE_RH;
|
||||
cornerFeatures.push_back(cornerPeakM[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//排序
|
||||
std::sort(cornerFeatures.begin(), cornerFeatures.end(), compareByIdx);
|
||||
|
||||
//进行组合获得Jump
|
||||
for (int i = 0, i_max = (int)cornerFeatures.size(); i < i_max-1; i++)
|
||||
{
|
||||
if (cornerFeatures[i].type == LINE_FEATURE_RIGHT_ANGLE_HR)
|
||||
{
|
||||
if (cornerFeatures[i + 1].type == LINE_FEATURE_RIGHT_ANGLE_RH)
|
||||
{
|
||||
int pntIdx_1 = cornerFeatures[i].pntIdx;
|
||||
int pntIdx_2 = cornerFeatures[i+1].pntIdx;
|
||||
//计算距离和跳变高度
|
||||
double dist = abs(vldPts[pntIdx_1].pt3D.y - vldPts[pntIdx_2].pt3D.y);
|
||||
double height = cornerFeatures[i + 1].forward_z - cornerFeatures[i].backward_z;
|
||||
if ((dist < cornerPara.minEndingGap) && (height < -cornerPara.minEndingGap_z))
|
||||
{
|
||||
SSG_basicFeature1D a_feature;
|
||||
a_feature.featureType = LINE_FEATURE_L_JUMP_L2H;
|
||||
a_feature.jumpPos = vldPts[pntIdx_1].pt3D;
|
||||
a_feature.jumpPos2D = { lineIdx, pntIdx_1 };
|
||||
jumpFeatures.push_back(a_feature);
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (cornerFeatures[i].type == LINE_FEATURE_RIGHT_ANGLE_HF)
|
||||
{
|
||||
if (cornerFeatures[i + 1].type == LINE_FEATURE_RIGHT_ANGLE_FH)
|
||||
{
|
||||
int pntIdx_1 = cornerFeatures[i].pntIdx;
|
||||
int pntIdx_2 = cornerFeatures[i + 1].pntIdx;
|
||||
//计算距离和跳变高度
|
||||
double dist = abs(vldPts[pntIdx_1].pt3D.y - vldPts[pntIdx_2].pt3D.y);
|
||||
double height = cornerFeatures[i + 1].forward_z - cornerFeatures[i].backward_z;
|
||||
if ((dist < cornerPara.minEndingGap) && (height > cornerPara.minEndingGap_z))
|
||||
{
|
||||
SSG_basicFeature1D a_feature;
|
||||
a_feature.featureType = LINE_FEATURE_L_JUMP_H2L;
|
||||
a_feature.jumpPos = vldPts[pntIdx_2].pt3D;
|
||||
a_feature.jumpPos2D = { lineIdx, pntIdx_2 };
|
||||
jumpFeatures.push_back(a_feature);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
//手套的环的特征：在扫描线的Peak点的前后一定范围内，存在前向角从正到负的变化范围超过150度（门限值）
|
||||
void wd_getLineGloveArcs(
|
||||
std::vector<SVzNL3DPosition>& lineData,
|
||||
|
||||
216
sourceCode/SX_lapWeldDetection.cpp
Normal file
216
sourceCode/SX_lapWeldDetection.cpp
Normal file
@ -0,0 +1,216 @@
|
||||
#include <vector>
|
||||
#include "SG_baseDataType.h"
|
||||
#include "SG_baseAlgo_Export.h"
|
||||
#include "SX_lapWeldDetection_Export.h"
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <limits>
|
||||
|
||||
//计算一个平面调平参数。
|
||||
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
|
||||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||||
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);
|
||||
}
|
||||
|
||||
//提取搭接焊缝
|
||||
void sx_getLapWeldPostion(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
const SSG_cornerParam cornerPara,
|
||||
SSG_treeGrowParam growParam,
|
||||
SSX_lapWeldParam lapWeldParam,
|
||||
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;
|
||||
//垂直arc特征提取
|
||||
std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_v;
|
||||
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_getLineJumpFeature_cornerMethod(
|
||||
&scanLines[line][0],
|
||||
dataSize,
|
||||
line,
|
||||
cornerPara, //scale通常取bagH的1/4
|
||||
a_line_features);
|
||||
jumpFeatures_v.push_back(a_line_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();
|
||||
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_getLineJumpFeature_cornerMethod(
|
||||
&hLines[line][0],
|
||||
dataSize,
|
||||
line,
|
||||
cornerPara, //scale通常取bagH的1/4
|
||||
a_line_features);
|
||||
|
||||
//进行配对匹配
|
||||
jumpFeatures_h.push_back(a_line_features);
|
||||
}
|
||||
|
||||
//特征生长
|
||||
//垂直方向特征生长（激光线方向）
|
||||
std::vector<SSG_featureTree> v_trees;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
bool isLastLine = false;
|
||||
if (line == lineNum - 1)
|
||||
isLastLine = true;
|
||||
std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_v[line];
|
||||
sg_lineFeaturesGrowing(
|
||||
line,
|
||||
isLastLine,
|
||||
a_lineJumpFeature,
|
||||
v_trees,
|
||||
growParam);
|
||||
}
|
||||
//水平方向特征生长（扫描运动方向）
|
||||
std::vector<SSG_featureTree> h_trees;
|
||||
for (int line = 0; line < lineNum_h; line++)
|
||||
{
|
||||
if (line == 650)
|
||||
int kkk = 1;
|
||||
bool isLastLine = false;
|
||||
if (line == lineNum_h - 1)
|
||||
isLastLine = true;
|
||||
std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_h[line];
|
||||
sg_lineFeaturesGrowing(
|
||||
line,
|
||||
isLastLine,
|
||||
a_lineJumpFeature,
|
||||
h_trees,
|
||||
growParam);
|
||||
}
|
||||
|
||||
//tree信息
|
||||
std::vector<SSG_treeInfo> allTreesInfo; //不包含边界
|
||||
SSG_treeInfo a_nullTree;
|
||||
memset(&a_nullTree, 0, sizeof(SSG_treeInfo));
|
||||
allTreesInfo.push_back(a_nullTree); //保持存储位置与treeIdx相同位置，方便索引
|
||||
//标记，根据起点的生长树进行标注
|
||||
int hvTreeIdx = 1;
|
||||
for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++)
|
||||
{
|
||||
SSG_featureTree* a_vTree = &v_trees[i];
|
||||
|
||||
//记录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);
|
||||
//在原始点云上标记，同时有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;
|
||||
}
|
||||
}
|
||||
}
|
||||
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);
|
||||
//在原始点云上标记，同时有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;
|
||||
}
|
||||
}
|
||||
}
|
||||
hvTreeIdx++;
|
||||
}
|
||||
int hvTreeSize = hvTreeIdx;
|
||||
|
||||
//输出结果
|
||||
}
|
||||
40
sourceCode/SX_lapWeldDetection_Export.h
Normal file
40
sourceCode/SX_lapWeldDetection_Export.h
Normal file
@ -0,0 +1,40 @@
|
||||
#pragma once
|
||||
|
||||
#if defined(SG_API_LIBRARY)
|
||||
# define SG_APISHARED_EXPORT __declspec(dllexport)
|
||||
#else
|
||||
# define SG_APISHARED_EXPORT __declspec(dllimport)
|
||||
#endif
|
||||
|
||||
#include "SG_baseDataType.h"
|
||||
#include <vector>
|
||||
#include <opencv2/opencv.hpp>
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double lapHeight;//搭接厚度
|
||||
double weldMinLen; //最小焊缝长度，用于过滤可能的虚假焊缝
|
||||
int weldRefPoints; //输出的直线焊缝的参考点，默认是2个（起点和终点）
|
||||
}SSX_lapWeldParam;
|
||||
|
||||
//计算一个平面调平参数。
|
||||
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
|
||||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||||
SG_APISHARED_EXPORT SSG_planeCalibPara sx_getBaseCalibPara(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines);
|
||||
|
||||
//相机姿态调平，并去除地面
|
||||
SG_APISHARED_EXPORT void sx_lineDataR(
|
||||
std::vector< SVzNL3DPosition>& a_line,
|
||||
const double* camPoseR,
|
||||
double groundH);
|
||||
|
||||
//提取搭接焊缝
|
||||
SG_APISHARED_EXPORT void sx_getLapWeldPostion(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
const SSG_cornerParam cornerPara,
|
||||
SSG_treeGrowParam growParam,
|
||||
SSX_lapWeldParam lapWeldParam,
|
||||
std::vector<std::vector<SVzNL3DPoint>>& objOps,
|
||||
int* errCode);
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user