diff --git a/rodAndBarDetection_test/rodAndBarDetection_test.cpp b/rodAndBarDetection_test/rodAndBarDetection_test.cpp index 540060b..bd42f1b 100644 --- a/rodAndBarDetection_test/rodAndBarDetection_test.cpp +++ b/rodAndBarDetection_test/rodAndBarDetection_test.cpp @@ -780,16 +780,22 @@ void _outputRGBDScan_RGBD_weldSeam( int centerFlag = pt3D->nPointIdx >> 4; if (centerFlag > 0) { - rgb = { 180, 0, 0 }; - size = 2; + if (centerFlag <= 2) + { + rgb = { 180, 0, 0 }; + size = 2; + } + else if(centerFlag == 4) + { + rgb = { 0, 250, 0 }; + size = 4; + } } else { rgb = objColor[pt3D->nPointIdx % 8]; size = 2; } - - } else //if (pt3D->nPointIdx == 0) { @@ -807,52 +813,75 @@ void _outputRGBDScan_RGBD_weldSeam( if (objNum > 0) { - sw << "Line_" << lineIdx << "_0_" << objNum << std::endl; - size = 12; + std::vector weldPoints; for (int i = 0; i < objNum; i++) { - if (i == 0) - rgb = { 250, 255, 0 }; + if (weldSeamInfo[i].weldType == KeWD_WELD_POINT) + weldPoints.push_back(weldSeamInfo[i].center); else - rgb = { 250, 0, 0 }; - float x = (float)weldSeamInfo[i].center.x; - float y = (float)weldSeamInfo[i].center.y; - float z = (float)weldSeamInfo[i].center.z; + { + weldPoints.push_back(weldSeamInfo[i].startPt); + weldPoints.push_back(weldSeamInfo[i].center); + weldPoints.push_back(weldSeamInfo[i].endPt); + } + } + + sw << "Line_" << lineIdx << "_0_" << (int)weldPoints.size() << std::endl; + size = 15; + for (int i = 0; i < (int)weldPoints.size(); i++) + { + rgb = { 250, 0, 0 }; + float x = (float)weldPoints[i].x; + float y = (float)weldPoints[i].y; + float z = (float)weldPoints[i].z; sw << "{" << x << "," << y << "," << z << "}-"; sw << "{0,0}-{0,0}-"; sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; } //输出法向 - size = 8; - double len = 60; + size = 4; + double len1 = 20; + double len2 = 20; lineIdx = 0; for (int i = 0; i < objNum; i++) { - if (i == 0) - rgb = { 250, 255, 0 }; + SVzNL3DPoint pt0, pt1; + if (weldSeamInfo[i].weldType == KeWD_WELD_POINT) + { + pt0 = weldSeamInfo[i].center; + pt1 = { weldSeamInfo[i].center.x + len1 * weldSeamInfo[i].axialDir.x, + weldSeamInfo[i].center.y + len1 * weldSeamInfo[i].axialDir.y, + weldSeamInfo[i].center.z + len1 * weldSeamInfo[i].axialDir.z }; + } else - rgb = { 250, 0, 0 }; - SVzNL3DPoint pt0 = { weldSeamInfo[i].center.x, weldSeamInfo[i].center.y, weldSeamInfo[i].center.z }; - SVzNL3DPoint pt2 = { weldSeamInfo[i].center.x + len * weldSeamInfo[i].normalDir.x, - weldSeamInfo[i].center.y + len * weldSeamInfo[i].normalDir.y, - weldSeamInfo[i].center.z + len * weldSeamInfo[i].normalDir.z }; + { + pt0 = weldSeamInfo[i].startPt; + pt1 = weldSeamInfo[i].endPt; + } + + SVzNL3DPoint pt2 = { weldSeamInfo[i].center.x, weldSeamInfo[i].center.y, weldSeamInfo[i].center.z }; + SVzNL3DPoint pt3 = { weldSeamInfo[i].center.x + len2 * weldSeamInfo[i].normalDir.x, + weldSeamInfo[i].center.y + len2 * weldSeamInfo[i].normalDir.y, + weldSeamInfo[i].center.z + len2 * weldSeamInfo[i].normalDir.z }; //显示轴向量 - sw << "Poly_" << lineIdx << "_2" << std::endl; - sw << "{" << (float)weldSeamInfo[i].startPt.x << "," << (float)weldSeamInfo[i].startPt.y << "," << (float)weldSeamInfo[i].startPt.z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; - sw << "{" << (float)weldSeamInfo[i].endPt.x << "," << (float)weldSeamInfo[i].endPt.y << "," << (float)weldSeamInfo[i].endPt.z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; - lineIdx++; - //显示法向量 + rgb = { 0, 0, 250 }; sw << "Poly_" << lineIdx << "_2" << std::endl; sw << "{" << (float)pt0.x << "," << (float)pt0.y << "," << (float)pt0.z << "}-"; sw << "{0,0}-{0,0}-"; sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << (float)pt1.x << "," << (float)pt1.y << "," << (float)pt1.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + //显示法向量 + rgb = { 250, 0, 0 }; + sw << "Poly_" << lineIdx << "_2" << std::endl; sw << "{" << (float)pt2.x << "," << (float)pt2.y << "," << (float)pt2.z << "}-"; sw << "{0,0}-{0,0}-"; sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << (float)pt3.x << "," << (float)pt3.y << "," << (float)pt3.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; lineIdx++; } lineIdx++; @@ -1156,18 +1185,19 @@ void rodWeldSeamPosition_test(void) cornerParam.jumpCornerTh_2 = 60; SSG_outlierFilterParam filterParam; - filterParam.continuityTh = 5.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声 - filterParam.outlierTh = 5; + filterParam.continuityTh = 4.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声 + filterParam.outlierTh = 4; SSG_treeGrowParam growParam; growParam.maxLineSkipNum = 5; - growParam.yDeviation_max = 10.0; + growParam.yDeviation_max = 5.0; growParam.maxSkipDistance = 20.0; - growParam.zDeviation_max = 10.0;// + growParam.zDeviation_max = 3.0;// growParam.minLTypeTreeLen = 50; //mm, 螺杆长度 growParam.minVTypeTreeLen = 50; //mm bool isHorizonScan = true; //true:激光线平行槽道；false:激光线垂直槽道 + double weldSeanRange = 100; //焊缝距钢筋交叉点的范围（最大值） int errCode = 0; std::vector weldSeamInfo; sx_rebarWeldSeamPositioning( @@ -1177,6 +1207,7 @@ void rodWeldSeamPosition_test(void) filterParam, growParam, rodParam, + weldSeanRange, weldSeamInfo, &errCode); long t2 = (long)GetTickCount64(); @@ -1192,7 +1223,7 @@ void rodWeldSeamPosition_test(void) int main() { -#if 1 //螺杆定位测试 +#if 0 //螺杆定位测试 #if 0 screwTest(); #else diff --git a/sourceCode/SG_errCode.h b/sourceCode/SG_errCode.h index fcc9992..1165b0d 100644 --- a/sourceCode/SG_errCode.h +++ b/sourceCode/SG_errCode.h @@ -47,3 +47,6 @@ //칤λ #define SX_ERR_UNKNOWN_PLATE_DIR -2601 +//ԣ춨λ +#define SX_ERR_NO_HIGHEST_ROD -2701 + diff --git a/sourceCode/SG_featureGrow.cpp b/sourceCode/SG_featureGrow.cpp index 4b4c4f9..ab02c36 100644 --- a/sourceCode/SG_featureGrow.cpp +++ b/sourceCode/SG_featureGrow.cpp @@ -843,7 +843,7 @@ void wd_getRodArcFeatureGrowingTrees( { for (int i = 0, i_max = (int)all_lineFeatures.size(); i < i_max; i++) { - if (i == 424) + if (i == 1147) 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 b8abfa0..eed9bf6 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -3824,10 +3824,64 @@ bool compareByPtIdx(const SSG_basicFeature1D& a, const SSG_basicFeature1D& b) { return a.jumpPos2D.y < b.jumpPos2D.y; } +//ݾԷֶ +void wd_lineDataSegment_dist( + std::vector< SWD3DPointPostion>& lineData, + std::vector& segs, + const double maxDistTh, + const int minSegSize +) +{ + int dataSize = (int)lineData.size(); + int runIdx = 1; + SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx + double pre_z = 0; + double pre_y = 0; + for (int i = 0; i < dataSize; i++) + { + if (lineData[i].point.z > 1e-4) + { + if (a_run.len < 0) + { + a_run.start = i; + a_run.len = 1; + a_run.value = i; + } + else + { + double dist = sqrt(pow(lineData[i].point.y - pre_y, 2) + pow(lineData[i].point.z - pre_z, 2)); + if (dist < maxDistTh) + { + a_run.len = i - a_run.start + 1; + a_run.value = i; + } + else + { + a_run.value = runIdx; + runIdx++; + if(a_run.len >= minSegSize) + segs.push_back(a_run); + + a_run.start = i; + a_run.len = 1; + a_run.value = i; + } + } + pre_y = lineData[i].point.y; + pre_z = lineData[i].point.z; + } + } + if (a_run.len > 0) + { + if(a_run.len >= minSegSize) + segs.push_back(a_run); + } +} + //ZԷֶ void wd_lineDataSegment_zDist( std::vector< SVzNL3DPosition>& lineData, - std::vector< SVzNL3DPosition>& vldPts, + std::vector< SVzNL3DPosition>& vldPts, std::vector& segs, std::vector& backIndexing, const SSG_cornerParam cornerPara @@ -4218,6 +4272,7 @@ bool _chkRodArcFeature( { //pkIdxܻв double realPkCorner = abs(ptDirAngles[pkIdx].forwardAngle + ptDirAngles[pkIdx].backwardAngle); + int validCorner = ptDirAngles[pkIdx].type; int realPkIdx = pkIdx; //ǰ for (int i = pkIdx - 1; i >= 0; i--) @@ -4225,12 +4280,21 @@ bool _chkRodArcFeature( if (lineData[i].pt3D.z < 1e-4) continue; + if (ptDirAngles[i].type < 0) + continue; + double dist = sqrt(pow(lineData[pkIdx].pt3D.y - lineData[i].pt3D.y, 2) + pow(lineData[pkIdx].pt3D.z - lineData[i].pt3D.z, 2)); if (dist > searchWin) break; double flatAngle = abs(ptDirAngles[i].forwardAngle + ptDirAngles[i].backwardAngle); - if (realPkCorner > flatAngle) + if( validCorner < 0) + { + realPkCorner = flatAngle; + realPkIdx = i; + validCorner = 0; + } + else if (realPkCorner > flatAngle) { realPkCorner = flatAngle; realPkIdx = i; @@ -4242,12 +4306,21 @@ bool _chkRodArcFeature( if (lineData[i].pt3D.z < 1e-4) continue; + if (ptDirAngles[i].type < 0) + continue; + double dist = sqrt(pow(lineData[pkIdx].pt3D.y - lineData[i].pt3D.y, 2) + pow(lineData[pkIdx].pt3D.z - lineData[i].pt3D.z, 2)); if (dist > searchWin) break; double flatAngle = abs(ptDirAngles[i].forwardAngle + ptDirAngles[i].backwardAngle); - if (realPkCorner > flatAngle) + if (validCorner < 0) + { + realPkCorner = flatAngle; + realPkIdx = i; + validCorner = 0; + } + else if (realPkCorner > flatAngle) { realPkCorner = flatAngle; realPkIdx = i; @@ -4316,7 +4389,7 @@ void wd_getRodArcFeature_peakCornerMethod( ) { double arcPerPointCornerMinValue = 5; //arcÿתСֵ - double arcTotalCornerMinValue = 45; //ArcתСֵ + double arcTotalCornerMinValue = 30; //ArcתСֵ //ǰǺͺ std::vector< SSG_pntDirAngle> ptDirAngles; wd_computeDirAngle_wholeLine( lineData, cornerPara, ptDirAngles); @@ -4356,10 +4429,10 @@ void wd_getRodArcFeature_peakCornerMethod( return; } //ȡcornerֵʵֺʹô˺дŻ -void _searchCornerPeaks( +void wd_searchCornerPeaks( std::vector< SSG_pntDirAngle>& corners, std::vector< SVzNL3DPosition>& vldPts, - const SSG_cornerParam cornerPara, + const double minCornerTh, double cornerMergeScale, std::vector< SSG_pntDirAngle>& cornerPeakP, std::vector< SSG_pntDirAngle>& cornerPeakM @@ -4379,20 +4452,19 @@ void _searchCornerPeaks( if (i == 275) int kkk = 1; SSG_pntDirAngle* curr_data = &corners[i]; - if (curr_data->pntIdx < 0) - continue; - if (curr_data->pntIdx < 0) { if (i == i_max - 1) //һ { if (1 == _state) // { - cornerPk_P.push_back(corners[eEdgePtIdx]); + if(corners[eEdgePtIdx].corner > 0) + cornerPk_P.push_back(corners[eEdgePtIdx]); } else if (2 == _state) //½ { - cornerPk_M.push_back(corners[eEdgePtIdx]); + if(corners[eEdgePtIdx].corner < 0) + cornerPk_M.push_back(corners[eEdgePtIdx]); } } continue; @@ -4451,7 +4523,7 @@ void _searchCornerPeaks( double square_distTh = cornerMergeScale * cornerMergeScale; //2cornerScale for (int i = 0, i_max = (int)cornerPk_P.size(); i < i_max; i++) { - if (cornerPk_P[i].corner < cornerPara.cornerTh) + if (cornerPk_P[i].corner < minCornerTh) continue; bool isPeak = true; @@ -4494,7 +4566,7 @@ void _searchCornerPeaks( for (int i = 0, i_max = (int)cornerPk_M.size(); i < i_max; i++) { - if (abs(cornerPk_M[i].corner) < cornerPara.cornerTh) + if (abs(cornerPk_M[i].corner) < minCornerTh) continue; bool isPeak = true; @@ -4534,8 +4606,6 @@ void _searchCornerPeaks( if (true == isPeak) cornerPeakM.push_back(cornerPk_M[i]); } - - } /// @@ -4703,10 +4773,10 @@ void wd_getLineCorerFeature( std::vector< SSG_pntDirAngle> cornerPeakM; double cornerMergeScale = cornerPara.scale * 2; //ȡcornerֵ - _searchCornerPeaks( + wd_searchCornerPeaks( corners, vldPts, - cornerPara, + cornerPara.cornerTh, cornerMergeScale, cornerPeakP, cornerPeakM @@ -4899,10 +4969,10 @@ void wd_getLineCorerFeature_accelerate( std::vector< SSG_pntDirAngle> cornerPeakM; double cornerMergeScale = cornerPara.scale * 2; //ȡcornerֵ - _searchCornerPeaks( + wd_searchCornerPeaks( corners, lineData, - cornerPara, + cornerPara.cornerTh, cornerMergeScale, cornerPeakP, cornerPeakM @@ -5645,10 +5715,10 @@ void sg_getLineZJumpFeature_cornerMethod( std::vector< SSG_pntDirAngle> cornerPeakP; std::vector< SSG_pntDirAngle> cornerPeakM; double cornerMergeScale = cornerPara.scale * 2; - _searchCornerPeaks( + wd_searchCornerPeaks( corners, vldPts, - cornerPara, + cornerPara.cornerTh, cornerMergeScale, cornerPeakP, cornerPeakM @@ -5884,10 +5954,10 @@ void sg_getLineLJumpFeature_cornerMethod( std::vector< SSG_pntDirAngle> cornerPeakP; std::vector< SSG_pntDirAngle> cornerPeakM; double cornerMergeScale = cornerPara.scale; - _searchCornerPeaks( + wd_searchCornerPeaks( corners, vldPts, - cornerPara, + cornerPara.cornerTh, cornerMergeScale, cornerPeakP, cornerPeakM diff --git a/sourceCode/dataFitting.cpp b/sourceCode/dataFitting.cpp index 2284b4c..b0aa76d 100644 --- a/sourceCode/dataFitting.cpp +++ b/sourceCode/dataFitting.cpp @@ -71,6 +71,56 @@ void lineFitting_abc(std::vector< SVzNL3DPoint>& inliers, double* _a, double* _b return; } +//ϳֱͨ߷ax+by+c=0ֱ +void indexingPtLineFitting_abc(std::vector< SWD3DPointPostion>& inliers, double* _a, double* _b, double* _c) +{ + //жǷΪֱ + int dataSize = (int)inliers.size(); + if (dataSize < 2) + return; + + double deltaX = abs(inliers[0].point.x - inliers[dataSize - 1].point.x); + double deltaY = abs(inliers[0].point.y - inliers[dataSize - 1].point.y); + std::vector< SVzNL3DPoint> fittingData; + if (deltaX < deltaY) + { + //x=ky+b + for (int i = 0; i < dataSize; i++) + { + SVzNL3DPoint a_fitPt; + a_fitPt.x = inliers[i].point.y; + a_fitPt.y = inliers[i].point.x; + a_fitPt.z = inliers[i].point.z; + fittingData.push_back(a_fitPt); + } + double k = 0, b = 0; + lineFitting(fittingData, &k, &b); + //ax+by+c + *_a = 1.0; + *_b = -k; + *_c = -b; + } + else + { + for (int i = 0; i < dataSize; i++) + { + SVzNL3DPoint a_fitPt; + a_fitPt.x = inliers[i].point.x; + a_fitPt.y = inliers[i].point.y; + a_fitPt.z = inliers[i].point.z; + fittingData.push_back(a_fitPt); + } + //y = kx+b + double k = 0, b = 0; + lineFitting(fittingData, &k, &b); + //ax+by+c + *_a = k; + *_b = -1; + *_c = b; + } + return; +} + //ԲС double fitCircleByLeastSquare( const std::vector& pointArray, diff --git a/sourceCode/rodAndBarDetection.cpp b/sourceCode/rodAndBarDetection.cpp index e51a4a7..6972c28 100644 --- a/sourceCode/rodAndBarDetection.cpp +++ b/sourceCode/rodAndBarDetection.cpp @@ -13,7 +13,8 @@ //version 1.2.2 : ˶λ̬壺Z򣩣ңY //version 1.2.3 : ݶֱ߶ȷҵ //version 1.2.4 : ݶϵµȷ̬ -std::string m_strVersion = "1.2.4"; +//version 1.2.5 : ԣֽȡAPI +std::string m_strVersion = "1.2.5"; const char* wd_rodAndBarDetectionVersion(void) { return m_strVersion.c_str(); @@ -1499,7 +1500,7 @@ void rodAarcFeatueDetection( int linePtNum = (int)scanLines[0].size(); for (int line = 0; line < lineNum; line++) { - if (line == 1100) + if (line == 697) int kkk = 1; std::vector& lineData = scanLines[line]; @@ -1859,6 +1860,852 @@ void sx_rodPositioning( return; } +typedef struct +{ + int lineType; + int treeIdx; + double xyLine_a, xyLine_b, xyLine_c; + SWD3DPointPostion line_start; + SWD3DPointPostion line_end; + std::vector linePts; +}_RodLineInfo; + +double _compute2DDistance(SVzNL3DPoint& pt1, SVzNL3DPoint& pt2) +{ + return sqrt(pow(pt1.x - pt2.x, 2) + pow(pt1.y - pt2.y, 2)); +} + +double _compute2DDistance_indexingPt(SWD3DPointPostion& pt1, SWD3DPointPostion& pt2) +{ + return sqrt(pow(pt1.point.x - pt2.point.x, 2) + pow(pt1.point.y - pt2.point.y, 2)); +} + +SWD3DPointPostion _computeNearestPoint(_RodLineInfo& a_rod, SVzNL3DPoint& refPt, int* refPtIdx) +{ + SWD3DPointPostion nearestLinePt= { 0, 0, {0, 0, -1} }; + // + double minDist = -1; + int idx = -1; + for (int i = 0; i < (int)a_rod.linePts.size(); i++) + { + SWD3DPointPostion a_linePt = a_rod.linePts[i]; + double dist = _compute2DDistance(a_linePt.point, refPt); + if ((minDist < 0) || (minDist > dist)) + { + minDist = dist; + nearestLinePt = a_linePt; + idx = i; + } + } + *refPtIdx = idx; + return nearestLinePt; +} + +SWD3DPointPostion _computeGivenDistancePoint(_RodLineInfo& a_rod, SVzNL3DPoint& refPt, int refPtIdx, double givenDistance, int* ptPos, bool searchDir_forward) +{ + SWD3DPointPostion nearestLinePt = { 0, 0, {0, 0, -1} }; + // + double minDist = -1; + int position = -1; + if (refPtIdx < 0) + { + for (int i = 0; i < (int)a_rod.linePts.size(); i++) + { + SWD3DPointPostion a_linePt = a_rod.linePts[i]; + double dist = _compute2DDistance(a_linePt.point, refPt); + double distDiff = abs(dist - givenDistance); + if ((minDist < 0) || (minDist > distDiff)) + { + minDist = distDiff; + nearestLinePt = a_linePt; + position = i; + } + } + } + else + { + if (true == searchDir_forward) + { + for (int i = refPtIdx; i < (int)a_rod.linePts.size(); i++) + { + SWD3DPointPostion a_linePt = a_rod.linePts[i]; + double dist = _compute2DDistance(a_linePt.point, refPt); + double distDiff = abs(dist - givenDistance); + if ((minDist < 0) || (minDist > distDiff)) + { + minDist = distDiff; + nearestLinePt = a_linePt; + position = i; + } + } + } + else + { + for (int i = refPtIdx; i >= 0; i--) + { + SWD3DPointPostion a_linePt = a_rod.linePts[i]; + double dist = _compute2DDistance(a_linePt.point, refPt); + double distDiff = abs(dist - givenDistance); + if ((minDist < 0) || (minDist > distDiff)) + { + minDist = distDiff; + nearestLinePt = a_linePt; + position = i; + } + } + } + } + *ptPos = position; + return nearestLinePt; +} + +bool _compareLineHeighth(_RodLineInfo& rod1, _RodLineInfo& rod2) +{ + //󽻵 + SVzNL3DPoint crossPt = computeLineCrossPt_abs( + rod1.xyLine_a, rod1.xyLine_b, rod1.xyLine_c, + rod2.xyLine_a, rod2.xyLine_b, rod2.xyLine_c); + //жϽϻ + //Ѱ뽻ĵ + int posIdx = -1; + double len_rod1 = _compute2DDistance_indexingPt(rod1.line_start, rod1.line_end); + double cross_rod1_start = _compute2DDistance(rod1.line_start.point, crossPt); + double cross_rod1_end = _compute2DDistance(rod1.line_end.point, crossPt); + SWD3DPointPostion nearestLinePt_rod1; + if ( (cross_rod1_start < len_rod1) && (cross_rod1_end < len_rod1)) // + nearestLinePt_rod1 = _computeNearestPoint(rod1, crossPt, &posIdx); + else + { + if (cross_rod1_start < cross_rod1_end) + nearestLinePt_rod1 = rod1.line_start; + else + nearestLinePt_rod1 = rod1.line_end; + } + if (nearestLinePt_rod1.point.z < 1e-4) + return true; + + double len_rod2 = _compute2DDistance_indexingPt(rod2.line_start, rod2.line_end); + double cross_rod2_start = _compute2DDistance(rod2.line_start.point, crossPt); + double cross_rod2_end = _compute2DDistance(rod2.line_end.point, crossPt); + SWD3DPointPostion nearestLinePt_rod2; + if ((cross_rod2_start < len_rod2) && (cross_rod2_end < len_rod2))// + nearestLinePt_rod2 = _computeNearestPoint(rod2, crossPt, &posIdx); + else + { + if (cross_rod2_start < cross_rod2_end) + nearestLinePt_rod2 = rod2.line_start; + else + nearestLinePt_rod2 = rod2.line_end; + } + if (nearestLinePt_rod2.point.z < 1e-4) + return true; + + //Ƚϸ߶ + if (nearestLinePt_rod1.point.z < nearestLinePt_rod2.point.z) + return true; + else + return false; +} + +//ѰɨϾֱĵ +SVzNL3DPosition _computeLineNearestPoint(std::vector& lineData, int ptIdx_start, int ptIdx_end, double line_a, double line_b, double line_c) +{ + SVzNL3DPosition nearestLinePt = { 0, {0, 0, 0} }; + double normData = sqrt(pow(line_a, 2) + pow(line_b, 2)); + line_a = line_a / normData; + line_b = line_b / normData; + line_c = line_c / normData; + // + double minDist = -1; + for (int i = ptIdx_start; i <= ptIdx_end; i++) + { + SVzNL3DPosition a_linePt = lineData[i]; + double dist = abs(a_linePt.pt3D.x * line_a + a_linePt.pt3D.y * line_b + line_c); + if ((minDist < 0) || (minDist > dist)) + { + minDist = dist; + a_linePt.nPointIdx = i; + nearestLinePt = a_linePt; + } + } + if( minDist > 2.0) + nearestLinePt = { 0, {0, 0, 0} }; + return nearestLinePt; +} + +//ѰˮƽɨϾֱĵ +SVzNL3DPosition _computeLineNearestPoint_H(std::vector>& scanLines, int ptIdx, int line_start, int line_end, double line_a, double line_b, double line_c) +{ + SVzNL3DPosition nearestLinePt = { 0, {0, 0, 0} }; + double normData = sqrt(pow(line_a, 2) + pow(line_b, 2)); + line_a = line_a / normData; + line_b = line_b / normData; + line_c = line_c / normData; + // + double minDist = -1; + for (int line = line_start; line <= line_end; line++) + { + SVzNL3DPosition a_linePt = scanLines[line][ptIdx]; + double dist = abs(a_linePt.pt3D.x * line_a + a_linePt.pt3D.y * line_b + line_c); + if ((minDist < 0) || (minDist > dist)) + { + minDist = dist; + a_linePt.nPointIdx = line; + nearestLinePt = a_linePt; + } + } + if (minDist > 2.0) + nearestLinePt = { 0, {0, 0, 0} }; + return nearestLinePt; +} + +//ֱֽĺ +SSX_weldSeamInfo _computeWeldPoint( + _RodLineInfo& highestRod, _RodLineInfo& vRod, + const SSG_cornerParam cornerPara, + const SSX_rodParam rodParam, + std::vector< std::vector>& scanLines) +{ + SSX_weldSeamInfo a_weldInfo; + a_weldInfo.weldType = KeWD_WELD_UNKNOWN; + + int lineSize = (int)scanLines.size(); + int linePtSize = (int)scanLines[0].size(); + int pointChkWin = 20; + //㽻 + SVzNL3DPoint crossPt = computeLineCrossPt_abs( + highestRod.xyLine_a, highestRod.xyLine_b, highestRod.xyLine_c, + vRod.xyLine_a, vRod.xyLine_b, vRod.xyLine_c); + int posIdx = -1; + SWD3DPointPostion nearestLinePt_topRebar = _computeNearestPoint(highestRod, crossPt, &posIdx); + double chkLen = rodParam.diameter * 2; + int position = -1; + SWD3DPointPostion chkStart = _computeGivenDistancePoint(vRod, crossPt, -1, chkLen, &position, true); + + //ɨߺ͵ķΧ + int ptIdx_start, ptIdx_end; + if (nearestLinePt_topRebar.ptIdx > chkStart.ptIdx) + { + ptIdx_start = chkStart.ptIdx; + ptIdx_end = nearestLinePt_topRebar.ptIdx; + } + else + { + ptIdx_start = nearestLinePt_topRebar.ptIdx; + ptIdx_end = chkStart.ptIdx; + } + int line_start, line_end; + if (nearestLinePt_topRebar.lineIdx > chkStart.lineIdx) + { + line_start = chkStart.lineIdx; + line_end = nearestLinePt_topRebar.lineIdx; + } + else + { + line_end = chkStart.lineIdx; + line_start = nearestLinePt_topRebar.lineIdx; + } + + double rotateAngle = 0; + if (vRod.lineType == 1) //ֱɨ + { + if (nearestLinePt_topRebar.lineIdx > chkStart.lineIdx) + rotateAngle = 45; + else + rotateAngle = -45; + } + else + { + if (nearestLinePt_topRebar.ptIdx > chkStart.ptIdx) + rotateAngle = 45; + else + rotateAngle = -45; + } + + //ϵĵ + std::vector peakPoints; + std::vector peakPostions; + + if (vRod.lineType == 1) //ֱɨ + { + ptIdx_start = ptIdx_start - pointChkWin; + if (ptIdx_start < 0) + ptIdx_start = 0; + ptIdx_end = ptIdx_end + pointChkWin; + if (ptIdx_end >= linePtSize) + ptIdx_end = linePtSize - 1; + for (int line = line_start; line <= line_end; line++) + { + SVzNL3DPosition a_pkPt = _computeLineNearestPoint(scanLines[line], ptIdx_start, ptIdx_end, vRod.xyLine_a, vRod.xyLine_b, vRod.xyLine_c); + if (a_pkPt.pt3D.z > 1e-4) + { + SSG_intPair a_pos; //ڿӻʾ + a_pos.data_0 = line; + a_pos.data_1 = a_pkPt.nPointIdx; + a_pos.idx = (int)peakPoints.size(); + peakPostions.push_back(a_pos); + + a_pkPt.nPointIdx = (int)peakPoints.size(); + double tmp = a_pkPt.pt3D.x; //ֽΪˮƽXYΪ㷽Y + a_pkPt.pt3D.x = a_pkPt.pt3D.y; + a_pkPt.pt3D.y = tmp; + peakPoints.push_back(a_pkPt); + + } + } + } + else //ˮƽɨ + { + line_start = line_start - pointChkWin; + if (line_start < 0) + line_start = 0; + line_end = line_end + pointChkWin; + if (line_end >= lineSize) + line_end = lineSize - 1; + + for (int ptIdx = ptIdx_start; ptIdx <= ptIdx_end; ptIdx++) + { + SVzNL3DPosition a_pkPt = _computeLineNearestPoint_H(scanLines, ptIdx, line_start, line_end, vRod.xyLine_a, vRod.xyLine_b, vRod.xyLine_c); + if (a_pkPt.pt3D.z > 1e-4) + { + SSG_intPair a_pos; //ڿӻʾ + a_pos.data_0 = a_pkPt.nPointIdx; + a_pos.data_1 = ptIdx; + a_pos.idx = (int)peakPoints.size(); + peakPostions.push_back(a_pos); + + a_pkPt.nPointIdx = (int)peakPoints.size(); + peakPoints.push_back(a_pkPt); + } + } + } + std::vector< SSG_pntDirAngle> ptDirAngles; + wd_computeDirAngle_wholeLine( + peakPoints, + cornerPara, + ptDirAngles + ); + + //ѰҺ㣺յĵ + std::vector< SSG_pntDirAngle> cornerPeakP; + std::vector< SSG_pntDirAngle> cornerPeakM; + wd_searchCornerPeaks( + ptDirAngles, + peakPoints, + cornerPara.cornerTh, + rodParam.diameter/2, + cornerPeakP, + cornerPeakM + ); + + if (cornerPeakP.size() > 0) + { + int maxIdx = 0; + for (int i = 1; i < (int)cornerPeakP.size(); i++) + { + if (cornerPeakP[i].corner > cornerPeakP[maxIdx].corner) + maxIdx = i; + } + //ɽ + //double rotateAngle = (cornerPeakP[maxIdx].backwardAngle + cornerPeakP[maxIdx].forwardAngle) / 2 + 90; + SVzNL3DPoint vec_axial = { highestRod.line_end.point.x - highestRod.line_start.point.x, + highestRod.line_end.point.y - highestRod.line_start.point.y, + highestRod.line_end.point.z - highestRod.line_start.point.z }; + SVzNL3DPoint vec_vRod = { vRod.line_end.point.x - vRod.line_start.point.x, + vRod.line_end.point.y - vRod.line_start.point.y, + vRod.line_end.point.z - vRod.line_start.point.z }; + SVzNL3DPoint v = vec3_cross(vec_axial, vec_vRod); + + SVzNL3DPoint v_dir = wd_rotateVectorInPlane(v, vec_axial, rotateAngle); + if (v_dir.z > 0) + v_dir = {-v_dir.x, -v_dir.y, -v_dir.z}; + v_dir = vec3_normalize(v_dir); + + int pkIdx = cornerPeakP[maxIdx].pntIdx; + int obj_lineIdx = peakPostions[pkIdx].data_0; + int obj_ptIdx = peakPostions[pkIdx].data_1; + a_weldInfo.weldType = KeWD_WELD_POINT; + a_weldInfo.center = scanLines[obj_lineIdx][obj_ptIdx].pt3D; + a_weldInfo.startPt = a_weldInfo.center; + a_weldInfo.endPt = a_weldInfo.center; + a_weldInfo.axialDir = vec3_normalize(vec_axial); + a_weldInfo.normalDir = v_dir; // + + //ע + for (int m = 0; m < (int)peakPostions.size(); m++) + { + int lineIdx = peakPostions[m].data_0; + int centerPtIdx = peakPostions[m].data_1; + scanLines[lineIdx][centerPtIdx].nPointIdx &= 0x0F; + scanLines[lineIdx][centerPtIdx].nPointIdx |= 0x40; + } + } + return a_weldInfo; +} + +typedef struct +{ + EWD_weldType type; + SWD3DPointPostion startPt; + SWD3DPointPostion endPt; + SVzNL3DPoint center; +}SSX_weldSlice; + +//㺸 +SSX_weldSlice _computeWeldSliceInfo( + std::vector< SWD3DPointPostion>& weldSliceData, + const SSX_rodParam rodParam, + const SSG_cornerParam cornerPara, + const double maxDistTh, + bool dirForward +) +{ + double rodR = rodParam.diameter / 2; + //ȡ + std::vector segs; + int minSegSize = 4; + wd_lineDataSegment_dist(weldSliceData, segs, maxDistTh, minSegSize); + if (segs.size() > 0) + { + SSG_RUN& base_seg = segs[0]; + int base_s = base_seg.start; + int base_e = base_seg.len + base_s - 1; + double base_w = abs(weldSliceData[base_s].point.y - weldSliceData[base_e].point.y); + if (base_w > (rodR * 1.1)) + { + std::vector< SVzNL3DPosition> line_data; + line_data.resize(weldSliceData.size()); + SVzNL3DPosition zeroData = { 0, {0, 0, 0} }; + std::fill(line_data.begin(), line_data.end(), zeroData); + for (int i = base_s; i <= base_e; i++) + { + line_data[i].nPointIdx = i; + line_data[i].pt3D = weldSliceData[i].point; + } + //Ѱҹյ + std::vector< SSG_pntDirAngle> ptDirAngles; + wd_computeDirAngle_wholeLine( + line_data, + cornerPara, + ptDirAngles + ); + for(int i = 0; i < (int)ptDirAngles.size(); i ++) + { + if (ptDirAngles[i].type < 0) + ptDirAngles[i].pntIdx = -1; + } + //ѰҺ㣺յĵ + std::vector< SSG_pntDirAngle> cornerPeakP; + std::vector< SSG_pntDirAngle> cornerPeakM; + wd_searchCornerPeaks( + ptDirAngles, + line_data, + cornerPara.cornerTh, + rodParam.diameter / 2, + cornerPeakP, + cornerPeakM + ); + //if (true == dirForward) + { + if (cornerPeakP.size() > 0) + { + int maxIdx = 0; + for (int i = 1; i < (int)cornerPeakP.size(); i++) + { + if (cornerPeakP[i].corner > cornerPeakP[maxIdx].corner) + maxIdx = i; + } + int pkIdx = cornerPeakP[maxIdx].pntIdx; + SSX_weldSlice a_slice; + a_slice.type = KeWD_WELD_SEAM; + a_slice.startPt = weldSliceData[pkIdx]; + a_slice.endPt = weldSliceData[pkIdx]; + a_slice.center = weldSliceData[pkIdx].point; + return a_slice; + } + } +#if 0 + else + { + if (cornerPeakM.size() > 0) + { + int maxIdx = 0; + for (int i = 1; i < (int)cornerPeakM.size(); i++) + { + if (cornerPeakM[i].corner < cornerPeakM[maxIdx].corner) + maxIdx = i; + } + int pkIdx = cornerPeakM[maxIdx].pntIdx; + SSX_weldSlice a_slice; + a_slice.type = KeWD_WELD_SEAM; + a_slice.startPt = weldSliceData[pkIdx]; + a_slice.endPt = weldSliceData[pkIdx]; + a_slice.center = weldSliceData[pkIdx].point; + return a_slice; + } + } +#endif + } + else + { + if (segs.size() > 1) + { + //Ѱҳ˸ֽ㣨0.5RodR + SWD3DPointPostion& refPt = weldSliceData[0]; + double minDist = -1; + int minIdx = -1; + for (int i = 1; i < (int)segs.size(); i++) + { + int seg_s = segs[i].start; + int seg_e = segs[i].len + seg_s - 1; + for (int j = seg_s; j <= seg_e; j++) + { + double dist = _compute2DDistance_indexingPt(refPt, weldSliceData[j]); + if ((minDist < 0) || (minDist > dist)) + { + minDist = dist; + minIdx = j; + } + } + } + if (minIdx >= 0) + { + SWD3DPointPostion endPoint = weldSliceData[minIdx]; + int seg_s = segs[0].start; + int seg_e = segs[0].len + seg_s - 1; // ѰZֵӽ + + double minZDiff = -1; + int minZDiff_idx = -1; + for (int i = seg_s; i <= seg_e; i++) + { + double zDiff = abs(endPoint.point.z - weldSliceData[i].point.z); + if ((minZDiff < 0) || (minZDiff > zDiff)) + { + minZDiff = zDiff; + minZDiff_idx = i; + } + } + if (minZDiff_idx >= 0) + { + SSX_weldSlice a_slice; + a_slice.type = KeWD_WELD_SEAM_GAP; + a_slice.startPt = weldSliceData[minZDiff_idx]; + a_slice.endPt = endPoint; + a_slice.center = {(a_slice.startPt.point.x + a_slice.endPt.point.x)/2, + (a_slice.startPt.point.y + a_slice.endPt.point.y) / 2, + (a_slice.startPt.point.z + a_slice.endPt.point.z) / 2}; + return a_slice; + } + } + } + } + } + SSX_weldSlice nullData; + memset(&nullData, 0, sizeof(SSX_weldSlice)); + return nullData; +} + +SSX_weldSeamInfo _computeWeldSeamInfo( + std::vector& seamData, + int rodScanType, + const SSG_cornerParam& weldPointCornerParam, + SVzNL3DPoint& v_zDir, + double rotateAngle +) +{ + // + std::vector fitting_xy; + std::vector fitting_yz; + double mean_w = 0; + for (int i = 0; i < (int)seamData.size(); i++) + { + fitting_xy.push_back(seamData[i].center); + SVzNL3DPoint a_pt = { seamData[i].center.y, seamData[i].center.z, seamData[i].center.x }; + fitting_yz.push_back(a_pt); + double w = abs(seamData[i].endPt.point.y - seamData[i].startPt.point.y); + mean_w += w; + } + mean_w = mean_w / (double)seamData.size(); + + double xyLine_a, xyLine_b, xyLine_c; + lineFitting_abc(fitting_xy, &xyLine_a, &xyLine_b, &xyLine_c); + //յ + SVzNL2DPointD foot_start_1 = sx_getFootPoint_abc(fitting_xy[0].x, fitting_xy[0].y, xyLine_a, xyLine_b, xyLine_c); + SVzNL2DPointD foot_end_1 = sx_getFootPoint_abc(fitting_xy.back().x, fitting_xy.back().y, xyLine_a, xyLine_b, xyLine_c); + + double yzLine_a, yzLine_b, yzLine_c; + lineFitting_abc(fitting_yz, &yzLine_a, &yzLine_b, &yzLine_c); + //յ + SVzNL2DPointD foot_start_2 = sx_getFootPoint_abc(fitting_yz[0].x, fitting_yz[0].y, yzLine_a, yzLine_b, yzLine_c); + SVzNL2DPointD foot_end_2 = sx_getFootPoint_abc(fitting_yz.back().x, fitting_yz.back().y, yzLine_a, yzLine_b, yzLine_c); + + SSX_weldSeamInfo a_weldSeam; + a_weldSeam.weldType = mean_w > weldPointCornerParam.scale ? KeWD_WELD_SEAM_GAP : KeWD_WELD_SEAM; + if (rodScanType == 1) //ֱ + { + a_weldSeam.startPt = { foot_start_1.x, foot_start_1.y, foot_start_2.y }; + a_weldSeam.endPt = { foot_end_1.x, foot_end_1.y, foot_end_2.y }; + a_weldSeam.center = { (a_weldSeam.startPt.x + a_weldSeam.endPt.x) / 2, + (a_weldSeam.startPt.y + a_weldSeam.endPt.y) / 2, + (a_weldSeam.startPt.z + a_weldSeam.endPt.z) / 2 }; + a_weldSeam.axialDir = { (a_weldSeam.endPt.x - a_weldSeam.startPt.x) / 2, + (a_weldSeam.endPt.y - a_weldSeam.startPt.y) / 2, + (a_weldSeam.endPt.z - a_weldSeam.startPt.z) / 2 }; + a_weldSeam.axialDir = vec3_normalize(a_weldSeam.axialDir); + SVzNL3DPoint v_dir = wd_rotateVectorInPlane(v_zDir, a_weldSeam.axialDir, rotateAngle); + if (v_dir.z > 0) + v_dir = { -v_dir.x, -v_dir.y, -v_dir.z }; + v_dir = vec3_normalize(v_dir); + a_weldSeam.normalDir = v_dir;// + for (int i = 0; i < (int)seamData.size(); i++) + { + a_weldSeam.centerPts.push_back(seamData[i].center); + SVzNL3DPoint pt_0 = seamData[i].startPt.point; + SVzNL3DPoint pt_1 = seamData[i].endPt.point; + a_weldSeam.edgePts_0.push_back(pt_0); + a_weldSeam.edgePts_1.push_back(pt_1); + } + } + else + { + a_weldSeam.startPt = { foot_start_1.y, foot_start_1.x, foot_start_2.y }; + a_weldSeam.endPt = { foot_end_1.y, foot_end_1.x, foot_end_2.y }; + a_weldSeam.center = { (a_weldSeam.startPt.x + a_weldSeam.endPt.x) / 2, + (a_weldSeam.startPt.y + a_weldSeam.endPt.y) / 2, + (a_weldSeam.startPt.z + a_weldSeam.endPt.z) / 2 }; + a_weldSeam.axialDir = { (a_weldSeam.endPt.x - a_weldSeam.startPt.x) / 2, + (a_weldSeam.endPt.y - a_weldSeam.startPt.y) / 2, + (a_weldSeam.endPt.z - a_weldSeam.startPt.z) / 2 }; + a_weldSeam.axialDir = vec3_normalize(a_weldSeam.axialDir); + SVzNL3DPoint v_dir = wd_rotateVectorInPlane(v_zDir, a_weldSeam.axialDir, rotateAngle); + if (v_dir.z > 0) + v_dir = { -v_dir.x, -v_dir.y, -v_dir.z }; + v_dir = vec3_normalize(v_dir); + a_weldSeam.normalDir = v_dir;// + for (int i = 0; i < (int)seamData.size(); i++) + { + SVzNL3DPoint a_pt = { seamData[i].center.y, seamData[i].center.x, seamData[i].center.z }; + a_weldSeam.centerPts.push_back(a_pt); + SVzNL3DPoint pt_0 = { seamData[i].startPt.point.y, seamData[i].startPt.point.x, seamData[i].startPt.point.z }; + SVzNL3DPoint pt_1 = { seamData[i].endPt.point.y, seamData[i].endPt.point.x, seamData[i].endPt.point.z }; + a_weldSeam.edgePts_0.push_back(pt_0); + a_weldSeam.edgePts_1.push_back(pt_1); + } + } + return a_weldSeam; +} + + +void _computeRebarWeldSeam( + _RodLineInfo& vRod, + SVzNL3DPoint& v_zDir, + bool searchDir_forward, + SVzNL3DPoint& crossPoint, + int crossPtIdx, + std::vector& rebarWeldSeam, + std::vector< std::vector>& scanLines, + const SSX_rodParam rodParam, + const SSG_cornerParam weldPointCornerParam, + const double weldSeanRange //ֽķΧֵ +) +{ + int lineSize = (int)scanLines.size(); + int linePtSize = (int)scanLines[0].size(); + //ȡΧڵֵ + int position_1 = -1; + int position_2 = -1; + SWD3DPointPostion chkPt_1 = _computeGivenDistancePoint(vRod, crossPoint, crossPtIdx, rodParam.diameter/2, &position_1, searchDir_forward); + SWD3DPointPostion chkPt_2 = _computeGivenDistancePoint(vRod, crossPoint, crossPtIdx, weldSeanRange, &position_2, searchDir_forward); + if ((position_1 < 0) || (position_2 < 0)) + return; + + std::vector> checkingData_1; //Peak + std::vector> checkingData_2; //peakһ + int startIdx = position_1 < position_2 ? position_1 : position_2; + int endIdx = position_1 < position_2 ? position_2 : position_1; + for (int i = startIdx; i <= endIdx; i++) + { + std::vector< SWD3DPointPostion> a_line_1; + std::vector< SWD3DPointPostion> a_line_2; + SWD3DPointPostion& refPt = vRod.linePts[i]; + if (vRod.lineType == 1) //ֱ + { + int lineIdx = refPt.lineIdx; + for (int j = refPt.ptIdx; j >= 0; j--) + { + SVzNL3DPosition& a_pt = scanLines[lineIdx][j]; + if (a_pt.pt3D.z > 1e-4) + { + double xyDist = sqrt(pow(a_pt.pt3D.x - refPt.point.x, 2) + pow(a_pt.pt3D.y - refPt.point.y, 2)); + double zDiff = abs(a_pt.pt3D.z - refPt.point.z); + if (xyDist > rodParam.diameter * 1.5) + break; + if (zDiff < rodParam.diameter) + { + SWD3DPointPostion a_chkPt; + a_chkPt.lineIdx = lineIdx; + a_chkPt.ptIdx = j; + a_chkPt.point = a_pt.pt3D; + a_line_1.push_back( a_chkPt); + } + } + } + // + for (int j = refPt.ptIdx; j < linePtSize; j++) + { + SVzNL3DPosition& a_pt = scanLines[lineIdx][j]; + if (a_pt.pt3D.z > 1e-4) + { + double xyDist = sqrt(pow(a_pt.pt3D.x - refPt.point.x, 2) + pow(a_pt.pt3D.y - refPt.point.y, 2)); + double zDiff = abs(a_pt.pt3D.z - refPt.point.z); + if (xyDist > rodParam.diameter * 1.5) + break; + if (zDiff < rodParam.diameter) + { + SWD3DPointPostion a_chkPt; + a_chkPt.lineIdx = lineIdx; + a_chkPt.ptIdx = j; + a_chkPt.point = a_pt.pt3D; + a_line_2.push_back(a_chkPt); + } + } + } + } + else + { + int ptIdx = refPt.ptIdx; + for (int j = refPt.lineIdx; j >= 0; j--) + { + SVzNL3DPosition& a_pt = scanLines[j][ptIdx]; + if (a_pt.pt3D.z > 1e-4) + { + double xyDist = sqrt(pow(a_pt.pt3D.x - refPt.point.x, 2) + pow(a_pt.pt3D.y - refPt.point.y, 2)); + double zDiff = abs(a_pt.pt3D.z - refPt.point.z); + if (xyDist > rodParam.diameter * 1.5) + break; + if (zDiff < rodParam.diameter) + { + SWD3DPointPostion a_chkPt; + a_chkPt.lineIdx = j; + a_chkPt.ptIdx = ptIdx; + a_chkPt.point.x = a_pt.pt3D.y; + a_chkPt.point.y = a_pt.pt3D.x; + a_chkPt.point.z = a_pt.pt3D.z; + a_line_1.push_back(a_chkPt); + } + } + } + // + for (int j = refPt.lineIdx; j < lineSize; j++) + { + SVzNL3DPosition& a_pt = scanLines[j][ptIdx]; + if (a_pt.pt3D.z > 1e-4) + { + double xyDist = sqrt(pow(a_pt.pt3D.x - refPt.point.x, 2) + pow(a_pt.pt3D.y - refPt.point.y, 2)); + double zDiff = abs(a_pt.pt3D.z - refPt.point.z); + if (xyDist > rodParam.diameter * 1.5) + break; + if (zDiff < rodParam.diameter) + { + SWD3DPointPostion a_chkPt; + a_chkPt.lineIdx = j; + a_chkPt.ptIdx = ptIdx; + a_chkPt.point.x = a_pt.pt3D.y; + a_chkPt.point.y = a_pt.pt3D.x; + a_chkPt.point.z = a_pt.pt3D.z; + a_line_2.push_back(a_chkPt); + } + } + } + } + checkingData_1.push_back(a_line_1); + checkingData_2.push_back(a_line_2); + } + //ȷ + int dataLineSize = (int)checkingData_1.size(); + std::vector lineW; + int seam_start = -1; + int seam_end = -1; + for (int i = 0; i < dataLineSize; i++) + { + double w; + if ((checkingData_1[i].size() == 0) || (checkingData_2[i].size() == 0)) + w = -1; + else + w = checkingData_2[i].back().point.y - checkingData_1[i].back().point.y; + + if (w > rodParam.diameter * 1.2) + { + if (seam_start < 0) + seam_start = i; + seam_end = i; + } + lineW.push_back(w); + } + // + double segment_maxDistTh = 2.0; + std::vector seamData_1; + std::vector seamData_2; + for (int i = seam_start; i <= seam_end; i++) + { + + SSX_weldSlice a_slice_1 = _computeWeldSliceInfo( + checkingData_1[i], + rodParam, + weldPointCornerParam, + segment_maxDistTh, + false + ); + if (KeWD_WELD_UNKNOWN != a_slice_1.type) + seamData_1.push_back(a_slice_1); + + SSX_weldSlice a_slice_2 = _computeWeldSliceInfo( + checkingData_2[i], + rodParam, + weldPointCornerParam, + segment_maxDistTh, + true + ); + if (KeWD_WELD_UNKNOWN != a_slice_2.type) + seamData_2.push_back(a_slice_2); + + } + + //ɺϢ + if (seamData_1.size() > 10) + { + double rotateAngle = vRod.lineType == 1 ? -45.0 : 45.0; + SSX_weldSeamInfo a_weldSeam = _computeWeldSeamInfo( seamData_1, vRod.lineType, weldPointCornerParam, v_zDir, rotateAngle); + rebarWeldSeam.push_back(a_weldSeam); + //ñ־debug + for (int i = 0; i < (int)seamData_1.size(); i++) + { + int lineIdx = seamData_1[i].startPt.lineIdx; + int ptIdx = seamData_1[i].startPt.ptIdx; + scanLines[lineIdx][ptIdx].nPointIdx |= 0x40; + lineIdx = seamData_1[i].endPt.lineIdx; + ptIdx = seamData_1[i].endPt.ptIdx; + scanLines[lineIdx][ptIdx].nPointIdx |= 0x40; + } + } + if (seamData_2.size() > 10) + { + double rotateAngle = vRod.lineType == 1 ? 45.0 : -45.0; + SSX_weldSeamInfo a_weldSeam = _computeWeldSeamInfo(seamData_2, vRod.lineType, weldPointCornerParam, v_zDir, rotateAngle); + rebarWeldSeam.push_back(a_weldSeam); + //ñ־debug + for (int i = 0; i < (int)seamData_2.size(); i++) + { + int lineIdx = seamData_2[i].startPt.lineIdx; + int ptIdx = seamData_2[i].startPt.ptIdx; + scanLines[lineIdx][ptIdx].nPointIdx |= 0x40; + lineIdx = seamData_2[i].endPt.lineIdx; + ptIdx = seamData_2[i].endPt.ptIdx; + scanLines[lineIdx][ptIdx].nPointIdx |= 0x40; + } + } +} + //ԣֽṹֽ춨λ void sx_rebarWeldSeamPositioning( std::vector< std::vector>& scanLines, @@ -1867,6 +2714,7 @@ void sx_rebarWeldSeamPositioning( const SSG_outlierFilterParam filterParam, const SSG_treeGrowParam growParam, const SSX_rodParam rodParam, + const double weldSeanRange, //ֽķΧֵ std::vector& weldSeamInfo, int* errCode) { @@ -1906,6 +2754,11 @@ void sx_rebarWeldSeamPositioning( sx_rodPosition_lineDataR(scanLines[i], poseCalibPara.planeCalib, cuttingZ); } + // + wd_noiseFilter(scanLines, filterParam, errCode); + if (*errCode != 0)//ݲʽ + return; + //Ѱˮƽʹֱĸֽ //ˮƽɨ std::vector> hLines_raw; @@ -1922,7 +2775,19 @@ void sx_rebarWeldSeamPositioning( hLines_raw[j][line].pt3D.y = scanLines[line][j].pt3D.x; } } - +#if 0 + for (int line = 0; line < linePtNum; line++) + { + //ƽ + sg_lineSegSmoothing( + hLines_raw[line], + double seg_y_deltaTh, //ֶεYڴ˼Ϊµķֶ + double seg_z_deltaTh,//ֶεZڴ˼Ϊµķֶ + int smoothWin, + std::vector&output); + } +#endif + //ڴֱϷֱȡARC std::vector> arcFeatures_v; rodAarcFeatueDetection(scanLines, cornerPara, filterParam, rodParam.diameter, arcFeatures_v); @@ -1952,30 +2817,303 @@ void sx_rebarWeldSeamPositioning( scanLines[line][j].nPointIdx = 0; //ԭʼݵ0תʹã } } - //ñ־debug + //ѡߵĿ + std::vector<_RodLineInfo> rodLineInfo; for (int i = 0; i < objNum_v; i++) { + _RodLineInfo a_rodInfo; int nodeNum = (int)rodArcTrees_v[i].treeNodes.size(); for (int j = 0; j < nodeNum; j++) { int lineIdx = rodArcTrees_v[i].treeNodes[j].lineIdx; int centerPtIdx = rodArcTrees_v[i].treeNodes[j].peakPtIdx; - for (int m = rodArcTrees_v[i].treeNodes[j].startPtIdx; m <= rodArcTrees_v[i].treeNodes[j].endPtIdx; m++) - scanLines[lineIdx][m].nPointIdx = 1; - scanLines[lineIdx][centerPtIdx].nPointIdx |= 0x10; + SWD3DPointPostion a_pt; + a_pt.lineIdx = lineIdx; + a_pt.ptIdx = centerPtIdx; + a_pt.point = scanLines[lineIdx][centerPtIdx].pt3D; + if(a_pt.point.z > 1e-4) + a_rodInfo.linePts.push_back(a_pt); } + a_rodInfo.line_start = a_rodInfo.linePts[0]; + a_rodInfo.line_end = a_rodInfo.linePts.back(); + a_rodInfo.lineType = 1; + a_rodInfo.treeIdx = i; + indexingPtLineFitting_abc(a_rodInfo.linePts, &a_rodInfo.xyLine_a, &a_rodInfo.xyLine_b, &a_rodInfo.xyLine_c); + rodLineInfo.push_back(a_rodInfo); } - //ñ־debug for (int i = 0; i < objNum_h; i++) { + _RodLineInfo a_rodInfo; int nodeNum = (int)rodArcTrees_h[i].treeNodes.size(); for (int j = 0; j < nodeNum; j++) { int ptIdx = rodArcTrees_h[i].treeNodes[j].lineIdx; int centerLineIdx = rodArcTrees_h[i].treeNodes[j].peakPtIdx; - for (int m = rodArcTrees_h[i].treeNodes[j].startPtIdx; m <= rodArcTrees_h[i].treeNodes[j].endPtIdx; m++) - scanLines[m][ptIdx].nPointIdx |= 2; - scanLines[centerLineIdx][ptIdx].nPointIdx |= 0x20; + SWD3DPointPostion a_pt; + a_pt.lineIdx = centerLineIdx; + a_pt.ptIdx = ptIdx; + a_pt.point = scanLines[centerLineIdx][ptIdx].pt3D; + if(a_pt.point.z > 1e-4) + a_rodInfo.linePts.push_back(a_pt); + } + a_rodInfo.line_start = a_rodInfo.linePts[0]; + a_rodInfo.line_end = a_rodInfo.linePts.back(); + a_rodInfo.lineType = 2; + a_rodInfo.treeIdx = i; + indexingPtLineFitting_abc(a_rodInfo.linePts, &a_rodInfo.xyLine_a, &a_rodInfo.xyLine_b, &a_rodInfo.xyLine_c); + rodLineInfo.push_back(a_rodInfo); + } + + int totalRodLineSize = (int)rodLineInfo.size(); + //ߵ + int highestLineIdx = -1; + for (int i = 0; i < totalRodLineSize; i++) + { + bool isHighest = true; + for (int j = i + 1; j < totalRodLineSize; j++) + { + bool isHigher = _compareLineHeighth(rodLineInfo[i], rodLineInfo[j]); + if (false == isHigher) + { + isHighest = false; + break; + } + } + if (true == isHighest) + { + highestLineIdx = i; + break; } } + if(highestLineIdx < 0) + { + *errCode = SX_ERR_NO_HIGHEST_ROD; + return; + } + + _RodLineInfo& highestRod = rodLineInfo[highestLineIdx]; + //ȷֱĸֽ + int vRodType; + if (highestRod.lineType == 1) + vRodType = 2; + else + vRodType = 1; + + //ؽѰ + //ڲ + double validCrossRebar_distTh = rodParam.diameter * 1.5; + + double len_highestRod = _compute2DDistance_indexingPt(highestRod.line_start, highestRod.line_end); + std::vector< _RodLineInfo> validVRod; + std::vector crossPoints_highestRod; + std::vector crossPoints_vRod; + for (int i = 0; i < totalRodLineSize; i++) + { + if (rodLineInfo[i].lineType == vRodType) + { + _RodLineInfo& vRod = rodLineInfo[i]; + //󽻵 + SVzNL3DPoint crossPt = computeLineCrossPt_abs( + highestRod.xyLine_a, highestRod.xyLine_b, highestRod.xyLine_c, + vRod.xyLine_a, vRod.xyLine_b, vRod.xyLine_c); + //жϽϻ + //ҪߵĸֽϣͬʱҪһֽ + double cross_highest_start = _compute2DDistance(highestRod.line_start.point, crossPt); + double cross_highest_end = _compute2DDistance(highestRod.line_end.point, crossPt); + if ((cross_highest_start < len_highestRod ) && ( cross_highest_end < len_highestRod )) + { + int posIdx = -1; + SWD3DPointPostion nearestLinePt_highestRod = _computeNearestPoint(highestRod, crossPt, &posIdx); + + //Ѱ뽻ĵ + double len_vRod = _compute2DDistance_indexingPt(vRod.line_start, vRod.line_end); + double cross_vRod_start = _compute2DDistance(vRod.line_start.point, crossPt); + double cross_vRod_end = _compute2DDistance(vRod.line_end.point, crossPt); + if ( (cross_vRod_start > len_vRod) || (cross_vRod_end > len_vRod)) // + { + SWD3DPointPostion nearestLinePt_vRod; + if (cross_vRod_start < cross_vRod_end) + nearestLinePt_vRod = vRod.line_start; + else + nearestLinePt_vRod = vRod.line_end; + + double distance = _compute2DDistance_indexingPt(nearestLinePt_vRod, nearestLinePt_highestRod); + double zDiff = nearestLinePt_vRod.point.z - nearestLinePt_highestRod.point.z; + if ((distance < validCrossRebar_distTh) && (zDiff < validCrossRebar_distTh)) + { + crossPoints_highestRod.push_back(nearestLinePt_highestRod); + crossPoints_vRod.push_back(nearestLinePt_vRod); + validVRod.push_back(vRod); + } + + } + } + + } + } + + //ñ־debug + if(highestRod.lineType == 1) //vTree + { + int nodeNum = (int)rodArcTrees_v[highestRod.treeIdx].treeNodes.size(); + for (int j = 0; j < nodeNum; j++) + { + int lineIdx = rodArcTrees_v[highestRod.treeIdx].treeNodes[j].lineIdx; + int centerPtIdx = rodArcTrees_v[highestRod.treeIdx].treeNodes[j].peakPtIdx; + for (int m = rodArcTrees_v[highestRod.treeIdx].treeNodes[j].startPtIdx; m <= rodArcTrees_v[highestRod.treeIdx].treeNodes[j].endPtIdx; m++) + scanLines[lineIdx][m].nPointIdx = 1; + scanLines[lineIdx][centerPtIdx].nPointIdx |= 0x10; + } + for (int i = 0; i < (int)validVRod.size(); i++) + { + int treeIdx = validVRod[i].treeIdx; + int nodeNum = (int)rodArcTrees_h[treeIdx].treeNodes.size(); + for (int j = 0; j < nodeNum; j++) + { + int ptIdx = rodArcTrees_h[treeIdx].treeNodes[j].lineIdx; + int centerLineIdx = rodArcTrees_h[treeIdx].treeNodes[j].peakPtIdx; + for (int m = rodArcTrees_h[treeIdx].treeNodes[j].startPtIdx; m <= rodArcTrees_h[treeIdx].treeNodes[j].endPtIdx; m++) + scanLines[m][ptIdx].nPointIdx |= 2; + scanLines[centerLineIdx][ptIdx].nPointIdx |= 0x20; + } + } + } + else + { + int nodeNum = (int)rodArcTrees_h[highestRod.treeIdx].treeNodes.size(); + for (int j = 0; j < nodeNum; j++) + { + int ptIdx = rodArcTrees_h[highestRod.treeIdx].treeNodes[j].lineIdx; + int centerLineIdx = rodArcTrees_h[highestRod.treeIdx].treeNodes[j].peakPtIdx; + for (int m = rodArcTrees_h[highestRod.treeIdx].treeNodes[j].startPtIdx; m <= rodArcTrees_h[highestRod.treeIdx].treeNodes[j].endPtIdx; m++) + scanLines[m][ptIdx].nPointIdx |= 1; + scanLines[centerLineIdx][ptIdx].nPointIdx |= 0x10; + } + //ñ־debug + for (int i = 0; i < (int)validVRod.size(); i++) + { + int treeIdx = validVRod[i].treeIdx; + int nodeNum = (int)rodArcTrees_v[treeIdx].treeNodes.size(); + for (int j = 0; j < nodeNum; j++) + { + int lineIdx = rodArcTrees_v[treeIdx].treeNodes[j].lineIdx; + int centerPtIdx = rodArcTrees_v[treeIdx].treeNodes[j].peakPtIdx; + for (int m = rodArcTrees_v[treeIdx].treeNodes[j].startPtIdx; m <= rodArcTrees_v[treeIdx].treeNodes[j].endPtIdx; m++) + scanLines[lineIdx][m].nPointIdx = 2; + scanLines[lineIdx][centerPtIdx].nPointIdx |= 0x20; + } + } + } + + SSG_cornerParam weldPointCornerParam; + memset(&weldPointCornerParam, 0, sizeof(SSG_cornerParam)); + weldPointCornerParam.cornerTh = 45; //45Ƚ + weldPointCornerParam.scale = 2.0; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + //㺸λ + for (int i = 0; i < (int)validVRod.size(); i++) + { + //ÿһ߸ֽཻĸֽһ + SSX_weldSeamInfo a_weldInfo = _computeWeldPoint( + highestRod, validVRod[i], + weldPointCornerParam, + rodParam, + scanLines); + if (a_weldInfo.weldType != KeWD_WELD_UNKNOWN) + weldSeamInfo.push_back(a_weldInfo); + } + + std::vector crossPoints; + std::vector vectorPositions; + for (int i = 0; i < (int)validVRod.size(); i++) + { + //㽻 + SVzNL3DPoint crossPt = computeLineCrossPt_abs( + highestRod.xyLine_a, highestRod.xyLine_b, highestRod.xyLine_c, + validVRod[i].xyLine_a, validVRod[i].xyLine_b, validVRod[i].xyLine_c); + int posIdx = -1; + SWD3DPointPostion nearestLinePt_topRebar = _computeNearestPoint(highestRod, crossPt, &posIdx); + crossPoints.push_back(nearestLinePt_topRebar); + + + //㴹ֱϵڼ㺸췽 + SVzNL3DPoint vec_axial = { highestRod.line_end.point.x - highestRod.line_start.point.x, + highestRod.line_end.point.y - highestRod.line_start.point.y, + highestRod.line_end.point.z - highestRod.line_start.point.z }; + SVzNL3DPoint vec_vRod = { validVRod[i].line_end.point.x - validVRod[i].line_start.point.x, + validVRod[i].line_end.point.y - validVRod[i].line_start.point.y, + validVRod[i].line_end.point.z - validVRod[i].line_start.point.z }; + SVzNL3DPoint v = vec3_cross(vec_axial, vec_vRod); + + SVzNL3DPosition a_vectorPosition; + a_vectorPosition.nPointIdx = posIdx; + a_vectorPosition.pt3D = v; + vectorPositions.push_back(a_vectorPosition); + //ֽĺ + //SWD3DPointPostion nearestLinePt_btmRebar = _computeNearestPoint(validVRod[i], crossPt); + std::vector a_rebarWeldSeam; + _computeRebarWeldSeam( + validVRod[i], + v, + true, + crossPt, + -1, + a_rebarWeldSeam, + scanLines, + rodParam, + weldPointCornerParam, + weldSeanRange //ֽķΧֵ + ); + weldSeamInfo.insert(weldSeamInfo.end(), a_rebarWeldSeam.begin(), a_rebarWeldSeam.end()); + } + + for (int i = 0; i < (int)crossPoints.size(); i++) + { + if (vectorPositions[i].nPointIdx < 0) + continue; + for (int j = i + 1; j < (int)crossPoints.size(); j++) + { + if (vectorPositions[j].nPointIdx < 0) + continue; + + double cross_dist = _compute2DDistance(crossPoints[i].point, crossPoints[j].point); + if (cross_dist < rodParam.diameter) + vectorPositions[j].nPointIdx = -1; + } + } + for (int i = 0; i < (int)crossPoints.size(); i++) + { + if (vectorPositions[i].nPointIdx < 0) + continue; + + //ֽĺ + std::vector a_rebarWeldSeam_0; + _computeRebarWeldSeam( + highestRod, + vectorPositions[i].pt3D, + true, + crossPoints[i].point, + vectorPositions[i].nPointIdx, + a_rebarWeldSeam_0, + scanLines, + rodParam, + weldPointCornerParam, + weldSeanRange //ֽķΧֵ + ); + weldSeamInfo.insert(weldSeamInfo.end(), a_rebarWeldSeam_0.begin(), a_rebarWeldSeam_0.end()); + //һ + std::vector a_rebarWeldSeam_1; + _computeRebarWeldSeam( + highestRod, + vectorPositions[i].pt3D, + false, + crossPoints[i].point, + vectorPositions[i].nPointIdx, + a_rebarWeldSeam_1, + scanLines, + rodParam, + weldPointCornerParam, + weldSeanRange //ֽķΧֵ + ); + weldSeamInfo.insert(weldSeamInfo.end(), a_rebarWeldSeam_1.begin(), a_rebarWeldSeam_1.end()); + } } diff --git a/sourceCode/rodAndBarDetection_Export.h b/sourceCode/rodAndBarDetection_Export.h index 3ec68bd..1cf9f5b 100644 --- a/sourceCode/rodAndBarDetection_Export.h +++ b/sourceCode/rodAndBarDetection_Export.h @@ -36,12 +36,24 @@ typedef struct SVzNL3DPoint endPt; }SSX_rodPositionInfo; +typedef enum +{ + KeWD_WELD_UNKNOWN = 0, + KeWD_WELD_POINT, // + KeWD_WELD_SEAM, // + KeWD_WELD_SEAM_GAP, //죬з϶ +}EWD_weldType; typedef struct { + EWD_weldType weldType; SVzNL3DPoint startPt; SVzNL3DPoint endPt; SVzNL3DPoint center; + SVzNL3DPoint axialDir; SVzNL3DPoint normalDir; // + std::vector< SVzNL3DPoint> centerPts; + std::vector< SVzNL3DPoint> edgePts_0; + std::vector< SVzNL3DPoint> edgePts_1; }SSX_weldSeamInfo; //汾 @@ -97,5 +109,6 @@ SG_APISHARED_EXPORT void sx_rebarWeldSeamPositioning( const SSG_outlierFilterParam filterParam, const SSG_treeGrowParam growParam, const SSX_rodParam rodParam, + const double weldSeanRange, //ֽķΧֵ std::vector& weldSeamInfo, int* errCode); \ No newline at end of file diff --git a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp index efa24f2..a9a34ef 100644 --- a/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp +++ b/workpieceHolePositioning_test/workpieceHolePositioning_test.cpp @@ -566,7 +566,7 @@ void TuoPuFa_holePosition_test(void) }; SVzNLRange fileIdx[TPF_TEST_GROUP] = { - {6,6}, {1, 16}, {18,18} + {6,6}, {1, 16}, {1,17} }; const char* ver = wd_workpieceHolePositioningVersion(); @@ -734,7 +734,7 @@ void TuoPuFa_holePosition_test(void) #endif #if TEST_COMPUTE_HOLE - for (int grp = 2; grp <= 2; grp++) + for (int grp = 1; grp <= 2; grp++) { SSG_planeCalibPara groundCalibPara; //初始化成单位阵