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

workpieceHolePositioning version 1.6.1 : 添加了拓普发工件2、工件3、工件4的姿态计算。

Browse Source
This commit is contained in:
jerryzeng 2026-07-14 01:05:51 +08:00
parent 2ed102c9db
commit 5c614084d9
10 changed files with 2678 additions and 411 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
sourceCode/SG_baseAlgo_Export.h
View File

@ -851,7 +851,7 @@ SG_APISHARED_EXPORT Plane robustFitPlane(
// ==============================================
SG_APISHARED_EXPORT Plane ransacFitPlane(
const std::vector<cv::Point3d>& points,
std::vector<cv::Point3d>& out_inliers,
std::vector<cv::Point3d>& in_inliers,
float dist_thresh = 0.3f, // 内点距离阈值
int max_iter = 1000, // 最大迭代
int stop_no_improve = 250 // 连续多少次无提升就提前退出

14
sourceCode/SG_baseDataType.h
View File

@ -13,6 +13,12 @@ typedef struct{
double yaw; // 绕Z轴旋转（偏航）
}SSG_EulerAngles;
typedef struct
{
double yaw_deg; //ˮƽ½Ç
double pitch_deg; //´¹Ö±½Ç
double range_mm; //¾àÀë
}SWD_poloarScan2D;
//以工作轴为X轴(扫描方向），平行地面与Z轴垂直的轴为Y轴，垂直地面的轴为Z轴
typedef struct
{
@ -108,6 +114,14 @@ typedef struct
int idx;
}SSG_intPair;
typedef struct
{
int data_0;
int data_1;
double value1;
double value2;
}SSG_intPairRelationD;
typedef struct
{
int flag;

1
sourceCode/SG_errCode.h
View File

@ -11,6 +11,7 @@
#define SG_ERR_LASER_DIR_NOT_SUPPORTED -1008
#define SG_ERR_SCAN_DIR_NOT_SUPPORTED -1009
#define SX_ERR_ZERO_OBJECTS -1010
#define SX_ERR_NULL_GROUND_PLANE -1011
//BQ_workpiece
#define SX_ERR_INVLD_VTREE_NUM -2001

4
sourceCode/SG_featureGrow.cpp
View File

@ -165,6 +165,8 @@ bool _segFeatureGrowing_2(SWD_segFeature& a_feature, const int lineIdx, std::vec
double z_diff_1 = abs(a_feature.startPt.z - last_node.startPt.z);
double z_diff_2 = abs(a_feature.endPt.z - last_node.endPt.z);
int line_diff = abs(a_feature.lineIdx - last_node.lineIdx);
if (line_diff == 1)
int kkk = 1;
double x_diff = (abs(a_feature.startPt.x - last_node.startPt.x) + abs(a_feature.endPt.x - last_node.endPt.x)) / 2;
if ((y_diff_1 < growParam.yDeviation_max) && (y_diff_2 < growParam.yDeviation_max) &&
(z_diff_1 < growParam.zDeviation_max) && (z_diff_2 < growParam.zDeviation_max) &&
@ -774,7 +776,7 @@ void wd_getSegFeatureGrowingTrees_2(
{
for (int i = 0, i_max = (int)all_lineFeatures.size(); i < i_max; i++)
{
if (i == 1047)
if (i == 111)
int kkk = 1;
std::vector<SWD_segFeature>& a_lineFeatures = all_lineFeatures[i];
for (int j = 0, j_max = (int)a_lineFeatures.size(); j < j_max; j++)

2
sourceCode/SG_lineFeature.cpp
View File

@ -232,7 +232,7 @@ void wd_getLineDataIntervals(
{
double z_diff = abs(lineData[i].pt3D.z - pre_z);
double y_diff = abs(lineData[i].pt3D.y - pre_y);
if ((z_diff < lineSegPara.segGapTh_z) && (y_diff < lineSegPara.segGapTh_y))
if ( (z_diff < lineSegPara.segGapTh_z) && (y_diff < lineSegPara.segGapTh_y))
{
a_run.len = i - a_run.start + 1;
a_run.value = i;

46
sourceCode/WD_holeDetection.cpp
View File

@ -107,7 +107,7 @@ void WD_getHoleInfo(
//提取线段端点特征
for (int line = 0; line < lineNum; line++)
{
if (line == 1047)
if (line == 137)
int kkk = 1;
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
@ -125,14 +125,20 @@ void WD_getHoleInfo(
{
int ptIdx_1 = segs[i].start;
int ptIdx_2 = segs[i].start + segs[i].len - 1;
SWD_segFeature a_gap;
a_gap.lineIdx = line;
a_gap.startPtIdx = ptIdx_1;
a_gap.endPtIdx = ptIdx_2;
a_gap.startPt = lineData[ptIdx_1].pt3D;
a_gap.endPt = lineData[ptIdx_2].pt3D;
a_gap.featureValue = abs(a_gap.startPt.y - a_gap.endPt.y);
line_gaps.push_back(a_gap);
SVzNL3DPoint startPt = lineData[ptIdx_1].pt3D;
SVzNL3DPoint endPt = lineData[ptIdx_2].pt3D;
double zDiff = abs(startPt.z - endPt.z);
if (zDiff < lineSegPara.distScale)
{
SWD_segFeature a_gap;
a_gap.lineIdx = line;
a_gap.startPtIdx = ptIdx_1;
a_gap.endPtIdx = ptIdx_2;
a_gap.startPt = startPt;
a_gap.endPt = endPt;
a_gap.featureValue = abs(a_gap.startPt.y - a_gap.endPt.y);
line_gaps.push_back(a_gap);
}
}
holeGaps.push_back(line_gaps);
}
@ -174,14 +180,20 @@ void WD_getHoleInfo(
{
int ptIdx_1 = segs[i].start;
int ptIdx_2 = segs[i].start + segs[i].len - 1;
SWD_segFeature a_gap;
a_gap.lineIdx = line;
a_gap.startPtIdx = ptIdx_1;
a_gap.endPtIdx = ptIdx_2;
a_gap.startPt = lineData[ptIdx_1].pt3D;
a_gap.endPt = lineData[ptIdx_2].pt3D;
a_gap.featureValue = abs(a_gap.startPt.y - a_gap.endPt.y);
line_gaps.push_back(a_gap);
SVzNL3DPoint startPt = lineData[ptIdx_1].pt3D;
SVzNL3DPoint endPt = lineData[ptIdx_2].pt3D;
double zDiff = abs(startPt.z - endPt.z);
if (zDiff < lineSegPara.distScale)
{
SWD_segFeature a_gap;
a_gap.lineIdx = line;
a_gap.startPtIdx = ptIdx_1;
a_gap.endPtIdx = ptIdx_2;
a_gap.startPt = startPt;
a_gap.endPt = endPt;
a_gap.featureValue = abs(a_gap.startPt.y - a_gap.endPt.y);
line_gaps.push_back(a_gap);
}
}
holeGaps_h.push_back(line_gaps);
}

6
sourceCode/dataFitting.cpp
View File

@ -873,13 +873,13 @@ Plane planeFrom3Points(const cv::Point3d& p1, const cv::Point3d& p2, const cv::P
// ==============================================
Plane ransacFitPlane(
const std::vector<cv::Point3d>& points,
std::vector<cv::Point3d>& out_inliers,
std::vector<cv::Point3d>& in_inliers,
float dist_thresh, // 内点距离阈值
int max_iter, // 最大迭代
int stop_no_improve // 连续多少次无提升就提前退出
)
{
out_inliers.clear();
in_inliers.clear();
int n = points.size();
if (n < 3) return Plane();
@ -928,7 +928,7 @@ Plane ransacFitPlane(
// 收集内点
for (auto& p : points) {
if (pointToPlaneDistance(p, best_plane) < dist_thresh)
out_inliers.push_back(p);
in_inliers.push_back(p);
}
return best_plane;

2042
sourceCode/workpieceHolePositioning.cpp
View File

File diff suppressed because it is too large Load Diff

23
sourceCode/workpieceHolePositioning_Export.h
View File

@ -70,7 +70,7 @@ SG_APISHARED_EXPORT void wd_lineDataR(
const double* camPoseR,
double groundH);
//工件孔定位-拓普发工件孔定位
//工件1: 孔定位-拓普发工件孔定位
SG_APISHARED_EXPORT void wd_workpieceHolePositioning(
std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
const WD_workpieceHoleParam workpiecePara,
@ -80,6 +80,27 @@ SG_APISHARED_EXPORT void wd_workpieceHolePositioning(
const SSG_planeCalibPara groundCalibPara,
std::vector< WD_workpieceInfo>& workpiecePositioning,
int* errCode);
//工件2: 孔定位-拓普发工件孔定位
SG_APISHARED_EXPORT void wd_workpieceHolePositioning_2(
std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
const WD_workpieceHoleParam workpiecePara,
const SSG_lineSegParam lineSegPara,
const SSG_outlierFilterParam filterParam,
const SSG_treeGrowParam growParam,
const SSG_planeCalibPara groundCalibPara,
std::vector< WD_workpieceInfo>& workpiecePositioning,
int* errCode);
//工件3: 孔定位-拓普发工件孔定位
SG_APISHARED_EXPORT void wd_workpieceHolePositioning_3(
std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
const WD_workpieceHoleParam smallHoleParam,
const double bigHoleDiameter,
const SSG_lineSegParam lineSegPara,
const SSG_outlierFilterParam filterParam,
const SSG_treeGrowParam growParam,
const SSG_planeCalibPara groundCalibPara,
std::vector< WD_workpieceInfo>& workpiecePositioning,
int* errCode);
//单个孔或凹坑定位-华航孔定位
SG_APISHARED_EXPORT void wd_HolePositioning(

949
workpieceHolePositioning_test/workpieceHolePositioning_test.cpp
View File

File diff suppressed because it is too large Load Diff