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

hybridPosePositioning

Browse Source 
version 1.4.0 : 转子钢芯， 添加了隔板的识别和定位。 当定位隔板时，工件类型为9
This commit is contained in:
jerryzeng 2026-09-18 13:28:09 +08:00
parent 6674c32adc
commit 79a7d6c49a
6 changed files with 929 additions and 328 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

177
hybridPosePositioning_test/hybridPosePositioning_test.cpp
View File

@ -720,6 +720,8 @@ void _outputRGBDResult_XYZ_RGBD_workpieceInfo(
pt3D->pt3D = { 0.0, 0.0, 0.0 }; pt3D->pt3D = { 0.0, 0.0, 0.0 };
int flag = pt3D->nPointIdx & 0xffff; int flag = pt3D->nPointIdx & 0xffff;
int highFlag = pt3D->nPointIdx >> 16; int highFlag = pt3D->nPointIdx >> 16;
int spacerFlag = highFlag & 0x4;
highFlag &= 0x3;
#if 0 #if 0
if (flag > 0) if (flag > 0)
{ {
@ -742,6 +744,11 @@ void _outputRGBDResult_XYZ_RGBD_workpieceInfo(
rgb = { 0, 0, 250 }; rgb = { 0, 0, 250 };
size = 2; size = 2;
} }
if (spacerFlag > 0)
{
rgb = { 0, 250, 250 };
size = 2;
}
float x = (float)pt3D->pt3D.x; float x = (float)pt3D->pt3D.x;
float y = (float)pt3D->pt3D.y; float y = (float)pt3D->pt3D.y;
@ -778,21 +785,68 @@ void _outputRGBDResult_XYZ_RGBD_workpieceInfo(
//double dirLen = 500; //double dirLen = 500;
for (int m = 0; m < (int)poseInfo.size(); m++) for (int m = 0; m < (int)poseInfo.size(); m++)
{ {
SVzNL3DPoint dirPt1 = { poseInfo[m].center.x + poseInfo[m].z_dir.x * dirLen, if (poseInfo[m].workpieceType != 9)
poseInfo[m].center.y + poseInfo[m].z_dir.y * dirLen, {
poseInfo[m].center.z + poseInfo[m].z_dir.z * dirLen }; SVzNL3DPoint dirPt1 = { poseInfo[m].center.x + poseInfo[m].z_dir.x * 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.y - poseInfo[m].z_dir.y * dirLen, poseInfo[m].center.z + poseInfo[m].z_dir.z * 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 << "Poly_" << lineIdx << "_2" << std::endl;
sw << "{" << dirPt1.x << "," << dirPt1.y << "," << dirPt1.z << "}-"; sw << "{" << dirPt1.x << "," << dirPt1.y << "," << dirPt1.z << "}-";
sw << "{0,0}-{0,0}-"; sw << "{0,0}-{0,0}-";
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
sw << "{" << dirPt2.x << "," << dirPt2.y << "," << dirPt2.z << "}-"; sw << "{" << dirPt2.x << "," << dirPt2.y << "," << dirPt2.z << "}-";
sw << "{0,0}-{0,0}-"; sw << "{0,0}-{0,0}-";
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
lineIdx++; 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) if (binInfo)
@ -1132,7 +1186,42 @@ void _outputWorkpieceInfo(char* fileName, std::vector< WD_workpieceInfo>& workpi
int number = (int)workpiecePositions.size(); int number = (int)workpiecePositions.size();
for (int i = 0; i < number; i++) 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; sw << dataStr << std::endl;
sprintf_s(dataStr, 50, " center: (%g, %g, %g)", workpiecePositions[i].center.x, workpiecePositions[i].center.y, workpiecePositions[i].center.z); 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); printf("ver:%s\n", ver);
char _calib_datafile[256]; 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; int lineNum = 0;
float lineV = 0.0f; float lineV = 0.0f;
int dataCalib = 0; int dataCalib = 0;
@ -2188,9 +2277,9 @@ void HaiRuiMa_groundCalib_test(void)
SSG_planeCalibPara calibPara = wd_getGroundCalibPara(scanData); SSG_planeCalibPara calibPara = wd_getGroundCalibPara(scanData);
// //
char calibFile[250]; 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); _outputCalibPara(calibFile, calibPara);
#if 1 #if 0
for (int grp = 0; grp < HRM_GroundCalib_TEST_GROUP; grp++) for (int grp = 0; grp < HRM_GroundCalib_TEST_GROUP; grp++)
{ {
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) 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) void HaiRuiMa_rotorCorePositioning_test(void)
{ {
const char* dataPath[HRM_RotorCore_TEST_GROUP] = { const char* dataPath[HRM_RotorCore_TEST_GROUP] = {
"F:/ShangGu/项目/冠钦项目/宁波海瑞马/转子钢芯/数据2D3D/", //0 "F:/ShangGu/项目/冠钦项目/宁波海瑞马/转子钢芯/数据2D3D/", //0
"F:/ShangGu/项目/冠钦项目/宁波海瑞马/转子钢芯/WorkpieceRotor/", //1 "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] = { 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(); const char* ver = wd_hybridPositioningVersion();
printf("ver:%s\n", ver); 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; SSG_planeCalibPara groundCalibPara;
//初始化成单位阵 //初始化成单位阵
@ -2300,6 +2391,7 @@ void HaiRuiMa_rotorCorePositioning_test(void)
int headNullLines = 0; int headNullLines = 0;
sprintf_s(_scan_file, "%s%d_cloud.txt", dataPath[grp], fidx); sprintf_s(_scan_file, "%s%d_cloud.txt", dataPath[grp], fidx);
_outputScanDataFile_SVzNLPositionD(_scan_file, scanLines, false, &headNullLines); _outputScanDataFile_SVzNLPositionD(_scan_file, scanLines, false, &headNullLines);
continue;
#endif #endif
WD_HRM_BinInfo binInfo; WD_HRM_BinInfo binInfo;
@ -2329,16 +2421,36 @@ void HaiRuiMa_rotorCorePositioning_test(void)
binInfo = _readBinInfo(calibFile); binInfo = _readBinInfo(calibFile);
} }
#endif #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<WD_objArea2D> objROIs; std::vector<WD_objArea2D> objROIs;
if( (grp == 0) && (fidx <= 9)) if( ((grp == 0) && (fidx <= 9)) || ((grp >=3)&&(grp<=4)))
vzReadObj2DROI(_scan_file, objROIs); vzReadObj2DROI(_scan_file, objROIs);
else else
vzReadObj2DROI_1(_scan_file, objROIs); vzReadObj2DROI_1(_scan_file, objROIs);
SWD_Cylinder workpieceParam; //圆柱形工件的标称半径和高度 SWD_Cylinder workpieceParam; //圆柱形工件的标称半径和高度
workpieceParam.radius = 45.0; if (grp == 3)
workpieceParam.height = 20.0; {
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; //分层系数，分层门限=分层系数*工件高度；当两个工件的高度差大于分层门限时，判断为在不同层 double layerRatio = 0.5; //分层系数，分层门限=分层系数*工件高度；当两个工件的高度差大于分层门限时，判断为在不同层
const double distToSideRatio = 1.1; //边距系数，边距门限=边距系数*工件直径；当工件距边的距离大于边距门限时，判断为边上的工件 const double distToSideRatio = 1.1; //边距系数，边距门限=边距系数*工件直径；当工件距边的距离大于边距门限时，判断为边上的工件
@ -2352,6 +2464,7 @@ void HaiRuiMa_rotorCorePositioning_test(void)
scanLines, scanLines,
objROIs, objROIs,
groundCalibPara, groundCalibPara,
spacerParam,
workpieceParam, workpieceParam,
layerRatio, layerRatio,
distToSideRatio, distToSideRatio,
@ -2369,7 +2482,15 @@ void HaiRuiMa_rotorCorePositioning_test(void)
else if (errCode == SX_ERR_ZERO_OBJECTS) else if (errCode == SX_ERR_ZERO_OBJECTS)
printf("%s: %d(ms), 无产品!\n", _scan_file, (int)(t2 - t1)); printf("%s: %d(ms), 无产品!\n", _scan_file, (int)(t2 - t1));
else 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); sprintf_s(calibFile, "%sresult/LaserLine%d_workpiece_info.txt", dataPath[grp], fidx);
_outputWorkpieceInfo(calibFile, workpiecePositions); _outputWorkpieceInfo(calibFile, workpiecePositions);
@ -2757,6 +2878,4 @@ int main()
HaiRuiMa_workpieceSizeCompute_test(); HaiRuiMa_workpieceSizeCompute_test();
HaiRuiMa_positionPlanning_test(); HaiRuiMa_positionPlanning_test();
} }
} }

12
sourceCode/SG_baseAlgo_Export.h
View File

@ -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(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 //计算垂足点，直线方程：y = kx + b
SG_APISHARED_EXPORT SVzNL2DPointD sx_getFootPoint( SG_APISHARED_EXPORT SVzNL2DPointD sx_getFootPoint(
double x0, double x0,
@ -874,7 +877,14 @@ SG_APISHARED_EXPORT Plane ransacFitPlane(
int max_iter = 1000, // 最大迭代 int max_iter = 1000, // 最大迭代
int stop_no_improve = 250 // 连续多少次无提升就提前退出 int stop_no_improve = 250 // 连续多少次无提升就提前退出
); );
SG_APISHARED_EXPORT Plane ransacFitPlane_2(
const std::vector<SVzNL3DPosition>& points,
std::vector<SVzNL3DPosition>& in_inliers,
std::vector<SVzNL3DPosition>& out_inliers,
float dist_thresh = 0.3f, // 内点距离阈值
int max_iter = 1000, // 最大迭代
int stop_no_improve = 250 // 连续多少次无提升就提前退出
);
// 输入：3D点云 std::vector<Eigen::Vector3d> // 输入：3D点云 std::vector<Eigen::Vector3d>
// 输出：axis 轴向单位向量, centroid 点云中心 // 输出：axis 轴向单位向量, centroid 点云中心
SG_APISHARED_EXPORT void computeCylinderAxisFromIncompletePCA( SG_APISHARED_EXPORT void computeCylinderAxisFromIncompletePCA(

2
sourceCode/SG_lineFeature.cpp
View File

@ -10173,6 +10173,8 @@ void wd_getLineVFlaws(
//计算gauss一阶和二阶导 //计算gauss一阶和二阶导
wd_convolveGauss_laserLine(a_line, startId, endId, deriv_1_narrow, deriv_2_narrow, chkWidth_narrow, narrow_scale); 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检测: 边沿检测和凹陷检测同时需要使用 //peak检测: 边沿检测和凹陷检测同时需要使用
std::vector<posAttri> deriv_1_peaks; std::vector<posAttri> deriv_1_peaks;

90
sourceCode/dataFitting.cpp
View File

@ -938,6 +938,96 @@ Plane ransacFitPlane(
return best_plane; 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<SVzNL3DPosition>& points,
std::vector<SVzNL3DPosition>& in_inliers,
std::vector<SVzNL3DPosition>& 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<Eigen::Vector3d> // 输入：3D点云 std::vector<Eigen::Vector3d>
// 输出：axis 轴向单位向量, centroid 点云中心 // 输出：axis 轴向单位向量, centroid 点云中心
void computeCylinderAxisFromIncompletePCA( void computeCylinderAxisFromIncompletePCA(

973
sourceCode/hybridPosePositioning.cpp
View File

File diff suppressed because it is too large Load Diff

3
sourceCode/hybridPosePositioning_Export.h
View File

@ -15,7 +15,7 @@ typedef struct
typedef struct typedef struct
{ {
int id; //序号从1开始 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 center;
SVzNL3DPoint z_dir; //z方向向量（归一化） SVzNL3DPoint z_dir; //z方向向量（归一化）
SVzNL3DPoint y_dir; //y方向向量（归一化） SVzNL3DPoint y_dir; //y方向向量（归一化）
@ -62,6 +62,7 @@ SG_APISHARED_EXPORT void wd_HRM_RotorCorePositioning(
std::vector< std::vector<SVzNLPositionD>>& scanLinesInput, std::vector< std::vector<SVzNLPositionD>>& scanLinesInput,
std::vector<WD_objArea2D>& objROIs, std::vector<WD_objArea2D>& objROIs,
const SSG_planeCalibPara groundCalibPara, const SSG_planeCalibPara groundCalibPara,
const SVzNLSizeD spacerParam, //隔板参数：长和宽
const SWD_Cylinder workpieceParam, //圆柱形工件的标称半径和高度 const SWD_Cylinder workpieceParam, //圆柱形工件的标称半径和高度
const double layerRatio, //分层系数，分层门限=分层系数*工件高度；当两个工件的高度差大于分层门限时，判断为在不同层 const double layerRatio, //分层系数，分层门限=分层系数*工件高度；当两个工件的高度差大于分层门限时，判断为在不同层
const double distToSideRatio, //边距系数，边距门限=边距系数*工件直径；当工件距边的距离大于边距门限时，判断为边上的工件 const double distToSideRatio, //边距系数，边距门限=边距系数*工件直径；当工件距边的距离大于边距门限时，判断为边上的工件