diff --git a/SG_Algorithm.sln b/SG_Algorithm.sln index 35046f8..9259f48 100644 --- a/SG_Algorithm.sln +++ b/SG_Algorithm.sln @@ -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 diff --git a/rodAndBarDetection_test/rodAndBarDetection_test.cpp b/rodAndBarDetection_test/rodAndBarDetection_test.cpp index d79c59c..1045052 100644 --- a/rodAndBarDetection_test/rodAndBarDetection_test.cpp +++ b/rodAndBarDetection_test/rodAndBarDetection_test.cpp @@ -12,6 +12,7 @@ #include #include #include +#include #include 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& 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> 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 validPts; + std::vector> 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 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(); diff --git a/rodAndBarDetection_test/rodAndBarDetection_test.vcxproj b/rodAndBarDetection_test/rodAndBarDetection_test.vcxproj index e1fed7d..b2be1d1 100644 --- a/rodAndBarDetection_test/rodAndBarDetection_test.vcxproj +++ b/rodAndBarDetection_test/rodAndBarDetection_test.vcxproj @@ -126,7 +126,7 @@ Console true ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories) - opencv_world320d.lib;rodAndBarDetection.lib;%(AdditionalDependencies) + opencv_world320d.lib;baseAlgorithm.lib;rodAndBarDetection.lib;%(AdditionalDependencies) @@ -145,7 +145,7 @@ true true ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories) - opencv_world320.lib;rodAndBarDetection.lib;%(AdditionalDependencies) + opencv_world320.lib;baseAlgorithm.lib;rodAndBarDetection.lib;%(AdditionalDependencies) diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index 156b777..74221ca 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -173,7 +173,7 @@ SG_APISHARED_EXPORT void wd_lineDataSegment_dist_2( std::vector< SVzNL3DPosition>& lineData, std::vector& 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& line_rodArcs // ); +/// +/// ͨYZƽڵʷȡԲ +/// seg˵㣺z +/// +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& line_rodArcs // +); + /// /// ȡϵԲϰλ /// seg˵㣺z @@ -663,6 +678,11 @@ SG_APISHARED_EXPORT double fitCircleByLeastSquare_2( const std::vector& pointArray, SVzNL3DPoint& center, double& radius); +//ԲС +SG_APISHARED_EXPORT double fitCircleByLeastSquare_3( + const std::vector& pointArray, + SVzNL2DPointD& center, + double& radius); //С y=ax^2 + bx + c SG_APISHARED_EXPORT bool leastSquareParabolaFitEigen( diff --git a/sourceCode/SG_baseDataType.h b/sourceCode/SG_baseDataType.h index e619576..b8fa42e 100644 --- a/sourceCode/SG_baseDataType.h +++ b/sourceCode/SG_baseDataType.h @@ -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 diff --git a/sourceCode/SG_featureGrow.cpp b/sourceCode/SG_featureGrow.cpp index f7e7410..8968de1 100644 --- a/sourceCode/SG_featureGrow.cpp +++ b/sourceCode/SG_featureGrow.cpp @@ -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& a_lineFeatures = all_lineFeatures[i]; for (int j = 0, j_max = (int)a_lineFeatures.size(); j < j_max; j++) diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index c08e9fd..808d6ab 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -2,6 +2,8 @@ #include "SG_baseAlgo_Export.h" #include +const double EPS = 1e-8; + typedef struct { int flag; @@ -3883,12 +3885,12 @@ void wd_lineDataSegment_dist_2( std::vector< SVzNL3DPosition>& lineData, std::vector& 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& 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& 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 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; +} +/// +/// ͨYZƽڵʷȡԲ +/// seg˵㣺z +/// +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& line_rodArcs // +) +{ + std::vector segs; + wd_lineDataSegment_dist_2( + lineData, + segs, + maxDistTh, + minSegSize + ); + //ǰǺͺ + std::vector ptCurvatures; + _computeCurvature_perSeg(lineData, segs, curvatureWin, ptCurvatures); + + // + std::vector 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(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; +} + /// /// ȡϵԲϰλ /// 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& line_rodArcs // diff --git a/sourceCode/dataFitting.cpp b/sourceCode/dataFitting.cpp index 0d4e926..be40cfd 100644 --- a/sourceCode/dataFitting.cpp +++ b/sourceCode/dataFitting.cpp @@ -253,6 +253,72 @@ double fitCircleByLeastSquare_2( return err; } +//ԲС +double fitCircleByLeastSquare_3( + const std::vector& pointArray, + SVzNL2DPointD& center, + double& radius) +{ + int N = pointArray.size(); + if (N < 3) { + return std::numeric_limits::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& points, double& a, double& b, double& c, diff --git a/sourceCode/rodAndBarDetection.cpp b/sourceCode/rodAndBarDetection.cpp index f7009fa..23da844 100644 --- a/sourceCode/rodAndBarDetection.cpp +++ b/sourceCode/rodAndBarDetection.cpp @@ -23,7 +23,8 @@ //version 1.3.0 : µĶλIJ //version 1.3.1 : λIJ //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& lineData = scanLines[line]; @@ -254,6 +255,19 @@ void rodAarcFeatueDetection( sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam); // Extract rod arc features std::vector 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> arcFeatures_v; rodAarcFeatueDetection(scanLines, cornerPara, filterParam, rodParam.diameter, arcFeatures_v); // - std::vector rodArcTrees_v; - wd_getRodArcFeatureGrowingTrees(arcFeatures_v, rodArcTrees_v, growParam); + std::vector allRodArcTrees_v; + wd_getRodArcFeatureGrowingTrees(arcFeatures_v, allRodArcTrees_v, growParam); + //ˮƽȹĿ꣨Ѿƽ + double maxTanValue = tan(10.0 * PI / 180); + std::vector 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> arcFeatures_h; rodAarcFeatueDetection(hLines_raw, cornerPara, filterParam, rodParam.diameter, arcFeatures_h); // + std::vector allRodArcTrees_h; + wd_getRodArcFeatureGrowingTrees(arcFeatures_h, allRodArcTrees_h, growParam); + //ˮƽȹĿ꣨Ѿƽ std::vector 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)