Merge branch 'main' of http://gitea.mnutil.com/jerryzeng/algoLib
This commit is contained in:
杰仔
commit
8524d9a88a
@ -641,6 +641,24 @@ SG_APISHARED_EXPORT void vzCaculateLaserPlane(
|
||||
std::vector<cv::Point3f> Points3ds,
|
||||
std::vector<double>& res);
|
||||
|
||||
SG_APISHARED_EXPORT Plane robustFitPlane(
|
||||
const std::vector<cv::Point3f>& points,
|
||||
RobustType type = TUKEY,
|
||||
double delta = 0.5, // 阈值：>此值视为离群点（mm）
|
||||
int maxIter = 20, // 迭代次数
|
||||
double convergeThresh = 1e-5 // 收敛阈值（平面变化足够小就停）
|
||||
);
|
||||
// ==============================================
|
||||
// 带 提前终止 的 RANSAC 平面拟合（工业正式版）
|
||||
// ==============================================
|
||||
SG_APISHARED_EXPORT Plane ransacFitPlane(
|
||||
const std::vector<cv::Point3f>& points,
|
||||
std::vector<cv::Point3f>& out_inliers,
|
||||
float dist_thresh = 0.5f, // 内点距离阈值
|
||||
int max_iter = 1000, // 最大迭代
|
||||
int stop_no_improve = 150 // 连续多少次无提升就提前退出
|
||||
);
|
||||
|
||||
//计算一个平面调平参数。
|
||||
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
|
||||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||||
@ -839,3 +857,9 @@ SG_APISHARED_EXPORT void WD_EulerRpyToRotation(double rpy[3], double matrix3d[9]
|
||||
|
||||
//从欧拉角计算方向矢量
|
||||
SG_APISHARED_EXPORT void WD_EulerRpyToDirVectors(double rpy[3], std::vector<cv::Point3d>& dirVectors);
|
||||
|
||||
//3x3平滑
|
||||
SG_APISHARED_EXPORT void scanLinesSmooting3x3(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& gridDataInput,
|
||||
std::vector< std::vector<SVzNL3DPosition>>& smoothingData
|
||||
);
|
||||
|
||||
@ -124,6 +124,16 @@ struct RGB {
|
||||
int b;
|
||||
};
|
||||
|
||||
struct Plane {
|
||||
double A, B, C, D; // Ć˝Ăć Ax+By+Cz+D=0
|
||||
Plane() : A(0), B(0), C(0), D(0) {}
|
||||
Plane(double a, double b, double c, double d) : A(a), B(b), C(c), D(d) {}
|
||||
};
|
||||
enum RobustType {
|
||||
HUBER,
|
||||
TUKEY
|
||||
};
|
||||
|
||||
#define LINE_FEATURE_NUM 16
|
||||
#define LINE_FEATURE_UNDEF 0
|
||||
#define LINE_FEATURE_L_JUMP_H2L 1
|
||||
|
||||
@ -3148,7 +3148,43 @@ void WD_EulerRpyToDirVectors(double rpy[3],std::vector<cv::Point3d>& dirVectors)
|
||||
return;
|
||||
}
|
||||
|
||||
void scanLinesSmooting3x3(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& gridDataInput,
|
||||
std::vector< std::vector<SVzNL3DPosition>>& smoothingData
|
||||
)
|
||||
{
|
||||
int lineNum = (int)gridDataInput.size();
|
||||
int linePtNum = (int)gridDataInput[0].size();
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++)
|
||||
{
|
||||
|
||||
smoothingData[line][ptIdx] = gridDataInput[line][ptIdx];
|
||||
if (gridDataInput[line][ptIdx].pt3D.z > 1e-4)
|
||||
{
|
||||
double sumZ = 0;
|
||||
int num = 0;
|
||||
for (int i = line - 1; i <= line + 1; i++)
|
||||
{
|
||||
for (int j = ptIdx - 1; j <= ptIdx + 1; j++)
|
||||
{
|
||||
if ((i >= 0) && (i < lineNum) && (j >= 0) && (j < linePtNum))
|
||||
{
|
||||
if (gridDataInput[i][j].pt3D.z > 1e-4)
|
||||
{
|
||||
sumZ += gridDataInput[i][j].pt3D.z;
|
||||
num++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
smoothingData[line][ptIdx].pt3D.z = sumZ / num;
|
||||
}
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
#if 0
|
||||
#include <iostream>
|
||||
|
||||
@ -383,3 +383,255 @@ bool fitLine3DLeastSquares(const std::vector<SVzNL3DPoint>& points, SVzNL3DPoint
|
||||
direction = { dir(0), dir(1), dir(2) };
|
||||
return true;
|
||||
}
|
||||
|
||||
// ============================== 工具函数 ==============================
|
||||
// 点到平面距离（带符号）
|
||||
double pointToPlaneSignedDist(const cv::Point3f& p, const Plane& plane) {
|
||||
return (plane.A * p.x + plane.B * p.y + plane.C * p.z + plane.D);
|
||||
}
|
||||
|
||||
// 点到平面的距离（绝对值）
|
||||
float pointToPlaneDistance(const cv::Point3f& p, const Plane& plane) {
|
||||
return fabsf(plane.A * p.x + plane.B * p.y + plane.C * p.z + plane.D)
|
||||
/ sqrtf(plane.A * plane.A + plane.B * plane.B + plane.C * plane.C);
|
||||
}
|
||||
|
||||
// 归一化平面（法向量模长=1）
|
||||
void normalizePlane(Plane& plane) {
|
||||
double norm = sqrt(plane.A * plane.A + plane.B * plane.B + plane.C * plane.C);
|
||||
if (norm < 1e-6) return;
|
||||
plane.A /= norm;
|
||||
plane.B /= norm;
|
||||
plane.C /= norm;
|
||||
plane.D /= norm;
|
||||
}
|
||||
|
||||
// ============================== 鲁棒损失函数 ==============================
|
||||
// Huber 权重
|
||||
double huberWeight(double r, double delta) {
|
||||
r = fabs(r);
|
||||
if (r <= delta) return 1.0;
|
||||
else return delta / r;
|
||||
}
|
||||
|
||||
// Tukey 权重（离群点=0，最适合凹坑/强噪声）
|
||||
double tukeyWeight(double r, double c) {
|
||||
r = fabs(r);
|
||||
if (r > c) return 0.0;
|
||||
double t = 1.0 - (r * r) / (c * c);
|
||||
return t * t;
|
||||
}
|
||||
|
||||
// ============================== 鲁棒加权最小二乘平面拟合 ==============================
|
||||
Plane robustFitPlane(
|
||||
const std::vector< cv::Point3f>& points,
|
||||
RobustType type,
|
||||
double delta, // 阈值：>此值视为离群点（mm）
|
||||
int maxIter, // 迭代次数
|
||||
double convergeThresh // 收敛阈值（平面变化足够小就停）
|
||||
)
|
||||
{
|
||||
int n = points.size();
|
||||
if (n < 3) return Plane();
|
||||
|
||||
// 1. 先用普通最小二乘初始化平面
|
||||
double cx = 0, cy = 0, cz = 0;
|
||||
for (auto& p : points) { cx += p.x; cy += p.y; cz += p.z; }
|
||||
cx /= n; cy /= n; cz /= n;
|
||||
|
||||
double xx = 0, xy = 0, xz = 0, yy = 0, yz = 0, zz = 0;
|
||||
for (auto& p : points) {
|
||||
double dx = p.x - cx;
|
||||
double dy = p.y - cy;
|
||||
double dz = p.z - cz;
|
||||
xx += dx * dx; xy += dx * dy; xz += dx * dz;
|
||||
yy += dy * dy; yz += dy * dz; zz += dz * dz;
|
||||
}
|
||||
|
||||
double detX = yy * zz - yz * yz;
|
||||
double detY = xx * zz - xz * xz;
|
||||
double detZ = xx * yy - xy * xy;
|
||||
double maxDet = std::max({ detX, detY, detZ });
|
||||
|
||||
Plane plane;
|
||||
if (maxDet == detX) {
|
||||
plane.A = 1;
|
||||
plane.B = (xy * yz - xz * yy) / detX;
|
||||
plane.C = (xz * yz - xy * zz) / detX;
|
||||
}
|
||||
else if (maxDet == detY) {
|
||||
plane.A = (xy * yz - xz * yy) / detY;
|
||||
plane.B = 1;
|
||||
plane.C = (xz * xy - xx * yz) / detY;
|
||||
}
|
||||
else {
|
||||
plane.A = (xz * yz - xy * zz) / detZ;
|
||||
plane.B = (yz * xy - xz * yy) / detZ;
|
||||
plane.C = 1;
|
||||
}
|
||||
plane.D = -(plane.A * cx + plane.B * cy + plane.C * cz);
|
||||
normalizePlane(plane);
|
||||
|
||||
// 2. 迭代加权最小二乘 (IRLS)
|
||||
std::vector<double> weights(n, 1.0);
|
||||
|
||||
for (int iter = 0; iter < maxIter; iter++) {
|
||||
Plane prevPlane = plane;
|
||||
|
||||
double sum_w = 0;
|
||||
double swx = 0, swy = 0, swz = 0;
|
||||
double swxx = 0, swxy = 0, swxz = 0, swyy = 0, swyz = 0;
|
||||
|
||||
for (int i = 0; i < n; i++) {
|
||||
double r = pointToPlaneSignedDist(points[i], plane);
|
||||
if (type == HUBER) weights[i] = huberWeight(r, delta);
|
||||
else weights[i] = tukeyWeight(r, delta);
|
||||
|
||||
double w = weights[i];
|
||||
sum_w += w;
|
||||
swx += w * points[i].x;
|
||||
swy += w * points[i].y;
|
||||
swz += w * points[i].z;
|
||||
}
|
||||
|
||||
double mx = swx / sum_w;
|
||||
double my = swy / sum_w;
|
||||
double mz = swz / sum_w;
|
||||
|
||||
for (int i = 0; i < n; i++) {
|
||||
double w = weights[i];
|
||||
double dx = points[i].x - mx;
|
||||
double dy = points[i].y - my;
|
||||
double dz = points[i].z - mz;
|
||||
swxx += w * dx * dx;
|
||||
swxy += w * dx * dy;
|
||||
swxz += w * dx * dz;
|
||||
swyy += w * dy * dy;
|
||||
swyz += w * dy * dz;
|
||||
}
|
||||
|
||||
// 解特征向量
|
||||
double detXw = swyy * swyz - swyz * swyz;
|
||||
double detYw = swxx * swyz - swxz * swxz;
|
||||
double detZw = swxx * swyy - swxy * swxy;
|
||||
double maxDw = std::max({ detXw, detYw, detZw });
|
||||
|
||||
if (maxDw == detXw) {
|
||||
plane.A = 1;
|
||||
plane.B = (swxy * swyz - swxz * swyy) / detXw;
|
||||
plane.C = (swxz * swyz - swxy * swyz) / detXw;
|
||||
}
|
||||
else if (maxDw == detYw) {
|
||||
plane.A = (swxy * swyz - swxz * swyy) / detYw;
|
||||
plane.B = 1;
|
||||
plane.C = (swxz * swxy - swxx * swyz) / detYw;
|
||||
}
|
||||
else {
|
||||
plane.A = (swxz * swyz - swxy * swyz) / detZw;
|
||||
plane.B = (swyz * swxy - swxz * swyy) / detZw;
|
||||
plane.C = 1;
|
||||
}
|
||||
plane.D = -(plane.A * mx + plane.B * my + plane.C * mz);
|
||||
normalizePlane(plane);
|
||||
|
||||
// ========== 关键：判断是否收敛 ==========
|
||||
double da = fabs(plane.A - prevPlane.A);
|
||||
double db = fabs(plane.B - prevPlane.B);
|
||||
double dc = fabs(plane.C - prevPlane.C);
|
||||
double dd = fabs(plane.D - prevPlane.D);
|
||||
|
||||
double maxDiff = std::max({ da, db, dc, dd });
|
||||
|
||||
// 平面几乎不再变化 → 提前终止
|
||||
if (maxDiff < convergeThresh) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
return plane;
|
||||
}
|
||||
|
||||
// 三点拟合平面
|
||||
Plane planeFrom3Points(const cv::Point3f& p1, const cv::Point3f& p2, const cv::Point3f& p3) {
|
||||
float v1x = p2.x - p1.x;
|
||||
float v1y = p2.y - p1.y;
|
||||
float v1z = p2.z - p1.z;
|
||||
|
||||
float v2x = p3.x - p1.x;
|
||||
float v2y = p3.y - p1.y;
|
||||
float v2z = p3.z - p1.z;
|
||||
|
||||
float A = v1y * v2z - v1z * v2y;
|
||||
float B = v1z * v2x - v1x * v2z;
|
||||
float C = v1x * v2y - v1y * v2x;
|
||||
float D = -(A * p1.x + B * p1.y + C * p1.z);
|
||||
|
||||
float 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);
|
||||
}
|
||||
|
||||
// ==============================================
|
||||
// 带 提前终止 的 RANSAC 平面拟合（工业正式版）
|
||||
// ==============================================
|
||||
Plane ransacFitPlane(
|
||||
const std::vector<cv::Point3f>& points,
|
||||
std::vector<cv::Point3f>& out_inliers,
|
||||
float dist_thresh, // 内点距离阈值
|
||||
int max_iter, // 最大迭代
|
||||
int stop_no_improve // 连续多少次无提升就提前退出
|
||||
)
|
||||
{
|
||||
out_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(
|
||||
points[idx[0]],
|
||||
points[idx[1]],
|
||||
points[idx[2]]
|
||||
);
|
||||
|
||||
// 统计内点
|
||||
int cnt = 0;
|
||||
for (auto& p : points) {
|
||||
if (pointToPlaneDistance(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(p, best_plane) < dist_thresh)
|
||||
out_inliers.push_back(p);
|
||||
}
|
||||
|
||||
return best_plane;
|
||||
}
|
||||
@ -12,7 +12,8 @@
|
||||
//version 1.3.0 : (1)算法进行了迭代 (2)对结果进行了分层和排序，输出最上层目标
|
||||
//version 1.4.0 : 添加了华航孔定位功能
|
||||
//version 1.4.1 : 华航孔定位客户发布初始版本，修正了1.4.0版本的一些问题
|
||||
std::string m_strVersion = "1.4.1";
|
||||
//version 1.4.2 : 华航孔定位改进：（1）法向量计算改进（2）添加3x3平滑（3）修正了bug
|
||||
std::string m_strVersion = "1.4.2";
|
||||
const char* wd_workpieceHolePositioningVersion(void)
|
||||
{
|
||||
return m_strVersion.c_str();
|
||||
@ -307,6 +308,7 @@ void wd_getHoleInfo(
|
||||
const SSG_lineSegParam lineSegPara,
|
||||
const SSG_outlierFilterParam filterParam,
|
||||
const SSG_treeGrowParam growParam,
|
||||
const double valieCommonNumRatio,
|
||||
std::vector<SWD_segFeatureTree>& segTrees_v,
|
||||
std::vector<SWD_segFeatureTree>& segTrees_h,
|
||||
std::vector<SSG_intPair>& validObjects
|
||||
@ -530,7 +532,9 @@ void wd_getHoleInfo(
|
||||
if (hTreeIdx >= 0)
|
||||
{
|
||||
int totalSize_h = treeInfo_h[hTreeIdx].treeType;
|
||||
if ((commonSize > totalSize_h / 4) && (commonSize > totalSize_v / 4))
|
||||
int sizeV_th = (int)((double)totalSize_v * valieCommonNumRatio);
|
||||
int sizeH_th = (int)((double)totalSize_h * valieCommonNumRatio);
|
||||
if ((commonSize > sizeH_th) && (commonSize > sizeV_th))
|
||||
{
|
||||
SSG_intPair a_obj;
|
||||
a_obj.data_0 = i;
|
||||
@ -676,7 +680,8 @@ void wd_workpieceHolePositioning(
|
||||
std::vector<SWD_segFeatureTree> segTrees_v;
|
||||
std::vector<SWD_segFeatureTree> segTrees_h;
|
||||
std::vector<SSG_intPair> validObjects;
|
||||
wd_getHoleInfo(scanLines, lineSegPara, filterParam, growParam, segTrees_v, segTrees_h, validObjects);
|
||||
double valieCommonNumRatio = 0.25;
|
||||
wd_getHoleInfo(scanLines, lineSegPara, filterParam, growParam, valieCommonNumRatio, segTrees_v, segTrees_h, validObjects);
|
||||
|
||||
//生成聚类信息，
|
||||
std::vector<std::vector< SVzNL2DPoint>> clusters; //只记录位置
|
||||
@ -954,6 +959,7 @@ typedef struct
|
||||
bool _getZMaxPeakROI(SVzNL2DPoint seedPos,
|
||||
std::vector<std::vector<SSG_pntDirAngle>>& pntDirAngles_v,
|
||||
std::vector<std::vector<SSG_pntDirAngle>>& pntDirAngles_h,
|
||||
double pntCornenrMax,
|
||||
SVzNLRect& roi
|
||||
)
|
||||
{
|
||||
@ -964,7 +970,7 @@ bool _getZMaxPeakROI(SVzNL2DPoint seedPos,
|
||||
int left = -1;
|
||||
for (int line = seedPos.x; line >= 0; line--)
|
||||
{
|
||||
if ((pntDirAngles_v[line][seedPos.y].type >= 0) && (abs(pntDirAngles_v[line][seedPos.y].corner) < 5.0))
|
||||
if ((pntDirAngles_v[line][seedPos.y].type >= 0) && (abs(pntDirAngles_v[line][seedPos.y].corner) < pntCornenrMax))
|
||||
{
|
||||
left = line;
|
||||
break;
|
||||
@ -974,7 +980,7 @@ bool _getZMaxPeakROI(SVzNL2DPoint seedPos,
|
||||
int right = -1;
|
||||
for (int line = seedPos.x; line < lineNum; line++)
|
||||
{
|
||||
if ((pntDirAngles_v[line][seedPos.y].type >= 0) && (abs(pntDirAngles_v[line][seedPos.y].corner) < 5.0))
|
||||
if ((pntDirAngles_v[line][seedPos.y].type >= 0) && (abs(pntDirAngles_v[line][seedPos.y].corner) < pntCornenrMax))
|
||||
{
|
||||
right = line;
|
||||
break;
|
||||
@ -984,7 +990,7 @@ bool _getZMaxPeakROI(SVzNL2DPoint seedPos,
|
||||
int top = -1;
|
||||
for (int ptIdx = seedPos.y; ptIdx >= 0; ptIdx--)
|
||||
{
|
||||
if ((pntDirAngles_h[ptIdx][seedPos.x].type >= 0) && (abs(pntDirAngles_h[ptIdx][seedPos.x].corner) < 5.0))
|
||||
if ((pntDirAngles_h[ptIdx][seedPos.x].type >= 0) && (abs(pntDirAngles_h[ptIdx][seedPos.x].corner) < pntCornenrMax))
|
||||
{
|
||||
top = ptIdx;
|
||||
break;
|
||||
@ -994,7 +1000,7 @@ bool _getZMaxPeakROI(SVzNL2DPoint seedPos,
|
||||
int bottom = -1;
|
||||
for (int ptIdx = seedPos.y; ptIdx < ptNum; ptIdx++)
|
||||
{
|
||||
if ((pntDirAngles_h[ptIdx][seedPos.x].type >= 0) && (abs(pntDirAngles_h[ptIdx][seedPos.x].corner) < 5.0))
|
||||
if ((pntDirAngles_h[ptIdx][seedPos.x].type >= 0) && (abs(pntDirAngles_h[ptIdx][seedPos.x].corner) < pntCornenrMax))
|
||||
{
|
||||
bottom = ptIdx;
|
||||
break;
|
||||
@ -1053,10 +1059,16 @@ void wd_HolePositioning(
|
||||
return;
|
||||
}
|
||||
|
||||
//添加3x3平滑
|
||||
scanLinesSmooting3x3(scanLinesInput, scanLines);
|
||||
|
||||
//内部参数
|
||||
double zPeakScale = 25.0; //计算ZPeak时的尺度
|
||||
double minZPeakHeight = 2.5; //最小的凹坑深度
|
||||
double planeInlierDistTh = 1.0; //平面点距离平面的距离。超出此距离被判别为离群点
|
||||
double zPeakScale = 10.0; //计算ZPeak时的尺度
|
||||
double minZPeakHeight = 2.0; //最小的凹坑深度
|
||||
double planeInlierDistTh = 1.5; //平面点距离平面的距离。超出此距离被判别为离群点
|
||||
double flagCornerMax = 3.0; //直线上点的拐角最大值
|
||||
double maxHoleDiameter = 30.0;//最大的孔直径
|
||||
double valieCommonNumRatio = 0.125; //1/8
|
||||
|
||||
//计算dirAngle
|
||||
std::vector<std::vector<SSG_pntDirAngle>> pntDirAngles_v;
|
||||
@ -1147,7 +1159,7 @@ void wd_HolePositioning(
|
||||
{
|
||||
//判断ROI
|
||||
SVzNLRect a_roi;
|
||||
bool validPk = _getZMaxPeakROI(a_lineZMax[j].jumpPos2D, pntDirAngles_v, pntDirAngles_h, a_roi);
|
||||
bool validPk = _getZMaxPeakROI(a_lineZMax[j].jumpPos2D, pntDirAngles_v, pntDirAngles_h, flagCornerMax, a_roi);
|
||||
if (true == validPk)
|
||||
{
|
||||
double meanZ = scanLines[a_roi.left][idx_pt].pt3D.z;
|
||||
@ -1156,7 +1168,10 @@ void wd_HolePositioning(
|
||||
meanZ += scanLines[idx_line][a_roi.bottom].pt3D.z;
|
||||
meanZ = meanZ / 4.0;
|
||||
double z_diff = a_lineZMax[j].jumpPos.z - meanZ;
|
||||
if (z_diff > minZPeakHeight)
|
||||
|
||||
double xLen = abs(scanLines[a_roi.right][idx_pt].pt3D.x - scanLines[a_roi.left][idx_pt].pt3D.x);
|
||||
double yLen = abs(scanLines[idx_line][a_roi.bottom].pt3D.y - scanLines[idx_line][a_roi.top].pt3D.y);
|
||||
if ( (z_diff > minZPeakHeight) && (xLen < maxHoleDiameter) && (yLen < maxHoleDiameter))
|
||||
{
|
||||
_zMaxInfo a_info;
|
||||
a_info.flag = 0;
|
||||
@ -1263,7 +1278,46 @@ void wd_HolePositioning(
|
||||
}
|
||||
//计算面参数: z = Ax + By + C , res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
|
||||
std::vector<double> res;
|
||||
#if 0
|
||||
vzCaculateLaserPlane(Points3ds, res);
|
||||
#else
|
||||
#if 0
|
||||
double delta = 1.0;
|
||||
int maxIter = 20;
|
||||
Plane robustPlane = robustFitPlane(
|
||||
Points3ds, TUKEY,
|
||||
delta , // 阈值：>此值视为离群点（mm）
|
||||
maxIter // 迭代次数
|
||||
);
|
||||
#else
|
||||
float dist_thresh = 0.5f;
|
||||
int max_iter = 1000;
|
||||
int stop_no_improve = 150;
|
||||
std::vector<cv::Point3f> out_inliers;
|
||||
Plane robustPlane = ransacFitPlane(
|
||||
Points3ds,
|
||||
out_inliers,
|
||||
dist_thresh, // 内点距离阈值
|
||||
max_iter, // 最大迭代
|
||||
stop_no_improve // 连续多少次无提升就提前退出
|
||||
);
|
||||
#endif
|
||||
res.resize(4);
|
||||
if (robustPlane.C > 0)
|
||||
{
|
||||
res[0] = -robustPlane.A;
|
||||
res[1] = -robustPlane.B;
|
||||
res[2] = -robustPlane.C;
|
||||
res[3] = -robustPlane.D;
|
||||
}
|
||||
else
|
||||
{
|
||||
res[0] = robustPlane.A;
|
||||
res[1] = robustPlane.B;
|
||||
res[2] = robustPlane.C;
|
||||
res[3] = robustPlane.D;
|
||||
}
|
||||
#endif
|
||||
double normValue = sqrt(pow(res[0], 2) + pow(res[1], 2) + pow(res[2],2));
|
||||
double norm_A = res[0] / normValue;
|
||||
double norm_B = res[1] / normValue;
|
||||
@ -1302,7 +1356,7 @@ void wd_HolePositioning(
|
||||
std::vector<SWD_segFeatureTree> segTrees_v;
|
||||
std::vector<SWD_segFeatureTree> segTrees_h;
|
||||
std::vector<SSG_intPair> validObjects;
|
||||
wd_getHoleInfo(roi_scanLines, lineSegPara, filterParam, growParam, segTrees_v, segTrees_h, validObjects);
|
||||
wd_getHoleInfo(roi_scanLines, lineSegPara, filterParam, growParam, valieCommonNumRatio, segTrees_v, segTrees_h, validObjects);
|
||||
if (validObjects.size() > 0)
|
||||
{
|
||||
SSG_intPair a_hvPair = validObjects[0];
|
||||
@ -1310,7 +1364,7 @@ void wd_HolePositioning(
|
||||
{
|
||||
for (int j = 1; j < (int)validObjects.size(); j++)
|
||||
{
|
||||
if (validObjects[j].idx < validObjects[j].idx)
|
||||
if (a_hvPair.idx < validObjects[j].idx)
|
||||
a_hvPair = validObjects[j];
|
||||
}
|
||||
}
|
||||
|
||||
@ -174,9 +174,9 @@ void _outputHoleInfo(char* fileName, std::vector< WD_HolePositionInfo>& holePosi
|
||||
{
|
||||
sprintf_s(dataStr, 250, "孔_%d", i + 1);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 50, " center: (%g, %g, %g)", holePositioning[i].center.x, holePositioning[i].center.y, holePositioning[i].center.z);
|
||||
sprintf_s(dataStr, 250, " center: (%g, %g, %g)", holePositioning[i].center.x, holePositioning[i].center.y, holePositioning[i].center.z);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 50, " norm_dir: (%g, %g, %g)", holePositioning[i].normDir.x, holePositioning[i].normDir.y, holePositioning[i].normDir.z);
|
||||
sprintf_s(dataStr, 250, " norm_dir: (%g, %g, %g)", holePositioning[i].normDir.x, holePositioning[i].normDir.y, holePositioning[i].normDir.z);
|
||||
sw << dataStr << std::endl;
|
||||
}
|
||||
sw.close();
|
||||
@ -823,7 +823,7 @@ void HuaHang_holePosition_test(void)
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[HH_TEST_GROUP] = {
|
||||
{1,15}, {1, 21},{1,19}, {1,9}
|
||||
{1,15}, {1, 21},{1,19}, {1,13}
|
||||
};
|
||||
|
||||
const char* ver = wd_workpieceHolePositioningVersion();
|
||||
@ -834,7 +834,7 @@ void HuaHang_holePosition_test(void)
|
||||
{
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
//fidx =2;
|
||||
//fidx =17;
|
||||
char _scan_file[256];
|
||||
sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx);
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanLines;
|
||||
@ -852,7 +852,7 @@ void HuaHang_holePosition_test(void)
|
||||
SSG_cornerParam cornerParam;
|
||||
cornerParam.cornerTh = 60; //45度角
|
||||
cornerParam.scale = 4; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
|
||||
cornerParam.minEndingGap = 20; // algoParam.bagParam.bagW / 4;
|
||||
cornerParam.minEndingGap = 10; // algoParam.bagParam.bagW / 4;
|
||||
cornerParam.minEndingGap_z = 5.0;
|
||||
cornerParam.jumpCornerTh_1 = 15; //水平角度，小于此角度视为水平
|
||||
cornerParam.jumpCornerTh_2 = 60;
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user