diff --git a/hybridPosePositioning_test/hybridPosePositioning_test.cpp b/hybridPosePositioning_test/hybridPosePositioning_test.cpp index dbb35e0..7fa41f0 100644 --- a/hybridPosePositioning_test/hybridPosePositioning_test.cpp +++ b/hybridPosePositioning_test/hybridPosePositioning_test.cpp @@ -2350,7 +2350,7 @@ void HaiRuiMa_groundCalib_test(void) } //拓普发工件孔定位（工件定位) -#define HRM_RotorCore_TEST_GROUP 5 +#define HRM_RotorCore_TEST_GROUP 6 void HaiRuiMa_rotorCorePositioning_test(void) { const char* dataPath[HRM_RotorCore_TEST_GROUP] = { @@ -2360,16 +2360,17 @@ void HaiRuiMa_rotorCorePositioning_test(void) "F:/ShangGu/项目/冠钦项目/宁波海瑞马/转子钢芯/现场数据/", //2 "F:/ShangGu/项目/冠钦项目/宁波海瑞马/转子钢芯/2026-09-海瑞马/09-14/", //3 "F:/ShangGu/项目/冠钦项目/宁波海瑞马/转子钢芯/2026-09-海瑞马/09-14中/", //4 + "F:/ShangGu/项目/冠钦项目/宁波海瑞马/转子钢芯/2026-09-海瑞马/异常数据/", //5 }; SVzNLRange fileIdx[HRM_RotorCore_TEST_GROUP] = { - {1,10}, {3,70},{3,7}, {1,47}, {1,54} + {1,10}, {3,70},{8,8}, {5,47}, {1,54}, {1,6} }; const char* ver = wd_hybridPositioningVersion(); printf("ver:%s\n", ver); - for (int grp = 2; grp <= 2; grp++) + for (int grp = 4; grp <= 4; grp++) { SSG_planeCalibPara groundCalibPara; //初始化成单位阵 @@ -2442,10 +2443,10 @@ void HaiRuiMa_rotorCorePositioning_test(void) vzReadObj2DROI(_scan_file, objROIs); SWD_Cylinder workpieceParam; //圆柱形工件的标称半径和高度 - if (grp == 3) + if ( (grp == 3) || (grp == 5)) { - workpieceParam.radius = 60.0; - workpieceParam.height = 70.0; + workpieceParam.radius = 62.0; + workpieceParam.height = 50.0; } else if (grp == 4) { @@ -2462,9 +2463,11 @@ void HaiRuiMa_rotorCorePositioning_test(void) spacerParam.dWidth = 720.0; spacerParam.dHeight = 520.0; - double layerRatio = 0.5; //分层系数，分层门限=分层系数*工件高度；当两个工件的高度差大于分层门限时，判断为在不同层 + int useGroundCalibNormal = 0; //1: 使用调平后的地面法向作为工件法向; 0:使用工件自身的法向 + + double layerRatio = -1.0; //不分层 //分层系数，分层门限=分层系数*工件高度；当两个工件的高度差大于分层门限时，判断为在不同层 const double distToSideRatio = 1.1; //边距系数，边距门限=边距系数*工件直径；当工件距边的距离大于边距门限时，判断为边上的工件 - double groundRemoveOffset = 200; //料筐地面去除的Z偏置，实际地面去除高度为 groundCalibPara.planeHeight - groundRemoveOffset + double groundRemoveOffset =20; //料筐地面去除的Z偏置，实际地面去除高度为 groundCalibPara.planeHeight - groundRemoveOffset long t1 = (long)GetTickCount64();//统计时间 int errCode = 0; @@ -2479,6 +2482,7 @@ void HaiRuiMa_rotorCorePositioning_test(void) layerRatio, distToSideRatio, groundRemoveOffset, + useGroundCalibNormal, workpiecePositions, binInfo, #ifdef _OUTPUT_DEBUG_DATA diff --git a/sourceCode/hybridPosePositioning.cpp b/sourceCode/hybridPosePositioning.cpp index ceb6261..df1fe15 100644 --- a/sourceCode/hybridPosePositioning.cpp +++ b/sourceCode/hybridPosePositioning.cpp @@ -25,7 +25,8 @@ //version 1.4.0 : תӸо ˸ʶͶλ λʱΪ9 //version 1.4.1 : תӸо ÿ2Dͼ //version 1.4.2 : תӸо Ż˵дֱȥ -std::string m_strVersion = "HybridPositioning 1.4.2"; +//version 1.4.3 : תӸо (1)ֲϵͱ߾ϵ-1.0ֵʾзֲ߾ࡣ2ѡѡʹùͳһʹõƽ淨 +std::string m_strVersion = "HybridPositioning 1.4.3"; const char* wd_hybridPositioningVersion(void) { return m_strVersion.c_str(); @@ -358,6 +359,7 @@ WD_workpieceInfo _computeWorkpiecePose( std::vector< std::vector>& scanLines, const SWDScanPosition centerPosition, const SWD_Cylinder workpieceParam, + const int useGroundCalibNormal, //1: ʹõƽĵ淨Ϊ; 0:ʹùķ std::vector& outsideEdgePtIndice, std::vector& centerPtIndice) { @@ -867,6 +869,7 @@ WD_workpieceInfo _computeWorkpiecePose( //һ pointArray.clear(); + std::vector normalDataArray; for (int i = 0; i < (int)objClusters.size(); i++) { std::vector< SWD_indexingSeg>& a_cluster = objClusters[i]; @@ -878,6 +881,10 @@ WD_workpieceInfo _computeWorkpiecePose( if (r1 < workpieceParam.radius * 1.05) { pointArray.push_back(a_cluster[m].outsideEdgePoint.pt3D); + cv::Point3d a_pt = cv::Point3d(a_cluster[m].outsideEdgePoint.pt3D.x, a_cluster[m].outsideEdgePoint.pt3D.y, a_cluster[m].outsideEdgePoint.pt3D.z); + normalDataArray.push_back(a_pt); + a_pt = cv::Point3d(a_cluster[m].centerPoint.pt3D.x, a_cluster[m].centerPoint.pt3D.y, a_cluster[m].centerPoint.pt3D.z); + normalDataArray.push_back(a_pt); //ɱעҪϢ if (a_seg.hScanFlag == 0) { @@ -897,6 +904,21 @@ WD_workpieceInfo _computeWorkpiecePose( } } + SVzNL3DPoint poseNormal = { 0.0, 0.0, 1.0 }; + if (0 == useGroundCalibNormal) //1: ʹõƽĵ淨Ϊ; 0:ʹùķ + { + std::vector res; + //: z = Ax + By + C + //res: [0]=A, [1]= B, [2]=-1.0, [3]=C, + vzCaculateLaserPlane(normalDataArray, res); + SVzNL3DPoint normVector = { res[0], res[1], res[2] }; + normVector = vec3_normalize(normVector); + if (normVector.z < 0) + poseNormal = { -normVector.x, -normVector.y, -normVector.z }; + else + poseNormal = normVector; + } + err = fitCircleByLeastSquare(pointArray, center, radius); double k = radius / workpieceParam.radius; double distToRefCenter = sqrt(pow(center.x - centerPoint.x, 2) + pow(center.y - centerPoint.y, 2)); @@ -904,9 +926,9 @@ WD_workpieceInfo _computeWorkpiecePose( { a_pose.center = { center.x, center.y, objZ }; a_pose.value = radius; - a_pose.z_dir = { 0.0, 0.0, 1.0 }; - a_pose.y_dir = { 0.0, 0.0, 0.0 }; - a_pose.x_dir = { 0.0, 0.0, 0.0 }; + a_pose.z_dir = poseNormal; + a_pose.x_dir = { 1.0, 0.0, 0.0 }; + a_pose.y_dir = vec3_cross(a_pose.z_dir, a_pose.x_dir); //˳y_dir } return a_pose; } @@ -1465,9 +1487,10 @@ void wd_HRM_RotorCorePositioning( const SSG_planeCalibPara groundCalibPara, const SVzNLSizeD spacerParam, //Ϳ const SWD_Cylinder workpieceParam, //Բιıư뾶͸߶ - const double layerRatio, //ֲϵֲ=ֲϵ*߶ȣĸ߶ȲڷֲʱжΪڲͬ - const double distToSideRatio, //߾ϵ߾=߾ϵ*ֱߵľڱ߾ʱжΪϵĹ + const double layerRatio, //ֲϵ-1.0:ֲ㡣 ֲ=ֲϵ*߶ȣĸ߶ȲڷֲʱжΪڲͬ + const double distToSideRatio, //߾ϵ-10ʹñ߾ϵ߾=߾ϵ*ֱߵľڱ߾ʱжΪϵĹ const double groundRemoveOffset, //ϿȥZƫãʵʵȥ߶Ϊ groundCalibPara.planeHeight - groundRemoveOffset + const int useGroundCalibNormal, //1: ʹõƽĵ淨Ϊ; 0:ʹùķ std::vector< WD_workpieceInfo>& workpiecePositions, WD_HRM_BinInfo& binInfo, //BinϢdebugȷϿϢȷ #ifdef _OUTPUT_DEBUG_DATA @@ -1813,6 +1836,7 @@ void wd_HRM_RotorCorePositioning( roiScanLines, centerPosition, workpieceParam, + useGroundCalibNormal, //1: ʹõƽĵ淨Ϊ; 0:ʹùķ outsideEdgePtIndice, centerPtIndice); a_workpiece.imgCenter = { nCx, nCy }; @@ -1892,19 +1916,26 @@ void wd_HRM_RotorCorePositioning( //ϿϢ SVzNLRangeD zSliceRange; zSliceRange.min = highest_z - workpieceParam.height; - zSliceRange.max = groundCalibPara.planeHeight - groundRemoveOffset; + zSliceRange.max = highest_z + workpieceParam.height * 2; + if(zSliceRange.max > (groundCalibPara.planeHeight - groundRemoveOffset)) + 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 (layerRatio >= 1e-4) { - if (allWorkpiece[i].center.z < topLayerTh) - zSortWorkpiece.push_back(allWorkpiece[i]); + 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]); + } } + else + zSortWorkpiece.insert(zSortWorkpiece.end(), allWorkpiece.begin(), allWorkpiece.end()); - if (binInfo.binTopZ > 1e-4) + if ( (binInfo.binTopZ > 1e-4) && (distToSideRatio > 1e-4)) { //㹤λ std::vector linePoint; diff --git a/sourceCode/hybridPosePositioning_Export.h b/sourceCode/hybridPosePositioning_Export.h index 35688a9..37e6883 100644 --- a/sourceCode/hybridPosePositioning_Export.h +++ b/sourceCode/hybridPosePositioning_Export.h @@ -68,6 +68,7 @@ SG_APISHARED_EXPORT void wd_HRM_RotorCorePositioning( const double layerRatio, //ֲϵֲ=ֲϵ*߶ȣĸ߶ȲڷֲʱжΪڲͬ const double distToSideRatio, //߾ϵ߾=߾ϵ*ֱߵľڱ߾ʱжΪϵĹ const double groundRemoveOffset, //ϿȥZƫãʵʵȥ߶Ϊ groundCalibPara.planeHeight - groundRemoveOffset + const int useGroundCalibNormal, //1: ʹõƽĵ淨Ϊ; 0:ʹùķ std::vector< WD_workpieceInfo>& workpiecePositions, WD_HRM_BinInfo& binInfo, //BinϢdebugȷϿϢȷ #ifdef _OUTPUT_DEBUG_DATA