rodAndBarDetection version 1.3.3 :
筑裕钢筋焊缝提取算法改进中间版本
This commit is contained in:
parent
4580256e47
commit
2d837cb108
@ -1,7 +1,7 @@
|
||||
|
||||
Microsoft Visual Studio Solution File, Format Version 12.00
|
||||
# Visual Studio Version 16
|
||||
VisualStudioVersion = 16.0.33027.164
|
||||
# Visual Studio Version 17
|
||||
VisualStudioVersion = 17.14.37216.2 d17.14
|
||||
MinimumVisualStudioVersion = 10.0.40219.1
|
||||
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "fireBrickPositioning", "fireBrickPositioning\fireBrickPositioning.vcxproj", "{BB3FA81E-21D8-40AD-ACE6-49173477F32D}"
|
||||
ProjectSection(ProjectDependencies) = postProject
|
||||
@ -112,7 +112,7 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "BQ_workpieceCornerExtractio
|
||||
{95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD}
|
||||
EndProjectSection
|
||||
EndProject
|
||||
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "BQ_workpieceCornerExtract_test", "BQ_workpieceCornerExtract_test\BQ_workpieceCornerExtract_test.vcxproj", "{CF563709-0402-447E-BFCC-7701CC90D0AF}"
|
||||
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "BQ_workpieceCornerExtraction_test", "BQ_workpieceCornerExtract_test\BQ_workpieceCornerExtract_test.vcxproj", "{CF563709-0402-447E-BFCC-7701CC90D0AF}"
|
||||
ProjectSection(ProjectDependencies) = postProject
|
||||
{95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD}
|
||||
{AD8415B7-A745-4184-87B8-95619E5066D6} = {AD8415B7-A745-4184-87B8-95619E5066D6}
|
||||
@ -227,15 +227,15 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "YouJiang_sheetPositioning_t
|
||||
{BDE8197F-8FD3-4DEF-B1EA-F6A2C7C5DA9B} = {BDE8197F-8FD3-4DEF-B1EA-F6A2C7C5DA9B}
|
||||
EndProjectSection
|
||||
EndProject
|
||||
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "steelPipeDetection", "steelPipeDetection\steelPipeDetection.vcxproj", "{8e5b3c2d-4a1f-4e9b-8d7c-3f2e5a6b7c8d}"
|
||||
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "steelPipeDetection", "steelPipeDetection\steelPipeDetection.vcxproj", "{8E5B3C2D-4A1F-4E9B-8D7C-3F2E5A6B7C8D}"
|
||||
ProjectSection(ProjectDependencies) = postProject
|
||||
{95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD}
|
||||
EndProjectSection
|
||||
EndProject
|
||||
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "steelPipeDetection_test", "steelPipeDetection_test\steelPipeDetection_test.vcxproj", "{1a2b3c4d-5e6f-4a7b-8c9d-0e1f2a3b4c5d}"
|
||||
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "steelPipeDetection_test", "steelPipeDetection_test\steelPipeDetection_test.vcxproj", "{1A2B3C4D-5E6F-4A7B-8C9D-0E1F2A3B4C5D}"
|
||||
ProjectSection(ProjectDependencies) = postProject
|
||||
{95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD}
|
||||
{8e5b3c2d-4a1f-4e9b-8d7c-3f2e5a6b7c8d} = {8e5b3c2d-4a1f-4e9b-8d7c-3f2e5a6b7c8d}
|
||||
{8E5B3C2D-4A1F-4E9B-8D7C-3F2E5A6B7C8D} = {8E5B3C2D-4A1F-4E9B-8D7C-3F2E5A6B7C8D}
|
||||
EndProjectSection
|
||||
EndProject
|
||||
Global
|
||||
@ -582,22 +582,6 @@ Global
|
||||
{BDE8197F-8FD3-4DEF-B1EA-F6A2C7C5DA9B}.Release|x64.Build.0 = Release|x64
|
||||
{BDE8197F-8FD3-4DEF-B1EA-F6A2C7C5DA9B}.Release|x86.ActiveCfg = Release|Win32
|
||||
{BDE8197F-8FD3-4DEF-B1EA-F6A2C7C5DA9B}.Release|x86.Build.0 = Release|Win32
|
||||
{8e5b3c2d-4a1f-4e9b-8d7c-3f2e5a6b7c8d}.Debug|x64.ActiveCfg = Debug|x64
|
||||
{8e5b3c2d-4a1f-4e9b-8d7c-3f2e5a6b7c8d}.Debug|x64.Build.0 = Debug|x64
|
||||
{8e5b3c2d-4a1f-4e9b-8d7c-3f2e5a6b7c8d}.Debug|x86.ActiveCfg = Debug|Win32
|
||||
{8e5b3c2d-4a1f-4e9b-8d7c-3f2e5a6b7c8d}.Debug|x86.Build.0 = Debug|Win32
|
||||
{8e5b3c2d-4a1f-4e9b-8d7c-3f2e5a6b7c8d}.Release|x64.ActiveCfg = Release|x64
|
||||
{8e5b3c2d-4a1f-4e9b-8d7c-3f2e5a6b7c8d}.Release|x64.Build.0 = Release|x64
|
||||
{8e5b3c2d-4a1f-4e9b-8d7c-3f2e5a6b7c8d}.Release|x86.ActiveCfg = Release|Win32
|
||||
{8e5b3c2d-4a1f-4e9b-8d7c-3f2e5a6b7c8d}.Release|x86.Build.0 = Release|Win32
|
||||
{1a2b3c4d-5e6f-4a7b-8c9d-0e1f2a3b4c5d}.Debug|x64.ActiveCfg = Debug|x64
|
||||
{1a2b3c4d-5e6f-4a7b-8c9d-0e1f2a3b4c5d}.Debug|x64.Build.0 = Debug|x64
|
||||
{1a2b3c4d-5e6f-4a7b-8c9d-0e1f2a3b4c5d}.Debug|x86.ActiveCfg = Debug|Win32
|
||||
{1a2b3c4d-5e6f-4a7b-8c9d-0e1f2a3b4c5d}.Debug|x86.Build.0 = Debug|Win32
|
||||
{1a2b3c4d-5e6f-4a7b-8c9d-0e1f2a3b4c5d}.Release|x64.ActiveCfg = Release|x64
|
||||
{1a2b3c4d-5e6f-4a7b-8c9d-0e1f2a3b4c5d}.Release|x64.Build.0 = Release|x64
|
||||
{1a2b3c4d-5e6f-4a7b-8c9d-0e1f2a3b4c5d}.Release|x86.ActiveCfg = Release|Win32
|
||||
{1a2b3c4d-5e6f-4a7b-8c9d-0e1f2a3b4c5d}.Release|x86.Build.0 = Release|Win32
|
||||
{80CBF888-79F3-424F-A094-17EDFD452E1E}.Debug|x64.ActiveCfg = Debug|x64
|
||||
{80CBF888-79F3-424F-A094-17EDFD452E1E}.Debug|x64.Build.0 = Debug|x64
|
||||
{80CBF888-79F3-424F-A094-17EDFD452E1E}.Debug|x86.ActiveCfg = Debug|Win32
|
||||
@ -606,6 +590,22 @@ Global
|
||||
{80CBF888-79F3-424F-A094-17EDFD452E1E}.Release|x64.Build.0 = Release|x64
|
||||
{80CBF888-79F3-424F-A094-17EDFD452E1E}.Release|x86.ActiveCfg = Release|Win32
|
||||
{80CBF888-79F3-424F-A094-17EDFD452E1E}.Release|x86.Build.0 = Release|Win32
|
||||
{8E5B3C2D-4A1F-4E9B-8D7C-3F2E5A6B7C8D}.Debug|x64.ActiveCfg = Debug|x64
|
||||
{8E5B3C2D-4A1F-4E9B-8D7C-3F2E5A6B7C8D}.Debug|x64.Build.0 = Debug|x64
|
||||
{8E5B3C2D-4A1F-4E9B-8D7C-3F2E5A6B7C8D}.Debug|x86.ActiveCfg = Debug|Win32
|
||||
{8E5B3C2D-4A1F-4E9B-8D7C-3F2E5A6B7C8D}.Debug|x86.Build.0 = Debug|Win32
|
||||
{8E5B3C2D-4A1F-4E9B-8D7C-3F2E5A6B7C8D}.Release|x64.ActiveCfg = Release|x64
|
||||
{8E5B3C2D-4A1F-4E9B-8D7C-3F2E5A6B7C8D}.Release|x64.Build.0 = Release|x64
|
||||
{8E5B3C2D-4A1F-4E9B-8D7C-3F2E5A6B7C8D}.Release|x86.ActiveCfg = Release|Win32
|
||||
{8E5B3C2D-4A1F-4E9B-8D7C-3F2E5A6B7C8D}.Release|x86.Build.0 = Release|Win32
|
||||
{1A2B3C4D-5E6F-4A7B-8C9D-0E1F2A3B4C5D}.Debug|x64.ActiveCfg = Debug|x64
|
||||
{1A2B3C4D-5E6F-4A7B-8C9D-0E1F2A3B4C5D}.Debug|x64.Build.0 = Debug|x64
|
||||
{1A2B3C4D-5E6F-4A7B-8C9D-0E1F2A3B4C5D}.Debug|x86.ActiveCfg = Debug|Win32
|
||||
{1A2B3C4D-5E6F-4A7B-8C9D-0E1F2A3B4C5D}.Debug|x86.Build.0 = Debug|Win32
|
||||
{1A2B3C4D-5E6F-4A7B-8C9D-0E1F2A3B4C5D}.Release|x64.ActiveCfg = Release|x64
|
||||
{1A2B3C4D-5E6F-4A7B-8C9D-0E1F2A3B4C5D}.Release|x64.Build.0 = Release|x64
|
||||
{1A2B3C4D-5E6F-4A7B-8C9D-0E1F2A3B4C5D}.Release|x86.ActiveCfg = Release|Win32
|
||||
{1A2B3C4D-5E6F-4A7B-8C9D-0E1F2A3B4C5D}.Release|x86.Build.0 = Release|Win32
|
||||
EndGlobalSection
|
||||
GlobalSection(SolutionProperties) = preSolution
|
||||
HideSolutionNode = FALSE
|
||||
|
||||
@ -12,6 +12,7 @@
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <Windows.h>
|
||||
#include <limits>
|
||||
#include <SG_baseAlgo_Export.h>
|
||||
|
||||
#include <cmath>
|
||||
typedef struct
|
||||
@ -32,6 +33,14 @@ typedef struct
|
||||
float b;
|
||||
} SPointXYZRGB;
|
||||
|
||||
bool fileExists(const char* path) {
|
||||
FILE* f = fopen(path, "r");
|
||||
if (f != NULL) {
|
||||
fclose(f);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// 点乘 dot
|
||||
double _dotMultiply(const SVzNL3DPoint& a, const SVzNL3DPoint& b)
|
||||
@ -1512,23 +1521,39 @@ void rodPositionTest(void)
|
||||
#endif
|
||||
}
|
||||
|
||||
int counterValidPts(std::vector<SVzNL3DPosition>& lineData)
|
||||
{
|
||||
int num = 0;
|
||||
for (int i = 0; i < (int)lineData.size(); i++)
|
||||
{
|
||||
if (lineData[i].pt3D.z > 1e-4)
|
||||
num++;
|
||||
else
|
||||
{
|
||||
lineData[i].pt3D = { 0, 0, 0 };
|
||||
}
|
||||
}
|
||||
return num;
|
||||
}
|
||||
|
||||
#define WELD_SEAM_TEST_GROUP 2
|
||||
#define WELD_SEAM_TEST_GROUP 4
|
||||
void rodWeldSeamPosition_test(void)
|
||||
{
|
||||
const char* dataPath[WELD_SEAM_TEST_GROUP] = {
|
||||
"F:/ShangGu/项目/冠钦项目/筑裕视觉钢结构焊接/视觉照片1/", //0
|
||||
"F:/ShangGu/项目/冠钦项目/筑裕视觉钢结构焊接/视觉照片2/", //0
|
||||
"F:/ShangGu/项目/冠钦项目/筑裕视觉钢结构焊接/视觉照片2/", //1
|
||||
"F:/ShangGu/项目/冠钦项目/筑裕视觉钢结构焊接/数据3/特征点1/", //2
|
||||
"F:/ShangGu/项目/冠钦项目/筑裕视觉钢结构焊接/数据3/特征点2/", //3
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[WELD_SEAM_TEST_GROUP] = {
|
||||
{1,20},{1,20},
|
||||
{1,20},{1,20},{1,27}, { 1,21 },
|
||||
};
|
||||
|
||||
const char* ver = wd_rodAndBarDetectionVersion();
|
||||
printf("ver:%s\n", ver);
|
||||
|
||||
for (int grp = 0; grp < WELD_SEAM_TEST_GROUP; grp++)
|
||||
for (int grp = 2; grp < WELD_SEAM_TEST_GROUP; grp++)
|
||||
{
|
||||
SSG_planeCalibPara poseCalibPara;
|
||||
//初始化成单位阵
|
||||
@ -1552,15 +1577,95 @@ void rodWeldSeamPosition_test(void)
|
||||
{
|
||||
//fidx =1;
|
||||
char _scan_file[256];
|
||||
sprintf_s(_scan_file, "%s%d_LaserData_ID019567.txt", dataPath[grp], fidx);
|
||||
sprintf_s(_scan_file, "%s%d_LaserData_groundAdjusted.txt", dataPath[grp], fidx);
|
||||
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanLines;
|
||||
wdReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
|
||||
|
||||
//转成plyTxt格式
|
||||
//sprintf_s(_scan_file, "%s%d_ply_Hi229229.txt", dataPath[grp], fidx);
|
||||
//wdSavePlyTxt(_scan_file, scanLines);
|
||||
if ((grp == 2) || (grp == 3)) //计算调平矩阵
|
||||
{
|
||||
#if 0
|
||||
std::vector<cv::Point3f> validPts;
|
||||
std::vector<std::vector< SVzNL3DPosition>> copy_scanLines;
|
||||
copy_scanLines.resize(scanLines.size());
|
||||
for (int line = 0; line < (int)scanLines.size(); line++)
|
||||
{
|
||||
std::vector< SVzNL3DPosition>& a_line = scanLines[line];
|
||||
copy_scanLines[line].resize(a_line.size());
|
||||
for (int j = 0; j < (int)a_line.size(); j++)
|
||||
{
|
||||
copy_scanLines[line][j] = a_line[j];
|
||||
if (a_line[j].pt3D.z > 1e-4)
|
||||
{
|
||||
cv::Point3f a_pt = cv::Point3f((float)a_line[j].pt3D.x, (float)a_line[j].pt3D.y, (float)a_line[j].pt3D.z);
|
||||
validPts.push_back(a_pt);
|
||||
}
|
||||
}
|
||||
}
|
||||
std::vector<cv::Point3f> out_inliers;
|
||||
Plane res = ransacFitPlane(validPts, out_inliers);
|
||||
if (res.C < 0)
|
||||
{
|
||||
res.A = -res.A;
|
||||
res.B = -res.B;
|
||||
res.C = -res.C;
|
||||
res.D = -res.D;
|
||||
}
|
||||
float meanH = 0;
|
||||
int counter = 0;
|
||||
for (int i = 0; i < (int)out_inliers.size(); i++)
|
||||
{
|
||||
meanH += out_inliers[i].z;
|
||||
counter++;
|
||||
}
|
||||
//计算投影向量
|
||||
SVzNL3DPoint vec_1 = { res.A, res.B, res.C };
|
||||
SVzNL3DPoint vec_2 = { 0, 0, 1.0 };
|
||||
SSG_planeCalibPara calibPara = wd_computeRTMatrix(vec_1, vec_2);
|
||||
for (int m = 0; m < 9; m++)
|
||||
{
|
||||
poseCalibPara.planeCalib[m] = calibPara.planeCalib[m];
|
||||
poseCalibPara.invRMatrix[m] = calibPara.invRMatrix[m];
|
||||
}
|
||||
poseCalibPara.planeHeight = meanH / counter;
|
||||
|
||||
char _out_file[256];
|
||||
sprintf_s(_out_file, "%s%d_calib_para.txt", dataPath[grp], fidx);
|
||||
_outputCalibPara(_out_file, poseCalibPara);
|
||||
|
||||
sprintf_s(_out_file, "%s%d_LaserData_groundAdjusted.txt", dataPath[grp], fidx);
|
||||
//if (false == fileExists(_out_file))
|
||||
{
|
||||
//输出调平效果
|
||||
int firstValidLine = -1;
|
||||
int lastValidLine = -1;
|
||||
for (int line = 0; line < (int)copy_scanLines.size(); line++)
|
||||
{
|
||||
//行处理
|
||||
//调平，去除地面
|
||||
double cuttingZ = poseCalibPara.planeHeight - 100;
|
||||
sx_rodPosition_lineDataR(copy_scanLines[line], poseCalibPara.planeCalib, cuttingZ);
|
||||
int ptsNum = counterValidPts(copy_scanLines[line]);
|
||||
if (ptsNum > 10)
|
||||
{
|
||||
if (firstValidLine < 0)
|
||||
firstValidLine = line;
|
||||
lastValidLine = line;
|
||||
}
|
||||
}
|
||||
if ((lastValidLine > 0) && (lastValidLine < copy_scanLines.size() - 1))
|
||||
{
|
||||
copy_scanLines.erase(copy_scanLines.begin() + lastValidLine + 1, copy_scanLines.end());
|
||||
}
|
||||
if(firstValidLine > 1)
|
||||
{
|
||||
copy_scanLines.erase(copy_scanLines.begin(), copy_scanLines.begin() + firstValidLine - 1);
|
||||
}
|
||||
_outputScanDataFile(_out_file, copy_scanLines, 0.0f, 0, 0);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
long t1 = (long)GetTickCount64();//统计时间
|
||||
|
||||
SSX_rodParam rodParam;
|
||||
@ -1583,7 +1688,7 @@ void rodWeldSeamPosition_test(void)
|
||||
growParam.maxLineSkipNum = 5;
|
||||
growParam.yDeviation_max = 5.0;
|
||||
growParam.maxSkipDistance = 20.0;
|
||||
growParam.zDeviation_max = 3.0;//
|
||||
growParam.zDeviation_max = 5.0;//
|
||||
growParam.minLTypeTreeLen = 50; //mm, 螺杆长度
|
||||
growParam.minVTypeTreeLen = 50; //mm
|
||||
|
||||
@ -1603,11 +1708,14 @@ void rodWeldSeamPosition_test(void)
|
||||
&errCode);
|
||||
long t2 = (long)GetTickCount64();
|
||||
printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1));
|
||||
//输出测试结果
|
||||
sprintf_s(_scan_file, "%sresult\\%d_result.txt", dataPath[grp], fidx);
|
||||
_outputRGBDScan_RGBD_weldSeam(_scan_file, scanLines, weldSeamInfo);
|
||||
sprintf_s(_scan_file, "%sresult\\%d_screw_info.txt", dataPath[grp], fidx);
|
||||
_outputWeldSeamInfo(_scan_file, weldSeamInfo);
|
||||
if (errCode == 0)
|
||||
{
|
||||
//输出测试结果
|
||||
sprintf_s(_scan_file, "%sresult\\%d_result.txt", dataPath[grp], fidx);
|
||||
_outputRGBDScan_RGBD_weldSeam(_scan_file, scanLines, weldSeamInfo);
|
||||
sprintf_s(_scan_file, "%sresult\\%d_screw_info.txt", dataPath[grp], fidx);
|
||||
_outputWeldSeamInfo(_scan_file, weldSeamInfo);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1627,8 +1735,8 @@ int main()
|
||||
//ESG_testMode testMode = keSG_测试_配天螺杆定位;
|
||||
//ESG_testMode testMode = keSG_测试_配天定位盘定位;
|
||||
//ESG_testMode testMode = keSG_测试_配天新定位盘定位;
|
||||
ESG_testMode testMode = keSG_测试_棒材抓取;
|
||||
//ESG_testMode testMode = keSG_测试_筑裕钢筋焊缝定位;
|
||||
//ESG_testMode testMode = keSG_测试_棒材抓取;
|
||||
ESG_testMode testMode = keSG_测试_筑裕钢筋焊缝定位;
|
||||
|
||||
if(keSG_测试_配天螺杆定位 == testMode)
|
||||
screwTest();
|
||||
|
||||
@ -126,7 +126,7 @@
|
||||
<SubSystem>Console</SubSystem>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
|
||||
<AdditionalDependencies>opencv_world320d.lib;rodAndBarDetection.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||
<AdditionalDependencies>opencv_world320d.lib;baseAlgorithm.lib;rodAndBarDetection.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
@ -145,7 +145,7 @@
|
||||
<OptimizeReferences>true</OptimizeReferences>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
|
||||
<AdditionalDependencies>opencv_world320.lib;rodAndBarDetection.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||
<AdditionalDependencies>opencv_world320.lib;baseAlgorithm.lib;rodAndBarDetection.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemGroup>
|
||||
|
||||
@ -173,7 +173,7 @@ SG_APISHARED_EXPORT void wd_lineDataSegment_dist_2(
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
std::vector<SSG_RUN>& segs,
|
||||
const double maxDistTh,
|
||||
const int minSegSize
|
||||
const double minSegSize
|
||||
);
|
||||
|
||||
SG_APISHARED_EXPORT void wd_lineDataSegment_zDist(
|
||||
@ -215,6 +215,21 @@ SG_APISHARED_EXPORT void wd_getRodArcFeature_peakCornerMethod(
|
||||
std::vector<SWD_rodArcFeature>& line_rodArcs //环
|
||||
);
|
||||
|
||||
/// <summary>
|
||||
/// 通过计算YZ平面内的曲率方法提取圆弧。
|
||||
/// seg端点：z距离大于门限
|
||||
/// </summary>
|
||||
SG_APISHARED_EXPORT void wd_getRodArcFeature_YZCurvatureMethod(
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
int lineIdx,
|
||||
const double maxDistTh,
|
||||
const double minSegSize,
|
||||
const double curvatureWin,
|
||||
const double rodDiameter,
|
||||
const double arcTotalCornerMinValue,
|
||||
std::vector<SWD_rodArcFeature>& line_rodArcs //环
|
||||
);
|
||||
|
||||
/// <summary>
|
||||
/// 提取激光线上的圆环的上半段弧。
|
||||
/// seg端点：z距离大于门限
|
||||
@ -663,6 +678,11 @@ SG_APISHARED_EXPORT double fitCircleByLeastSquare_2(
|
||||
const std::vector<SVzNL3DPosition>& pointArray,
|
||||
SVzNL3DPoint& center,
|
||||
double& radius);
|
||||
//圆最小二乘拟合
|
||||
SG_APISHARED_EXPORT double fitCircleByLeastSquare_3(
|
||||
const std::vector<SVzNL2DPointD>& pointArray,
|
||||
SVzNL2DPointD& center,
|
||||
double& radius);
|
||||
|
||||
//抛物线最小二乘拟合 y=ax^2 + bx + c
|
||||
SG_APISHARED_EXPORT bool leastSquareParabolaFitEigen(
|
||||
|
||||
@ -291,6 +291,7 @@ typedef struct
|
||||
SVzNL3DPoint endPt;
|
||||
SVzNL3DPoint peakPt;
|
||||
double featureValue;
|
||||
int flag;
|
||||
}SWD_rodArcFeature;
|
||||
|
||||
typedef struct
|
||||
@ -441,6 +442,7 @@ typedef struct
|
||||
int start;
|
||||
int len;
|
||||
int value;
|
||||
double curveLen;
|
||||
}SSG_RUN;
|
||||
|
||||
typedef struct
|
||||
|
||||
@ -843,7 +843,7 @@ void wd_getRodArcFeatureGrowingTrees(
|
||||
{
|
||||
for (int i = 0, i_max = (int)all_lineFeatures.size(); i < i_max; i++)
|
||||
{
|
||||
if (i == 1304)
|
||||
if (i == 770)
|
||||
int kkk = 1;
|
||||
std::vector<SWD_rodArcFeature>& a_lineFeatures = all_lineFeatures[i];
|
||||
for (int j = 0, j_max = (int)a_lineFeatures.size(); j < j_max; j++)
|
||||
|
||||
@ -2,6 +2,8 @@
|
||||
#include "SG_baseAlgo_Export.h"
|
||||
#include <vector>
|
||||
|
||||
const double EPS = 1e-8;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int flag;
|
||||
@ -3883,12 +3885,12 @@ void wd_lineDataSegment_dist_2(
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
std::vector<SSG_RUN>& segs,
|
||||
const double maxDistTh,
|
||||
const int minSegSize
|
||||
const double minSegSize
|
||||
)
|
||||
{
|
||||
int dataSize = (int)lineData.size();
|
||||
int runIdx = 1;
|
||||
SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx
|
||||
SSG_RUN a_run = { 0, -1, 0, 0.0}; //startIdx, len, lastIdx
|
||||
double pre_z = 0;
|
||||
double pre_y = 0;
|
||||
for (int i = 0; i < dataSize; i++)
|
||||
@ -3900,6 +3902,7 @@ void wd_lineDataSegment_dist_2(
|
||||
a_run.start = i;
|
||||
a_run.len = 1;
|
||||
a_run.value = i;
|
||||
a_run.curveLen = 0.0;
|
||||
}
|
||||
else
|
||||
{
|
||||
@ -3908,24 +3911,26 @@ void wd_lineDataSegment_dist_2(
|
||||
{
|
||||
a_run.len = i - a_run.start + 1;
|
||||
a_run.value = i;
|
||||
a_run.curveLen += dist;
|
||||
}
|
||||
else
|
||||
{
|
||||
a_run.value = runIdx;
|
||||
runIdx++;
|
||||
if (a_run.len >= minSegSize)
|
||||
if (a_run.curveLen >= minSegSize)
|
||||
segs.push_back(a_run);
|
||||
|
||||
a_run.start = i;
|
||||
a_run.len = 1;
|
||||
a_run.value = i;
|
||||
a_run.curveLen = 0;
|
||||
}
|
||||
}
|
||||
pre_y = lineData[i].pt3D.y;
|
||||
pre_z = lineData[i].pt3D.z;
|
||||
}
|
||||
}
|
||||
if (a_run.len > 0)
|
||||
if (a_run.curveLen > 0)
|
||||
{
|
||||
if (a_run.len >= minSegSize)
|
||||
segs.push_back(a_run);
|
||||
@ -4574,6 +4579,322 @@ void wd_getRodArcFeature_peakCornerMethod(
|
||||
return;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 三点圆法计算单个点的曲率（有序点列专用，线激光扫描线适用）
|
||||
* @param points 有序2D点列（整条激光扫描线）
|
||||
* @param index 当前要计算曲率的点索引
|
||||
* @param neighbor 邻域点数（建议 2~4，越大越平滑，推荐3）
|
||||
* @return 曲率值 k = 1/R，直线≈0，圆弧为稳定常数
|
||||
* @note 曲率符号：左凸为正，右凸为负；共线/三点重合返回0（直线）
|
||||
*/
|
||||
double calcYZCurvatureByThreePoints(const std::vector<SVzNL3DPosition>& points,
|
||||
int index,
|
||||
int neighbor = 3)
|
||||
{
|
||||
// 边界保护：防止越界
|
||||
int n = (int)points.size();
|
||||
if (index < neighbor || index >= n - neighbor) {
|
||||
return 0.0; // 边缘点无法计算，返回直线曲率
|
||||
}
|
||||
|
||||
// 取前后邻域点（增强稳定性，避免单点噪声）
|
||||
const SVzNL3DPosition& pPrev = points[index - neighbor];
|
||||
const SVzNL3DPosition& pCurr = points[index];
|
||||
const SVzNL3DPosition& pNext = points[index + neighbor];
|
||||
|
||||
// 提取坐标
|
||||
double x1 = pPrev.pt3D.y, y1 = pPrev.pt3D.z;
|
||||
double x2 = pCurr.pt3D.y, y2 = pCurr.pt3D.z;
|
||||
double x3 = pNext.pt3D.y, y3 = pNext.pt3D.z;
|
||||
|
||||
// 计算分母（三角形面积×2），判断是否共线
|
||||
double area2 = (x2 - x1) * (y3 - y1) - (y2 - y1) * (x3 - x1);
|
||||
if (fabs(area2) < 1e-8) {
|
||||
return 0.0; // 三点共线 → 直线，曲率=0
|
||||
}
|
||||
|
||||
// 计算外接圆半径 R
|
||||
double a = hypot(x2 - x3, y2 - y3);
|
||||
double b = hypot(x1 - x3, y1 - y3);
|
||||
double c = hypot(x1 - x2, y1 - y2);
|
||||
double radius = (a * b * c) / fabs(2.0 * area2);
|
||||
|
||||
// 曲率 k = 1/R，带符号（区分凹凸方向）
|
||||
double curvature = radius;
|
||||
return (area2 > 0) ? curvature : -curvature;
|
||||
}
|
||||
|
||||
//逐段计算曲率
|
||||
void _computeCurvature_perSeg(
|
||||
std::vector< SVzNL3DPosition>& linePts,
|
||||
std::vector<SSG_RUN>& segs,
|
||||
const double curvatureWin,
|
||||
std::vector< double>& ptCurvatures
|
||||
)
|
||||
{
|
||||
ptCurvatures.resize(linePts.size());
|
||||
std::fill(ptCurvatures.begin(), ptCurvatures.end(), 0);
|
||||
|
||||
//逐段进行
|
||||
int segSize = (int)segs.size();
|
||||
for (int segIdx = 0; segIdx < segSize; segIdx++)
|
||||
{
|
||||
if ((segs[segIdx].len < 2) || (segs[segIdx].curveLen < 1e-4))
|
||||
continue;
|
||||
double meanPtDist = segs[segIdx].curveLen / (segs[segIdx].len - 1);
|
||||
int neighbourWin = (int)(curvatureWin / meanPtDist) + 1;
|
||||
|
||||
int vPtIdxStart = segs[segIdx].start;
|
||||
int vPtIdxEnd = vPtIdxStart + segs[segIdx].len - 1;
|
||||
for (int i = vPtIdxStart; i <= vPtIdxEnd; i++)
|
||||
{
|
||||
double curvature = calcYZCurvatureByThreePoints(linePts, i, neighbourWin);
|
||||
ptCurvatures[i] = curvature;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 圆结构体：圆心 + 半径
|
||||
struct Circle2d
|
||||
{
|
||||
SVzNL2DPointD center;
|
||||
double radius;
|
||||
Circle2d() : radius(0) {}
|
||||
Circle2d(SVzNL2DPointD c, double r) : center(c), radius(r) {}
|
||||
};
|
||||
/**
|
||||
* @brief 计算圆弧段转角（圆心角）
|
||||
* @param circle 拟合得到的圆
|
||||
* @param pStart 弧段起点
|
||||
* @param pEnd 弧段终点
|
||||
* @param outAngleDeg 输出角度制转角(°)
|
||||
* @return 弧度制圆心角
|
||||
*/
|
||||
double calcArcAngle(const Circle2d& circle,
|
||||
const SVzNL2DPointD& pStart,
|
||||
const SVzNL2DPointD& pEnd,
|
||||
double& outAngleDeg)
|
||||
{
|
||||
SVzNL2DPointD O = circle.center;
|
||||
double R = circle.radius;
|
||||
if (R < EPS)
|
||||
{
|
||||
outAngleDeg = 0.0;
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
// 向量 OP_s , OP_e
|
||||
double dx1 = pStart.x - O.x;
|
||||
double dy1 = pStart.y - O.y;
|
||||
double dx2 = pEnd.x - O.x;
|
||||
double dy2 = pEnd.y - O.y;
|
||||
|
||||
// 点积
|
||||
double dot = dx1 * dx2 + dy1 * dy2;
|
||||
double cosTheta = dot / (R * R);
|
||||
|
||||
// 数值钳位，避免浮点误差导致 acos 越界 [-1,1]
|
||||
cosTheta = std::max(-1.0, std::min(1.0, cosTheta));
|
||||
|
||||
// 弧度制圆心角
|
||||
double rad = acos(cosTheta);
|
||||
// 转为角度
|
||||
outAngleDeg = rad * 180.0 / PI;
|
||||
|
||||
return rad;
|
||||
}
|
||||
|
||||
bool _fittingArc(
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
int startIdx, int endIdx,
|
||||
double& outRadius, double& outArcAngle
|
||||
)
|
||||
{
|
||||
std::vector<SVzNL2DPointD> pointArray;
|
||||
for (int j = startIdx; j <= endIdx; j++)
|
||||
{
|
||||
if (lineData[j].pt3D.z > 1e-4)
|
||||
{
|
||||
SVzNL2DPointD pt2d = { lineData[j].pt3D.y, lineData[j].pt3D.z };
|
||||
pointArray.push_back(pt2d);
|
||||
}
|
||||
}
|
||||
if (pointArray.size() < 5)
|
||||
return false;
|
||||
//计算转角
|
||||
SVzNL2DPointD center = { 0, 0 };
|
||||
double radius = -1.0;
|
||||
fitCircleByLeastSquare_3(pointArray, center, radius);
|
||||
//计算转角
|
||||
Circle2d circle = Circle2d(center, radius);
|
||||
double outAngleDeg = 0;
|
||||
double angleRad = calcArcAngle(circle, pointArray[0], pointArray.back(), outAngleDeg);
|
||||
outRadius = radius;
|
||||
outArcAngle = outAngleDeg;
|
||||
return true;
|
||||
}
|
||||
|
||||
int _findValidMidIdx(std::vector< SVzNL3DPosition>& lineData, int startIdx, int endIdx)
|
||||
{
|
||||
int midIdx = (startIdx + endIdx) / 2;
|
||||
int minDiff = -1;
|
||||
int validMidIdx = -1;
|
||||
for (int i = startIdx; i <= endIdx; i++)
|
||||
{
|
||||
if (lineData[i].pt3D.z > 1e-4)
|
||||
{
|
||||
int diff = midIdx - i;
|
||||
if (diff < 0)
|
||||
diff = -diff;
|
||||
if ( (validMidIdx < 0) || (minDiff > diff))
|
||||
{
|
||||
validMidIdx = i;
|
||||
minDiff = diff;
|
||||
}
|
||||
}
|
||||
}
|
||||
return validMidIdx;
|
||||
}
|
||||
/// <summary>
|
||||
/// 通过计算YZ平面内的曲率方法提取圆弧。
|
||||
/// seg端点：z距离大于门限
|
||||
/// </summary>
|
||||
void wd_getRodArcFeature_YZCurvatureMethod(
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
int lineIdx,
|
||||
const double maxDistTh,
|
||||
const double minSegSize,
|
||||
const double curvatureWin,
|
||||
const double rodDiameter,
|
||||
const double arcTotalCornerMinValue,
|
||||
std::vector<SWD_rodArcFeature>& line_rodArcs //环
|
||||
)
|
||||
{
|
||||
std::vector<SSG_RUN> segs;
|
||||
wd_lineDataSegment_dist_2(
|
||||
lineData,
|
||||
segs,
|
||||
maxDistTh,
|
||||
minSegSize
|
||||
);
|
||||
//计算前向角和后向角
|
||||
std::vector<double> ptCurvatures;
|
||||
_computeCurvature_perSeg(lineData, segs, curvatureWin, ptCurvatures);
|
||||
|
||||
//搜索曲率
|
||||
std::vector<SSG_RUN> allArcs;
|
||||
int segSize = (int)segs.size();
|
||||
for (int segIdx = 0; segIdx < segSize; segIdx++)
|
||||
{
|
||||
SSG_RUN an_arc = { 0, -1, 0, 0.0 }; //startIdx, len, lastIdx
|
||||
int vPtIdxStart = segs[segIdx].start;
|
||||
int vPtIdxEnd = vPtIdxStart + segs[segIdx].len - 1;
|
||||
for (int i = vPtIdxStart; i <= vPtIdxEnd; i++)
|
||||
{
|
||||
if (lineData[i].pt3D.z < 1e-4)
|
||||
continue;
|
||||
|
||||
if ((ptCurvatures[i] > rodDiameter / 4) && (ptCurvatures[i] < rodDiameter*4))
|
||||
{
|
||||
if (an_arc.len < 0)
|
||||
{
|
||||
an_arc.start = i;
|
||||
an_arc.len = 1;
|
||||
an_arc.value = segIdx;
|
||||
an_arc.curveLen = 0.0;
|
||||
}
|
||||
else
|
||||
{
|
||||
an_arc.len = i - an_arc.start + 1;
|
||||
an_arc.value = segIdx;
|
||||
}
|
||||
}
|
||||
else if (an_arc.len > 0)
|
||||
{
|
||||
allArcs.push_back(an_arc);
|
||||
an_arc = { 0, -1, 0, 0.0 };
|
||||
}
|
||||
}
|
||||
if (an_arc.len > 0)
|
||||
{
|
||||
allArcs.push_back(an_arc);
|
||||
}
|
||||
}
|
||||
|
||||
if (allArcs.size() > 0)
|
||||
{
|
||||
for (int i = 0; i < (int)allArcs.size(); i++)
|
||||
{
|
||||
int vPtIdxStart = allArcs[i].start;
|
||||
int vPtIdxEnd = vPtIdxStart + allArcs[i].len - 1;
|
||||
|
||||
double fittingR=0, fittingAngle = 0;
|
||||
bool validResult = _fittingArc(
|
||||
lineData,
|
||||
vPtIdxStart, vPtIdxEnd,
|
||||
fittingR, fittingAngle);
|
||||
|
||||
if(false == validResult)
|
||||
continue;
|
||||
|
||||
if ( (fittingAngle > arcTotalCornerMinValue) && (fittingR > rodDiameter /8) && (fittingR < rodDiameter) )
|
||||
{
|
||||
SWD_rodArcFeature a_feature;
|
||||
memset(&a_feature, 0, sizeof(SWD_rodArcFeature));
|
||||
a_feature.startPtIdx = vPtIdxStart;
|
||||
a_feature.startPt = lineData[vPtIdxStart].pt3D;
|
||||
a_feature.endPtIdx = vPtIdxEnd;
|
||||
a_feature.endPt = lineData[vPtIdxEnd].pt3D;
|
||||
a_feature.lineIdx = lineIdx;
|
||||
a_feature.featureValue = fittingR;
|
||||
a_feature.flag = allArcs[i].value;
|
||||
int validMidIdx = _findValidMidIdx(lineData, a_feature.startPtIdx, a_feature.endPtIdx);
|
||||
a_feature.peakPtIdx = validMidIdx;
|
||||
a_feature.peakPt = lineData[validMidIdx].pt3D;
|
||||
//检查是否合并
|
||||
if (line_rodArcs.size() > 0)
|
||||
{
|
||||
int lastIdx = static_cast<int>(line_rodArcs.size()) -1;
|
||||
SWD_rodArcFeature& lastArc = line_rodArcs[lastIdx];
|
||||
if (((a_feature.startPtIdx - lastArc.endPtIdx) < 5) &&
|
||||
(a_feature.flag == lastArc.flag)) //同一个Segment，合并
|
||||
{
|
||||
vPtIdxStart = lastArc.startPtIdx;
|
||||
vPtIdxEnd = a_feature.endPtIdx;
|
||||
validResult = _fittingArc(
|
||||
lineData,
|
||||
vPtIdxStart, vPtIdxEnd,
|
||||
fittingR, fittingAngle);
|
||||
|
||||
if (false == validResult)
|
||||
line_rodArcs.push_back(a_feature);
|
||||
else
|
||||
{
|
||||
if ((fittingAngle > arcTotalCornerMinValue) && (fittingR > rodDiameter / 4) && (fittingR < rodDiameter))
|
||||
{
|
||||
lastArc.endPtIdx = a_feature.endPtIdx;
|
||||
lastArc.endPt = a_feature.endPt;
|
||||
lastArc.featureValue = fittingR;
|
||||
validMidIdx = _findValidMidIdx(lineData, lastArc.startPtIdx, lastArc.endPtIdx);
|
||||
lastArc.peakPtIdx = validMidIdx;
|
||||
lastArc.peakPt = lineData[validMidIdx].pt3D;
|
||||
}
|
||||
else
|
||||
line_rodArcs.push_back(a_feature);
|
||||
}
|
||||
}
|
||||
else
|
||||
line_rodArcs.push_back(a_feature);
|
||||
}
|
||||
else
|
||||
line_rodArcs.push_back(a_feature);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 提取激光线上的圆环的上半段弧。
|
||||
/// seg端点：z距离大于门限
|
||||
@ -4591,7 +4912,7 @@ void wd_getRodArcFeature_segmentPeakCornerMethod(
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
int lineIdx,
|
||||
const double maxDistTh,
|
||||
const int minSegSize,
|
||||
const double minSegSize,
|
||||
const double peakChkWin,
|
||||
const SSG_cornerParam cornerPara,
|
||||
std::vector<SWD_rodArcFeature>& line_rodArcs //环
|
||||
|
||||
@ -253,6 +253,72 @@ double fitCircleByLeastSquare_2(
|
||||
return err;
|
||||
}
|
||||
|
||||
//Ô˛×îĐĄśţłËÄâşĎ
|
||||
double fitCircleByLeastSquare_3(
|
||||
const std::vector<SVzNL2DPointD>& pointArray,
|
||||
SVzNL2DPointD& center,
|
||||
double& radius)
|
||||
{
|
||||
int N = pointArray.size();
|
||||
if (N < 3) {
|
||||
return std::numeric_limits<double>::max();
|
||||
}
|
||||
|
||||
double sumX = 0.0;
|
||||
double sumY = 0.0;
|
||||
double sumX2 = 0.0;
|
||||
double sumY2 = 0.0;
|
||||
double sumX3 = 0.0;
|
||||
double sumY3 = 0.0;
|
||||
double sumXY = 0.0;
|
||||
double sumXY2 = 0.0;
|
||||
double sumX2Y = 0.0;
|
||||
|
||||
for (int pId = 0; pId < N; ++pId) {
|
||||
sumX += pointArray[pId].x;
|
||||
sumY += pointArray[pId].y;
|
||||
|
||||
double x2 = pointArray[pId].x * pointArray[pId].x;
|
||||
double y2 = pointArray[pId].y * pointArray[pId].y;
|
||||
sumX2 += x2;
|
||||
sumY2 += y2;
|
||||
|
||||
sumX3 += x2 * pointArray[pId].x;
|
||||
sumY3 += y2 * pointArray[pId].y;
|
||||
sumXY += pointArray[pId].x * pointArray[pId].y;
|
||||
sumXY2 += pointArray[pId].x * y2;
|
||||
sumX2Y += x2 * pointArray[pId].y;
|
||||
}
|
||||
|
||||
double C, D, E, G, H;
|
||||
double a, b, c;
|
||||
|
||||
C = N * sumX2 - sumX * sumX;
|
||||
D = N * sumXY - sumX * sumY;
|
||||
E = N * sumX3 + N * sumXY2 - (sumX2 + sumY2) * sumX;
|
||||
G = N * sumY2 - sumY * sumY;
|
||||
H = N * sumX2Y + N * sumY3 - (sumX2 + sumY2) * sumY;
|
||||
|
||||
a = (H * D - E * G) / (C * G - D * D);
|
||||
b = (H * C - E * D) / (D * D - G * C);
|
||||
c = -(a * sumX + b * sumY + sumX2 + sumY2) / N;
|
||||
|
||||
center.x = -a / 2.0;
|
||||
center.y = -b / 2.0;
|
||||
radius = sqrt(a * a + b * b - 4 * c) / 2.0;
|
||||
|
||||
double err = 0.0;
|
||||
double e;
|
||||
double r2 = radius * radius;
|
||||
for (int pId = 0; pId < N; ++pId) {
|
||||
e = pow(pointArray[pId].x - center.x, 2) + pow(pointArray[pId].y - center.y, 2) - r2;
|
||||
if (e > err) {
|
||||
err = e;
|
||||
}
|
||||
}
|
||||
return err;
|
||||
}
|
||||
|
||||
#if 0
|
||||
bool leastSquareParabolaFit(const std::vector<cv::Point2d>& points,
|
||||
double& a, double& b, double& c,
|
||||
|
||||
@ -23,7 +23,8 @@
|
||||
//version 1.3.0 : 新的定位盘中心测量功能
|
||||
//version 1.3.1 : 定位盘中心测量功能添加了噪声过滤
|
||||
//version 1.3.2 : 矩森棒材抓取修正一个错误码返回错误，测试了地面调平API
|
||||
std::string m_strVersion = "1.3.2";
|
||||
//version 1.3.3 : 筑裕钢筋焊缝提取算法改进中间版本
|
||||
std::string m_strVersion = "1.3.3";
|
||||
const char* wd_rodAndBarDetectionVersion(void)
|
||||
{
|
||||
return m_strVersion.c_str();
|
||||
@ -246,7 +247,7 @@ void rodAarcFeatueDetection(
|
||||
int linePtNum = (int)scanLines[0].size();
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
if (line == 413)
|
||||
if (line == 1062)
|
||||
int kkk = 1;
|
||||
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
|
||||
|
||||
@ -254,6 +255,19 @@ void rodAarcFeatueDetection(
|
||||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
|
||||
// Extract rod arc features
|
||||
std::vector<SWD_rodArcFeature> line_rodArcs;
|
||||
#if 1
|
||||
double arcTotalCornerMinValue = 22.5; //整个Arc的转角最小值, 360/16
|
||||
wd_getRodArcFeature_YZCurvatureMethod(
|
||||
lineData,
|
||||
line,
|
||||
rodDiameter / 4,
|
||||
rodDiameter / 4,
|
||||
rodDiameter / 4,
|
||||
rodDiameter,
|
||||
arcTotalCornerMinValue,
|
||||
line_rodArcs //环
|
||||
);
|
||||
#else
|
||||
wd_getRodArcFeature_segmentPeakCornerMethod(
|
||||
lineData,
|
||||
line,
|
||||
@ -263,6 +277,7 @@ void rodAarcFeatueDetection(
|
||||
cornerPara,
|
||||
line_rodArcs //环
|
||||
);
|
||||
#endif
|
||||
//wd_getRodArcFeature_peakCornerMethod(lineData, line, rodDiameter / 2, cornerPara, line_rodArcs);
|
||||
arcFeatures.push_back(line_rodArcs);
|
||||
}
|
||||
@ -3675,6 +3690,7 @@ void sx_rebarWeldSeamPositioning(
|
||||
int* errCode)
|
||||
{
|
||||
*errCode = 0;
|
||||
|
||||
int lineNum = (int)scanLines.size();
|
||||
if (lineNum == 0)
|
||||
{
|
||||
@ -3706,7 +3722,11 @@ void sx_rebarWeldSeamPositioning(
|
||||
int kkk = 1;
|
||||
//行处理
|
||||
//调平，去除地面
|
||||
double cuttingZ = -1;
|
||||
double cuttingZ;
|
||||
if (poseCalibPara.planeHeight < 0)
|
||||
cuttingZ = -1;
|
||||
else
|
||||
cuttingZ = poseCalibPara.planeHeight - 100;// -1;
|
||||
sx_rodPosition_lineDataR(scanLines[i], poseCalibPara.planeCalib, cuttingZ);
|
||||
}
|
||||
|
||||
@ -3748,15 +3768,40 @@ void sx_rebarWeldSeamPositioning(
|
||||
std::vector<std::vector<SWD_rodArcFeature>> arcFeatures_v;
|
||||
rodAarcFeatueDetection(scanLines, cornerPara, filterParam, rodParam.diameter, arcFeatures_v);
|
||||
//特征生长
|
||||
std::vector<SWD_rodArcFeatureTree> rodArcTrees_v;
|
||||
wd_getRodArcFeatureGrowingTrees(arcFeatures_v, rodArcTrees_v, growParam);
|
||||
std::vector<SWD_rodArcFeatureTree> allRodArcTrees_v;
|
||||
wd_getRodArcFeatureGrowingTrees(arcFeatures_v, allRodArcTrees_v, growParam);
|
||||
|
||||
//根据水平度过滤目标（已经调平）
|
||||
double maxTanValue = tan(10.0 * PI / 180);
|
||||
std::vector<SWD_rodArcFeatureTree> rodArcTrees_v;
|
||||
for (int i = 0; i < (int)allRodArcTrees_v.size(); i++)
|
||||
{
|
||||
SWD_rodArcFeature node_first = allRodArcTrees_v[i].treeNodes[0];
|
||||
SWD_rodArcFeature node_last = allRodArcTrees_v[i].treeNodes.back();
|
||||
double dist_xy = sqrt(pow(node_first.peakPt.x - node_last.peakPt.x, 2) + pow(node_first.peakPt.y - node_last.peakPt.y, 2));
|
||||
double dist_z = abs(node_first.peakPt.z - node_last.peakPt.z);
|
||||
double tanValue = dist_z / dist_xy;
|
||||
if (tanValue < maxTanValue)
|
||||
rodArcTrees_v.push_back(allRodArcTrees_v[i]);
|
||||
}
|
||||
//水平方向
|
||||
std::vector<std::vector<SWD_rodArcFeature>> arcFeatures_h;
|
||||
rodAarcFeatueDetection(hLines_raw, cornerPara, filterParam, rodParam.diameter, arcFeatures_h);
|
||||
//特征生长
|
||||
std::vector<SWD_rodArcFeatureTree> allRodArcTrees_h;
|
||||
wd_getRodArcFeatureGrowingTrees(arcFeatures_h, allRodArcTrees_h, growParam);
|
||||
//根据水平度过滤目标（已经调平）
|
||||
std::vector<SWD_rodArcFeatureTree> rodArcTrees_h;
|
||||
wd_getRodArcFeatureGrowingTrees(arcFeatures_h, rodArcTrees_h, growParam);
|
||||
for (int i = 0; i < (int)allRodArcTrees_h.size(); i++)
|
||||
{
|
||||
SWD_rodArcFeature node_first = allRodArcTrees_h[i].treeNodes[0];
|
||||
SWD_rodArcFeature node_last = allRodArcTrees_h[i].treeNodes.back();
|
||||
double dist_xy = sqrt(pow(node_first.peakPt.x - node_last.peakPt.x, 2) + pow(node_first.peakPt.y - node_last.peakPt.y, 2));
|
||||
double dist_z = abs(node_first.peakPt.z - node_last.peakPt.z);
|
||||
double tanValue = dist_z / dist_xy;
|
||||
if (tanValue < maxTanValue)
|
||||
rodArcTrees_h.push_back(allRodArcTrees_h[i]);
|
||||
}
|
||||
|
||||
if ((rodArcTrees_v.size() == 0) && (rodArcTrees_h.size() == 0))
|
||||
{
|
||||
@ -3885,7 +3930,7 @@ void sx_rebarWeldSeamPositioning(
|
||||
double len_vRod = _compute2DDistance_indexingPt(vRod.line_start, vRod.line_end);
|
||||
double cross_vRod_start = _compute2DDistance(vRod.line_start.point, crossPt);
|
||||
double cross_vRod_end = _compute2DDistance(vRod.line_end.point, crossPt);
|
||||
if ( (cross_vRod_start > len_vRod) || (cross_vRod_end > len_vRod)) //ÏßÍâ
|
||||
if ( (cross_vRod_start > (len_vRod-rodParam.diameter/2)) || (cross_vRod_end > (len_vRod-rodParam.diameter/2))) //线外
|
||||
{
|
||||
SWD3DPointPostion nearestLinePt_vRod;
|
||||
if (cross_vRod_start < cross_vRod_end)
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user