diff --git a/beltTearingDetection/beltTearingDetection.vcxproj b/beltTearingDetection/beltTearingDetection.vcxproj index 7f4d0c7..45938b9 100644 --- a/beltTearingDetection/beltTearingDetection.vcxproj +++ b/beltTearingDetection/beltTearingDetection.vcxproj @@ -48,13 +48,13 @@ DynamicLibrary true - v142 + v143 Unicode DynamicLibrary false - v142 + v143 true Unicode diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index 6df613f..e34d023 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -139,6 +139,12 @@ SG_APISHARED_EXPORT void wd_getLineCornerFeature_PSM( const SSG_cornerParam cornerPara, SSG_lineFeature* line_features); +SG_APISHARED_EXPORT void wd_computeDirAngle_wholeLine( + std::vector< SVzNL3DPosition>& line_data, + const SSG_cornerParam cornerPara, + std::vector< SSG_pntDirAngle>& ptDirAngles +); + /// 提取激光线上的圆环的上半段弧。宽度由圆环的宽度确定 /// seg端点：z距离大于门限 /// nPointIdx被重新定义成Feature类型 diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index 8d746a4..34cd059 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -3969,7 +3969,7 @@ void _computeDirAngle_perSeg( } //计算方向角(不分段） -void _computeDirAngle_wholeLine( +void wd_computeDirAngle_wholeLine( std::vector< SVzNL3DPosition>& line_data, const SSG_cornerParam cornerPara, std::vector< SSG_pntDirAngle>& ptDirAngles @@ -4309,7 +4309,7 @@ void wd_getRodArcFeature_peakCornerMethod( double arcTotalCornerMinValue = 45; //整个Arc的转角最小值 //计算前向角和后向角 std::vector< SSG_pntDirAngle> ptDirAngles; - _computeDirAngle_wholeLine( lineData, cornerPara, ptDirAngles); + wd_computeDirAngle_wholeLine( lineData, cornerPara, ptDirAngles); int dataSize = (int)lineData.size(); //搜索z极值。 diff --git a/sourceCode/workpieceHolePositioning.cpp b/sourceCode/workpieceHolePositioning.cpp index bd74599..acc285f 100644 --- a/sourceCode/workpieceHolePositioning.cpp +++ b/sourceCode/workpieceHolePositioning.cpp @@ -10,7 +10,8 @@ //version 1.1.0 : c瀵瑰伐浠跺Э鎬佽鑼冨寲涓轰腑蹇冪偣锛堟搷浣滅偣锛夊姞涓変釜鏂瑰悜鐭㈤噺 //version 1.2.0 : 绠楁硶瀹屾垚浜6杞撮獙璇 //version 1.3.0 : (1)绠楁硶杩涜浜嗚凯浠 (2)瀵圭粨鏋滆繘琛屼簡鍒嗗眰鍜屾帓搴忥紝杈撳嚭鏈涓婂眰鐩爣 -std::string m_strVersion = "1.3.0"; +//version 1.4.0 : 娣诲姞浜嗗崕鑸瓟瀹氫綅鍔熻兘 +std::string m_strVersion = "1.4.0"; const char* wd_workpieceHolePositioningVersion(void) { return m_strVersion.c_str(); @@ -299,89 +300,19 @@ void _getYTopLine( std::sort(firstLine.begin(), firstLine.end(), _compareByXValue); return; } -//宸ヤ欢瀛斿畾浣 -void wd_workpieceHolePositioning( - std::vector< std::vector>& scanLinesInput, - const WD_workpieceHoleParam workpiecePara, + +void wd_getHoleInfo( + std::vector< std::vector>& scanLines, const SSG_lineSegParam lineSegPara, const SSG_outlierFilterParam filterParam, const SSG_treeGrowParam growParam, - const SSG_planeCalibPara groundCalibPara, - std::vector< WD_workpieceInfo>& workpiecePositioning, - int* errCode) + std::vector& segTrees_v, + std::vector& segTrees_h, + std::vector& validObjects +) { - *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]; - } - } + int lineNum = (int)scanLines.size(); + int linePtNum = (int)scanLines[0].size(); std::vector> pointMask; pointMask.resize(lineNum); @@ -421,7 +352,6 @@ void wd_workpieceHolePositioning( holeGaps.push_back(line_gaps); } //鐗瑰緛鐢熼暱 - std::vector segTrees_v; wd_getSegFeatureGrowingTrees_2(holeGaps, segTrees_v, growParam); //鐢熸垚姘村钩鎵弿 @@ -471,7 +401,6 @@ void wd_workpieceHolePositioning( holeGaps_h.push_back(line_gaps); } //鐗瑰緛鐢熼暱 - std::vector segTrees_h; wd_getSegFeatureGrowingTrees_2(holeGaps_h, segTrees_h, growParam); //鍒涘缓Tree鎵鍦ㄥ瓟娲炵殑Mask @@ -497,9 +426,9 @@ void wd_workpieceHolePositioning( 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}; + treeInfo_v[i].roi = { -1, -1, -1, -1 }; int nullPtSize = 0; - SSG_ROIRectD roi = {-1, -1, -1, -1}; + SSG_ROIRectD roi = { -1, -1, -1, -1 }; SVzNLRange ptIdxRange = { -1, -1 }; if (a_tree.treeNodes.size() > 2) { @@ -648,7 +577,7 @@ void wd_workpieceHolePositioning( } } } - std::vector validObjects; + for (int i = 0; i < (int)objects.size(); i++) { int vTreeIdx = objects[i].data_0; @@ -657,6 +586,96 @@ void wd_workpieceHolePositioning( validObjects.push_back(objects[i]); } +} + +//宸ヤ欢瀛斿畾浣-鎷撴櫘鍙戝伐浠跺瓟瀹氫綅 +void wd_workpieceHolePositioning( + 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; + wd_getHoleInfo(scanLines, lineSegPara, filterParam, growParam, segTrees_v, segTrees_h, validObjects); //鐢熸垚鑱氱被淇℃伅锛 std::vector> clusters; //鍙褰曚綅缃 @@ -921,4 +940,445 @@ void wd_workpieceHolePositioning( } return; +} + +typedef struct +{ + int flag; + SWDIndexing3DPoint zMaxPos; + SVzNLRect peakROI; + double pkValue; +}_zMaxInfo; + +bool _getZMaxPeakROI(SVzNL2DPoint seedPos, + std::vector>& pntDirAngles_v, + std::vector>& pntDirAngles_h, + SVzNLRect& roi +) +{ + int lineNum = (int)pntDirAngles_v.size(); + int ptNum = (int)pntDirAngles_h.size(); + + //鍚戝乏鎼滅储 + int left = -1; + for (int line = seedPos.x; line >= 0; line--) + { + if ((pntDirAngles_v[line][seedPos.y].type >= 0) && (abs(pntDirAngles_v[line][seedPos.y].corner) < 5.0)) + { + left = line; + break; + } + } + //鍚戝彸鎼滅储 + int right = -1; + for (int line = seedPos.x; line < lineNum; line++) + { + if ((pntDirAngles_v[line][seedPos.y].type >= 0) && (abs(pntDirAngles_v[line][seedPos.y].corner) < 5.0)) + { + right = line; + break; + } + } + //鍚戜笂鎼滅储 + int top = -1; + for (int ptIdx = seedPos.y; ptIdx >= 0; ptIdx--) + { + if ((pntDirAngles_h[ptIdx][seedPos.x].type >= 0) && (abs(pntDirAngles_h[ptIdx][seedPos.x].corner) < 5.0)) + { + top = ptIdx; + break; + } + } + //鍚戜笅鎼滅储 + int bottom = -1; + for (int ptIdx = seedPos.y; ptIdx < ptNum; ptIdx++) + { + if ((pntDirAngles_h[ptIdx][seedPos.x].type >= 0) && (abs(pntDirAngles_h[ptIdx][seedPos.x].corner) < 5.0)) + { + bottom = ptIdx; + break; + } + } + if ((left < 0) || (right < 0) || (top < 0) || (bottom < 0)) + return false; + roi.left = left; + roi.right = right; + roi.top = top; + roi.bottom = bottom; + return true; +} + +SVzNLRect _mergeROI(SVzNLRect roi_1, SVzNLRect roi_2) +{ + SVzNLRect roi; + roi.left = roi_1.left < roi_2.left ? roi_1.left : roi_2.left; + roi.right = roi_1.right > roi_2.right ? roi_1.right : roi_2.right; + roi.top = roi_1.top < roi_2.top ? roi_1.top : roi_2.top; + roi.bottom = roi_1.bottom > roi_2.bottom ? roi_1.bottom : roi_2.bottom; + return roi; +} + +//鍗曚釜瀛旀垨鍑瑰潙瀹氫綅-鍗庤埅瀛斿畾浣 +void wd_HolePositioning( + std::vector< std::vector>& scanLinesInput, + const SSG_lineSegParam lineSegPara, + const SSG_cornerParam cornerParam, + const SSG_outlierFilterParam filterParam, + const SSG_treeGrowParam growParam, + std::vector< WD_HolePositionInfo>& holePositioning, + 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; + } + + //鍐呴儴鍙傛暟 + double zPeakScale = 25.0; //璁＄畻ZPeak鏃剁殑灏哄害 + double minZPeakHeight = 2.5; //鏈灏忕殑鍑瑰潙娣卞害 + double planeInlierDistTh = 1.0; //骞抽潰鐐硅窛绂诲钩闈㈢殑璺濈銆傝秴鍑烘璺濈琚垽鍒负绂荤兢鐐 + + //璁＄畻dirAngle + std::vector> pntDirAngles_v; + std::vector> zMaxPeaks_v; + for (int line = 0; line < lineNum; line++) + { + if (line == 1047) + int kkk = 1; + + std::vector& lineData = scanLines[line]; + //婊ゆ尝锛屾护闄ゅ紓甯哥偣 + sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam); + + std::vector< SSG_pntDirAngle> line_ptDirAngles; + wd_computeDirAngle_wholeLine(lineData, cornerParam, line_ptDirAngles); + pntDirAngles_v.push_back(line_ptDirAngles); + + std::vector< SSG_basicFeature1D> localZMax; + std::vector< SSG_basicFeature1D> localZMin; + sg_getLineLocalPeaks_2( lineData, line, zPeakScale, localZMax, localZMin); + zMaxPeaks_v.push_back(localZMax); + } + + //鐢熸垚姘村钩鎵弿 + 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鐗瑰緛鎻愬彇 + //鍒涘缓姘村钩ZMax鐨凪ask + std::vector> zMaxMask_h; + zMaxMask_h.resize(lineNum); + for (int i = 0; i < lineNum; i++) + { + zMaxMask_h[i].resize(linePtNum); + std::fill(zMaxMask_h[i].begin(), zMaxMask_h[i].end(), 0); + } + + std::vector> pntDirAngles_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< SSG_pntDirAngle> line_ptDirAngles; + wd_computeDirAngle_wholeLine(lineData, cornerParam, line_ptDirAngles); + pntDirAngles_h.push_back(line_ptDirAngles); + + std::vector< SSG_basicFeature1D> localZMax; + std::vector< SSG_basicFeature1D> localZMin; + sg_getLineLocalPeaks_2(lineData, line, zPeakScale, localZMax, localZMin); + for (int j = 0; j < (int)localZMax.size(); j++) + { + int idx_line = localZMax[j].jumpPos2D.y; + int idx_pt = localZMax[j].jumpPos2D.x; + zMaxMask_h[idx_line][idx_pt] = 1; + } + } + + //鍒ゆ柇鐪熸鐨刏Max骞惰褰 + //锛1锛夌湡姝ｇ殑ZMax涓瀹氭槸鍚屾椂涓篨鏂瑰悜鐨刏Max鍜孻鏂瑰悜鐨刏Max.(2)婊¤冻涓瀹氱殑Z宸 + std::vector<_zMaxInfo> objPeaks; + for (int line = 0; line < lineNum; line++) + { + std::vector& a_lineZMax = zMaxPeaks_v[line]; + for (int j = 0; j < (int)a_lineZMax.size(); j++) + { + int idx_line = a_lineZMax[j].jumpPos2D.x; + int idx_pt = a_lineZMax[j].jumpPos2D.y; + if (zMaxMask_h[idx_line][idx_pt] > 0) //鍦╔鍜孻鏂瑰悜鍧囦负鏋佸ぇ鍊 + { + //鍒ゆ柇ROI + SVzNLRect a_roi; + bool validPk = _getZMaxPeakROI(a_lineZMax[j].jumpPos2D, pntDirAngles_v, pntDirAngles_h, a_roi); + if (true == validPk) + { + double meanZ = scanLines[a_roi.left][idx_pt].pt3D.z; + meanZ += scanLines[a_roi.right][idx_pt].pt3D.z; + meanZ += scanLines[idx_line][a_roi.top].pt3D.z; + meanZ += scanLines[idx_line][a_roi.bottom].pt3D.z; + meanZ = meanZ / 4.0; + double z_diff = a_lineZMax[j].jumpPos.z - meanZ; + if (z_diff > minZPeakHeight) + { + _zMaxInfo a_info; + a_info.flag = 0; + a_info.peakROI = a_roi; + a_info.zMaxPos.lineIdx = a_lineZMax[j].jumpPos2D.x; + a_info.zMaxPos.ptIdx = a_lineZMax[j].jumpPos2D.y; + a_info.zMaxPos.point = { a_lineZMax[j].jumpPos.x, a_lineZMax[j].jumpPos.y, a_lineZMax[j].jumpPos.z }; + a_info.pkValue = a_lineZMax[j].jumpPos.z; + objPeaks.push_back(a_info); + } + } + } + } + } + int pkSize = (int)objPeaks.size(); + for (int i = 0; i < pkSize; i++) + { + _zMaxInfo a_info = objPeaks[i]; + if (a_info.flag < 0) + continue; + + for (int j = i + 1; j < pkSize; j++) + { + if (objPeaks[j].flag < 0) + continue; + + if ((a_info.peakROI.right >= objPeaks[j].peakROI.left) && (objPeaks[j].peakROI.right >= a_info.peakROI.left) && + (a_info.peakROI.bottom >= objPeaks[j].peakROI.top) && (objPeaks[j].peakROI.bottom >= a_info.peakROI.top)) //閲嶅彔 + { + a_info.peakROI = _mergeROI(a_info.peakROI, objPeaks[j].peakROI); + if (a_info.pkValue < objPeaks[j].pkValue) + { + a_info.pkValue = objPeaks[j].pkValue; + a_info.zMaxPos = objPeaks[j].zMaxPos; + objPeaks[j].flag = -1; + } + } + } + objPeaks[i] = a_info; + } + //閫愪釜鐩爣鎻愬彇 + int contourWin = 4; //鍙栧懆鍥3琛屽拰3鍒楃殑鐐 + for (int peakIdx = 0; peakIdx < pkSize; peakIdx++) + { + if (objPeaks[peakIdx].flag < 0) + continue; + + SVzNLRect& a_roi = objPeaks[peakIdx].peakROI; + //鎻愬彇瀛斿懆鍥寸偣 + SVzNLRect extend_roi = { a_roi.left - contourWin, a_roi.right + contourWin, a_roi.top - contourWin , a_roi.bottom + contourWin }; + if (extend_roi.left < 0) extend_roi.left = 0; + if (extend_roi.right >= lineNum) extend_roi.right = lineNum - 1; + if (extend_roi.top < 0) extend_roi.top = 0; + if (extend_roi.bottom >= linePtNum) extend_roi.bottom = linePtNum - 1; + + std::vector Points3ds; + //宸 + for (int line = extend_roi.left; line <= a_roi.left-1; line++) + { + for (int j = a_roi.top; j <= a_roi.bottom; j++) + { + if (scanLines[line][j].pt3D.z > 1e-4) + { + cv::Point3f a_pt = cv::Point3f(scanLines[line][j].pt3D.x , scanLines[line][j].pt3D.y, scanLines[line][j].pt3D.z); + Points3ds.push_back(a_pt); + } + } + } + //鍙 + for (int line = a_roi.right+1; line <=extend_roi.right; line++) + { + for (int j = a_roi.top; j <= a_roi.bottom; j++) + { + if (scanLines[line][j].pt3D.z > 1e-4) + { + cv::Point3f a_pt = cv::Point3f(scanLines[line][j].pt3D.x, scanLines[line][j].pt3D.y, scanLines[line][j].pt3D.z); + Points3ds.push_back(a_pt); + } + } + } + //涓 + for (int ptIdx = extend_roi.top; ptIdx <= a_roi.top-1; ptIdx++) + { + for (int j = a_roi.left; j <= a_roi.right; j++) + { + if (scanLines[j][ptIdx].pt3D.z > 1e-4) + { + cv::Point3f a_pt = cv::Point3f(scanLines[j][ptIdx].pt3D.x, scanLines[j][ptIdx].pt3D.y, scanLines[j][ptIdx].pt3D.z); + Points3ds.push_back(a_pt); + } + } + } + //涓 + for (int ptIdx = a_roi.bottom+1; ptIdx < extend_roi.bottom; ptIdx++) + { + for (int j = a_roi.left; j <= a_roi.right; j++) + { + if (scanLines[j][ptIdx].pt3D.z > 1e-4) + { + cv::Point3f a_pt = cv::Point3f(scanLines[j][ptIdx].pt3D.x, scanLines[j][ptIdx].pt3D.y, scanLines[j][ptIdx].pt3D.z); + Points3ds.push_back(a_pt); + } + } + } + //璁＄畻闈㈠弬鏁: z = Ax + By + C , res: [0]=A, [1]= B, [2]=-1.0, [3]=C, + std::vector res; + vzCaculateLaserPlane(Points3ds, res); + double normValue = sqrt(pow(res[0], 2) + pow(res[1], 2) + pow(res[2],2)); + double norm_A = res[0] / normValue; + double norm_B = res[1] / normValue; + double norm_C = res[2] / normValue; + double norm_D = res[3] / normValue; + //鐢熸垚ROI scanLines + int roi_lineNum = extend_roi.right - extend_roi.left + 1; + int roi_ptNum = extend_roi.bottom - extend_roi.top + 1; + std::vector< std::vector> roi_scanLines; + roi_scanLines.resize(roi_lineNum); + for (int j = 0; j < roi_lineNum; j++) + roi_scanLines[j].resize(roi_ptNum); + + //鐢熸垚闈笂鐐 + for (int line = extend_roi.left; line <= extend_roi.right; line++) + { + int roi_line = line - extend_roi.left; + for (int ptIdx = extend_roi.top; ptIdx <= extend_roi.bottom; ptIdx++) + { + int roi_ptIdx = ptIdx - extend_roi.top; + roi_scanLines[roi_line][roi_ptIdx] = scanLines[line][ptIdx]; + //鏈澶栧湀涓嶈繘琛屽鐞 + if ((line != extend_roi.left) && (line != extend_roi.right) && (ptIdx != extend_roi.top) && (ptIdx != extend_roi.bottom)) + { + if (scanLines[line][ptIdx].pt3D.z > 1e-4) + { + SVzNL3DPoint& a_pt3D = scanLines[line][ptIdx].pt3D; + double dist = abs(norm_A * a_pt3D.x + norm_B * a_pt3D.y + norm_C * a_pt3D.z + norm_D); + if (dist > planeInlierDistTh) + roi_scanLines[roi_line][roi_ptIdx].pt3D = { 0, 0, 0 }; + } + } + } + } + + std::vector segTrees_v; + std::vector segTrees_h; + std::vector validObjects; + wd_getHoleInfo(roi_scanLines, lineSegPara, filterParam, growParam, segTrees_v, segTrees_h, validObjects); + if (validObjects.size() > 0) + { + SSG_intPair a_hvPair = validObjects[0]; + if (validObjects.size() > 1) + { + for (int j = 1; j < (int)validObjects.size(); j++) + { + if (validObjects[j].idx < validObjects[j].idx) + a_hvPair = validObjects[j]; + } + } + //鐢熸垚Contour + std::vector cluster_pointArray; + int vTreeIdx = a_hvPair.data_0; + int hTreeIdx = a_hvPair.data_1; + for (int m = 0; m < (int)segTrees_v[vTreeIdx].treeNodes.size(); m++) + { + SWD_segFeature& a_seg = segTrees_v[vTreeIdx].treeNodes[m]; + if (roi_scanLines[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx == 0) + { + roi_scanLines[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = vTreeIdx + 1; // 0x01; + SVzNL3DPoint a_pos = roi_scanLines[a_seg.lineIdx][a_seg.endPtIdx].pt3D; + cluster_pointArray.push_back(a_pos); + } + if (roi_scanLines[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx == 0) + { + roi_scanLines[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = vTreeIdx + 1; // 0x01; + SVzNL3DPoint a_pos = roi_scanLines[a_seg.lineIdx][a_seg.startPtIdx].pt3D; + cluster_pointArray.push_back(a_pos); + } + } + for (int m = 0; m < (int)segTrees_h[hTreeIdx].treeNodes.size(); m++) + { + SWD_segFeature& a_seg = segTrees_h[hTreeIdx].treeNodes[m]; + if (roi_scanLines[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx == 0) + { + roi_scanLines[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02; + SVzNL3DPoint a_pos = roi_scanLines[a_seg.startPtIdx][a_seg.lineIdx].pt3D; + cluster_pointArray.push_back(a_pos); + } + if (roi_scanLines[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx == 0) + { + roi_scanLines[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02; + SVzNL3DPoint a_pos = roi_scanLines[a_seg.endPtIdx][a_seg.lineIdx].pt3D; + cluster_pointArray.push_back(a_pos); + } + } + //璁＄畻閲嶅績 +#if 0 + SVzNL3DPoint center; + double radius; + double err = fitCircleByLeastSquare(cluster_pointArray, center, radius); +#endif + if (cluster_pointArray.size() > 0) + { + SVzNL3DPoint center = { 0, 0, 0 }; + int ptSize = (int)cluster_pointArray.size(); + for (int m = 0; m < ptSize; m++) + { + center.x += cluster_pointArray[m].x; + center.y += cluster_pointArray[m].y; + center.z += cluster_pointArray[m].z; + } + center.x = center.x / ptSize; + center.y = center.y / ptSize; + center.z = center.z / ptSize; + //姹傚湪骞抽潰涓婄殑鍨傝冻 + double t = -(center.x * norm_A + center.y * norm_B + center.z * norm_C + norm_D); + SVzNL3DPoint realCenter; + realCenter.x = center.x + t * norm_A; + realCenter.y = center.y + t * norm_B; + realCenter.z = center.z + t * norm_C; + WD_HolePositionInfo a_hole; + a_hole.center = realCenter; + a_hole.normDir = { norm_A, norm_B, norm_C }; + holePositioning.push_back(a_hole); + } + } + + } + return; } \ No newline at end of file diff --git a/sourceCode/workpieceHolePositioning_Export.h b/sourceCode/workpieceHolePositioning_Export.h index d632b8e..8b32030 100644 --- a/sourceCode/workpieceHolePositioning_Export.h +++ b/sourceCode/workpieceHolePositioning_Export.h @@ -27,6 +27,12 @@ typedef struct SVzNL3DPoint x_dir; //x方向向量（归一化） }WD_workpieceInfo; +typedef struct +{ + SVzNL3DPoint center; + SVzNL3DPoint normDir; //方向向量（归一化） +}WD_HolePositionInfo; + //读版本号 SG_APISHARED_EXPORT const char* wd_workpieceHolePositioningVersion(void); @@ -41,7 +47,7 @@ SG_APISHARED_EXPORT void wd_lineDataR( const double* camPoseR, double groundH); -//工件孔定位 +//工件孔定位-拓普发工件孔定位 SG_APISHARED_EXPORT void wd_workpieceHolePositioning( std::vector< std::vector>& scanLinesInput, const WD_workpieceHoleParam workpiecePara, @@ -51,3 +57,13 @@ SG_APISHARED_EXPORT void wd_workpieceHolePositioning( const SSG_planeCalibPara groundCalibPara, std::vector< WD_workpieceInfo>& workpiecePositioning, int* errCode); + +//单个孔或凹坑定位-华航孔定位 +SG_APISHARED_EXPORT void wd_HolePositioning( + std::vector< std::vector>& scanLinesInput, + const SSG_lineSegParam lineSegPara, + const SSG_cornerParam cornerParam, + const SSG_outlierFilterParam filterParam, + const SSG_treeGrowParam growParam, + std::vector< WD_HolePositionInfo>& holePositioning, + int* errCode); \ No newline at end of file diff --git a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp index d7f2397..ce98551 100644 --- a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp +++ b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp @@ -164,6 +164,24 @@ void _outputWorkpieceInfo(char* fileName, std::vector< WD_workpieceInfo>& workpi sw.close(); } +void _outputHoleInfo(char* fileName, std::vector< WD_HolePositionInfo>& holePositioning) +{ + std::ofstream sw(fileName); + char dataStr[250]; + + int number = (int)holePositioning.size(); + for (int i = 0; i < number; i++) + { + sprintf_s(dataStr, 250, "瀛擾%d", i + 1); + sw << dataStr << std::endl; + sprintf_s(dataStr, 50, " center: (%g, %g, %g)", holePositioning[i].center.x, holePositioning[i].center.y, holePositioning[i].center.z); + sw << dataStr << std::endl; + sprintf_s(dataStr, 50, " norm_dir: (%g, %g, %g)", holePositioning[i].normDir.x, holePositioning[i].normDir.y, holePositioning[i].normDir.z); + sw << dataStr << std::endl; + } + sw.close(); +} + void _outputScanDataFile_vector(char* fileName, std::vector>& scanLines, bool removeZeros, int* headNullLines) { std::ofstream sw(fileName); @@ -410,6 +428,117 @@ void _outputRGBDResult_RGBD( sw.close(); } +void _outputRGBDResult_HoleInfo( + char* fileName, + std::vector>& scanLines, + std::vector< WD_HolePositionInfo>& holePositioning) +{ + std::vector objects; + int objNumber = (int)holePositioning.size(); + for (int i = 0; i < objNumber; i++) + { + SVzNL3DPosition a_objPt; + a_objPt.pt3D = holePositioning[i].center; + objects.push_back(a_objPt); + } + + int lineNum = (int)scanLines.size(); + std::ofstream sw(fileName); + int realLines = (objNumber == 0) ? lineNum : (lineNum + 1); + sw << "LineNum:" << realLines << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed: 0" << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp: 0_0" << std::endl; + + int maxLineIndex = 0; + int max_stamp = 0; + SG_color rgb = { 0, 0, 0 }; + SG_color 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; + int lineIdx = 0; + for (int line = 0; line < lineNum; line++) + { + int linePtNum = (int)scanLines[line].size(); + if (linePtNum == 0) + continue; + + sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl; + lineIdx++; + for (int i = 0; i < linePtNum; i++) + { + SVzNL3DPosition* pt3D = &scanLines[line][i]; + if (pt3D->nPointIdx > 0) + int kkk = 1; + int flag = pt3D->nPointIdx & 0xffff; + if (flag > 0) + { + rgb = objColor[flag % 8]; // { 255, 97, 0 }; + size = 5; + } + else + { + rgb = { 200, 200, 200 }; + size = 1; + } + float x = (float)pt3D->pt3D.x; + float y = (float)pt3D->pt3D.y; + float z = (float)pt3D->pt3D.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + } + + int linePtNum = (int)objects.size(); + sw << "Line_" << lineNum << "_0_" << linePtNum + 1 << std::endl; + lineNum++; + size = 10; + for (int i = 0; i < linePtNum; i++) + { + rgb = {255, 0, 0}; + float x = (float)objects[i].pt3D.x; + float y = (float)objects[i].pt3D.y; + float z = (float)objects[i].pt3D.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + //杈撳嚭鏂瑰悜绾挎潯 + rgb = { 255, 0, 0 }; + size = 2; + for (int i = 0; i < objNumber; i++) + { + SVzNL3DPoint dirPt_1, dirPt_2; + dirPt_1 = { holePositioning[i].center.x - holePositioning[i].normDir.x * 20, + holePositioning[i].center.y - holePositioning[i].normDir.y * 20, + holePositioning[i].center.z - holePositioning[i].normDir.z * 20 }; + dirPt_2 = { holePositioning[i].center.x + holePositioning[i].normDir.x * 10, + holePositioning[i].center.y + holePositioning[i].normDir.y * 10, + holePositioning[i].center.z + holePositioning[i].normDir.z * 10 }; + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << dirPt_1.x << "," << dirPt_1.y << "," << dirPt_1.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << dirPt_2.x << "," << dirPt_2.y << "," << dirPt_2.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + } + + sw.close(); +} + + SVzNL3DPoint _pointRT(SVzNL3DPoint& origin, const double* R, const double* T) { SVzNL3DPoint result; @@ -425,17 +554,17 @@ SVzNL3DPoint _pointRT(SVzNL3DPoint& origin, const double* R, const double* T) #define TEST_COMPUTE_CALIB_PARA 0 #define TEST_COMPUTE_HOLE 1 #define TEST_COMPUTE_RT 0 -#define TEST_GROUP 2 -int main() +#define TPF_TEST_GROUP 2 +//鎷撴櫘鍙戝伐浠跺瓟瀹氫綅锛堝伐浠跺畾浣) +void TuoPuFa_holePosition_test(void) { - const char* dataPath[TEST_GROUP] = { + const char* dataPath[TPF_TEST_GROUP] = { "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/鎷撴櫘鍙戠偣浜/", //0 "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鎷撴櫘鍙戝伐浠跺瓟瀹氫綅/鎷撴櫘鍙戠偣浜2/", //1 - }; - SVzNLRange fileIdx[TEST_GROUP] = { + SVzNLRange fileIdx[TPF_TEST_GROUP] = { {6,6}, {1, 15} }; @@ -681,13 +810,92 @@ int main() #endif } -// 杩愯绋嬪簭: Ctrl + F5 鎴栬皟璇 >鈥滃紑濮嬫墽琛(涓嶈皟璇)鈥濊彍鍗 -// 璋冭瘯绋嬪簭: F5 鎴栬皟璇 >鈥滃紑濮嬭皟璇曗濊彍鍗 +//鍗庤埅瀛斿畾浣 +#define HH_TEST_GROUP 4 +void HuaHang_holePosition_test(void) +{ + const char* dataPath[HH_TEST_GROUP] = { -// 鍏ラ棬浣跨敤鎶宸: -// 1. 浣跨敤瑙ｅ喅鏂规璧勬簮绠＄悊鍣ㄧ獥鍙ｆ坊鍔/绠＄悊鏂囦欢 -// 2. 浣跨敤鍥㈤槦璧勬簮绠＄悊鍣ㄧ獥鍙ｈ繛鎺ュ埌婧愪唬鐮佺鐞 -// 3. 浣跨敤杈撳嚭绐楀彛鏌ョ湅鐢熸垚杈撳嚭鍜屽叾浠栨秷鎭 -// 4. 浣跨敤閿欒鍒楄〃绐楀彛鏌ョ湅閿欒 -// 5. 杞埌鈥滈」鐩>鈥滄坊鍔犳柊椤光濅互鍒涘缓鏂扮殑浠ｇ爜鏂囦欢锛屾垨杞埌鈥滈」鐩>鈥滄坊鍔犵幇鏈夐」鈥濅互灏嗙幇鏈変唬鐮佹枃浠舵坊鍔犲埌椤圭洰 -// 6. 灏嗘潵锛岃嫢瑕佸啀娆℃墦寮姝ら」鐩紝璇疯浆鍒扳滄枃浠垛>鈥滄墦寮鈥>鈥滈」鐩濆苟閫夋嫨 .sln 鏂囦欢 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鍗庤埅瀛斿畾浣/鍦嗗瓟璇嗗埆_鏁版嵁1/", //0 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鍗庤埅瀛斿畾浣/鍦嗗瓟璇嗗埆_鏁版嵁2/1/", //1 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鍗庤埅瀛斿畾浣/鍦嗗瓟璇嗗埆_鏁版嵁2/2/", //1 + "F:/ShangGu/椤圭洰/鍐犻挦椤圭洰/鍗庤埅瀛斿畾浣/鍦嗗瓟璇嗗埆_鏁版嵁2/3/", //1 + }; + + SVzNLRange fileIdx[HH_TEST_GROUP] = { + {1,15}, {1, 21},{1,19}, {1,9} + }; + + const char* ver = wd_workpieceHolePositioningVersion(); + printf("ver:%s\n", ver); + +#if TEST_COMPUTE_HOLE + for (int grp = 1; grp <= 3; grp++) + { + for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) + { + //fidx =4; + char _scan_file[256]; + sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx); + std::vector> scanLines; + vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + if (scanLines.size() == 0) + continue; + + long t1 = (long)GetTickCount64();//缁熻鏃堕棿 + + SSG_lineSegParam lineSegPara; + lineSegPara.distScale = 25.0; + lineSegPara.segGapTh_y = 15.0; // + lineSegPara.segGapTh_z = 0.0; //z鏂瑰悜闂撮殧澶т簬10mm璁や负鏄垎娈 + + SSG_cornerParam cornerParam; + cornerParam.cornerTh = 60; //45搴﹁ + cornerParam.scale = 4; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + cornerParam.minEndingGap = 20; // algoParam.bagParam.bagW / 4; + cornerParam.minEndingGap_z = 5.0; + cornerParam.jumpCornerTh_1 = 15; //姘村钩瑙掑害锛屽皬浜庢瑙掑害瑙嗕负姘村钩 + cornerParam.jumpCornerTh_2 = 60; + + SSG_outlierFilterParam filterParam; + filterParam.continuityTh = 20.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_HolePositionInfo> holePositioning; + wd_HolePositioning(scanLines, lineSegPara, cornerParam, filterParam, growParam, holePositioning, &errCode); + + long t2 = (long)GetTickCount64(); + printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1)); + //杈撳嚭娴嬭瘯缁撴灉 + sprintf_s(_scan_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx); + _outputRGBDResult_HoleInfo(_scan_file, scanLines, holePositioning); + sprintf_s(_scan_file, "%sresult\\LaserLine%d_corner_info.txt", dataPath[grp], fidx); + _outputHoleInfo(_scan_file, holePositioning); + } + } +#endif +} + +int main() +{ +#if 0 + TuoPuFa_holePosition_test(); +#else + HuaHang_holePosition_test(); +#endif +}