diff --git a/CMakeLists.txt b/CMakeLists.txt index c56fbd7..793884e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -77,6 +77,8 @@ if(OPENCV_VERSION STREQUAL "320") ADD_LIBRARY(workpieceHolePositioning SHARED sourceCode/workpieceHolePositioning.cpp) # ADD_LIBRARY(motorStatorPosition SHARED sourceCode/motorStatorPosition.cpp) + + ADD_LIBRARY(steelPipeDetection SHARED sourceCode/steelPipeDetection.cpp) elseif(OPENCV_VERSION STREQUAL "480") ADD_LIBRARY(binocularMarkCam SHARED sourceCode/binocularMarkCam.cpp diff --git a/SG_Algorithm.sln b/SG_Algorithm.sln index fbea4e8..35046f8 100644 --- a/SG_Algorithm.sln +++ b/SG_Algorithm.sln @@ -227,6 +227,17 @@ 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}" + 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}" + ProjectSection(ProjectDependencies) = postProject + {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} + {8e5b3c2d-4a1f-4e9b-8d7c-3f2e5a6b7c8d} = {8e5b3c2d-4a1f-4e9b-8d7c-3f2e5a6b7c8d} + EndProjectSection +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|x64 = Debug|x64 @@ -571,6 +582,22 @@ 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 diff --git a/rodAndBarDetection_test/rodAndBarDetection_test.cpp b/rodAndBarDetection_test/rodAndBarDetection_test.cpp index bd42f1b..456e95c 100644 --- a/rodAndBarDetection_test/rodAndBarDetection_test.cpp +++ b/rodAndBarDetection_test/rodAndBarDetection_test.cpp @@ -333,9 +333,13 @@ void _outputRGBDScan_RGBD( else if (pt3D->nPointIdx == 2) { rgb = { 250, 0, 0 }; - size = 5; + size = 3; + } + else if (pt3D->nPointIdx == 4) + { + rgb = { 250, 250, 0 }; + size = 6; } - else //if (pt3D->nPointIdx == 0) { rgb = { 200, 200, 200 }; @@ -889,27 +893,31 @@ void _outputRGBDScan_RGBD_weldSeam( sw.close(); } -#define SCREW_TEST_GROUP 1 +#define SCREW_TEST_GROUP 2 void screwTest(void) { const char* dataPath[SCREW_TEST_GROUP] = { "F:/ShangGu/项目/冠钦项目/螺杆测量/数据/模拟数据/", //0 + "F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/螺杆点云2/上方两根/", //1 }; SVzNLRange fileIdx[SCREW_TEST_GROUP] = { - {1,4}, + {1,4},{1,30} }; const char* ver = wd_rodAndBarDetectionVersion(); printf("ver:%s\n", ver); - for (int grp = 0; grp < SCREW_TEST_GROUP; grp++) + for (int grp = 1; grp < SCREW_TEST_GROUP; grp++) { for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { //fidx =7; char _scan_file[256]; - sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx); + if(1 == grp) + sprintf_s(_scan_file, "%s%d_LaserData_Jl26C299.txt", dataPath[grp], fidx); + else + sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx); std::vector> scanLines; wdReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); @@ -920,7 +928,13 @@ void screwTest(void) long t1 = (long)GetTickCount64();//统计时间 - double rodDiameter = 10.0; + double rodDiameter; + if (grp == 0) + rodDiameter = 10.0; + else + rodDiameter = 28.0; //现场螺杆直径28mm + + //double rodDiameter = 10.0; SSG_cornerParam cornerParam; cornerParam.cornerTh = 60; //45度角 @@ -1223,8 +1237,8 @@ void rodWeldSeamPosition_test(void) int main() { -#if 0 //螺杆定位测试 -#if 0 +#if 1 //螺杆定位测试 +#if 1 screwTest(); #else locatingPlateTest(); diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index a05da66..0920d68 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -166,6 +166,14 @@ SG_APISHARED_EXPORT void wd_lineDataSegment_dist( const int minSegSize ); +//ݾԷֶ +SG_APISHARED_EXPORT void wd_lineDataSegment_dist_2( + std::vector< SVzNL3DPosition>& lineData, + std::vector& segs, + const double maxDistTh, + const int minSegSize +); + SG_APISHARED_EXPORT void wd_lineDataSegment_zDist( std::vector< SVzNL3DPosition>& lineData, std::vector< SVzNL3DPosition>& vldPts, @@ -205,6 +213,29 @@ SG_APISHARED_EXPORT void wd_getRodArcFeature_peakCornerMethod( std::vector& line_rodArcs // ); +/// +/// ȡϵԲϰλ +/// seg˵㣺z +/// nPointIdx¶Feature +/// 㷨̣ +/// 1μǰǺͺ +/// 2ͬĹս +/// 2Zֵ +/// 2սǣ˳ʱΪʱΪ +/// 3սǵļֵ +/// 4жϹսǷΪ +/// +/// +SG_APISHARED_EXPORT void wd_getRodArcFeature_segmentPeakCornerMethod( + std::vector< SVzNL3DPosition>& lineData, + int lineIdx, + const double maxDistTh, + const int minSegSize, + const double peakChkWin, + const SSG_cornerParam cornerPara, + std::vector& line_rodArcs // +); + //ȡcornerֵʵֺʹô˺дŻ SG_APISHARED_EXPORT void wd_searchCornerPeaks( std::vector< SSG_pntDirAngle>& corners, diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index eed9bf6..1901b8b 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -3878,6 +3878,60 @@ void wd_lineDataSegment_dist( } } +//ݾԷֶ +void wd_lineDataSegment_dist_2( + std::vector< SVzNL3DPosition>& lineData, + std::vector& segs, + const double maxDistTh, + const int minSegSize +) +{ + int dataSize = (int)lineData.size(); + int runIdx = 1; + SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx + double pre_z = 0; + double pre_y = 0; + for (int i = 0; i < dataSize; i++) + { + if (lineData[i].pt3D.z > 1e-4) + { + if (a_run.len < 0) + { + a_run.start = i; + a_run.len = 1; + a_run.value = i; + } + else + { + double dist = sqrt(pow(lineData[i].pt3D.y - pre_y, 2) + pow(lineData[i].pt3D.z - pre_z, 2)); + if (dist < maxDistTh) + { + a_run.len = i - a_run.start + 1; + a_run.value = i; + } + else + { + a_run.value = runIdx; + runIdx++; + if (a_run.len >= minSegSize) + segs.push_back(a_run); + + a_run.start = i; + a_run.len = 1; + a_run.value = i; + } + } + pre_y = lineData[i].pt3D.y; + pre_z = lineData[i].pt3D.z; + } + } + if (a_run.len > 0) + { + if (a_run.len >= minSegSize) + segs.push_back(a_run); + } +} + //ZԷֶ void wd_lineDataSegment_zDist( std::vector< SVzNL3DPosition>& lineData, @@ -3959,6 +4013,18 @@ void _computeDirAngle_perSeg( ) { ptDirAngles.resize(vldPts.size()); + for (int i = 0; i < (int)vldPts.size(); i++) + { + ptDirAngles[i].pntIdx = -1; + ptDirAngles[i].type = -1; + ptDirAngles[i].forwardPntIdx = 0; + ptDirAngles[i].backwardPntIdx = 0; + ptDirAngles[i].forwardAngle = 0; + ptDirAngles[i].backwardAngle = 0; + ptDirAngles[i].corner = 0; + ptDirAngles[i].forwardDiffZ = 0; + ptDirAngles[i].backwardDiffZ = 0; + } //ν int segSize = (int)segs.size(); for (int segIdx = 0; segIdx < segSize; segIdx++) @@ -3992,24 +4058,89 @@ void _computeDirAngle_perSeg( } } //ս - if ((pre_i < 0) || (post_i < 0)) - { - ptDirAngles[i].pntIdx = -1; - ptDirAngles[i].forwardPntIdx = 0; - ptDirAngles[i].backwardPntIdx = 0; - ptDirAngles[i].forwardAngle = 0; - ptDirAngles[i].backwardAngle = 0; - ptDirAngles[i].corner = 0; - ptDirAngles[i].forwardDiffZ = 0; - ptDirAngles[i].backwardDiffZ = 0; - } - else + if ((pre_i >= 0) && (post_i >= 0)) { 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; ptDirAngles[i].pntIdx = i; + ptDirAngles[i].type = segIdx; + ptDirAngles[i].forwardPntIdx = post_i; + ptDirAngles[i].backwardPntIdx = pre_i; + ptDirAngles[i].forwardAngle = forwardAngle; + ptDirAngles[i].backwardAngle = backwardAngle; + ptDirAngles[i].corner = -(forwardAngle - backwardAngle); //ͼϵϵy෴С- + ptDirAngles[i].forwardDiffZ = vldPts[post_i].pt3D.z - vldPts[i].pt3D.z; + ptDirAngles[i].backwardDiffZ = vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z; + } + } + } +} + +//μ㷽 +void _computeDirAngle_perSeg_2( + std::vector< SVzNL3DPosition>& vldPts, + std::vector& segs, + const SSG_cornerParam cornerPara, + std::vector< SSG_pntDirAngle>& ptDirAngles +) +{ + ptDirAngles.resize(vldPts.size()); + for (int i = 0; i < (int)vldPts.size(); i++) + { + ptDirAngles[i].pntIdx = -1; + ptDirAngles[i].type = -1; + ptDirAngles[i].forwardPntIdx = 0; + ptDirAngles[i].backwardPntIdx = 0; + ptDirAngles[i].forwardAngle = 0; + ptDirAngles[i].backwardAngle = 0; + ptDirAngles[i].corner = 0; + ptDirAngles[i].forwardDiffZ = 0; + ptDirAngles[i].backwardDiffZ = 0; + } + + //ν + int segSize = (int)segs.size(); + for (int segIdx = 0; segIdx < segSize; segIdx++) + { + int vPtIdxStart = segs[segIdx].start; + int vPtIdxEnd = vPtIdxStart + segs[segIdx].len - 1; + for (int i = vPtIdxStart; i <= vPtIdxEnd; i++) + { + //ǰѰ + int pre_i = -1; + for (int j = i - 1; j >= vPtIdxStart; 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 <= vPtIdxEnd; 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)) + { + 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; + ptDirAngles[i].pntIdx = i; + ptDirAngles[i].type = segIdx; ptDirAngles[i].forwardPntIdx = post_i; ptDirAngles[i].backwardPntIdx = pre_i; ptDirAngles[i].forwardAngle = forwardAngle; @@ -4286,18 +4417,21 @@ bool _chkRodArcFeature( double dist = sqrt(pow(lineData[pkIdx].pt3D.y - lineData[i].pt3D.y, 2) + pow(lineData[pkIdx].pt3D.z - lineData[i].pt3D.z, 2)); if (dist > searchWin) break; + if (ptDirAngles[i].pntIdx >= 0) + { + double flatAngle = abs(ptDirAngles[i].forwardAngle + ptDirAngles[i].backwardAngle); + if (validCorner < 0) + { + realPkCorner = flatAngle; + realPkIdx = i; + validCorner = ptDirAngles[i].type; + } + else if (realPkCorner > flatAngle) + { - double flatAngle = abs(ptDirAngles[i].forwardAngle + ptDirAngles[i].backwardAngle); - if( validCorner < 0) - { - realPkCorner = flatAngle; - realPkIdx = i; - validCorner = 0; - } - else if (realPkCorner > flatAngle) - { - realPkCorner = flatAngle; - realPkIdx = i; + realPkCorner = flatAngle; + realPkIdx = i; + } } } // @@ -4313,19 +4447,24 @@ bool _chkRodArcFeature( if (dist > searchWin) break; - double flatAngle = abs(ptDirAngles[i].forwardAngle + ptDirAngles[i].backwardAngle); - if (validCorner < 0) + if (ptDirAngles[i].pntIdx >= 0) { - realPkCorner = flatAngle; - realPkIdx = i; - validCorner = 0; - } - else if (realPkCorner > flatAngle) - { - realPkCorner = flatAngle; - realPkIdx = i; + double flatAngle = abs(ptDirAngles[i].forwardAngle + ptDirAngles[i].backwardAngle); + if (validCorner < 0) + { + realPkCorner = flatAngle; + realPkIdx = i; + validCorner = ptDirAngles[i].type; + } + else if (realPkCorner > flatAngle) + { + realPkCorner = flatAngle; + realPkIdx = i; + } } } + if (validCorner < 0) + return false; if ((ptDirAngles[realPkIdx].forwardAngle > 0) && (ptDirAngles[realPkIdx].backwardAngle < 0) && (realPkCorner <10) &&(ptDirAngles[realPkIdx].corner < -arcPerPointCornerMinValue)) //ǰΪΪֵȣ<10), սΪֵ10 @@ -4334,8 +4473,11 @@ bool _chkRodArcFeature( int pre_idx = -1; for (int i = realPkIdx - 1; i >= 0; i--) { - if (ptDirAngles[i].type < 0) + if ((ptDirAngles[i].type < 0) || (ptDirAngles[i].pntIdx < 0)) continue; + else if (validCorner != ptDirAngles[i].type) + break; + if (ptDirAngles[i].corner < -arcPerPointCornerMinValue) pre_idx = i; else @@ -4345,8 +4487,11 @@ bool _chkRodArcFeature( int post_idx = -1; for (int i = realPkIdx + 1; i < (int)ptDirAngles.size(); i++) { - if (ptDirAngles[i].type < 0) + if ((ptDirAngles[i].type < 0) || (ptDirAngles[i].pntIdx < 0)) continue; + else if (validCorner != ptDirAngles[i].type) + break; + if (ptDirAngles[i].corner < -arcPerPointCornerMinValue) post_idx = i; else @@ -4428,6 +4573,79 @@ void wd_getRodArcFeature_peakCornerMethod( } return; } + +/// +/// ȡϵԲϰλ +/// seg˵㣺z +/// nPointIdx¶Feature +/// 㷨̣ +/// 1μǰǺͺ +/// 2ͬĹս +/// 2Zֵ +/// 2սǣ˳ʱΪʱΪ +/// 3սǵļֵ +/// 4жϹսǷΪ +/// +/// +void wd_getRodArcFeature_segmentPeakCornerMethod( + std::vector< SVzNL3DPosition>& lineData, + int lineIdx, + const double maxDistTh, + const int minSegSize, + const double peakChkWin, + const SSG_cornerParam cornerPara, + std::vector& line_rodArcs // +) +{ + double arcPerPointCornerMinValue = 5; //arcÿתСֵ + double arcTotalCornerMinValue = 30; //ArcתСֵ + + std::vector segs; + wd_lineDataSegment_dist_2( + lineData, + segs, + maxDistTh, + minSegSize + ); + //ǰǺͺ + std::vector< SSG_pntDirAngle> ptDirAngles; + _computeDirAngle_perSeg_2(lineData, segs, cornerPara, ptDirAngles); + + int dataSize = (int)lineData.size(); + //zֵ + std::vector< SSG_basicFeature1D> localZMax; + std::vector< SSG_basicFeature1D> localZMin; + sg_getLineLocalPeaks_2(lineData, lineIdx, peakChkWin, localZMax, localZMin); + + //Сֵж + int peakNum = (int)localZMin.size(); + for (int i = 0; i < peakNum; i++) + { + int pkIdx = localZMin[i].jumpPos2D.y; + SSG_intPair a_rodPos; + bool isArc = _chkRodArcFeature( + pkIdx, cornerPara.scale, + ptDirAngles, lineData, + arcPerPointCornerMinValue, arcTotalCornerMinValue, + a_rodPos); + if (true == isArc) + { + SWD_rodArcFeature a_rodArc; + memset(&a_rodArc, 0, sizeof(SWD_rodArcFeature)); + a_rodArc.lineIdx = lineIdx; + a_rodArc.startPtIdx = a_rodPos.data_0; + a_rodArc.endPtIdx = a_rodPos.data_1; + a_rodArc.peakPtIdx = a_rodPos.idx; + a_rodArc.startPt = lineData[a_rodPos.data_0].pt3D; + a_rodArc.endPt = lineData[a_rodPos.data_1].pt3D; + a_rodArc.peakPt = lineData[a_rodPos.idx].pt3D; + a_rodArc.featureValue = ptDirAngles[a_rodPos.idx].corner; + line_rodArcs.push_back(a_rodArc); + } + } + return; +} + //ȡcornerֵʵֺʹô˺дŻ void wd_searchCornerPeaks( std::vector< SSG_pntDirAngle>& corners, diff --git a/sourceCode/rodAndBarDetection.cpp b/sourceCode/rodAndBarDetection.cpp index 79cf1bc..2b64d36 100644 --- a/sourceCode/rodAndBarDetection.cpp +++ b/sourceCode/rodAndBarDetection.cpp @@ -15,7 +15,8 @@ //version 1.2.4 : ݶϵµȷ̬ //version 1.2.5 : ԣֽȡAPI //version 1.2.6 : λλ˼RANSACΪϣԾǷ -std::string m_strVersion = "1.2.6"; +//version 1.2.7 : (1)ʵʵݸ˶λ㷨2λλ˼ĻRANSAC +std::string m_strVersion = "1.2.7"; const char* wd_rodAndBarDetectionVersion(void) { return m_strVersion.c_str(); @@ -74,7 +75,7 @@ SVzNL3DPoint getArcPeak( SVzNL3DPoint getArcPeak_parabolaFitting( std::vector< std::vector>& scanLines, - SWD_segFeature& a_arcFeature, + SWD_rodArcFeature& a_arcFeature, SVzNL2DPoint& arcPos) { std::vector points; @@ -99,7 +100,7 @@ SVzNL3DPoint getArcPeak_parabolaFitting( mse, max_err); double yP = -b / (2 * a); //ѰyPĵΪPeak - SVzNL3DPoint arcPeak; + SVzNL3DPoint arcPeak = {0, 0, 0}; double minDist = -1; for (int i = a_arcFeature.startPtIdx + 1; i <= a_arcFeature.endPtIdx; i++) { @@ -131,6 +132,8 @@ void xoyROIProjection( std::vector< std::vector>& scanLines, const double* rtMatrix, SSG_ROIRectD& roi_xoy, + int objClusterId, + std::vector>& clusterIdMask, std::vector& projectPoints ) { @@ -144,6 +147,10 @@ void xoyROIProjection( SVzNL3DPoint a_pt = a_line[i].pt3D; if (a_pt.z < 1e-4) continue; + int cId = clusterIdMask[line][i]; + if (cId != objClusterId) + continue; + double x = a_pt.x * rtMatrix[0] + a_pt.y * rtMatrix[1] + a_pt.z * rtMatrix[2]; double y = a_pt.x * rtMatrix[3] + a_pt.y * rtMatrix[4] + a_pt.z * rtMatrix[5]; double z = a_pt.x * rtMatrix[6] + a_pt.y * rtMatrix[7] + a_pt.z * rtMatrix[8]; @@ -223,18 +230,83 @@ void rodAarcFeatueDetection( int linePtNum = (int)scanLines[0].size(); for (int line = 0; line < lineNum; line++) { + if (line == 827) + int kkk = 1; std::vector& lineData = scanLines[line]; // Filter outliers from line data sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam); // Extract rod arc features std::vector line_rodArcs; - wd_getRodArcFeature_peakCornerMethod(lineData, line, rodDiameter / 2, cornerPara, line_rodArcs); + wd_getRodArcFeature_segmentPeakCornerMethod( + lineData, + line, + rodDiameter/4, + rodDiameter / 8, + rodDiameter / 2, + cornerPara, + line_rodArcs // + ); + //wd_getRodArcFeature_peakCornerMethod(lineData, line, rodDiameter / 2, cornerPara, line_rodArcs); arcFeatures.push_back(line_rodArcs); } return; } +//еROI: vecotrʽ, zС0ĵ㣩 +SVzNL3DRangeD _getPointCloudROI(std::vector& scanData) +{ + SVzNL3DRangeD roi; + roi.xRange = { 0, -1 }; + roi.yRange = { 0, -1 }; + roi.zRange = { 0, -1 }; + + int nPositionCnt = (int)scanData.size(); + for (int i = 0; i < nPositionCnt; i++) + { + SVzNL3DPoint& pt3D = scanData[i]; + if (roi.xRange.max < roi.xRange.min) + { + roi.xRange.min = pt3D.x; + roi.xRange.max = pt3D.x; + } + else + { + if (roi.xRange.min > pt3D.x) + roi.xRange.min = pt3D.x; + if (roi.xRange.max < pt3D.x) + roi.xRange.max = pt3D.x; + } + //y + if (roi.yRange.max < roi.yRange.min) + { + roi.yRange.min = pt3D.y; + roi.yRange.max = pt3D.y; + } + else + { + if (roi.yRange.min > pt3D.y) + roi.yRange.min = pt3D.y; + if (roi.yRange.max < pt3D.y) + roi.yRange.max = pt3D.y; + } + //z + if (roi.zRange.max < roi.zRange.min) + { + roi.zRange.min = pt3D.z; + roi.zRange.max = pt3D.z; + } + else + { + if (roi.zRange.min > pt3D.z) + roi.zRange.min = pt3D.z; + if (roi.zRange.max < pt3D.z) + roi.zRange.max = pt3D.z; + } + } + + return roi; +} //ݸ˶˲ĵλ void sx_hexHeadScrewMeasure( @@ -273,6 +345,51 @@ void rodAarcFeatueDetection( return; } + // + std::vector clusterDataSrc; + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + if (scanLines[line][j].pt3D.z > 1e-4) + { + SVzNL3DPosition a_pt; + a_pt.nPointIdx = (line << 16) | (j & 0xffff); + a_pt.pt3D = scanLines[line][j].pt3D; + clusterDataSrc.push_back(a_pt); + } + } + } + int clusterCheckWin = 5; + int clusterDist = sqrt(pow(growParam.zDeviation_max, 2) + pow(growParam.maxSkipDistance, 2) + pow(growParam.yDeviation_max, 2)); + std::vector> objClusters; + wd_pointClustering_speedUp( + clusterDataSrc, + lineNum, linePtNum, clusterCheckWin, // + clusterDist, + 1, //0 - 2d distance; 1- 3d distance + objClusters //result + ); + //ClusterMask + int clusterSize = (int)objClusters.size(); + std::vector> clusterIdMask; + clusterIdMask.resize(lineNum); + for (int i = 0; i < lineNum; i++) + { + clusterIdMask[i].resize(linePtNum); + std::fill(clusterIdMask[i].begin(), clusterIdMask[i].end(), -1); + } + for (int i = 0; i < clusterSize; i++) + { + for (int j = 0; j < (int)objClusters[i].size(); j++) + { + int line = objClusters[i][j].nPointIdx >> 16; + int ptIdx = objClusters[i][j].nPointIdx & 0x0000FFFF; + clusterIdMask[line][ptIdx] = i; + } + } + + //ɨݸ std::vector< std::vector> data_lines; if (false == isHorizonScan) { @@ -368,17 +485,23 @@ void rodAarcFeatueDetection( } } - //ȷĿ + //ȷĿ꣬ԼĿе //ROIzСΪĿ std::vector objROIs; + std::vector objCluster; for (int i = 0; i < objNum; i++) { // Initialize min and max values // Calculate X, Y and Z ranges SVzNL3DRangeD a_roi3D; - a_roi3D.xRange.min = DBL_MAX; a_roi3D.xRange.max = DBL_MIN; - a_roi3D.yRange.min = DBL_MAX; a_roi3D.yRange.max = DBL_MIN; - a_roi3D.zRange.min = DBL_MAX; a_roi3D.zRange.max = DBL_MIN; + a_roi3D.xRange.min = DBL_MAX; a_roi3D.xRange.max = -DBL_MAX; + a_roi3D.yRange.min = DBL_MAX; a_roi3D.yRange.max = -DBL_MAX; + a_roi3D.zRange.min = DBL_MAX; a_roi3D.zRange.max = -DBL_MAX; + + std::vector clusterHist; + clusterHist.resize(clusterSize); + std::fill(clusterHist.begin(), clusterHist.end(), 0); + int nodeNum = (int)rodArcTrees[i].treeNodes.size(); for (int j = 0; j < nodeNum; j++) { @@ -388,126 +511,207 @@ void rodAarcFeatueDetection( { int lineIdx = rodArcTrees[i].treeNodes[j].lineIdx; int ptIdx = rodArcTrees[i].treeNodes[j].peakPtIdx; - SVzNL3DPoint peak = scanLines[lineIdx][ptIdx].pt3D; + peak = scanLines[lineIdx][ptIdx].pt3D; + int clusterId = clusterIdMask[lineIdx][ptIdx]; + if (clusterId >= 0) + clusterHist[clusterId]++; } else { int ptIdx = rodArcTrees[i].treeNodes[j].lineIdx; int lineIdx = rodArcTrees[i].treeNodes[j].peakPtIdx; - SVzNL3DPoint peak = scanLines[lineIdx][ptIdx].pt3D; + peak = scanLines[lineIdx][ptIdx].pt3D; + int clusterId = clusterIdMask[lineIdx][ptIdx]; + if (clusterId >= 0) + clusterHist[clusterId]++; + } + if (peak.z > 1e-4) + { + a_roi3D.xRange.min = std::min(a_roi3D.xRange.min, peak.x); + a_roi3D.xRange.max = std::max(a_roi3D.xRange.max, peak.x); + a_roi3D.yRange.min = std::min(a_roi3D.yRange.min, peak.y); + a_roi3D.yRange.max = std::max(a_roi3D.yRange.max, peak.y); + a_roi3D.zRange.min = std::min(a_roi3D.zRange.min, peak.z); + a_roi3D.zRange.max = std::max(a_roi3D.zRange.max, peak.z); } - a_roi3D.xRange.min = std::min(a_roi3D.xRange.min, peak.x); - a_roi3D.xRange.max = std::max(a_roi3D.xRange.max, peak.x); - a_roi3D.yRange.min = std::min(a_roi3D.yRange.min, peak.y); - a_roi3D.yRange.max = std::max(a_roi3D.yRange.max, peak.y); - a_roi3D.zRange.min = std::min(a_roi3D.zRange.min, peak.z); - a_roi3D.zRange.max = std::max(a_roi3D.zRange.max, peak.z); } + int objClusterId = 0; + for (int m = 1; m < clusterSize; m++) + { + if (clusterHist[objClusterId] < clusterHist[m]) + objClusterId = m; + } + objROIs.push_back(a_roi3D); + objCluster.push_back(objClusterId); } -#if 0 - //Ŀ괦 + + //ѡZСΪĿ + int objIdx = -1; for (int i = 0; i < objNum; i++) { - //ռֱ - std::vector fitPoints; - std::vector fit2DPos; - int nodeSize = (int)growTrees[i].treeNodes.size(); - for (int j = 0; j < nodeSize; j++) + if (objIdx < 0) + objIdx = i; + else if (objROIs[objIdx].zRange.min > objROIs[i].zRange.min) + objIdx = i; + } + + if(objIdx < 0) + { + *errCode = SX_ERR_ZERO_OBJECTS; + return; + } + + int resultObjClusterId = objCluster[objIdx]; + //ȡ + //ռֱ + std::vector fitPoints; + std::vector fit2DPos; + int nodeSize = (int)rodArcTrees[objIdx].treeNodes.size(); + for (int j = 0; j < nodeSize; j++) + { + SVzNL2DPoint arcPos; + SVzNL3DPoint a_pt = getArcPeak_parabolaFitting(data_lines, rodArcTrees[objIdx].treeNodes[j], arcPos); + if (false == isHorizonScan) { - SVzNL2DPoint arcPos; - SVzNL3DPoint a_pt = getArcPeak_parabolaFitting(data_lines, growTrees[i].treeNodes[j], arcPos); - //SVzNL3DPoint a_pt = getArcPeak(data_lines, growTrees[i].treeNodes[j], arcPos); fitPoints.push_back(a_pt); fit2DPos.push_back(arcPos); } - if (fitPoints.size() < 27) - continue; - //ȥͷβ5㣬ֹڶ˲͸ɨʱи - fitPoints.erase(fitPoints.begin(), fitPoints.begin() + 10); - fit2DPos.erase(fit2DPos.begin(), fit2DPos.begin() + 10); + else + { + arcPos = { arcPos.y, arcPos.x }; + a_pt = {a_pt.y, a_pt.x, a_pt.z}; + fitPoints.push_back(a_pt); + fit2DPos.push_back(arcPos); + } + } + if (fitPoints.size() < 20) + { + *errCode = SX_ERR_ZERO_OBJECTS; + return; + } + + //ȥͷ50mm β5㣬ֹڶ˲͸ɨʱи + SVzNL3DPoint pt_0 = fitPoints[0]; + SVzNL3DPoint pt_last = fitPoints.back(); + double headLenToRemove = 40; + if (pt_0.z < pt_last.z) + { + int eraseIdx = -1; + for (int i = 1; i < (int)fitPoints.size(); i++) + { + double dist = sqrt(pow(fitPoints[i].x - pt_0.x, 2) + pow(fitPoints[i].y - pt_0.y, 2) + pow(fitPoints[i].z - pt_0.z, 2)); + if (dist > headLenToRemove) + { + eraseIdx = i; + break; + } + } + fitPoints.erase(fitPoints.begin(), fitPoints.begin() + eraseIdx); + fit2DPos.erase(fit2DPos.begin(), fit2DPos.begin() + eraseIdx); fitPoints.erase(fitPoints.end() - 5, fitPoints.end()); fit2DPos.erase(fit2DPos.end() - 5, fit2DPos.end()); - //ñ־ - for (int j = 0; j < (int)fit2DPos.size(); j++) + } + else + { + int eraseIdx = -1; + for (int i = (int)fitPoints.size()-1; i >= 0; i--) { - int lineIdx, ptIdx; - if (false == isHorizonScan) + double dist = sqrt(pow(fitPoints[i].x - pt_last.x, 2) + pow(fitPoints[i].y - pt_last.y, 2) + pow(fitPoints[i].z - pt_last.z, 2)); + if (dist > headLenToRemove) { - lineIdx = fit2DPos[j].x; - ptIdx = fit2DPos[j].y; + eraseIdx = i; + break; } - else - { - lineIdx = fit2DPos[j].y; - ptIdx = fit2DPos[j].x; - } - scanLines[lineIdx][ptIdx].nPointIdx = 2; } + int eraseNum = (int)fitPoints.size() - eraseIdx; + fitPoints.erase(fitPoints.end() - eraseNum, fitPoints.end()); + fit2DPos.erase(fit2DPos.end() - eraseNum, fit2DPos.end()); + fitPoints.erase(fitPoints.begin(), fitPoints.begin() + 5); + fit2DPos.erase(fit2DPos.begin(), fit2DPos.begin() + 5); + } + //ñ־ + for (int j = 0; j < (int)fit2DPos.size(); j++) + { + int lineIdx, ptIdx; + lineIdx = fit2DPos[j].x; + ptIdx = fit2DPos[j].y; + scanLines[lineIdx][ptIdx].nPointIdx = 4; + } - // - SVzNL3DPoint P0_center, P1_dir; - bool result = fitLine3DLeastSquares(fitPoints, P0_center, P1_dir); - if (false == result) - continue; - //ͶӰ - //ת - SVzNL3DPoint vector1 = P1_dir; - SVzNL3DPoint vector2 = { 0, 0, -1.0 }; - SSG_planeCalibPara rotatePara = wd_computeRTMatrix( vector1, vector2); - // - SVzNL3DPoint P0_rotate = _ptRotate(P0_center, rotatePara.planeCalib); - SSG_ROIRectD roi_xoy; - roi_xoy.left = P0_rotate.x - rodDiameter * 2; //2DΧ - roi_xoy.right = P0_rotate.x + rodDiameter * 2; //2DΧ - roi_xoy.top = P0_rotate.y - rodDiameter * 2; //2DΧ - roi_xoy.bottom = P0_rotate.y + rodDiameter * 2; //2DΧ + // + SVzNL3DPoint P0_center, P1_dir; + bool result = fitLine3DLeastSquares(fitPoints, P0_center, P1_dir); + if (false == result) + { + *errCode = SX_ERR_ZERO_OBJECTS; + return; + } + //ͶӰ + //ת + SVzNL3DPoint vector1 = P1_dir; + SVzNL3DPoint vector2 = { 0, 0, 1.0 }; + SSG_planeCalibPara rotatePara = wd_computeRTMatrix( vector1, vector2); + // + SVzNL3DPoint test_center = { P0_center.x, P0_center.y + rodDiameter / 2, P0_center.z - 100 }; + SVzNL3DPoint P0_rotate = _ptRotate(P0_center, rotatePara.planeCalib); + SVzNL3DPoint test_center_rotate = _ptRotate(test_center, rotatePara.planeCalib); + SSG_ROIRectD roi_xoy; + roi_xoy.left = P0_rotate.x - rodDiameter* 2.0; //2DΧ + roi_xoy.right = P0_rotate.x + rodDiameter * 2.0; //2DΧ + roi_xoy.top = P0_rotate.y - rodDiameter * 2.0; //2DΧ + roi_xoy.bottom = P0_rotate.y + rodDiameter * 2.0; //2DΧ #if 1 - std::vector< SVzNL3DPoint> verifyData; - for (int m = 0; m < (int)fitPoints.size(); m++) - { - SVzNL3DPoint rPt = _ptRotate(fitPoints[m], rotatePara.planeCalib); - verifyData.push_back(rPt); - } -#endif - std::vector< SVzNL3DPoint> roiProjectionData; - xoyROIProjection(data_lines, rotatePara.planeCalib, roi_xoy, roiProjectionData); - //ȡ - SVzNLRangeD zRange = getZRange(roiProjectionData); - SVzNLRangeD cutZRange; - cutZRange.min = zRange.min; - cutZRange.max = zRange.min + 5.0; //5mmĶ - std::vector surfacePoints; - zCutPointClouds(roiProjectionData, cutZRange, surfacePoints); - //ĵ - SVzNL3DPoint projectionCenter;// = getXoYCentroid(surfacePoints); - SVzNL3DRangeD roi3D = wd_getPointCloudROI(surfacePoints); - projectionCenter.x = (roi3D.xRange.min + roi3D.xRange.max) / 2; - projectionCenter.y = (roi3D.yRange.min + roi3D.yRange.max) / 2; - projectionCenter.z = zRange.min; - //תԭϵ - SVzNL3DPoint surfaceCenter = _ptRotate(projectionCenter, rotatePara.invRMatrix); - //RodϢ - SSX_rodPoseInfo a_rod; - a_rod.center = surfaceCenter; - a_rod.axialDir = P1_dir; - screwInfo.push_back(a_rod); - } - if (true == isHorizonScan) + std::vector< SVzNL3DPoint> verifyData; + for (int m = 0; m < (int)fitPoints.size(); m++) { - int objNum = (int)screwInfo.size(); - for (int i = 0; i < objNum; i++) - { - double tmp = screwInfo[i].center.x; - screwInfo[i].center.x = screwInfo[i].center.y; - screwInfo[i].center.y = tmp; - tmp = screwInfo[i].axialDir.x; - screwInfo[i].axialDir.x = screwInfo[i].axialDir.y; - screwInfo[i].axialDir.y = tmp; - //screwInfo[i].rotateAngle += 90; - } + SVzNL3DPoint rPt = _ptRotate(fitPoints[m], rotatePara.planeCalib); + verifyData.push_back(rPt); } #endif + std::vector< SVzNL3DPoint> roiProjectionData; + xoyROIProjection(scanLines, rotatePara.planeCalib, roi_xoy, resultObjClusterId, clusterIdMask, roiProjectionData); + + //ȡ + SVzNLRangeD zRange = getZRange(roiProjectionData); + SVzNLRangeD cutZRange; + cutZRange.min = zRange.min; + cutZRange.max = zRange.min + 5.0; //5mmĶ + std::vector surfacePoints; + zCutPointClouds(roiProjectionData, cutZRange, surfacePoints); + //ĵ + SVzNL3DPoint projectionCenter;// = getXoYCentroid(surfacePoints); + SVzNL3DRangeD roi3D = _getPointCloudROI(surfacePoints); + projectionCenter.x = (roi3D.xRange.min + roi3D.xRange.max) / 2; + projectionCenter.y = (roi3D.yRange.min + roi3D.yRange.max) / 2; + projectionCenter.z = zRange.min; + //projectionCenterΪ5mmzĵĵΪĵ + double searchR = 5.0; + int centerIdx = -1; + for (int i = 0; i < (int)surfacePoints.size(); i++) + { + double dist = sqrt(pow(surfacePoints[i].x - projectionCenter.x, 2) + pow(surfacePoints[i].y - projectionCenter.y, 2)); + if (dist < searchR) + { + if (centerIdx < 0) + centerIdx = i; + else if(surfacePoints[centerIdx].z < surfacePoints[i].z) + centerIdx = i; + } + } + if(centerIdx < 0) + { + *errCode = SX_ERR_ZERO_OBJECTS; + return; + } + + //תԭϵ + SVzNL3DPoint surfaceCenter = _ptRotate(surfacePoints[centerIdx], rotatePara.invRMatrix); + //RodϢ + SSX_rodPoseInfo a_rod; + a_rod.center = surfaceCenter; + a_rod.axialDir = P1_dir; + screwInfo.push_back(a_rod); return; } @@ -1051,7 +1255,7 @@ SSX_platePoseInfo sx_getLocationPlatePose( } //: z = Ax + By + C //res: [0]=A, [1]= B, [2]=-1.0, [3]=C, -#if 0 +#if 1 std::vector out_inliers; Plane res = ransacFitPlane( Points3ds, out_inliers ); if (res.C < 0)