jerryzeng/algoLib
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

hybridPosePositioning

Browse Source 
version 1.4.3 : 转子钢芯， (1)分层系数和边距系数添加-1.0参数值，表示不进行分层或边距。（2）添加选项选择使用工件的自身法向或统一使用调平地面法向
This commit is contained in:
jerryzeng 2026-09-25 22:18:02 +08:00
parent 8962f03d2a
commit 7b3cb958ef
3 changed files with 56 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
hybridPosePositioning_test/hybridPosePositioning_test.cpp
View File

@ -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

55
sourceCode/hybridPosePositioning.cpp
View File

@ -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<SVzNL3DPosition>>& scanLines,
const SWDScanPosition centerPosition,
const SWD_Cylinder workpieceParam,
const int useGroundCalibNormal, //1: 使用调平后的地面法向作为工件法向; 0:使用工件自身的法向
std::vector<SWDScanPosition>& outsideEdgePtIndice,
std::vector<SWDScanPosition>& centerPtIndice)
{
@ -867,6 +869,7 @@ WD_workpieceInfo _computeWorkpiecePose(
//迭代一次
pointArray.clear();
std::vector<cv::Point3d> 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<double> 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, //边距系数，-1。0：不使用边距系数。边距门限=边距系数*工件直径；当工件距边的距离大于边距门限时，判断为边上的工件
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<SWD_3DPointPair> linePoint;

1
sourceCode/hybridPosePositioning_Export.h
View File

@ -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