From 79a7d6c49a1e06438261ec1f4509119b4ff7a3d1 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Fri, 18 Sep 2026 13:28:09 +0800 Subject: [PATCH] =?UTF-8?q?hybridPosePositioning=20version=201.4.0=20:=20?= =?UTF-8?q?=E8=BD=AC=E5=AD=90=E9=92=A2=E8=8A=AF=EF=BC=8C=20=E6=B7=BB?= =?UTF-8?q?=E5=8A=A0=E4=BA=86=E9=9A=94=E6=9D=BF=E7=9A=84=E8=AF=86=E5=88=AB?= =?UTF-8?q?=E5=92=8C=E5=AE=9A=E4=BD=8D=E3=80=82=20=E5=BD=93=E5=AE=9A?= =?UTF-8?q?=E4=BD=8D=E9=9A=94=E6=9D=BF=E6=97=B6=EF=BC=8C=E5=B7=A5=E4=BB=B6?= =?UTF-8?q?=E7=B1=BB=E5=9E=8B=E4=B8=BA9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../hybridPosePositioning_test.cpp | 177 +++- sourceCode/SG_baseAlgo_Export.h | 12 +- sourceCode/SG_lineFeature.cpp | 2 + sourceCode/dataFitting.cpp | 90 ++ sourceCode/hybridPosePositioning.cpp | 973 ++++++++++++------ sourceCode/hybridPosePositioning_Export.h | 3 +- 6 files changed, 929 insertions(+), 328 deletions(-) diff --git a/hybridPosePositioning_test/hybridPosePositioning_test.cpp b/hybridPosePositioning_test/hybridPosePositioning_test.cpp index 5998521..f572584 100644 --- a/hybridPosePositioning_test/hybridPosePositioning_test.cpp +++ b/hybridPosePositioning_test/hybridPosePositioning_test.cpp @@ -720,6 +720,8 @@ void _outputRGBDResult_XYZ_RGBD_workpieceInfo( pt3D->pt3D = { 0.0, 0.0, 0.0 }; int flag = pt3D->nPointIdx & 0xffff; int highFlag = pt3D->nPointIdx >> 16; + int spacerFlag = highFlag & 0x4; + highFlag &= 0x3; #if 0 if (flag > 0) { @@ -742,6 +744,11 @@ void _outputRGBDResult_XYZ_RGBD_workpieceInfo( rgb = { 0, 0, 250 }; size = 2; } + if (spacerFlag > 0) + { + rgb = { 0, 250, 250 }; + size = 2; + } float x = (float)pt3D->pt3D.x; float y = (float)pt3D->pt3D.y; @@ -778,21 +785,68 @@ void _outputRGBDResult_XYZ_RGBD_workpieceInfo( //double dirLen = 500; for (int m = 0; m < (int)poseInfo.size(); m++) { - SVzNL3DPoint dirPt1 = { poseInfo[m].center.x + poseInfo[m].z_dir.x * dirLen, - poseInfo[m].center.y + poseInfo[m].z_dir.y * dirLen, - poseInfo[m].center.z + poseInfo[m].z_dir.z * dirLen }; - SVzNL3DPoint dirPt2 = { poseInfo[m].center.x - poseInfo[m].z_dir.x * dirLen, - poseInfo[m].center.y - poseInfo[m].z_dir.y * dirLen, - poseInfo[m].center.z - poseInfo[m].z_dir.z * dirLen }; + if (poseInfo[m].workpieceType != 9) + { + SVzNL3DPoint dirPt1 = { poseInfo[m].center.x + poseInfo[m].z_dir.x * dirLen, + poseInfo[m].center.y + poseInfo[m].z_dir.y * dirLen, + poseInfo[m].center.z + poseInfo[m].z_dir.z * dirLen }; + SVzNL3DPoint dirPt2 = { poseInfo[m].center.x - poseInfo[m].z_dir.x * dirLen, + poseInfo[m].center.y - poseInfo[m].z_dir.y * dirLen, + poseInfo[m].center.z - poseInfo[m].z_dir.z * dirLen }; - sw << "Poly_" << lineIdx << "_2" << std::endl; - sw << "{" << dirPt1.x << "," << dirPt1.y << "," << dirPt1.z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; - sw << "{" << dirPt2.x << "," << dirPt2.y << "," << dirPt2.z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; - lineIdx++; + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << dirPt1.x << "," << dirPt1.y << "," << dirPt1.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << dirPt2.x << "," << dirPt2.y << "," << dirPt2.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + } + else + { + SVzNL3DPoint dirPt1 = { poseInfo[m].center.x + poseInfo[m].z_dir.x * dirLen, + poseInfo[m].center.y + poseInfo[m].z_dir.y * dirLen, + poseInfo[m].center.z + poseInfo[m].z_dir.z * dirLen }; + SVzNL3DPoint dirPt2 = { poseInfo[m].center.x - poseInfo[m].z_dir.x * dirLen, + poseInfo[m].center.y - poseInfo[m].z_dir.y * dirLen, + poseInfo[m].center.z - poseInfo[m].z_dir.z * dirLen }; + + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << dirPt1.x << "," << dirPt1.y << "," << dirPt1.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << dirPt2.x << "," << dirPt2.y << "," << dirPt2.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + double sapcerDirLen = 200; + dirPt1 = { poseInfo[m].center.x + poseInfo[m].x_dir.x * sapcerDirLen, + poseInfo[m].center.y + poseInfo[m].x_dir.y * sapcerDirLen, + poseInfo[m].center.z + poseInfo[m].x_dir.z * sapcerDirLen }; + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << poseInfo[m].center.x << "," << poseInfo[m].center.y << "," << poseInfo[m].center.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << dirPt1.x << "," << dirPt1.y << "," << dirPt1.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + dirPt2 = { poseInfo[m].center.x + poseInfo[m].y_dir.x * sapcerDirLen, + poseInfo[m].center.y + poseInfo[m].y_dir.y * sapcerDirLen, + poseInfo[m].center.z + poseInfo[m].y_dir.z * sapcerDirLen }; + + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << poseInfo[m].center.x << "," << poseInfo[m].center.y << "," << poseInfo[m].center.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << dirPt2.x << "," << dirPt2.y << "," << dirPt2.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + } } if (binInfo) @@ -1132,7 +1186,42 @@ void _outputWorkpieceInfo(char* fileName, std::vector< WD_workpieceInfo>& workpi int number = (int)workpiecePositions.size(); for (int i = 0; i < number; i++) { - sprintf_s(dataStr, 250, "工件_%d: id=%d, type=%d", i + 1, workpiecePositions[i].id, workpiecePositions[i].workpieceType); + //0:中央; 1-上边; 2-下边; 3-左边; 4-右边; 5-左上角; 6-右上角; 7-左下角; 8-右下角; 9-隔板 + switch (workpiecePositions[i].workpieceType) + { + case 1: + sprintf_s(dataStr, 250, "工件_%d: id=%d, type= 上边", i + 1, workpiecePositions[i].id); + break; + case 2: + sprintf_s(dataStr, 250, "工件_%d: id=%d, type= 下边", i + 1, workpiecePositions[i].id); + break; + case 3: + sprintf_s(dataStr, 250, "工件_%d: id=%d, type= 左边", i + 1, workpiecePositions[i].id); + break; + case 4: + sprintf_s(dataStr, 250, "工件_%d: id=%d, type= 右边", i + 1, workpiecePositions[i].id); + break; + case 5: + sprintf_s(dataStr, 250, "工件_%d: id=%d, type= 左上角", i + 1, workpiecePositions[i].id); + break; + case 6: + sprintf_s(dataStr, 250, "工件_%d: id=%d, type= 右上角", i + 1, workpiecePositions[i].id); + break; + case 7: + sprintf_s(dataStr, 250, "工件_%d: id=%d, type= 左下角", i + 1, workpiecePositions[i].id); + break; + case 8: + sprintf_s(dataStr, 250, "工件_%d: id=%d, type= 右下角", i + 1, workpiecePositions[i].id); + break; + case 9: + sprintf_s(dataStr, 250, "工件_%d: id=%d, type= 隔板", i + 1, workpiecePositions[i].id); + break; + default: + sprintf_s(dataStr, 250, "工件_%d: id=%d, type= 中央", i + 1, workpiecePositions[i].id); + break; + + } + //sprintf_s(dataStr, 250, "工件_%d: id=%d, type=%d", i + 1, workpiecePositions[i].id, workpiecePositions[i].workpieceType); sw << dataStr << std::endl; sprintf_s(dataStr, 50, " center: (%g, %g, %g)", workpiecePositions[i].center.x, workpiecePositions[i].center.y, workpiecePositions[i].center.z); @@ -2168,7 +2257,7 @@ void HaiRuiMa_groundCalib_test(void) printf("ver:%s\n", ver); char _calib_datafile[256]; - sprintf_s(_calib_datafile, "F:/ShangGu/项目/冠钦项目/宁波海瑞马/转子钢芯/WorkpieceRotor/转子线圈调平.txt"); + sprintf_s(_calib_datafile, "F:/ShangGu/项目/冠钦项目/宁波海瑞马/转子钢芯/2026-09-海瑞马/09-14中/调平.txt"); int lineNum = 0; float lineV = 0.0f; int dataCalib = 0; @@ -2188,9 +2277,9 @@ void HaiRuiMa_groundCalib_test(void) SSG_planeCalibPara calibPara = wd_getGroundCalibPara(scanData); // char calibFile[250]; - sprintf_s(calibFile, "F:/ShangGu/项目/冠钦项目/宁波海瑞马/转子钢芯/WorkpieceRotor/ground_calib_para.txt"); + sprintf_s(calibFile, "F:/ShangGu/项目/冠钦项目/宁波海瑞马/转子钢芯/2026-09-海瑞马/09-14中/ground_calib_para.txt"); _outputCalibPara(calibFile, calibPara); -#if 1 +#if 0 for (int grp = 0; grp < HRM_GroundCalib_TEST_GROUP; grp++) { for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) @@ -2248,24 +2337,26 @@ void HaiRuiMa_groundCalib_test(void) } //拓普发工件孔定位（工件定位) -#define HRM_RotorCore_TEST_GROUP 3 +#define HRM_RotorCore_TEST_GROUP 5 void HaiRuiMa_rotorCorePositioning_test(void) { const char* dataPath[HRM_RotorCore_TEST_GROUP] = { "F:/ShangGu/项目/冠钦项目/宁波海瑞马/转子钢芯/数据2D3D/", //0 "F:/ShangGu/项目/冠钦项目/宁波海瑞马/转子钢芯/WorkpieceRotor/", //1 - "F:/ShangGu/项目/冠钦项目/宁波海瑞马/转子钢芯/现场数据/" + "F:/ShangGu/项目/冠钦项目/宁波海瑞马/转子钢芯/现场数据/", //2 + "F:/ShangGu/项目/冠钦项目/宁波海瑞马/转子钢芯/2026-09-海瑞马/09-14/", //3 + "F:/ShangGu/项目/冠钦项目/宁波海瑞马/转子钢芯/2026-09-海瑞马/09-14中/", //4 }; SVzNLRange fileIdx[HRM_RotorCore_TEST_GROUP] = { - {1,10}, {3,70},{2,2} + {1,10}, {3,70},{2,2}, {1,47}, {1,54} }; const char* ver = wd_hybridPositioningVersion(); printf("ver:%s\n", ver); - for (int grp = 2; grp < HRM_RotorCore_TEST_GROUP; grp++) + for (int grp = 3; grp < HRM_RotorCore_TEST_GROUP; grp++) { SSG_planeCalibPara groundCalibPara; //初始化成单位阵 @@ -2300,6 +2391,7 @@ void HaiRuiMa_rotorCorePositioning_test(void) int headNullLines = 0; sprintf_s(_scan_file, "%s%d_cloud.txt", dataPath[grp], fidx); _outputScanDataFile_SVzNLPositionD(_scan_file, scanLines, false, &headNullLines); + continue; #endif WD_HRM_BinInfo binInfo; @@ -2329,16 +2421,36 @@ void HaiRuiMa_rotorCorePositioning_test(void) binInfo = _readBinInfo(calibFile); } #endif - sprintf_s(_scan_file, "%s%d_result.txt", dataPath[grp], fidx); + if( (grp >=3) && (grp <= 4)) + sprintf_s(_scan_file, "%s%d_origin_result.txt", dataPath[grp], fidx); + else + sprintf_s(_scan_file, "%s%d_result.txt", dataPath[grp], fidx); std::vector objROIs; - if( (grp == 0) && (fidx <= 9)) + if( ((grp == 0) && (fidx <= 9)) || ((grp >=3)&&(grp<=4))) vzReadObj2DROI(_scan_file, objROIs); else vzReadObj2DROI_1(_scan_file, objROIs); SWD_Cylinder workpieceParam; //圆柱形工件的标称半径和高度 - workpieceParam.radius = 45.0; - workpieceParam.height = 20.0; + if (grp == 3) + { + workpieceParam.radius = 60.0; + workpieceParam.height = 70.0; + } + else if (grp == 4) + { + workpieceParam.radius = 42.0; + workpieceParam.height = 50.0; + } + else + { + workpieceParam.radius = 45.0; + workpieceParam.height = 20.0; + } + + SVzNLSizeD spacerParam; //隔板参数：长和宽 + spacerParam.dWidth = 720.0; + spacerParam.dHeight = 510.0; double layerRatio = 0.5; //分层系数，分层门限=分层系数*工件高度；当两个工件的高度差大于分层门限时，判断为在不同层 const double distToSideRatio = 1.1; //边距系数，边距门限=边距系数*工件直径；当工件距边的距离大于边距门限时，判断为边上的工件 @@ -2352,6 +2464,7 @@ void HaiRuiMa_rotorCorePositioning_test(void) scanLines, objROIs, groundCalibPara, + spacerParam, workpieceParam, layerRatio, distToSideRatio, @@ -2369,7 +2482,15 @@ void HaiRuiMa_rotorCorePositioning_test(void) else if (errCode == SX_ERR_ZERO_OBJECTS) printf("%s: %d(ms), 无产品!\n", _scan_file, (int)(t2 - t1)); else - printf("%s: %d(ms), errCode=%d ...", _scan_file, (int)(t2 - t1), errCode); + { + int num = (int)workpiecePositions.size(); + if(num == 0) + printf("%s: %d(ms), errCode=%d 结果: 0 ...", _scan_file, (int)(t2 - t1), errCode); + else if( (num == 1) && (workpiecePositions[0].workpieceType == 9)) + printf("%s: %d(ms), errCode=%d 结果: 隔板 ...", _scan_file, (int)(t2 - t1), errCode); + else + printf("%s: %d(ms), errCode=%d 结果: %d...", _scan_file, (int)(t2 - t1), errCode, num); + } //输出测试结果 sprintf_s(calibFile, "%sresult/LaserLine%d_workpiece_info.txt", dataPath[grp], fidx); _outputWorkpieceInfo(calibFile, workpiecePositions); @@ -2757,6 +2878,4 @@ int main() HaiRuiMa_workpieceSizeCompute_test(); HaiRuiMa_positionPlanning_test(); } - - } diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index cfd82f6..fe7d6a9 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -816,6 +816,9 @@ SG_APISHARED_EXPORT double computeXOYVertexAngle(SVzNL3DPoint p0, SVzNL3DPoint p //㵽ֱ߾ SG_APISHARED_EXPORT double computePtDistToLine(double x0, double y0, double a, double b, double c); +//㵽ֱ߾룬ؾķ +SG_APISHARED_EXPORT double computePtDistToLine_2(double x0, double y0, double a, double b, double c, int* sign); + //㴹㣬ֱ߷̣y = kx + b SG_APISHARED_EXPORT SVzNL2DPointD sx_getFootPoint( double x0, @@ -874,7 +877,14 @@ SG_APISHARED_EXPORT Plane ransacFitPlane( int max_iter = 1000, // int stop_no_improve = 250 // ٴǰ˳ ); - +SG_APISHARED_EXPORT Plane ransacFitPlane_2( + const std::vector& points, + std::vector& in_inliers, + std::vector& out_inliers, + float dist_thresh = 0.3f, // ڵֵ + int max_iter = 1000, // + int stop_no_improve = 250 // ٴǰ˳ +); // 룺3D std::vector // axis λ, centroid SG_APISHARED_EXPORT void computeCylinderAxisFromIncompletePCA( diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index be110e3..e41376d 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -10173,6 +10173,8 @@ void wd_getLineVFlaws( //gaussһ׺Ͷ׵ wd_convolveGauss_laserLine(a_line, startId, endId, deriv_1_narrow, deriv_2_narrow, chkWidth_narrow, narrow_scale); + if ((deriv_1_narrow.size() == 0) || (deriv_2_narrow.size() == 0)) + return; // //peak: ؼͰݼͬʱҪʹ std::vector deriv_1_peaks; diff --git a/sourceCode/dataFitting.cpp b/sourceCode/dataFitting.cpp index df4a176..26292e5 100644 --- a/sourceCode/dataFitting.cpp +++ b/sourceCode/dataFitting.cpp @@ -938,6 +938,96 @@ Plane ransacFitPlane( return best_plane; } +// ƽ +Plane planeFrom3Points_2(const SVzNL3DPosition& p1, const SVzNL3DPosition& p2, const SVzNL3DPosition& p3) { + double v1x = p2.pt3D.x - p1.pt3D.x; + double v1y = p2.pt3D.y - p1.pt3D.y; + double v1z = p2.pt3D.z - p1.pt3D.z; + + double v2x = p3.pt3D.x - p1.pt3D.x; + double v2y = p3.pt3D.y - p1.pt3D.y; + double v2z = p3.pt3D.z - p1.pt3D.z; + + double A = v1y * v2z - v1z * v2y; + double B = v1z * v2x - v1x * v2z; + double C = v1x * v2y - v1y * v2x; + double D = -(A * p1.pt3D.x + B * p1.pt3D.y + C * p1.pt3D.z); + + double norm = sqrtf(A * A + B * B + C * C); + if (norm > 1e-6) { A /= norm; B /= norm; C /= norm; D /= norm; } + return Plane(A, B, C, D); +} +// 㵽ƽľ루ֵ +float pointToPlaneDistance_2(const SVzNL3DPosition& p, const Plane& plane) { + return fabs(plane.A * p.pt3D.x + plane.B * p.pt3D.y + plane.C * p.pt3D.z + plane.D) + / sqrt(plane.A * plane.A + plane.B * plane.B + plane.C * plane.C); +} +Plane ransacFitPlane_2( + const std::vector& points, + std::vector& in_inliers, + std::vector& out_inliers, + float dist_thresh, // ڵֵ + int max_iter, // + int stop_no_improve // ٴǰ˳ +) +{ + in_inliers.clear(); + int n = points.size(); + if (n < 3) return Plane(); + + int best_inlier = 0; + Plane best_plane; + + srand((unsigned)time(nullptr)); + + int no_improve_count = 0; // + + for (int i = 0; i < max_iter; ++i) { + // 3ظ + int idx[3]; + idx[0] = rand() % n; + do { idx[1] = rand() % n; } while (idx[1] == idx[0]); + do { idx[2] = rand() % n; } while (idx[2] == idx[0] || idx[2] == idx[1]); + + Plane plane = planeFrom3Points_2( + points[idx[0]], + points[idx[1]], + points[idx[2]] + ); + + // ͳڵ + int cnt = 0; + for (auto& p : points) { + if (pointToPlaneDistance_2(p, plane) < dist_thresh) cnt++; + } + + // ģ + if (cnt > best_inlier) { + best_inlier = cnt; + best_plane = plane; + no_improve_count = 0; // + } + else { + no_improve_count++; + } + + // ====================== ǰֹ ====================== + if (no_improve_count >= stop_no_improve) { + break; + } + } + + // ռڵ + for (auto& p : points) { + if (pointToPlaneDistance_2(p, best_plane) < dist_thresh) + in_inliers.push_back(p); + else + out_inliers.push_back(p); + } + + return best_plane; +} + // 룺3D std::vector // axis λ, centroid void computeCylinderAxisFromIncompletePCA( diff --git a/sourceCode/hybridPosePositioning.cpp b/sourceCode/hybridPosePositioning.cpp index c65f46b..4aca26b 100644 --- a/sourceCode/hybridPosePositioning.cpp +++ b/sourceCode/hybridPosePositioning.cpp @@ -22,7 +22,8 @@ //version 1.3.9 : תӸо ĽϿⷽ //version 1.3.10 : תӸо ֲϵڲתΪ //version 1.3.11 : תӸо ߾ϵڲתΪ -std::string m_strVersion = "HybridPositioning 1.3.11"; +//version 1.4.0 : תӸо ˸ʶͶλ λʱΪ9 +std::string m_strVersion = "HybridPositioning 1.4.0"; const char* wd_hybridPositioningVersion(void) { return m_strVersion.c_str(); @@ -96,6 +97,21 @@ int _get2DRegion(SVzNLPositionD& a_pt2D3D, std::vector& objROIs) return -1; } +SSG_ROIRectD _getListROI(std::vector< SVzNL3DPosition>& listData) +{ + if (listData.size() == 0) + return { 0,0,0,0 }; + SSG_ROIRectD roi = { listData[0].pt3D.x, listData[0].pt3D.x, listData[0].pt3D.y, listData[0].pt3D.y }; + for (int i = 0; i < (int)listData.size(); i++) + { + roi.left = roi.left > listData[i].pt3D.x ? listData[i].pt3D.x : roi.left; + roi.right = roi.right < listData[i].pt3D.x ? listData[i].pt3D.x : roi.right; + roi.top = roi.top > listData[i].pt3D.y ? listData[i].pt3D.y : roi.top; + roi.bottom = roi.bottom < listData[i].pt3D.y ? listData[i].pt3D.y : roi.bottom; + } + return roi; +} + bool _compareByZValue(SVzNL3DPosition& a, SVzNL3DPosition& b) { return a.pt3D.z < b.pt3D.z; @@ -1086,10 +1102,327 @@ WD_HRM_BinInfo wd_HRM_getBinSize_2( return resultPose; } +// +void spacerBoardDetection( + std::vector>& scanLinesIn, + const SVzNLSizeD spacerParam, // + int xDownSample, int chkWin, double localR, + const SVzNL3DPoint calib_board_normal, const double spacerAngleRange, + WD_HRM_BinInfo& spacerInfo, + std::vector& spacerPoints) +{ + //3:1 + int lineNum = (int)scanLinesIn.size() / xDownSample; + int linePtNum = (int)scanLinesIn[0].size(); + std::vector> scanLines; + scanLines.resize(lineNum); + for (int line = 0; line < lineNum; line++) + { + //scanLines[line].resize(linePtNum); + scanLines[line].insert(scanLines[line].end(), scanLinesIn[line * 3].begin(), scanLinesIn[line * 3].end()); + } + std::vector> localNormals; + localNormals.resize(lineNum); + + for (int line = 0; line < lineNum; line++) + { + localNormals[line].resize(linePtNum); + for (int j = 0; j < (int)scanLines[line].size(); j++) + { + SVzNL3DPosition a_normal; + SVzNL3DPoint currPt = scanLines[line][j].pt3D; + if (currPt.z < 1e-4) + { + a_normal.nPointIdx = -1; + a_normal.pt3D = { 0.0, 0.0, 0.0 }; + localNormals[line][j] = a_normal; + continue; + } + //ľֲ + //ȡֲ + std::vector localPoints; +#if 1 //ȡʮϵĵ + for (int m = 0; m <= chkWin; m++) + { + int win_line = line - m; + if ((win_line < 0) || (win_line >= lineNum)) + continue; + int win_ptIdx = j; + if (scanLines[win_line][win_ptIdx].pt3D.z < 1e-4) + continue; + +#if 1 + double r = abs(scanLines[win_line][win_ptIdx].pt3D.x - currPt.x) + + abs(scanLines[win_line][win_ptIdx].pt3D.y - currPt.y) + abs(scanLines[win_line][win_ptIdx].pt3D.z - currPt.z); +#else + double r = sqrt(pow(scanLines[win_line][win_ptIdx].pt3D.x - currPt.x, 2) + + pow(scanLines[win_line][win_ptIdx].pt3D.y - currPt.y, 2) + pow(scanLines[win_line][win_ptIdx].pt3D.z - currPt.z, 2)); +#endif + if (r <= localR) + localPoints.push_back(scanLines[win_line][win_ptIdx]); + else + break; + } + for (int m = 0; m <= chkWin; m++) + { + int win_line = line + m; + if ((win_line < 0) || (win_line >= lineNum)) + continue; + int win_ptIdx = j; + if (scanLines[win_line][win_ptIdx].pt3D.z < 1e-4) + continue; + +#if 1 + double r = abs(scanLines[win_line][win_ptIdx].pt3D.x - currPt.x) + + abs(scanLines[win_line][win_ptIdx].pt3D.y - currPt.y) + abs(scanLines[win_line][win_ptIdx].pt3D.z - currPt.z); +#else + double r = sqrt(pow(scanLines[win_line][win_ptIdx].pt3D.x - currPt.x, 2) + + pow(scanLines[win_line][win_ptIdx].pt3D.y - currPt.y, 2) + pow(scanLines[win_line][win_ptIdx].pt3D.z - currPt.z, 2)); +#endif + if (r <= localR) + localPoints.push_back(scanLines[win_line][win_ptIdx]); + else + break; + } + for (int m = 0; m <= chkWin; m++) + { + int win_line = line; + int win_ptIdx = j - m; + if ((win_ptIdx < 0) || (win_ptIdx >= (int)scanLines[line].size())) + continue; + + if (scanLines[win_line][win_ptIdx].pt3D.z < 1e-4) + continue; + +#if 1 + double r = abs(scanLines[win_line][win_ptIdx].pt3D.x - currPt.x) + + abs(scanLines[win_line][win_ptIdx].pt3D.y - currPt.y) + abs(scanLines[win_line][win_ptIdx].pt3D.z - currPt.z); +#else + double r = sqrt(pow(scanLines[win_line][win_ptIdx].pt3D.x - currPt.x, 2) + + pow(scanLines[win_line][win_ptIdx].pt3D.y - currPt.y, 2) + pow(scanLines[win_line][win_ptIdx].pt3D.z - currPt.z, 2)); +#endif + if (r <= localR) + localPoints.push_back(scanLines[win_line][win_ptIdx]); + else + break; + } + for (int m = 0; m <= chkWin; m++) + { + int win_line = line; + int win_ptIdx = j + m; + if ((win_ptIdx < 0) || (win_ptIdx >= (int)scanLines[line].size())) + continue; + + if (scanLines[win_line][win_ptIdx].pt3D.z < 1e-4) + continue; + +#if 1 + double r = abs(scanLines[win_line][win_ptIdx].pt3D.x - currPt.x) + + abs(scanLines[win_line][win_ptIdx].pt3D.y - currPt.y) + abs(scanLines[win_line][win_ptIdx].pt3D.z - currPt.z); +#else + double r = sqrt(pow(scanLines[win_line][win_ptIdx].pt3D.x - currPt.x, 2) + + pow(scanLines[win_line][win_ptIdx].pt3D.y - currPt.y, 2) + pow(scanLines[win_line][win_ptIdx].pt3D.z - currPt.z, 2)); +#endif + if (r <= localR) + localPoints.push_back(scanLines[win_line][win_ptIdx]); + else + break; + } + +#else + for (int m = -chkWin; m <= chkWin; m++) + { + int win_line = line + m; + if ((win_line < 0) || (win_line >= lineNum)) + continue; + for (int n = -chkWin; n <= chkWin; n++) + { + int win_ptIdx = j + n; + if ((win_ptIdx < 0) || (win_ptIdx >= (int)scanLines[line].size())) + continue; + + if (scanLines[win_line][win_ptIdx].pt3D.z < 1e-4) + continue; + +#if 1 + double r = abs(scanLines[win_line][win_ptIdx].pt3D.x - currPt.x) + + abs(scanLines[win_line][win_ptIdx].pt3D.y - currPt.y) + abs(scanLines[win_line][win_ptIdx].pt3D.z - currPt.z); +#else + double r = sqrt(pow(scanLines[win_line][win_ptIdx].pt3D.x - currPt.x, 2) + + pow(scanLines[win_line][win_ptIdx].pt3D.y - currPt.y, 2) + pow(scanLines[win_line][win_ptIdx].pt3D.z - currPt.z, 2)); +#endif + if(r <= localR ) + localPoints.push_back(scanLines[win_line][win_ptIdx]); + } + } +#endif + SVzNL3DPoint vec_normal, vec_centroi; + computePlaneNormalByPCA( localPoints, vec_normal, vec_centroi); + if (vec_centroi.z < 1e-4) + { + a_normal.nPointIdx = -1; + a_normal.pt3D = { 0.0, 0.0, 0.0 }; + } + else + { + a_normal.nPointIdx = 0; + a_normal.pt3D = vec_normal; + } + localNormals[line][j] = a_normal; + } + } + + //ȥбϵĵ + std::vector validPoints; + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < (int)scanLines[line].size(); j++) + { + SVzNL3DPosition a_normal = localNormals[line][j]; + if (a_normal.nPointIdx < 0) + scanLines[line][j].pt3D.z = 0.0; + else + { + float angle = vec3_computeVecAngle(a_normal.pt3D, calib_board_normal); + if (angle > 90) + angle = 180 - angle; + if(angle > spacerAngleRange) + scanLines[line][j].pt3D.z = 0.0; + } + if (scanLines[line][j].pt3D.z > 1e-4) + { + SVzNL3DPosition a_pt; + a_pt.nPointIdx = (line << 16) | j; + a_pt.pt3D = scanLines[line][j].pt3D; + validPoints.push_back(a_pt); + } + } + } + // + int clusterCheckWin = 3; + double clusterDist = 3.0; + int distType = 1; //0 - 2d distance; 1- 3d distance + std::vector> objClusters; //result + wd_pointClustering_speedUp( + validPoints, + lineNum, linePtNum, clusterCheckWin, // + clusterDist, + distType, + objClusters //result + ); + + //Ϊspacer + int maxSizeId = -1; + int maxSize = 0; + for (int i = 0; i < (int)objClusters.size(); i++) + { + int size = objClusters[i].size(); + if ( maxSize < size) + { + maxSize = size; + maxSizeId = i; + } + } + + SSG_ROIRectD spacer_roi = _getListROI(objClusters[maxSizeId]); + double roi_xw = spacer_roi.right - spacer_roi.left; + double roi_yw = spacer_roi.bottom - spacer_roi.top; + double roi_w = roi_xw < roi_yw ? roi_yw : roi_xw; + double roi_h = roi_xw > roi_yw ? roi_yw : roi_xw; + double ratio_w = roi_w / spacerParam.dWidth; + double ratio_h = roi_h / spacerParam.dHeight; + if ((ratio_w > 0.8) && (ratio_h / 0.8)) + { + SVzNLRangeD spacer_zRange; + double spacer_meanZ = _getListMeanZ(objClusters[maxSizeId], spacer_zRange); + + //СӾ + std::vector points; + for (int i = 0; i < (int)objClusters[maxSizeId].size(); i++) + { + cv::Point2f a_pt = cv::Point2f(objClusters[maxSizeId][i].pt3D.x, objClusters[maxSizeId][i].pt3D.y); + points.push_back(a_pt); + } + if (points.size() == 0) + { + return; + } + + cv::RotatedRect rect = minAreaRect(points); + cv::Point2f vertices[4]; + rect.points(vertices); + double width = rect.size.width; //ͶӰĿ͸ + double height = rect.size.height; + if (width < height) + { + double tmp = height; + height = width; + width = tmp; + } + //̬vertices[0]ת + double dist_v0v3 = sqrt(pow(vertices[0].x - vertices[3].x, 2) + pow(vertices[0].y - vertices[3].y, 2)); + double width_diff = abs(dist_v0v3 - width); + double pose_yaw; + if (CV_VERSION == "3.2.0") + { + if (width_diff < 10.0)//width + { + pose_yaw = -rect.angle; + } + else //ȷ + { + pose_yaw = -rect.angle - 90; + if (pose_yaw < -90) + pose_yaw = 180 + pose_yaw; + } + } + else //if (CV_VERSION == "4.8.0") + { + if (width_diff < 10.0) //width + { + pose_yaw = -rect.angle; + } + else//ȷ + { + pose_yaw = -rect.angle + 90; + if (pose_yaw > 90) + pose_yaw = pose_yaw - 180; + } + } + //ϿϢ + double sinTheta = sin(-PI * pose_yaw / 180); + double cosTheta = cos(-PI * pose_yaw / 180); + spacerInfo.center = { rect.center.x, rect.center.y, spacer_meanZ }; + spacerInfo.bottomNormal = { 0.0, 0.0, 1.0 }; + spacerInfo.x_dir = { cosTheta , sinTheta , 0.0 }; + spacerInfo.y_dir = vec3_cross(spacerInfo.bottomNormal, spacerInfo.x_dir); //˳y_dir + spacerInfo.length = width; + spacerInfo.width = height; + spacerInfo.binTopZ = spacer_zRange.min; + spacerInfo.minRectVertex[0] = { vertices[0].x, vertices[0].y, spacer_meanZ }; + spacerInfo.minRectVertex[1] = { vertices[1].x, vertices[1].y, spacer_meanZ }; + spacerInfo.minRectVertex[2] = { vertices[2].x, vertices[2].y, spacer_meanZ }; + spacerInfo.minRectVertex[3] = { vertices[3].x, vertices[3].y, spacer_meanZ }; + + //пӻ + spacerPoints.clear(); + for (int i = 0; i < (int)objClusters[maxSizeId].size(); i++) + { + int line = objClusters[maxSizeId][i].nPointIdx >> 16; + int ptIdx = objClusters[maxSizeId][i].nPointIdx & 0xffff; + objClusters[maxSizeId][i].nPointIdx = ((line * xDownSample) << 16) | ptIdx; + } + spacerPoints.insert(spacerPoints.end(), objClusters[maxSizeId].begin(), objClusters[maxSizeId].end()); + } + + return; +} + void wd_HRM_RotorCorePositioning( std::vector< std::vector>& scanLinesInput, std::vector& objROIs, const SSG_planeCalibPara groundCalibPara, + const SVzNLSizeD spacerParam, //Ϳ const SWD_Cylinder workpieceParam, //Բιıư뾶͸߶ const double layerRatio, //ֲϵֲ=ֲϵ*߶ȣĸ߶ȲڷֲʱжΪڲͬ const double distToSideRatio, //߾ϵ߾=߾ϵ*ֱߵľڱ߾ʱжΪϵĹ @@ -1103,12 +1436,6 @@ void wd_HRM_RotorCorePositioning( { *errCode = 0; - if (objROIs.size() == 0) - { - *errCode = SX_ERR_ZERO_2D_OBJECTS; - return; - } - //жݸʽǷΪgrid㷨ֻܴgridݸʽ int lineNum = (int)scanLinesInput.size(); int linePtNum = (int)scanLinesInput[0].size(); @@ -1312,6 +1639,9 @@ void wd_HRM_RotorCorePositioning( scanLines[line][j].nPointIdx = 0; //ԭʼݵ0תʹã } //ֱ߶ȥ + //ɸ + std::vector> spacerDetectPoints; + spacerDetectPoints.resize(scanLines.size()); for (int line = 0; line < lineNum; line++) { for (int j = 0; j < linePtNum; j++) @@ -1319,6 +1649,29 @@ void wd_HRM_RotorCorePositioning( if (flags[line][j] > 0) scanLines[line][j].pt3D.z = 0; } + spacerDetectPoints[line].insert(spacerDetectPoints[line].end(), scanLines[line].begin(), scanLines[line].end()); + } + + //⣬ʹRANSACƽ棬 ԭǸ峤ʹú + WD_HRM_BinInfo spacerInfo; + memset(&spacerInfo, 0, sizeof(WD_workpieceInfo)); + + int chkWin = 15; + double localR = 5.0; + SVzNL3DPoint vec_norm = { 0.0, 0.0, -1.0 }; + SVzNL3DPoint calib_board_normal = wd_ptRotate(vec_norm, groundCalibPara.invRMatrix); + double spacerAngleRange = 10.0; + int xDownSample = 3;//ɨ跽31 + std::vector spacerPoints; + spacerBoardDetection( spacerDetectPoints, spacerParam, xDownSample, chkWin, localR, calib_board_normal, spacerAngleRange, spacerInfo, spacerPoints); + if (spacerInfo.center.z > 1e-4) + { + for (int i = 0; i < (int)spacerPoints.size(); i++) + { + int ptLine = spacerPoints[i].nPointIdx >> 16; + int ptIdx = spacerPoints[i].nPointIdx & 0xffff; + scanLines[ptLine][ptIdx].nPointIdx |= 0x40000; + } } int objNum = (int)objROIs.size(); @@ -1446,300 +1799,338 @@ void wd_HRM_RotorCorePositioning( allWorkpiece.push_back(a_workpiece); } + if (allWorkpiece.size() == 0) - return; - - // - //Ŀ - double highest_z = allWorkpiece[0].center.z; - for (int i = 1; i < (int)allWorkpiece.size(); i++) - highest_z = highest_z > allWorkpiece[i].center.z ? allWorkpiece[i].center.z : highest_z; - - //ϿϢ - SVzNLRangeD zSliceRange; - zSliceRange.min = highest_z - workpieceParam.height; - zSliceRange.max = groundCalibPara.planeHeight - groundRemoveOffset; - binInfo = wd_HRM_getBinSize_2(dataCopy, zSliceRange); - - //z - std::vector< WD_workpieceInfo> zSortWorkpiece; - double topLayerTh = highest_z + workpieceParam.height * layerRatio; - for (int i = 0; i < (int)allWorkpiece.size(); i++) { - if (allWorkpiece[i].center.z < topLayerTh) - zSortWorkpiece.push_back(allWorkpiece[i]); - } - - if (binInfo.binTopZ > 1e-4) - { - //㹤λ - std::vector linePoint; - linePoint.resize(4); - linePoint[0].pt1 = binInfo.minRectVertex[0]; - linePoint[0].pt2 = binInfo.minRectVertex[1]; - linePoint[1].pt1 = binInfo.minRectVertex[1]; - linePoint[1].pt2 = binInfo.minRectVertex[2]; - linePoint[2].pt1 = binInfo.minRectVertex[2]; - linePoint[2].pt2 = binInfo.minRectVertex[3]; - linePoint[3].pt1 = binInfo.minRectVertex[3]; - linePoint[3].pt2 = binInfo.minRectVertex[0]; - int lineTopIdx = -1; - for (int i = 0; i < (int)linePoint.size(); i++) + //жϸ + if (spacerInfo.center.z > 1e-4) // { - if ((linePoint[i].pt1.y < binInfo.center.y) && (linePoint[i].pt2.y < binInfo.center.y)) - { - lineTopIdx = i; - break; - } + WD_workpieceInfo a_workpiece; + a_workpiece.center = spacerInfo.center; + a_workpiece.id = 0; + a_workpiece.workpieceType = 9; // + a_workpiece.value = spacerInfo.binTopZ; + a_workpiece.x_dir = spacerInfo.x_dir; + a_workpiece.y_dir = spacerInfo.y_dir; + a_workpiece.z_dir = spacerInfo.bottomNormal; + workpiecePositions.push_back(a_workpiece); } - if (lineTopIdx < 0) + else { - *errCode = SX_ERR_BIN_EDGE_ERROR; - //תȥ - int workpieceNum = (int)workpiecePositions.size(); - for (int i = 0; i < workpieceNum; i++) - { - SVzNL3DPoint rpt; - rpt = wd_ptRotate(workpiecePositions[i].center, groundCalibPara.invRMatrix); - workpiecePositions[i].center = rpt; - rpt = wd_ptRotate(workpiecePositions[i].y_dir, groundCalibPara.invRMatrix); - workpiecePositions[i].y_dir = rpt; - rpt = wd_ptRotate(workpiecePositions[i].z_dir, groundCalibPara.invRMatrix); - workpiecePositions[i].z_dir = rpt; - rpt = wd_ptRotate(workpiecePositions[i].x_dir, groundCalibPara.invRMatrix); - workpiecePositions[i].x_dir = rpt; - } + *errCode = SX_ERR_ZERO_OBJECTS; return; } - SWD_3DPointPair lineTop = linePoint[lineTopIdx]; - linePoint.erase(linePoint.begin() + lineTopIdx);//ȥѾʶedge - - int lineBtmIdx = -1; - for (int i = 0; i < (int)linePoint.size(); i++) - { - if ((linePoint[i].pt1.y > binInfo.center.y) && (linePoint[i].pt2.y > binInfo.center.y)) - { - lineBtmIdx = i; - break; - } - } - if (lineBtmIdx < 0) - { - *errCode = SX_ERR_BIN_EDGE_ERROR; - //תȥ - int workpieceNum = (int)workpiecePositions.size(); - for (int i = 0; i < workpieceNum; i++) - { - SVzNL3DPoint rpt; - rpt = wd_ptRotate(workpiecePositions[i].center, groundCalibPara.invRMatrix); - workpiecePositions[i].center = rpt; - rpt = wd_ptRotate(workpiecePositions[i].y_dir, groundCalibPara.invRMatrix); - workpiecePositions[i].y_dir = rpt; - rpt = wd_ptRotate(workpiecePositions[i].z_dir, groundCalibPara.invRMatrix); - workpiecePositions[i].z_dir = rpt; - rpt = wd_ptRotate(workpiecePositions[i].x_dir, groundCalibPara.invRMatrix); - workpiecePositions[i].x_dir = rpt; - } - return; - } - SWD_3DPointPair lineBtm = linePoint[lineBtmIdx]; - linePoint.erase(linePoint.begin() + lineBtmIdx); //ȥѾʶedge - - int lineLeftIdx = -1; - for (int i = 0; i < (int)linePoint.size(); i++) - { - if ((linePoint[i].pt1.x < binInfo.center.x) && (linePoint[i].pt2.x < binInfo.center.x)) - { - lineLeftIdx = i; - break; - } - } - if (lineLeftIdx < 0) - { - *errCode = SX_ERR_BIN_EDGE_ERROR; - //תȥ - int workpieceNum = (int)workpiecePositions.size(); - for (int i = 0; i < workpieceNum; i++) - { - SVzNL3DPoint rpt; - rpt = wd_ptRotate(workpiecePositions[i].center, groundCalibPara.invRMatrix); - workpiecePositions[i].center = rpt; - rpt = wd_ptRotate(workpiecePositions[i].y_dir, groundCalibPara.invRMatrix); - workpiecePositions[i].y_dir = rpt; - rpt = wd_ptRotate(workpiecePositions[i].z_dir, groundCalibPara.invRMatrix); - workpiecePositions[i].z_dir = rpt; - rpt = wd_ptRotate(workpiecePositions[i].x_dir, groundCalibPara.invRMatrix); - workpiecePositions[i].x_dir = rpt; - } - return; - } - SWD_3DPointPair lineLeft = linePoint[lineLeftIdx]; - linePoint.erase(linePoint.begin() + lineLeftIdx); //ȥѾʶedge - - int lineRightIdx = -1; - for (int i = 0; i < (int)linePoint.size(); i++) - { - if ((linePoint[i].pt1.x > binInfo.center.x) && (linePoint[i].pt2.x > binInfo.center.x)) - { - lineRightIdx = i; - break; - } - } - if (lineRightIdx < 0) - { - *errCode = SX_ERR_BIN_EDGE_ERROR; - //תȥ - int workpieceNum = (int)workpiecePositions.size(); - for (int i = 0; i < workpieceNum; i++) - { - SVzNL3DPoint rpt; - rpt = wd_ptRotate(workpiecePositions[i].center, groundCalibPara.invRMatrix); - workpiecePositions[i].center = rpt; - rpt = wd_ptRotate(workpiecePositions[i].y_dir, groundCalibPara.invRMatrix); - workpiecePositions[i].y_dir = rpt; - rpt = wd_ptRotate(workpiecePositions[i].z_dir, groundCalibPara.invRMatrix); - workpiecePositions[i].z_dir = rpt; - rpt = wd_ptRotate(workpiecePositions[i].x_dir, groundCalibPara.invRMatrix); - workpiecePositions[i].x_dir = rpt; - } - return; - } - SWD_3DPointPair lineRight = linePoint[lineRightIdx]; - //㹤λãϡϡ¡ĸǣߣ벿 - //е۶ԲֵͬĹòͬץȡ - double distToSideTh = (workpieceParam.radius * 2) * distToSideRatio; - double lineTop_a, lineTop_b, lineTop_c; - compute2ptLine(lineTop.pt1, lineTop.pt2, &lineTop_a, &lineTop_b, &lineTop_c); - double lineBtm_a, lineBtm_b, lineBtm_c; - compute2ptLine(lineBtm.pt1, lineBtm.pt2, &lineBtm_a, &lineBtm_b, &lineBtm_c); - double lineLeft_a, lineLeft_b, lineLeft_c; - compute2ptLine(lineLeft.pt1, lineLeft.pt2, &lineLeft_a, &lineLeft_b, &lineLeft_c); - double lineRight_a, lineRight_b, lineRight_c; - compute2ptLine(lineRight.pt1, lineRight.pt2, &lineRight_a, &lineRight_b, &lineRight_c); - std::vector< WD_workpieceInfo> topSide; //ϱ - std::vector< WD_workpieceInfo> btmSide; //± - std::vector< WD_workpieceInfo> leftSide; // - std::vector< WD_workpieceInfo> rightSide; //ұ - std::vector< WD_workpieceInfo> LTCorner; //Ͻ - std::vector< WD_workpieceInfo> RTCorner; //Ͻ - std::vector< WD_workpieceInfo> LBCorner; // - std::vector< WD_workpieceInfo> RBCorner; //ұ - std::vector< WD_workpieceInfo> centerRegion; //ұ - for (int i = 0; i < (int)zSortWorkpiece.size(); i++) //ϲĿз - { - SVzNL3DPoint& objCenter = zSortWorkpiece[i].center; - double distTop = computePtDistToLine(objCenter.x, objCenter.y, lineTop_a, lineTop_b, lineTop_c); - double distBtm = computePtDistToLine(objCenter.x, objCenter.y, lineBtm_a, lineBtm_b, lineBtm_c); - double distLeft = computePtDistToLine(objCenter.x, objCenter.y, lineLeft_a, lineLeft_b, lineLeft_c); - double distRight = computePtDistToLine(objCenter.x, objCenter.y, lineRight_a, lineRight_b, lineRight_c); - - if (distTop < distToSideTh) - { - if (distLeft < distToSideTh) - { - zSortWorkpiece[i].workpieceType = 5; // - LTCorner.push_back(zSortWorkpiece[i]); - } - else if (distRight < distToSideTh) - { - zSortWorkpiece[i].workpieceType = 6; // - RTCorner.push_back(zSortWorkpiece[i]); - } - else - { - zSortWorkpiece[i].workpieceType = 1; // - topSide.push_back(zSortWorkpiece[i]); - } - } - else if (distBtm < distToSideTh) - { - if (distLeft < distToSideTh) - { - zSortWorkpiece[i].workpieceType = 7; // - LBCorner.push_back(zSortWorkpiece[i]); - } - else if (distRight < distToSideTh) - { - zSortWorkpiece[i].workpieceType = 8; // - RBCorner.push_back(zSortWorkpiece[i]); - } - else - { - zSortWorkpiece[i].workpieceType = 2; // - btmSide.push_back(zSortWorkpiece[i]); - } - } - else if (distLeft < distToSideTh) - { - zSortWorkpiece[i].workpieceType = 3; // - leftSide.push_back(zSortWorkpiece[i]); - } - else if (distRight < distToSideTh) - { - zSortWorkpiece[i].workpieceType = 4; // - rightSide.push_back(zSortWorkpiece[i]); - } - else - { - zSortWorkpiece[i].workpieceType = 0; // - centerRegion.push_back(zSortWorkpiece[i]); - } - } - //벿ֹ - if (centerRegion.size() > 0) - { - std::vector distanceIndice; - distanceIndice.resize(centerRegion.size()); - for (int m = 0; m < (int)centerRegion.size(); m++) - { - distanceIndice[m].dist = sqrt(pow(centerRegion[m].center.x - binInfo.center.x, 2) + - pow(centerRegion[m].center.y - binInfo.center.y, 2)); - distanceIndice[m].idx = m; - } - std::sort(distanceIndice.begin(), distanceIndice.end(), _compareByDistance); - for (int m = 0; m < (int)distanceIndice.size(); m++) - { - int idx = distanceIndice[m].idx; - workpiecePositions.push_back(centerRegion[idx]); - } - } - if(topSide.size() > 0) - workpiecePositions.insert(workpiecePositions.end(), topSide.begin(), topSide.end()); - if (btmSide.size() > 0) - workpiecePositions.insert(workpiecePositions.end(), btmSide.begin(), btmSide.end()); - if(leftSide.size() >0) - workpiecePositions.insert(workpiecePositions.end(), leftSide.begin(), leftSide.end()); - if (rightSide.size() > 0) - workpiecePositions.insert(workpiecePositions.end(), rightSide.begin(), rightSide.end()); - if(LTCorner.size() >0) - workpiecePositions.insert(workpiecePositions.end(), LTCorner.begin(), LTCorner.end()); - if (RTCorner.size() > 0) - workpiecePositions.insert(workpiecePositions.end(), RTCorner.begin(), RTCorner.end()); - if (LBCorner.size() > 0) - workpiecePositions.insert(workpiecePositions.end(), LBCorner.begin(), LBCorner.end()); - if (RBCorner.size() > 0) - workpiecePositions.insert(workpiecePositions.end(), RBCorner.begin(), RBCorner.end()); } else { - //ˮƽ - while (zSortWorkpiece.size() > 0) + // + //Ŀ + double highest_z = allWorkpiece[0].center.z; + for (int i = 1; i < (int)allWorkpiece.size(); i++) + highest_z = highest_z > allWorkpiece[i].center.z ? allWorkpiece[i].center.z : highest_z; + + //жϵ + if ( (spacerInfo.binTopZ < highest_z) && (spacerInfo.center.z > 1e-4)) { - std::vector< WD_workpieceInfo> firstLine; - std::vector< WD_workpieceInfo> restWorkpiece; - _getYTopLine(zSortWorkpiece, firstLine, restWorkpiece, workpieceParam.radius); - workpiecePositions.insert(workpiecePositions.end(), firstLine.begin(), firstLine.end()); - zSortWorkpiece.clear(); - zSortWorkpiece.insert(zSortWorkpiece.end(), restWorkpiece.begin(), restWorkpiece.end()); + allWorkpiece.clear(); + WD_workpieceInfo a_workpiece; + a_workpiece.center = spacerInfo.center; + a_workpiece.id = 0; + a_workpiece.workpieceType = 9; // + a_workpiece.value = spacerInfo.binTopZ; + a_workpiece.x_dir = spacerInfo.x_dir; + a_workpiece.y_dir = spacerInfo.y_dir; + a_workpiece.z_dir = spacerInfo.bottomNormal; + workpiecePositions.push_back(a_workpiece); + } + else + { + //ϿϢ + SVzNLRangeD zSliceRange; + zSliceRange.min = highest_z - workpieceParam.height; + zSliceRange.max = groundCalibPara.planeHeight - groundRemoveOffset; + binInfo = wd_HRM_getBinSize_2(dataCopy, zSliceRange); + + //z + std::vector< WD_workpieceInfo> zSortWorkpiece; + double topLayerTh = highest_z + workpieceParam.height * layerRatio; + for (int i = 0; i < (int)allWorkpiece.size(); i++) + { + if (allWorkpiece[i].center.z < topLayerTh) + zSortWorkpiece.push_back(allWorkpiece[i]); + } + + if (binInfo.binTopZ > 1e-4) + { + //㹤λ + std::vector linePoint; + linePoint.resize(4); + linePoint[0].pt1 = binInfo.minRectVertex[0]; + linePoint[0].pt2 = binInfo.minRectVertex[1]; + linePoint[1].pt1 = binInfo.minRectVertex[1]; + linePoint[1].pt2 = binInfo.minRectVertex[2]; + linePoint[2].pt1 = binInfo.minRectVertex[2]; + linePoint[2].pt2 = binInfo.minRectVertex[3]; + linePoint[3].pt1 = binInfo.minRectVertex[3]; + linePoint[3].pt2 = binInfo.minRectVertex[0]; + int lineTopIdx = -1; + for (int i = 0; i < (int)linePoint.size(); i++) + { + if ((linePoint[i].pt1.y < binInfo.center.y) && (linePoint[i].pt2.y < binInfo.center.y)) + { + lineTopIdx = i; + break; + } + } + if (lineTopIdx < 0) + { + *errCode = SX_ERR_BIN_EDGE_ERROR; + //תȥ + int workpieceNum = (int)workpiecePositions.size(); + for (int i = 0; i < workpieceNum; i++) + { + SVzNL3DPoint rpt; + rpt = wd_ptRotate(workpiecePositions[i].center, groundCalibPara.invRMatrix); + workpiecePositions[i].center = rpt; + rpt = wd_ptRotate(workpiecePositions[i].y_dir, groundCalibPara.invRMatrix); + workpiecePositions[i].y_dir = rpt; + rpt = wd_ptRotate(workpiecePositions[i].z_dir, groundCalibPara.invRMatrix); + workpiecePositions[i].z_dir = rpt; + rpt = wd_ptRotate(workpiecePositions[i].x_dir, groundCalibPara.invRMatrix); + workpiecePositions[i].x_dir = rpt; + } + return; + } + SWD_3DPointPair lineTop = linePoint[lineTopIdx]; + linePoint.erase(linePoint.begin() + lineTopIdx);//ȥѾʶedge + + int lineBtmIdx = -1; + for (int i = 0; i < (int)linePoint.size(); i++) + { + if ((linePoint[i].pt1.y > binInfo.center.y) && (linePoint[i].pt2.y > binInfo.center.y)) + { + lineBtmIdx = i; + break; + } + } + if (lineBtmIdx < 0) + { + *errCode = SX_ERR_BIN_EDGE_ERROR; + //תȥ + int workpieceNum = (int)workpiecePositions.size(); + for (int i = 0; i < workpieceNum; i++) + { + SVzNL3DPoint rpt; + rpt = wd_ptRotate(workpiecePositions[i].center, groundCalibPara.invRMatrix); + workpiecePositions[i].center = rpt; + rpt = wd_ptRotate(workpiecePositions[i].y_dir, groundCalibPara.invRMatrix); + workpiecePositions[i].y_dir = rpt; + rpt = wd_ptRotate(workpiecePositions[i].z_dir, groundCalibPara.invRMatrix); + workpiecePositions[i].z_dir = rpt; + rpt = wd_ptRotate(workpiecePositions[i].x_dir, groundCalibPara.invRMatrix); + workpiecePositions[i].x_dir = rpt; + } + return; + } + SWD_3DPointPair lineBtm = linePoint[lineBtmIdx]; + linePoint.erase(linePoint.begin() + lineBtmIdx); //ȥѾʶedge + + int lineLeftIdx = -1; + for (int i = 0; i < (int)linePoint.size(); i++) + { + if ((linePoint[i].pt1.x < binInfo.center.x) && (linePoint[i].pt2.x < binInfo.center.x)) + { + lineLeftIdx = i; + break; + } + } + if (lineLeftIdx < 0) + { + *errCode = SX_ERR_BIN_EDGE_ERROR; + //תȥ + int workpieceNum = (int)workpiecePositions.size(); + for (int i = 0; i < workpieceNum; i++) + { + SVzNL3DPoint rpt; + rpt = wd_ptRotate(workpiecePositions[i].center, groundCalibPara.invRMatrix); + workpiecePositions[i].center = rpt; + rpt = wd_ptRotate(workpiecePositions[i].y_dir, groundCalibPara.invRMatrix); + workpiecePositions[i].y_dir = rpt; + rpt = wd_ptRotate(workpiecePositions[i].z_dir, groundCalibPara.invRMatrix); + workpiecePositions[i].z_dir = rpt; + rpt = wd_ptRotate(workpiecePositions[i].x_dir, groundCalibPara.invRMatrix); + workpiecePositions[i].x_dir = rpt; + } + return; + } + SWD_3DPointPair lineLeft = linePoint[lineLeftIdx]; + linePoint.erase(linePoint.begin() + lineLeftIdx); //ȥѾʶedge + + int lineRightIdx = -1; + for (int i = 0; i < (int)linePoint.size(); i++) + { + if ((linePoint[i].pt1.x > binInfo.center.x) && (linePoint[i].pt2.x > binInfo.center.x)) + { + lineRightIdx = i; + break; + } + } + if (lineRightIdx < 0) + { + *errCode = SX_ERR_BIN_EDGE_ERROR; + //תȥ + int workpieceNum = (int)workpiecePositions.size(); + for (int i = 0; i < workpieceNum; i++) + { + SVzNL3DPoint rpt; + rpt = wd_ptRotate(workpiecePositions[i].center, groundCalibPara.invRMatrix); + workpiecePositions[i].center = rpt; + rpt = wd_ptRotate(workpiecePositions[i].y_dir, groundCalibPara.invRMatrix); + workpiecePositions[i].y_dir = rpt; + rpt = wd_ptRotate(workpiecePositions[i].z_dir, groundCalibPara.invRMatrix); + workpiecePositions[i].z_dir = rpt; + rpt = wd_ptRotate(workpiecePositions[i].x_dir, groundCalibPara.invRMatrix); + workpiecePositions[i].x_dir = rpt; + } + return; + } + SWD_3DPointPair lineRight = linePoint[lineRightIdx]; + //㹤λãϡϡ¡ĸǣߣ벿 + //е۶ԲֵͬĹòͬץȡ + double distToSideTh = (workpieceParam.radius * 2) * distToSideRatio; + double lineTop_a, lineTop_b, lineTop_c; + compute2ptLine(lineTop.pt1, lineTop.pt2, &lineTop_a, &lineTop_b, &lineTop_c); + double lineBtm_a, lineBtm_b, lineBtm_c; + compute2ptLine(lineBtm.pt1, lineBtm.pt2, &lineBtm_a, &lineBtm_b, &lineBtm_c); + double lineLeft_a, lineLeft_b, lineLeft_c; + compute2ptLine(lineLeft.pt1, lineLeft.pt2, &lineLeft_a, &lineLeft_b, &lineLeft_c); + double lineRight_a, lineRight_b, lineRight_c; + compute2ptLine(lineRight.pt1, lineRight.pt2, &lineRight_a, &lineRight_b, &lineRight_c); + std::vector< WD_workpieceInfo> topSide; //ϱ + std::vector< WD_workpieceInfo> btmSide; //± + std::vector< WD_workpieceInfo> leftSide; // + std::vector< WD_workpieceInfo> rightSide; //ұ + std::vector< WD_workpieceInfo> LTCorner; //Ͻ + std::vector< WD_workpieceInfo> RTCorner; //Ͻ + std::vector< WD_workpieceInfo> LBCorner; // + std::vector< WD_workpieceInfo> RBCorner; //ұ + std::vector< WD_workpieceInfo> centerRegion; //ұ + for (int i = 0; i < (int)zSortWorkpiece.size(); i++) //ϲĿз + { + SVzNL3DPoint& objCenter = zSortWorkpiece[i].center; + double distTop = computePtDistToLine(objCenter.x, objCenter.y, lineTop_a, lineTop_b, lineTop_c); + double distBtm = computePtDistToLine(objCenter.x, objCenter.y, lineBtm_a, lineBtm_b, lineBtm_c); + double distLeft = computePtDistToLine(objCenter.x, objCenter.y, lineLeft_a, lineLeft_b, lineLeft_c); + double distRight = computePtDistToLine(objCenter.x, objCenter.y, lineRight_a, lineRight_b, lineRight_c); + + if (distTop < distToSideTh) + { + if (distLeft < distToSideTh) + { + zSortWorkpiece[i].workpieceType = 5; // + LTCorner.push_back(zSortWorkpiece[i]); + } + else if (distRight < distToSideTh) + { + zSortWorkpiece[i].workpieceType = 6; // + RTCorner.push_back(zSortWorkpiece[i]); + } + else + { + zSortWorkpiece[i].workpieceType = 1; // + topSide.push_back(zSortWorkpiece[i]); + } + } + else if (distBtm < distToSideTh) + { + if (distLeft < distToSideTh) + { + zSortWorkpiece[i].workpieceType = 7; // + LBCorner.push_back(zSortWorkpiece[i]); + } + else if (distRight < distToSideTh) + { + zSortWorkpiece[i].workpieceType = 8; // + RBCorner.push_back(zSortWorkpiece[i]); + } + else + { + zSortWorkpiece[i].workpieceType = 2; // + btmSide.push_back(zSortWorkpiece[i]); + } + } + else if (distLeft < distToSideTh) + { + zSortWorkpiece[i].workpieceType = 3; // + leftSide.push_back(zSortWorkpiece[i]); + } + else if (distRight < distToSideTh) + { + zSortWorkpiece[i].workpieceType = 4; // + rightSide.push_back(zSortWorkpiece[i]); + } + else + { + zSortWorkpiece[i].workpieceType = 0; // + centerRegion.push_back(zSortWorkpiece[i]); + } + } + //벿ֹ + if (centerRegion.size() > 0) + { + std::vector distanceIndice; + distanceIndice.resize(centerRegion.size()); + for (int m = 0; m < (int)centerRegion.size(); m++) + { + distanceIndice[m].dist = sqrt(pow(centerRegion[m].center.x - binInfo.center.x, 2) + + pow(centerRegion[m].center.y - binInfo.center.y, 2)); + distanceIndice[m].idx = m; + } + std::sort(distanceIndice.begin(), distanceIndice.end(), _compareByDistance); + for (int m = 0; m < (int)distanceIndice.size(); m++) + { + int idx = distanceIndice[m].idx; + workpiecePositions.push_back(centerRegion[idx]); + } + } + if (topSide.size() > 0) + workpiecePositions.insert(workpiecePositions.end(), topSide.begin(), topSide.end()); + if (btmSide.size() > 0) + workpiecePositions.insert(workpiecePositions.end(), btmSide.begin(), btmSide.end()); + if (leftSide.size() > 0) + workpiecePositions.insert(workpiecePositions.end(), leftSide.begin(), leftSide.end()); + if (rightSide.size() > 0) + workpiecePositions.insert(workpiecePositions.end(), rightSide.begin(), rightSide.end()); + if (LTCorner.size() > 0) + workpiecePositions.insert(workpiecePositions.end(), LTCorner.begin(), LTCorner.end()); + if (RTCorner.size() > 0) + workpiecePositions.insert(workpiecePositions.end(), RTCorner.begin(), RTCorner.end()); + if (LBCorner.size() > 0) + workpiecePositions.insert(workpiecePositions.end(), LBCorner.begin(), LBCorner.end()); + if (RBCorner.size() > 0) + workpiecePositions.insert(workpiecePositions.end(), RBCorner.begin(), RBCorner.end()); + } + else + { + //ˮƽ + while (zSortWorkpiece.size() > 0) + { + std::vector< WD_workpieceInfo> firstLine; + std::vector< WD_workpieceInfo> restWorkpiece; + _getYTopLine(zSortWorkpiece, firstLine, restWorkpiece, workpieceParam.radius); + workpiecePositions.insert(workpiecePositions.end(), firstLine.begin(), firstLine.end()); + zSortWorkpiece.clear(); + zSortWorkpiece.insert(zSortWorkpiece.end(), restWorkpiece.begin(), restWorkpiece.end()); + } + } } } - //תȥ int workpieceNum = (int)workpiecePositions.size(); for (int i = 0; i < workpieceNum; i++) { SVzNL3DPoint rpt; - rpt = wd_ptRotate(workpiecePositions[i].center, groundCalibPara.invRMatrix); + rpt = wd_ptRotate(workpiecePositions[i].center, groundCalibPara.invRMatrix); workpiecePositions[i].center = rpt; rpt = wd_ptRotate(workpiecePositions[i].y_dir, groundCalibPara.invRMatrix); workpiecePositions[i].y_dir = rpt; @@ -1757,6 +2148,7 @@ void wd_HRM_RotorCorePositioning( for (int j = 0; j < (int)scanLines[line].size(); j++) debugScanLines[line][j] = scanLines[line][j]; } + #endif return; } @@ -1899,20 +2291,7 @@ void wd_HRM_TaperedWorkpiecePositioning( } -SSG_ROIRectD _getListROI(std::vector< SVzNL3DPosition>& listData) -{ - if (listData.size() == 0) - return { 0,0,0,0 }; - SSG_ROIRectD roi = { listData[0].pt3D.x, listData[0].pt3D.x, listData[0].pt3D.y, listData[0].pt3D.y }; - for (int i = 0; i < (int)listData.size(); i++) - { - roi.left = roi.left > listData[i].pt3D.x ? listData[i].pt3D.x : roi.left; - roi.right = roi.right < listData[i].pt3D.x ? listData[i].pt3D.x : roi.right; - roi.top = roi.top > listData[i].pt3D.y ? listData[i].pt3D.y : roi.top; - roi.bottom = roi.bottom < listData[i].pt3D.y ? listData[i].pt3D.y : roi.bottom; - } - return roi; -} + //ʱתʱ > 0 ˳ʱתʱ < 0 cv::Point2f _rotate2D(cv::Point2f pt, double sinTheta, double cosTheta) diff --git a/sourceCode/hybridPosePositioning_Export.h b/sourceCode/hybridPosePositioning_Export.h index 33901f8..611e3d8 100644 --- a/sourceCode/hybridPosePositioning_Export.h +++ b/sourceCode/hybridPosePositioning_Export.h @@ -15,7 +15,7 @@ typedef struct typedef struct { int id; //Ŵ1ʼ - int workpieceType; //0:; 1-ϱ; 2-±; 3-; 4-ұ; 5-Ͻ; 6-Ͻ; 7-½; 8-½; + int workpieceType; //0:; 1-ϱ; 2-±; 3-; 4-ұ; 5-Ͻ; 6-Ͻ; 7-½; 8-½; 9- SVzNL3DPoint center; SVzNL3DPoint z_dir; //zһ SVzNL3DPoint y_dir; //yһ @@ -62,6 +62,7 @@ SG_APISHARED_EXPORT void wd_HRM_RotorCorePositioning( std::vector< std::vector>& scanLinesInput, std::vector& objROIs, const SSG_planeCalibPara groundCalibPara, + const SVzNLSizeD spacerParam, //Ϳ const SWD_Cylinder workpieceParam, //Բιıư뾶͸߶ const double layerRatio, //ֲϵֲ=ֲϵ*߶ȣĸ߶ȲڷֲʱжΪڲͬ const double distToSideRatio, //߾ϵ߾=߾ϵ*ֱߵľڱ߾ʱжΪϵĹ