diff --git a/rodAndBarDetection_test/rodAndBarDetection_test.cpp b/rodAndBarDetection_test/rodAndBarDetection_test.cpp index 1978215..1090f3d 100644 --- a/rodAndBarDetection_test/rodAndBarDetection_test.cpp +++ b/rodAndBarDetection_test/rodAndBarDetection_test.cpp @@ -973,33 +973,36 @@ void _outputRGBDScan_RGBD_weldSeam( sw.close(); } -#define SCREW_TEST_GROUP 10 +#define SCREW_TEST_GROUP 12 void screwTest(void) { const char* dataPath[SCREW_TEST_GROUP] = { - "F:/ShangGu/项目/冠钦项目/螺杆测量/数据/模拟数据/", //0 - "F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/螺杆点云2/上方两根/", //1 - "F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/螺杆点云3/", //2 - "F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/螺杆点云4/位置1/", //3 - "F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/螺杆点云4/位置2/", //4 - "F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/螺杆点云4/位置2未动螺杆拧进去100mm左右/", //5 - "F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/螺杆点云4/位置2向前100mm螺杆拧进去100mm左右/", //6 - "F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/螺杆点云20260621/另一根螺杆/", //7 - "F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/螺杆点云20260621/拍照点1/", //8 - "F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/螺杆点云20260621/拍照点1xyz移动10mm/", //9 + "F:/ShangGu/项目/冠钦项目/配天螺杆测量/数据/模拟数据/", //0 + "F:/ShangGu/项目/冠钦项目/配天螺杆测量/配天现场点云/螺杆点云2/上方两根/", //1 + "F:/ShangGu/项目/冠钦项目/配天螺杆测量/配天现场点云/螺杆点云3/", //2 + "F:/ShangGu/项目/冠钦项目/配天螺杆测量/配天现场点云/螺杆点云4/位置1/", //3 + "F:/ShangGu/项目/冠钦项目/配天螺杆测量/配天现场点云/螺杆点云4/位置2/", //4 + "F:/ShangGu/项目/冠钦项目/配天螺杆测量/配天现场点云/螺杆点云4/位置2未动螺杆拧进去100mm左右/", //5 + "F:/ShangGu/项目/冠钦项目/配天螺杆测量/配天现场点云/螺杆点云4/位置2向前100mm螺杆拧进去100mm左右/", //6 + "F:/ShangGu/项目/冠钦项目/配天螺杆测量/配天现场点云/螺杆点云20260621/另一根螺杆/", //7 + "F:/ShangGu/项目/冠钦项目/配天螺杆测量/配天现场点云/螺杆点云20260621/拍照点1/", //8 + "F:/ShangGu/项目/冠钦项目/配天螺杆测量/配天现场点云/螺杆点云20260621/拍照点1xyz移动10mm/", //9 + "F:/ShangGu/项目/冠钦项目/配天螺杆测量/配天现场点云/异常点云0615/", //10 + "F:/ShangGu/项目/冠钦项目/配天螺杆测量/配天现场点云/异常点云0618/", //11 }; SVzNLRange fileIdx[SCREW_TEST_GROUP] = { {1,4},{1,30},{1,11}, {1,20}, {1,20}, {1,5}, {1,21}, - {1,10}, {1,10}, {1,10} + {1,10}, {1,10}, {1,10}, + {1,9},{1,30} }; const char* ver = wd_rodAndBarDetectionVersion(); printf("ver:%s\n", ver); - for (int grp = 3; grp <= 6; grp++) + for (int grp = 11; grp <= 11; grp++) { for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { @@ -1010,9 +1013,12 @@ void screwTest(void) sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx); else if( (grp>= 7)&&(grp <= 9)) sprintf_s(_scan_file, "%sLaserline_%d.txt", dataPath[grp], fidx); + else if(10 == grp) + sprintf_s(_scan_file, "%s%d_LaserData.txt", dataPath[grp], fidx); else sprintf_s(_scan_file, "%s%d_LaserData_Jl26C299.txt", dataPath[grp], fidx); + std::vector> scanLines; wdReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); @@ -1026,13 +1032,13 @@ void screwTest(void) if (grp == 0) rodDiameter = 10.0; else - rodDiameter = 28.0; //现场螺杆直径28mm + rodDiameter = 32.0; //现场螺杆直径28mm //double rodDiameter = 10.0; SSG_cornerParam cornerParam; cornerParam.cornerTh = 30; //45度角 - cornerParam.scale = rodDiameter/4; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + cornerParam.scale = rodDiameter / 4; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; cornerParam.minEndingGap = 20; // algoParam.bagParam.bagW / 4; cornerParam.minEndingGap_z = 5.0; cornerParam.jumpCornerTh_1 = 15; //水平角度,小于此角度视为水平 @@ -1089,7 +1095,7 @@ void screwTest(void) void locatingPlateTest(void) { const char* dataPath[LOCATING_PALTE_TEST_GROUP] = { - "F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/定位盘点云/", //0 + "F:/ShangGu/项目/冠钦项目/配天螺杆测量/配天现场点云/定位盘点云/", //0 }; SVzNLRange fileIdx[LOCATING_PALTE_TEST_GROUP] = { @@ -1147,8 +1153,8 @@ void locatingPlateTest(void) void newLocatingPlateTest(void) { const char* dataPath[NEW_LOCATING_PALTE_TEST_GROUP] = { - "F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/定位盘点云20260521/", //0 - "F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/定位盘点云20260603/", //1 + "F:/ShangGu/项目/冠钦项目/配天螺杆测量/配天现场点云/定位盘点云20260521/", //0 + "F:/ShangGu/项目/冠钦项目/配天螺杆测量/配天现场点云/定位盘点云20260603/", //1 }; SVzNLRange fileIdx[NEW_LOCATING_PALTE_TEST_GROUP] = { @@ -1309,7 +1315,7 @@ void _convertToGridData_XYZRGB(std::vector>& sca return; } -#define ROD_POSITION_TEST_GROUP 5 +#define ROD_POSITION_TEST_GROUP 6 #define TEST_CONVERT_TO_GRID 0 #define TEST_COMPUTE_CALIB_PARA 0 #define TEST_COMPUTE_ROD_POSITION 1 @@ -1348,7 +1354,7 @@ void rodPositionTest(void) printf("all converted done!\n", _scan_src_file); #endif #if TEST_COMPUTE_CALIB_PARA - const char* calibDataPath = "F:/ShangGu/项目/冠钦项目/矩森棒材抓取/空料框及内剩一根棒材/调平/位置1/"; + const char* calibDataPath = "F:/ShangGu/项目/冠钦项目/矩森棒材抓取/异常点云/"; char _calib_datafile[256]; sprintf_s(_calib_datafile, "%sGroundData_1.txt", calibDataPath); int lineNum = 0; @@ -1413,17 +1419,19 @@ void rodPositionTest(void) "F:/ShangGu/项目/冠钦项目/胶布圆棒抓取/模拟测试数据/", //3 "F:/ShangGu/项目/冠钦项目/胶布圆棒抓取/模拟测试数据2/", //4 + + "F:/ShangGu/项目/冠钦项目/矩森棒材抓取/异常点云/", //5 }; SVzNLRange fileIdx[ROD_POSITION_TEST_GROUP] = { {1,8}, {1,3}, {1,31}, - {1,5},{1,11} + {1,5},{1,11},{1,26} }; const char* ver = wd_rodAndBarDetectionVersion(); printf("ver:%s\n", ver); - for (int grp = 0; grp <= 2; grp++) + for (int grp = 5; grp <= 5; grp++) { SSG_planeCalibPara poseCalibPara; //初始化成单位阵 @@ -1451,10 +1459,12 @@ void rodPositionTest(void) for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { - //fidx =5; + //fidx =16; char _scan_file[256]; if( (3 == grp) ||(4 == grp)) sprintf_s(_scan_file, "%s%d_LaserData_Hi229156.txt", dataPath[grp], fidx); + else if(5 == grp) + sprintf_s(_scan_file, "%s%d_LaserData_Ik256066.txt", dataPath[grp], fidx); else sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx); @@ -1472,6 +1482,11 @@ void rodPositionTest(void) { rodParam = rodParas[fidx]; } + else if (grp == 5) + { + rodParam.diameter = 96.0; //圆棒直径 + rodParam.len = 440; + } else { rodParam.diameter = 68.0; //圆棒直径 @@ -1706,6 +1721,13 @@ void rodWeldSeamPosition_test(void) weldSeamRange.min = 0; weldSeamRange.max = 80; } + + EWD_weldingCategory weldCategory; + if ((grp == 0) || (grp == 1)) + weldCategory = KeWD_WELD_CATEGPRY_I; + else + weldCategory = KeWD_WELD_CATEGPRY_II; + int errCode = 0; std::vector weldSeamInfo; sx_rebarWeldSeamPositioning( @@ -1716,6 +1738,7 @@ void rodWeldSeamPosition_test(void) growParam, rodParam, weldSeamRange, + weldCategory, weldSeamInfo, &errCode); long t2 = (long)GetTickCount64(); @@ -1738,16 +1761,16 @@ typedef enum keSG_测试_配天螺杆定位 = 0, keSG_测试_配天定位盘定位, keSG_测试_配天新定位盘定位, - keSG_测试_棒材抓取, + keSG_测试_矩森棒材抓取, keSG_测试_筑裕钢筋焊缝定位, } ESG_testMode; 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) @@ -1756,7 +1779,7 @@ int main() locatingPlateTest(); else if (keSG_测试_配天新定位盘定位 == testMode) newLocatingPlateTest(); - else if(keSG_测试_棒材抓取 == testMode) + else if(keSG_测试_矩森棒材抓取 == testMode) rodPositionTest(); else if(keSG_测试_筑裕钢筋焊缝定位 == testMode) rodWeldSeamPosition_test(); diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index 70e3024..775d1ec 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -250,7 +250,7 @@ SG_APISHARED_EXPORT 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 // @@ -311,6 +311,18 @@ SG_APISHARED_EXPORT void wd_getXYVertialFeature_dirAngleMethod( const SSG_cornerParam cornerPara, std::vector& xyVerticalFlags // ); +/// +/// ȡϵXYƽ洹ֱֱΣ +/// +SG_APISHARED_EXPORT void wd_getXYVertialFeature_perSeg_dirAngleMethod( + std::vector< SVzNL3DPosition>& lineData, + int lineIdx, + const double maxDistTh, + const double minSegSize, + const SSG_cornerParam cornerPara, + std::vector& xyVerticalFlags +); + /// /// ȡϵXYƽˮƽˮƽΣ /// @@ -433,6 +445,18 @@ SG_APISHARED_EXPORT void sg_getLineLocalPeaks_2( std::vector< SSG_basicFeature1D>& localZMax, std::vector< SSG_basicFeature1D>& localZMin); +/// +/// ȡϵļֵ㣨ֵͼСֵ㣩 +/// +/// +SG_APISHARED_EXPORT void sg_getLineLocalPeaks_perSegment( + std::vector& lineData, + std::vector& segs, + int lineIdx, + const double scaleWin, + std::vector< SSG_basicFeature1D>& localZMax, + std::vector< SSG_basicFeature1D>& localZMin); + SG_APISHARED_EXPORT void sg_getFlatLineLocalPeaks_vector( std::vector& lineData, int lineIdx, diff --git a/sourceCode/SG_featureGrow.cpp b/sourceCode/SG_featureGrow.cpp index 8968de1..2c05506 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 == 770) + if (i == 630) 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 fccc73e..7ce4a71 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -839,6 +839,210 @@ void sg_getLineLocalPeaks_2( return; } +/// +/// ȡϵļֵ㣨ֵͼСֵ㣩 +/// +/// +void sg_getLineLocalPeaks_perSegment( + std::vector& lineData, + std::vector& segs, + int lineIdx, + const double scaleWin, + std::vector< SSG_basicFeature1D>& localZMax, + std::vector< SSG_basicFeature1D>& localZMin) +{ + int dataSize = (int)lineData.size(); + if (dataSize < 2) + return; + + //Mask + std::vector segMask; + segMask.resize(lineData.size()); + std::fill(segMask.begin(), segMask.end(), -1); + for (int i = 0; i < (int)segs.size(); i++) + { + int sIdx = segs[i].start; + int eIdx = segs[i].start + segs[i].len - 1; + for (int j = sIdx; j <= eIdx; j++) + segMask[j] = i; + } + + int _state = 0; + int pre_i = -1; + int sEdgePtIdx = -1; + int eEdgePtIdx = -1; + SVzNL3DPosition* pre_data = NULL; + std::vector< SVzNL3DPosition> zTop; + std::vector< SVzNL3DPosition> zBtm; + for (int i = 0; i < dataSize; i++) + { + if (i == 141) + int kkk = 1; + lineData[i].nPointIdx = i; + SVzNL3DPosition* curr_data = &lineData[i]; + if (curr_data->pt3D.z < 1e-4) + { + if (i == dataSize - 1) //һ + { + if (1 == _state) // + { + zTop.push_back(lineData[eEdgePtIdx]); + } + else if (2 == _state) //½ + { + zBtm.push_back(lineData[eEdgePtIdx]); + } + } + continue; + } + + if (NULL == pre_data) + { + sEdgePtIdx = i; + eEdgePtIdx = i; + pre_data = curr_data; + pre_i = i; + continue; + } + + eEdgePtIdx = i; + double z_diff = curr_data->pt3D.z - pre_data->pt3D.z; + switch (_state) + { + case 0: //̬ + if (z_diff < 0) //½ + { + _state = 2; + } + else if (z_diff > 0) // + { + _state = 1; + } + break; + case 1: // + if (z_diff < 0) //½ + { + zTop.push_back(*pre_data); + _state = 2; + } + break; + case 2: //½ + if (z_diff > 0) // + { + zBtm.push_back(*pre_data); + _state = 1; + } + break; + default: + _state = 0; + break; + } + pre_data = curr_data; + pre_i = i; + } + //ע⣺һΪλ + + //Сֵ㣨嶥 + //ֵȽϣڳ߶ȴѰҾֲֵ + double square_distTh = scaleWin * scaleWin; + for (int i = 0, i_max = (int)zTop.size(); i < i_max; i++) + { + bool isPeak = true; + int curr_ptIdx = zTop[i].nPointIdx; + int curr_segIdx = segMask[curr_ptIdx]; + //ǰ + for (int j = i - 1; j >= 0; j--) + { + int chk_ptIdx = zTop[j].nPointIdx; + int chk_segIdx = segMask[chk_ptIdx]; + double dist = pow(zTop[i].pt3D.y - zTop[j].pt3D.y, 2) + pow(zTop[i].pt3D.z - zTop[j].pt3D.z, 2); + if ((dist > square_distTh)||(chk_segIdx != curr_segIdx)) //߶ȴ + break; + + if (zTop[i].pt3D.z < zTop[j].pt3D.z) + { + isPeak = false; + break; + } + } + // + if (true == isPeak) + { + for (int j = i + 1; j < i_max; j++) + { + int chk_ptIdx = zTop[j].nPointIdx; + int chk_segIdx = segMask[chk_ptIdx]; + double dist = pow(zTop[i].pt3D.y - zTop[j].pt3D.y, 2) + pow(zTop[i].pt3D.z - zTop[j].pt3D.z, 2); + if ((dist > square_distTh) || (chk_segIdx != curr_segIdx)) //߶ȴ + break; + + if (zTop[i].pt3D.z < zTop[j].pt3D.z) + { + isPeak = false; + break; + } + } + } + if (true == isPeak) + { + SSG_basicFeature1D a_feature; + a_feature.featureType = LINE_FEATURE_PEAK_TOP; + a_feature.jumpPos = zTop[i].pt3D; + a_feature.jumpPos2D = { lineIdx, zTop[i].nPointIdx }; + localZMax.push_back(a_feature); + } + } + //ֵ㣨ȵף + for (int i = 0, i_max = (int)zBtm.size(); i < i_max; i++) + { + bool isPeak = true; + int curr_ptIdx = zBtm[i].nPointIdx; + int curr_segIdx = segMask[curr_ptIdx]; + //ǰ + for (int j = i - 1; j >= 0; j--) + { + int chk_ptIdx = zBtm[j].nPointIdx; + int chk_segIdx = segMask[chk_ptIdx]; + double dist = pow(zBtm[i].pt3D.y - zBtm[j].pt3D.y, 2) + pow(zBtm[i].pt3D.z - zBtm[j].pt3D.z, 2); + if ((dist > square_distTh) || (chk_segIdx != curr_segIdx)) //߶ȴ + break; + + if (zBtm[i].pt3D.z > zBtm[j].pt3D.z) + { + isPeak = false; + break; + } + } + // + if (true == isPeak) + { + for (int j = i + 1; j < i_max; j++) + { + int chk_ptIdx = zBtm[j].nPointIdx; + int chk_segIdx = segMask[chk_ptIdx]; + double dist = pow(zBtm[i].pt3D.y - zBtm[j].pt3D.y, 2); pow(zBtm[i].pt3D.z - zBtm[j].pt3D.z, 2); + if ((dist > square_distTh) || (chk_segIdx != curr_segIdx)) //߶ȴ + break; + + if (zBtm[i].pt3D.z > zBtm[j].pt3D.z) + { + isPeak = false; + break; + } + } + } + if (true == isPeak) + { + SSG_basicFeature1D a_feature; + a_feature.featureType = LINE_FEATURE_PEAK_BOTTOM; + a_feature.jumpPos = zBtm[i].pt3D; + a_feature.jumpPos2D = { lineIdx, zBtm[i].nPointIdx }; + localZMin.push_back(a_feature); + } + } + return; +} + /// /// ȡɨеб£¼беڵ֮䡣 /// ͬʱ¼EndingûмӣֻҴʱEndingEnding֮СΪӿȵ1/4 @@ -4936,7 +5140,8 @@ void wd_getRodArcFeature_segmentPeakCornerMethod( //zֵ std::vector< SSG_basicFeature1D> localZMax; std::vector< SSG_basicFeature1D> localZMin; - sg_getLineLocalPeaks_2(lineData, lineIdx, peakChkWin, localZMax, localZMin); + //ȡzֵ + sg_getLineLocalPeaks_perSegment(lineData, segs, lineIdx, peakChkWin, localZMax, localZMin); //Сֵж int peakNum = (int)localZMin.size(); @@ -5736,6 +5941,89 @@ void wd_getXYVertialFeature_dirAngleMethod( return; } +/// +/// ȡϵXYƽ洹ֱֱΣ +/// +void wd_getXYVertialFeature_perSeg_dirAngleMethod( + std::vector< SVzNL3DPosition>& lineData, + int lineIdx, + const double maxDistTh, + const double minSegSize, + const SSG_cornerParam cornerPara, + std::vector& xyVerticalFlags +) +{ + if (lineIdx == 562) + int kkk = 1; + double minVerticalAngle = cornerPara.cornerTh; //arcÿתֵ + + xyVerticalFlags.resize(lineData.size()); + std::fill(xyVerticalFlags.begin(), xyVerticalFlags.end(), 0); + + //zԷֶ + std::vector segs; + wd_lineDataSegment_dist_2( + lineData, + segs, + maxDistTh, + minSegSize + ); + //ǰǺͺ + std::vector< SSG_pntDirAngle> ptDirAngles; + _computeDirAngle_perSeg_2(lineData, segs, cornerPara, ptDirAngles); + + for (int i = 0; i < (int)ptDirAngles.size(); i++) + { + if ((ptDirAngles[i].type < 0) || (ptDirAngles[i].pntIdx < 0)) + continue; + + if (((ptDirAngles[i].backwardAngle < -minVerticalAngle) && (ptDirAngles[i].forwardAngle < -minVerticalAngle)) || + ((ptDirAngles[i].backwardAngle > minVerticalAngle) && (ptDirAngles[i].forwardAngle > minVerticalAngle))) + xyVerticalFlags[i] = 1; + } + //Ե + for (int i = 1; i < (int)ptDirAngles.size() - 1; i++) + { + if ((xyVerticalFlags[i - 1] == 0) && (xyVerticalFlags[i] == 1)) + { + int sIdx = ptDirAngles[i].backwardPntIdx; + bool isEdge = true; + for (int j = sIdx; j < i; j++) + { + if (ptDirAngles[j].pntIdx >= 0) + { + isEdge = false; + break; + } + } + if (true == isEdge) + { + for (int j = sIdx; j < i; j++) + xyVerticalFlags[j] = 1; + } + } + if ((xyVerticalFlags[i + 1] == 0) && (xyVerticalFlags[i] == 1)) + { + int eIdx = ptDirAngles[i].forwardPntIdx; + bool isEdge = true; + for (int j = i + 1; j <= eIdx; j++) + { + if (ptDirAngles[j].pntIdx >= 0) + { + isEdge = false; + break; + } + } + if (true == isEdge) + { + for (int j = i + 1; j <= eIdx; j++) + xyVerticalFlags[j] = 1; + } + } + } + return; +} + /// /// ȡϵXYƽˮƽˮƽΣ /// seg˵㣺z diff --git a/sourceCode/rodAndBarDetection.cpp b/sourceCode/rodAndBarDetection.cpp index 4ac90fe..52f9b9c 100644 --- a/sourceCode/rodAndBarDetection.cpp +++ b/sourceCode/rodAndBarDetection.cpp @@ -29,7 +29,8 @@ //version 1.3.6 : µĶλIJܣŻǰĴֱȥ㷨֤ȷ //version 1.3.7 : µĶλIJܣһŻڲŻ˴ֱȥЧ //version 1.3.8 : µݸ˶λ㷨ʹPCAȷݸ -std::string m_strVersion = "1.3.8"; +//version 1.4.0 : ɭץȡ㷨Ľ1ıʶһ⣨2ǿ˶̶ȣǿ³ +std::string m_strVersion = "RodAndBarDetection_1.4.0"; const char* wd_rodAndBarDetectionVersion(void) { return m_strVersion.c_str(); @@ -241,12 +242,67 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9]) } -void rodAarcFeatueDetection( +void rodArcFeatueDetection( + std::vector< std::vector>& scanLines, + const SSG_cornerParam cornerPara, + const SSG_outlierFilterParam filterParam, + const double segment_maxDistTh, + const double segment_minSegSze, + const double rodDiameter, + std::vector>& arcFeatures) +{ + int lineNum = (int)scanLines.size(); + int linePtNum = (int)scanLines[0].size(); + for (int line = 0; line < lineNum; line++) + { + if (line == 780) + 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; +#if 0 + 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, + segment_maxDistTh, + segment_minSegSze, + rodDiameter / 2, + cornerPara, + line_rodArcs // + ); +#endif + //wd_getRodArcFeature_peakCornerMethod(lineData, line, rodDiameter / 2, cornerPara, line_rodArcs); + arcFeatures.push_back(line_rodArcs); + } + return; +} + +#if 0 +//ȡɨArcVнӽ90ڰ͹ڸֽ +void lineArcAndVFeatueDetection( std::vector< std::vector>& scanLines, const SSG_cornerParam cornerPara, const SSG_outlierFilterParam filterParam, const double rodDiameter, - std::vector>& arcFeatures) + std::vector>& arcFeatures, + std::vector>& convexVFeature, + std::vector>& concaveVFeature +) { int lineNum = (int)scanLines.size(); int linePtNum = (int)scanLines[0].size(); @@ -276,7 +332,7 @@ void rodAarcFeatueDetection( wd_getRodArcFeature_segmentPeakCornerMethod( lineData, line, - rodDiameter/4, + rodDiameter / 4, rodDiameter / 8, rodDiameter / 2, cornerPara, @@ -288,6 +344,7 @@ void rodAarcFeatueDetection( } return; } +#endif //еROI: vecotrʽ, zС0ĵ㣩 SVzNL3DRangeD _getPointCloudROI(std::vector& scanData) @@ -467,10 +524,14 @@ SVzNL3DRangeD _getPointCloudROI(std::vector& scanData) } std::vector> arcFeatures; - rodAarcFeatueDetection( + const double segment_maxDistTh = rodDiameter / 4; + const double segment_minSegSize = rodDiameter / 8; + rodArcFeatueDetection( data_lines, cornerPara, filterParam, + segment_maxDistTh, + segment_minSegSize, rodDiameter, arcFeatures); @@ -3193,11 +3254,12 @@ SVzNL3DPoint _exchangeXY(SVzNL3DPoint pt) void _computeRodInfo( SWD_rodArcFeatureTree& a_objTree, + int startIdx, + int endIdx, bool treeIsHorizon, std::vector< SVzNL3DPoint>& fittingPoints, SSX_rodPositionInfo& a_objRod) { - int nodeSize = a_objTree.treeNodes.size(); // double _a, _b, _c; lineFitting_abc(fittingPoints, &_a, &_b, &_c); @@ -3231,7 +3293,7 @@ void _computeRodInfo( _c = _c / tmpData; SVzNL3DPoint realStart, realEnd; bool foundStart = false; - for (int j = 0; j < nodeSize; j++) + for (int j = startIdx; j < endIdx; j++) { SVzNL3DPoint a_pt = a_objTree.treeNodes[j].peakPt; if (true == treeIsHorizon) @@ -3246,13 +3308,13 @@ void _computeRodInfo( } if (false == foundStart) { - realStart = a_objTree.treeNodes[0].peakPt; + realStart = a_objTree.treeNodes[startIdx].peakPt; if (true == treeIsHorizon) realStart = _exchangeXY(realStart); } bool foundEnd = false; - for (int j = nodeSize - 1; j >= 0; j--) + for (int j = endIdx; j >= startIdx; j--) { SVzNL3DPoint a_pt = a_objTree.treeNodes[j].peakPt; if (true == treeIsHorizon) @@ -3267,7 +3329,7 @@ void _computeRodInfo( } if (false == foundEnd) { - realEnd = a_objTree.treeNodes[nodeSize - 1].peakPt; + realEnd = a_objTree.treeNodes[endIdx].peakPt; if (true == treeIsHorizon) realEnd = _exchangeXY(realEnd); } @@ -3277,7 +3339,7 @@ void _computeRodInfo( double dist_s = sqrt(pow(foot_s.x - foot1.x, 2) + pow(foot_s.y - foot1.y, 2)); double dist_e = sqrt(pow(foot_e.x - foot1.x, 2) + pow(foot_e.y - foot1.y, 2)); //ĿϢ - ; + a_objRod.startPt = { foot_s.x, foot_s.y, -(dist_s / len) * deltaZ + fittingPoints[0].z }; a_objRod.endPt = { foot_e.x, foot_e.y, (dist_e / len) * deltaZ + fittingPoints[0].z }; a_objRod.center = { (a_objRod.startPt.x + a_objRod.endPt.x) / 2, @@ -3377,16 +3439,19 @@ void sx_rodPositioning( } } + //ڲ + const double segment_maxDistTh = 5.0; + const double segment_minSegSize = rodParam.diameter / 8; //ڴֱϷֱȡARC std::vector> arcFeatures_v; - rodAarcFeatueDetection( scanLines, cornerPara, filterParam, rodParam.diameter, arcFeatures_v); + rodArcFeatueDetection( scanLines, cornerPara, filterParam, segment_maxDistTh, segment_minSegSize, rodParam.diameter, arcFeatures_v); // std::vector rodArcTrees_v; wd_getRodArcFeatureGrowingTrees(arcFeatures_v, rodArcTrees_v, growParam); //ˮƽ std::vector> arcFeatures_h; - rodAarcFeatueDetection(hLines_raw, cornerPara, filterParam, rodParam.diameter, arcFeatures_h); + rodArcFeatueDetection(hLines_raw, cornerPara, filterParam, segment_maxDistTh, segment_minSegSize, rodParam.diameter, arcFeatures_h); // std::vector rodArcTrees_h; wd_getRodArcFeatureGrowingTrees(arcFeatures_h, rodArcTrees_h, growParam); @@ -3441,32 +3506,65 @@ void sx_rodPositioning( int nodeSize = rodArcTrees_v[i].treeNodes.size(); SVzNL3DPoint startCenter = rodArcTrees_v[i].treeNodes[0].peakPt; SVzNL3DPoint endCenter = rodArcTrees_v[i].treeNodes[nodeSize-1].peakPt; - double len = sqrt(pow(startCenter.x - endCenter.x, 2) + - pow(startCenter.y - endCenter.y, 2) + - pow(startCenter.z - endCenter.z, 2)); - double lenDiff = abs(len - rodParam.len); - double lenDiff2 = abs(len - 2 * rodParam.len); - if ( (lenDiff < rodParam.len* 0.15) || (lenDiff2 < rodParam.len * 0.15)) //validObj + double sumLen = sqrt(pow(startCenter.x - endCenter.x, 2) + + pow(startCenter.y - endCenter.y, 2) + + pow(startCenter.z - endCenter.z, 2)); + double len_ratio = sumLen / rodParam.len; + int len_mod = (int)(len_ratio + 0.5); + double len_diff = (double)len_mod - len_ratio; + if (len_diff < 0.15) { - //XYƽֱ - //Ϊ˷ֹ˲Ӱ죬 - std::vector fittingPoints; - for (int j = 0; j < nodeSize; j++) + double preSegLen = sumLen / (double)len_mod; + std::vector segEndings; + segEndings.resize(len_mod); + int preIdx = -1; + for (int m = 0; m < len_mod; m++) { - double dist1 = sqrt(pow(rodArcTrees_v[i].treeNodes[j].peakPt.x - startCenter.x, 2) + - pow(rodArcTrees_v[i].treeNodes[j].peakPt.y - startCenter.y, 2) + - pow(rodArcTrees_v[i].treeNodes[j].peakPt.z - startCenter.z, 2)); - double dist2 = sqrt(pow(rodArcTrees_v[i].treeNodes[j].peakPt.x - endCenter.x, 2) + - pow(rodArcTrees_v[i].treeNodes[j].peakPt.y - endCenter.y, 2) + - pow(rodArcTrees_v[i].treeNodes[j].peakPt.z - endCenter.z, 2)); - if ((dist1 > rodParam.diameter / 2) && (dist2 > rodParam.diameter / 2)) - fittingPoints.push_back(rodArcTrees_v[i].treeNodes[j].peakPt); + int distIdx = preIdx + 1; + startCenter = rodArcTrees_v[i].treeNodes[distIdx].peakPt; + SVzNLRange a_segEnding = { distIdx , distIdx }; + double currSegLen = 0; + while (distIdx < nodeSize) + { + currSegLen = sqrt(pow(rodArcTrees_v[i].treeNodes[distIdx].peakPt.x - startCenter.x, 2) + + pow(rodArcTrees_v[i].treeNodes[distIdx].peakPt.y - startCenter.y, 2) + + pow(rodArcTrees_v[i].treeNodes[distIdx].peakPt.z - startCenter.z, 2)); + if (currSegLen > preSegLen) + break; + else + a_segEnding.nMax = distIdx; + distIdx++; + } + segEndings[m] = a_segEnding; + preIdx = a_segEnding.nMax; + } + for (int m = 0; m < len_mod; m++) + { + //XYƽֱ + //Ϊ˷ֹ˲Ӱ죬 + std::vector fittingPoints; + for (int j = segEndings[m].nMin; j <= segEndings[m].nMax; j++) + { + int sIdx = segEndings[m].nMin; + int eIdx = segEndings[m].nMax; + startCenter = rodArcTrees_v[i].treeNodes[sIdx].peakPt; + endCenter = rodArcTrees_v[i].treeNodes[eIdx].peakPt; + + double dist1 = sqrt(pow(rodArcTrees_v[i].treeNodes[j].peakPt.x - startCenter.x, 2) + + pow(rodArcTrees_v[i].treeNodes[j].peakPt.y - startCenter.y, 2) + + pow(rodArcTrees_v[i].treeNodes[j].peakPt.z - startCenter.z, 2)); + double dist2 = sqrt(pow(rodArcTrees_v[i].treeNodes[j].peakPt.x - endCenter.x, 2) + + pow(rodArcTrees_v[i].treeNodes[j].peakPt.y - endCenter.y, 2) + + pow(rodArcTrees_v[i].treeNodes[j].peakPt.z - endCenter.z, 2)); + if ((dist1 > rodParam.diameter / 2) && (dist2 > rodParam.diameter / 2)) + fittingPoints.push_back(rodArcTrees_v[i].treeNodes[j].peakPt); + } + if (fittingPoints.size() < 3) + continue; + SSX_rodPositionInfo a_objRod; + _computeRodInfo(rodArcTrees_v[i], segEndings[m].nMin, segEndings[m].nMax, false, fittingPoints, a_objRod); + rodInfo.push_back(a_objRod); } - if (fittingPoints.size() < 3) - continue; - SSX_rodPositionInfo a_objRod; - _computeRodInfo(rodArcTrees_v[i], false, fittingPoints, a_objRod); - rodInfo.push_back(a_objRod); } } //ˮƽĿ @@ -3478,32 +3576,67 @@ void sx_rodPositioning( SVzNL3DPoint endCenter = rodArcTrees_h[i].treeNodes[nodeSize - 1].peakPt; endCenter = _exchangeXY(endCenter); - double len = sqrt(pow(startCenter.x - endCenter.x, 2) + + double sumLen = sqrt(pow(startCenter.x - endCenter.x, 2) + pow(startCenter.y - endCenter.y, 2) + pow(startCenter.z - endCenter.z, 2)); - double lenDiff = abs(len - rodParam.len); - if (lenDiff < rodParam.len * 0.15) //validObj + double len_ratio = sumLen / rodParam.len; + int len_mod = (int)(len_ratio + 0.5); + double len_diff = (double)len_mod - len_ratio; + if (len_diff < 0.15) { - //XYƽֱ - //Ϊ˷ֹ˲Ӱ죬 - std::vector fittingPoints; - for (int j = 0; j < nodeSize; j++) + double preSegLen = sumLen / (double)len_mod; + std::vector segEndings; + segEndings.resize(len_mod); + int preIdx = -1; + for (int m = 0; m < len_mod; m++) { - SVzNL3DPoint a_pt = _exchangeXY(rodArcTrees_h[i].treeNodes[j].peakPt); - double dist1 = sqrt(pow(a_pt.x - startCenter.x, 2) + pow(a_pt.y - startCenter.y, 2) + pow(a_pt.z - startCenter.z, 2)); - double dist2 = sqrt(pow(a_pt.x - endCenter.x, 2) + pow(a_pt.y - endCenter.y, 2) + pow(a_pt.z - endCenter.z, 2)); - if ((dist1 > rodParam.diameter / 2) && (dist2 > rodParam.diameter / 2)) - fittingPoints.push_back(a_pt); + int distIdx = preIdx + 1; + startCenter = _exchangeXY(rodArcTrees_h[i].treeNodes[distIdx].peakPt); + SVzNLRange a_segEnding = { distIdx , distIdx }; + double currSegLen = 0; + while (distIdx < nodeSize) + { + SVzNL3DPoint a_pt = _exchangeXY(rodArcTrees_h[i].treeNodes[distIdx].peakPt); + currSegLen = sqrt(pow(a_pt.x - startCenter.x, 2) + + pow(a_pt.y - startCenter.y, 2) + + pow(a_pt.z - startCenter.z, 2)); + if (currSegLen > preSegLen) + break; + else + a_segEnding.nMax = distIdx; + distIdx++; + } + segEndings[m] = a_segEnding; + preIdx = a_segEnding.nMax; } - if (fittingPoints.size() < 3) - continue; + for (int m = 0; m < len_mod; m++) + { + //XYƽֱ + //Ϊ˷ֹ˲Ӱ죬 + int sIdx = segEndings[m].nMin; + int eIdx = segEndings[m].nMax; + startCenter = _exchangeXY(rodArcTrees_h[i].treeNodes[sIdx].peakPt); + endCenter = _exchangeXY(rodArcTrees_h[i].treeNodes[eIdx].peakPt); - SSX_rodPositionInfo a_objRod; - _computeRodInfo(rodArcTrees_h[i], true, fittingPoints, a_objRod); - //Ƿ봹ֱĿص - bool isExist = checkObjEixst(a_objRod, rodInfo, rodParam); - if(false == isExist) - rodInfo.push_back(a_objRod); + std::vector fittingPoints; + for (int j = segEndings[m].nMin; j < segEndings[m].nMax; j++) + { + SVzNL3DPoint a_pt = _exchangeXY(rodArcTrees_h[i].treeNodes[j].peakPt); + double dist1 = sqrt(pow(a_pt.x - startCenter.x, 2) + pow(a_pt.y - startCenter.y, 2) + pow(a_pt.z - startCenter.z, 2)); + double dist2 = sqrt(pow(a_pt.x - endCenter.x, 2) + pow(a_pt.y - endCenter.y, 2) + pow(a_pt.z - endCenter.z, 2)); + if ((dist1 > rodParam.diameter / 2) && (dist2 > rodParam.diameter / 2)) + fittingPoints.push_back(a_pt); + } + if (fittingPoints.size() < 3) + continue; + + SSX_rodPositionInfo a_objRod; + _computeRodInfo(rodArcTrees_h[i], segEndings[m].nMin, segEndings[m].nMax, true, fittingPoints, a_objRod); + //Ƿ봹ֱĿص + bool isExist = checkObjEixst(a_objRod, rodInfo, rodParam); + if (false == isExist) + rodInfo.push_back(a_objRod); + } } } //2ڵж @@ -4630,6 +4763,7 @@ void sx_rebarWeldSeamPositioning( const SSG_treeGrowParam growParam, const SSX_rodParam rodParam, const SVzNLRangeD weldSeamRange, //ֽķΧСֵֵ + EWD_weldingCategory weldCategory, std::vector& weldSeamInfo, int* errCode) { @@ -4708,13 +4842,24 @@ void sx_rebarWeldSeamPositioning( } #endif - //ڴֱϷֱȡARC + + //ڴֱϷֱȡARC const double segment_maxDistTh = rodDiameter / 4; + const double segment_maxDistTh = rodParam.diameter / 4; + const double segment_minSegSize = rodParam.diameter / 8; std::vector> arcFeatures_v; - rodAarcFeatueDetection(scanLines, cornerPara, filterParam, rodParam.diameter, arcFeatures_v); +#if 1 + rodArcFeatueDetection(scanLines, cornerPara, filterParam, segment_maxDistTh, segment_minSegSize, rodParam.diameter, arcFeatures_v); +#else + lineArcAndWeldFeatueDetection(scanLines, cornerPara, filterParam, rodParam.diameter, arcFeatures_v); +#endif // std::vector allRodArcTrees_v; wd_getRodArcFeatureGrowingTrees(arcFeatures_v, allRodArcTrees_v, growParam); + //ȡVV(ν) + + + //ˮƽȹĿ꣨Ѿƽ double maxTanValue = tan(10.0 * PI / 180); std::vector rodArcTrees_v; @@ -4730,7 +4875,7 @@ void sx_rebarWeldSeamPositioning( } //ˮƽ std::vector> arcFeatures_h; - rodAarcFeatueDetection(hLines_raw, cornerPara, filterParam, rodParam.diameter, arcFeatures_h); + rodArcFeatueDetection(hLines_raw, cornerPara, filterParam, segment_maxDistTh, segment_minSegSize, rodParam.diameter, arcFeatures_h); // std::vector allRodArcTrees_h; wd_getRodArcFeatureGrowingTrees(arcFeatures_h, allRodArcTrees_h, growParam); @@ -4844,6 +4989,7 @@ void sx_rebarWeldSeamPositioning( else vRodType = 1; + //ؽѰ //ڲ double validCrossRebar_distTh = rodParam.diameter * 1.5; @@ -4978,7 +5124,6 @@ void sx_rebarWeldSeamPositioning( int posIdx = -1; SWD3DPointPostion nearestLinePt_topRebar = _computeNearestPoint(highestRod, crossPt, &posIdx); crossPoints.push_back(nearestLinePt_topRebar); - //㴹ֱϵڼ㺸췽 SVzNL3DPoint vec_axial = { highestRod.line_end.point.x - highestRod.line_start.point.x, @@ -4993,29 +5138,33 @@ void sx_rebarWeldSeamPositioning( a_vectorPosition.nPointIdx = posIdx; a_vectorPosition.pt3D = v; vectorPositions.push_back(a_vectorPosition); - //ֽĺ - //SWD3DPointPostion nearestLinePt_btmRebar = _computeNearestPoint(validVRod[i], crossPt); - std::vector a_rebarWeldSeam; - _computeRebarWeldSeam( - validVRod[i], - v, - true, - crossPt, - -1, - a_rebarWeldSeam, - scanLines, - rodParam, - weldPointCornerParam, - weldSeamRange //ֽķΧֵ - ); - weldSeamInfo.insert(weldSeamInfo.end(), a_rebarWeldSeam.begin(), a_rebarWeldSeam.end()); + + if (KeWD_WELD_CATEGPRY_I == weldCategory) + { + //ֽĺ + //SWD3DPointPostion nearestLinePt_btmRebar = _computeNearestPoint(validVRod[i], crossPt); + std::vector a_rebarWeldSeam; + _computeRebarWeldSeam( + validVRod[i], + v, + true, + crossPt, + -1, + a_rebarWeldSeam, + scanLines, + rodParam, + weldPointCornerParam, + weldSeamRange //ֽķΧֵ + ); + weldSeamInfo.insert(weldSeamInfo.end(), a_rebarWeldSeam.begin(), a_rebarWeldSeam.end()); + } } - for (int i = 0; i < (int)crossPoints.size(); i++) + for (int i = 0; i < (int)vectorPositions.size(); i++) { if (vectorPositions[i].nPointIdx < 0) continue; - for (int j = i + 1; j < (int)crossPoints.size(); j++) + for (int j = i + 1; j < (int)vectorPositions.size(); j++) { if (vectorPositions[j].nPointIdx < 0) continue; @@ -5025,7 +5174,7 @@ void sx_rebarWeldSeamPositioning( vectorPositions[j].nPointIdx = -1; } } - for (int i = 0; i < (int)crossPoints.size(); i++) + for (int i = 0; i < (int)vectorPositions.size(); i++) { if (vectorPositions[i].nPointIdx < 0) continue; diff --git a/sourceCode/rodAndBarDetection_Export.h b/sourceCode/rodAndBarDetection_Export.h index 75d24a1..bf2f05f 100644 --- a/sourceCode/rodAndBarDetection_Export.h +++ b/sourceCode/rodAndBarDetection_Export.h @@ -36,6 +36,13 @@ typedef struct SVzNL3DPoint endPt; }SSX_rodPositionInfo; +typedef enum +{ + KeWD_WELD_CATEGORY_UNKNOWN = 0, + KeWD_WELD_CATEGPRY_I, //1ÿֽ֧ + KeWD_WELD_CATEGPRY_II, //2ֽĸֽ֧ +}EWD_weldingCategory; + typedef enum { KeWD_WELD_UNKNOWN = 0, @@ -128,5 +135,6 @@ SG_APISHARED_EXPORT void sx_rebarWeldSeamPositioning( const SSG_treeGrowParam growParam, const SSX_rodParam rodParam, const SVzNLRangeD weldSeamRange, //ֽķΧСֵֵ + EWD_weldingCategory weldCategory, std::vector& weldSeamInfo, int* errCode); \ No newline at end of file