From 94e8a1efe8f65a195ed7aeae75dbab06de45d7cd Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Thu, 11 Sep 2025 19:37:22 +0800 Subject: [PATCH] =?UTF-8?q?=E6=90=AD=E6=8E=A5=E7=84=8A=E7=BC=9D=E6=A3=80?= =?UTF-8?q?=E6=B5=8B=E7=AE=97=E6=B3=95=E6=8F=90=E4=BA=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../lapWeldDetection_test.cpp | 115 ++++++--- sourceCode/SG_baseAlgo_Export.h | 2 +- sourceCode/SG_baseFunc.cpp | 10 +- sourceCode/SG_lineFeature.cpp | 14 +- sourceCode/SX_lapWeldDetection.cpp | 244 ++++++++++++++---- sourceCode/SX_lapWeldDetection_Export.h | 8 + 6 files changed, 299 insertions(+), 94 deletions(-) diff --git a/lapWeldDetection_test/lapWeldDetection_test.cpp b/lapWeldDetection_test/lapWeldDetection_test.cpp index 266ef49..7ba8ba9 100644 --- a/lapWeldDetection_test/lapWeldDetection_test.cpp +++ b/lapWeldDetection_test/lapWeldDetection_test.cpp @@ -149,7 +149,8 @@ void _outputScanDataFile_vector_h(char* fileName, std::vector>& scanLines, - std::vector< std::vector> weldOPs) + std::vector< std::vector> weldOPs, + bool outDebugInfo) { int lineNum = (int)scanLines.size(); std::ofstream sw(fileName); @@ -191,18 +192,38 @@ void _outputRGBDScanLapWeld_RGBD( for (int i = 0; i < linePtNum; i++) { SVzNL3DPosition* pt3D = &scanLines[line][i]; + if (pt3D->nPointIdx > 0) + int kkk = 1; int featureType_v = pt3D->nPointIdx & 0xffff; int featureType_h = featureType_v >> 4; featureType_v &= 0xff; - if (LINE_FEATURE_PEAK_TOP == featureType_v) + if (true == outDebugInfo) { - rgb = { 255, 97, 0 }; - size = 5; - } - else if (LINE_FEATURE_PEAK_TOP == featureType_h) - { - rgb = { 97, 255, 0 }; - size = 5; + if (LINE_FEATURE_L_JUMP_H2L == featureType_v) + { + rgb = { 255, 97, 0 }; + size = 5; + } + else if (LINE_FEATURE_L_JUMP_L2H == featureType_v) + { + rgb = objColor[7]; + size = 5; + } + else if (LINE_FEATURE_L_JUMP_H2L == featureType_h) + { + rgb = objColor[6]; + size = 5; + } + else if (LINE_FEATURE_L_JUMP_L2H == featureType_h) + { + rgb = { 97, 255, 0 }; + size = 5; + } + else + { + rgb = { 200, 200, 200 }; + size = 1; + } } else { @@ -225,11 +246,10 @@ void _outputRGBDScanLapWeld_RGBD( linePtNum += (int)weldOPs[i].size(); sw << "Line_" << lineNum << "_0_" << linePtNum + 1 << std::endl; - rgb = { 0, 0, 255 }; + rgb = { 255, 0, 0 }; size = 25; for (int i = 0; i < weldNum; i++) { - rgb = objColor[i % 8]; for (int j = 0; j < (int)weldOPs[i].size(); j++) { float x = (float)weldOPs[i][j].x; @@ -241,13 +261,37 @@ void _outputRGBDScanLapWeld_RGBD( } } //加一个点，用于跳过显示工具bug - rgb = objColor[0]; float x = (float)weldOPs[0][0].x; float y = (float)weldOPs[0][0].y; float z = (float)weldOPs[0][0].z; sw << "{" << x << "," << y << "," << z << "}-"; sw << "{0,0}-{0,0}-"; sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + + double exLen = 5.0; + rgb = objColor[0]; + size = 3; + for (int i = 0; i < weldNum; i++) + { + SVzNL3DPoint pt0 = weldOPs[i][0]; + SVzNL3DPoint pt1 = weldOPs[i].back(); + double len = sqrt(pow(pt0.x - pt1.x, 2) + pow(pt0.y - pt1.y, 2)); + double k = exLen / len; + sw << "Poly_" << i << "_2" << std::endl; + double x = -k * (pt1.x - pt0.x) + pt0.x; + double y = -k * (pt1.y - pt0.y) + pt0.y; + double z = -k * (pt1.z - pt0.z) + pt0.z; + sw << "{" << (float)x << "," << (float)y << "," << (float)z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + + x = -k * (pt0.x - pt1.x) + pt1.x; + y = -k * (pt0.y - pt1.y) + pt1.y; + z = -k * (pt0.z - pt1.z) + pt1.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + } } sw.close(); } @@ -655,7 +699,7 @@ void _convertToGridData(std::vector< std::vector>& scanLines, #define CONVERT_TO_GRID 0 #define TEST_COMPUTE_CALIB_PARA 0 -#define TEST_COMPUTE_GLOVE_POSITION 1 +#define TEST_COMPUTE_POSITION 1 #define TEST_GROUP 1 int main() { @@ -665,7 +709,7 @@ int main() }; SVzNLRange fileIdx[TEST_GROUP] = { - {0,6} + {1,6} }; #if CONVERT_TO_GRID @@ -720,7 +764,7 @@ int main() int kkk = 1; //行处理 //调平，去除地面 - sx_lineDataR(scanData[i], calibPara.planeCalib, -1);// calibPara.planeHeight); + sx_lineDataR(scanData[i], calibPara.planeCalib, -1); // calibPara.planeHeight); } // char calibFile[250]; @@ -734,7 +778,7 @@ int main() } #endif -#if TEST_COMPUTE_GLOVE_POSITION +#if TEST_COMPUTE_POSITION for (int grp = 0; grp <= 0; grp++) { SSG_planeCalibPara poseCalibPara; @@ -771,7 +815,7 @@ int main() int kkk = 1; //行处理 //调平，去除地面 - sx_lineDataR(scanLines[i], poseCalibPara.planeCalib, -1);// calibPara.planeHeight); + sx_lineDataR(scanLines[i], poseCalibPara.planeCalib, -1); } #if 0 char _out_file[256]; @@ -779,24 +823,26 @@ int main() int headNullLines = 0; _outputScanDataFile_vector(_out_file, scanLines, false, &headNullLines); #endif - SSG_treeGrowParam growParam; - growParam.maxLineSkipNum = 5; - growParam.yDeviation_max = 1.0; - growParam.maxSkipDistance = 5.0; - growParam.zDeviation_max = 2;// algoParam.bagParam.bagH / 2; //袋子高度1/2 - growParam.minLTypeTreeLen = 30.0; //mm - growParam.minVTypeTreeLen = 30.0; //mm - SSG_cornerParam cornerParam; - cornerParam.cornerTh =30; //45度角 - cornerParam.scale = 4; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; - cornerParam.minEndingGap = 20;// algoParam.bagParam.bagW / 4; - cornerParam.minEndingGap_z = 20; // algoParam.bagParam.bagH / 4; - cornerParam.jumpCornerTh_1 = 10; - cornerParam.jumpCornerTh_2 = 30; + SSX_lapWeldParam lapWeldParam; - lapWeldParam.lapHeight = 2.0; - lapWeldParam.weldMinLen = 2.0; + lapWeldParam.lapHeight = 1.5; + lapWeldParam.weldMinLen = 80.0; lapWeldParam.weldRefPoints = 2; + lapWeldParam.scanMode = keSX_ScanMode_V; + SSG_cornerParam cornerParam; + cornerParam.cornerTh = 25; //45度角 + cornerParam.scale = 4; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + cornerParam.minEndingGap = 6;// algoParam.bagParam.bagW / 4; + cornerParam.minEndingGap_z = lapWeldParam.lapHeight; // algoParam.bagParam.bagH / 4; + cornerParam.jumpCornerTh_1 = 10; + cornerParam.jumpCornerTh_2 = 25; + SSG_treeGrowParam growParam; + growParam.maxLineSkipNum = 10; + growParam.yDeviation_max = 3.0; + growParam.maxSkipDistance = 5.0; + growParam.zDeviation_max = 1.0;// algoParam.bagParam.bagH / 2; //袋子高度1/2 + growParam.minLTypeTreeLen = lapWeldParam.weldMinLen; //mm + growParam.minVTypeTreeLen = lapWeldParam.weldMinLen; //mm int errCode = 0; std::vector> weldOps; sx_getLapWeldPostion( @@ -804,6 +850,7 @@ int main() cornerParam, growParam, lapWeldParam, + poseCalibPara, weldOps, &errCode); @@ -811,7 +858,7 @@ int main() printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1)); //输出测试结果 sprintf_s(_scan_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx); - _outputRGBDScanLapWeld_RGBD(_scan_file, scanLines, weldOps); + _outputRGBDScanLapWeld_RGBD(_scan_file, scanLines, weldOps, false); } } #endif diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index d04c772..faf5070 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -74,7 +74,7 @@ SG_APISHARED_EXPORT void sg_getLineJumpFeature_cornerMethod( int dataSize, int lineIdx, const SSG_cornerParam cornerPara, //scaleͨȡbagH1/4 - std::vector< SSG_basicFeature1D> jumpFeatures); + std::vector< SSG_basicFeature1D>& jumpFeatures); SG_APISHARED_EXPORT void wd_getLineGloveArcs( std::vector& lineData, diff --git a/sourceCode/SG_baseFunc.cpp b/sourceCode/SG_baseFunc.cpp index f251c97..9a7aa81 100644 --- a/sourceCode/SG_baseFunc.cpp +++ b/sourceCode/SG_baseFunc.cpp @@ -1507,6 +1507,8 @@ SSG_planeCalibPara sg_getPlaneCalibPara2( //ݽת SVzNLRangeD calibZRange = { 0, -1 }; topZRange = { 0, -1 }; + double sumMeanZ = 0; + int sumSize = 0; for (int i = 0, i_max = (int)Points3ds.size(); i < i_max; i++) { //z @@ -1531,6 +1533,8 @@ SSG_planeCalibPara sg_getPlaneCalibPara2( { calibZRange.min = a_calibPt.z; calibZRange.max = a_calibPt.z; + sumMeanZ += a_calibPt.z; + sumSize++; } else { @@ -1538,9 +1542,13 @@ SSG_planeCalibPara sg_getPlaneCalibPara2( calibZRange.min = a_calibPt.z; if (calibZRange.max < a_calibPt.z) calibZRange.max = a_calibPt.z; + sumMeanZ += a_calibPt.z; + sumSize++; } } - planePara.planeHeight = calibZRange.min; + if (sumSize > 0) + sumMeanZ = sumMeanZ / (double)sumSize; + planePara.planeHeight = sumMeanZ; // calibZRange.min; return planePara; } diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index f39f7ac..ba3fde9 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -1455,7 +1455,7 @@ void sg_getLineJumpFeature_cornerMethod( int dataSize, int lineIdx, const SSG_cornerParam cornerPara, //scaleͨȡbagH1/4 - std::vector< SSG_basicFeature1D> jumpFeatures) + std::vector< SSG_basicFeature1D>& jumpFeatures) { //ȥ std::vector< SVzNL3DPosition> vldPts; @@ -1775,13 +1775,14 @@ void sg_getLineJumpFeature_cornerMethod( int pntIdx_2 = cornerFeatures[i+1].pntIdx; //߶ double dist = abs(vldPts[pntIdx_1].pt3D.y - vldPts[pntIdx_2].pt3D.y); - double height = cornerFeatures[i + 1].forward_z - cornerFeatures[i].backward_z; - if ((dist < cornerPara.minEndingGap) && (height < -cornerPara.minEndingGap_z)) + double height = abs(cornerFeatures[i + 1].forward_z - cornerFeatures[i].backward_z); + if ((dist < cornerPara.minEndingGap) && (height > cornerPara.minEndingGap_z)) { SSG_basicFeature1D a_feature; a_feature.featureType = LINE_FEATURE_L_JUMP_L2H; a_feature.jumpPos = vldPts[pntIdx_1].pt3D; - a_feature.jumpPos2D = { lineIdx, pntIdx_1 }; + a_feature.jumpPos2D = { lineIdx, vldPts[pntIdx_1].nPointIdx }; + a_feature.featureValue = cornerFeatures[i].backward_z; jumpFeatures.push_back(a_feature); } } @@ -1794,13 +1795,14 @@ void sg_getLineJumpFeature_cornerMethod( int pntIdx_2 = cornerFeatures[i + 1].pntIdx; //߶ double dist = abs(vldPts[pntIdx_1].pt3D.y - vldPts[pntIdx_2].pt3D.y); - double height = cornerFeatures[i + 1].forward_z - cornerFeatures[i].backward_z; + double height = abs(cornerFeatures[i + 1].forward_z - cornerFeatures[i].backward_z); if ((dist < cornerPara.minEndingGap) && (height > cornerPara.minEndingGap_z)) { SSG_basicFeature1D a_feature; a_feature.featureType = LINE_FEATURE_L_JUMP_H2L; a_feature.jumpPos = vldPts[pntIdx_2].pt3D; - a_feature.jumpPos2D = { lineIdx, pntIdx_2 }; + a_feature.jumpPos2D = { lineIdx, vldPts[pntIdx_2].nPointIdx }; + a_feature.featureValue = cornerFeatures[i + 1].forward_z; jumpFeatures.push_back(a_feature); } } diff --git a/sourceCode/SX_lapWeldDetection.cpp b/sourceCode/SX_lapWeldDetection.cpp index 03f0a63..f9f33c6 100644 --- a/sourceCode/SX_lapWeldDetection.cpp +++ b/sourceCode/SX_lapWeldDetection.cpp @@ -23,12 +23,78 @@ void sx_lineDataR( lineDataRT_vector(a_line, camPoseR, groundH); } +SVzNL2DPoint getFootPoint(double x0, double y0, double k, double b) +{ + double A = k; + double B = -1; + double C = b; + SVzNL2DPoint foot; + foot.x = (B * B * x0 - A * B * y0 - A * C) / (A * A + B * B); + foot.y = (-A * B * x0 + A * A * y0 - B * C) / (A * A + B * B); + return foot; +} + +double getWinMeanZ(std::vector& points, int startIdx, int win) +{ + int size = (int)points.size(); + double sumZ = 0; + double sumSize = 0; + for (int i = 0; i < win; i++) + { + int idx = i + startIdx; + if (idx < size) + { + if (points[idx].z > 1e-4) + { + sumZ += points[idx].z; + sumSize++; + } + } + } + if (sumSize > 0) + sumZ = sumZ / (double)sumSize; + return sumZ; +} +//ӺɨȡյϢ +void getWeldPoint(std::vector& a_weld_contour, int midPtNum, std::vector& a_weld) +{ + if (a_weld_contour.size() == 0) + return; + + //ֱ + double k, b; + lineFitting(a_weld_contour, &k, &b); + //Ѱյ + SVzNL3DPoint startPt = a_weld_contour[0]; + SVzNL3DPoint endPt = a_weld_contour.back(); + SVzNL2DPoint foot_s = getFootPoint(startPt.x, startPt.y, k, b); + SVzNL2DPoint foot_e = getFootPoint(endPt.x, endPt.y, k, b); + double meanZ_s = getWinMeanZ(a_weld_contour, 0, 5); + double meanZ_e = getWinMeanZ(a_weld_contour, (int)a_weld_contour.size()-1-5, 5); + + SVzNL3DPoint pt_0 = { foot_s.x, foot_s.y, meanZ_s }; + SVzNL3DPoint pt_1 = { foot_e.x, foot_e.y, meanZ_e }; + a_weld.push_back(pt_0); + double ratio = 1.0 / ((double)midPtNum + 1.0); + + for (int i = 1; i <= midPtNum; i++) + { + SVzNL3DPoint a_pt; + a_pt.x = (double)i * ratio * (pt_1.x - pt_0.x) + pt_0.x; + a_pt.y = (double)i * ratio * (pt_1.y - pt_0.y) + pt_0.y; + a_pt.z = (double)i * ratio * (pt_1.z - pt_0.z) + pt_0.z; + a_weld.push_back(a_pt); + } + a_weld.push_back(pt_1); +} + //ȡӺ void sx_getLapWeldPostion( std::vector< std::vector>& scanLines, const SSG_cornerParam cornerPara, SSG_treeGrowParam growParam, SSX_lapWeldParam lapWeldParam, + SSG_planeCalibPara groundCalibPara, std::vector>& objOps, int* errCode) { @@ -42,26 +108,39 @@ void sx_getLapWeldPostion( int linePtNum = (int)scanLines[0].size(); bool isGridData = true; - //ֱarcȡ + //ֱȡ std::vector> jumpFeatures_v; - for (int line = 0; line < lineNum; line++) + if ((keSX_ScanMode_V == lapWeldParam.scanMode) || (keSX_ScanMode_Both == lapWeldParam.scanMode)) { - if (line == 400) - int kkk = 1; + for (int line = 0; line < lineNum; line++) + { + if (line == 400) + int kkk = 1; - std::vector& lineData = scanLines[line]; - if (linePtNum != (int)lineData.size()) - isGridData = false; + std::vector& lineData = scanLines[line]; + if (linePtNum != (int)lineData.size()) + isGridData = false; - std::vector a_line_features; - int dataSize = (int)lineData.size(); - sg_getLineJumpFeature_cornerMethod( - &scanLines[line][0], - dataSize, - line, - cornerPara, //scaleͨȡbagH1/4 - a_line_features); - jumpFeatures_v.push_back(a_line_features); + std::vector a_line_features; + int dataSize = (int)lineData.size(); + sg_getLineJumpFeature_cornerMethod( + &scanLines[line][0], + dataSize, + line, + cornerPara, //scaleͨȡbagH1/4 + a_line_features); + //˳ + std::vector vld_features; + for (int i = 0, i_max = a_line_features.size(); i < i_max; i++) + { + //γɵȥ + if (a_line_features[i].featureValue < (groundCalibPara.planeHeight - 0.25)) + { + vld_features.push_back(a_line_features[i]); + } + } + jumpFeatures_v.push_back(vld_features); + } } if (false == isGridData)//ݲʽ @@ -88,55 +167,77 @@ void sx_getLapWeldPostion( //ˮƽarcȡ std::vector> jumpFeatures_h; int lineNum_h = (int)hLines.size(); - for (int line = 0; line < lineNum_h; line++) + if ((keSX_ScanMode_H == lapWeldParam.scanMode) || (keSX_ScanMode_Both == lapWeldParam.scanMode)) { - std::vector& lineData = hLines[line]; + for (int line = 0; line < lineNum_h; line++) + { + std::vector& lineData = hLines[line]; - std::vector a_line_features; - int dataSize = (int)lineData.size(); - sg_getLineJumpFeature_cornerMethod( - &hLines[line][0], - dataSize, - line, - cornerPara, //scaleͨȡbagH1/4 - a_line_features); + std::vector a_line_features; + int dataSize = (int)lineData.size(); + sg_getLineJumpFeature_cornerMethod( + &hLines[line][0], + dataSize, + line, + cornerPara, //scaleͨȡbagH1/4 + a_line_features); - //ƥ - jumpFeatures_h.push_back(a_line_features); + //˳ + std::vector vld_features; + for (int i = 0, i_max = a_line_features.size(); i < i_max; i++) + { + //γɵȥ + if (a_line_features[i].featureValue < (groundCalibPara.planeHeight - 0.25)) + { + vld_features.push_back(a_line_features[i]); + } + } + jumpFeatures_h.push_back(vld_features); + } } // //ֱ߷ std::vector v_trees; - for (int line = 0; line < lineNum; line++) + if ((keSX_ScanMode_V == lapWeldParam.scanMode) || (keSX_ScanMode_Both == lapWeldParam.scanMode)) { - bool isLastLine = false; - if (line == lineNum - 1) - isLastLine = true; - std::vector& a_lineJumpFeature = jumpFeatures_v[line]; - sg_lineFeaturesGrowing( - line, - isLastLine, - a_lineJumpFeature, - v_trees, - growParam); + for (int line = 0; line < lineNum; line++) + { + bool isLastLine = false; + if (line == lineNum - 1) + isLastLine = true; + std::vector& a_lineJumpFeature = jumpFeatures_v[line]; + if (a_lineJumpFeature.size() > 0) + int kkk = 1; + if (line == 400) + int kkk = 1; + sg_lineFeaturesGrowing( + line, + isLastLine, + a_lineJumpFeature, + v_trees, + growParam); + } } //ˮƽɨ˶ std::vector h_trees; - for (int line = 0; line < lineNum_h; line++) + if ((keSX_ScanMode_H == lapWeldParam.scanMode) || (keSX_ScanMode_Both == lapWeldParam.scanMode)) { - if (line == 650) - int kkk = 1; - bool isLastLine = false; - if (line == lineNum_h - 1) - isLastLine = true; - std::vector& a_lineJumpFeature = jumpFeatures_h[line]; - sg_lineFeaturesGrowing( - line, - isLastLine, - a_lineJumpFeature, - h_trees, - growParam); + for (int line = 0; line < lineNum_h; line++) + { + if (line == 650) + int kkk = 1; + bool isLastLine = false; + if (line == lineNum_h - 1) + isLastLine = true; + std::vector& a_lineJumpFeature = jumpFeatures_h[line]; + sg_lineFeaturesGrowing( + line, + isLastLine, + a_lineJumpFeature, + h_trees, + growParam); + } } //treeϢ @@ -159,6 +260,8 @@ void sx_getLapWeldPostion( a_treeInfo.eLineIdx = a_vTree->eLineIdx; a_treeInfo.roi = a_vTree->roi; allTreesInfo.push_back(a_treeInfo); + + std::vector a_weld_contour; //ԭʼϱǣͬʱMaskϱ for (int j = 0, j_max = (int)a_vTree->treeNodes.size(); j < j_max; j++) { @@ -172,8 +275,13 @@ void sx_getLapWeldPostion( scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx &= 0xffff; scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx += hvTreeIdx << 16; } + a_weld_contour.push_back(scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D); } } + std::vector a_weld; + getWeldPoint(a_weld_contour, lapWeldParam.weldRefPoints, a_weld); + if (a_weld.size() > 0) + objOps.push_back(a_weld); hvTreeIdx++; } int hTreeStart = hvTreeIdx; @@ -193,6 +301,8 @@ void sx_getLapWeldPostion( a_treeInfo.roi.top = a_hTree->roi.left; a_treeInfo.roi.bottom = a_hTree->roi.right; allTreesInfo.push_back(a_treeInfo); + + std::vector a_weld_contour; //ԭʼϱǣͬʱMaskϱ for (int j = 0, j_max = (int)a_hTree->treeNodes.size(); j < j_max; j++) { @@ -206,11 +316,41 @@ void sx_getLapWeldPostion( scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx &= 0xffff; scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += hvTreeIdx << 16; } + a_weld_contour.push_back(scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D); } } + std::vector a_weld; + getWeldPoint(a_weld_contour, lapWeldParam.weldRefPoints, a_weld); + if (a_weld.size() > 0) + objOps.push_back(a_weld); + hvTreeIdx++; } int hvTreeSize = hvTreeIdx; + //ͶԭϵԱ۱궨ȷ + for (int i = 0; i < lineNum; i++) + sx_lineDataR(scanLines[i], groundCalibPara.invRMatrix, -1); + //Ͷԭϵ + for (int i = 0, i_max = (int)objOps.size(); i < i_max; i++) + { + std::vector& a_weld = objOps[i]; + for (int j = 0, j_max = a_weld.size(); j < j_max; j++) + { + double x = a_weld[j].x * groundCalibPara.invRMatrix[0] + + a_weld[j].y * groundCalibPara.invRMatrix[1] + + a_weld[j].z * groundCalibPara.invRMatrix[2]; + double y = a_weld[j].x * groundCalibPara.invRMatrix[3] + + a_weld[j].y * groundCalibPara.invRMatrix[4] + + a_weld[j].z * groundCalibPara.invRMatrix[5]; + double z = a_weld[j].x * groundCalibPara.invRMatrix[6] + + a_weld[j].y * groundCalibPara.invRMatrix[7] + + a_weld[j].z * groundCalibPara.invRMatrix[8]; + a_weld[j].x = x; + a_weld[j].y = y; + a_weld[j].z = z; + } + } + // } \ No newline at end of file diff --git a/sourceCode/SX_lapWeldDetection_Export.h b/sourceCode/SX_lapWeldDetection_Export.h index 7c60495..d6570e6 100644 --- a/sourceCode/SX_lapWeldDetection_Export.h +++ b/sourceCode/SX_lapWeldDetection_Export.h @@ -10,11 +10,18 @@ #include #include +typedef enum +{ + keSX_ScanMode_V = 0, //ߴֱɨ + keSX_ScanMode_H, //ƽкɨ + keSX_ScanMode_Both, //дֱɨ裬Ҳˮƽɨ +} ESX_WeldScanMode; typedef struct { double lapHeight;//Ӻ double weldMinLen; //С쳤ȣڹ˿ܵٺ int weldRefPoints; //ֱߺIJο㣬Ĭ2յ㣩 + ESX_WeldScanMode scanMode; }SSX_lapWeldParam; //һƽƽ @@ -35,6 +42,7 @@ SG_APISHARED_EXPORT void sx_getLapWeldPostion( const SSG_cornerParam cornerPara, SSG_treeGrowParam growParam, SSX_lapWeldParam lapWeldParam, + SSG_planeCalibPara groundCalibPara, std::vector>& objOps, int* errCode);