diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index 71dddbf..3f025c9 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -851,7 +851,7 @@ SG_APISHARED_EXPORT Plane robustFitPlane( // ============================================== SG_APISHARED_EXPORT Plane ransacFitPlane( const std::vector& points, - std::vector& out_inliers, + std::vector& in_inliers, float dist_thresh = 0.3f, // 内点距离阈值 int max_iter = 1000, // 最大迭代 int stop_no_improve = 250 // 连续多少次无提升就提前退出 diff --git a/sourceCode/SG_baseDataType.h b/sourceCode/SG_baseDataType.h index b7d09b1..3f719fb 100644 --- a/sourceCode/SG_baseDataType.h +++ b/sourceCode/SG_baseDataType.h @@ -13,6 +13,12 @@ typedef struct{ double yaw; // 绕Z轴旋转（偏航） }SSG_EulerAngles; +typedef struct +{ + double yaw_deg; //水平角 + double pitch_deg; //垂直角 + double range_mm; //距离 +}SWD_poloarScan2D; //以工作轴为X轴(扫描方向），平行地面与Z轴垂直的轴为Y轴，垂直地面的轴为Z轴 typedef struct { @@ -108,6 +114,14 @@ typedef struct int idx; }SSG_intPair; +typedef struct +{ + int data_0; + int data_1; + double value1; + double value2; +}SSG_intPairRelationD; + typedef struct { int flag; diff --git a/sourceCode/SG_errCode.h b/sourceCode/SG_errCode.h index 1165b0d..812f8d8 100644 --- a/sourceCode/SG_errCode.h +++ b/sourceCode/SG_errCode.h @@ -11,6 +11,7 @@ #define SG_ERR_LASER_DIR_NOT_SUPPORTED -1008 #define SG_ERR_SCAN_DIR_NOT_SUPPORTED -1009 #define SX_ERR_ZERO_OBJECTS -1010 +#define SX_ERR_NULL_GROUND_PLANE -1011 //BQ_workpiece #define SX_ERR_INVLD_VTREE_NUM -2001 diff --git a/sourceCode/SG_featureGrow.cpp b/sourceCode/SG_featureGrow.cpp index 2c05506..4a326d9 100644 --- a/sourceCode/SG_featureGrow.cpp +++ b/sourceCode/SG_featureGrow.cpp @@ -165,6 +165,8 @@ bool _segFeatureGrowing_2(SWD_segFeature& a_feature, const int lineIdx, std::vec double z_diff_1 = abs(a_feature.startPt.z - last_node.startPt.z); double z_diff_2 = abs(a_feature.endPt.z - last_node.endPt.z); int line_diff = abs(a_feature.lineIdx - last_node.lineIdx); + if (line_diff == 1) + int kkk = 1; double x_diff = (abs(a_feature.startPt.x - last_node.startPt.x) + abs(a_feature.endPt.x - last_node.endPt.x)) / 2; if ((y_diff_1 < growParam.yDeviation_max) && (y_diff_2 < growParam.yDeviation_max) && (z_diff_1 < growParam.zDeviation_max) && (z_diff_2 < growParam.zDeviation_max) && @@ -774,7 +776,7 @@ void wd_getSegFeatureGrowingTrees_2( { for (int i = 0, i_max = (int)all_lineFeatures.size(); i < i_max; i++) { - if (i == 1047) + if (i == 111) 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 cc61ff7..66293df 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -232,7 +232,7 @@ void wd_getLineDataIntervals( { double z_diff = abs(lineData[i].pt3D.z - pre_z); double y_diff = abs(lineData[i].pt3D.y - pre_y); - if ((z_diff < lineSegPara.segGapTh_z) && (y_diff < lineSegPara.segGapTh_y)) + if ( (z_diff < lineSegPara.segGapTh_z) && (y_diff < lineSegPara.segGapTh_y)) { a_run.len = i - a_run.start + 1; a_run.value = i; diff --git a/sourceCode/WD_holeDetection.cpp b/sourceCode/WD_holeDetection.cpp index 2ed6219..1365e87 100644 --- a/sourceCode/WD_holeDetection.cpp +++ b/sourceCode/WD_holeDetection.cpp @@ -107,7 +107,7 @@ void WD_getHoleInfo( //提取线段端点特征 for (int line = 0; line < lineNum; line++) { - if (line == 1047) + if (line == 137) int kkk = 1; std::vector& lineData = scanLines[line]; @@ -125,14 +125,20 @@ void WD_getHoleInfo( { int ptIdx_1 = segs[i].start; int ptIdx_2 = segs[i].start + segs[i].len - 1; - SWD_segFeature a_gap; - a_gap.lineIdx = line; - a_gap.startPtIdx = ptIdx_1; - a_gap.endPtIdx = ptIdx_2; - a_gap.startPt = lineData[ptIdx_1].pt3D; - a_gap.endPt = lineData[ptIdx_2].pt3D; - a_gap.featureValue = abs(a_gap.startPt.y - a_gap.endPt.y); - line_gaps.push_back(a_gap); + SVzNL3DPoint startPt = lineData[ptIdx_1].pt3D; + SVzNL3DPoint endPt = lineData[ptIdx_2].pt3D; + double zDiff = abs(startPt.z - endPt.z); + if (zDiff < lineSegPara.distScale) + { + SWD_segFeature a_gap; + a_gap.lineIdx = line; + a_gap.startPtIdx = ptIdx_1; + a_gap.endPtIdx = ptIdx_2; + a_gap.startPt = startPt; + a_gap.endPt = endPt; + a_gap.featureValue = abs(a_gap.startPt.y - a_gap.endPt.y); + line_gaps.push_back(a_gap); + } } holeGaps.push_back(line_gaps); } @@ -174,14 +180,20 @@ void WD_getHoleInfo( { int ptIdx_1 = segs[i].start; int ptIdx_2 = segs[i].start + segs[i].len - 1; - SWD_segFeature a_gap; - a_gap.lineIdx = line; - a_gap.startPtIdx = ptIdx_1; - a_gap.endPtIdx = ptIdx_2; - a_gap.startPt = lineData[ptIdx_1].pt3D; - a_gap.endPt = lineData[ptIdx_2].pt3D; - a_gap.featureValue = abs(a_gap.startPt.y - a_gap.endPt.y); - line_gaps.push_back(a_gap); + SVzNL3DPoint startPt = lineData[ptIdx_1].pt3D; + SVzNL3DPoint endPt = lineData[ptIdx_2].pt3D; + double zDiff = abs(startPt.z - endPt.z); + if (zDiff < lineSegPara.distScale) + { + SWD_segFeature a_gap; + a_gap.lineIdx = line; + a_gap.startPtIdx = ptIdx_1; + a_gap.endPtIdx = ptIdx_2; + a_gap.startPt = startPt; + a_gap.endPt = endPt; + a_gap.featureValue = abs(a_gap.startPt.y - a_gap.endPt.y); + line_gaps.push_back(a_gap); + } } holeGaps_h.push_back(line_gaps); } diff --git a/sourceCode/dataFitting.cpp b/sourceCode/dataFitting.cpp index ec2b97c..8fcda05 100644 --- a/sourceCode/dataFitting.cpp +++ b/sourceCode/dataFitting.cpp @@ -873,13 +873,13 @@ Plane planeFrom3Points(const cv::Point3d& p1, const cv::Point3d& p2, const cv::P // ============================================== Plane ransacFitPlane( const std::vector& points, - std::vector& out_inliers, + std::vector& in_inliers, float dist_thresh, // 内点距离阈值 int max_iter, // 最大迭代 int stop_no_improve // 连续多少次无提升就提前退出 ) { - out_inliers.clear(); + in_inliers.clear(); int n = points.size(); if (n < 3) return Plane(); @@ -928,7 +928,7 @@ Plane ransacFitPlane( // 收集内点 for (auto& p : points) { if (pointToPlaneDistance(p, best_plane) < dist_thresh) - out_inliers.push_back(p); + in_inliers.push_back(p); } return best_plane; diff --git a/sourceCode/workpieceHolePositioning.cpp b/sourceCode/workpieceHolePositioning.cpp index d5b0722..5773d8a 100644 --- a/sourceCode/workpieceHolePositioning.cpp +++ b/sourceCode/workpieceHolePositioning.cpp @@ -30,7 +30,8 @@ //version 1.5.5 : 灞变笢鏈簨鏈虹數杞儙瀹氫綅銆傦紙1锛夋坊鍔犱簡杞儙鍐呭緞澶у皬鐨勮緭鍑猴紝 锛2锛夋寜楂樺害鎺掑簭銆傛渶涓婇潰鐨勭洰鏍囨帓鍦ㄧ涓涓 //version 1.5.6 : 灞变笢鏈簨鏈虹數杞儙瀹氫綅:浼樺寲浜嗚疆鑳庡畾浣嶇畻娉曪紝浣跨敤鍦嗘壂鎻忔彁鍙栬疆寤撶偣銆 //version 1.6.0 : 娣诲姞浜嗙帠鐟炲伐浠讹紙鍗曚欢锛夌殑濮挎佽绠椼 -std::string m_strVersion = "HolePostion 1.6.0"; +//version 1.6.1 : 娣诲姞浜嗘嫇鏅彂宸ヤ欢2銆佸伐浠3銆佸伐浠4鐨勫Э鎬佽绠椼 +std::string m_strVersion = "HolePostion 1.6.1"; const char* wd_workpieceHolePositioningVersion(void) { return m_strVersion.c_str(); @@ -209,6 +210,40 @@ void _getLinePoints( } } +void _getCirclePoints( + std::vector>& quantiValue, + SVzNL3DPoint& seed, + SVzNLRect& roi2D, SVzNLRangeD& distRange, + std::vector& Points3ds) +{ + int w = (int)quantiValue.size(); + int h = (int)quantiValue[0].size(); + int width = (int)(distRange.max + 0.5); + int x0 = (int)seed.x - roi2D.left; + int y0 = (int)seed.y - roi2D.top; + for (int x = -width; x <= width; x++) + { + for (int y = -width; y <= width; y++) + { + int x1 = x0 + x; + int y1 = y0 + y; + if ((x1 < 0) || (x1 >= w) || (y1 < 0) || (y1 >= h)) + continue; + + double dist = sqrt(x * x + y * y); + if ((dist >= distRange.min) && (dist <= distRange.max)) + { + cv::Point3d a_pt3d; + a_pt3d.x = (double)(x + x0 + roi2D.left) + 0.5; + a_pt3d.y = (double)(y + y0 + roi2D.top) + 0.5; + a_pt3d.z = quantiValue[x + x0][y + y0]; + if (a_pt3d.z > 1e-4) + Points3ds.push_back(a_pt3d); + } + } + } +} + bool _validateLineZChange( std::vector>& quantiValue, SVzNL3DPoint& seed1, SVzNL3DPoint& seed2, @@ -284,6 +319,33 @@ int distanceSearchObject( return result; } +//鎼滅储鏈鎺ヨ繎distance鐨勭洰鏍 +void searchObjectInRange( + SVzNL3DPoint seed, + std::vector& holes, + SVzNLRangeD& distRange, const double deltaZ, + std::vector& searchResults) +{ + int result = -1; + int holeSize = (int)holes.size(); + for (int i = 0; i < holeSize; i++) + { + if (holes[i].radius < 0) + continue; + + if ((holes[i].center.x > 320) && (holes[i].center.x < 380) && (holes[i].center.y > -57) && (holes[i].center.y < 24)) + int kkk = 1; + double dist = sqrt(pow(seed.x - holes[i].center.x, 2) + pow(seed.y - holes[i].center.y, 2)); + if ((dist < distRange.min) || (dist > distRange.max)) + continue; + + double zDiff = abs(seed.z - holes[i].center.z); + if (zDiff < deltaZ) + searchResults.push_back(i); + } + return; +} + //鎼滅储鏈鎺ヨ繎distance涓旇搴︿负angle鐨勭洰鏍, 浠ヨ搴︿负浼樺厛 int angleConditionDistanceSearch( SVzNL3DPoint seed, SVzNL3DPoint angleSide, @@ -420,381 +482,6 @@ void _updateRoi3D(SVzNL3DRangeD& roi, SVzNL3DPoint& a_pt) return; } -#if 0 -void _updateROI(SSG_ROIRectD& roi, SVzNL3DPoint& a_pt) -{ - if (a_pt.z > 1E-4) - { - if (roi.left < 0) - { - roi.left = a_pt.x; - roi.right = a_pt.x; - roi.left = a_pt.y; - roi.right = a_pt.y; - } - else - { - if (roi.left > a_pt.x) - roi.left = a_pt.x; - if (roi.right < a_pt.x) - roi.right = a_pt.x; - if (roi.top > a_pt.y) - roi.top = a_pt.y; - if (roi.bottom < a_pt.y) - roi.bottom = a_pt.y; - } - } - return; -} - -void _updateRenge(SVzNLRange& range, int idx) -{ - if (range.nMin < 0) - { - range.nMin = idx; - range.nMax = idx; - } - else - { - if (range.nMin > idx) - range.nMin = idx; - if (range.nMax < idx) - range.nMax = idx; - } - return; -} - -bool _checkExist(int id, std::vector& buff) -{ - for (int i = 0; i < (int)buff.size(); i++) - { - if (id == buff[i]) - return true; - } - return false; -} - -void _searchNeighbours( - int selfId, int chkExtening, - int sLineIdx, int eLineIdx, - SVzNLRange ptIdxRange, - std::vector>& treeMask, - std::vector& neighbours) -{ - int lineNum = (int)treeMask.size(); - int ptNum = treeMask[0].size(); - for (int line = sLineIdx - chkExtening; line <= eLineIdx+ chkExtening; line++) - { - if ((line >= 0) && (line < lineNum)) - { - for (int ptIdx = ptIdxRange.nMin - chkExtening; ptIdx <= ptIdxRange.nMax + chkExtening; ptIdx++) - { - if ((ptIdx >= 0) && (ptIdx < ptNum)) - { - if ((treeMask[line][ptIdx] >= 0) && (treeMask[line][ptIdx] != selfId)) - { - bool isExist = _checkExist(treeMask[line][ptIdx], neighbours); - if (false == isExist) - neighbours.push_back(treeMask[line][ptIdx]); - } - } - } - } - } - return; -} - -void WD_getHoleInfo( - std::vector< std::vector>& scanLines, - const SSG_lineSegParam lineSegPara, - const SSG_outlierFilterParam filterParam, - const SSG_treeGrowParam growParam, - const double valieCommonNumRatio, - std::vector& segTrees_v, - std::vector& segTrees_h, - std::vector& validObjects -) -{ - int lineNum = (int)scanLines.size(); - int linePtNum = (int)scanLines[0].size(); - - std::vector> pointMask; - pointMask.resize(lineNum); - - //鎻愬彇绌虹櫧绾挎鐗瑰緛锛堝瓟鐗瑰緛锛 - std::vector> holeGaps; - //鎻愬彇绾挎绔偣鐗瑰緛 - for (int line = 0; line < lineNum; line++) - { - if (line == 1047) - int kkk = 1; - - std::vector& lineData = scanLines[line]; - pointMask[line].resize(lineData.size()); - std::fill(pointMask[line].begin(), pointMask[line].end(), 0);//鍒濆鍖栦负0 - //婊ゆ尝锛屾护闄ゅ紓甯哥偣 - sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam); - - std::vector segs; - wd_getLineDataNullIntervals(lineData, lineSegPara, segs); - - //灏唖eg绔偣浣滀负杈圭紭鐐广傚仛浜嗗湴闈㈣皟骞冲悗锛屽瀭鐩村瓟鐨勫唴渚у湪XY骞抽潰涓婂潎涓鸿竟缂樼偣銆 - std::vector line_gaps; - for (int i = 0, i_max = (int)segs.size(); i < i_max; i++) - { - int ptIdx_1 = segs[i].start; - int ptIdx_2 = segs[i].start + segs[i].len - 1; - SWD_segFeature a_gap; - a_gap.lineIdx = line; - a_gap.startPtIdx = ptIdx_1; - a_gap.endPtIdx = ptIdx_2; - a_gap.startPt = lineData[ptIdx_1].pt3D; - a_gap.endPt = lineData[ptIdx_2].pt3D; - a_gap.featureValue = abs(a_gap.startPt.y - a_gap.endPt.y); - line_gaps.push_back(a_gap); - } - holeGaps.push_back(line_gaps); - } - //鐗瑰緛鐢熼暱 - wd_getSegFeatureGrowingTrees_2(holeGaps, segTrees_v, growParam); - - //鐢熸垚姘村钩鎵弿 - std::vector> hLines_raw; - hLines_raw.resize(linePtNum); - for (int i = 0; i < linePtNum; i++) - hLines_raw[i].resize(lineNum); - for (int line = 0; line < lineNum; line++) - { - for (int j = 0; j < linePtNum; j++) - { - scanLines[line][j].nPointIdx = 0; //灏嗗師濮嬫暟鎹殑搴忓垪娓0锛堜細杞箟浣跨敤锛 - hLines_raw[j][line] = scanLines[line][j]; - hLines_raw[j][line].pt3D.x = scanLines[line][j].pt3D.y; - hLines_raw[j][line].pt3D.y = scanLines[line][j].pt3D.x; - } - } - //姘村钩arc鐗瑰緛鎻愬彇 - std::vector> holeGaps_h; - for (int line = 0; line < linePtNum; line++) - { - if (line == 974) - int kkk = 1; - std::vector& lineData = hLines_raw[line]; - //婊ゆ尝锛屾护闄ゅ紓甯哥偣 - int ptNum = (int)lineData.size(); - sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, filterParam); - - std::vector segs; - wd_getLineDataNullIntervals(lineData, lineSegPara, segs); - - //灏唖eg绔偣浣滀负杈圭紭鐐广傚仛浜嗗湴闈㈣皟骞冲悗锛屽瀭鐩村瓟鐨勫唴渚у湪XY骞抽潰涓婂潎涓鸿竟缂樼偣銆 - std::vector line_gaps; - for (int i = 0, i_max = (int)segs.size(); i < i_max; i++) - { - int ptIdx_1 = segs[i].start; - int ptIdx_2 = segs[i].start + segs[i].len - 1; - SWD_segFeature a_gap; - a_gap.lineIdx = line; - a_gap.startPtIdx = ptIdx_1; - a_gap.endPtIdx = ptIdx_2; - a_gap.startPt = lineData[ptIdx_1].pt3D; - a_gap.endPt = lineData[ptIdx_2].pt3D; - a_gap.featureValue = abs(a_gap.startPt.y - a_gap.endPt.y); - line_gaps.push_back(a_gap); - } - holeGaps_h.push_back(line_gaps); - } - //鐗瑰緛鐢熼暱 - wd_getSegFeatureGrowingTrees_2(holeGaps_h, segTrees_h, growParam); - - //鍒涘缓Tree鎵鍦ㄥ瓟娲炵殑Mask - std::vector> treeMask_v; - treeMask_v.resize(lineNum); - std::vector> treeMask_h; - treeMask_h.resize(lineNum); - for (int i = 0; i < lineNum; i++) - { - treeMask_v[i].resize(linePtNum); - std::fill(treeMask_v[i].begin(), treeMask_v[i].end(), -1); - treeMask_h[i].resize(linePtNum); - std::fill(treeMask_h[i].begin(), treeMask_h[i].end(), -1); - } - //鏍囨敞 - std::vector treeInfo_v; - treeInfo_v.resize(segTrees_v.size()); - for (int i = 0; i < (int)segTrees_v.size(); i++) - { - SWD_segFeatureTree& a_tree = segTrees_v[i]; - treeInfo_v[i].treeIdx = i; - treeInfo_v[i].sLineIdx = a_tree.sLineIdx; - treeInfo_v[i].eLineIdx = a_tree.eLineIdx; - treeInfo_v[i].vTreeFlag = 0; - treeInfo_v[i].treeType = 0; - treeInfo_v[i].roi = { -1, -1, -1, -1 }; - int nullPtSize = 0; - SSG_ROIRectD roi = { -1, -1, -1, -1 }; - SVzNLRange ptIdxRange = { -1, -1 }; - if (a_tree.treeNodes.size() > 0) - { - for (int m = 0; m < (int)a_tree.treeNodes.size(); m++) - { - SWD_segFeature& a_seg = a_tree.treeNodes[m]; - for (int n = a_seg.startPtIdx; n <= a_seg.endPtIdx; n++) - treeMask_v[a_seg.lineIdx][n] = i; - - nullPtSize += a_seg.endPtIdx - a_seg.startPtIdx + 1; - _updateROI(roi, scanLines[a_seg.lineIdx][a_seg.startPtIdx].pt3D); - _updateROI(roi, scanLines[a_seg.lineIdx][a_seg.endPtIdx].pt3D); - _updateRenge(ptIdxRange, a_seg.startPtIdx); - _updateRenge(ptIdxRange, a_seg.endPtIdx); - //scanLinesInput[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = 0x01; - //scanLinesInput[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = 0x01; - } - treeInfo_v[i].treeType = nullPtSize; - treeInfo_v[i].roi = roi; - treeInfo_v[i].ptIdxRange = ptIdxRange; - } - } - std::vector treeInfo_h; - treeInfo_h.resize(segTrees_h.size()); - for (int i = 0; i < (int)segTrees_h.size(); i++) - { - SWD_segFeatureTree& a_tree = segTrees_h[i]; - treeInfo_h[i].treeIdx = i; - treeInfo_h[i].sLineIdx = a_tree.sLineIdx; - treeInfo_h[i].eLineIdx = a_tree.eLineIdx; - treeInfo_h[i].vTreeFlag = 0; - treeInfo_h[i].treeType = 0; - treeInfo_h[i].roi = { -1, -1, -1, -1 }; - int nullPtSize = 0; - SSG_ROIRectD roi = { -1, -1, -1, -1 }; - SVzNLRange ptIdxRange = { -1, -1 }; - if (a_tree.treeNodes.size() > 0) - { - for (int m = 0; m < (int)a_tree.treeNodes.size(); m++) - { - SWD_segFeature& a_seg = a_tree.treeNodes[m]; - for (int n = a_seg.startPtIdx; n <= a_seg.endPtIdx; n++) - treeMask_h[n][a_seg.lineIdx] = i; - - nullPtSize += a_seg.endPtIdx - a_seg.startPtIdx + 1; - _updateROI(roi, scanLines[a_seg.startPtIdx][a_seg.lineIdx].pt3D); - _updateROI(roi, scanLines[a_seg.endPtIdx][a_seg.lineIdx].pt3D); - _updateRenge(ptIdxRange, a_seg.startPtIdx); - _updateRenge(ptIdxRange, a_seg.endPtIdx); - //scanLinesInput[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx |= 0x02; - //scanLinesInput[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx |= 0x02; - } - treeInfo_h[i].treeType = nullPtSize; - treeInfo_h[i].roi = roi; - treeInfo_h[i].ptIdxRange = ptIdxRange; - } - } - //姘村钩鍜屽瀭鐩寸洰鏍囧悎骞 - int vTreeSize = (int)segTrees_v.size(); - int hTreeSize = (int)segTrees_h.size(); - std::vector> treeHVInfo; //缁熻鍨傜洿鍜屾按骞崇殑tree鐨勪綅缃俊鎭 - treeHVInfo.resize(vTreeSize); - for (int i = 0; i < vTreeSize; i++) - { - treeHVInfo[i].resize(hTreeSize); - std::fill(treeHVInfo[i].begin(), treeHVInfo[i].end(), 0); - } - for (int line = 0; line < lineNum; line++) - { - for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++) - { - int idx_v = treeMask_v[line][ptIdx]; - int idx_h = treeMask_h[line][ptIdx]; - if ((idx_v >= 0) && (idx_h >= 0)) - treeHVInfo[idx_v][idx_h]++; - } - } - - //鐢熸垚鍊欓夌洰鏍 - std::vector objects; - for (int i = 0; i < vTreeSize; i++) - { - SWD_segFeatureTree& a_tree = segTrees_v[i]; - if ((a_tree.sLineIdx <= 1047) && (a_tree.eLineIdx >= 1047)) - int kkk = 1; - - int totalSize_v = treeInfo_v[i].treeType; - int commonSize = 0; - int hTreeIdx = -1; - for (int j = 0; j < hTreeSize; j++) - { - if (commonSize < treeHVInfo[i][j]) - { - hTreeIdx = j; - commonSize = treeHVInfo[i][j]; - } - } - if (hTreeIdx >= 0) - { - int totalSize_h = treeInfo_h[hTreeIdx].treeType; - int sizeV_th = (int)((double)totalSize_v * valieCommonNumRatio); - int sizeH_th = (int)((double)totalSize_h * valieCommonNumRatio); - if ((commonSize > sizeH_th) && (commonSize > sizeV_th)) - { - SSG_intPair a_obj; - a_obj.data_0 = i; - a_obj.data_1 = hTreeIdx; - a_obj.idx = commonSize; - objects.push_back(a_obj); - treeInfo_v[i].data = commonSize; - treeInfo_h[hTreeIdx].data = commonSize; - } - } - } - - //婊ら櫎鐩搁偦銆傛瘡涓洰鏍囦繚鐣欎竴涓 - for (int i = 0; i < (int)objects.size(); i++) - { - int vTreeIdx = objects[i].data_0; - if (treeInfo_v[vTreeIdx].vTreeFlag < 0) - continue; - - std::vector neighbours; - _searchNeighbours(vTreeIdx, 3, - treeInfo_v[vTreeIdx].sLineIdx, treeInfo_v[vTreeIdx].eLineIdx, - treeInfo_v[vTreeIdx].ptIdxRange, treeMask_v, neighbours); - - int bestIdx = vTreeIdx; - int maxValue = treeInfo_v[vTreeIdx].data; - if (neighbours.size() > 0) - { - for (int j = 0; j < (int)neighbours.size(); j++) - { - int idx = neighbours[j]; - if (maxValue < treeInfo_v[idx].data) - { - maxValue = treeInfo_v[idx].data; - bestIdx = idx; - } - } - if (bestIdx != vTreeIdx) - treeInfo_v[vTreeIdx].vTreeFlag = -1; - for (int j = 0; j < (int)neighbours.size(); j++) - { - int idx = neighbours[j]; - if (bestIdx != idx) - treeInfo_v[idx].vTreeFlag = -1; - } - } - } - - for (int i = 0; i < (int)objects.size(); i++) - { - int vTreeIdx = objects[i].data_0; - if (treeInfo_v[vTreeIdx].vTreeFlag < 0) - continue; - - validObjects.push_back(objects[i]); - } -} -#endif - bool _compareByXValue(WD_workpieceInfo& a, WD_workpieceInfo& b) { return a.center.x < b.center.x; @@ -853,7 +540,7 @@ double _getMinZ(std::vector& Points3ds) return minZ; } -//宸ヤ欢瀛斿畾浣-鎷撴櫘鍙戝伐浠跺瓟瀹氫綅 +//宸ヤ欢1: 瀛斿畾浣-鎷撴櫘鍙戝伐浠跺瓟瀹氫綅 void wd_workpieceHolePositioning( std::vector< std::vector>& scanLinesInput, const WD_workpieceHoleParam workpiecePara, @@ -1054,10 +741,13 @@ void wd_workpieceHolePositioning( double highest_z = -1; for (int objIdx = 0; objIdx < objectSize; objIdx++) { + if (objIdx == 503) + int kkk = 1; + if (holes[objIdx].radius < 0) continue; - holes[objIdx].radius = -1; + //holes[objIdx].radius = -1; SWD_HoleInfo& p0 = holes[objIdx]; int idx1 = distanceSearchObject(p0.center, holes, workpiecePara.holeDist_W, distDeviation, 0, workpiecePara.H / 2, quantiValue, roi2D, linePointRange); @@ -1096,6 +786,7 @@ void wd_workpieceHolePositioning( if (dist_k > 2.0) continue; + p0.radius = -1; p1.radius = -1; p2.radius = -1; p3.radius = -1; @@ -1335,6 +1026,1601 @@ void wd_workpieceHolePositioning( } +//妫鏌ュ伐浠惰弓涓棿瀛旀暟閲 +void _checkWorkpieceCentralHoles( + SWD_HoleInfo & p0, + SWD_HoleInfo & p1, + SWD_HoleInfo & p2, + SWD_HoleInfo & p3, + std::vector>&holeMask, + std::vector< SWD_HoleInfo>& holes, + SVzNLRect & roi2D, + SVzNLRangeD& distRange, + std::vector& innerHoleIndice) +{ + std::vector contours; + cv::Point contour_P0, contour_P1, contour_P2, contour_P3; + contour_P0 = cv::Point((int)p0.center.x - roi2D.left, (int)p0.center.y - roi2D.top); + int x_min = contour_P0.x, x_max = contour_P0.x, y_min = contour_P0.y, y_max = contour_P0.y; + + contour_P1 = cv::Point((int)p1.center.x - roi2D.left, (int)p1.center.y - roi2D.top); + x_min = x_min > contour_P1.x ? contour_P1.x : x_min; + x_max = x_max < contour_P1.x ? contour_P1.x : x_max; + y_min = y_min > contour_P1.y ? contour_P1.y : y_min; + y_max = y_max < contour_P1.y ? contour_P1.y : y_max; + + contour_P2 = cv::Point((int)p2.center.x - roi2D.left, (int)p2.center.y - roi2D.top); + x_min = x_min > contour_P2.x ? contour_P2.x : x_min; + x_max = x_max < contour_P2.x ? contour_P2.x : x_max; + y_min = y_min > contour_P2.y ? contour_P2.y : y_min; + y_max = y_max < contour_P2.y ? contour_P2.y : y_max; + + contour_P3 = cv::Point((int)p3.center.x - roi2D.left, (int)p3.center.y - roi2D.top); + x_min = x_min > contour_P3.x ? contour_P3.x : x_min; + x_max = x_max < contour_P3.x ? contour_P3.x : x_max; + y_min = y_min > contour_P3.y ? contour_P3.y : y_min; + y_max = y_max < contour_P3.y ? contour_P3.y : y_max; + + contours.push_back(contour_P0); + contours.push_back(contour_P1); + contours.push_back(contour_P2); + contours.push_back(contour_P3); + + //鍦2D鐨凴OI鍐呭揩閫熸悳绱㈢洰鏍 + std::vector indice; + for (int x = x_min; x < x_max; x++) + { + for (int y = y_min; y <= y_max; y++) + { + int objIdx = holeMask[x][y]; + if (objIdx >= 0) + { + indice.push_back(objIdx); + } + } + } + + if (indice.size() == 0) + return; + + //绮剧‘鍒ゆ柇鐩爣鏄惁涓哄唴瀛 + for (int i = 0; i < (int)indice.size(); i++) + { + int objIdx = indice[i]; + SWD_HoleInfo& centralHole = holes[objIdx]; + //if ((abs(centralHole.center.x - 187.2) < 5.0) && (abs(centralHole.center.y - 147.0) < 10.0)) + // int kkk = 1; + cv::Point2f centerPt = cv::Point2f((int)centralHole.center.x - roi2D.left, (int)centralHole.center.y - roi2D.top); + int dist = cv::pointPolygonTest(contours, centerPt, false); + if (1 == dist) + { + double d1 = sqrt(pow(centralHole.center.x - p0.center.x, 2) + pow(centralHole.center.y - p0.center.y, 2)); + double d2 = sqrt(pow(centralHole.center.x - p2.center.x, 2) + pow(centralHole.center.y - p2.center.y, 2)); + double k; + if (d1 < d2) + k = d1 / d2; + else + k = d2 / d1; + //printf("k=%g\n", k); + + if( (k >= distRange.min) && (k <= distRange.max)) + innerHoleIndice.push_back(objIdx); + } + } + return; +} + +//宸ヤ欢2: 瀛斿畾浣-鎷撴櫘鍙戝伐浠跺瓟瀹氫綅 +void wd_workpieceHolePositioning_2( + std::vector< std::vector>& scanLinesInput, + const WD_workpieceHoleParam workpiecePara, + const SSG_lineSegParam lineSegPara, + const SSG_outlierFilterParam filterParam, + const SSG_treeGrowParam growParam, + const SSG_planeCalibPara groundCalibPara, + std::vector< WD_workpieceInfo>& workpiecePositioning, + int* errCode) +{ + *errCode = 0; + + int lineNum = (int)scanLinesInput.size(); + std::vector< std::vector> scanLines; + scanLines.resize(lineNum); + int linePtNum = (int)scanLinesInput[0].size(); + bool isGridData = true; + for (int i = 0; i < lineNum; i++) + { + if (linePtNum != (int)scanLinesInput[i].size()) + isGridData = false; + + scanLines[i].resize(scanLinesInput[i].size()); + std::copy(scanLinesInput[i].begin(), scanLinesInput[i].end(), scanLines[i].begin()); // 浣跨敤std::copy绠楁硶 + + for (int j = 0; j < (int)scanLinesInput[i].size(); j++) + scanLinesInput[i][j].nPointIdx = 0; //娓呴浂锛岀敤浜巇ebug鏃惰褰曚俊鎭 + } + if (false == isGridData)//鏁版嵁涓嶆槸缃戞牸鏍煎紡 + { + *errCode = SG_ERR_NOT_GRID_FORMAT; + return; + } + for (int i = 0; i < lineNum; i++) + { //琛屽鐞 + //璋冨钩锛屽幓闄ゅ湴闈 + wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1); + } + + //鐢熸垚閲忓寲鏁版嵁锛屼互1mm涓洪噺鍖栧昂搴︼紝鐢ㄤ簬纭畾宸ヤ欢琛ㄩ潰楂樺害 + SVzNL3DRangeD roi3D = sg_getScanDataROI_vector(scanLines); + SVzNLRect roi2D; + roi2D.left = (int)roi3D.xRange.min; + roi2D.right = (int)roi3D.xRange.max; + roi2D.top = (int)roi3D.yRange.min; + roi2D.bottom = (int)roi3D.yRange.max; + int quanti_X = roi2D.right - roi2D.left + 1; + int quanti_Y = roi2D.bottom - roi2D.top + 1; + std::vector> quantiValue; + std::vector> quantiHist; + quantiValue.resize(quanti_X); + quantiHist.resize(quanti_X); + for (int i = 0; i < quanti_X; i++) + { + quantiValue[i].resize(quanti_Y); + std::fill(quantiValue[i].begin(), quantiValue[i].end(), 0);//鍒濆鍖栦负0 + quantiHist[i].resize(quanti_Y); + std::fill(quantiHist[i].begin(), quantiHist[i].end(), 0);//鍒濆鍖栦负0 + } + //浠1mm灏哄害閲忓寲 + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + SVzNL3DPoint& a_pt = scanLines[line][j].pt3D; + if (a_pt.z > 1e-4) + { + int qx = (int)a_pt.x - roi2D.left; + int qy = (int)a_pt.y - roi2D.top; + quantiValue[qx][qy] += a_pt.z; + quantiHist[qx][qy] += 1; + } + } + } + for (int ix = 0; ix < quanti_X; ix++) + { + for (int iy = 0; iy < quanti_Y; iy++) + { + if (quantiHist[ix][iy] > 0) + quantiValue[ix][iy] = quantiValue[ix][iy] / quantiHist[ix][iy]; + } + } + + std::vector segTrees_v; + std::vector segTrees_h; + std::vector validObjects; + double valieCommonNumRatio = 0.25; + WD_getHoleInfo(scanLines, lineSegPara, filterParam, growParam, valieCommonNumRatio, segTrees_v, segTrees_h, validObjects); + + for (int i = 0; i < lineNum; i++) + { + for (int j = 0; j < (int)scanLines[i].size(); j++) + scanLines[i][j].nPointIdx = 0; //娓呴浂 + } + + //鐢熸垚鑱氱被淇℃伅锛 + std::vector> clusters; //鍙褰曚綅缃 + std::vector clustersRoi3D; + for (int i = 0; i < (int)validObjects.size(); i++) + { + std::vector< SVzNL2DPoint> a_cluster; + SVzNL3DRangeD a_roi3D = { {-1, -1}, {-1, -1}, {-1, -1 } }; + + int vTreeIdx = validObjects[i].data_0; + int hTreeIdx = validObjects[i].data_1; + for (int m = 0; m < (int)segTrees_v[vTreeIdx].treeNodes.size(); m++) + { + SWD_segFeature& a_seg = segTrees_v[vTreeIdx].treeNodes[m]; + if (scanLines[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx == 0) + { + scanLines[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = vTreeIdx + 1; // 0x01; + scanLinesInput[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = vTreeIdx + 1; // 0x01; + SVzNL2DPoint a_pos = { a_seg.lineIdx , a_seg.endPtIdx }; + a_cluster.push_back(a_pos); + _updateRoi3D(a_roi3D, scanLines[a_seg.lineIdx][a_seg.endPtIdx].pt3D); + } + if (scanLines[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx == 0) + { + scanLines[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = vTreeIdx + 1; // 0x01; + scanLinesInput[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = vTreeIdx + 1; // 0x01; + SVzNL2DPoint a_pos = { a_seg.lineIdx , a_seg.startPtIdx }; + a_cluster.push_back(a_pos); + _updateRoi3D(a_roi3D, scanLines[a_seg.lineIdx][a_seg.startPtIdx].pt3D); + } + } + for (int m = 0; m < (int)segTrees_h[hTreeIdx].treeNodes.size(); m++) + { + SWD_segFeature& a_seg = segTrees_h[hTreeIdx].treeNodes[m]; + if (scanLines[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx == 0) + { + scanLines[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02; + scanLinesInput[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02; + SVzNL2DPoint a_pos = { a_seg.startPtIdx , a_seg.lineIdx }; + a_cluster.push_back(a_pos); + _updateRoi3D(a_roi3D, scanLines[a_seg.startPtIdx][a_seg.lineIdx].pt3D); + } + if (scanLines[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx == 0) + { + scanLines[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02; + scanLinesInput[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02; + SVzNL2DPoint a_pos = { a_seg.endPtIdx , a_seg.lineIdx }; + a_cluster.push_back(a_pos); + _updateRoi3D(a_roi3D, scanLines[a_seg.endPtIdx][a_seg.lineIdx].pt3D); + } + } + clusters.push_back(a_cluster); + clustersRoi3D.push_back(a_roi3D); + } + + //鑱氱被缁撴灉鍒嗘瀽 + std::vector validCluserIndexing; + int clusterSize = (int)clusters.size(); + for (int i = 0; i < clusterSize; i++) + { + SVzNL3DRangeD& a_roi = clustersRoi3D[i]; + double L = a_roi.xRange.max - a_roi.xRange.min; + double W = a_roi.yRange.max - a_roi.yRange.min; + if ((L > workpiecePara.holeDiameter * 0.5) && (L < workpiecePara.holeDiameter * 2.5) && + (W > workpiecePara.holeDiameter * 0.5) && (W < workpiecePara.holeDiameter * 2.5)) + validCluserIndexing.push_back(i); + } + //鐢熸垚缁撴灉 + std::vector< SWD_HoleInfo> holes; + int objectSize = (int)validCluserIndexing.size(); + for (int objIdx = 0; objIdx < objectSize; objIdx++) + { + std::vector pointArray; + int clusterIdx = validCluserIndexing[objIdx]; + //鍙朿luster涓婄殑鐐 + int clusterPtSize = (int)clusters[clusterIdx].size(); + double minZ = DBL_MAX; + for (int i = 0; i < clusterPtSize; i++) + { + SVzNL2DPoint a_pos = clusters[clusterIdx][i]; + int lineIdx = a_pos.x; + int ptIdx = a_pos.y; + SVzNL3DPoint a_pt3d = scanLines[lineIdx][ptIdx].pt3D; + if (minZ > a_pt3d.z) + minZ = a_pt3d.z; + pointArray.push_back(a_pt3d); + } + //鍦嗘嫙鍚 + SVzNL3DPoint center; + double radius; + double err = fitCircleByLeastSquare(pointArray, center, radius); + center.z = minZ; + SWD_HoleInfo a_hole; + a_hole.center = { center.x, center.y, center.z }; + a_hole.radius = radius; + holes.push_back(a_hole); + } + + //鐢熸垚Mask锛岀敤浜庢悳绱㈢洰鏍囩殑涓績鏄笉鏄湁瀛斻傚伐浠2鐨勭壒鐐规槸宸ヤ欢涓棿鏈夊瓟 + std::vector> holeMask; + holeMask.resize(quantiValue.size()); + for (int i = 0; i < (int)quantiValue.size(); i++) + { + holeMask[i].resize(quantiValue[i].size()); + std:fill(holeMask[i].begin(), holeMask[i].end(), -1); + } + + for (int objIdx = 0; objIdx < objectSize; objIdx++) + { + SWD_HoleInfo& a_hole = holes[objIdx]; + int px = (int)a_hole.center.x - roi2D.left; + int py = (int)a_hole.center.y - roi2D.top; + holeMask[px][py] = objIdx; + } + + //鍒嗗壊 + //鏂规硶锛氬厛鎼滅储涓嶹鏈鎺ヨ繎鐨勭偣锛岀劧鍚庢潯浠舵悳绱紙鍨傜洿锛変笌L鏈鎺ヨ繎鐨勭偣 + double distDeviation = 5.0; //璺濈鎼滅储鐨勫悎鏍奸棬闄愩傚皬浜庢璺濈锛岃涓烘悳绱㈠埌鐨勭洰鏍囦负鏈夋晥 + SVzNLRangeD linePointRange = { 0.3, 0.7 }; + + std::vector< WD_workpieceInfo> allWorkpiece; + double highest_z = -1; + + //閲囩敤杩唬娉曘傞鍏堝皢鍙俊搴﹂珮鐨勭洰鏍囨鍑猴紝 鐒跺悗缁х画妫娴嬪墿涓嬬殑鐩爣 + SVzNLRangeD innerHoleDistRange; + innerHoleDistRange.min = 0.4; + innerHoleDistRange.max = 0.8; + //棣栧厛鎸戦夊嚭淇″害楂樼殑宸ヤ欢锛氬唴瀛旀暟閲忓ぇ浜1 + for (int objIdx = 0; objIdx < objectSize; objIdx++) + { + if (objIdx == 503) + int kkk = 1; + + if (holes[objIdx].radius < 0) + continue; + + //holes[objIdx].radius = -1; + SWD_HoleInfo& p0 = holes[objIdx]; + int idx1 = distanceSearchObject(p0.center, holes, workpiecePara.holeDist_W, distDeviation, 0, workpiecePara.H / 2, + quantiValue, roi2D, linePointRange); + if (idx1 < 0) + continue; + + SVzNLRangeD angleRange = { 80, 100 }; //鍨傜洿锛5搴﹁寖鍥 + SWD_HoleInfo& p1 = holes[idx1]; + //鎼滅储鏈鎺ヨ繎distance涓旇搴︿负angle鐨勭洰鏍, 浠ヨ搴︿负浼樺厛 + int idx2 = angleConditionDistanceSearch( + p0.center, p1.center, + holes, + workpiecePara.holeDist_L, distDeviation, workpiecePara.H / 2, + angleRange, + quantiValue, roi2D, linePointRange); + if (idx2 < 0) + continue; + + SWD_HoleInfo& p2 = holes[idx2]; + //鎼滅储鏈鎺ヨ繎distance涓旇搴︿负angle鐨勭洰鏍, 浠ヨ搴︿负浼樺厛 + int idx3 = angleConditionDistanceSearch( + p1.center, p0.center, + holes, + workpiecePara.holeDist_L, distDeviation, workpiecePara.H / 2, + angleRange, + quantiValue, roi2D, linePointRange); + if (idx3 < 0) + continue; + + SWD_HoleInfo& p3 = holes[idx3]; + + //浜ゅ弶妫鏌0P1, P2P3鐨勯暱搴﹀樊 + double dist_P0P1 = sqrt(pow(p0.center.x - p1.center.x, 2) + pow(p0.center.y - p1.center.y, 2)); + double dist_P2P3 = sqrt(pow(p2.center.x - p3.center.x, 2) + pow(p2.center.y - p3.center.y, 2)); + double dist_k = dist_P2P3 / dist_P0P1; + if (dist_k > 2.0) + continue; + + //妫鏌ュ伐浠朵腑闂存湁娌℃湁瀛 + std::vector innerHoleIndice; + _checkWorkpieceCentralHoles(p0, p1, p3, p2, holeMask, holes, roi2D, innerHoleDistRange, innerHoleIndice); + if (innerHoleIndice.size() < 2) + continue; + + p0.radius = -1; + p1.radius = -1; + p2.radius = -1; + p3.radius = -1; + for (int m = 0; m < (int)innerHoleIndice.size(); m++) + { + int holeIdx = innerHoleIndice[m]; + holes[holeIdx].radius = -1; + } + + //閲嶆柊璁＄畻Z鍊笺傚洜涓烘矇瀛旂殑鍘熷洜锛孼鍊间細涓嶅噯纭傚彇鍥涙潯杈圭殑涓偣澶勭殑Z鍊肩殑鍧囧间綔涓烘暣涓殑Z鍊 + SVzNL3DPoint center_p0p1 = { (p0.center.x + p1.center.x) / 2,(p0.center.y + p1.center.y) / 2, (p0.center.z + p1.center.z) / 2 }; + SVzNL3DPoint center_p0p2 = { (p0.center.x + p2.center.x) / 2,(p0.center.y + p2.center.y) / 2, (p0.center.z + p2.center.z) / 2 }; + SVzNL3DPoint center_p1p3 = { (p1.center.x + p3.center.x) / 2,(p1.center.y + p3.center.y) / 2, (p1.center.z + p3.center.z) / 2 }; + SVzNL3DPoint center_p2p3 = { (p2.center.x + p3.center.x) / 2,(p2.center.y + p3.center.y) / 2, (p2.center.z + p3.center.z) / 2 }; + double rectR = 5.0; + double z1 = _getMeanZ(quantiValue, center_p0p1, roi2D, rectR, workpiecePara.H / 2); + if (z1 < 1e-4) + z1 = center_p0p1.z; + double z2 = _getMeanZ(quantiValue, center_p0p2, roi2D, rectR, workpiecePara.H / 2); + if (z2 < 1e-4) + z2 = center_p0p2.z; + double z3 = _getMeanZ(quantiValue, center_p1p3, roi2D, rectR, workpiecePara.H / 2); + if (z3 < 1e-4) + z3 = center_p1p3.z; + double z4 = _getMeanZ(quantiValue, center_p2p3, roi2D, rectR, workpiecePara.H / 2); + if (z4 < 1e-4) + z4 = center_p2p3.z; + + p0.center.z = (z1 + z2) / 2; + p1.center.z = (z1 + z3) / 2; + p2.center.z = (z2 + z4) / 2; + p3.center.z = (z3 + z4) / 2; + + SVzNL3DPoint centerPoint = { (p0.center.x + p1.center.x + p2.center.x + p3.center.x) / 4, + (p0.center.y + p1.center.y + p2.center.y + p3.center.y) / 4, + (z1 + z2 + z3 + z4) / 4 }; + //鍙栧伐浠惰〃闈㈢殑鐐 + std::vector Points3ds; + _getLinePoints(quantiValue, p0.center, p1.center, roi2D, linePointRange, Points3ds); + int size_1 = (int)Points3ds.size(); + _getLinePoints(quantiValue, p0.center, p2.center, roi2D, linePointRange, Points3ds); + _getLinePoints(quantiValue, p1.center, p3.center, roi2D, linePointRange, Points3ds); + _getLinePoints(quantiValue, p2.center, p3.center, roi2D, linePointRange, Points3ds); + + double minZ = _getMinZ(Points3ds); + if (minZ > 1e-4) + { + if ((highest_z < 0) || (highest_z > minZ)) + highest_z = minZ; + } + + //璁＄畻宸ヤ欢娉曞悜 + SVzNL3DPoint normalDir; + SVzNL3DPoint vec_norm; + SVzNL3DPoint y_dir; + if ((int)(Points3ds.size() > size_1) && (size_1 > 0)) + { + //璁＄畻闈㈠弬鏁: z = Ax + By + C + //res: [0]=A, [1]= B, [2]=-1.0, [3]=C, + std::vector res; + vzCaculateLaserPlane(Points3ds, res); + SVzNL3DPoint vec_1 = { -res[0], -res[1], 1.0 }; + vec_norm = vec3_normalize(vec_1); + vec_norm = vec3_multiply(vec_norm, 20.0); + normalDir = { centerPoint.x + vec_norm.x, centerPoint.y + vec_norm.y, centerPoint.z + vec_norm.z }; + + SVzNL3DPoint vector2 = { 0, 0, 1 }; + SSG_planeCalibPara adjustPara = wd_computeRTMatrix(vec_1, vector2); + SVzNL3DPoint rotate_p0 = _ptRotate(p0.center, adjustPara.planeCalib); + SVzNL3DPoint rotate_p1 = _ptRotate(p1.center, adjustPara.planeCalib); + if (rotate_p0.x < rotate_p1.x) + y_dir = { rotate_p1.x - rotate_p0.x, rotate_p1.y - rotate_p0.y, 0 }; + else + y_dir = { rotate_p0.x - rotate_p1.x, rotate_p0.y - rotate_p1.y, 0 }; + y_dir = _ptRotate(y_dir, adjustPara.invRMatrix); + } + else + { + normalDir = { centerPoint.x, centerPoint.y, centerPoint.z + 20 }; + vec_norm = { 0, 0, 20 }; + if (p0.center.x < p1.center.x) + y_dir = { p1.center.x - p0.center.x, p1.center.y - p0.center.y, 0 }; + else + y_dir = { p0.center.x - p1.center.x, p0.center.y - p1.center.y, 0 }; + } + + WD_workpieceInfo a_workpiece; + a_workpiece.workpieceType = workpiecePara.workpieceType; + a_workpiece.holes.push_back(p0.center); + a_workpiece.holes.push_back(p1.center); + a_workpiece.holes.push_back(p2.center); + a_workpiece.holes.push_back(p3.center); + + for (int m = 0; m < 4; m++) + { + SVzNL3DPoint a_pt = a_workpiece.holes[m]; + a_pt = { a_pt.x + vec_norm.x, a_pt.y + vec_norm.y, a_pt.z + vec_norm.z };//娉曞悜 + a_workpiece.holesDir.push_back(a_pt); + } + a_workpiece.center = centerPoint; + + y_dir = vec3_normalize(y_dir); + a_workpiece.y_dir = { y_dir.x * 20 + a_workpiece.center.x, y_dir.y * 20 + a_workpiece.center.y, a_workpiece.center.z + y_dir.z * 20 }; + a_workpiece.z_dir = normalDir; + allWorkpiece.push_back(a_workpiece); + } + //鍐嶆寫閫夊嚭鍓╀綑鐨勫伐浠讹細鍐呭瓟鏁伴噺==1 + for (int objIdx = 0; objIdx < objectSize; objIdx++) + { + if (objIdx == 503) + int kkk = 1; + + if (holes[objIdx].radius < 0) + continue; + + //holes[objIdx].radius = -1; + SWD_HoleInfo& p0 = holes[objIdx]; + int idx1 = distanceSearchObject(p0.center, holes, workpiecePara.holeDist_W, distDeviation, 0, workpiecePara.H / 2, + quantiValue, roi2D, linePointRange); + if (idx1 < 0) + continue; + + SVzNLRangeD angleRange = { 80, 100 }; //鍨傜洿锛5搴﹁寖鍥 + SWD_HoleInfo& p1 = holes[idx1]; + //鎼滅储鏈鎺ヨ繎distance涓旇搴︿负angle鐨勭洰鏍, 浠ヨ搴︿负浼樺厛 + int idx2 = angleConditionDistanceSearch( + p0.center, p1.center, + holes, + workpiecePara.holeDist_L, distDeviation, workpiecePara.H / 2, + angleRange, + quantiValue, roi2D, linePointRange); + if (idx2 < 0) + continue; + + SWD_HoleInfo& p2 = holes[idx2]; + //鎼滅储鏈鎺ヨ繎distance涓旇搴︿负angle鐨勭洰鏍, 浠ヨ搴︿负浼樺厛 + int idx3 = angleConditionDistanceSearch( + p1.center, p0.center, + holes, + workpiecePara.holeDist_L, distDeviation, workpiecePara.H / 2, + angleRange, + quantiValue, roi2D, linePointRange); + if (idx3 < 0) + continue; + + SWD_HoleInfo& p3 = holes[idx3]; + + //浜ゅ弶妫鏌0P1, P2P3鐨勯暱搴﹀樊 + double dist_P0P1 = sqrt(pow(p0.center.x - p1.center.x, 2) + pow(p0.center.y - p1.center.y, 2)); + double dist_P2P3 = sqrt(pow(p2.center.x - p3.center.x, 2) + pow(p2.center.y - p3.center.y, 2)); + double dist_k = dist_P2P3 / dist_P0P1; + if (dist_k > 2.0) + continue; + + //妫鏌ュ伐浠朵腑闂存湁娌℃湁瀛 + std::vector innerHoleIndice; + _checkWorkpieceCentralHoles(p0, p1, p3, p2, holeMask, holes, roi2D, innerHoleDistRange, innerHoleIndice); + if (innerHoleIndice.size() == 0 ) + continue; + + p0.radius = -1; + p1.radius = -1; + p2.radius = -1; + p3.radius = -1; + for (int m = 0; m < (int)innerHoleIndice.size(); m++) + { + int holeIdx = innerHoleIndice[m]; + holes[holeIdx].radius = -1; + } + + //閲嶆柊璁＄畻Z鍊笺傚洜涓烘矇瀛旂殑鍘熷洜锛孼鍊间細涓嶅噯纭傚彇鍥涙潯杈圭殑涓偣澶勭殑Z鍊肩殑鍧囧间綔涓烘暣涓殑Z鍊 + SVzNL3DPoint center_p0p1 = { (p0.center.x + p1.center.x) / 2,(p0.center.y + p1.center.y) / 2, (p0.center.z + p1.center.z) / 2 }; + SVzNL3DPoint center_p0p2 = { (p0.center.x + p2.center.x) / 2,(p0.center.y + p2.center.y) / 2, (p0.center.z + p2.center.z) / 2 }; + SVzNL3DPoint center_p1p3 = { (p1.center.x + p3.center.x) / 2,(p1.center.y + p3.center.y) / 2, (p1.center.z + p3.center.z) / 2 }; + SVzNL3DPoint center_p2p3 = { (p2.center.x + p3.center.x) / 2,(p2.center.y + p3.center.y) / 2, (p2.center.z + p3.center.z) / 2 }; + double rectR = 5.0; + double z1 = _getMeanZ(quantiValue, center_p0p1, roi2D, rectR, workpiecePara.H / 2); + if (z1 < 1e-4) + z1 = center_p0p1.z; + double z2 = _getMeanZ(quantiValue, center_p0p2, roi2D, rectR, workpiecePara.H / 2); + if (z2 < 1e-4) + z2 = center_p0p2.z; + double z3 = _getMeanZ(quantiValue, center_p1p3, roi2D, rectR, workpiecePara.H / 2); + if (z3 < 1e-4) + z3 = center_p1p3.z; + double z4 = _getMeanZ(quantiValue, center_p2p3, roi2D, rectR, workpiecePara.H / 2); + if (z4 < 1e-4) + z4 = center_p2p3.z; + + p0.center.z = (z1 + z2) / 2; + p1.center.z = (z1 + z3) / 2; + p2.center.z = (z2 + z4) / 2; + p3.center.z = (z3 + z4) / 2; + + SVzNL3DPoint centerPoint = { (p0.center.x + p1.center.x + p2.center.x + p3.center.x) / 4, + (p0.center.y + p1.center.y + p2.center.y + p3.center.y) / 4, + (z1 + z2 + z3 + z4) / 4 }; + //鍙栧伐浠惰〃闈㈢殑鐐 + std::vector Points3ds; + _getLinePoints(quantiValue, p0.center, p1.center, roi2D, linePointRange, Points3ds); + int size_1 = (int)Points3ds.size(); + _getLinePoints(quantiValue, p0.center, p2.center, roi2D, linePointRange, Points3ds); + _getLinePoints(quantiValue, p1.center, p3.center, roi2D, linePointRange, Points3ds); + _getLinePoints(quantiValue, p2.center, p3.center, roi2D, linePointRange, Points3ds); + + double minZ = _getMinZ(Points3ds); + if (minZ > 1e-4) + { + if ((highest_z < 0) || (highest_z > minZ)) + highest_z = minZ; + } + + //璁＄畻宸ヤ欢娉曞悜 + SVzNL3DPoint normalDir; + SVzNL3DPoint vec_norm; + SVzNL3DPoint y_dir; + if ((int)(Points3ds.size() > size_1) && (size_1 > 0)) + { + //璁＄畻闈㈠弬鏁: z = Ax + By + C + //res: [0]=A, [1]= B, [2]=-1.0, [3]=C, + std::vector res; + vzCaculateLaserPlane(Points3ds, res); + SVzNL3DPoint vec_1 = { -res[0], -res[1], 1.0 }; + vec_norm = vec3_normalize(vec_1); + vec_norm = vec3_multiply(vec_norm, 20.0); + normalDir = { centerPoint.x + vec_norm.x, centerPoint.y + vec_norm.y, centerPoint.z + vec_norm.z }; + + SVzNL3DPoint vector2 = { 0, 0, 1 }; + SSG_planeCalibPara adjustPara = wd_computeRTMatrix(vec_1, vector2); + SVzNL3DPoint rotate_p0 = _ptRotate(p0.center, adjustPara.planeCalib); + SVzNL3DPoint rotate_p1 = _ptRotate(p1.center, adjustPara.planeCalib); + if (rotate_p0.x < rotate_p1.x) + y_dir = { rotate_p1.x - rotate_p0.x, rotate_p1.y - rotate_p0.y, 0 }; + else + y_dir = { rotate_p0.x - rotate_p1.x, rotate_p0.y - rotate_p1.y, 0 }; + y_dir = _ptRotate(y_dir, adjustPara.invRMatrix); + } + else + { + normalDir = { centerPoint.x, centerPoint.y, centerPoint.z + 20 }; + vec_norm = { 0, 0, 20 }; + if (p0.center.x < p1.center.x) + y_dir = { p1.center.x - p0.center.x, p1.center.y - p0.center.y, 0 }; + else + y_dir = { p0.center.x - p1.center.x, p0.center.y - p1.center.y, 0 }; + } + + WD_workpieceInfo a_workpiece; + a_workpiece.workpieceType = workpiecePara.workpieceType; + a_workpiece.holes.push_back(p0.center); + a_workpiece.holes.push_back(p1.center); + a_workpiece.holes.push_back(p2.center); + a_workpiece.holes.push_back(p3.center); + + for (int m = 0; m < 4; m++) + { + SVzNL3DPoint a_pt = a_workpiece.holes[m]; + a_pt = { a_pt.x + vec_norm.x, a_pt.y + vec_norm.y, a_pt.z + vec_norm.z };//娉曞悜 + a_workpiece.holesDir.push_back(a_pt); + } + a_workpiece.center = centerPoint; + + y_dir = vec3_normalize(y_dir); + a_workpiece.y_dir = { y_dir.x * 20 + a_workpiece.center.x, y_dir.y * 20 + a_workpiece.center.y, a_workpiece.center.z + y_dir.z * 20 }; + a_workpiece.z_dir = normalDir; + allWorkpiece.push_back(a_workpiece); + } + + int workpieceNum = (int)allWorkpiece.size(); + if (workpieceNum == 0) + { + *errCode = SX_ERR_ZERO_OBJECTS; + return; + } + + //瀵绘壘鏈楂樼偣 + if (highest_z < 0) + { + highest_z = allWorkpiece[0].center.z; + for (int i = 1; i < workpieceNum; i++) + { + if (highest_z > allWorkpiece[i].center.z) + highest_z = allWorkpiece[i].center.z; + } + } + + //妫娴嬩笂灞傛槸鍚︽湁娈嬬暀 + double zSliceTh = highest_z - workpiecePara.H / 2; //寰涓婂伐浠禜鐨勪竴鍗 + std::vector topLayerPts; + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + if ((scanLines[line][j].pt3D.z > 1e-4) && (scanLines[line][j].pt3D.z < zSliceTh)) + { + SVzNL3DPosition a_pt; + a_pt.nPointIdx = (line << 16) | (j & 0xffff); + a_pt.pt3D = scanLines[line][j].pt3D; + topLayerPts.push_back(a_pt); + } + } + } + //杩涜鑱氱被锛屽垽鏂仛绫诲ぇ灏忋傚ぇ浜庝竴瀹氶棬闄愬垽鏂负鏈夊紓鐗 + int topLayerClusterWin = 5; + double clusterDist = 5.0; + int distType = 1; //0 - 2d distance; 1- 3d distance + std::vector> objClusters; + wd_pointClustering_speedUp( + topLayerPts, + lineNum, linePtNum, topLayerClusterWin, //鎼滅储绐楀彛 + clusterDist, + distType, //0 - 2d distance; 1- 3d distance + objClusters //result + ); + double vaidObjSize = workpiecePara.xLen * workpiecePara.yLen / 4.0; + for (int i = 0; i < (int)objClusters.size(); i++) + { + SSG_ROIRectD a_roi = _getClusterROI(objClusters[i]); + double size = (a_roi.right - a_roi.left) * (a_roi.bottom - a_roi.top); + if (size >= vaidObjSize) + { + *errCode = SX_ERR_UNKNOWN_OBJECT; + return; + } + } + + //鎺掑簭 + //z鏂瑰悜鎺掑簭 + std::vector< WD_workpieceInfo> zSortWorkpiece; + double topLayerTh = highest_z + workpiecePara.H / 2; + for (int i = 0; i < workpieceNum; i++) + { + if (allWorkpiece[i].center.z < topLayerTh) + zSortWorkpiece.push_back(allWorkpiece[i]); + } + //姘村钩鏂瑰悜鎺掑簭 + while (zSortWorkpiece.size() > 0) + { + std::vector< WD_workpieceInfo> firstLine; + std::vector< WD_workpieceInfo> restWorkpiece; + _getYTopLine(zSortWorkpiece, firstLine, restWorkpiece, workpiecePara.yLen / 2); + workpiecePositioning.insert(workpiecePositioning.end(), firstLine.begin(), firstLine.end()); + zSortWorkpiece.clear(); + zSortWorkpiece.insert(zSortWorkpiece.end(), restWorkpiece.begin(), restWorkpiece.end()); + } + + //鏃嬭浆鍥炲幓 + workpieceNum = (int)workpiecePositioning.size(); + for (int i = 0; i < workpieceNum; i++) + { + SVzNL3DPoint rpt; + rpt = _ptRotate(workpiecePositioning[i].center, groundCalibPara.invRMatrix); + workpiecePositioning[i].center = rpt; + rpt = _ptRotate(workpiecePositioning[i].y_dir, groundCalibPara.invRMatrix); + workpiecePositioning[i].y_dir = rpt; + rpt = _ptRotate(workpiecePositioning[i].z_dir, groundCalibPara.invRMatrix); + workpiecePositioning[i].z_dir = rpt; + for (int j = 0, j_max = (int)workpiecePositioning[i].holes.size(); j < j_max; j++) + { + rpt = _ptRotate(workpiecePositioning[i].holes[j], groundCalibPara.invRMatrix); + workpiecePositioning[i].holes[j] = rpt; + rpt = _ptRotate(workpiecePositioning[i].holesDir[j], groundCalibPara.invRMatrix); + workpiecePositioning[i].holesDir[j] = rpt; + } + + SVzNL3DPoint vector_z = { workpiecePositioning[i].z_dir.x - workpiecePositioning[i].center.x, + workpiecePositioning[i].z_dir.y - workpiecePositioning[i].center.y, + workpiecePositioning[i].z_dir.z - workpiecePositioning[i].center.z }; + SVzNL3DPoint vector_y = { workpiecePositioning[i].y_dir.x - workpiecePositioning[i].center.x, + workpiecePositioning[i].y_dir.y - workpiecePositioning[i].center.y, + workpiecePositioning[i].y_dir.z - workpiecePositioning[i].center.z }; + double mod_vz = sqrt(pow(vector_z.x, 2) + pow(vector_z.y, 2) + pow(vector_z.z, 2)); + vector_z = { vector_z.x / mod_vz, vector_z.y / mod_vz, vector_z.z / mod_vz }; //褰掍竴鍖 + double mod_vy = sqrt(pow(vector_y.x, 2) + pow(vector_y.y, 2) + pow(vector_y.z, 2)); + vector_y = { vector_y.x / mod_vy, vector_y.y / mod_vy, vector_y.z / mod_vy }; //褰掍竴鍖 + //鍙変箻鍑簐ector_x + SVzNL3DPoint vector_x; + vector_x.x = vector_y.y * vector_z.z - vector_z.y * vector_y.z; + vector_x.y = vector_y.z * vector_z.x - vector_z.z * vector_y.x; + vector_x.z = vector_y.x * vector_z.y - vector_z.x * vector_y.y; + workpiecePositioning[i].x_dir = vector_x; + workpiecePositioning[i].y_dir = vector_y; + workpiecePositioning[i].z_dir = vector_z; +#if 0 + //寰楀埌鏃嬭浆鐭╅樀 + double R[3][3]; + R[0][0] = vector_x.x; + R[1][0] = vector_x.y; + R[2][0] = vector_x.z; + + R[0][1] = vector_y.x; + R[1][1] = vector_y.y; + R[2][1] = vector_y.z; + + R[0][2] = vector_z.x; + R[1][2] = vector_z.y; + R[2][2] = vector_z.z; + SSG_EulerAngles eulerAngle = rotationMatrixToEulerZYX(R); + workpiecePositioning[i].workpiecePose = eulerAngle; +#endif + } + return; + +} + +void _recursiveSearch( + int seedHoleIdx, + std::vector< SWD_HoleInfo>& holes, + std::vector& holeIndice, + const double grpDistance, + std::vector& a_grp) +{ + std::vector grpHoleIndice; + grpHoleIndice.push_back(seedHoleIdx); + int i = 0; + while (i < grpHoleIndice.size()) + { + int seedIdx = grpHoleIndice[i]; + SWD_HoleInfo& seed_hole = holes[seedIdx]; + for (int m = 0; m < (int)holeIndice.size(); m++) + { + int chkIdx = holeIndice[m]; + if ( chkIdx < 0) + continue; + SWD_HoleInfo& chk_hole = holes[chkIdx]; + double dist = sqrt(pow(seed_hole.center.x - chk_hole.center.x, 2) + pow(seed_hole.center.y - chk_hole.center.y, 2)); + if (dist < grpDistance) + { + grpHoleIndice.push_back(chkIdx); + holeIndice[m] = -1; + } + } + i++; + } + // + for (int m = 0; m < (int)grpHoleIndice.size(); m++) + { + int idx = grpHoleIndice[m]; + a_grp.push_back(holes[idx]); + } + return; +} + +void _distanceSeparate( + std::vector& srcIndice, + std::vector< SWD_HoleInfo>& holes, + const double grpDistance, + std::vector>& groups) +{ + while (srcIndice.size() > 0) + { + int idx = srcIndice.back(); + srcIndice.pop_back(); + if (idx < 0) + continue; + + std::vector< SWD_HoleInfo> a_grp; + _recursiveSearch(idx, holes, srcIndice, grpDistance, a_grp); + groups.push_back(a_grp); + } + return; +} + +void _computeDistanceRelationships(std::vector& a_grp, std::vector< SSG_intPairRelationD>& distanceRelations) +{ + for (int m = 0; m < (int)a_grp.size(); m++) + { + SWD_HoleInfo& hole_1 = a_grp[m]; + for (int n = m + 1; n < (int)a_grp.size(); n++) + { + SWD_HoleInfo& hole_2 = a_grp[n]; + double distance = sqrt(pow(hole_1.center.x - hole_2.center.x, 2) + pow(hole_1.center.y - hole_2.center.y, 2)); + SSG_intPairRelationD a_pair; + a_pair.data_0 = m; + a_pair.data_1 = n; + a_pair.value1 = distance; + a_pair.value2 = abs(hole_1.center.z - hole_2.center.z); + distanceRelations.push_back(a_pair); + } + } + return; +} + +//宸ヤ欢3: 瀛斿畾浣-鎷撴櫘鍙戝伐浠跺瓟瀹氫綅 +void wd_workpieceHolePositioning_3( + std::vector< std::vector>& scanLinesInput, + const WD_workpieceHoleParam smallHoleParam, + const double bigHoleDiameter, + const SSG_lineSegParam lineSegPara, + const SSG_outlierFilterParam filterParam, + const SSG_treeGrowParam growParam, + const SSG_planeCalibPara groundCalibPara, + std::vector< WD_workpieceInfo>& workpiecePositioning, + int* errCode) +{ + *errCode = 0; + + int lineNum = (int)scanLinesInput.size(); + std::vector< std::vector> scanLines; + scanLines.resize(lineNum); + int linePtNum = (int)scanLinesInput[0].size(); + bool isGridData = true; + for (int i = 0; i < lineNum; i++) + { + if (linePtNum != (int)scanLinesInput[i].size()) + isGridData = false; + + scanLines[i].resize(scanLinesInput[i].size()); + std::copy(scanLinesInput[i].begin(), scanLinesInput[i].end(), scanLines[i].begin()); // 浣跨敤std::copy绠楁硶 + + for (int j = 0; j < (int)scanLinesInput[i].size(); j++) + scanLinesInput[i][j].nPointIdx = 0; //娓呴浂锛岀敤浜巇ebug鏃惰褰曚俊鎭 + } + if (false == isGridData)//鏁版嵁涓嶆槸缃戞牸鏍煎紡 + { + *errCode = SG_ERR_NOT_GRID_FORMAT; + return; + } + for (int i = 0; i < lineNum; i++) + { //琛屽鐞 + //璋冨钩锛屽幓闄ゅ湴闈 + wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1); + wd_lineDataR(scanLinesInput[i], groundCalibPara.planeCalib, -1); + } + + //杩囨护鎺夊嚬娈 + SSG_lineSegParam filterSegPara; + filterSegPara.distScale = 0; + filterSegPara.segGapTh_y = DBL_MAX; + filterSegPara.segGapTh_z = 15.0; + for (int line = 0; line < lineNum; line++) + { + if (line == 312) + int kkk = 1; + //鍒嗘 + std::vector segs; + wd_getLineDataIntervals(scanLines[line], filterSegPara, segs); + std::vector filterIndice; + //鑻ヤ竴涓棣栧熬閮芥瘮鐩搁偦娈典綆锛岃涓哄嚬闄锋 + for (int i = 1; i < (int)segs.size() - 1; i++) + { + int tail_1 = segs[i - 1].start + segs[i - 1].len - 1; + int head_1 = segs[i].start; + int tail_2 = segs[i].start + segs[i].len - 1; + int head_2 = segs[i + 1].start; + double z_diff_1 = scanLines[line][head_1].pt3D.z - scanLines[line][tail_1].pt3D.z; + double z_diff_2 = scanLines[line][tail_2].pt3D.z - scanLines[line][head_2].pt3D.z; + if ((z_diff_1 > 0) && (z_diff_2 > 0)) + filterIndice.push_back(i); + } + for (int m = 0; m < (int)filterIndice.size(); m++) + { + int idx = filterIndice[m]; + int offset = segs[idx].start; + for (int i = 0; i < segs[idx].len; i++) + { + scanLines[line][offset + i].pt3D.z = 0; + scanLinesInput[line][offset + i].pt3D.z = 0; + } + } + } + +#if 0 + //浜х敓姘村钩鎵弿鏁版嵁 + std::vector< std::vector> scanLines_h; + scanLines_h.resize(linePtNum); + for (int i = 0; i < linePtNum; i++) + scanLines_h[i].resize(lineNum); + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + scanLines[line][j].nPointIdx = 0; //灏嗗師濮嬫暟鎹殑搴忓垪娓0锛堜細杞箟浣跨敤锛 + scanLines_h[j][line] = scanLines[line][j]; + scanLines_h[j][line].pt3D.x = scanLines[line][j].pt3D.y; + scanLines_h[j][line].pt3D.y = scanLines[line][j].pt3D.x; + } + } + for (int line = 0; line < linePtNum; line++) + { + for (int j = 0, j_max = (int)scanLines_h[line].size(); j < j_max; j++) + scanLines_h[line][j].nPointIdx = j; + } + //鍘诲瀭鐩存 + //鍐呴儴鍙傛暟 + SSG_cornerParam removeVertialPara; + memset(&removeVertialPara, 0, sizeof(SSG_cornerParam)); + removeVertialPara.scale = 10.0; + removeVertialPara.cornerTh = 60; + + std::vector> flags; + flags.resize(lineNum); + for (int i = 0; i < lineNum; i++) + { + flags[i].resize(linePtNum); + std::fill(flags[i].begin(), flags[i].end(), 0); + } + std::vector> zVertivalFlags; + for (int line = 0; line < lineNum; line++) + { + if (line == 700) + int kkk = 1; + std::vector line_verticalFlags; + wd_getXYVertialFeature_dirAngleMethod( + scanLines[line], + line, + removeVertialPara, + line_verticalFlags + ); + zVertivalFlags.push_back(line_verticalFlags); + + for (int i = 0; i < (int)line_verticalFlags.size(); i++) + { + if (line_verticalFlags[i] > 0) + flags[line][i] = 1; + } + } + + std::vector> zVertivalFlags_h; + for (int line = 0; line < linePtNum; line++) + { + if (line == 1177) + int kkk = 1; + std::vector line_verticalFlags; + wd_getXYVertialFeature_dirAngleMethod( + scanLines_h[line], + line, + removeVertialPara, + line_verticalFlags + ); + zVertivalFlags_h.push_back(line_verticalFlags); + + for (int i = 0; i < (int)line_verticalFlags.size(); i++) + { + if (line_verticalFlags[i] > 0) + flags[i][line] = 1; + } + } + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + if (flags[line][j] > 0) + { + scanLines[line][j].pt3D.z = 0; + scanLines_h[j][line].pt3D.z = 0; + } + } + } + //杩唬涓娆 + SSG_lineSegParam filterLineSegPara; + filterLineSegPara.distScale = 5.0; + filterLineSegPara.segGapTh_y = 5.0; + filterLineSegPara.segGapTh_z = 5.0; + const int minSegLen = 5; + for (int line = 0; line < lineNum; line++) + { + std::vector segs; + wd_getLineDataIntervals( + scanLines[line], + filterLineSegPara, + segs); + for (int i = 0; i < (int)segs.size(); i++) + { + if (segs[i].len <= minSegLen) + { + int idx0 = segs[i].start; + for (int j = 0; j < segs[i].len; j++) + flags[line][idx0 + j] = 1; + } + } + } + for (int line = 0; line < linePtNum; line++) + { + std::vector segs; + wd_getLineDataIntervals( + scanLines_h[line], + filterLineSegPara, + segs); + for (int i = 0; i < (int)segs.size(); i++) + { + if (segs[i].len <= minSegLen) + { + int idx0 = segs[i].start; + for (int j = 0; j < segs[i].len; j++) + flags[idx0 + j][line] = 1; + } + } + } + + //鏍囨敞 + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + scanLinesInput[line][j].nPointIdx = 0; //灏嗗師濮嬫暟鎹殑搴忓垪娓0锛堜細杞箟浣跨敤锛 + scanLines[line][j].nPointIdx = 0; + } + } + //灏嗗瀭鐩寸嚎娈靛幓闄 + std::vector< SVzNL3DPosition> validPoints; + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + if (flags[line][j] > 0) + { + scanLines[line][j].pt3D.z = 0; + scanLinesInput[line][j].pt3D.z = 0; + } + } + } +#endif + + //鐢熸垚閲忓寲鏁版嵁锛屼互1mm涓洪噺鍖栧昂搴︼紝鐢ㄤ簬纭畾宸ヤ欢琛ㄩ潰楂樺害 + SVzNL3DRangeD roi3D = sg_getScanDataROI_vector(scanLines); + SVzNLRect roi2D; + roi2D.left = (int)roi3D.xRange.min; + roi2D.right = (int)roi3D.xRange.max; + roi2D.top = (int)roi3D.yRange.min; + roi2D.bottom = (int)roi3D.yRange.max; + int quanti_X = roi2D.right - roi2D.left + 1; + int quanti_Y = roi2D.bottom - roi2D.top + 1; + std::vector> quantiValue; + std::vector> quantiHist; + quantiValue.resize(quanti_X); + quantiHist.resize(quanti_X); + for (int i = 0; i < quanti_X; i++) + { + quantiValue[i].resize(quanti_Y); + std::fill(quantiValue[i].begin(), quantiValue[i].end(), 0);//鍒濆鍖栦负0 + quantiHist[i].resize(quanti_Y); + std::fill(quantiHist[i].begin(), quantiHist[i].end(), 0);//鍒濆鍖栦负0 + } + //浠1mm灏哄害閲忓寲 + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + SVzNL3DPoint& a_pt = scanLines[line][j].pt3D; + if (a_pt.z > 1e-4) + { + int qx = (int)a_pt.x - roi2D.left; + int qy = (int)a_pt.y - roi2D.top; + quantiValue[qx][qy] += a_pt.z; + quantiHist[qx][qy] += 1; + } + } + } + for (int ix = 0; ix < quanti_X; ix++) + { + for (int iy = 0; iy < quanti_Y; iy++) + { + if (quantiHist[ix][iy] > 0) + quantiValue[ix][iy] = quantiValue[ix][iy] / quantiHist[ix][iy]; + } + } + + std::vector segTrees_v; + std::vector segTrees_h; + std::vector validObjects; + double valieCommonNumRatio = 0.25; + WD_getHoleInfo(scanLines, lineSegPara, filterParam, growParam, valieCommonNumRatio, segTrees_v, segTrees_h, validObjects); + + for (int i = 0; i < lineNum; i++) + { + for (int j = 0; j < (int)scanLines[i].size(); j++) + scanLines[i][j].nPointIdx = 0; //娓呴浂 + } + + //鐢熸垚鑱氱被淇℃伅锛 + std::vector> clusters; //鍙褰曚綅缃 + std::vector clustersRoi3D; + for (int i = 0; i < (int)validObjects.size(); i++) + { + std::vector< SVzNL2DPoint> a_cluster; + SVzNL3DRangeD a_roi3D = { {-1, -1}, {-1, -1}, {-1, -1 } }; + + int vTreeIdx = validObjects[i].data_0; + int hTreeIdx = validObjects[i].data_1; + for (int m = 0; m < (int)segTrees_v[vTreeIdx].treeNodes.size(); m++) + { + SWD_segFeature& a_seg = segTrees_v[vTreeIdx].treeNodes[m]; + if (scanLines[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx == 0) + { + scanLines[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = vTreeIdx + 1; // 0x01; + scanLinesInput[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = vTreeIdx + 1; // 0x01; + SVzNL2DPoint a_pos = { a_seg.lineIdx , a_seg.endPtIdx }; + a_cluster.push_back(a_pos); + _updateRoi3D(a_roi3D, scanLines[a_seg.lineIdx][a_seg.endPtIdx].pt3D); + } + if (scanLines[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx == 0) + { + scanLines[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = vTreeIdx + 1; // 0x01; + scanLinesInput[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = vTreeIdx + 1; // 0x01; + SVzNL2DPoint a_pos = { a_seg.lineIdx , a_seg.startPtIdx }; + a_cluster.push_back(a_pos); + _updateRoi3D(a_roi3D, scanLines[a_seg.lineIdx][a_seg.startPtIdx].pt3D); + } + } + for (int m = 0; m < (int)segTrees_h[hTreeIdx].treeNodes.size(); m++) + { + SWD_segFeature& a_seg = segTrees_h[hTreeIdx].treeNodes[m]; + if (scanLines[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx == 0) + { + scanLines[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02; + scanLinesInput[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02; + SVzNL2DPoint a_pos = { a_seg.startPtIdx , a_seg.lineIdx }; + a_cluster.push_back(a_pos); + _updateRoi3D(a_roi3D, scanLines[a_seg.startPtIdx][a_seg.lineIdx].pt3D); + } + if (scanLines[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx == 0) + { + scanLines[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02; + scanLinesInput[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02; + SVzNL2DPoint a_pos = { a_seg.endPtIdx , a_seg.lineIdx }; + a_cluster.push_back(a_pos); + _updateRoi3D(a_roi3D, scanLines[a_seg.endPtIdx][a_seg.lineIdx].pt3D); + } + } + clusters.push_back(a_cluster); + clustersRoi3D.push_back(a_roi3D); + } + + //鑱氱被缁撴灉鍒嗘瀽 + std::vector bigCluserIndice; //澶у瓟 + std::vector smallCluserIndice; //灏忓瓟 + int clusterSize = (int)clusters.size(); + for (int i = 0; i < clusterSize; i++) + { + SVzNL3DRangeD& a_roi = clustersRoi3D[i]; + double L = a_roi.xRange.max - a_roi.xRange.min; + double W = a_roi.yRange.max - a_roi.yRange.min; + if ((L > smallHoleParam.holeDiameter * 0.5) && (L < smallHoleParam.holeDiameter * 2.5) && + (W > smallHoleParam.holeDiameter * 0.5) && (W < smallHoleParam.holeDiameter * 2.5)) + smallCluserIndice.push_back(i); + else if ((L > bigHoleDiameter * 0.75) && (L < bigHoleDiameter * 1.25) && + (W > bigHoleDiameter * 0.75) && (W < bigHoleDiameter * 1.25)) + bigCluserIndice.push_back(i); + } + + //鐢熸垚Hole淇℃伅 + //澶у瓟 + std::vector< SWD_HoleInfo> bigHoles; + int bigHoleSize = (int)bigCluserIndice.size(); + for (int objIdx = 0; objIdx < bigHoleSize; objIdx++) + { + std::vector pointArray; + int clusterIdx = bigCluserIndice[objIdx]; + //鍙朿luster涓婄殑鐐 + int clusterPtSize = (int)clusters[clusterIdx].size(); + double sumZ = 0; + int counter = 0; + for (int i = 0; i < clusterPtSize; i++) + { + SVzNL2DPoint a_pos = clusters[clusterIdx][i]; + int lineIdx = a_pos.x; + int ptIdx = a_pos.y; + SVzNL3DPoint a_pt3d = scanLines[lineIdx][ptIdx].pt3D; + if (a_pt3d.z > 1e-4) + { + sumZ += a_pt3d.z; + counter++; + } + pointArray.push_back(a_pt3d); + } + + //鍦嗘嫙鍚 + SVzNL3DPoint center; + double radius; + double err = fitCircleByLeastSquare(pointArray, center, radius); + center.z = sumZ / counter; + SWD_HoleInfo a_hole; + a_hole.center = { center.x, center.y, center.z }; + a_hole.radius = radius; + bigHoles.push_back(a_hole); + } + //灏忓瓟 + std::vector< SWD_HoleInfo> smallHoles; + int smallHoleSize = (int)smallCluserIndice.size(); + for (int objIdx = 0; objIdx < smallHoleSize; objIdx++) + { + std::vector pointArray; + int clusterIdx = smallCluserIndice[objIdx]; + //鍙朿luster涓婄殑鐐 + int clusterPtSize = (int)clusters[clusterIdx].size(); + double minZ = DBL_MAX; + double maxZ = 0; + for (int i = 0; i < clusterPtSize; i++) + { + SVzNL2DPoint a_pos = clusters[clusterIdx][i]; + int lineIdx = a_pos.x; + int ptIdx = a_pos.y; + SVzNL3DPoint a_pt3d = scanLines[lineIdx][ptIdx].pt3D; + if (minZ > a_pt3d.z) + minZ = a_pt3d.z; + if (maxZ < a_pt3d.z) + maxZ = a_pt3d.z; + pointArray.push_back(a_pt3d); + } + //鍦嗘嫙鍚 + double zDiff = maxZ - minZ; + if (zDiff < lineSegPara.distScale) + { + SVzNL3DPoint center; + double radius; + double err = fitCircleByLeastSquare(pointArray, center, radius); + center.z = minZ; + SWD_HoleInfo a_hole; + a_hole.center = { center.x, center.y, center.z }; + a_hole.radius = radius; + smallHoles.push_back(a_hole); + } + } + + //鐢熸垚鐩爣 + //鍐呴儴鍙傛暟 + double smallHoleMinDistance = 8.0; + double maxDistanceFromCenterToLine = 25.0; + SVzNLRangeD distRange = { 0, bigHoleDiameter * 1.2 }; + double deltaZ = 10.0; + std::vector< WD_workpieceInfo> allWorkpiece; + double highest_z = -1; + for (int objIdx = 0; objIdx < bigHoleSize; objIdx++) + { + if (objIdx == 42) + int kkk = 1; + + if (bigHoles[objIdx].radius < 0) + continue; + + //holes[objIdx].radius = -1; + SWD_HoleInfo& p0 = bigHoles[objIdx]; + //鎼滅储鐩镐俊灏忓瓟 + std::vector searchResults; + searchObjectInRange(p0.center, smallHoles, distRange, deltaZ, searchResults); + //鏍规嵁璺濈杩涜鍒嗙粍 + std::vector> groups; + _distanceSeparate(searchResults, smallHoles, bigHoleDiameter, groups); + if (groups.size() == 0) //宸ヤ欢3鍜屽伐浣4鐨勭壒鐐规槸灏忓瓟鍒嗕负涓婁笅鍚3涓傚垎缁 + continue; + + //鎼滅储姣忕粍涓窛绂诲ぇ浜庡ぇ瀛旂洿寰0.8鍊嶇殑鐩爣 + SWD_HoleInfo p1, p2; + bool foundObj = false; + bool isYDIr = true; + for (int grp = 0; grp < (int)groups.size(); grp++) + { + std::vector& a_grp = groups[grp]; + if (a_grp.size() < 2) + continue; + + std::vector< SSG_intPairRelationD> distanceRelations; + _computeDistanceRelationships(a_grp, distanceRelations); + double minDistance = distanceRelations[0].value1; + for (int m = 1; m < (int)distanceRelations.size(); m ++) + minDistance = minDistance > distanceRelations[m].value1 ? distanceRelations[m].value1 : minDistance; + if (minDistance < smallHoleMinDistance) //鏈夐潪娉曞皬瀛 + continue; + + std::vector< SSG_intPairRelationD> validRelations; + for (int m = 0; m < (int)distanceRelations.size(); m++) + { + if (distanceRelations[m].value1 > bigHoleDiameter * 0.8) + validRelations.push_back(distanceRelations[m]); + } + + if (validRelations.size() == 1) + { + int idx1 = validRelations[0].data_0; + int idx2= validRelations[0].data_1; + p1 = a_grp[idx1]; + p2 = a_grp[idx2]; + foundObj = true; + break; + } + else if (validRelations.size() > 1) + { + SSG_intPairRelationD bestPair = validRelations[0]; + for (int m = 1; m < (int)validRelations.size(); m++) + { + if (bestPair.value2 > validRelations[m].value2) + bestPair = validRelations[m]; + } + int idx1 = bestPair.data_0; + int idx2 = bestPair.data_1; + p1 = a_grp[idx1]; + p2 = a_grp[idx2]; + foundObj = true; + } + } + if (false == foundObj) + { + //灏濊瘯绗簩绉嶆柟娉曪紝瀵绘壘鍨傜洿鏂瑰悜鐨勭洰鏍 + if (groups.size() == 2) + { + SWD_HoleInfo pTmp1, pTmp2; + double minDistToLine = -1; + for (int m = 0; m < (int)groups[0].size(); m++) + { + SWD_HoleInfo& pp1 = groups[0][m]; + for (int n = 0; n < (int)groups[1].size(); n++) + { + SWD_HoleInfo& pp2 = groups[1][n]; + double _a, _b, _c; + compute2ptLine( pp1.center, pp2.center, &_a, &_b, &_c); + double dist = computePtDistToLine(p0.center.x, p0.center.x, _a, _b, _c); + if (minDistToLine < 0) + { + minDistToLine = dist; + pTmp1 = pp1; + pTmp2 = pp2; + } + else + { + if (minDistToLine > dist) + { + minDistToLine = dist; + pTmp1 = pp1; + pTmp2 = pp2; + } + } + } + } + if ((minDistToLine >= 0)&&(minDistToLine < maxDistanceFromCenterToLine)) + { + p1 = pTmp1; + p2 = pTmp2; + foundObj = true; + isYDIr = false; + } + } + + } + if (false == foundObj) + continue; + + SVzNL3DPoint centerPoint = p0.center; + + //璁＄畻宸ヤ欢娉曞悜 + SVzNLRangeD distRange = { bigHoleDiameter / 2 + 2.0, bigHoleDiameter / 2 + 4.0 }; + std::vector Points3ds; + _getCirclePoints(quantiValue, p0.center,roi2D, distRange, Points3ds); + + //璁＄畻闈㈠弬鏁: z = Ax + By + C + //res: [0]=A, [1]= B, [2]=-1.0, [3]=C, + std::vector res; + vzCaculateLaserPlane(Points3ds, res); + SVzNL3DPoint vec_1 = { -res[0], -res[1], 1.0 }; + SVzNL3DPoint vec_norm = vec3_normalize(vec_1); + SVzNL3DPoint vec_norm_multi = vec3_multiply(vec_norm, 20.0); + SVzNL3DPoint normalDir = { centerPoint.x + vec_norm_multi.x, centerPoint.y + vec_norm_multi.y, centerPoint.z + vec_norm_multi.z }; + + SVzNL3DPoint vector2 = { 0, 0, 1 }; + SSG_planeCalibPara adjustPara = wd_computeRTMatrix(vec_1, vector2); + + SVzNL3DPoint rotate_p1 = _ptRotate(p1.center, adjustPara.planeCalib); + SVzNL3DPoint rotate_p2 = _ptRotate(p2.center, adjustPara.planeCalib); + + + SVzNL3DPoint y_dir; + if (true == isYDIr) + { + if (rotate_p2.x < rotate_p1.x) + y_dir = { rotate_p1.x - rotate_p2.x, rotate_p1.y - rotate_p2.y, 0 }; + else + y_dir = { rotate_p2.x - rotate_p1.x, rotate_p2.y - rotate_p1.y, 0 }; + y_dir = _ptRotate(y_dir, adjustPara.invRMatrix); + } + else + { + SVzNL3DPoint x_dir; + if (rotate_p2.x < rotate_p1.x) + x_dir = { rotate_p1.x - rotate_p2.x, rotate_p1.y - rotate_p2.y, 0 }; + else + x_dir = { rotate_p2.x - rotate_p1.x, rotate_p2.y - rotate_p1.y, 0 }; + + x_dir = vec3_normalize(x_dir); + //鍙変箻鍑簓Dir + SVzNL3DPoint vec_norm = vec3_normalize(vec_1); + y_dir.x = vec_norm.y * x_dir.z - x_dir.y * vec_norm.z; + y_dir.y = vec_norm.z * x_dir.x - x_dir.z * vec_norm.x; + y_dir.z = vec_norm.x * x_dir.y - x_dir.x * vec_norm.y; + if (y_dir.x < 0) + y_dir = { -y_dir.x, -y_dir.y, -y_dir.z }; + + y_dir = _ptRotate(y_dir, adjustPara.invRMatrix); + } + + WD_workpieceInfo a_workpiece; + a_workpiece.workpieceType = smallHoleParam.workpieceType; + a_workpiece.center = centerPoint; + y_dir = vec3_normalize(y_dir); + a_workpiece.y_dir = { y_dir.x * 20 + a_workpiece.center.x, y_dir.y * 20 + a_workpiece.center.y, a_workpiece.center.z + y_dir.z * 20 }; + a_workpiece.z_dir = normalDir; + allWorkpiece.push_back(a_workpiece); + } + + int workpieceNum = (int)allWorkpiece.size(); + if (workpieceNum == 0) + { + *errCode = SX_ERR_ZERO_OBJECTS; + return; + } + + //瀵绘壘鏈楂樼偣 + if (highest_z < 0) + { + highest_z = allWorkpiece[0].center.z; + for (int i = 1; i < workpieceNum; i++) + { + if (highest_z > allWorkpiece[i].center.z) + highest_z = allWorkpiece[i].center.z; + } + } + + //妫娴嬩笂灞傛槸鍚︽湁娈嬬暀 + double zSliceTh = highest_z - smallHoleParam.H / 2; //寰涓婂伐浠禜鐨勪竴鍗 + std::vector topLayerPts; + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + if ((scanLines[line][j].pt3D.z > 1e-4) && (scanLines[line][j].pt3D.z < zSliceTh)) + { + SVzNL3DPosition a_pt; + a_pt.nPointIdx = (line << 16) | (j & 0xffff); + a_pt.pt3D = scanLines[line][j].pt3D; + topLayerPts.push_back(a_pt); + } + } + } + //杩涜鑱氱被锛屽垽鏂仛绫诲ぇ灏忋傚ぇ浜庝竴瀹氶棬闄愬垽鏂负鏈夊紓鐗 + int topLayerClusterWin = 5; + double clusterDist = 5.0; + int distType = 1; //0 - 2d distance; 1- 3d distance + std::vector> objClusters; + wd_pointClustering_speedUp( + topLayerPts, + lineNum, linePtNum, topLayerClusterWin, //鎼滅储绐楀彛 + clusterDist, + distType, //0 - 2d distance; 1- 3d distance + objClusters //result + ); + double vaidObjSize = smallHoleParam.xLen * smallHoleParam.yLen / 4.0; + for (int i = 0; i < (int)objClusters.size(); i++) + { + SSG_ROIRectD a_roi = _getClusterROI(objClusters[i]); + double size = (a_roi.right - a_roi.left) * (a_roi.bottom - a_roi.top); + if (size >= vaidObjSize) + { + *errCode = SX_ERR_UNKNOWN_OBJECT; + return; + } + } + + //鎺掑簭 + //z鏂瑰悜鎺掑簭 + std::vector< WD_workpieceInfo> zSortWorkpiece; + double topLayerTh = highest_z + smallHoleParam.H / 2; + for (int i = 0; i < workpieceNum; i++) + { + if (allWorkpiece[i].center.z < topLayerTh) + zSortWorkpiece.push_back(allWorkpiece[i]); + } + //姘村钩鏂瑰悜鎺掑簭 + while (zSortWorkpiece.size() > 0) + { + std::vector< WD_workpieceInfo> firstLine; + std::vector< WD_workpieceInfo> restWorkpiece; + _getYTopLine(zSortWorkpiece, firstLine, restWorkpiece, smallHoleParam.yLen / 2); + workpiecePositioning.insert(workpiecePositioning.end(), firstLine.begin(), firstLine.end()); + zSortWorkpiece.clear(); + zSortWorkpiece.insert(zSortWorkpiece.end(), restWorkpiece.begin(), restWorkpiece.end()); + } + + //鏃嬭浆鍥炲幓 + workpieceNum = (int)workpiecePositioning.size(); + for (int i = 0; i < workpieceNum; i++) + { + SVzNL3DPoint rpt; + rpt = _ptRotate(workpiecePositioning[i].center, groundCalibPara.invRMatrix); + workpiecePositioning[i].center = rpt; + rpt = _ptRotate(workpiecePositioning[i].y_dir, groundCalibPara.invRMatrix); + workpiecePositioning[i].y_dir = rpt; + rpt = _ptRotate(workpiecePositioning[i].z_dir, groundCalibPara.invRMatrix); + workpiecePositioning[i].z_dir = rpt; + for (int j = 0, j_max = (int)workpiecePositioning[i].holes.size(); j < j_max; j++) + { + rpt = _ptRotate(workpiecePositioning[i].holes[j], groundCalibPara.invRMatrix); + workpiecePositioning[i].holes[j] = rpt; + rpt = _ptRotate(workpiecePositioning[i].holesDir[j], groundCalibPara.invRMatrix); + workpiecePositioning[i].holesDir[j] = rpt; + } + + SVzNL3DPoint vector_z = { workpiecePositioning[i].z_dir.x - workpiecePositioning[i].center.x, + workpiecePositioning[i].z_dir.y - workpiecePositioning[i].center.y, + workpiecePositioning[i].z_dir.z - workpiecePositioning[i].center.z }; + SVzNL3DPoint vector_y = { workpiecePositioning[i].y_dir.x - workpiecePositioning[i].center.x, + workpiecePositioning[i].y_dir.y - workpiecePositioning[i].center.y, + workpiecePositioning[i].y_dir.z - workpiecePositioning[i].center.z }; + double mod_vz = sqrt(pow(vector_z.x, 2) + pow(vector_z.y, 2) + pow(vector_z.z, 2)); + vector_z = { vector_z.x / mod_vz, vector_z.y / mod_vz, vector_z.z / mod_vz }; //褰掍竴鍖 + double mod_vy = sqrt(pow(vector_y.x, 2) + pow(vector_y.y, 2) + pow(vector_y.z, 2)); + vector_y = { vector_y.x / mod_vy, vector_y.y / mod_vy, vector_y.z / mod_vy }; //褰掍竴鍖 + //鍙変箻鍑簐ector_x + SVzNL3DPoint vector_x; + vector_x.x = vector_y.y * vector_z.z - vector_z.y * vector_y.z; + vector_x.y = vector_y.z * vector_z.x - vector_z.z * vector_y.x; + vector_x.z = vector_y.x * vector_z.y - vector_z.x * vector_y.y; + workpiecePositioning[i].x_dir = vector_x; + workpiecePositioning[i].y_dir = vector_y; + workpiecePositioning[i].z_dir = vector_z; +#if 0 + //寰楀埌鏃嬭浆鐭╅樀 + double R[3][3]; + R[0][0] = vector_x.x; + R[1][0] = vector_x.y; + R[2][0] = vector_x.z; + + R[0][1] = vector_y.x; + R[1][1] = vector_y.y; + R[2][1] = vector_y.z; + + R[0][2] = vector_z.x; + R[1][2] = vector_z.y; + R[2][2] = vector_z.z; + SSG_EulerAngles eulerAngle = rotationMatrixToEulerZYX(R); + workpiecePositioning[i].workpiecePose = eulerAngle; +#endif + } + return; + +} typedef struct { int flag; diff --git a/sourceCode/workpieceHolePositioning_Export.h b/sourceCode/workpieceHolePositioning_Export.h index e24b67d..1816c39 100644 --- a/sourceCode/workpieceHolePositioning_Export.h +++ b/sourceCode/workpieceHolePositioning_Export.h @@ -70,7 +70,7 @@ SG_APISHARED_EXPORT void wd_lineDataR( const double* camPoseR, double groundH); -//工件孔定位-拓普发工件孔定位 +//工件1: 孔定位-拓普发工件孔定位 SG_APISHARED_EXPORT void wd_workpieceHolePositioning( std::vector< std::vector>& scanLinesInput, const WD_workpieceHoleParam workpiecePara, @@ -80,6 +80,27 @@ SG_APISHARED_EXPORT void wd_workpieceHolePositioning( const SSG_planeCalibPara groundCalibPara, std::vector< WD_workpieceInfo>& workpiecePositioning, int* errCode); +//工件2: 孔定位-拓普发工件孔定位 +SG_APISHARED_EXPORT void wd_workpieceHolePositioning_2( + std::vector< std::vector>& scanLinesInput, + const WD_workpieceHoleParam workpiecePara, + const SSG_lineSegParam lineSegPara, + const SSG_outlierFilterParam filterParam, + const SSG_treeGrowParam growParam, + const SSG_planeCalibPara groundCalibPara, + std::vector< WD_workpieceInfo>& workpiecePositioning, + int* errCode); +//工件3: 孔定位-拓普发工件孔定位 +SG_APISHARED_EXPORT void wd_workpieceHolePositioning_3( + std::vector< std::vector>& scanLinesInput, + const WD_workpieceHoleParam smallHoleParam, + const double bigHoleDiameter, + const SSG_lineSegParam lineSegPara, + const SSG_outlierFilterParam filterParam, + const SSG_treeGrowParam growParam, + const SSG_planeCalibPara groundCalibPara, + std::vector< WD_workpieceInfo>& workpiecePositioning, + int* errCode); //单个孔或凹坑定位-华航孔定位 SG_APISHARED_EXPORT void wd_HolePositioning( diff --git a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp index ab7848c..20a872c 100644 --- a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp +++ b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp @@ -32,6 +32,15 @@ typedef struct float b; } SPointXYZRGB; +SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9]) +{ + SVzNL3DPoint _r_pt; + _r_pt.x = pt3D.x * matrix3d[0] + pt3D.y * matrix3d[1] + pt3D.z * matrix3d[2]; + _r_pt.y = pt3D.x * matrix3d[3] + pt3D.y * matrix3d[4] + pt3D.z * matrix3d[5]; + _r_pt.z = pt3D.x * matrix3d[6] + pt3D.y * matrix3d[7] + pt3D.z * matrix3d[8]; + return _r_pt; +} + void vzReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector>& scanData) { std::ifstream inputFile(fileName); @@ -82,6 +91,39 @@ void vzReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector& lineData) +{ + int num = 0; + for (int i = 0; i < (int)lineData.size(); i++) + { + if (lineData[i].pt3D.z > 1e-4) + num++; + } + return num; +} +void _removeZeroLines(std::vector>& scanData) +{ + int lineNum = (int)scanData.size(); + int firstLine = -1; + int lastLine = 0; + for (int line = 0; line < lineNum; line++) + { + int num = counterLinePtNum(scanData[line]); + if ((num > 0) && (firstLine < 0)) + firstLine = line; + if (num > 0) + lastLine = line; + } + if (firstLine < 0) + scanData.clear(); + + if (lastLine < (lineNum - 1)) + scanData.erase(scanData.begin() + lastLine + 1, scanData.end()); + + scanData.erase(scanData.begin(), scanData.begin() + firstLine); + return; +} + void _outputScanDataFile_XYZ_vector(char* fileName, std::vector>& scanData) { std::ofstream sw(fileName); @@ -399,15 +441,20 @@ void _outputRGBDResult_RGBD( int linePtNum = (int)objects.size(); sw << "Line_" << lineNum << "_0_" << linePtNum + 1 << std::endl; lineNum++; - size = 10; for (int i = 0; i < linePtNum; i++) { if (i == 0) - size = 20; + { + size = 5; + rgb = { 255, 0, 0 }; + } else - size = 10; - int colorIdx = objects[i].nPointIdx % 8; - rgb = objColor[colorIdx]; + { + rgb = { 255, 255, 0 }; + size = 5; + } + //int colorIdx = objects[i].nPointIdx % 8; + //rgb = objColor[colorIdx]; float x = (float)objects[i].pt3D.x; float y = (float)objects[i].pt3D.y; float z = (float)objects[i].pt3D.z; @@ -836,6 +883,245 @@ SVzNL3DPoint _pointRT(SVzNL3DPoint& origin, const double* R, const double* T) return result; } +void _XOYprojection( + cv::Mat& img, + std::vector>& dataLines, + std::vector< WD_workpieceInfo>& holes, + const double scale, + const int sideWidth, + const SVzNLRangeD x_range, + const SVzNLRangeD y_range, + bool drawDirAngle, + const double dirAngleLen) +{ + cv::Vec3b rgb = cv::Vec3b(0, 0, 0); + cv::Vec3b objColor[8] = { + {245,222,179},//娣￠粍鑹 + {210,105, 30},//宸у厠鍔涜壊 + {240,230,140},//榛勮鑹 + {135,206,235},//澶╄摑鑹 + {250,235,215},//鍙よ懀鐧 + {189,252,201},//钖勮嵎鑹 + {221,160,221},//姊呯孩鑹 + {188,143,143},//鐜懓绾㈣壊 + }; + int size = 1; + for (int line = 0; line < dataLines.size(); line++) + { + std::vector< SVzNL3DPosition>& a_line = dataLines[line]; + for (int i = 0; i < a_line.size(); i++) + { + SVzNL3DPosition& pt3D = a_line[i]; + if (pt3D.pt3D.z < 1e-4) + continue; + + int flag = pt3D.nPointIdx & 0xffff; + if (flag > 0) + { + rgb = objColor[flag % 8]; // { 255, 97, 0 }; + size = 1; + } + else + { + rgb = { 150, 150, 150 }; + size = 1; + } + + double x = pt3D.pt3D.x; + double y = pt3D.pt3D.y; + int px = (int)((x - x_range.min) / scale + sideWidth); + int py = (int)((y - y_range.min) / scale + sideWidth); + if (size == 1) + img.at(py, px) = cv::Vec3b(rgb[2], rgb[1], rgb[0]); + else + cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1); + } + } + if (holes.size() > 0) + { + for (int i = 0; i < (int)holes.size(); i++) + { + if (i == 0) + { + rgb = { 255, 0, 0 }; + size = int(3.0/scale); + } + else + { + rgb = { 255, 255, 0 }; + size = int(3.0 / scale); + } + WD_workpieceInfo& a_hole = holes[i]; + int px = (int)((a_hole.center.x - x_range.min) / scale + sideWidth); + int py = (int)((a_hole.center.y - y_range.min) / scale + sideWidth); + cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1); + rgb = { 0, 255, 0 }; + size = int(2.0 / scale); + for (int m = 0; m < (int)a_hole.holes.size(); m++) + { + px = (int)((a_hole.holes[m].x - x_range.min) / scale + sideWidth); + py = (int)((a_hole.holes[m].y - y_range.min) / scale + sideWidth); + cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1); + } + if (true == drawDirAngle) + { + //鐢荤嚎 + double x1 = a_hole.center.x; + double y1 = a_hole.center.y; + int px1 = (int)((x1 - x_range.min) / scale + sideWidth); + int py1 = (int)((y1 - y_range.min) / scale + sideWidth); + //x杞 + double x2 = a_hole.center.x + dirAngleLen * a_hole.x_dir.x; + double y2 = a_hole.center.y + dirAngleLen * a_hole.x_dir.y; + int px2 = (int)((x2 - x_range.min) / scale + sideWidth); + int py2 = (int)((y2 - y_range.min) / scale + sideWidth); + rgb = { 255, 0, 0 }; //x杞寸敤绾㈣壊 + cv::line(img, cv::Point(px1, py1), cv::Point(px2, py2), cv::Scalar(rgb[2], rgb[1], rgb[0]), 1); + //y杞 + x2 = a_hole.center.x + dirAngleLen * a_hole.y_dir.x; + y2 = a_hole.center.y + dirAngleLen * a_hole.y_dir.y; + px2 = (int)((x2 - x_range.min) / scale + sideWidth); + py2 = (int)((y2 - y_range.min) / scale + sideWidth); + rgb = { 0, 255, 0 }; //y杞寸敤缁胯壊 + cv::line(img, cv::Point(px1, py1), cv::Point(px2, py2), cv::Scalar(rgb[2], rgb[1], rgb[0]), 1); + //z杞 + x2 = a_hole.center.x + dirAngleLen * a_hole.z_dir.x; + y2 = a_hole.center.y + dirAngleLen * a_hole.z_dir.y; + px2 = (int)((x2 - x_range.min) / scale + sideWidth); + py2 = (int)((y2 - y_range.min) / scale + sideWidth); + rgb = { 0, 0, 255 }; //z杞寸敤钃濊壊 + cv::line(img, cv::Point(px1, py1), cv::Point(px2, py2), cv::Scalar(rgb[2], rgb[1], rgb[0]), 1); + } + } + } + +} + +void EulerRpyToRotation1(const double rpy[3], double matrix3d[9]) { + double cos0 = cos(rpy[0] * PI / 180); + double sin0 = sin(rpy[0] * PI / 180); + double cos1 = cos(rpy[1] * PI / 180); + double sin1 = sin(rpy[1] * PI / 180); + double cos2 = cos(rpy[2] * PI / 180); + double sin2 = sin(rpy[2] * PI / 180); + matrix3d[0] = cos2 * cos1; + matrix3d[1] = cos2 * sin1 * sin0 - sin2 * cos0; + matrix3d[2] = cos2 * sin1 * cos0 + sin2 * sin0; + matrix3d[3] = sin2 * cos1; + matrix3d[4] = sin2 * sin1 * sin0 + cos2 * cos0; + matrix3d[5] = sin2 * sin1 * cos0 - cos2 * sin0; + matrix3d[6] = -sin1; + matrix3d[7] = cos1 * sin0; + matrix3d[8] = cos1 * cos0; + return; +} + +void _rotateCloudPts( + std::vector>& scanLines, + double matrix3d[9], + std::vector>& rotateLines, + SVzNLRangeD& rx_range, SVzNLRangeD& ry_range) +{ + int lineNum = (int)scanLines.size(); + rotateLines.resize(lineNum); + rx_range.min = 0; + rx_range.max = -1; + ry_range.min = 0; + ry_range.max = -1; + for (int line = 0; line < lineNum; line++) + { + for (int i = 0; i < (int)scanLines[line].size(); i++) + { + SVzNL3DPosition& pt3D = scanLines[line][i]; + if (pt3D.pt3D.z < 1e-4) + continue; + + SVzNL3DPosition r_pt; + r_pt.pt3D = _ptRotate(pt3D.pt3D, matrix3d); + r_pt.nPointIdx = pt3D.nPointIdx; + rotateLines[line].push_back(r_pt); + if (rx_range.max < rx_range.min) + { + rx_range.min = r_pt.pt3D.x; + rx_range.max = r_pt.pt3D.x; + } + else + { + if (rx_range.min > r_pt.pt3D.x) + rx_range.min = r_pt.pt3D.x; + if (rx_range.max < r_pt.pt3D.x) + rx_range.max = r_pt.pt3D.x; + } + if (ry_range.max < ry_range.min) + { + ry_range.min = r_pt.pt3D.y; + ry_range.max = r_pt.pt3D.y; + } + else + { + if (ry_range.min > r_pt.pt3D.y) + ry_range.min = r_pt.pt3D.y; + if (ry_range.max < r_pt.pt3D.y) + ry_range.max = r_pt.pt3D.y; + } + } + } +} + +void _genXOYProjectionImage( + cv::String& fileName, + std::vector>& scanLines, + const double scale, + std::vector< WD_workpieceInfo>& holes, + const double rpy[3], const double dirLen) +{ + //鏃嬭浆瑙嗚鏄剧ず + double matrix3d[9]; + EulerRpyToRotation1(rpy, matrix3d); + std::vector r_objOps; + r_objOps.resize(holes.size()); + for (int i = 0; i < (int)holes.size(); i++) + { + r_objOps[i].workpieceType = holes[i].workpieceType; + r_objOps[i].center = _ptRotate(holes[i].center, matrix3d); + r_objOps[i].x_dir = _ptRotate(holes[i].x_dir, matrix3d); + r_objOps[i].y_dir = _ptRotate(holes[i].y_dir, matrix3d); + r_objOps[i].z_dir = _ptRotate(holes[i].z_dir, matrix3d); + for (int m = 0; m < (int)holes[i].holes.size(); m++) + { + SVzNL3DPoint r_pt = _ptRotate(holes[i].holes[m], matrix3d); + r_objOps[i].holes.push_back(r_pt); + } + for (int m = 0; m < (int)holes[i].holesDir.size(); m++) + { + SVzNL3DPoint r_pt = _ptRotate(holes[i].holesDir[m], matrix3d); + r_objOps[i].holesDir.push_back(r_pt); + } + } + std::vector> rotateLines; + SVzNLRangeD rx_range, ry_range; + _rotateCloudPts(scanLines, matrix3d, rotateLines, rx_range, ry_range); + + //缁熻X鍜孻鐨勮寖鍥 + int x_cols = (int)((rx_range.max - rx_range.min) / scale) + 1; + if (x_cols % 2 == 1) + x_cols += 1; + int y_rows = (int)((ry_range.max - ry_range.min) / scale) + 1; + if (y_rows % 2 == 1) + y_rows += 1; + + int sideWidth = 4; + int imgCols = x_cols + sideWidth * 2; + int imgRows = y_rows + sideWidth * 2; + + cv::Mat img = cv::Mat::zeros(imgRows, imgCols, CV_8UC3); + //璁＄畻鎶曞奖姣斾緥 + _XOYprojection(img, rotateLines, r_objOps, scale, sideWidth, rx_range, ry_range, true, dirLen); + cv::imwrite(fileName, img); + return; +} + + #define TEST_COMPUTE_CALIB_PARA 0 #define TEST_COMPUTE_HOLE 1 #define TEST_COMPUTE_RT 0 @@ -1046,6 +1332,7 @@ void TuoPuFa_holePosition_test(void) sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx); std::vector> scanLines; vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + _removeZeroLines(scanLines); if (scanLines.size() == 0) continue; @@ -1096,6 +1383,634 @@ void TuoPuFa_holePosition_test(void) _outputRGBDResult_RGBD(_scan_file, scanLines, workpiecePositioning); sprintf_s(calibFile, "%sresult\\LaserLine%d_corner_info.txt", dataPath[grp], fidx); _outputWorkpieceInfo(calibFile, workpiecePositioning); + + sprintf_s(calibFile, "%sresult/LaserLine%d_holes_projection.png", dataPath[grp], fidx); + double rpy[3] = { -30, 15, 0 }; //{ 0,-45, 0 }; // + double angleDrawLen = 15; + double displayScale = 0.3; + cv::String imgName(calibFile); + _genXOYProjectionImage(imgName, scanLines, displayScale, workpiecePositioning, rpy, angleDrawLen); + } + } +#endif +} + +#define TPF2_TEST_GROUP 10 +//鎷撴櫘鍙戝伐浠跺瓟瀹氫綅锛堝伐浠跺畾浣) +void TuoPuFa_holePosition_test2(void) +{ + const char* dataPath[TPF2_TEST_GROUP] = { + + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢2/1灞/", //0 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢2/2灞/", //1 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢2/3灞/", //2 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢2/4灞/", //3 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢2/5灞/", //4 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢2/6灞/", //5 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢2/7灞/", //6 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢2/8灞/", //7 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢2/9灞/", //8 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢2/10灞/", //9 + }; + + SVzNLRange fileIdx[TPF2_TEST_GROUP] = { + {1,10}, {1, 11}, {1,10}, {1,10}, {1, 11}, + {1,10}, {1, 10}, {1,11}, {1,10}, {1,10} + }; + + const char* ver = wd_workpieceHolePositioningVersion(); + printf("ver:%s\n", ver); + +#if TEST_COMPUTE_CALIB_PARA + char _calib_datafile[256]; + sprintf_s(_calib_datafile, "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢2/璋冨钩鏁版嵁.txt"); + int lineNum = 0; + float lineV = 0.0f; + int dataCalib = 0; + int maxTimeStamp = 0; + int clockPerSecond = 0; + std::vector> scanData; + vzReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanData); + + lineNum = (int)scanData.size(); + if (scanData.size() > 0) + { + SSG_planeCalibPara calibPara = wd_getGroundCalibPara(scanData); + //缁撴灉杩涜楠岃瘉 + for (int i = 0; i < lineNum; i++) + { + if (i == 14) + int kkk = 1; + //琛屽鐞 + //璋冨钩锛屽幓闄ゅ湴闈 + wd_lineDataR(scanData[i], calibPara.planeCalib, -1); + } + // + char calibFile[250]; + sprintf_s(calibFile, "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢2/ground_calib_para.txt"); + _outputCalibPara(calibFile, calibPara); + char _out_file[256]; + sprintf_s(_out_file, "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢2/scanData_ground_calib_verify.txt"); + int headNullLines = 0; + _outputScanDataFile_vector(_out_file, scanData, false, &headNullLines); + printf("%s: calib done!\n", _calib_datafile); + } +#endif + +#if TEST_COMPUTE_HOLE + for (int grp = 0; grp < TPF2_TEST_GROUP; grp++) + { + SSG_planeCalibPara groundCalibPara; + //鍒濆鍖栨垚鍗曚綅闃 + groundCalibPara.planeCalib[0] = 1.0; + groundCalibPara.planeCalib[1] = 0.0; + groundCalibPara.planeCalib[2] = 0.0; + groundCalibPara.planeCalib[3] = 0.0; + groundCalibPara.planeCalib[4] = 1.0; + groundCalibPara.planeCalib[5] = 0.0; + groundCalibPara.planeCalib[6] = 0.0; + groundCalibPara.planeCalib[7] = 0.0; + groundCalibPara.planeCalib[8] = 1.0; + groundCalibPara.planeHeight = -1.0; + for (int i = 0; i < 9; i++) + groundCalibPara.invRMatrix[i] = groundCalibPara.planeCalib[i]; + char calibFile[250]; + sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]); + //groundCalibPara = _readCalibPara(calibFile); + + for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) + { + //fidx =2; + char _scan_file[256]; + sprintf_s(_scan_file, "%s%d_LaserData_Jh26B074.txt", dataPath[grp], fidx); + std::vector> scanLines; + vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + _removeZeroLines(scanLines); + if (scanLines.size() == 0) + continue; + + long t1 = (long)GetTickCount64();//缁熻鏃堕棿 + + SSG_lineSegParam lineSegPara; + lineSegPara.distScale = 3.0; + lineSegPara.segGapTh_y = 15.0; // + lineSegPara.segGapTh_z = 0.0; //z鏂瑰悜闂撮殧澶т簬10mm璁や负鏄垎娈 + SSG_outlierFilterParam filterParam; + filterParam.continuityTh = 5.0; //鍣０婊ら櫎銆傚綋鐩搁偦鐐圭殑z璺冲彉澶т簬姝ら棬闄愭椂锛屾鏌ユ槸鍚︿负鍣０銆傝嫢闀垮害灏忎簬outlierLen锛 瑙嗕负鍣０ + filterParam.outlierTh = 5; + SSG_treeGrowParam growParam; + growParam.maxLineSkipNum = 2; + growParam.yDeviation_max = 4.0; + growParam.maxSkipDistance = 0.0; + growParam.zDeviation_max = 10.0;// + growParam.minLTypeTreeLen = 2.0; //mm + growParam.minVTypeTreeLen = 2.0; //mm + WD_workpieceHoleParam workpiecePara; + workpiecePara.workpieceType = 0; + workpiecePara.holeDiameter = 6.0; // + workpiecePara.holeDist_W = 32.0; + workpiecePara.holeDist_L = 40.0; + workpiecePara.xLen = 44; + workpiecePara.yLen = 70; + workpiecePara.H = 47; + int errCode = 0; + std::vector< WD_workpieceInfo> workpiecePositioning; + wd_workpieceHolePositioning_2( + scanLines, + workpiecePara, + lineSegPara, + filterParam, + growParam, + groundCalibPara, + workpiecePositioning, + &errCode); + long t2 = (long)GetTickCount64(); + if (errCode == SX_ERR_UNKNOWN_OBJECT) + printf("%s: %d(ms), 鏈夊紓鐗╂畫鐣!\n", _scan_file, (int)(t2 - t1)); + else if (errCode == SX_ERR_ZERO_OBJECTS) + printf("%s: %d(ms), 鏃犱骇鍝!\n", _scan_file, (int)(t2 - t1)); + else + printf("%s: %d(ms), errCode=%d ...", _scan_file, (int)(t2 - t1), errCode); + //杈撳嚭娴嬭瘯缁撴灉 + sprintf_s(_scan_file, "%sresult/LaserLine%d_result.txt", dataPath[grp], fidx); + _outputRGBDResult_RGBD(_scan_file, scanLines, workpiecePositioning); + sprintf_s(calibFile, "%sresult/LaserLine%d_hole_info.txt", dataPath[grp], fidx); + _outputWorkpieceInfo(calibFile, workpiecePositioning); + + sprintf_s(calibFile, "%sresult/LaserLine%d_holes_projection.png", dataPath[grp], fidx); + double rpy[3] = { -30, 15, 0 }; //{ 0,-45, 0 }; // + double angleDrawLen = 15; + double displayScale = 0.3; + cv::String imgName(calibFile); + _genXOYProjectionImage( imgName, scanLines, displayScale, workpiecePositioning, rpy, angleDrawLen); + printf("done\n"); + } + } +#endif +} + +#define TPF3_TEST_GROUP 7 +//鎷撴櫘鍙戝伐浠跺瓟瀹氫綅锛堝伐浠跺畾浣) +void TuoPuFa_holePosition_test3(void) +{ + const char* dataPath[TPF3_TEST_GROUP] = { + + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢3/1灞/", //0 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢3/2灞/", //1 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢3/3灞/", //2 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢3/4灞/", //3 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢3/5灞/", //4 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢3/6灞/", //5 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢3/7灞/", //6 + }; + + SVzNLRange fileIdx[TPF3_TEST_GROUP] = { + {1,10}, {1, 10}, {1,10}, {1,10}, {1, 10}, + {1,10}, {1, 10}, + }; + + const char* ver = wd_workpieceHolePositioningVersion(); + printf("ver:%s\n", ver); + +#if TEST_COMPUTE_CALIB_PARA + char _calib_datafile[256]; + sprintf_s(_calib_datafile, "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢2/璋冨钩鏁版嵁.txt"); + int lineNum = 0; + float lineV = 0.0f; + int dataCalib = 0; + int maxTimeStamp = 0; + int clockPerSecond = 0; + std::vector> scanData; + vzReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanData); + + lineNum = (int)scanData.size(); + if (scanData.size() > 0) + { + SSG_planeCalibPara calibPara = wd_getGroundCalibPara(scanData); + //缁撴灉杩涜楠岃瘉 + for (int i = 0; i < lineNum; i++) + { + if (i == 14) + int kkk = 1; + //琛屽鐞 + //璋冨钩锛屽幓闄ゅ湴闈 + wd_lineDataR(scanData[i], calibPara.planeCalib, -1); + } + // + char calibFile[250]; + sprintf_s(calibFile, "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢2/ground_calib_para.txt"); + _outputCalibPara(calibFile, calibPara); + char _out_file[256]; + sprintf_s(_out_file, "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢2/scanData_ground_calib_verify.txt"); + int headNullLines = 0; + _outputScanDataFile_vector(_out_file, scanData, false, &headNullLines); + printf("%s: calib done!\n", _calib_datafile); + } +#endif + +#if TEST_COMPUTE_HOLE + for (int grp = 0; grp < TPF3_TEST_GROUP; grp++) + { + SSG_planeCalibPara groundCalibPara; + //鍒濆鍖栨垚鍗曚綅闃 + groundCalibPara.planeCalib[0] = 1.0; + groundCalibPara.planeCalib[1] = 0.0; + groundCalibPara.planeCalib[2] = 0.0; + groundCalibPara.planeCalib[3] = 0.0; + groundCalibPara.planeCalib[4] = 1.0; + groundCalibPara.planeCalib[5] = 0.0; + groundCalibPara.planeCalib[6] = 0.0; + groundCalibPara.planeCalib[7] = 0.0; + groundCalibPara.planeCalib[8] = 1.0; + groundCalibPara.planeHeight = -1.0; + for (int i = 0; i < 9; i++) + groundCalibPara.invRMatrix[i] = groundCalibPara.planeCalib[i]; + char calibFile[250]; + sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]); + //groundCalibPara = _readCalibPara(calibFile); + + for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) + { + //fidx = 2; + char _scan_file[256]; + sprintf_s(_scan_file, "%s%d_LaserData_Jh26B074.txt", dataPath[grp], fidx); + std::vector> scanLines; + vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + _removeZeroLines(scanLines); + if (scanLines.size() == 0) + continue; + + long t1 = (long)GetTickCount64();//缁熻鏃堕棿 + + + SSG_outlierFilterParam filterParam; + filterParam.continuityTh = 5.0; //鍣０婊ら櫎銆傚綋鐩搁偦鐐圭殑z璺冲彉澶т簬姝ら棬闄愭椂锛屾鏌ユ槸鍚︿负鍣０銆傝嫢闀垮害灏忎簬outlierLen锛 瑙嗕负鍣０ + filterParam.outlierTh = 5; + SSG_treeGrowParam growParam; + growParam.maxLineSkipNum = 2; + growParam.yDeviation_max = 4.0; + growParam.maxSkipDistance = 0.0; + growParam.zDeviation_max = 10.0;// + growParam.minLTypeTreeLen = 2.0; //mm + growParam.minVTypeTreeLen = 2.0; //mm + WD_workpieceHoleParam workpiecePara; + workpiecePara.workpieceType = 0; + workpiecePara.holeDiameter = 6.0; // + workpiecePara.holeDist_W = 32.0; + workpiecePara.holeDist_L = 40.0; + workpiecePara.xLen = 44; + workpiecePara.yLen = 70; + workpiecePara.H = 60; + SSG_lineSegParam lineSegPara; + lineSegPara.distScale = workpiecePara.H * 0.5; //姝ゅ弬鏁扮敤浣滃垽鏂伐浠跺瓟鐐规槸鍚﹀湪鍚屼竴涓伐浠朵笂 + lineSegPara.segGapTh_y = 40.0; // + lineSegPara.segGapTh_z = 0.0; //z鏂瑰悜闂撮殧澶т簬10mm璁や负鏄垎娈 + double bigHoleDiameter = 33.0; + int errCode = 0; + std::vector< WD_workpieceInfo> workpiecePositioning; + wd_workpieceHolePositioning_3( + scanLines, + workpiecePara, + bigHoleDiameter, + lineSegPara, + filterParam, + growParam, + groundCalibPara, + workpiecePositioning, + &errCode); + long t2 = (long)GetTickCount64(); + if (errCode == SX_ERR_UNKNOWN_OBJECT) + printf("%s: %d(ms), 鏈夊紓鐗╂畫鐣! ...", _scan_file, (int)(t2 - t1)); + else if (errCode == SX_ERR_ZERO_OBJECTS) + printf("%s: %d(ms), 鏃犱骇鍝! ...", _scan_file, (int)(t2 - t1)); + else + printf("%s: %d(ms), errCode=%d ...", _scan_file, (int)(t2 - t1), errCode); + //杈撳嚭娴嬭瘯缁撴灉 + sprintf_s(_scan_file, "%sresult/LaserLine%d_result.txt", dataPath[grp], fidx); + _outputRGBDResult_RGBD(_scan_file, scanLines, workpiecePositioning); + sprintf_s(calibFile, "%sresult/LaserLine%d_hole_info.txt", dataPath[grp], fidx); + _outputWorkpieceInfo(calibFile, workpiecePositioning); + + sprintf_s(calibFile, "%sresult/LaserLine%d_holes_projection.png", dataPath[grp], fidx); + double rpy[3] = { -30, 15, 0 }; //{ 0,-45, 0 }; // + double angleDrawLen = 15; + double displayScale = 0.3; + cv::String imgName(calibFile); + _genXOYProjectionImage(imgName, scanLines, displayScale, workpiecePositioning, rpy, angleDrawLen); + printf("done\n"); + } + } +#endif +} +#define TPF4_TEST_GROUP 7 +//鎷撴櫘鍙戝伐浠跺瓟瀹氫綅锛堝伐浠跺畾浣) +void TuoPuFa_holePosition_test4(void) +{ + + const char* dataPath[TPF4_TEST_GROUP] = { + + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢4/1灞/", //0 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢4/2灞/", //1 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢4/3灞/", //2 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢4/4灞/", //3 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢4/5灞/", //4 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢4/6灞/", //5 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢4/7灞/", //6 + }; + + SVzNLRange fileIdx[TPF4_TEST_GROUP] = { + {1,11}, {1, 11}, {1,11}, {1,10}, {1, 10}, + {1,10}, {1, 10}, + }; + + const char* ver = wd_workpieceHolePositioningVersion(); + printf("ver:%s\n", ver); + +#if TEST_COMPUTE_CALIB_PARA + char _calib_datafile[256]; + sprintf_s(_calib_datafile, "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢2/璋冨钩鏁版嵁.txt"); + int lineNum = 0; + float lineV = 0.0f; + int dataCalib = 0; + int maxTimeStamp = 0; + int clockPerSecond = 0; + std::vector> scanData; + vzReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanData); + + lineNum = (int)scanData.size(); + if (scanData.size() > 0) + { + SSG_planeCalibPara calibPara = wd_getGroundCalibPara(scanData); + //缁撴灉杩涜楠岃瘉 + for (int i = 0; i < lineNum; i++) + { + if (i == 14) + int kkk = 1; + //琛屽鐞 + //璋冨钩锛屽幓闄ゅ湴闈 + wd_lineDataR(scanData[i], calibPara.planeCalib, -1); + } + // + char calibFile[250]; + sprintf_s(calibFile, "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢2/ground_calib_para.txt"); + _outputCalibPara(calibFile, calibPara); + char _out_file[256]; + sprintf_s(_out_file, "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢2/scanData_ground_calib_verify.txt"); + int headNullLines = 0; + _outputScanDataFile_vector(_out_file, scanData, false, &headNullLines); + printf("%s: calib done!\n", _calib_datafile); + } +#endif + +#if TEST_COMPUTE_HOLE + for (int grp = 0; grp < TPF4_TEST_GROUP; grp++) + { + SSG_planeCalibPara groundCalibPara; + //鍒濆鍖栨垚鍗曚綅闃 + groundCalibPara.planeCalib[0] = 1.0; + groundCalibPara.planeCalib[1] = 0.0; + groundCalibPara.planeCalib[2] = 0.0; + groundCalibPara.planeCalib[3] = 0.0; + groundCalibPara.planeCalib[4] = 1.0; + groundCalibPara.planeCalib[5] = 0.0; + groundCalibPara.planeCalib[6] = 0.0; + groundCalibPara.planeCalib[7] = 0.0; + groundCalibPara.planeCalib[8] = 1.0; + groundCalibPara.planeHeight = -1.0; + for (int i = 0; i < 9; i++) + groundCalibPara.invRMatrix[i] = groundCalibPara.planeCalib[i]; + char calibFile[250]; + sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]); + //groundCalibPara = _readCalibPara(calibFile); + + for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) + { + //fidx = 2; + char _scan_file[256]; + sprintf_s(_scan_file, "%s%d_LaserData_Jh26B074.txt", dataPath[grp], fidx); + std::vector> scanLines; + vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + _removeZeroLines(scanLines); + if (scanLines.size() == 0) + continue; + + long t1 = (long)GetTickCount64();//缁熻鏃堕棿 + + + SSG_outlierFilterParam filterParam; + filterParam.continuityTh = 5.0; //鍣０婊ら櫎銆傚綋鐩搁偦鐐圭殑z璺冲彉澶т簬姝ら棬闄愭椂锛屾鏌ユ槸鍚︿负鍣０銆傝嫢闀垮害灏忎簬outlierLen锛 瑙嗕负鍣０ + filterParam.outlierTh = 5; + SSG_treeGrowParam growParam; + growParam.maxLineSkipNum = 2; + growParam.yDeviation_max = 4.0; + growParam.maxSkipDistance = 0.0; + growParam.zDeviation_max = 10.0;// + growParam.minLTypeTreeLen = 2.0; //mm + growParam.minVTypeTreeLen = 2.0; //mm + WD_workpieceHoleParam workpiecePara; + workpiecePara.workpieceType = 0; + workpiecePara.holeDiameter = 6.0; // + workpiecePara.holeDist_W = 32.0; + workpiecePara.holeDist_L = 40.0; + workpiecePara.xLen = 44; + workpiecePara.yLen = 70; + workpiecePara.H = 60; + SSG_lineSegParam lineSegPara; + lineSegPara.distScale = workpiecePara.H * 0.5; //姝ゅ弬鏁扮敤浣滃垽鏂伐浠跺瓟鐐规槸鍚﹀湪鍚屼竴涓伐浠朵笂 + lineSegPara.segGapTh_y = 40.0; // + lineSegPara.segGapTh_z = 0.0; //z鏂瑰悜闂撮殧澶т簬10mm璁や负鏄垎娈 + double bigHoleDiameter = 33.0; + int errCode = 0; + std::vector< WD_workpieceInfo> workpiecePositioning; + wd_workpieceHolePositioning_3( + scanLines, + workpiecePara, + bigHoleDiameter, + lineSegPara, + filterParam, + growParam, + groundCalibPara, + workpiecePositioning, + &errCode); + long t2 = (long)GetTickCount64(); + if (errCode == SX_ERR_UNKNOWN_OBJECT) + printf("%s: %d(ms), 鏈夊紓鐗╂畫鐣! ...", _scan_file, (int)(t2 - t1)); + else if (errCode == SX_ERR_ZERO_OBJECTS) + printf("%s: %d(ms), 鏃犱骇鍝! ...", _scan_file, (int)(t2 - t1)); + else + printf("%s: %d(ms), errCode=%d ...", _scan_file, (int)(t2 - t1), errCode); + //杈撳嚭娴嬭瘯缁撴灉 + sprintf_s(_scan_file, "%sresult/LaserLine%d_result.txt", dataPath[grp], fidx); + _outputRGBDResult_RGBD(_scan_file, scanLines, workpiecePositioning); + sprintf_s(calibFile, "%sresult/LaserLine%d_hole_info.txt", dataPath[grp], fidx); + _outputWorkpieceInfo(calibFile, workpiecePositioning); + + sprintf_s(calibFile, "%sresult/LaserLine%d_holes_projection.png", dataPath[grp], fidx); + double rpy[3] = { -30, 15, 0 }; //{ 0,-45, 0 }; // + double angleDrawLen = 15; + double displayScale = 0.3; + cv::String imgName(calibFile); + _genXOYProjectionImage(imgName, scanLines, displayScale, workpiecePositioning, rpy, angleDrawLen); + printf("done\n"); + } + } +#endif +} + +#define TPF5_TEST_GROUP 14 +//鎷撴櫘鍙戝伐浠跺瓟瀹氫綅锛堝伐浠跺畾浣) +void TuoPuFa_holePosition_test5(void) +{ + const char* dataPath[TPF5_TEST_GROUP] = { + + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢5/1灞/", //0 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢5/2灞/", //1 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢5/3灞/", //2 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢5/4灞/", //3 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢5/5灞/", //4 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢5/6灞/", //5 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢5/7灞/", //6 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢5/8灞/", //7 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢5/9灞/", //8 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢5/10灞/", //9 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢5/11灞/", //10 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢5/12灞/", //11 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢5/13灞/", //12 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢5/14灞/", //13 + }; + + SVzNLRange fileIdx[TPF5_TEST_GROUP] = { + {1,10}, {1, 10}, {1,10}, {1,10}, {1, 10}, + {1,11}, {1, 10}, {1,11}, {1,10}, {1,10}, + {1, 10}, {1,10}, {1,11}, {1,10} + }; + + const char* ver = wd_workpieceHolePositioningVersion(); + printf("ver:%s\n", ver); + +#if TEST_COMPUTE_CALIB_PARA + char _calib_datafile[256]; + sprintf_s(_calib_datafile, "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢2/璋冨钩鏁版嵁.txt"); + int lineNum = 0; + float lineV = 0.0f; + int dataCalib = 0; + int maxTimeStamp = 0; + int clockPerSecond = 0; + std::vector> scanData; + vzReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanData); + + lineNum = (int)scanData.size(); + if (scanData.size() > 0) + { + SSG_planeCalibPara calibPara = wd_getGroundCalibPara(scanData); + //缁撴灉杩涜楠岃瘉 + for (int i = 0; i < lineNum; i++) + { + if (i == 14) + int kkk = 1; + //琛屽鐞 + //璋冨钩锛屽幓闄ゅ湴闈 + wd_lineDataR(scanData[i], calibPara.planeCalib, -1); + } + // + char calibFile[250]; + sprintf_s(calibFile, "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢2/ground_calib_para.txt"); + _outputCalibPara(calibFile, calibPara); + char _out_file[256]; + sprintf_s(_out_file, "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/宸ヤ欢2/scanData_ground_calib_verify.txt"); + int headNullLines = 0; + _outputScanDataFile_vector(_out_file, scanData, false, &headNullLines); + printf("%s: calib done!\n", _calib_datafile); + } +#endif + +#if TEST_COMPUTE_HOLE + for (int grp = 0; grp < TPF5_TEST_GROUP; grp++) + { + SSG_planeCalibPara groundCalibPara; + //鍒濆鍖栨垚鍗曚綅闃 + groundCalibPara.planeCalib[0] = 1.0; + groundCalibPara.planeCalib[1] = 0.0; + groundCalibPara.planeCalib[2] = 0.0; + groundCalibPara.planeCalib[3] = 0.0; + groundCalibPara.planeCalib[4] = 1.0; + groundCalibPara.planeCalib[5] = 0.0; + groundCalibPara.planeCalib[6] = 0.0; + groundCalibPara.planeCalib[7] = 0.0; + groundCalibPara.planeCalib[8] = 1.0; + groundCalibPara.planeHeight = -1.0; + for (int i = 0; i < 9; i++) + groundCalibPara.invRMatrix[i] = groundCalibPara.planeCalib[i]; + char calibFile[250]; + sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]); + //groundCalibPara = _readCalibPara(calibFile); + + for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) + { + //fidx =2; + char _scan_file[256]; + sprintf_s(_scan_file, "%s%d_LaserData_Jh26B074.txt", dataPath[grp], fidx); + std::vector> scanLines; + vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + _removeZeroLines(scanLines); + if (scanLines.size() == 0) + continue; + + long t1 = (long)GetTickCount64();//缁熻鏃堕棿 + + SSG_lineSegParam lineSegPara; + lineSegPara.distScale = 3.0; + lineSegPara.segGapTh_y = 15.0; // + lineSegPara.segGapTh_z = 0.0; //z鏂瑰悜闂撮殧澶т簬10mm璁や负鏄垎娈 + SSG_outlierFilterParam filterParam; + filterParam.continuityTh = 5.0; //鍣０婊ら櫎銆傚綋鐩搁偦鐐圭殑z璺冲彉澶т簬姝ら棬闄愭椂锛屾鏌ユ槸鍚︿负鍣０銆傝嫢闀垮害灏忎簬outlierLen锛 瑙嗕负鍣０ + filterParam.outlierTh = 5; + SSG_treeGrowParam growParam; + growParam.maxLineSkipNum = 2; + growParam.yDeviation_max = 4.0; + growParam.maxSkipDistance = 0.0; + growParam.zDeviation_max = 10.0;// + growParam.minLTypeTreeLen = 2.0; //mm + growParam.minVTypeTreeLen = 2.0; //mm + WD_workpieceHoleParam workpiecePara; + workpiecePara.workpieceType = 0; + workpiecePara.holeDiameter = 10.0; // + workpiecePara.holeDist_W = 32.0; + workpiecePara.holeDist_L = 40.0; + workpiecePara.xLen = 44; + workpiecePara.yLen = 70; + workpiecePara.H = 28; + int errCode = 0; + std::vector< WD_workpieceInfo> workpiecePositioning; + wd_workpieceHolePositioning_2( + scanLines, + workpiecePara, + lineSegPara, + filterParam, + growParam, + groundCalibPara, + workpiecePositioning, + &errCode); + long t2 = (long)GetTickCount64(); + if (errCode == SX_ERR_UNKNOWN_OBJECT) + printf("%s: %d(ms), 鏈夊紓鐗╂畫鐣!\n", _scan_file, (int)(t2 - t1)); + else if (errCode == SX_ERR_ZERO_OBJECTS) + printf("%s: %d(ms), 鏃犱骇鍝!\n", _scan_file, (int)(t2 - t1)); + else + printf("%s: %d(ms), errCode=%d ...", _scan_file, (int)(t2 - t1), errCode); + //杈撳嚭娴嬭瘯缁撴灉 + sprintf_s(_scan_file, "%sresult/LaserLine%d_result.txt", dataPath[grp], fidx); + _outputRGBDResult_RGBD(_scan_file, scanLines, workpiecePositioning); + sprintf_s(calibFile, "%sresult/LaserLine%d_hole_info.txt", dataPath[grp], fidx); + _outputWorkpieceInfo(calibFile, workpiecePositioning); + + sprintf_s(calibFile, "%sresult/LaserLine%d_holes_projection.png", dataPath[grp], fidx); + double rpy[3] = { -30, 15, 0 }; //{ 0,-45, 0 }; // + double angleDrawLen = 15; + double displayScale = 0.3; + cv::String imgName(calibFile); + _genXOYProjectionImage(imgName, scanLines, displayScale, workpiecePositioning, rpy, angleDrawLen); + printf("done\n"); } } #endif @@ -1775,7 +2690,11 @@ void JiuRui_workpiecePosition_test(void) typedef enum { - keSG_鎷撴櫘鍙戝瓟瀹氫綅 = 0, + keSG_鎷撴櫘鍙戝瓟瀹氫綅_宸ヤ欢1 = 0, + keSG_鎷撴櫘鍙戝瓟瀹氫綅_宸ヤ欢2, + keSG_鎷撴櫘鍙戝瓟瀹氫綅_宸ヤ欢3, + keSG_鎷撴櫘鍙戝瓟瀹氫綅_宸ヤ欢4, + keSG_鎷撴櫘鍙戝瓟瀹氫綅_宸ヤ欢5, keSG_鍗庤埅瀛斿畾浣, keSG_寰姏鐮傝疆鐩樺畾浣, keSG_寰姏鐮傝疆鏋跺瓙瀹氫綅, @@ -1785,16 +2704,28 @@ typedef enum } ESG_testMode; int main() { - //ESG_testMode testMode = keSG_鎷撴櫘鍙戝瓟瀹氫綅; + //ESG_testMode testMode = keSG_鎷撴櫘鍙戝瓟瀹氫綅_宸ヤ欢1; + //ESG_testMode testMode = keSG_鎷撴櫘鍙戝瓟瀹氫綅_宸ヤ欢2; + //ESG_testMode testMode = keSG_鎷撴櫘鍙戝瓟瀹氫綅_宸ヤ欢3; + //ESG_testMode testMode = keSG_鎷撴櫘鍙戝瓟瀹氫綅_宸ヤ欢4; + ESG_testMode testMode = keSG_鎷撴櫘鍙戝瓟瀹氫綅_宸ヤ欢5; //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) + if (keSG_鎷撴櫘鍙戝瓟瀹氫綅_宸ヤ欢1 == testMode) TuoPuFa_holePosition_test(); + else if (keSG_鎷撴櫘鍙戝瓟瀹氫綅_宸ヤ欢2 == testMode) + TuoPuFa_holePosition_test2(); + else if (keSG_鎷撴櫘鍙戝瓟瀹氫綅_宸ヤ欢3 == testMode) + TuoPuFa_holePosition_test3(); + else if (keSG_鎷撴櫘鍙戝瓟瀹氫綅_宸ヤ欢4 == testMode) + TuoPuFa_holePosition_test4(); + else if (keSG_鎷撴櫘鍙戝瓟瀹氫綅_宸ヤ欢5 == testMode) + TuoPuFa_holePosition_test5(); else if (keSG_鍗庤埅瀛斿畾浣 == testMode) HuaHang_holePosition_test(); else if (keSG_寰姏鐮傝疆鐩樺畾浣 == testMode)