hybridPosePositioning
version 1.4.0 : 转子钢芯， 添加了隔板的识别和定位。 当定位隔板时，工件类型为9
This commit is contained in:
parent
6674c32adc
commit
79a7d6c49a
@ -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<WD_objArea2D> 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();
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
@ -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<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>
|
||||
// 输出：axis 轴向单位向量, centroid 点云中心
|
||||
SG_APISHARED_EXPORT void computeCylinderAxisFromIncompletePCA(
|
||||
|
||||
@ -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<posAttri> deriv_1_peaks;
|
||||
|
||||
@ -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<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>
|
||||
// 输出：axis 轴向单位向量, centroid 点云中心
|
||||
void computeCylinderAxisFromIncompletePCA(
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -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<SVzNLPositionD>>& scanLinesInput,
|
||||
std::vector<WD_objArea2D>& objROIs,
|
||||
const SSG_planeCalibPara groundCalibPara,
|
||||
const SVzNLSizeD spacerParam, //隔板参数：长和宽
|
||||
const SWD_Cylinder workpieceParam, //圆柱形工件的标称半径和高度
|
||||
const double layerRatio, //分层系数，分层门限=分层系数*工件高度；当两个工件的高度差大于分层门限时，判断为在不同层
|
||||
const double distToSideRatio, //边距系数，边距门限=边距系数*工件直径；当工件距边的距离大于边距门限时，判断为边上的工件
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user