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

sourceCode/SG_baseAlgo_Export.h

@@ -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           // 连续多少次无提升就提前退出

sourceCode/SG_baseDataType.h

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

sourceCode/SG_errCode.h

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

sourceCode/SG_featureGrow.cpp

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

sourceCode/SG_lineFeature.cpp

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

sourceCode/WD_holeDetection.cpp

@@ -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;
+			SVzNL3DPoint startPt = lineData[ptIdx_1].pt3D;
-			a_gap.lineIdx = line;
+			SVzNL3DPoint endPt = lineData[ptIdx_2].pt3D;
-			a_gap.startPtIdx = ptIdx_1;
+			double zDiff = abs(startPt.z - endPt.z);
-			a_gap.endPtIdx = ptIdx_2;
+			if (zDiff < lineSegPara.distScale)
-			a_gap.startPt = lineData[ptIdx_1].pt3D;
+			{
-			a_gap.endPt = lineData[ptIdx_2].pt3D;
+				SWD_segFeature a_gap;
-			a_gap.featureValue = abs(a_gap.startPt.y - a_gap.endPt.y);
+				a_gap.lineIdx = line;
-			line_gaps.push_back(a_gap);
+				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;
+			SVzNL3DPoint startPt = lineData[ptIdx_1].pt3D;
-			a_gap.lineIdx = line;
+			SVzNL3DPoint endPt = lineData[ptIdx_2].pt3D;
-			a_gap.startPtIdx = ptIdx_1;
+			double zDiff = abs(startPt.z - endPt.z);
-			a_gap.endPtIdx = ptIdx_2;
+			if (zDiff < lineSegPara.distScale)
-			a_gap.startPt = lineData[ptIdx_1].pt3D;
+			{
-			a_gap.endPt = lineData[ptIdx_2].pt3D;
+				SWD_segFeature a_gap;
-			a_gap.featureValue = abs(a_gap.startPt.y - a_gap.endPt.y);
+				a_gap.lineIdx = line;
-			line_gaps.push_back(a_gap);
+				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);
 	}

sourceCode/dataFitting.cpp

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

sourceCode/workpieceHolePositioning_Export.h

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