workpieceHolePositioning version 1.5.6 : 山东本事机电轮胎定位:优化了轮胎定位算法，使用圆扫描提取轮廓点。

sourceCode/SG_baseAlgo_Export.h

@@ -319,9 +319,8 @@ SG_APISHARED_EXPORT void wd_getXYVertialFeature_dirAngleMethod(
 /// </summary>
 SG_APISHARED_EXPORT void wd_getXYVertialFeature_perSeg_dirAngleMethod(
     std::vector< SVzNL3DPosition>& lineData,
+    std::vector<SSG_RUN>& segs,
     int lineIdx,
-    const double maxDistTh,
-    const double minSegSize,
     const SSG_cornerParam cornerPara,
     std::vector<int>& xyVerticalFlags
 );
@@ -338,6 +337,16 @@ SG_APISHARED_EXPORT void wd_getXYHorizontalFeature_dirAngleMethod(
     std::vector<int>& xyHorizontalFlags
 );
 
+SG_APISHARED_EXPORT void wd_getRimConvexFeature(
+    std::vector< SVzNL3DPosition>& lineData,
+    std::vector<SSG_RUN>& segs,
+    int lineIdx,
+    const SWDRimEdgeParam rimPara_1,
+    const SWDRimEdgeParam rimPara_2,
+    const SVzNLRangeD rimWidth,
+    std::vector<SSG_RUN>& rimFeatures
+);
+
 /// 提取激光线上的拐点特征。是在PSM， LVTypeFeature, BQ等拐点算法的基础上的版本。
 /// 使用平均点距进行加速
 /// nPointIdx被重新定义成Feature类型
@@ -723,6 +732,14 @@ SG_APISHARED_EXPORT double fitCircleByLeastSquare_3(
     const std::vector<SVzNL2DPointD>& pointArray,
     SVzNL2DPointD& center,
     double& radius);
+// 加权迭代最小二乘圆拟合（Huber权重或Tukey权重）
+SG_APISHARED_EXPORT double fitCircleRobust(
+    const std::vector<SVzNL3DPosition>& points,
+    SVzNL3DPoint& robustCenter,
+    double& robustRadius,
+    double outlierThreshold = 2.5,  // 标准差倍数
+    int maxIter = 20,
+    double minPointsRatio = 0.5);
 
 //抛物线最小二乘拟合 y=ax^2 + bx + c
 SG_APISHARED_EXPORT bool leastSquareParabolaFitEigen(
@@ -819,7 +836,7 @@ SG_APISHARED_EXPORT void SG_TwoPassLabel(
 //计算面参数: z = Ax + By + C
 //res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
 SG_APISHARED_EXPORT void vzCaculateLaserPlane(
-    std::vector<cv::Point3d> Points3ds,
+    std::vector<cv::Point3d>& Points3ds,
     std::vector<double>& res);
 
 SG_APISHARED_EXPORT Plane robustFitPlane(

sourceCode/SG_baseDataType.h

@@ -41,6 +41,17 @@ typedef struct
 	double z;
 }SWD3DPoint;
 
+typedef struct 
+{
+	double cx, cy, r;
+}SWD_Circle;
+
+typedef struct
+{
+	double rimEdgeAngleTh;
+	double rimCornerTh;
+}SWDRimEdgeParam;
+
 typedef struct
 {
 	int lineIdx;

sourceCode/SG_lineFeature.cpp

@@ -5946,9 +5946,8 @@ void wd_getXYVertialFeature_dirAngleMethod(
 /// </summary>
 void wd_getXYVertialFeature_perSeg_dirAngleMethod(
 	std::vector< SVzNL3DPosition>& lineData,
+	std::vector<SSG_RUN>& segs,
 	int lineIdx,
-	const double maxDistTh,
-	const double minSegSize,
 	const SSG_cornerParam cornerPara,
 	std::vector<int>& xyVerticalFlags
 )
@@ -5960,14 +5959,7 @@ void wd_getXYVertialFeature_perSeg_dirAngleMethod(
 	xyVerticalFlags.resize(lineData.size());
 	std::fill(xyVerticalFlags.begin(), xyVerticalFlags.end(), 0);
 
-	//根据z连续性分段
+
-	std::vector<SSG_RUN> segs;
-	wd_lineDataSegment_dist_2(
-		lineData,
-		segs,
-		maxDistTh,
-		minSegSize
-	);
 	//计算前向角和后向角
 	std::vector< SSG_pntDirAngle> ptDirAngles;
 	_computeDirAngle_perSeg_2(lineData, segs, cornerPara, ptDirAngles);
@@ -6112,6 +6104,147 @@ void wd_getXYHorizontalFeature_dirAngleMethod(
 	return;
 }
 
+//提取边沿特征
+void wd_getRimConvexFeature(
+	std::vector< SVzNL3DPosition>& lineData,
+	std::vector<SSG_RUN>& segs,
+	int lineIdx,
+	const SWDRimEdgeParam rimPara_1,
+	const SWDRimEdgeParam rimPara_2,
+	const SVzNLRangeD rimWidth,
+	std::vector<SSG_RUN>& rimFeatures
+)
+{
+	//双尺度检测，分别用于缓变和剧变（对应step为2和1）
+	std::vector< SSG_pntDirAngle> ptDirAngles_1;
+	ptDirAngles_1.resize(lineData.size());
+	std::vector< SSG_pntDirAngle> ptDirAngles_2;
+	ptDirAngles_2.resize(lineData.size());
+	for (int i = 0; i < (int)lineData.size(); i++)
+	{
+		ptDirAngles_1[i].pntIdx = -1;
+		ptDirAngles_1[i].type = -1;
+		ptDirAngles_1[i].forwardPntIdx = 0;
+		ptDirAngles_1[i].backwardPntIdx = 0;
+		ptDirAngles_1[i].forwardAngle = 0;
+		ptDirAngles_1[i].backwardAngle = 0;
+		ptDirAngles_1[i].corner = 0;
+		ptDirAngles_1[i].forwardDiffZ = 0;
+		ptDirAngles_1[i].backwardDiffZ = 0;
+
+		ptDirAngles_2[i].pntIdx = -1;
+		ptDirAngles_2[i].type = -1;
+		ptDirAngles_2[i].forwardPntIdx = 0;
+		ptDirAngles_2[i].backwardPntIdx = 0;
+		ptDirAngles_2[i].forwardAngle = 0;
+		ptDirAngles_2[i].backwardAngle = 0;
+		ptDirAngles_2[i].corner = 0;
+		ptDirAngles_2[i].forwardDiffZ = 0;
+		ptDirAngles_2[i].backwardDiffZ = 0;
+	}
+	//逐段进行
+	int segSize = (int)segs.size();
+	for (int segIdx = 0; segIdx < segSize; segIdx++)
+	{
+		int vPtIdxStart = segs[segIdx].start;
+		int vPtIdxEnd = vPtIdxStart + segs[segIdx].len - 1;
+		std::vector<int> ptIndice;
+		for (int i = vPtIdxStart; i < vPtIdxEnd; i++)
+		{
+			ptDirAngles_1[i].type = segIdx;
+			ptDirAngles_2[i].type = segIdx;
+			if (lineData[i].pt3D.z > 1e-4)
+				ptIndice.push_back(i);
+		}
+
+		int vldNum = (int)ptIndice.size();
+		for (int idx = 1; idx < vldNum-1; idx++)
+		{
+			int i = ptIndice[idx];
+			int pre_i = ptIndice[idx-1];
+			int post_i = ptIndice[idx+1];
+
+			//计算拐角
+			double tanValue_pre = (lineData[i].pt3D.z - lineData[pre_i].pt3D.z) / abs(lineData[i].pt3D.y - lineData[pre_i].pt3D.y);
+			double tanValue_post = (lineData[post_i].pt3D.z - lineData[i].pt3D.z) / abs(lineData[post_i].pt3D.y - lineData[i].pt3D.y);
+			double forwardAngle = atan(tanValue_post) * 180.0 / PI;
+			double backwardAngle = atan(tanValue_pre) * 180.0 / PI;
+			ptDirAngles_1[i].pntIdx = i;
+			ptDirAngles_1[i].forwardPntIdx = post_i;
+			ptDirAngles_1[i].backwardPntIdx = pre_i;
+			ptDirAngles_1[i].forwardAngle = forwardAngle;
+			ptDirAngles_1[i].backwardAngle = backwardAngle;
+			ptDirAngles_1[i].corner = (forwardAngle - backwardAngle);
+			ptDirAngles_1[i].forwardDiffZ = lineData[post_i].pt3D.z - lineData[i].pt3D.z;
+			ptDirAngles_1[i].backwardDiffZ = lineData[i].pt3D.z - lineData[pre_i].pt3D.z;
+		}
+
+		for (int idx = 2; idx < vldNum - 2; idx++)
+		{
+			int i = ptIndice[idx];
+			int pre_i = ptIndice[idx - 2];
+			int post_i = ptIndice[idx + 2];
+
+			//计算拐角
+			double tanValue_pre = (lineData[i].pt3D.z - lineData[pre_i].pt3D.z) / abs(lineData[i].pt3D.y - lineData[pre_i].pt3D.y);
+			double tanValue_post = (lineData[post_i].pt3D.z - lineData[i].pt3D.z) / abs(lineData[post_i].pt3D.y - lineData[i].pt3D.y);
+			double forwardAngle = atan(tanValue_post) * 180.0 / PI;
+			double backwardAngle = atan(tanValue_pre) * 180.0 / PI;
+			ptDirAngles_2[i].pntIdx = i;
+			ptDirAngles_2[i].forwardPntIdx = post_i;
+			ptDirAngles_2[i].backwardPntIdx = pre_i;
+			ptDirAngles_2[i].forwardAngle = forwardAngle;
+			ptDirAngles_2[i].backwardAngle = backwardAngle;
+			ptDirAngles_2[i].corner = (forwardAngle - backwardAngle);
+			ptDirAngles_2[i].forwardDiffZ = lineData[post_i].pt3D.z - lineData[i].pt3D.z;
+			ptDirAngles_2[i].backwardDiffZ = lineData[i].pt3D.z - lineData[pre_i].pt3D.z;
+		}
+
+		//分段搜索拐点
+		int pre_start_pos = -1;
+		for (int i = vPtIdxStart; i <= vPtIdxEnd; i++)
+		{
+			//搜索开始
+			if ((i == vPtIdxStart) ||
+				((ptDirAngles_1[i].pntIdx > 0) && (ptDirAngles_1[i].backwardAngle < -rimPara_1.rimEdgeAngleTh) && (ptDirAngles_1[i].corner > rimPara_1.rimCornerTh)) ||
+				((ptDirAngles_1[i].pntIdx > 0) && (ptDirAngles_1[i].backwardAngle < -rimPara_2.rimEdgeAngleTh) && (ptDirAngles_1[i].corner > rimPara_2.rimCornerTh)) ||
+				((ptDirAngles_2[i].pntIdx > 0) && (ptDirAngles_2[i].backwardAngle < -rimPara_1.rimEdgeAngleTh) && (ptDirAngles_2[i].corner > rimPara_1.rimCornerTh)) ||
+				((ptDirAngles_2[i].pntIdx > 0) && (ptDirAngles_2[i].backwardAngle < -rimPara_2.rimEdgeAngleTh) && (ptDirAngles_2[i].corner > rimPara_2.rimCornerTh)))
+				pre_start_pos = i;
+
+			//搜索结束
+			if ((i == vPtIdxEnd) ||
+				((ptDirAngles_1[i].pntIdx > 0) && (ptDirAngles_1[i].forwardAngle > rimPara_1.rimEdgeAngleTh) && (ptDirAngles_1[i].corner > rimPara_1.rimCornerTh)) ||
+				((ptDirAngles_1[i].pntIdx > 0) && (ptDirAngles_1[i].forwardAngle > rimPara_2.rimEdgeAngleTh) && (ptDirAngles_1[i].corner > rimPara_2.rimCornerTh)) ||
+				((ptDirAngles_2[i].pntIdx > 0) && (ptDirAngles_2[i].forwardAngle > rimPara_1.rimEdgeAngleTh) && (ptDirAngles_2[i].corner > rimPara_1.rimCornerTh)) ||
+				((ptDirAngles_2[i].pntIdx > 0) && (ptDirAngles_2[i].forwardAngle > rimPara_2.rimEdgeAngleTh) && (ptDirAngles_2[i].corner > rimPara_2.rimCornerTh)))
+			{
+				if (pre_start_pos >= 0)
+				{
+					if ((ptDirAngles_1[pre_start_pos].pntIdx >= 0) || (ptDirAngles_1[i].pntIdx >= 0) ||
+						(ptDirAngles_2[pre_start_pos].pntIdx >= 0) || (ptDirAngles_2[i].pntIdx >= 0))
+					{
+						double width = sqrt(pow(lineData[pre_start_pos].pt3D.x - lineData[i].pt3D.x, 2) +
+							pow(lineData[pre_start_pos].pt3D.y - lineData[i].pt3D.y, 2) +
+							pow(lineData[pre_start_pos].pt3D.z - lineData[i].pt3D.z, 2));
+						if ((width >= rimWidth.min) && (width <= rimWidth.max))
+						{
+							//一个rim
+							SSG_RUN a_rim;
+							a_rim.start = pre_start_pos;
+							a_rim.len = i - pre_start_pos + 1;
+							a_rim.curveLen = width;
+							a_rim.value = segIdx;
+							rimFeatures.push_back(a_rim);
+							pre_start_pos = -1;
+						}
+					}
+				}
+			}
+		}
+	}
+}
+
 /// 提取激光线上的拐点特征。是在PSM， LVTypeFeature, BQ等拐点算法的基础上的版本。
 /// 使用平均点距进行加速
 /// nPointIdx被重新定义成Feature类型

sourceCode/dataFitting.cpp

@@ -319,6 +319,122 @@ double fitCircleByLeastSquare_3(
 	return err;
 }
 
+// 计算点到圆的几何距离（带符号）
+double geometricDistance(const SVzNL3DPosition& p, const SVzNL3DPoint& center, const double R) {
+	return std::sqrt((p.pt3D.x - center.x) * (p.pt3D.x - center.x) + (p.pt3D.y - center.y) * (p.pt3D.y - center.y)) - R;
+}
+
+// ========== 计算几何误差（点到圆的距离） ==========
+double calcGeometricError(const SVzNL3DPosition& p, const SWD_Circle& c) {
+	double dx = p.pt3D.x - c.cx;
+	double dy = p.pt3D.y - c.cy;
+	return fabs(sqrt(dx * dx + dy * dy) - c.r);
+}
+
+// ========== 计算所有点的误差统计 ==========
+void calcErrorStats(const std::vector<SVzNL3DPosition>& points, const SWD_Circle& c,
+	std::vector<double>& errors, double& mean, double& stddev) 
+{
+	int n = points.size();
+	errors.resize(n);
+	mean = 0;
+	for (int i = 0; i < n; ++i) {
+		errors[i] = calcGeometricError(points[i], c);
+		mean += errors[i];
+	}
+	mean /= n;
+
+	stddev = 0;
+	for (int i = 0; i < n; ++i) {
+		double d = errors[i] - mean;
+		stddev += d * d;
+	}
+	stddev = sqrt(stddev / n);
+}
+
+// ========== 主函数：迭代剔除离群点 + 最小二乘拟合 ==========
+double fitCircleRobust(
+	const std::vector<SVzNL3DPosition>& points,
+	SVzNL3DPoint& robustCenter,
+	double& robustRadius,
+	double outlierThreshold,  // 标准差倍数
+	int maxIter,
+	double minPointsRatio) 
+{
+	if (points.size() < 3) return 0;
+
+	// 用所有点初始化
+	std::vector<SVzNL3DPosition> workingPoints = points;
+	SVzNL3DPoint c0;
+	double r0;
+	double errVal = fitCircleByLeastSquare_2(workingPoints, c0, r0);
+	SWD_Circle bestCircle = { c0.x, c0.y, r0 };
+
+	double bestMeanError = std::numeric_limits<double>::max();
+	std::vector<SVzNL3DPosition> bestPoints = workingPoints;
+
+	for (int iter = 0; iter < maxIter; ++iter) {
+		// 计算当前拟合圆的误差统计
+		std::vector<double> errors;
+		double mean, stddev;
+		calcErrorStats(workingPoints, bestCircle, errors, mean, stddev);
+
+		// 检查收敛：如果平均误差不再显著改善
+		if (mean >= bestMeanError * 0.99 && iter > 0) 
+			break;
+		bestMeanError = mean;
+
+		// 如果标准差太小，说明已经收敛
+		if (stddev < 1e-8) break;
+
+		// 剔除离群点：误差 > mean + outlierThreshold * stddev
+		double threshold = mean + outlierThreshold * stddev;
+		std::vector<SVzNL3DPosition> filteredPoints;
+		filteredPoints.reserve(workingPoints.size());
+
+		for (size_t i = 0; i < workingPoints.size(); ++i) {
+			if (errors[i] <= threshold) {
+				filteredPoints.push_back(workingPoints[i]);
+			}
+		}
+
+		// 如果剩余点太少，停止剔除
+		if (filteredPoints.size() < points.size() * minPointsRatio ||
+			filteredPoints.size() < 3) {
+			break;
+		}
+
+		// 用剩余点重新拟合
+		SVzNL3DPoint center;
+		double radius;
+		errVal = fitCircleByLeastSquare_2(filteredPoints, center, radius);
+		SWD_Circle newCircle = { center.x, center.y, radius };
+
+		// 检查新圆是否更优（用平均误差衡量）
+		double newMean = 0;
+		for (const auto& p : filteredPoints) {
+			newMean += calcGeometricError(p, newCircle);
+		}
+		newMean /= filteredPoints.size();
+
+		if (newMean < bestMeanError) {
+			bestCircle = newCircle;
+			workingPoints = filteredPoints;
+			bestPoints = filteredPoints;
+		}
+		else {
+			// 新圆不如旧圆好，停止迭代
+			break;
+		}
+	}
+	robustCenter.x = bestCircle.cx;
+	robustCenter.y = bestCircle.cy;
+	robustCenter.z = 0;
+	robustRadius = bestCircle.r;
+	return bestMeanError;
+}
+
+
 #if 0
 bool leastSquareParabolaFit(const std::vector<cv::Point2d>& points,
 	double& a, double& b, double& c,
@@ -464,7 +580,7 @@ bool leastSquareParabolaFitEigen(
 
 //计算面参数: z = Ax + By + C
 //res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
-void vzCaculateLaserPlane(std::vector<cv::Point3d> Points3ds, std::vector<double>& res)
+void vzCaculateLaserPlane(std::vector<cv::Point3d>& Points3ds, std::vector<double>& res)
 {
 	//最小二乘法拟合平面
 	//获取cv::Mat的坐标系以纵向为x轴，横向为y轴，而cvPoint等则相反

sourceCode/workpieceHolePositioning_Export.h

@@ -33,6 +33,13 @@ typedef struct
 	double thickness;  //轮胎宽度
 }WD_tireParam;
 
+typedef struct
+{
+	double len; //工件长
+	double width;  //工件宽度
+	double thickness;
+}WD_JiuruiWorkpieceParam;
+
 typedef struct
 {
 	SVzNL3DPoint center;
@@ -43,6 +50,7 @@ typedef struct
 typedef struct
 {
 	double rotorDiameter;
+	double rotorH;
 	double removeGroundHOffset; //去除时的高度偏置，用于设置架子高度
 	double basket_L;  //零件筐-长
 	double basket_W;  //零件筐-宽
@@ -112,3 +120,12 @@ SG_APISHARED_EXPORT void sx_getRotorCorePose(
 	const WD_rotorAppParam rotorParam,
 	std::vector<WD_HolePositionInfo>& rotorPositions,
 	int* errCode);
+
+//北京玖瑞工件定位
+SG_APISHARED_EXPORT void Jiurui_getWorkpiecePose(
+	std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
+	const SSG_cornerParam cornerPara,
+	const SSG_planeCalibPara groundCalibPara,
+	const WD_JiuruiWorkpieceParam workpieceParam,
+	std::vector<SSG_pointPose>& tirePositions,
+	int* errCode);

workpieceHolePositioning_test/workpieceHolePositioning_test.cpp

@@ -635,11 +635,22 @@ void _outputRGBDResult_HoleInfo(
 		if (linePtNum == 0)
 			continue;
 
+		std::vector< SVzNL3DPosition> vlPts;
+		for (int i = 0; i < linePtNum; i++)
+		{
+			if (scanLines[line][i].pt3D.z > 1e-4)
+				vlPts.push_back(scanLines[line][i]);
+		}
+
+		linePtNum = (int)vlPts.size();
+		if (linePtNum == 0)
+			continue;
+
 		sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl;
 		lineIdx++;
 		for (int i = 0; i < linePtNum; i++)
 		{
-			SVzNL3DPosition* pt3D = &scanLines[line][i];
+			SVzNL3DPosition* pt3D = &vlPts[i];
 			if (pt3D->nPointIdx > 0)
 				int kkk = 1;
 			int flag = pt3D->nPointIdx & 0xffff;
@@ -1306,7 +1317,7 @@ void Benshi_tirePosition_test(void)
 	};
 
 	SVzNLRange fileIdx[BENSHI_TIRE_TEST_GROUP] = {
-		{1,19},
+		{1,34},
 	};
 
 	const char* ver = wd_workpieceHolePositioningVersion();
@@ -1394,7 +1405,7 @@ void Benshi_tirePosition_test(void)
 
 		for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
 		{
-			//fidx =17;
+			//fidx =18;
 			char _scan_file[256];
 			sprintf_s(_scan_file, "%s%d_LaserData_Ik256400.txt", dataPath[grp], fidx);
 			std::vector<std::vector< SVzNL3DPosition>> scanLines;
@@ -1439,18 +1450,7 @@ void Benshi_tirePosition_test(void)
 #endif
 }
 
-
+//海瑞马
-typedef enum
-{
-	keSG_拓普发孔定位 = 0,
-	keSG_华航孔定位,
-	keSG_微力砂轮盘定位,
-	keSG_微力砂轮架子定位,
-	keSG_山东本事轮胎定位,
-	keSG_宁波海瑞马转子芯定位,
-} ESG_testMode;
-
-//山东本事机电轮胎定位
 #define HAIRUIMA_COMPUTE_CALIB_PARA	0
 #define HAIRUIMA_HOLE_POSITION		1
 
@@ -1572,7 +1572,8 @@ void HaiRuiMa_rotorCorePosition_test(void)
 			cornerParam.jumpCornerTh_2 = 60;
 
 			WD_rotorAppParam rotorParam;
-			rotorParam.rotorDiameter = 76;
+			rotorParam.rotorDiameter = 72;
+			rotorParam.rotorH = 40.0; 
 			rotorParam.removeGroundHOffset = 100.0;  //去除时的高度偏置，用于设置架子高度
 			rotorParam.basket_L = 770;
 			rotorParam.basket_W = 580;
@@ -1599,7 +1600,166 @@ void HaiRuiMa_rotorCorePosition_test(void)
 #endif
 }
 
+#define JIURUI_COMPUTE_CALIB_PARA	0
+#define JIURUI_HOLE_POSITION		1
 
+#define JIURUI_TIRE_TEST_GROUP 2
+void JiuRui_workpiecePosition_test(void)
+{
+	const char* dataPath[JIURUI_TIRE_TEST_GROUP] = {
+		"F:/ShangGu/项目/冠钦项目/北京玖瑞/工件识别/工件数据/1367mm/",//0
+		"F:/ShangGu/项目/冠钦项目/北京玖瑞/工件识别/工件数据/距离工件1183mm/", //1
+	};
+
+	SVzNLRange fileIdx[JIURUI_TIRE_TEST_GROUP] = {
+		{1,8},{1,6}
+	};
+
+	const char* ver = wd_workpieceHolePositioningVersion();
+	printf("ver:%s\n", ver);
+
+#if JIURUI_COMPUTE_CALIB_PARA
+	int cvtGrp = 0;
+	char _calib_datafile[256];
+	sprintf_s(_calib_datafile, "%s调平.txt", dataPath[cvtGrp]);
+	int lineNum = 0;
+	float lineV = 0.0f;
+	int dataCalib = 0;
+	int maxTimeStamp = 0;
+	int clockPerSecond = 0;
+	std::vector<std::vector< SVzNL3DPosition>> scanData;
+	vzReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanData);
+
+	lineNum = (int)scanData.size();
+	if (scanData.size() > 0)
+	{
+		SSG_planeCalibPara calibPara = wd_getGroundCalibPara(scanData);
+		//结果进行验证  
+		for (int i = 0; i < lineNum; i++)
+		{
+			if (i == 14)
+				int kkk = 1;
+			//行处理
+			//调平，去除地面
+			wd_lineDataR(scanData[i], calibPara.planeCalib, -1);
+		}
+		//
+		char calibFile[250];
+		sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[cvtGrp]);
+		_outputCalibPara(calibFile, calibPara);
+		char _out_file[256];
+		sprintf_s(_out_file, "%sscanData_ground_calib_verify.txt", dataPath[cvtGrp]);
+		int headNullLines = 0;
+		_outputScanDataFile_vector(_out_file, scanData, false, &headNullLines);
+#if 0
+		for (int fidx = fileIdx[cvtGrp].nMin; fidx <= fileIdx[cvtGrp].nMax; fidx++)
+		{
+			//fidx =4;
+			char _scan_file[256];
+			sprintf_s(_scan_file, "%s%d_LaserData_Ik256400.txt", dataPath[cvtGrp], fidx);
+			std::vector<std::vector< SVzNL3DPosition>> scanLines;
+			vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
+			if (scanLines.size() == 0)
+				continue;
+			lineNum = (int)scanLines.size();
+			for (int i = 0; i < lineNum; i++)
+			{
+				//调平，去除地面
+				wd_lineDataR(scanLines[i], calibPara.planeCalib, -1);
+			}
+			sprintf_s(_scan_file, "%sLaserData_%d_calib_verify.txt", dataPath[cvtGrp], fidx);
+			int headNullLines = 0;
+			_outputScanDataFile_vector(_scan_file, scanLines, false, &headNullLines);
+		}
+#endif
+		printf("%s: calib done!\n", _calib_datafile);
+	}
+#endif
+
+#if JIURUI_HOLE_POSITION
+	for (int grp = 0; grp <= 1; grp++)
+	{
+		SSG_planeCalibPara groundCalibPara;
+		//初始化成单位阵
+		groundCalibPara.planeCalib[0] = 1.0;
+		groundCalibPara.planeCalib[1] = 0.0;
+		groundCalibPara.planeCalib[2] = 0.0;
+		groundCalibPara.planeCalib[3] = 0.0;
+		groundCalibPara.planeCalib[4] = 1.0;
+		groundCalibPara.planeCalib[5] = 0.0;
+		groundCalibPara.planeCalib[6] = 0.0;
+		groundCalibPara.planeCalib[7] = 0.0;
+		groundCalibPara.planeCalib[8] = 1.0;
+		groundCalibPara.planeHeight = -1.0;
+		for (int i = 0; i < 9; i++)
+			groundCalibPara.invRMatrix[i] = groundCalibPara.planeCalib[i];
+		char calibFile[250];
+		sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]);
+		//groundCalibPara = _readCalibPara(calibFile);
+
+		for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
+		{
+			//fidx =17;
+			char _scan_file[256];
+			sprintf_s(_scan_file, "%s%d_LaserData_Hi229229.txt", dataPath[grp], fidx);
+			std::vector<std::vector< SVzNL3DPosition>> scanLines;
+			vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
+			if (scanLines.size() == 0)
+				continue;
+
+			long t1 = (long)GetTickCount64();//统计时间
+
+			SSG_cornerParam cornerParam;
+			cornerParam.cornerTh = 60;  //45度角
+			cornerParam.scale = 10; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
+			cornerParam.minEndingGap = 10; // algoParam.bagParam.bagW / 4;
+			cornerParam.minEndingGap_z = 5.0;
+			cornerParam.jumpCornerTh_1 = 15; //水平角度，小于此角度视为水平
+			cornerParam.jumpCornerTh_2 = 60;
+
+			WD_JiuruiWorkpieceParam workpieceParam;
+			workpieceParam.len = 320;
+			workpieceParam.width = 140;
+			workpieceParam.thickness = 48;
+
+			int errCode = 0;
+			std::vector<SSG_pointPose> workpiecePositions;
+			Jiurui_getWorkpiecePose(
+				scanLines,
+				cornerParam,
+				groundCalibPara,
+				workpieceParam,
+				workpiecePositions,
+				&errCode);
+
+			long t2 = (long)GetTickCount64();
+			printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1));
+			//输出测试结果
+			double dirLen = 200;
+			sprintf_s(_scan_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx);
+			SSG_pointPose a_pose;
+			memset(&a_pose, 0, sizeof(SSG_pointPose));
+			if (workpiecePositions.size() > 0)
+				a_pose = workpiecePositions[0];
+			_outputRGBDResult_RGBD_2(_scan_file, scanLines, a_pose);
+			sprintf_s(_scan_file, "%sresult\\LaserLine%d_corner_info.txt", dataPath[grp], fidx);
+			_outputHoleInfo_3(_scan_file, a_pose);
+		}
+	}
+#endif
+}
+
+
+typedef enum
+{
+	keSG_拓普发孔定位 = 0,
+	keSG_华航孔定位,
+	keSG_微力砂轮盘定位,
+	keSG_微力砂轮架子定位,
+	keSG_山东本事轮胎定位,
+	keSG_宁波海瑞马转子芯定位,
+	keSG_北京玖瑞工件定位,
+} ESG_testMode;
 int main()
 {
 	//ESG_testMode testMode = keSG_拓普发孔定位;
@@ -1608,6 +1768,7 @@ int main()
 	//ESG_testMode testMode = keSG_微力砂轮架子定位;
 	ESG_testMode testMode = keSG_山东本事轮胎定位;
 	//ESG_testMode testMode = keSG_宁波海瑞马转子芯定位;
+	//ESG_testMode testMode = keSG_北京玖瑞工件定位;
 
 	if (keSG_拓普发孔定位 == testMode)
 		TuoPuFa_holePosition_test();
@@ -1621,4 +1782,6 @@ int main()
 		Benshi_tirePosition_test();
 	else if (keSG_宁波海瑞马转子芯定位 == testMode)
 		HaiRuiMa_rotorCorePosition_test();
+	else if (keSG_北京玖瑞工件定位 == testMode)
+		JiuRui_workpiecePosition_test();
 }