#include <vector>
#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include "workpieceHolePositioning_Export.h"
#include <opencv2/opencv.hpp>
#include <limits>

#define _DEBUG_OUTPUT

//version 1.0.0 : base version release to customer
//version 1.0.2 : 添加了工件姿态（欧拉角输出)
//version 1.1.0 : c对工件姿态规范化为中心点（操作点）加三个方向矢量
//version 1.2.0 : 算法完成了6轴验证
//version 1.3.0 : (1)算法进行了迭代 (2)对结果进行了分层和排序，输出最上层目标
//version 1.4.0 : 添加了华航孔定位功能
//version 1.4.1 : 华航孔定位客户发布初始版本，修正了1.4.0版本的一些问题
//version 1.4.2 : 华航孔定位改进：（1）法向量计算改进（2）添加3x3平滑（3）修正了bug
//version 1.4.3 : 对1.3.0拓普发孔定位中调平Bug进行了修正
//version 1.4.4 : 对1.4.3一个小的修正， 不影响结果
//version 1.4.5 : 添加异物检测，通过errCode输出“有异物”和“无产品”结果
//version 1.4.6 : 修正问题：4个孔组成工作时，需要4个孔的高度基本一致。
//version 1.4.7 : 修正问题：在进行异物检测时，计算ZSliceTh时。添加Z计算的保护，防止异常值混入
//version 1.4.8 : 将工件法向调整为垂直于工件表面
//version 1.4.9 : 修正工件法向计算的一个Bug
//version 1.5.0 : 修正工件Y方向矢量计算的一个Bug（不在工件表面平面内）
//version 1.5.1 : 异物检测时，检测高度由工件最小Z向上1.5mm调整为工件最小Z向上工件高度的一半，工件倾斜时容错性能更好。
//version 1.5.2 : 修正工件判断中的一个逻辑错误。
//version 1.5.3 : 添加郑州微力砂轮盘和轮盘架定位API。
//version 1.5.4 : 添加山东本事机电轮胎定位API。
//version 1.5.5 : 山东本事机电轮胎定位。（1）添加了轮胎内径大小的输出， （2）按高度排序。最上面的目标排在第一个。
//version 1.5.6 : 山东本事机电轮胎定位:优化了轮胎定位算法，使用圆扫描提取轮廓点。
//version 1.6.0 : 添加了玖瑞工件（单件）的姿态计算。
//version 1.6.1 : 添加了拓普发工件2、工件3、工件4的姿态计算。
//version 1.6.2 : 拓普发算法代码同步。
//version 1.6.3 : 拓普发5种工件算法。共3个API。 工件2和工作5使用API2, 工件3和工件4使用API3。
//version 1.6.4 : 拓普发5种工件添加正反判断。
//version 1.6.5 : 拓普发工件4和工件5姿态计算优化，首先寻找垂直方向孔（原算法首先寻找水平方向孔）。
std::string	m_strVersion = "HolePostion 1.6.5";
const char* wd_workpieceHolePositioningVersion(void)
{
	return m_strVersion.c_str();
}

typedef struct
{
	int data_0;
	int data_1;
	double pointDistance;
	double distFromCenterToLine; //参考中心点到两点构成的直线的距离
	double pointZDiff;
}SSG_pointPairRelationD;

#ifdef _DEBUG_OUTPUT
// 二维 int vector (灰度像素 0~255) 转 Mat
cv::Mat vec2Int2VisMat(const std::vector<std::vector<int>>& data)
{
	if (data.empty())
		return cv::Mat();

	int rows = data.size();
	int cols = data[0].size();
	cv::Mat mat(rows, cols, CV_32FC1);

	// 填充浮点数据
	for (int i = 0; i < rows; i++)
	{
		float* ptr = mat.ptr<float>(i);
		for (int j = 0; j < cols; j++)
		{
			ptr[j] = (float)data[i][j];
		}
	}

	// 归一化到 0~255 用于显示
	cv::Mat vis;
	cv::normalize(mat, vis, 0, 255, cv::NORM_MINMAX, CV_8UC1);
	return vis;
}
// 二维 float vector (深度/浮点数据) 转可视化灰度图
cv::Mat vec2Float2VisMat(const std::vector<std::vector<double>>& data)
{
	if (data.empty())
		return cv::Mat();

	int rows = data.size();
	int cols = data[0].size();
	cv::Mat mat(rows, cols, CV_32FC1);

	// 填充浮点数据
	for (int i = 0; i < rows; i++)
	{
		float* ptr = mat.ptr<float>(i);
		for (int j = 0; j < cols; j++)
		{
			ptr[j] = (float)data[i][j];
		}
	}

	// 归一化到 0~255 用于显示
	cv::Mat vis;
	cv::normalize(mat, vis, 0, 255, cv::NORM_MINMAX, CV_8UC1);
	return vis;
}
// 二维 方差 (深度/浮点数据) 转可视化灰度图
cv::Mat vec2Var2VisMat(const std::vector<std::vector<SSG_meanVar>>& data)
{
	if (data.empty())
		return cv::Mat();

	int rows = data.size();
	int cols = data[0].size();
	cv::Mat mat(rows, cols, CV_32FC1);

	// 填充浮点数据
	for (int i = 0; i < rows; i++)
	{
		float* ptr = mat.ptr<float>(i);
		for (int j = 0; j < cols; j++)
		{
			ptr[j] = (float)data[i][j].var;
		}
	}

	// 归一化到 0~255 用于显示
	cv::Mat vis;
	cv::normalize(mat, vis, 0, 255, cv::NORM_MINMAX, CV_8UC1);
	return vis;
}
// 二维 方差 (深度/浮点数据) 转可视化灰度图
cv::Mat vec2VarAdd2VisMat(const std::vector<std::vector<SSG_meanVar>>& data1, const std::vector<std::vector<SSG_meanVar>>& data2)
{
	if ((data1.empty())|| (data2.empty()))
		return cv::Mat();

	int rows = data1.size();
	int cols = data1[0].size();
	if( (data2.size() != rows) || (data2[0].size() != cols))
		return cv::Mat();

	cv::Mat mat(rows, cols, CV_32FC1);
	// 填充浮点数据
	for (int i = 0; i < rows; i++)
	{
		float* ptr = mat.ptr<float>(i);
		for (int j = 0; j < cols; j++)
		{
			ptr[j] = (float)(data1[i][j].var + data2[i][j].var);
		}
	}

	// 归一化到 0~255 用于显示
	cv::Mat vis;
	cv::normalize(mat, vis, 0, 255, cv::NORM_MINMAX, CV_8UC1);
	return vis;
}
#endif

//相机水平安装计算地面调平参数。
//相机Z轴基本平行地面时，需要以地面为参照，将相机调水平
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
SSG_planeCalibPara wd_getGroundCalibPara(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
	return sg_getPlaneCalibPara2(scanLines);
}

//相机水平时姿态调平，并去除地面
void wd_lineDataR(
	std::vector< SVzNL3DPosition>& a_line,
	const double* camPoseR,
	double groundH)
{
	lineDataRT_vector(a_line, camPoseR, groundH);
}

SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, const double matrix3d[9])
{
	SVzNL3DPoint _r_pt;
	_r_pt.x = pt3D.x * matrix3d[0] + pt3D.y * matrix3d[1] + pt3D.z * matrix3d[2];
	_r_pt.y = pt3D.x * matrix3d[3] + pt3D.y * matrix3d[4] + pt3D.z * matrix3d[5];
	_r_pt.z = pt3D.x * matrix3d[6] + pt3D.y * matrix3d[7] + pt3D.z * matrix3d[8];
	return _r_pt;
}

void _getLinePositions(SVzNL3DPoint& seed1, SVzNL3DPoint& seed2, SVzNLRect& roi2D, SVzNLRangeD& distRange, std::vector<SVzNL2DPoint>& pts)
{
	double len = sqrt(pow(seed2.x - seed1.x, 2) + pow(seed2.y - seed1.y, 2));
	int x0 = (int)seed1.x - roi2D.left;
	int y0 = (int)seed1.y - roi2D.top;

	int x1 = (int)seed2.x - roi2D.left;
	int y1 = (int)seed2.y - roi2D.top;

	std::vector<SVzNL2DPoint> pts_all;
	drawLine(x0, y0, x1, y1, pts_all);

	for (int i = 0; i < (int)pts_all.size(); i++)
	{
		double data = double((pts_all[i].x - x0) * (pts_all[i].x - x0) + (pts_all[i].y - y0) * (pts_all[i].y - y0));
		double dist = sqrt(data);
		double dist_k = dist / len;
		if ((dist_k >= distRange.min) && (dist_k <= distRange.max))
			pts.push_back(pts_all[i]);
	}
}

void _getLinePoints(
	std::vector<std::vector<double>>& quantiValue,
	SVzNL3DPoint& seed1, SVzNL3DPoint& seed2,
	SVzNLRect& roi2D, SVzNLRangeD& distRange,
	std::vector<cv::Point3d>& Points3ds)
{
	std::vector<SVzNL2DPoint> pts_2d;
	_getLinePositions(seed1, seed2, roi2D, distRange, pts_2d);
	for (int i = 0; i < (int)pts_2d.size(); i++)
	{
		cv::Point3d a_pt3d;
		a_pt3d.x = (double)(pts_2d[i].x + roi2D.left) + 0.5;
		a_pt3d.y = (double)(pts_2d[i].y + roi2D.top) + 0.5;
		a_pt3d.z = quantiValue[pts_2d[i].x][pts_2d[i].y];
		if (a_pt3d.z > 1e-4)
			Points3ds.push_back(a_pt3d);
	}
}

void _getCirclePoints(
	std::vector<std::vector<double>>& quantiValue,
	SVzNL3DPoint& seed, 
	SVzNLRect& roi2D, SVzNLRangeD& distRange,
	std::vector<cv::Point3d>& Points3ds)
{
	int w = (int)quantiValue.size();
	int h = (int)quantiValue[0].size();
	int width = (int)(distRange.max + 0.5);
	int x0 = (int)seed.x - roi2D.left;
	int y0 = (int)seed.y - roi2D.top;
	for (int x = -width; x <= width; x++)
	{
		for (int y = -width; y <= width; y++)
		{
			int x1 = x0 + x;
			int y1 = y0 + y;
			if ((x1 < 0) || (x1 >= w) || (y1 < 0) || (y1 >= h))
				continue;

			double dist = sqrt(x * x + y * y);
			if ((dist >= distRange.min) && (dist <= distRange.max))
			{
				cv::Point3d a_pt3d;
				a_pt3d.x = (double)(x + x0 + roi2D.left) + 0.5;
				a_pt3d.y = (double)(y + y0 + roi2D.top) + 0.5;
				a_pt3d.z = quantiValue[x + x0][y + y0];
				if (a_pt3d.z > 1e-4)
					Points3ds.push_back(a_pt3d);
			}
		}
	}
}

bool _validateLineZChange(
	std::vector<std::vector<double>>& quantiValue,
	SVzNL3DPoint& seed1, SVzNL3DPoint& seed2,
	SVzNLRect& roi2D, SVzNLRangeD& distRange,
	double maxZChange
	)
{
	std::vector<cv::Point3d> Points3ds;
	_getLinePoints(
		quantiValue,
		seed1, seed2,
		roi2D, distRange,
		Points3ds);

	double refZ = (seed1.z + seed2.z) / 2;
	for (int i = 0; i < (int)Points3ds.size(); i++)
	{
		double zDiff = abs(Points3ds[i].z - refZ);
		if (zDiff > maxZChange)
			return false;
	}
	return true;
}

//搜索最接近distance的目标
int distanceSearchObject(
	SVzNL3DPoint seed, 
	std::vector<SWD_HoleInfo>& holes, 
	double distance, double distDeviation, double dirAngle, double deltaZ, 
	std::vector<std::vector<double>>& quantiValue, 
	SVzNLRect& roi2D, 
	SVzNLRangeD& distRange)
{
	const double angleDiffTh = 15; 
	int result = -1;
	int holeSize = (int)holes.size();
	double minDistDiff = DBL_MAX;
	int minDistIndex = -1;
	for (int i = 0; i < holeSize; i++)
	{
		if (holes[i].radius < 0)
			continue;

		double dist = sqrt(pow(seed.x - holes[i].center.x, 2) + pow(seed.y - holes[i].center.y, 2));
		double distDiff = abs(dist - distance);
		double zDiff = abs(seed.z - holes[i].center.z);
		double angle = atan2(seed.y - holes[i].center.y, seed.x - holes[i].center.x);
		angle = angle * 180.0 / PI;
		double angleDiff = computeAngleDiff(angle, dirAngle);
		if (angleDiff > 90)
			angleDiff = 180 - angleDiff;

		if ((zDiff < deltaZ) && (angleDiff < angleDiffTh))
		{
			if (minDistDiff > distDiff)
			{
				//添加距离检测
				bool isValid = _validateLineZChange(
					quantiValue,
					seed, holes[i].center,
					roi2D, distRange,
					deltaZ
				);
				if (true == isValid)
				{
					minDistDiff = distDiff;
					minDistIndex = i;
				}
			}
		}
	}
	if ((minDistIndex >= 0) && (minDistDiff < distDeviation))
		result = minDistIndex;
	return result;
}

//搜索最接近distance的目标
void searchObjectInRange(
	SVzNL3DPoint seed,
	std::vector<SWD_HoleInfo>& holes,
	SVzNLRangeD& distRange, const double deltaZ,
	std::vector<int>& searchResults)
{
	int result = -1;
	int holeSize = (int)holes.size();
	for (int i = 0; i < holeSize; i++)
	{
		if (holes[i].radius < 0)
			continue;

		if ((holes[i].center.x > 320) && (holes[i].center.x < 380) && (holes[i].center.y > -57) && (holes[i].center.y < 24))
			int kkk = 1;
		double dist = sqrt(pow(seed.x - holes[i].center.x, 2) + pow(seed.y - holes[i].center.y, 2));
		if ((dist < distRange.min) || (dist > distRange.max))
			continue;

		double zDiff = abs(seed.z - holes[i].center.z);
		if (zDiff < deltaZ)
			searchResults.push_back(i);
	}
	return;
}

//搜索最接近distance且角度为angle的目标, 以角度为优先
int angleConditionDistanceSearch(
	SVzNL3DPoint seed, SVzNL3DPoint angleSide,
	std::vector<SWD_HoleInfo>& holes,
	double distance, double distDeviation, double deltaZ, 
	SVzNLRangeD angleRange,
	std::vector<std::vector<double>>& quantiValue,
	SVzNLRect& roi2D,
	SVzNLRangeD& distRange)
{
	int result = -1;
	int holeSize = (int)holes.size();
	std::vector< int> distValidHoleIndex;
	for (int i = 0; i < holeSize; i++)
	{
		if (holes[i].radius < 0)
			continue;

		double dist = sqrt(pow(seed.x - holes[i].center.x, 2) + pow(seed.y - holes[i].center.y, 2));
		double distDiff = abs(dist - distance);
		double zDiff = abs(seed.z - holes[i].center.z);
		if ( (distDiff < distDeviation) && (zDiff < deltaZ))
		{
			distValidHoleIndex.push_back(i);
		}
	}
	if (distValidHoleIndex.size() == 1)
	{
		int idx = distValidHoleIndex[0];
		double angle = computeXOYVertexAngle(seed, angleSide, holes[idx].center);
		if( (angle >= angleRange.min) &&(angle <= angleRange.max))
			result = idx;
	}
	else if (distValidHoleIndex.size() > 1)
	{
		double bestAngle = (angleRange.min + angleRange.max) / 2;
		double minAngleDeviateion = DBL_MAX;
		int minAngleIdx = -1;
		for (int i = 0, i_max = (int)distValidHoleIndex.size(); i < i_max; i++)
		{
			int idx = distValidHoleIndex[i];
			double angle = computeXOYVertexAngle(seed, angleSide, holes[idx].center);
			if ((angle >= angleRange.min) && (angle <= angleRange.max))
			{
				double angleDiff = abs(angle - bestAngle);
				if (minAngleDeviateion > angleDiff)
				{
					//添加距离检测
					bool isValid = _validateLineZChange(
						quantiValue,
						seed, holes[idx].center,
						roi2D, distRange,
						deltaZ
					);
					if (true == isValid)
					{
						minAngleDeviateion = angleDiff;
						minAngleIdx = idx;
					}
				}
			}
		}
		result = minAngleIdx;
	}
	return result;
}

double _getMeanZ(std::vector<std::vector<double>>& quantiValue, SVzNL3DPoint seed, SVzNLRect& roi2D, double rectR, double zRange)
{
	int cols = (int)quantiValue.size();
	int rows = (int)quantiValue[0].size();

	int px = (int)seed.x - roi2D.left;
	int py = (int)seed.y - roi2D.top;
	int win = (int)rectR;
	int hist = 0;
	double zSum = 0;
	double refZ = seed.z;
	for (int i = -win; i <= win; i++)
	{
		for (int j = -win; j <= win; j++)
		{
			int qx = px + i;
			int qy = py + j;
			if ((qx >= 0) && (qx < cols) && (qy >= 0) && (qy < rows))
			{
				if (quantiValue[qx][qy] > 1e-4)
				{
					double zDiff = abs(quantiValue[qx][qy] - refZ);
					if (zDiff < zRange)
					{
						zSum += quantiValue[qx][qy];
						hist++;
					}
				}
			}
		}
	}
	if (hist == 0)
		return 0;
	else
		return (zSum / hist);
}

void _updateRoi3D(SVzNL3DRangeD& roi, SVzNL3DPoint& a_pt)
{
	if (a_pt.z > 1E-4)
	{
		if (roi.zRange.max < 0)
		{
			roi.xRange.min = a_pt.x;
			roi.xRange.max = a_pt.x;
			roi.yRange.min = a_pt.y;
			roi.yRange.max = a_pt.y;
			roi.zRange.min = a_pt.z;
			roi.zRange.max = a_pt.z;
		}
		else
		{
			if (roi.xRange.min > a_pt.x)
				roi.xRange.min = a_pt.x;
			if (roi.xRange.max < a_pt.x)
				roi.xRange.max = a_pt.x;
			if (roi.yRange.min > a_pt.y)
				roi.yRange.min = a_pt.y;
			if (roi.yRange.max < a_pt.y)
				roi.yRange.max = a_pt.y;
			if (roi.zRange.min > a_pt.z)
				roi.zRange.min = a_pt.z;
			if (roi.zRange.max < a_pt.z)
				roi.zRange.max = a_pt.z;
		}
	}
	return;
}

bool _compareByXValue(WD_workpieceInfo& a, WD_workpieceInfo& b)
{
	return a.center.x < b.center.x;
}
void _getYTopLine(
	std::vector< WD_workpieceInfo>& workpieceSrc,
	std::vector< WD_workpieceInfo>& firstLine,
	std::vector< WD_workpieceInfo>& restWorkpiece,
	double yLayerTh)
{
	//搜索Y最小值
	if (workpieceSrc.size() == 0)
		return;

	double minY = workpieceSrc[0].center.y;
	for (int i = 1; i < (int)workpieceSrc.size(); i++)
	{
		if (minY > workpieceSrc[i].center.y)
			minY = workpieceSrc[i].center.y;
	}
	double topLayerTh = minY + yLayerTh;
	for (int i = 0; i < (int)workpieceSrc.size(); i++)
	{
		if (workpieceSrc[i].center.y < topLayerTh)
			firstLine.push_back(workpieceSrc[i]);
		else
			restWorkpiece.push_back(workpieceSrc[i]);
	}
	std::sort(firstLine.begin(), firstLine.end(), _compareByXValue);
	return;
}

SSG_ROIRectD _getClusterROI(std::vector< SVzNL3DPosition>& listData)
{
	if (listData.size() == 0)
		return { 0,0,0,0 };
	SSG_ROIRectD roi = { listData[0].pt3D.x, listData[0].pt3D.x, listData[0].pt3D.y, listData[0].pt3D.y };
	for (int i = 0; i < (int)listData.size(); i++)
	{
		roi.left = roi.left > listData[i].pt3D.x ? listData[i].pt3D.x : roi.left;
		roi.right = roi.right < listData[i].pt3D.x ? listData[i].pt3D.x : roi.right;
		roi.top = roi.top > listData[i].pt3D.y ? listData[i].pt3D.y : roi.top;
		roi.bottom = roi.bottom < listData[i].pt3D.y ? listData[i].pt3D.y : roi.bottom;
	}
	return roi;
}

double _getMinZ(std::vector<cv::Point3d>& Points3ds)
{
	double minZ = -1;
	for (int i = 0; i < (int)Points3ds.size(); i++)
	{
		if ((minZ < 0) || (minZ > Points3ds[i].z))
			minZ = Points3ds[i].z;
	}
	return minZ;
}

//去除扫描线上的凹坑
void _rmoveScanlineConcaves(
	std::vector<SVzNL3DPosition>& lineData,
	const double steppingScale,
	const double invalidScale, //超出此尺度，方向角计算无效
	const double minCornerTh, //凹坑对应的最小拐角
	const double minConcaveDepth //最小凹坑不深度
)
{
	//计算方向角
	std::vector< SSG_pntDirAngle> ptDirAngles;
	wd_computeDirAngle_wholeLine2(
		lineData,
		steppingScale,
		invalidScale, //超出此尺度，方向角计算无效
		ptDirAngles
	);

	//寻找拐点极大值
	std::vector< SSG_pntDirAngle> cornerPeakP;
	std::vector< SSG_pntDirAngle> cornerPeakM;
	wd_searchCornerPeaks(
		ptDirAngles,
		lineData,
		minCornerTh,
		steppingScale * 2,
		cornerPeakP,
		cornerPeakM
	);

	//去除
	const double removeDepthK = 0.75;
	for (int idx = 0; idx < (int)cornerPeakP.size(); idx++)
	{
		SSG_pntDirAngle& a_peak = cornerPeakP[idx];
		if ((abs(a_peak.forwardDiffZ) > minConcaveDepth) && (abs(a_peak.backwardDiffZ) > minConcaveDepth))
		{
			//寻找前后截止点：diffZ的 3/4
			double zTh = a_peak.curr_z - abs(a_peak.backwardDiffZ) * removeDepthK;
			for (int i = a_peak.pntIdx; i >= a_peak.backwardPntIdx; i--)
			{
				if (lineData[i].pt3D.z > zTh)
					lineData[i].pt3D.z = 0;
				else if ((lineData[i].pt3D.z > 1e-4) && (lineData[i].pt3D.z < zTh))
					break;
			}
			zTh = a_peak.curr_z - abs(a_peak.forwardDiffZ) * removeDepthK;
			for (int i = a_peak.pntIdx; i <= a_peak.forwardPntIdx; i++)
			{
				if (lineData[i].pt3D.z > zTh)
					lineData[i].pt3D.z = 0;
				else if ((lineData[i].pt3D.z > 1e-4) && (lineData[i].pt3D.z < zTh))
					break;
			}
		}
	}
}

//工件1: 孔定位-拓普发工件孔定位
void wd_workpieceHolePositioning(
	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)
{
	*errCode = 0;

	int lineNum = (int)scanLinesInput.size();
	std::vector< std::vector<SVzNL3DPosition>> scanLines;
	scanLines.resize(lineNum);
	int linePtNum = (int)scanLinesInput[0].size();
	bool isGridData = true;
	for (int i = 0; i < lineNum; i++)
	{
		if (linePtNum != (int)scanLinesInput[i].size())
			isGridData = false;

		scanLines[i].resize(scanLinesInput[i].size());
		std::copy(scanLinesInput[i].begin(), scanLinesInput[i].end(), scanLines[i].begin()); // 使用std::copy算法

		for (int j = 0; j < (int)scanLinesInput[i].size(); j++)
			scanLinesInput[i][j].nPointIdx = 0;  //清零，用于debug时记录信息
	}
	if (false == isGridData)//数据不是网格格式
	{
		*errCode = SG_ERR_NOT_GRID_FORMAT;
		return;
	}
	for (int i = 0; i < lineNum; i++)
	{																					 //行处理
		//调平，去除地面
		wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1);
	}
	//去除凹坑:只有垂直方向上进行。只对小孔有效（小孔在扫描时会形成凹坑
	double steppingScale = workpiecePara.holeDiameter;
	double invalidScale = workpiecePara.H; //超出此尺度，方向角计算无效
	double minCornerTh = 60; //凹坑对应的最小拐角
	double minConcaveDepth = 5.0;
	for (int line = 0; line < lineNum; line++)
	{
		if (line == 436)
			int kkk = 1;
		//remove concave
		_rmoveScanlineConcaves(scanLines[line], steppingScale, invalidScale, minCornerTh, minConcaveDepth);
		//_rmoveScanlineConcaves(scanLinesInput[line], steppingScale, invalidScale, minCornerTh, minConcaveDepth);
	}

	//生成量化数据，以1mm为量化尺度，用于确定工件表面高度
	SVzNL3DRangeD roi3D = sg_getScanDataROI_vector(scanLines);
	SVzNLRect roi2D;
	roi2D.left = (int)roi3D.xRange.min;
	roi2D.right = (int)roi3D.xRange.max;
	roi2D.top = (int)roi3D.yRange.min;
	roi2D.bottom = (int)roi3D.yRange.max;
	int quanti_X = roi2D.right - roi2D.left + 1;
	int quanti_Y = roi2D.bottom - roi2D.top + 1;
	std::vector<std::vector<double>> quantiValue;
	std::vector<std::vector<int>> quantiHist;
	quantiValue.resize(quanti_X);
	quantiHist.resize(quanti_X);
	for (int i = 0; i < quanti_X; i++)
	{
		quantiValue[i].resize(quanti_Y);
		std::fill(quantiValue[i].begin(), quantiValue[i].end(), 0);//初始化为0
		quantiHist[i].resize(quanti_Y);
		std::fill(quantiHist[i].begin(), quantiHist[i].end(), 0);//初始化为0
	}
	//以1mm尺度量化
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			SVzNL3DPoint& a_pt = scanLines[line][j].pt3D;
			if (a_pt.z > 1e-4)
			{
				int qx = (int)a_pt.x - roi2D.left;
				int qy = (int)a_pt.y - roi2D.top;
				quantiValue[qx][qy] += a_pt.z;
				quantiHist[qx][qy] += 1;
			}
		}
	}
	for (int ix = 0; ix < quanti_X; ix++)
	{
		for (int iy = 0; iy < quanti_Y; iy++)
		{
			if (quantiHist[ix][iy] > 0)
				quantiValue[ix][iy] = quantiValue[ix][iy] / quantiHist[ix][iy];
		}
	}

	std::vector<SWD_segFeatureTree> segTrees_v;
	std::vector<SWD_segFeatureTree> segTrees_h;
	std::vector<SSG_intPair> validObjects;
	double valieCommonNumRatio = 0.25;
	WD_getHoleInfo(scanLines, lineSegPara, filterParam, growParam, valieCommonNumRatio, segTrees_v, segTrees_h, validObjects);

	for (int i = 0; i < lineNum; i++)
	{
		for (int j = 0; j < (int)scanLines[i].size(); j++)
			scanLines[i][j].nPointIdx = 0;  //清零
	}

	//生成聚类信息， 
	std::vector<std::vector< SVzNL2DPoint>> clusters; //只记录位置
	std::vector<SVzNL3DRangeD> clustersRoi3D;
	for (int i = 0; i < (int)validObjects.size(); i++)
	{
		std::vector< SVzNL2DPoint> a_cluster;
		SVzNL3DRangeD a_roi3D = { {-1, -1}, {-1, -1}, {-1, -1 } };

		int vTreeIdx = validObjects[i].data_0;
		int hTreeIdx = validObjects[i].data_1;
		for (int m = 0; m < (int)segTrees_v[vTreeIdx].treeNodes.size(); m++)
		{
			SWD_segFeature& a_seg = segTrees_v[vTreeIdx].treeNodes[m];
			if (scanLines[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx == 0)
			{
				scanLines[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = vTreeIdx + 1; // 0x01;
				scanLinesInput[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = vTreeIdx + 1; // 0x01;
				SVzNL2DPoint a_pos = { a_seg.lineIdx , a_seg.endPtIdx };
				a_cluster.push_back(a_pos);
				_updateRoi3D(a_roi3D, scanLines[a_seg.lineIdx][a_seg.endPtIdx].pt3D);
			}
			if (scanLines[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx == 0)
			{
				scanLines[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = vTreeIdx + 1; // 0x01;
				scanLinesInput[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = vTreeIdx + 1; // 0x01;
				SVzNL2DPoint a_pos = { a_seg.lineIdx , a_seg.startPtIdx };
				a_cluster.push_back(a_pos);
				_updateRoi3D(a_roi3D, scanLines[a_seg.lineIdx][a_seg.startPtIdx].pt3D);
			}
		}
		for (int m = 0; m < (int)segTrees_h[hTreeIdx].treeNodes.size(); m++)
		{
			SWD_segFeature& a_seg = segTrees_h[hTreeIdx].treeNodes[m];
			if (scanLines[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx == 0)
			{
				scanLines[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02;
				scanLinesInput[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02;
				SVzNL2DPoint a_pos = { a_seg.startPtIdx , a_seg.lineIdx };
				a_cluster.push_back(a_pos);
				_updateRoi3D(a_roi3D, scanLines[a_seg.startPtIdx][a_seg.lineIdx].pt3D);
			}
			if (scanLines[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx == 0)
			{
				scanLines[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02;
				scanLinesInput[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02;
				SVzNL2DPoint a_pos = { a_seg.endPtIdx , a_seg.lineIdx };
				a_cluster.push_back(a_pos);
				_updateRoi3D(a_roi3D, scanLines[a_seg.endPtIdx][a_seg.lineIdx].pt3D);
			}
		}
		clusters.push_back(a_cluster);
		clustersRoi3D.push_back(a_roi3D);
	}

	//聚类结果分析
	std::vector<int> validCluserIndexing;
	int clusterSize = (int)clusters.size();
	for (int i = 0; i < clusterSize; i++)
	{
		SVzNL3DRangeD& a_roi = clustersRoi3D[i];
		double L = a_roi.xRange.max - a_roi.xRange.min;
		double W = a_roi.yRange.max - a_roi.yRange.min;
		if ((L > workpiecePara.holeDiameter * 0.5) && (L < workpiecePara.holeDiameter * 2.5) &&
			(W > workpiecePara.holeDiameter * 0.5) && (W < workpiecePara.holeDiameter * 2.5))
			validCluserIndexing.push_back(i);
	}
	//生成结果
	std::vector< SWD_HoleInfo> holes;
	int objectSize = (int)validCluserIndexing.size();
	for (int objIdx = 0; objIdx < objectSize; objIdx++)
	{
		std::vector<SVzNL3DPoint> pointArray;
		int clusterIdx = validCluserIndexing[objIdx];
		//取cluster上的点
		int clusterPtSize = (int)clusters[clusterIdx].size();
		double minZ = DBL_MAX;
		for (int i = 0; i < clusterPtSize; i++)
		{
			SVzNL2DPoint a_pos = clusters[clusterIdx][i];
			int lineIdx = a_pos.x;
			int ptIdx = a_pos.y;
			SVzNL3DPoint a_pt3d = scanLines[lineIdx][ptIdx].pt3D;
			if (minZ > a_pt3d.z)
				minZ = a_pt3d.z;
			pointArray.push_back(a_pt3d);
		}
		//圆拟合
		SVzNL3DPoint center;
		double radius;
		double err = fitCircleByLeastSquare(pointArray, center, radius);
		center.z = minZ;
		SWD_HoleInfo a_hole;
		a_hole.center = { center.x, center.y, center.z };
		a_hole.radius = radius;
		holes.push_back(a_hole);
	}
	//分割
	//方法：先搜索与W最接近的点，然后条件搜索（垂直）与L最接近的点
	double distDeviation = 5.0; //距离搜索的合格门限。小于此距离，认为搜索到的目标为有效
	SVzNLRangeD linePointRange = { 0.3,  0.7 };

	std::vector< WD_workpieceInfo> allWorkpiece;
	double highest_z = -1;
	for (int objIdx = 0; objIdx < objectSize; objIdx++)
	{
		if (objIdx == 503)
			int kkk = 1;

		if (holes[objIdx].radius < 0)
			continue;

		//holes[objIdx].radius = -1;
		SWD_HoleInfo& p0 = holes[objIdx];
		int idx1 = distanceSearchObject(p0.center, holes, workpiecePara.holeDist_W, distDeviation, 0, workpiecePara.H / 2,
			quantiValue, roi2D, linePointRange);
		if (idx1 < 0)
			continue;

		SVzNLRangeD angleRange = { 80, 100 }; //垂直，5度范围
		SWD_HoleInfo& p1 = holes[idx1];
		//搜索最接近distance且角度为angle的目标, 以角度为优先
		int idx2 = angleConditionDistanceSearch(
			p0.center, p1.center,
			holes,
			workpiecePara.holeDist_L, distDeviation, workpiecePara.H / 2,
			angleRange, 
			quantiValue, roi2D, linePointRange);
		if (idx2 < 0)
			continue;

		SWD_HoleInfo& p2 = holes[idx2];
		//搜索最接近distance且角度为angle的目标, 以角度为优先
		int idx3 = angleConditionDistanceSearch(
			p1.center, p0.center,
			holes,
			workpiecePara.holeDist_L, distDeviation, workpiecePara.H / 2,
			angleRange, 
			quantiValue, roi2D, linePointRange);
		if (idx3 < 0)
			continue;

		SWD_HoleInfo& p3 = holes[idx3];

		//交叉检查P0P1, P2P3的长度差
		double dist_P0P1 = sqrt(pow(p0.center.x - p1.center.x, 2) + pow(p0.center.y - p1.center.y, 2));
		double dist_P2P3 = sqrt(pow(p2.center.x - p3.center.x, 2) + pow(p2.center.y - p3.center.y, 2));
		double dist_k = dist_P2P3 / dist_P0P1;
		if (dist_k > 2.0)
			continue;

		p0.radius = -1;
		p1.radius = -1;
		p2.radius = -1;
		p3.radius = -1;

		//重新计算Z值。因为沉孔的原因，Z值会不准确。取四条边的中点处的Z值的均值作为整个的Z值
		SVzNL3DPoint center_p0p1 = { (p0.center.x + p1.center.x) / 2,(p0.center.y + p1.center.y) / 2, (p0.center.z + p1.center.z) / 2 };
		SVzNL3DPoint center_p0p2 = { (p0.center.x + p2.center.x) / 2,(p0.center.y + p2.center.y) / 2, (p0.center.z + p2.center.z) / 2 };
		SVzNL3DPoint center_p1p3 = { (p1.center.x + p3.center.x) / 2,(p1.center.y + p3.center.y) / 2, (p1.center.z + p3.center.z) / 2 };
		SVzNL3DPoint center_p2p3 = { (p2.center.x + p3.center.x) / 2,(p2.center.y + p3.center.y) / 2, (p2.center.z + p3.center.z) / 2 };
		double rectR = 5.0;
		double z1 = _getMeanZ(quantiValue, center_p0p1, roi2D, rectR, workpiecePara.H / 2);
		if (z1 < 1e-4)
			z1 = center_p0p1.z;
		double z2 = _getMeanZ(quantiValue, center_p0p2, roi2D, rectR, workpiecePara.H / 2);
		if (z2 < 1e-4)
			z2 = center_p0p2.z;
		double z3 = _getMeanZ(quantiValue, center_p1p3, roi2D, rectR, workpiecePara.H / 2);
		if (z3 < 1e-4)
			z3 = center_p1p3.z;
		double z4 = _getMeanZ(quantiValue, center_p2p3, roi2D, rectR, workpiecePara.H / 2);
		if (z4 < 1e-4)
			z4 = center_p2p3.z;

		p0.center.z = (z1 + z2) / 2;
		p1.center.z = (z1 + z3) / 2;
		p2.center.z = (z2 + z4) / 2;
		p3.center.z = (z3 + z4) / 2;

		SVzNL3DPoint centerPoint = { (p0.center.x + p1.center.x + p2.center.x + p3.center.x) / 4,
								(p0.center.y + p1.center.y + p2.center.y + p3.center.y) / 4,
								(z1 + z2 + z3 + z4) / 4 };
		//取工件表面的点
		std::vector<cv::Point3d> Points3ds;
		_getLinePoints(quantiValue, p0.center, p1.center, roi2D, linePointRange, Points3ds);
		int size_1 = (int)Points3ds.size();
		_getLinePoints(quantiValue, p0.center, p2.center, roi2D, linePointRange, Points3ds);
		_getLinePoints(quantiValue, p1.center, p3.center, roi2D, linePointRange, Points3ds);
		_getLinePoints(quantiValue, p2.center, p3.center, roi2D, linePointRange, Points3ds);

		double minZ = _getMinZ(Points3ds);
		if (minZ > 1e-4)
		{
			if ((highest_z < 0) || (highest_z > minZ))
				highest_z = minZ;
		}

		//计算工件法向
		SVzNL3DPoint normalDir;
		SVzNL3DPoint vec_norm;
		SVzNL3DPoint y_dir;
		if ((int)(Points3ds.size() > size_1) && (size_1 > 0))
		{
			//计算面参数: z = Ax + By + C
			//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
			std::vector<double> res;
			vzCaculateLaserPlane(Points3ds, res);
			SVzNL3DPoint vec_1 = { -res[0], -res[1], 1.0 };
			vec_norm = vec3_normalize(vec_1);
			vec_norm = vec3_multiply(vec_norm, 20.0);
			normalDir = { centerPoint.x + vec_norm.x, centerPoint.y + vec_norm.y, centerPoint.z + vec_norm.z };

			SVzNL3DPoint vector2 = { 0, 0, 1 };
			SSG_planeCalibPara adjustPara = wd_computeRTMatrix(vec_1, vector2);
			SVzNL3DPoint rotate_p0 = _ptRotate(p0.center, adjustPara.planeCalib);
			SVzNL3DPoint rotate_p1 = _ptRotate(p1.center, adjustPara.planeCalib);
			if (rotate_p0.x < rotate_p1.x)
				y_dir = { rotate_p1.x - rotate_p0.x, rotate_p1.y - rotate_p0.y, 0 };
			else
				y_dir = { rotate_p0.x - rotate_p1.x, rotate_p0.y - rotate_p1.y, 0 };
			y_dir = _ptRotate(y_dir, adjustPara.invRMatrix);
		}
		else
		{
			normalDir = { centerPoint.x, centerPoint.y, centerPoint.z + 20 };
			vec_norm = { 0, 0, 20 };
			if (p0.center.x < p1.center.x)
				y_dir = { p1.center.x - p0.center.x, p1.center.y - p0.center.y, 0 };
			else
				y_dir = { p0.center.x - p1.center.x, p0.center.y - p1.center.y, 0 };
		}

		WD_workpieceInfo a_workpiece;
		a_workpiece.workpieceType = workpiecePara.workpieceType;
		a_workpiece.holes.push_back(p0.center);
		a_workpiece.holes.push_back(p1.center);
		a_workpiece.holes.push_back(p2.center);
		a_workpiece.holes.push_back(p3.center);

		for (int m = 0; m < 4; m++)
		{
			SVzNL3DPoint  a_pt = a_workpiece.holes[m];
			a_pt = { a_pt.x + vec_norm.x, a_pt.y + vec_norm.y, a_pt.z + vec_norm.z };//法向
			a_workpiece.holesDir.push_back(a_pt);
		}
		a_workpiece.center = centerPoint;

		y_dir = vec3_normalize(y_dir);
		a_workpiece.y_dir = { y_dir.x * 20 + a_workpiece.center.x, y_dir.y * 20 + a_workpiece.center.y, a_workpiece.center.z  + y_dir.z * 20 };
		a_workpiece.z_dir = normalDir;
		allWorkpiece.push_back(a_workpiece);
	}

	int workpieceNum = (int)allWorkpiece.size();
	if (workpieceNum == 0)
	{
		*errCode = SX_ERR_ZERO_OBJECTS;
		return;
	}

	//寻找最高点
	if (highest_z < 0)
	{
		highest_z = allWorkpiece[0].center.z;
		for (int i = 1; i < workpieceNum; i++)
		{
			if (highest_z > allWorkpiece[i].center.z)
				highest_z = allWorkpiece[i].center.z;
		}
	}

	//检测上层是否有残留
	double zSliceTh = highest_z - workpiecePara.H/2;  //往上工件H的一半
	std::vector<SVzNL3DPosition> topLayerPts;
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			if ((scanLines[line][j].pt3D.z > 1e-4) && (scanLines[line][j].pt3D.z < zSliceTh))
			{
				SVzNL3DPosition a_pt; 
				a_pt.nPointIdx = (line << 16) | (j & 0xffff);
				a_pt.pt3D = scanLines[line][j].pt3D;
				topLayerPts.push_back(a_pt);
			}
		}
	}
	//进行聚类，判断聚类大小。大于一定门限判断为有异物
	int topLayerClusterWin = 5;
	double clusterDist = 5.0;
	int distType = 1; //0 - 2d distance; 1- 3d distance 
	std::vector<std::vector< SVzNL3DPosition>> objClusters;
	wd_pointClustering_speedUp(
		topLayerPts,
		lineNum, linePtNum, topLayerClusterWin, //搜索窗口
		clusterDist,
		distType, //0 - 2d distance; 1- 3d distance 
		objClusters  //result
	);
	double vaidObjSize = workpiecePara.xLen * workpiecePara.yLen / 4.0;
	for (int i = 0; i < (int)objClusters.size(); i++)
	{
		SSG_ROIRectD a_roi = _getClusterROI(objClusters[i]);
		double size = (a_roi.right - a_roi.left) * (a_roi.bottom - a_roi.top);
		if (size >= vaidObjSize)
		{
			*errCode = SX_ERR_UNKNOWN_OBJECT;
			return;
		}
	}

	//排序
	//z方向排序 
	std::vector< WD_workpieceInfo> zSortWorkpiece;
	double topLayerTh = highest_z + workpiecePara.H / 2;
	for (int i = 0; i < workpieceNum; i++)
	{
		if (allWorkpiece[i].center.z < topLayerTh)
			zSortWorkpiece.push_back(allWorkpiece[i]);
	}
	//水平方向排序
	while (zSortWorkpiece.size() > 0)
	{
		std::vector< WD_workpieceInfo> firstLine;
		std::vector< WD_workpieceInfo> restWorkpiece;
		_getYTopLine(zSortWorkpiece, firstLine, restWorkpiece, 	workpiecePara.yLen / 2);
		workpiecePositioning.insert(workpiecePositioning.end(), firstLine.begin(), firstLine.end());
		zSortWorkpiece.clear();
		zSortWorkpiece.insert(zSortWorkpiece.end(), restWorkpiece.begin(), restWorkpiece.end());
	}

	//旋转回去
	workpieceNum = (int)workpiecePositioning.size();
	for (int i = 0; i < workpieceNum; i++)
	{
		SVzNL3DPoint rpt;
		rpt = _ptRotate(workpiecePositioning[i].center, groundCalibPara.invRMatrix);
		workpiecePositioning[i].center = rpt;
		rpt = _ptRotate(workpiecePositioning[i].y_dir, groundCalibPara.invRMatrix);
		workpiecePositioning[i].y_dir = rpt;
		rpt = _ptRotate(workpiecePositioning[i].z_dir, groundCalibPara.invRMatrix);
		workpiecePositioning[i].z_dir = rpt;
		for (int j = 0, j_max = (int)workpiecePositioning[i].holes.size(); j < j_max; j++)
		{
			rpt = _ptRotate(workpiecePositioning[i].holes[j], groundCalibPara.invRMatrix);
			workpiecePositioning[i].holes[j] = rpt;
			rpt = _ptRotate(workpiecePositioning[i].holesDir[j], groundCalibPara.invRMatrix);
			workpiecePositioning[i].holesDir[j] = rpt;
		}

		SVzNL3DPoint vector_z = { workpiecePositioning[i].z_dir.x - workpiecePositioning[i].center.x,
									workpiecePositioning[i].z_dir.y - workpiecePositioning[i].center.y,
									workpiecePositioning[i].z_dir.z - workpiecePositioning[i].center.z };
		SVzNL3DPoint vector_y = { workpiecePositioning[i].y_dir.x - workpiecePositioning[i].center.x,
									workpiecePositioning[i].y_dir.y - workpiecePositioning[i].center.y,
									workpiecePositioning[i].y_dir.z - workpiecePositioning[i].center.z };
		double mod_vz = sqrt(pow(vector_z.x, 2) + pow(vector_z.y, 2) + pow(vector_z.z, 2));
		vector_z = { vector_z.x / mod_vz, vector_z.y / mod_vz, vector_z.z / mod_vz }; //归一化
		double mod_vy = sqrt(pow(vector_y.x, 2) + pow(vector_y.y, 2) + pow(vector_y.z, 2));
		vector_y = { vector_y.x / mod_vy, vector_y.y / mod_vy, vector_y.z / mod_vy }; //归一化
		//叉乘出vector_x
		SVzNL3DPoint vector_x;
		vector_x.x = vector_y.y * vector_z.z - vector_z.y * vector_y.z;
		vector_x.y = vector_y.z * vector_z.x - vector_z.z * vector_y.x;
		vector_x.z = vector_y.x * vector_z.y - vector_z.x * vector_y.y;
		workpiecePositioning[i].x_dir = vector_x;
		workpiecePositioning[i].y_dir = vector_y;
		workpiecePositioning[i].z_dir = vector_z;
#if 0
		//得到旋转矩阵
		double R[3][3];
		R[0][0] = vector_x.x;
		R[1][0] = vector_x.y;
		R[2][0] = vector_x.z;

		R[0][1] = vector_y.x;
		R[1][1] = vector_y.y;
		R[2][1] = vector_y.z;

		R[0][2] = vector_z.x;
		R[1][2] = vector_z.y;
		R[2][2] = vector_z.z;
		SSG_EulerAngles eulerAngle = rotationMatrixToEulerZYX(R);
		workpiecePositioning[i].workpiecePose = eulerAngle;
#endif
	}
	return;

}

//检查工件蹭中间孔数量
void _checkWorkpieceCentralHoles(
	SWD_HoleInfo & p0, 
	SWD_HoleInfo & p1, 
	SWD_HoleInfo & p2, 
	SWD_HoleInfo & p3, 
	std::vector<std::vector<int>>&holeMask, 
	std::vector< SWD_HoleInfo>& holes,
	SVzNLRect & roi2D,
	SVzNLRangeD& distRange,
	std::vector<int>& innerHoleIndice)
{
	std::vector<cv::Point> contours;
	cv::Point contour_P0, contour_P1, contour_P2, contour_P3;
	contour_P0 = cv::Point((int)p0.center.x - roi2D.left, (int)p0.center.y - roi2D.top);
	int x_min = contour_P0.x,	x_max = contour_P0.x,	y_min = contour_P0.y,	y_max = contour_P0.y;

	contour_P1 = cv::Point((int)p1.center.x - roi2D.left, (int)p1.center.y - roi2D.top);
	x_min = x_min > contour_P1.x ? contour_P1.x : x_min;
	x_max = x_max < contour_P1.x ? contour_P1.x : x_max;
	y_min = y_min > contour_P1.y ? contour_P1.y : y_min;
	y_max = y_max < contour_P1.y ? contour_P1.y : y_max;

	contour_P2 = cv::Point((int)p2.center.x - roi2D.left, (int)p2.center.y - roi2D.top);
	x_min = x_min > contour_P2.x ? contour_P2.x : x_min;
	x_max = x_max < contour_P2.x ? contour_P2.x : x_max;
	y_min = y_min > contour_P2.y ? contour_P2.y : y_min;
	y_max = y_max < contour_P2.y ? contour_P2.y : y_max;

	contour_P3 = cv::Point((int)p3.center.x - roi2D.left, (int)p3.center.y - roi2D.top);
	x_min = x_min > contour_P3.x ? contour_P3.x : x_min;
	x_max = x_max < contour_P3.x ? contour_P3.x : x_max;
	y_min = y_min > contour_P3.y ? contour_P3.y : y_min;
	y_max = y_max < contour_P3.y ? contour_P3.y : y_max;

	contours.push_back(contour_P0);
	contours.push_back(contour_P1);
	contours.push_back(contour_P2);
	contours.push_back(contour_P3);

	//在2D的ROI内快速搜索目标
	std::vector<int> indice;
	for (int x = x_min; x < x_max; x++)
	{
		for (int y = y_min; y <= y_max; y++)
		{
			int objIdx = holeMask[x][y];
			if (objIdx >= 0)
			{
				indice.push_back(objIdx);
			}
		}
	}

	if (indice.size() == 0)
		return;

	//精确判断目标是否为内孔
	for (int i = 0; i < (int)indice.size(); i++)
	{
		int objIdx = indice[i];
		SWD_HoleInfo& centralHole = holes[objIdx];
		//if ((abs(centralHole.center.x - 187.2) < 5.0) && (abs(centralHole.center.y - 147.0) < 10.0))
		//	int kkk = 1;
		cv::Point2f centerPt = cv::Point2f((int)centralHole.center.x - roi2D.left, (int)centralHole.center.y - roi2D.top);
		int dist = cv::pointPolygonTest(contours, centerPt, false);
		if (1 == dist)
		{
			double d1 = sqrt(pow(centralHole.center.x - p0.center.x, 2) + pow(centralHole.center.y - p0.center.y, 2));
			double d2 = sqrt(pow(centralHole.center.x - p2.center.x, 2) + pow(centralHole.center.y - p2.center.y, 2));
			double k;
			if (d1 < d2)
				k = d1 / d2;
			else
				k = d2 / d1;
			//printf("k=%g\n", k);

			if( (k >= distRange.min)  && (k <= distRange.max))
				innerHoleIndice.push_back(objIdx);
		}
	}
	return;
}

//工件2: 孔定位-拓普发工件孔定位
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)
{
	*errCode = 0;

	int lineNum = (int)scanLinesInput.size();
	std::vector< std::vector<SVzNL3DPosition>> scanLines;
	scanLines.resize(lineNum);
	int linePtNum = (int)scanLinesInput[0].size();
	bool isGridData = true;
	for (int i = 0; i < lineNum; i++)
	{
		if (linePtNum != (int)scanLinesInput[i].size())
			isGridData = false;

		scanLines[i].resize(scanLinesInput[i].size());
		std::copy(scanLinesInput[i].begin(), scanLinesInput[i].end(), scanLines[i].begin()); // 使用std::copy算法

		for (int j = 0; j < (int)scanLinesInput[i].size(); j++)
			scanLinesInput[i][j].nPointIdx = 0;  //清零，用于debug时记录信息
	}
	if (false == isGridData)//数据不是网格格式
	{
		*errCode = SG_ERR_NOT_GRID_FORMAT;
		return;
	}
	for (int i = 0; i < lineNum; i++)
	{																					 //行处理
		//调平，去除地面
		wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1);
		//wd_lineDataR(scanLinesInput[i], groundCalibPara.planeCalib, -1);
	}

	//去除凹坑:只有垂直方向上进行。只对小孔有效（小孔在扫描时会形成凹坑
	double steppingScale = workpiecePara.holeDiameter;
	double invalidScale = workpiecePara.H; //超出此尺度，方向角计算无效
	double minCornerTh = 45; //凹坑对应的最小拐角
	double minConcaveDepth = 3.0;
	for (int line = 0; line < lineNum; line++)
	{
		if (line == 436)
			int kkk = 1;
		//remove concave
		_rmoveScanlineConcaves( scanLines[line], steppingScale, invalidScale, minCornerTh, minConcaveDepth);
		//_rmoveScanlineConcaves(scanLinesInput[line], steppingScale, invalidScale, minCornerTh, minConcaveDepth);
	}

	//生成量化数据，以1mm为量化尺度，用于确定工件表面高度
	SVzNL3DRangeD roi3D = sg_getScanDataROI_vector(scanLines);
	SVzNLRect roi2D;
	roi2D.left = (int)roi3D.xRange.min;
	roi2D.right = (int)roi3D.xRange.max;
	roi2D.top = (int)roi3D.yRange.min;
	roi2D.bottom = (int)roi3D.yRange.max;
	int quanti_X = roi2D.right - roi2D.left + 1;
	int quanti_Y = roi2D.bottom - roi2D.top + 1;
	std::vector<std::vector<double>> quantiValue;
	std::vector<std::vector<int>> quantiHist;
	quantiValue.resize(quanti_X);
	quantiHist.resize(quanti_X);
	for (int i = 0; i < quanti_X; i++)
	{
		quantiValue[i].resize(quanti_Y);
		std::fill(quantiValue[i].begin(), quantiValue[i].end(), 0);//初始化为0
		quantiHist[i].resize(quanti_Y);
		std::fill(quantiHist[i].begin(), quantiHist[i].end(), 0);//初始化为0
	}
	//以1mm尺度量化
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			SVzNL3DPoint& a_pt = scanLines[line][j].pt3D;
			if (a_pt.z > 1e-4)
			{
				int qx = (int)a_pt.x - roi2D.left;
				int qy = (int)a_pt.y - roi2D.top;
				quantiValue[qx][qy] += a_pt.z;
				quantiHist[qx][qy] += 1;
			}
		}
	}
	for (int ix = 0; ix < quanti_X; ix++)
	{
		for (int iy = 0; iy < quanti_Y; iy++)
		{
			if (quantiHist[ix][iy] > 0)
				quantiValue[ix][iy] = quantiValue[ix][iy] / quantiHist[ix][iy];
		}
	}

	std::vector<SWD_segFeatureTree> segTrees_v;
	std::vector<SWD_segFeatureTree> segTrees_h;
	std::vector<SSG_intPair> validObjects;
	double valieCommonNumRatio = 0.25;
	WD_getHoleInfo(scanLines, lineSegPara, filterParam, growParam, valieCommonNumRatio, segTrees_v, segTrees_h, validObjects);

	for (int i = 0; i < lineNum; i++)
	{
		for (int j = 0; j < (int)scanLines[i].size(); j++)
			scanLines[i][j].nPointIdx = 0;  //清零
	}

	//生成聚类信息， 
	std::vector<std::vector< SVzNL2DPoint>> clusters; //只记录位置
	std::vector<SVzNL3DRangeD> clustersRoi3D;
	for (int i = 0; i < (int)validObjects.size(); i++)
	{
		std::vector< SVzNL2DPoint> a_cluster;
		SVzNL3DRangeD a_roi3D = { {-1, -1}, {-1, -1}, {-1, -1 } };

		int vTreeIdx = validObjects[i].data_0;
		int hTreeIdx = validObjects[i].data_1;
		for (int m = 0; m < (int)segTrees_v[vTreeIdx].treeNodes.size(); m++)
		{
			SWD_segFeature& a_seg = segTrees_v[vTreeIdx].treeNodes[m];
			if (scanLines[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx == 0)
			{
				scanLines[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = vTreeIdx + 1; // 0x01;
				scanLinesInput[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = vTreeIdx + 1; // 0x01;
				SVzNL2DPoint a_pos = { a_seg.lineIdx , a_seg.endPtIdx };
				a_cluster.push_back(a_pos);
				_updateRoi3D(a_roi3D, scanLines[a_seg.lineIdx][a_seg.endPtIdx].pt3D);
			}
			if (scanLines[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx == 0)
			{
				scanLines[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = vTreeIdx + 1; // 0x01;
				scanLinesInput[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = vTreeIdx + 1; // 0x01;
				SVzNL2DPoint a_pos = { a_seg.lineIdx , a_seg.startPtIdx };
				a_cluster.push_back(a_pos);
				_updateRoi3D(a_roi3D, scanLines[a_seg.lineIdx][a_seg.startPtIdx].pt3D);
			}
		}
		for (int m = 0; m < (int)segTrees_h[hTreeIdx].treeNodes.size(); m++)
		{
			SWD_segFeature& a_seg = segTrees_h[hTreeIdx].treeNodes[m];
			if (scanLines[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx == 0)
			{
				scanLines[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02;
				scanLinesInput[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02;
				SVzNL2DPoint a_pos = { a_seg.startPtIdx , a_seg.lineIdx };
				a_cluster.push_back(a_pos);
				_updateRoi3D(a_roi3D, scanLines[a_seg.startPtIdx][a_seg.lineIdx].pt3D);
			}
			if (scanLines[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx == 0)
			{
				scanLines[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02;
				scanLinesInput[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02;
				SVzNL2DPoint a_pos = { a_seg.endPtIdx , a_seg.lineIdx };
				a_cluster.push_back(a_pos);
				_updateRoi3D(a_roi3D, scanLines[a_seg.endPtIdx][a_seg.lineIdx].pt3D);
			}
		}
		clusters.push_back(a_cluster);
		clustersRoi3D.push_back(a_roi3D);
	}

	//聚类结果分析
	std::vector<int> validCluserIndexing;
	int clusterSize = (int)clusters.size();
	for (int i = 0; i < clusterSize; i++)
	{
		SVzNL3DRangeD& a_roi = clustersRoi3D[i];
		double L = a_roi.xRange.max - a_roi.xRange.min;
		double W = a_roi.yRange.max - a_roi.yRange.min;
		if ((a_roi.xRange.max > 260) && (a_roi.xRange.min < 270) && (a_roi.yRange.max > 10) && (a_roi.yRange.min < 20))
			int kkk = 1;
		if ((L > workpiecePara.holeDiameter * 0.25) && (L < workpiecePara.holeDiameter * 2.5) &&
			(W > workpiecePara.holeDiameter * 0.25) && (W < workpiecePara.holeDiameter * 2.5))
			validCluserIndexing.push_back(i);
	}
	//生成结果
	std::vector< SWD_HoleInfo> holes;
	int objectSize = (int)validCluserIndexing.size();
	for (int objIdx = 0; objIdx < objectSize; objIdx++)
	{
		std::vector<SVzNL3DPoint> pointArray;
		int clusterIdx = validCluserIndexing[objIdx];
		//取cluster上的点
		int clusterPtSize = (int)clusters[clusterIdx].size();
		double minZ = DBL_MAX;
		for (int i = 0; i < clusterPtSize; i++)
		{
			SVzNL2DPoint a_pos = clusters[clusterIdx][i];
			int lineIdx = a_pos.x;
			int ptIdx = a_pos.y;
			SVzNL3DPoint a_pt3d = scanLines[lineIdx][ptIdx].pt3D;
			if (minZ > a_pt3d.z)
				minZ = a_pt3d.z;
			pointArray.push_back(a_pt3d);
		}
		//圆拟合
		SVzNL3DPoint center;
		double radius;
		double err = fitCircleByLeastSquare(pointArray, center, radius);
		center.z = minZ;
		SWD_HoleInfo a_hole;
		a_hole.center = { center.x, center.y, center.z };
		a_hole.radius = radius;
		holes.push_back(a_hole);
	}

	//生成Mask，用于搜索目标的中心是不是有孔。工件2的特点是工件中间有孔
	std::vector<std::vector<int>> holeMask;
	holeMask.resize(quantiValue.size());
	for (int i = 0; i < (int)quantiValue.size(); i++)
	{
		holeMask[i].resize(quantiValue[i].size());
		std:fill(holeMask[i].begin(), holeMask[i].end(), -1);
	}

	for (int objIdx = 0; objIdx < objectSize; objIdx++)
	{
		SWD_HoleInfo& a_hole = holes[objIdx];
		int px = (int)a_hole.center.x - roi2D.left;
		int py = (int)a_hole.center.y - roi2D.top;
		holeMask[px][py] = objIdx;
	}

	//分割
	//方法：先搜索与W最接近的点，然后条件搜索（垂直）与L最接近的点
	double distDeviation = 5.0; //距离搜索的合格门限。小于此距离，认为搜索到的目标为有效
	SVzNLRangeD linePointRange = { 0.3,  0.7 };

	std::vector< WD_workpieceInfo> allWorkpiece;
	double highest_z = -1;

	//采用迭代法。首先将可信度高的目标检出， 然后继续检测剩下的目标
	SVzNLRangeD innerHoleDistRange;
	innerHoleDistRange.min = 0.4;
	innerHoleDistRange.max = 0.8;
	//首先挑选出信度高的工件：内孔数量大于1
	for (int objIdx = 0; objIdx < objectSize; objIdx++)
	{
		if (objIdx == 503)
			int kkk = 1;

		if (holes[objIdx].radius < 0)
			continue;

		//holes[objIdx].radius = -1;
		SWD_HoleInfo& p0 = holes[objIdx];
		if ((p0.center.x > 230) && (p0.center.x < 240) && (p0.center.y > 10) && (p0.center.y < 25))
			int kkk = 1;
		if ((p0.center.x > 230) && (p0.center.x < 240) && (p0.center.y > 50) && (p0.center.y < 65))
			int kkk = 1;
		int idx1 = distanceSearchObject(p0.center, holes, workpiecePara.holeDist_W, distDeviation, 0, workpiecePara.H / 2,
			quantiValue, roi2D, linePointRange);
		if (idx1 < 0)
			continue;

		SVzNLRangeD angleRange = { 80, 100 }; //垂直，5度范围
		SWD_HoleInfo& p1 = holes[idx1];
		//搜索最接近distance且角度为angle的目标, 以角度为优先
		int idx2 = angleConditionDistanceSearch(
			p0.center, p1.center,
			holes,
			workpiecePara.holeDist_L, distDeviation, workpiecePara.H / 2,
			angleRange,
			quantiValue, roi2D, linePointRange);
		if (idx2 < 0)
			continue;

		SWD_HoleInfo& p2 = holes[idx2];
		//搜索最接近distance且角度为angle的目标, 以角度为优先
		int idx3 = angleConditionDistanceSearch(
			p1.center, p0.center,
			holes,
			workpiecePara.holeDist_L, distDeviation, workpiecePara.H / 2,
			angleRange,
			quantiValue, roi2D, linePointRange);
		if (idx3 < 0)
			continue;

		SWD_HoleInfo& p3 = holes[idx3];

		//交叉检查P0P1, P2P3的长度差
		double dist_P0P1 = sqrt(pow(p0.center.x - p1.center.x, 2) + pow(p0.center.y - p1.center.y, 2));
		double dist_P2P3 = sqrt(pow(p2.center.x - p3.center.x, 2) + pow(p2.center.y - p3.center.y, 2));
		double dist_k = dist_P2P3 / dist_P0P1;
		if (dist_k > 2.0)
			continue;

		//检查工件中间有没有孔
		std::vector<int> innerHoleIndice;
		_checkWorkpieceCentralHoles(p0, p1, p3, p2, holeMask, holes,  roi2D, innerHoleDistRange, innerHoleIndice);
		if (innerHoleIndice.size() < 2)
			continue;

		p0.radius = -1;
		p1.radius = -1;
		p2.radius = -1;
		p3.radius = -1;
		for (int m = 0; m < (int)innerHoleIndice.size(); m++)
		{
			int holeIdx = innerHoleIndice[m];
			holes[holeIdx].radius = -1;
		}

		//重新计算Z值。因为沉孔的原因，Z值会不准确。取四条边的中点处的Z值的均值作为整个的Z值
		SVzNL3DPoint center_p0p1 = { (p0.center.x + p1.center.x) / 2,(p0.center.y + p1.center.y) / 2, (p0.center.z + p1.center.z) / 2 };
		SVzNL3DPoint center_p0p2 = { (p0.center.x + p2.center.x) / 2,(p0.center.y + p2.center.y) / 2, (p0.center.z + p2.center.z) / 2 };
		SVzNL3DPoint center_p1p3 = { (p1.center.x + p3.center.x) / 2,(p1.center.y + p3.center.y) / 2, (p1.center.z + p3.center.z) / 2 };
		SVzNL3DPoint center_p2p3 = { (p2.center.x + p3.center.x) / 2,(p2.center.y + p3.center.y) / 2, (p2.center.z + p3.center.z) / 2 };
		double rectR = 5.0;
		double z1 = _getMeanZ(quantiValue, center_p0p1, roi2D, rectR, workpiecePara.H / 2);
		if (z1 < 1e-4)
			z1 = center_p0p1.z;
		double z2 = _getMeanZ(quantiValue, center_p0p2, roi2D, rectR, workpiecePara.H / 2);
		if (z2 < 1e-4)
			z2 = center_p0p2.z;
		double z3 = _getMeanZ(quantiValue, center_p1p3, roi2D, rectR, workpiecePara.H / 2);
		if (z3 < 1e-4)
			z3 = center_p1p3.z;
		double z4 = _getMeanZ(quantiValue, center_p2p3, roi2D, rectR, workpiecePara.H / 2);
		if (z4 < 1e-4)
			z4 = center_p2p3.z;

		p0.center.z = (z1 + z2) / 2;
		p1.center.z = (z1 + z3) / 2;
		p2.center.z = (z2 + z4) / 2;
		p3.center.z = (z3 + z4) / 2;

		SVzNL3DPoint centerPoint = { (p0.center.x + p1.center.x + p2.center.x + p3.center.x) / 4,
								(p0.center.y + p1.center.y + p2.center.y + p3.center.y) / 4,
								(z1 + z2 + z3 + z4) / 4 };
		//取工件表面的点
		std::vector<cv::Point3d> Points3ds;
		_getLinePoints(quantiValue, p0.center, p1.center, roi2D, linePointRange, Points3ds);
		int size_1 = (int)Points3ds.size();
		_getLinePoints(quantiValue, p0.center, p2.center, roi2D, linePointRange, Points3ds);
		_getLinePoints(quantiValue, p1.center, p3.center, roi2D, linePointRange, Points3ds);
		_getLinePoints(quantiValue, p2.center, p3.center, roi2D, linePointRange, Points3ds);

		double minZ = _getMinZ(Points3ds);
		if (minZ > 1e-4)
		{
			if ((highest_z < 0) || (highest_z > minZ))
				highest_z = minZ;
		}

		//计算工件法向
		SVzNL3DPoint normalDir;
		SVzNL3DPoint vec_norm;
		SVzNL3DPoint y_dir;
		if ((int)(Points3ds.size() > size_1) && (size_1 > 0))
		{
			//计算面参数: z = Ax + By + C
			//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
			std::vector<double> res;
			vzCaculateLaserPlane(Points3ds, res);
			SVzNL3DPoint vec_1 = { -res[0], -res[1], 1.0 };
			vec_norm = vec3_normalize(vec_1);
			vec_norm = vec3_multiply(vec_norm, 20.0);
			normalDir = { centerPoint.x + vec_norm.x, centerPoint.y + vec_norm.y, centerPoint.z + vec_norm.z };

			SVzNL3DPoint vector2 = { 0, 0, 1 };
			SSG_planeCalibPara adjustPara = wd_computeRTMatrix(vec_1, vector2);
			SVzNL3DPoint rotate_p0 = _ptRotate(p0.center, adjustPara.planeCalib);
			SVzNL3DPoint rotate_p1 = _ptRotate(p1.center, adjustPara.planeCalib);
			if (rotate_p0.x < rotate_p1.x)
				y_dir = { rotate_p1.x - rotate_p0.x, rotate_p1.y - rotate_p0.y, 0 };
			else
				y_dir = { rotate_p0.x - rotate_p1.x, rotate_p0.y - rotate_p1.y, 0 };
			y_dir = _ptRotate(y_dir, adjustPara.invRMatrix);
		}
		else
		{
			normalDir = { centerPoint.x, centerPoint.y, centerPoint.z + 20 };
			vec_norm = { 0, 0, 20 };
			if (p0.center.x < p1.center.x)
				y_dir = { p1.center.x - p0.center.x, p1.center.y - p0.center.y, 0 };
			else
				y_dir = { p0.center.x - p1.center.x, p0.center.y - p1.center.y, 0 };
		}

		WD_workpieceInfo a_workpiece;
		a_workpiece.workpieceType = workpiecePara.workpieceType;
		a_workpiece.holes.push_back(p0.center);
		a_workpiece.holes.push_back(p1.center);
		a_workpiece.holes.push_back(p2.center);
		a_workpiece.holes.push_back(p3.center);

		for (int m = 0; m < 4; m++)
		{
			SVzNL3DPoint  a_pt = a_workpiece.holes[m];
			a_pt = { a_pt.x + vec_norm.x, a_pt.y + vec_norm.y, a_pt.z + vec_norm.z };//法向
			a_workpiece.holesDir.push_back(a_pt);
		}
		a_workpiece.center = centerPoint;

		y_dir = vec3_normalize(y_dir);
		a_workpiece.y_dir = { y_dir.x * 20 + a_workpiece.center.x, y_dir.y * 20 + a_workpiece.center.y, a_workpiece.center.z + y_dir.z * 20 };
		a_workpiece.z_dir = normalDir;
		allWorkpiece.push_back(a_workpiece);
	}
	//再挑选出剩余的工件：内孔数量==1
	for (int objIdx = 0; objIdx < objectSize; objIdx++)
	{
		if (objIdx == 503)
			int kkk = 1;

		if (holes[objIdx].radius < 0)
			continue;

		//holes[objIdx].radius = -1;
		SWD_HoleInfo& p0 = holes[objIdx];

		int idx1 = distanceSearchObject(p0.center, holes, workpiecePara.holeDist_W, distDeviation, 0, workpiecePara.H / 2,
			quantiValue, roi2D, linePointRange);
		if (idx1 < 0)
			continue;

		SVzNLRangeD angleRange = { 80, 100 }; //垂直，5度范围
		SWD_HoleInfo& p1 = holes[idx1];
		//搜索最接近distance且角度为angle的目标, 以角度为优先
		int idx2 = angleConditionDistanceSearch(
			p0.center, p1.center,
			holes,
			workpiecePara.holeDist_L, distDeviation, workpiecePara.H / 2,
			angleRange,
			quantiValue, roi2D, linePointRange);
		if (idx2 < 0)
			continue;

		SWD_HoleInfo& p2 = holes[idx2];
		//搜索最接近distance且角度为angle的目标, 以角度为优先
		int idx3 = angleConditionDistanceSearch(
			p1.center, p0.center,
			holes,
			workpiecePara.holeDist_L, distDeviation, workpiecePara.H / 2,
			angleRange,
			quantiValue, roi2D, linePointRange);
		if (idx3 < 0)
			continue;

		SWD_HoleInfo& p3 = holes[idx3];

		//交叉检查P0P1, P2P3的长度差
		double dist_P0P1 = sqrt(pow(p0.center.x - p1.center.x, 2) + pow(p0.center.y - p1.center.y, 2));
		double dist_P2P3 = sqrt(pow(p2.center.x - p3.center.x, 2) + pow(p2.center.y - p3.center.y, 2));
		double dist_k = dist_P2P3 / dist_P0P1;
		if (dist_k > 2.0)
			continue;

		//检查工件中间有没有孔
		std::vector<int> innerHoleIndice;
		_checkWorkpieceCentralHoles(p0, p1, p3, p2, holeMask, holes, roi2D, innerHoleDistRange, innerHoleIndice);
		if (innerHoleIndice.size() == 0 )
			continue;

		p0.radius = -1;
		p1.radius = -1;
		p2.radius = -1;
		p3.radius = -1;
		for (int m = 0; m < (int)innerHoleIndice.size(); m++)
		{
			int holeIdx = innerHoleIndice[m];
			holes[holeIdx].radius = -1;
		}

		//重新计算Z值。因为沉孔的原因，Z值会不准确。取四条边的中点处的Z值的均值作为整个的Z值
		SVzNL3DPoint center_p0p1 = { (p0.center.x + p1.center.x) / 2,(p0.center.y + p1.center.y) / 2, (p0.center.z + p1.center.z) / 2 };
		SVzNL3DPoint center_p0p2 = { (p0.center.x + p2.center.x) / 2,(p0.center.y + p2.center.y) / 2, (p0.center.z + p2.center.z) / 2 };
		SVzNL3DPoint center_p1p3 = { (p1.center.x + p3.center.x) / 2,(p1.center.y + p3.center.y) / 2, (p1.center.z + p3.center.z) / 2 };
		SVzNL3DPoint center_p2p3 = { (p2.center.x + p3.center.x) / 2,(p2.center.y + p3.center.y) / 2, (p2.center.z + p3.center.z) / 2 };
		double rectR = 5.0;
		double z1 = _getMeanZ(quantiValue, center_p0p1, roi2D, rectR, workpiecePara.H / 2);
		if (z1 < 1e-4)
			z1 = center_p0p1.z;
		double z2 = _getMeanZ(quantiValue, center_p0p2, roi2D, rectR, workpiecePara.H / 2);
		if (z2 < 1e-4)
			z2 = center_p0p2.z;
		double z3 = _getMeanZ(quantiValue, center_p1p3, roi2D, rectR, workpiecePara.H / 2);
		if (z3 < 1e-4)
			z3 = center_p1p3.z;
		double z4 = _getMeanZ(quantiValue, center_p2p3, roi2D, rectR, workpiecePara.H / 2);
		if (z4 < 1e-4)
			z4 = center_p2p3.z;

		p0.center.z = (z1 + z2) / 2;
		p1.center.z = (z1 + z3) / 2;
		p2.center.z = (z2 + z4) / 2;
		p3.center.z = (z3 + z4) / 2;

		SVzNL3DPoint centerPoint = { (p0.center.x + p1.center.x + p2.center.x + p3.center.x) / 4,
								(p0.center.y + p1.center.y + p2.center.y + p3.center.y) / 4,
								(z1 + z2 + z3 + z4) / 4 };
		//取工件表面的点
		std::vector<cv::Point3d> Points3ds;
		_getLinePoints(quantiValue, p0.center, p1.center, roi2D, linePointRange, Points3ds);
		int size_1 = (int)Points3ds.size();
		_getLinePoints(quantiValue, p0.center, p2.center, roi2D, linePointRange, Points3ds);
		_getLinePoints(quantiValue, p1.center, p3.center, roi2D, linePointRange, Points3ds);
		_getLinePoints(quantiValue, p2.center, p3.center, roi2D, linePointRange, Points3ds);

		double minZ = _getMinZ(Points3ds);
		if (minZ > 1e-4)
		{
			if ((highest_z < 0) || (highest_z > minZ))
				highest_z = minZ;
		}

		//计算工件法向
		SVzNL3DPoint normalDir;
		SVzNL3DPoint vec_norm;
		SVzNL3DPoint y_dir;
		if ((int)(Points3ds.size() > size_1) && (size_1 > 0))
		{
			//计算面参数: z = Ax + By + C
			//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
			std::vector<double> res;
			vzCaculateLaserPlane(Points3ds, res);
			SVzNL3DPoint vec_1 = { -res[0], -res[1], 1.0 };
			vec_norm = vec3_normalize(vec_1);
			vec_norm = vec3_multiply(vec_norm, 20.0);
			normalDir = { centerPoint.x + vec_norm.x, centerPoint.y + vec_norm.y, centerPoint.z + vec_norm.z };

			SVzNL3DPoint vector2 = { 0, 0, 1 };
			SSG_planeCalibPara adjustPara = wd_computeRTMatrix(vec_1, vector2);
			SVzNL3DPoint rotate_p0 = _ptRotate(p0.center, adjustPara.planeCalib);
			SVzNL3DPoint rotate_p1 = _ptRotate(p1.center, adjustPara.planeCalib);
			if (rotate_p0.x < rotate_p1.x)
				y_dir = { rotate_p1.x - rotate_p0.x, rotate_p1.y - rotate_p0.y, 0 };
			else
				y_dir = { rotate_p0.x - rotate_p1.x, rotate_p0.y - rotate_p1.y, 0 };
			y_dir = _ptRotate(y_dir, adjustPara.invRMatrix);
		}
		else
		{
			normalDir = { centerPoint.x, centerPoint.y, centerPoint.z + 20 };
			vec_norm = { 0, 0, 20 };
			if (p0.center.x < p1.center.x)
				y_dir = { p1.center.x - p0.center.x, p1.center.y - p0.center.y, 0 };
			else
				y_dir = { p0.center.x - p1.center.x, p0.center.y - p1.center.y, 0 };
		}

		WD_workpieceInfo a_workpiece;
		a_workpiece.workpieceType = workpiecePara.workpieceType;
		a_workpiece.holes.push_back(p0.center);
		a_workpiece.holes.push_back(p1.center);
		a_workpiece.holes.push_back(p2.center);
		a_workpiece.holes.push_back(p3.center);

		for (int m = 0; m < 4; m++)
		{
			SVzNL3DPoint  a_pt = a_workpiece.holes[m];
			a_pt = { a_pt.x + vec_norm.x, a_pt.y + vec_norm.y, a_pt.z + vec_norm.z };//法向
			a_workpiece.holesDir.push_back(a_pt);
		}
		a_workpiece.center = centerPoint;

		y_dir = vec3_normalize(y_dir);
		a_workpiece.y_dir = { y_dir.x * 20 + a_workpiece.center.x, y_dir.y * 20 + a_workpiece.center.y, a_workpiece.center.z + y_dir.z * 20 };
		a_workpiece.z_dir = normalDir;
		allWorkpiece.push_back(a_workpiece);
	}

	int workpieceNum = (int)allWorkpiece.size();
	if (workpieceNum == 0)
	{
		*errCode = SX_ERR_ZERO_OBJECTS;
		return;
	}

	//寻找最高点
	if (highest_z < 0)
	{
		highest_z = allWorkpiece[0].center.z;
		for (int i = 1; i < workpieceNum; i++)
		{
			if (highest_z > allWorkpiece[i].center.z)
				highest_z = allWorkpiece[i].center.z;
		}
	}

	//检测上层是否有残留
	double zSliceTh = highest_z - workpiecePara.H / 2;  //往上工件H的一半
	std::vector<SVzNL3DPosition> topLayerPts;
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			if ((scanLines[line][j].pt3D.z > 1e-4) && (scanLines[line][j].pt3D.z < zSliceTh))
			{
				SVzNL3DPosition a_pt;
				a_pt.nPointIdx = (line << 16) | (j & 0xffff);
				a_pt.pt3D = scanLines[line][j].pt3D;
				topLayerPts.push_back(a_pt);
			}
		}
	}
	//进行聚类，判断聚类大小。大于一定门限判断为有异物
	int topLayerClusterWin = 5;
	double clusterDist = 5.0;
	int distType = 1; //0 - 2d distance; 1- 3d distance 
	std::vector<std::vector< SVzNL3DPosition>> objClusters;
	wd_pointClustering_speedUp(
		topLayerPts,
		lineNum, linePtNum, topLayerClusterWin, //搜索窗口
		clusterDist,
		distType, //0 - 2d distance; 1- 3d distance 
		objClusters  //result
	);
	double vaidObjSize = workpiecePara.xLen * workpiecePara.yLen / 4.0;
	for (int i = 0; i < (int)objClusters.size(); i++)
	{
		SSG_ROIRectD a_roi = _getClusterROI(objClusters[i]);
		double size = (a_roi.right - a_roi.left) * (a_roi.bottom - a_roi.top);
		if (size >= vaidObjSize)
		{
			*errCode = SX_ERR_UNKNOWN_OBJECT;
			return;
		}
	}

	//排序
	//z方向排序 
	std::vector< WD_workpieceInfo> zSortWorkpiece;
	double topLayerTh = highest_z + workpiecePara.H / 2;
	for (int i = 0; i < workpieceNum; i++)
	{
		if (allWorkpiece[i].center.z < topLayerTh)
			zSortWorkpiece.push_back(allWorkpiece[i]);
	}
	//水平方向排序
	while (zSortWorkpiece.size() > 0)
	{
		std::vector< WD_workpieceInfo> firstLine;
		std::vector< WD_workpieceInfo> restWorkpiece;
		_getYTopLine(zSortWorkpiece, firstLine, restWorkpiece, workpiecePara.yLen / 2);
		workpiecePositioning.insert(workpiecePositioning.end(), firstLine.begin(), firstLine.end());
		zSortWorkpiece.clear();
		zSortWorkpiece.insert(zSortWorkpiece.end(), restWorkpiece.begin(), restWorkpiece.end());
	}

	//旋转回去
	workpieceNum = (int)workpiecePositioning.size();
	for (int i = 0; i < workpieceNum; i++)
	{
		SVzNL3DPoint rpt;
		rpt = _ptRotate(workpiecePositioning[i].center, groundCalibPara.invRMatrix);
		workpiecePositioning[i].center = rpt;
		rpt = _ptRotate(workpiecePositioning[i].y_dir, groundCalibPara.invRMatrix);
		workpiecePositioning[i].y_dir = rpt;
		rpt = _ptRotate(workpiecePositioning[i].z_dir, groundCalibPara.invRMatrix);
		workpiecePositioning[i].z_dir = rpt;
		for (int j = 0, j_max = (int)workpiecePositioning[i].holes.size(); j < j_max; j++)
		{
			rpt = _ptRotate(workpiecePositioning[i].holes[j], groundCalibPara.invRMatrix);
			workpiecePositioning[i].holes[j] = rpt;
			rpt = _ptRotate(workpiecePositioning[i].holesDir[j], groundCalibPara.invRMatrix);
			workpiecePositioning[i].holesDir[j] = rpt;
		}

		SVzNL3DPoint vector_z = { workpiecePositioning[i].z_dir.x - workpiecePositioning[i].center.x,
									workpiecePositioning[i].z_dir.y - workpiecePositioning[i].center.y,
									workpiecePositioning[i].z_dir.z - workpiecePositioning[i].center.z };
		SVzNL3DPoint vector_y = { workpiecePositioning[i].y_dir.x - workpiecePositioning[i].center.x,
									workpiecePositioning[i].y_dir.y - workpiecePositioning[i].center.y,
									workpiecePositioning[i].y_dir.z - workpiecePositioning[i].center.z };
		double mod_vz = sqrt(pow(vector_z.x, 2) + pow(vector_z.y, 2) + pow(vector_z.z, 2));
		vector_z = { vector_z.x / mod_vz, vector_z.y / mod_vz, vector_z.z / mod_vz }; //归一化
		double mod_vy = sqrt(pow(vector_y.x, 2) + pow(vector_y.y, 2) + pow(vector_y.z, 2));
		vector_y = { vector_y.x / mod_vy, vector_y.y / mod_vy, vector_y.z / mod_vy }; //归一化
		//叉乘出vector_x
		SVzNL3DPoint vector_x;
		vector_x.x = vector_y.y * vector_z.z - vector_z.y * vector_y.z;
		vector_x.y = vector_y.z * vector_z.x - vector_z.z * vector_y.x;
		vector_x.z = vector_y.x * vector_z.y - vector_z.x * vector_y.y;
		workpiecePositioning[i].x_dir = vector_x;
		workpiecePositioning[i].y_dir = vector_y;
		workpiecePositioning[i].z_dir = vector_z;
#if 0
		//得到旋转矩阵
		double R[3][3];
		R[0][0] = vector_x.x;
		R[1][0] = vector_x.y;
		R[2][0] = vector_x.z;

		R[0][1] = vector_y.x;
		R[1][1] = vector_y.y;
		R[2][1] = vector_y.z;

		R[0][2] = vector_z.x;
		R[1][2] = vector_z.y;
		R[2][2] = vector_z.z;
		SSG_EulerAngles eulerAngle = rotationMatrixToEulerZYX(R);
		workpiecePositioning[i].workpiecePose = eulerAngle;
#endif
	}
	return;

}

void _recursiveSearch(
	int seedHoleIdx, 
	std::vector< SWD_HoleInfo>& holes, 
	std::vector<int>& holeIndice, 
	const double grpDistance,
	std::vector<SWD_HoleInfo>& a_grp)
{
	std::vector<int> grpHoleIndice;
	grpHoleIndice.push_back(seedHoleIdx);
	int i = 0;
	while (i < grpHoleIndice.size())
	{
		int seedIdx = grpHoleIndice[i];
		SWD_HoleInfo& seed_hole = holes[seedIdx];
		for (int m = 0; m < (int)holeIndice.size(); m++)
		{
			int chkIdx = holeIndice[m];
			if ( chkIdx < 0) 
				continue;
			SWD_HoleInfo& chk_hole = holes[chkIdx];
			double dist = sqrt(pow(seed_hole.center.x - chk_hole.center.x, 2) + pow(seed_hole.center.y - chk_hole.center.y, 2));
			if (dist < grpDistance)
			{
				grpHoleIndice.push_back(chkIdx);
				holeIndice[m] = -1;
			}
		}
		i++;
	}
	//
	for (int m = 0; m < (int)grpHoleIndice.size(); m++)
	{
		int idx = grpHoleIndice[m];
		a_grp.push_back(holes[idx]);
	}
	return;
}

void _distanceSeparate(
	std::vector<int>& srcIndice, 
	std::vector< SWD_HoleInfo>& holes, 
	const double grpDistance,
	std::vector<std::vector<SWD_HoleInfo>>& groups)
{
	while (srcIndice.size() > 0)
	{
		int idx = srcIndice.back();
		srcIndice.pop_back();
		if (idx < 0)
			continue;

		std::vector< SWD_HoleInfo> a_grp;
		_recursiveSearch(idx, holes, srcIndice, grpDistance, a_grp);
		groups.push_back(a_grp);
	}
	return;
}

void _computeDistanceRelationships(std::vector<int>& indice, std::vector<SWD_HoleInfo>& holes, const SVzNL3DPoint refPoint, std::vector< SSG_pointPairRelationD>& distanceRelations)
{
	for (int m = 0; m < (int)indice.size(); m++)
	{
		int idx1 = indice[m];
		SWD_HoleInfo& hole_1 = holes[idx1];
		for (int n = m + 1; n < (int)indice.size(); n++)
		{
			int idx2 = indice[n];
			SWD_HoleInfo& hole_2 = holes[idx2];
			double distance = sqrt(pow(hole_1.center.x - hole_2.center.x, 2) + pow(hole_1.center.y - hole_2.center.y, 2));
			
			double _a, _b, _c;
			compute2ptLine(hole_1.center, hole_2.center, &_a, &_b, &_c);
			double dist2 = computePtDistToLine(refPoint.x, refPoint.y, _a, _b, _c);

			SSG_pointPairRelationD a_pair;
			a_pair.data_0 = idx1;
			a_pair.data_1 = idx2;
			a_pair.pointDistance = distance;
			a_pair.distFromCenterToLine = dist2;
			a_pair.pointZDiff = abs(hole_1.center.z - hole_2.center.z);
			distanceRelations.push_back(a_pair);
		}
	}
	return;
}

void _computeDistanceRelationships_2(std::vector<SWD_HoleInfo>& holes, const SVzNL3DPoint refPoint, std::vector< SSG_pointPairRelationD>& distanceRelations)
{
	for (int m = 0; m < (int)holes.size(); m++)
	{
		SWD_HoleInfo& hole_1 = holes[m];
		for (int n = m + 1; n < (int)holes.size(); n++)
		{
			SWD_HoleInfo& hole_2 = holes[n];
			double distance = sqrt(pow(hole_1.center.x - hole_2.center.x, 2) + pow(hole_1.center.y - hole_2.center.y, 2));

			double _a, _b, _c;
			compute2ptLine(hole_1.center, hole_2.center, &_a, &_b, &_c);
			double dist2 = computePtDistToLine(refPoint.x, refPoint.y, _a, _b, _c);

			SSG_pointPairRelationD a_pair;
			a_pair.data_0 = m;
			a_pair.data_1 = n;
			a_pair.pointDistance = distance;
			a_pair.distFromCenterToLine = dist2;
			a_pair.pointZDiff = abs(hole_1.center.z - hole_2.center.z);
			distanceRelations.push_back(a_pair);
		}
	}
	return;
}

//工件3: 孔定位-拓普发工件孔定位
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)
{
	*errCode = 0;

	int lineNum = (int)scanLinesInput.size();
	std::vector< std::vector<SVzNL3DPosition>> scanLines;
	scanLines.resize(lineNum);
	int linePtNum = (int)scanLinesInput[0].size();
	bool isGridData = true;
	for (int i = 0; i < lineNum; i++)
	{
		if (linePtNum != (int)scanLinesInput[i].size())
			isGridData = false;

		scanLines[i].resize(scanLinesInput[i].size());
		std::copy(scanLinesInput[i].begin(), scanLinesInput[i].end(), scanLines[i].begin()); // 使用std::copy算法

		for (int j = 0; j < (int)scanLinesInput[i].size(); j++)
			scanLinesInput[i][j].nPointIdx = 0;  //清零，用于debug时记录信息
	}
	if (false == isGridData)//数据不是网格格式
	{
		*errCode = SG_ERR_NOT_GRID_FORMAT;
		return;
	}
	for (int i = 0; i < lineNum; i++)
	{																					 //行处理
		//调平，去除地面
		wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1);
		//wd_lineDataR(scanLinesInput[i], groundCalibPara.planeCalib, -1);
	}

	//过滤掉凹段
	SSG_lineSegParam filterSegPara;
	filterSegPara.distScale = 0;
	filterSegPara.segGapTh_y = DBL_MAX;
	filterSegPara.segGapTh_z = 15.0;
	for (int line = 0; line < lineNum; line++)
	{
		if (line == 312)
			int kkk = 1;
		//分段
		std::vector<SSG_RUN> segs;
		wd_getLineDataIntervals(scanLines[line], filterSegPara, segs);
		std::vector<int> filterIndice;
		//若一个段首尾都比相邻段低，视为凹陷段
		for (int i = 1; i < (int)segs.size() - 1; i++)
		{
			int tail_1 = segs[i - 1].start + segs[i - 1].len - 1;
			int head_1 = segs[i].start;
			int tail_2 = segs[i].start + segs[i].len - 1;
			int head_2 = segs[i + 1].start;
			double z_diff_1 = scanLines[line][head_1].pt3D.z - scanLines[line][tail_1].pt3D.z;
			double z_diff_2 = scanLines[line][tail_2].pt3D.z - scanLines[line][head_2].pt3D.z;
			if ((z_diff_1 > 0) && (z_diff_2 > 0))
				filterIndice.push_back(i);
		}
		for (int m = 0; m < (int)filterIndice.size(); m++)
		{
			int idx = filterIndice[m];
			int offset = segs[idx].start;
			for (int i = 0; i < segs[idx].len; i++)
			{
				scanLines[line][offset + i].pt3D.z = 0;
				scanLinesInput[line][offset + i].pt3D.z = 0;
			}
		}
	}

#if 0
	//产生水平扫描数据
	std::vector< std::vector<SVzNL3DPosition>> scanLines_h;
	scanLines_h.resize(linePtNum);
	for (int i = 0; i < linePtNum; i++)
		scanLines_h[i].resize(lineNum);
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
			scanLines_h[j][line] = scanLines[line][j];
			scanLines_h[j][line].pt3D.x = scanLines[line][j].pt3D.y;
			scanLines_h[j][line].pt3D.y = scanLines[line][j].pt3D.x;
		}
	}
	for (int line = 0; line < linePtNum; line++)
	{
		for (int j = 0, j_max = (int)scanLines_h[line].size(); j < j_max; j++)
			scanLines_h[line][j].nPointIdx = j;
	}
	//去垂直段
	//内部参数
	SSG_cornerParam removeVertialPara;
	memset(&removeVertialPara, 0, sizeof(SSG_cornerParam));
	removeVertialPara.scale = 10.0;
	removeVertialPara.cornerTh = 60;

	std::vector<std::vector<int>> flags;
	flags.resize(lineNum);
	for (int i = 0; i < lineNum; i++)
	{
		flags[i].resize(linePtNum);
		std::fill(flags[i].begin(), flags[i].end(), 0);
	}
	std::vector<std::vector<int>> zVertivalFlags;
	for (int line = 0; line < lineNum; line++)
	{
		if (line == 700)
			int kkk = 1;
		std::vector<int> line_verticalFlags;
		wd_getXYVertialFeature_dirAngleMethod(
			scanLines[line],
			line,
			removeVertialPara,
			line_verticalFlags
		);
		zVertivalFlags.push_back(line_verticalFlags);

		for (int i = 0; i < (int)line_verticalFlags.size(); i++)
		{
			if (line_verticalFlags[i] > 0)
				flags[line][i] = 1;
		}
	}

	std::vector<std::vector<int>> zVertivalFlags_h;
	for (int line = 0; line < linePtNum; line++)
	{
		if (line == 1177)
			int kkk = 1;
		std::vector<int> line_verticalFlags;
		wd_getXYVertialFeature_dirAngleMethod(
			scanLines_h[line],
			line,
			removeVertialPara,
			line_verticalFlags
		);
		zVertivalFlags_h.push_back(line_verticalFlags);

		for (int i = 0; i < (int)line_verticalFlags.size(); i++)
		{
			if (line_verticalFlags[i] > 0)
				flags[i][line] = 1;
		}
	}
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			if (flags[line][j] > 0)
			{
				scanLines[line][j].pt3D.z = 0;
				scanLines_h[j][line].pt3D.z = 0;
			}
		}
	}
	//迭代一次
	SSG_lineSegParam filterLineSegPara;
	filterLineSegPara.distScale = 5.0;
	filterLineSegPara.segGapTh_y = 5.0;
	filterLineSegPara.segGapTh_z = 5.0;
	const int minSegLen = 5;
	for (int line = 0; line < lineNum; line++)
	{
		std::vector<SSG_RUN> segs;
		wd_getLineDataIntervals(
			scanLines[line],
			filterLineSegPara,
			segs);
		for (int i = 0; i < (int)segs.size(); i++)
		{
			if (segs[i].len <= minSegLen)
			{
				int idx0 = segs[i].start;
				for (int j = 0; j < segs[i].len; j++)
					flags[line][idx0 + j] = 1;
			}
		}
	}
	for (int line = 0; line < linePtNum; line++)
	{
		std::vector<SSG_RUN> segs;
		wd_getLineDataIntervals(
			scanLines_h[line],
			filterLineSegPara,
			segs);
		for (int i = 0; i < (int)segs.size(); i++)
		{
			if (segs[i].len <= minSegLen)
			{
				int idx0 = segs[i].start;
				for (int j = 0; j < segs[i].len; j++)
					flags[idx0 + j][line] = 1;
			}
		}
	}

	//标注
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			scanLinesInput[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
			scanLines[line][j].nPointIdx = 0;
		}
	}
	//将垂直线段去除
	std::vector< SVzNL3DPosition> validPoints;
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			if (flags[line][j] > 0)
			{
				scanLines[line][j].pt3D.z = 0;
				scanLinesInput[line][j].pt3D.z = 0;
			}
		}
	}
#endif

	//生成量化数据，以1mm为量化尺度，用于确定工件表面高度
	SVzNL3DRangeD roi3D = sg_getScanDataROI_vector(scanLines);
	SVzNLRect roi2D;
	roi2D.left = (int)roi3D.xRange.min;
	roi2D.right = (int)roi3D.xRange.max;
	roi2D.top = (int)roi3D.yRange.min;
	roi2D.bottom = (int)roi3D.yRange.max;
	int quanti_X = roi2D.right - roi2D.left + 1;
	int quanti_Y = roi2D.bottom - roi2D.top + 1;
	std::vector<std::vector<double>> quantiValue;
	std::vector<std::vector<int>> quantiHist;
	quantiValue.resize(quanti_X);
	quantiHist.resize(quanti_X);
	for (int i = 0; i < quanti_X; i++)
	{
		quantiValue[i].resize(quanti_Y);
		std::fill(quantiValue[i].begin(), quantiValue[i].end(), 0);//初始化为0
		quantiHist[i].resize(quanti_Y);
		std::fill(quantiHist[i].begin(), quantiHist[i].end(), 0);//初始化为0
	}
	//以1mm尺度量化
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			SVzNL3DPoint& a_pt = scanLines[line][j].pt3D;
			if (a_pt.z > 1e-4)
			{
				int qx = (int)a_pt.x - roi2D.left;
				int qy = (int)a_pt.y - roi2D.top;
				quantiValue[qx][qy] += a_pt.z;
				quantiHist[qx][qy] += 1;
			}
		}
	}
	for (int ix = 0; ix < quanti_X; ix++)
	{
		for (int iy = 0; iy < quanti_Y; iy++)
		{
			if (quantiHist[ix][iy] > 0)
				quantiValue[ix][iy] = quantiValue[ix][iy] / quantiHist[ix][iy];
		}
	}

	std::vector<SWD_segFeatureTree> segTrees_v;
	std::vector<SWD_segFeatureTree> segTrees_h;
	std::vector<SSG_intPair> validObjects;
	double valieCommonNumRatio = 0.25;
	WD_getHoleInfo(scanLines, lineSegPara, filterParam, growParam, valieCommonNumRatio, segTrees_v, segTrees_h, validObjects);

	for (int i = 0; i < lineNum; i++)
	{
		for (int j = 0; j < (int)scanLines[i].size(); j++)
			scanLines[i][j].nPointIdx = 0;  //清零
	}

	//生成聚类信息， 
	std::vector<std::vector< SVzNL2DPoint>> clusters; //只记录位置
	std::vector<SVzNL3DRangeD> clustersRoi3D;
	for (int i = 0; i < (int)validObjects.size(); i++)
	{
		std::vector< SVzNL2DPoint> a_cluster;
		SVzNL3DRangeD a_roi3D = { {-1, -1}, {-1, -1}, {-1, -1 } };

		int vTreeIdx = validObjects[i].data_0;
		int hTreeIdx = validObjects[i].data_1;
		for (int m = 0; m < (int)segTrees_v[vTreeIdx].treeNodes.size(); m++)
		{
			SWD_segFeature& a_seg = segTrees_v[vTreeIdx].treeNodes[m];
			if (scanLines[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx == 0)
			{
				scanLines[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = vTreeIdx + 1; // 0x01;
				scanLinesInput[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = vTreeIdx + 1; // 0x01;
				SVzNL2DPoint a_pos = { a_seg.lineIdx , a_seg.endPtIdx };
				a_cluster.push_back(a_pos);
				_updateRoi3D(a_roi3D, scanLines[a_seg.lineIdx][a_seg.endPtIdx].pt3D);
			}
			if (scanLines[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx == 0)
			{
				scanLines[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = vTreeIdx + 1; // 0x01;
				scanLinesInput[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = vTreeIdx + 1; // 0x01;
				SVzNL2DPoint a_pos = { a_seg.lineIdx , a_seg.startPtIdx };
				a_cluster.push_back(a_pos);
				_updateRoi3D(a_roi3D, scanLines[a_seg.lineIdx][a_seg.startPtIdx].pt3D);
			}
		}
		for (int m = 0; m < (int)segTrees_h[hTreeIdx].treeNodes.size(); m++)
		{
			SWD_segFeature& a_seg = segTrees_h[hTreeIdx].treeNodes[m];
			if (scanLines[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx == 0)
			{
				scanLines[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02;
				scanLinesInput[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02;
				SVzNL2DPoint a_pos = { a_seg.startPtIdx , a_seg.lineIdx };
				a_cluster.push_back(a_pos);
				_updateRoi3D(a_roi3D, scanLines[a_seg.startPtIdx][a_seg.lineIdx].pt3D);
			}
			if (scanLines[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx == 0)
			{
				scanLines[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02;
				scanLinesInput[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02;
				SVzNL2DPoint a_pos = { a_seg.endPtIdx , a_seg.lineIdx };
				a_cluster.push_back(a_pos);
				_updateRoi3D(a_roi3D, scanLines[a_seg.endPtIdx][a_seg.lineIdx].pt3D);
			}
		}
		clusters.push_back(a_cluster);
		clustersRoi3D.push_back(a_roi3D);
	}

	//聚类结果分析
	std::vector<int> bigCluserIndice;  //大孔
	std::vector<int> smallCluserIndice; //小孔
	int clusterSize = (int)clusters.size();
	for (int i = 0; i < clusterSize; i++)
	{
		SVzNL3DRangeD& a_roi = clustersRoi3D[i];
		double L = a_roi.xRange.max - a_roi.xRange.min;
		double W = a_roi.yRange.max - a_roi.yRange.min;
		if ((L > smallHoleParam.holeDiameter * 0.5) && (L < smallHoleParam.holeDiameter * 2.5) &&
			(W > smallHoleParam.holeDiameter * 0.5) && (W < smallHoleParam.holeDiameter * 2.5))
			smallCluserIndice.push_back(i);
		else if ((L > bigHoleDiameter * 0.75) && (L < bigHoleDiameter * 1.25) &&
			(W > bigHoleDiameter * 0.75) && (W < bigHoleDiameter * 1.25))
			bigCluserIndice.push_back(i);
	}

	//生成Hole信息
	//大孔
	std::vector< SWD_HoleInfo> bigHoles;
	int bigHoleSize = (int)bigCluserIndice.size();
	for (int objIdx = 0; objIdx < bigHoleSize; objIdx++)
	{
		std::vector<SVzNL3DPoint> pointArray;
		int clusterIdx = bigCluserIndice[objIdx];
		//取cluster上的点
		int clusterPtSize = (int)clusters[clusterIdx].size();
		double sumZ = 0;
		int counter = 0;
		for (int i = 0; i < clusterPtSize; i++)
		{
			SVzNL2DPoint a_pos = clusters[clusterIdx][i];
			int lineIdx = a_pos.x;
			int ptIdx = a_pos.y;
			SVzNL3DPoint a_pt3d = scanLines[lineIdx][ptIdx].pt3D;
			if (a_pt3d.z > 1e-4)
			{
				sumZ += a_pt3d.z;
				counter++;
			}
			pointArray.push_back(a_pt3d);
		}
		
		//圆拟合
		SVzNL3DPoint center;
		double radius;
		double err = fitCircleByLeastSquare(pointArray, center, radius);
		center.z = sumZ / counter;
		SWD_HoleInfo a_hole;
		a_hole.center = { center.x, center.y, center.z };
		a_hole.radius = radius;
		bigHoles.push_back(a_hole);
	}
	//小孔
	std::vector< SWD_HoleInfo> smallHoles;
	int smallHoleSize = (int)smallCluserIndice.size();
	for (int objIdx = 0; objIdx < smallHoleSize; objIdx++)
	{
		std::vector<SVzNL3DPoint> pointArray;
		int clusterIdx = smallCluserIndice[objIdx];
		//取cluster上的点
		int clusterPtSize = (int)clusters[clusterIdx].size();
		double minZ = DBL_MAX;
		double maxZ = 0;
		for (int i = 0; i < clusterPtSize; i++)
		{
			SVzNL2DPoint a_pos = clusters[clusterIdx][i];
			int lineIdx = a_pos.x;
			int ptIdx = a_pos.y;
			SVzNL3DPoint a_pt3d = scanLines[lineIdx][ptIdx].pt3D;
			if (minZ > a_pt3d.z)
				minZ = a_pt3d.z;
			if (maxZ < a_pt3d.z)
				maxZ = a_pt3d.z;
			pointArray.push_back(a_pt3d);
		}
		//圆拟合
		double zDiff = maxZ - minZ;
		if (zDiff < lineSegPara.distScale)
		{
			SVzNL3DPoint center;
			double radius;
			double err = fitCircleByLeastSquare(pointArray, center, radius);
			center.z = minZ;
			SWD_HoleInfo a_hole;
			a_hole.center = { center.x, center.y, center.z };
			a_hole.radius = radius;
			smallHoles.push_back(a_hole);
		}
	}

	//生成目标
	//内部参数
	double smallHoleMinDistance = 8.0;
	double maxDistanceFromCenterToLine = 1.0;
	SVzNLRangeD vHoleDistance = {45.0, 60.0};
	double reverseJudgeDistToCenterTH = 33.0;
	double midPointToLongerHoleDIstTh = 20.0;  //
	double distP1P2_short_minTh = 35.0;
	double distP1P2_short_maxTh = 48.0;
	double distP1P2_long_minTh = 25.0;
	double distP1P2_long_maxTh = 38.0;
	SVzNLRangeD distRange = { 0, bigHoleDiameter * 1.2 };
	double deltaZ = 10.0;
	std::vector< WD_workpieceInfo> allWorkpiece;
	double highest_z = -1;
	for (int objIdx = 0; objIdx < bigHoleSize; objIdx++)
	{
		if (objIdx == 11)
			int kkk = 1;

		if (bigHoles[objIdx].radius < 0)
			continue;

		//holes[objIdx].radius = -1;
		SWD_HoleInfo& p0 = bigHoles[objIdx];
		//搜索相信小孔
		std::vector<int> searchResults;
		searchObjectInRange(p0.center, smallHoles, distRange, deltaZ, searchResults);

		std::vector< SSG_pointPairRelationD> clusterSmallHoleRelations;
		_computeDistanceRelationships(searchResults, smallHoles, p0.center, clusterSmallHoleRelations);

		//搜索垂直方向
		double minDistToLine = -1;
		SSG_pointPairRelationD bestPair;
		for (int m = 0; m < (int)clusterSmallHoleRelations.size(); m++)
		{
			if ((clusterSmallHoleRelations[m].pointDistance > vHoleDistance.min) &&
				(clusterSmallHoleRelations[m].pointDistance < vHoleDistance.max) &&
				(clusterSmallHoleRelations[m].distFromCenterToLine < maxDistanceFromCenterToLine) &&
				(clusterSmallHoleRelations[m].pointZDiff < smallHoleParam.H / 4))
			{
				if (minDistToLine < 0)
				{
					minDistToLine = clusterSmallHoleRelations[m].distFromCenterToLine;
					bestPair = clusterSmallHoleRelations[m];
				}
				else if (minDistToLine > clusterSmallHoleRelations[m].distFromCenterToLine)
				{
					minDistToLine = clusterSmallHoleRelations[m].distFromCenterToLine;
					bestPair = clusterSmallHoleRelations[m];
				}
			}
		}

		SWD_HoleInfo p1, p2;
		bool foundObj = false;
		bool isYDIr = true;
		bool isReverse = false;
		if (minDistToLine >= 0)
		{
			p1 = smallHoles[bestPair.data_0];
			p2 = smallHoles[bestPair.data_1];

			foundObj = true;
			isYDIr = false;
			//判断正反
			bool judeOk = false;
			for (int n = 0; n < (int)searchResults.size(); n++)
			{
				int idx = searchResults[n];
				if ( (idx == bestPair.data_0) || (idx == bestPair.data_1))
					continue;

				double distToCenter = sqrt(pow(p0.center.x - smallHoles[idx].center.x, 2) +
					pow(p0.center.y - smallHoles[idx].center.y, 2));
				double _distToP1 = sqrt(pow(p1.center.x - smallHoles[idx].center.x, 2) +
					pow(p1.center.y - smallHoles[idx].center.y, 2));
				if ((distToCenter > reverseJudgeDistToCenterTH) && (_distToP1 < midPointToLongerHoleDIstTh))
				{
					judeOk = true;
					if (smallHoles[idx].center.y < p0.center.y)
						isReverse = true;
					break;
				}
			}
			if (false == judeOk)
			{
				for (int n = 0; n < (int)searchResults.size(); n++)
				{
					int idx = searchResults[n];
					if ((idx == bestPair.data_0) || (idx == bestPair.data_1))
						continue;

					double distToCenter = sqrt(pow(p0.center.x - smallHoles[idx].center.x, 2) +
						pow(p0.center.y - smallHoles[idx].center.y, 2));
					double _distToP2 = sqrt(pow(p2.center.x - smallHoles[idx].center.x, 2) +
						pow(p2.center.y - smallHoles[idx].center.y, 2));
					if ((distToCenter > reverseJudgeDistToCenterTH) && (_distToP2 < midPointToLongerHoleDIstTh))
					{
						judeOk = true;
						if (smallHoles[idx].center.y < p0.center.y)
							isReverse = true;
						break;
					}
				}
			}
		}
		else
		{
			//根据距离进行分组
			std::vector<std::vector<SWD_HoleInfo>> groups;
			_distanceSeparate(searchResults, smallHoles, bigHoleDiameter, groups);
			if (groups.size() == 0)  //工件3和工作4的特点是小孔分为上下各3个。分组
				continue;

			std::vector<std::vector<double>> distancesToCenter;
			distancesToCenter.resize(groups.size());
			for (int grp = 0; grp < (int)groups.size(); grp++)
			{
				distancesToCenter[grp].resize(groups[grp].size());
				for (int m = 0; m < (int)groups[grp].size(); m++)
				{
					double dist = sqrt(pow(p0.center.x - groups[grp][m].center.x, 2) +
						pow(p0.center.y - groups[grp][m].center.y, 2));
					distancesToCenter[grp][m] = dist;
				}
			}
			//搜索每组中距离大于大孔直径0.8倍的目标
			for (int grp = 0; grp < (int)groups.size(); grp++)
			{
				std::vector<SWD_HoleInfo>& a_grp = groups[grp];
				if (a_grp.size() < 2)
					continue;

				std::vector< SSG_pointPairRelationD> distanceRelations;
				_computeDistanceRelationships_2(a_grp, p0.center, distanceRelations);
				double minDistance = distanceRelations[0].pointDistance;
				for (int m = 1; m < (int)distanceRelations.size(); m++)
					minDistance = minDistance > distanceRelations[m].pointDistance ? distanceRelations[m].pointDistance : minDistance;
				if (minDistance < smallHoleMinDistance) //有非法小孔
					continue;

				std::vector< SSG_pointPairRelationD> validRelations;
				for (int m = 0; m < (int)distanceRelations.size(); m++)
				{
					if (distanceRelations[m].pointDistance > bigHoleDiameter * 0.8)
						validRelations.push_back(distanceRelations[m]);
				}

				if (validRelations.size() == 1)
				{
					int idx1 = validRelations[0].data_0;
					int idx2 = validRelations[0].data_1;
					p1 = a_grp[idx1];
					p2 = a_grp[idx2];
					foundObj = true;

					double dist1 = distancesToCenter[grp][idx1];
					double dist2 = distancesToCenter[grp][idx2];
					if ((dist1 > reverseJudgeDistToCenterTH) && (dist2 > reverseJudgeDistToCenterTH))  //longer
					{
						if (p1.center.y < p0.center.y)  //长的在上面，反
							isReverse = true;
						foundObj = true;
						break;
					}
					else if ((dist1 < reverseJudgeDistToCenterTH) && (dist2 < reverseJudgeDistToCenterTH)) //shorter
					{
						if (p1.center.y > p0.center.y)  //短的在下面，反
							isReverse = true;
						foundObj = true;
						break;
					}
					break;
				}
				else if (validRelations.size() > 1)
				{
					SSG_pointPairRelationD bestPair = validRelations[0];
					for (int m = 1; m < (int)validRelations.size(); m++)
					{
						if (bestPair.pointZDiff > validRelations[m].pointZDiff)
							bestPair = validRelations[m];
					}
					int idx1 = bestPair.data_0;
					int idx2 = bestPair.data_1;
					p1 = a_grp[idx1];
					p2 = a_grp[idx2];

					double dist1 = distancesToCenter[grp][idx1];
					double dist2 = distancesToCenter[grp][idx2];
					if ((dist1 > reverseJudgeDistToCenterTH) && (dist2 > reverseJudgeDistToCenterTH))  //longer
					{
						if ((bestPair.pointDistance > distP1P2_long_minTh) && (bestPair.pointDistance < distP1P2_long_maxTh))
						{
							if (p1.center.y < p0.center.y)  //长的在上面，反
								isReverse = true;
							foundObj = true;
							break;
						}
					}
					else if ((dist1 < reverseJudgeDistToCenterTH) && (dist2 < reverseJudgeDistToCenterTH)) //shorter
					{
						if ((bestPair.pointDistance > distP1P2_short_minTh) && (bestPair.pointDistance < distP1P2_short_maxTh))
						{
							if (p1.center.y > p0.center.y)  //短的在下面，反
								isReverse = true;
							foundObj = true;
							break;
						}
					}
				}
			}
		}
		if (false == foundObj)
			continue;

		SVzNL3DPoint centerPoint = p0.center;
		//计算工件法向
		SVzNLRangeD distRange = { bigHoleDiameter / 2 + 2.0, bigHoleDiameter / 2 + 4.0 };
		std::vector<cv::Point3d> Points3ds;
		_getCirclePoints(quantiValue, p0.center,roi2D, distRange, Points3ds);

		//计算面参数: z = Ax + By + C
		//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
		std::vector<double> res;
		vzCaculateLaserPlane(Points3ds, res);
		SVzNL3DPoint vec_1 = { -res[0], -res[1], 1.0 };
		SVzNL3DPoint vec_norm = vec3_normalize(vec_1);
		SVzNL3DPoint vec_norm_multi = vec3_multiply(vec_norm, 20.0);
		SVzNL3DPoint normalDir = { centerPoint.x + vec_norm_multi.x, centerPoint.y + vec_norm_multi.y, centerPoint.z + vec_norm_multi.z };

		SVzNL3DPoint vector2 = { 0, 0, 1 };
		SSG_planeCalibPara adjustPara = wd_computeRTMatrix(vec_1, vector2);

		SVzNL3DPoint rotate_p1 = _ptRotate(p1.center, adjustPara.planeCalib);
		SVzNL3DPoint rotate_p2 = _ptRotate(p2.center, adjustPara.planeCalib);


		SVzNL3DPoint y_dir;
		if (true == isYDIr)
		{
			if (rotate_p2.x < rotate_p1.x)
				y_dir = { rotate_p1.x - rotate_p2.x, rotate_p1.y - rotate_p2.y, 0 };
			else
				y_dir = { rotate_p2.x - rotate_p1.x, rotate_p2.y - rotate_p1.y, 0 };
			y_dir = _ptRotate(y_dir, adjustPara.invRMatrix);
		}
		else
		{
			SVzNL3DPoint x_dir;
			if (rotate_p2.x < rotate_p1.x)
				x_dir = { rotate_p1.x - rotate_p2.x, rotate_p1.y - rotate_p2.y, 0 };
			else
				x_dir = { rotate_p2.x - rotate_p1.x, rotate_p2.y - rotate_p1.y, 0 };

			x_dir = vec3_normalize(x_dir);
			//叉乘出yDir
			SVzNL3DPoint vec_norm = vec3_normalize(vec_1);
			y_dir.x = vec_norm.y * x_dir.z - x_dir.y * vec_norm.z;
			y_dir.y = vec_norm.z * x_dir.x - x_dir.z * vec_norm.x;
			y_dir.z = vec_norm.x * x_dir.y - x_dir.x * vec_norm.y;
			if (y_dir.x < 0)
				y_dir = { -y_dir.x, -y_dir.y, -y_dir.z };

			y_dir = _ptRotate(y_dir, adjustPara.invRMatrix);
		}

		WD_workpieceInfo a_workpiece;
		a_workpiece.workpieceType = smallHoleParam.workpieceType;
		if (true == isReverse) //反
			a_workpiece.workpieceType = a_workpiece.workpieceType | 0x10000;
		a_workpiece.center = centerPoint;
		y_dir = vec3_normalize(y_dir);
		a_workpiece.y_dir = { y_dir.x * 20 + a_workpiece.center.x, y_dir.y * 20 + a_workpiece.center.y, a_workpiece.center.z + y_dir.z * 20 };
		a_workpiece.z_dir = normalDir;
		allWorkpiece.push_back(a_workpiece);
	}

	int workpieceNum = (int)allWorkpiece.size();
	if (workpieceNum == 0)
	{
		*errCode = SX_ERR_ZERO_OBJECTS;
		return;
	}

	//寻找最高点
	if (highest_z < 0)
	{
		highest_z = allWorkpiece[0].center.z;
		for (int i = 1; i < workpieceNum; i++)
		{
			if (highest_z > allWorkpiece[i].center.z)
				highest_z = allWorkpiece[i].center.z;
		}
	}

	//检测上层是否有残留
	double zSliceTh = highest_z - smallHoleParam.H / 2;  //往上工件H的一半
	std::vector<SVzNL3DPosition> topLayerPts;
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			if ((scanLines[line][j].pt3D.z > 1e-4) && (scanLines[line][j].pt3D.z < zSliceTh))
			{
				SVzNL3DPosition a_pt;
				a_pt.nPointIdx = (line << 16) | (j & 0xffff);
				a_pt.pt3D = scanLines[line][j].pt3D;
				topLayerPts.push_back(a_pt);
			}
		}
	}
	//进行聚类，判断聚类大小。大于一定门限判断为有异物
	int topLayerClusterWin = 5;
	double clusterDist = 5.0;
	int distType = 1; //0 - 2d distance; 1- 3d distance 
	std::vector<std::vector< SVzNL3DPosition>> objClusters;
	wd_pointClustering_speedUp(
		topLayerPts,
		lineNum, linePtNum, topLayerClusterWin, //搜索窗口
		clusterDist,
		distType, //0 - 2d distance; 1- 3d distance 
		objClusters  //result
	);
	double vaidObjSize = smallHoleParam.xLen * smallHoleParam.yLen / 4.0;
	for (int i = 0; i < (int)objClusters.size(); i++)
	{
		SSG_ROIRectD a_roi = _getClusterROI(objClusters[i]);
		double size = (a_roi.right - a_roi.left) * (a_roi.bottom - a_roi.top);
		if (size >= vaidObjSize)
		{
			*errCode = SX_ERR_UNKNOWN_OBJECT;
			return;
		}
	}

	//排序
	//z方向排序 
	std::vector< WD_workpieceInfo> zSortWorkpiece;
	double topLayerTh = highest_z + smallHoleParam.H / 2;
	for (int i = 0; i < workpieceNum; i++)
	{
		if (allWorkpiece[i].center.z < topLayerTh)
			zSortWorkpiece.push_back(allWorkpiece[i]);
	}
	//水平方向排序
	while (zSortWorkpiece.size() > 0)
	{
		std::vector< WD_workpieceInfo> firstLine;
		std::vector< WD_workpieceInfo> restWorkpiece;
		_getYTopLine(zSortWorkpiece, firstLine, restWorkpiece, smallHoleParam.yLen / 2);
		workpiecePositioning.insert(workpiecePositioning.end(), firstLine.begin(), firstLine.end());
		zSortWorkpiece.clear();
		zSortWorkpiece.insert(zSortWorkpiece.end(), restWorkpiece.begin(), restWorkpiece.end());
	}

	//旋转回去
	workpieceNum = (int)workpiecePositioning.size();
	for (int i = 0; i < workpieceNum; i++)
	{
		SVzNL3DPoint rpt;
		rpt = _ptRotate(workpiecePositioning[i].center, groundCalibPara.invRMatrix);
		workpiecePositioning[i].center = rpt;
		rpt = _ptRotate(workpiecePositioning[i].y_dir, groundCalibPara.invRMatrix);
		workpiecePositioning[i].y_dir = rpt;
		rpt = _ptRotate(workpiecePositioning[i].z_dir, groundCalibPara.invRMatrix);
		workpiecePositioning[i].z_dir = rpt;
		for (int j = 0, j_max = (int)workpiecePositioning[i].holes.size(); j < j_max; j++)
		{
			rpt = _ptRotate(workpiecePositioning[i].holes[j], groundCalibPara.invRMatrix);
			workpiecePositioning[i].holes[j] = rpt;
			rpt = _ptRotate(workpiecePositioning[i].holesDir[j], groundCalibPara.invRMatrix);
			workpiecePositioning[i].holesDir[j] = rpt;
		}

		SVzNL3DPoint vector_z = { workpiecePositioning[i].z_dir.x - workpiecePositioning[i].center.x,
									workpiecePositioning[i].z_dir.y - workpiecePositioning[i].center.y,
									workpiecePositioning[i].z_dir.z - workpiecePositioning[i].center.z };
		SVzNL3DPoint vector_y = { workpiecePositioning[i].y_dir.x - workpiecePositioning[i].center.x,
									workpiecePositioning[i].y_dir.y - workpiecePositioning[i].center.y,
									workpiecePositioning[i].y_dir.z - workpiecePositioning[i].center.z };
		double mod_vz = sqrt(pow(vector_z.x, 2) + pow(vector_z.y, 2) + pow(vector_z.z, 2));
		vector_z = { vector_z.x / mod_vz, vector_z.y / mod_vz, vector_z.z / mod_vz }; //归一化
		double mod_vy = sqrt(pow(vector_y.x, 2) + pow(vector_y.y, 2) + pow(vector_y.z, 2));
		vector_y = { vector_y.x / mod_vy, vector_y.y / mod_vy, vector_y.z / mod_vy }; //归一化
		//叉乘出vector_x
		SVzNL3DPoint vector_x;
		vector_x.x = vector_y.y * vector_z.z - vector_z.y * vector_y.z;
		vector_x.y = vector_y.z * vector_z.x - vector_z.z * vector_y.x;
		vector_x.z = vector_y.x * vector_z.y - vector_z.x * vector_y.y;
		workpiecePositioning[i].x_dir = vector_x;
		workpiecePositioning[i].y_dir = vector_y;
		workpiecePositioning[i].z_dir = vector_z;
#if 0
		//得到旋转矩阵
		double R[3][3];
		R[0][0] = vector_x.x;
		R[1][0] = vector_x.y;
		R[2][0] = vector_x.z;

		R[0][1] = vector_y.x;
		R[1][1] = vector_y.y;
		R[2][1] = vector_y.z;

		R[0][2] = vector_z.x;
		R[1][2] = vector_z.y;
		R[2][2] = vector_z.z;
		SSG_EulerAngles eulerAngle = rotationMatrixToEulerZYX(R);
		workpiecePositioning[i].workpiecePose = eulerAngle;
#endif
	}
	return;

}
typedef struct
{
	int flag;
	SWDIndexing3DPoint zMaxPos;
	SVzNLRect peakROI;
	double pkValue;
}_zMaxInfo;

bool _getZMaxPeakROI(SVzNL2DPoint seedPos,
	std::vector<std::vector<SSG_pntDirAngle>>& pntDirAngles_v,
	std::vector<std::vector<SSG_pntDirAngle>>& pntDirAngles_h,
	double pntCornenrMax,
	double minZHeight,
	SVzNLRect& roi
)
{
	int lineNum = (int)pntDirAngles_v.size();
	int ptNum = (int)pntDirAngles_h.size();

	double seed_z = pntDirAngles_v[seedPos.x][seedPos.y].curr_z;
	//向左搜索
	int left = -1;
	for (int line = seedPos.x; line >= 0; line--)
	{
		double z_diff = seed_z - pntDirAngles_v[line][seedPos.y].curr_z;
		if ((pntDirAngles_v[line][seedPos.y].type >= 0) && (abs(pntDirAngles_v[line][seedPos.y].corner) < pntCornenrMax)
			&& (z_diff > minZHeight))
		{
			left = line;
			break;
		}
	}
	//向右搜索
	int right = -1;
	for (int line = seedPos.x; line < lineNum; line++)
	{
		double z_diff = seed_z - pntDirAngles_v[line][seedPos.y].curr_z;
		if ((pntDirAngles_v[line][seedPos.y].type >= 0) && (abs(pntDirAngles_v[line][seedPos.y].corner) < pntCornenrMax) 
			&& (z_diff > minZHeight))
		{
			right = line;
			break;
		}
	}
	//向上搜索
	int top = -1;
	for (int ptIdx = seedPos.y; ptIdx >= 0; ptIdx--)
	{
		double z_diff = seed_z - pntDirAngles_h[ptIdx][seedPos.x].curr_z;
		if ((pntDirAngles_h[ptIdx][seedPos.x].type >= 0) && (abs(pntDirAngles_h[ptIdx][seedPos.x].corner) < pntCornenrMax)
			&& (z_diff > minZHeight))
		{
			top = ptIdx;
			break;
		}
	}
	//向下搜索
	int bottom = -1;
	for (int ptIdx = seedPos.y; ptIdx < ptNum; ptIdx++)
	{
		double z_diff = seed_z - pntDirAngles_h[ptIdx][seedPos.x].curr_z;
		if ((pntDirAngles_h[ptIdx][seedPos.x].type >= 0) && (abs(pntDirAngles_h[ptIdx][seedPos.x].corner) < pntCornenrMax)
			&& (z_diff > minZHeight))
		{
			bottom = ptIdx;
			break;
		}
	}
	if ((left < 0) || (right < 0) || (top < 0) || (bottom < 0))
		return false;
	roi.left = left;
	roi.right = right;
	roi.top = top;
	roi.bottom = bottom;
	return true;
}

SVzNLRect _mergeROI(SVzNLRect roi_1, SVzNLRect roi_2)
{
	SVzNLRect roi;
	roi.left = roi_1.left < roi_2.left ? roi_1.left : roi_2.left;
	roi.right = roi_1.right > roi_2.right ? roi_1.right : roi_2.right;
	roi.top = roi_1.top < roi_2.top ? roi_1.top : roi_2.top;
	roi.bottom = roi_1.bottom > roi_2.bottom ? roi_1.bottom : roi_2.bottom;
	return roi;
}

//单个孔或凹坑定位-华航孔定位
void wd_HolePositioning(
	std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
	const SSG_lineSegParam lineSegPara,
	const SSG_cornerParam cornerParam,
	const SSG_outlierFilterParam filterParam,
	const SSG_treeGrowParam growParam,
	std::vector< WD_HolePositionInfo>& holePositioning,
	int* errCode)
{
	*errCode = 0;

	int lineNum = (int)scanLinesInput.size();
	std::vector< std::vector<SVzNL3DPosition>> scanLines;
	scanLines.resize(lineNum);
	int linePtNum = (int)scanLinesInput[0].size();
	bool isGridData = true;
	for (int i = 0; i < lineNum; i++)
	{
		if (linePtNum != (int)scanLinesInput[i].size())
			isGridData = false;

		scanLines[i].resize(scanLinesInput[i].size());
		std::copy(scanLinesInput[i].begin(), scanLinesInput[i].end(), scanLines[i].begin()); // 使用std::copy算法

		for (int j = 0; j < (int)scanLinesInput[i].size(); j++)
			scanLinesInput[i][j].nPointIdx = 0;  //清零，用于debug时记录信息
	}
	if (false == isGridData)//数据不是网格格式
	{
		*errCode = SG_ERR_NOT_GRID_FORMAT;
		return;
	}

	//添加3x3平滑
	scanLinesSmooting3x3(scanLinesInput, scanLines);

	//内部参数
	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;
	std::vector<std::vector<SSG_basicFeature1D>> zMaxPeaks_v;
	for (int line = 0; line < lineNum; line++)
	{
		if (line == 1062)
			int kkk = 1;

		std::vector<SVzNL3DPosition>& lineData = scanLines[line];
		//滤波，滤除异常点
		//sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);

		std::vector< SSG_pntDirAngle> line_ptDirAngles;
		wd_computeDirAngle_wholeLine(lineData, cornerParam, line_ptDirAngles);
		pntDirAngles_v.push_back(line_ptDirAngles);

		std::vector< SSG_basicFeature1D> localZMax;
		std::vector< SSG_basicFeature1D> localZMin;
		sg_getLineLocalPeaks_2( lineData, line,	zPeakScale,	localZMax, localZMin);
		zMaxPeaks_v.push_back(localZMax);
	}

	//生成水平扫描
	std::vector<std::vector<SVzNL3DPosition>> hLines_raw;
	hLines_raw.resize(linePtNum);
	for (int i = 0; i < linePtNum; i++)
		hLines_raw[i].resize(lineNum);
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
			hLines_raw[j][line] = scanLines[line][j];
			hLines_raw[j][line].pt3D.x = scanLines[line][j].pt3D.y;
			hLines_raw[j][line].pt3D.y = scanLines[line][j].pt3D.x;
		}
	}

	//水平arc特征提取
	//创建水平ZMax的Mask
	std::vector<std::vector<int>> zMaxMask_h;
	zMaxMask_h.resize(lineNum);
	for (int i = 0; i < lineNum; i++)
	{
		zMaxMask_h[i].resize(linePtNum);
		std::fill(zMaxMask_h[i].begin(), zMaxMask_h[i].end(), 0);
	}

	std::vector<std::vector<SSG_pntDirAngle>> pntDirAngles_h;
	for (int line = 0; line < linePtNum; line++)
	{
		if (line == 234)
			int kkk = 1;
		std::vector<SVzNL3DPosition>& lineData = hLines_raw[line];
		//滤波，滤除异常点
		int ptNum = (int)lineData.size();
		//sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, filterParam);

		std::vector< SSG_pntDirAngle> line_ptDirAngles;
		wd_computeDirAngle_wholeLine(lineData, cornerParam, line_ptDirAngles);
		pntDirAngles_h.push_back(line_ptDirAngles);

		std::vector< SSG_basicFeature1D> localZMax;
		std::vector< SSG_basicFeature1D> localZMin;
		sg_getLineLocalPeaks_2(lineData, line, zPeakScale, localZMax, localZMin);
		for (int j = 0; j < (int)localZMax.size(); j++)
		{
			int idx_line = localZMax[j].jumpPos2D.y;
			int idx_pt = localZMax[j].jumpPos2D.x;
			zMaxMask_h[idx_line][idx_pt] = 1;
		}
	}

	//判断真正的ZMax并记录
	//（1）真正的ZMax一定是同时为X方向的ZMax和Y方向的ZMax.(2)满足一定的Z差值
	std::vector<_zMaxInfo> objPeaks;
	for (int line = 0; line < lineNum; line++)
	{
		if (line == 144)
			int kkk = 1;
		std::vector<SSG_basicFeature1D>& a_lineZMax = zMaxPeaks_v[line];
		for (int j = 0; j < (int)a_lineZMax.size(); j++)
		{
			int idx_line = a_lineZMax[j].jumpPos2D.x;
			int idx_pt = a_lineZMax[j].jumpPos2D.y;
			if (zMaxMask_h[idx_line][idx_pt] > 0)  //在X和Y方向均为极大值
			{
				//判断ROI
				SVzNLRect a_roi;
				bool validPk = _getZMaxPeakROI(a_lineZMax[j].jumpPos2D, pntDirAngles_v, pntDirAngles_h, flagCornerMax, minZPeakHeight, a_roi);
				if (true == validPk)
				{
					double meanZ = scanLines[a_roi.left][idx_pt].pt3D.z;
					double minZ = scanLines[a_roi.left][idx_pt].pt3D.z;
					double maxZ = scanLines[a_roi.left][idx_pt].pt3D.z;
					meanZ += scanLines[a_roi.right][idx_pt].pt3D.z;
					minZ = minZ < scanLines[a_roi.right][idx_pt].pt3D.z ? minZ : scanLines[a_roi.right][idx_pt].pt3D.z;
					maxZ = maxZ > scanLines[a_roi.right][idx_pt].pt3D.z ? maxZ : scanLines[a_roi.right][idx_pt].pt3D.z;
					meanZ += scanLines[idx_line][a_roi.top].pt3D.z;
					minZ = minZ < scanLines[idx_line][a_roi.top].pt3D.z ? minZ : scanLines[idx_line][a_roi.top].pt3D.z;
					maxZ = maxZ > scanLines[idx_line][a_roi.top].pt3D.z ? maxZ : scanLines[idx_line][a_roi.top].pt3D.z;
					meanZ += scanLines[idx_line][a_roi.bottom].pt3D.z;
					minZ = minZ < scanLines[idx_line][a_roi.bottom].pt3D.z ? minZ : scanLines[idx_line][a_roi.bottom].pt3D.z;
					maxZ = maxZ > scanLines[idx_line][a_roi.bottom].pt3D.z ? maxZ : scanLines[idx_line][a_roi.bottom].pt3D.z;
					double zMaxMin = maxZ - minZ;
					meanZ = meanZ / 4.0;
					double z_diff = a_lineZMax[j].jumpPos.z - meanZ;

					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) && (zMaxMin < planeInlierDistTh*2))
					{
						_zMaxInfo a_info;
						a_info.flag = 0;
						a_info.peakROI = a_roi;
						a_info.zMaxPos.lineIdx = a_lineZMax[j].jumpPos2D.x;
						a_info.zMaxPos.ptIdx = a_lineZMax[j].jumpPos2D.y;
						a_info.zMaxPos.point = { a_lineZMax[j].jumpPos.x, a_lineZMax[j].jumpPos.y, a_lineZMax[j].jumpPos.z };
						a_info.pkValue = a_lineZMax[j].jumpPos.z;
						objPeaks.push_back(a_info);
					}
				}
			}
		}
	}
	int pkSize = (int)objPeaks.size();
	for (int i = 0; i < pkSize; i++)
	{
		_zMaxInfo a_info = objPeaks[i];  
		if (a_info.flag < 0)
			continue;

		for (int j = i + 1; j < pkSize; j++)
		{
			if (objPeaks[j].flag < 0)
				continue;

			if ((a_info.peakROI.right >= objPeaks[j].peakROI.left) && (objPeaks[j].peakROI.right >= a_info.peakROI.left) &&
				(a_info.peakROI.bottom >= objPeaks[j].peakROI.top) && (objPeaks[j].peakROI.bottom >= a_info.peakROI.top))  //重叠
			{
				a_info.peakROI = _mergeROI(a_info.peakROI, objPeaks[j].peakROI);
				if (a_info.pkValue < objPeaks[j].pkValue)
				{
					a_info.pkValue = objPeaks[j].pkValue;
					a_info.zMaxPos = objPeaks[j].zMaxPos;
				}
				objPeaks[j].flag = -1;
			}
		}
		objPeaks[i] = a_info;
	}
	//逐个目标提取
	int contourWin = 4; //取周围3行和3列的点
	for (int peakIdx = 0; peakIdx < pkSize; peakIdx++)
	{
		if (objPeaks[peakIdx].flag < 0)
			continue;

		SVzNLRect& a_roi = objPeaks[peakIdx].peakROI;
		//提取孔周围点
		SVzNLRect extend_roi = { a_roi.left - contourWin, a_roi.right + contourWin, a_roi.top - contourWin , a_roi.bottom + contourWin };
		if (extend_roi.left < 0)	extend_roi.left = 0;
		if (extend_roi.right >= lineNum)	extend_roi.right = lineNum - 1;
		if (extend_roi.top < 0) extend_roi.top = 0;
		if (extend_roi.bottom >= linePtNum) extend_roi.bottom = linePtNum - 1;

		std::vector<cv::Point3d> Points3ds;
		//左
		for (int line = extend_roi.left; line <= a_roi.left-1; line++)
		{
			for (int j = a_roi.top; j <= a_roi.bottom; j++)
			{
				if (scanLines[line][j].pt3D.z > 1e-4)
				{
					cv::Point3d a_pt = cv::Point3d(scanLines[line][j].pt3D.x , scanLines[line][j].pt3D.y, scanLines[line][j].pt3D.z);
					Points3ds.push_back(a_pt);
				}
			}
		}
		//右
		for (int line = a_roi.right+1; line <=extend_roi.right; line++)
		{
			for (int j = a_roi.top; j <= a_roi.bottom; j++)
			{
				if (scanLines[line][j].pt3D.z > 1e-4)
				{
					cv::Point3d a_pt = cv::Point3d(scanLines[line][j].pt3D.x, scanLines[line][j].pt3D.y, scanLines[line][j].pt3D.z);
					Points3ds.push_back(a_pt);
				}
			}
		}
		//上
		for (int ptIdx = extend_roi.top; ptIdx <= a_roi.top-1; ptIdx++)
		{
			for (int j = a_roi.left; j <= a_roi.right; j++)
			{
				if (scanLines[j][ptIdx].pt3D.z > 1e-4)
				{
					cv::Point3d a_pt = cv::Point3d(scanLines[j][ptIdx].pt3D.x, scanLines[j][ptIdx].pt3D.y, scanLines[j][ptIdx].pt3D.z);
					Points3ds.push_back(a_pt);
				}
			}
		}
		//下
		for (int ptIdx = a_roi.bottom+1; ptIdx < extend_roi.bottom; ptIdx++)
		{
			for (int j = a_roi.left; j <= a_roi.right; j++)
			{
				if (scanLines[j][ptIdx].pt3D.z > 1e-4)
				{
					cv::Point3d a_pt = cv::Point3d(scanLines[j][ptIdx].pt3D.x, scanLines[j][ptIdx].pt3D.y, scanLines[j][ptIdx].pt3D.z);
					Points3ds.push_back(a_pt);
				}
			}
		}
		//计算面参数: 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::Point3d> 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;
		double norm_C = res[2] / normValue;
		double norm_D = res[3] / normValue;
		//生成ROI scanLines
		int roi_lineNum = extend_roi.right - extend_roi.left + 1;
		int roi_ptNum = extend_roi.bottom - extend_roi.top + 1;
		std::vector< std::vector<SVzNL3DPosition>> roi_scanLines;
		roi_scanLines.resize(roi_lineNum);
		for (int j = 0; j < roi_lineNum; j++)
			roi_scanLines[j].resize(roi_ptNum);
		
		//生成面上点
		for (int line = extend_roi.left; line <= extend_roi.right; line++)
		{
			int roi_line = line - extend_roi.left;
			for (int ptIdx = extend_roi.top; ptIdx <= extend_roi.bottom; ptIdx++)
			{
				int roi_ptIdx = ptIdx - extend_roi.top;
				roi_scanLines[roi_line][roi_ptIdx] = scanLines[line][ptIdx];
				//最外圈不进行处理
				if ((line != extend_roi.left) && (line != extend_roi.right) && (ptIdx != extend_roi.top) && (ptIdx != extend_roi.bottom))
				{
					if (scanLines[line][ptIdx].pt3D.z > 1e-4)
					{
						SVzNL3DPoint& a_pt3D = scanLines[line][ptIdx].pt3D;
						double dist = abs(norm_A * a_pt3D.x + norm_B * a_pt3D.y + norm_C * a_pt3D.z + norm_D);
						if (dist > planeInlierDistTh)
							roi_scanLines[roi_line][roi_ptIdx].pt3D = { 0, 0, 0 };
					}
				}
			}
		}

		std::vector<SWD_segFeatureTree> segTrees_v;
		std::vector<SWD_segFeatureTree> segTrees_h;
		std::vector<SSG_intPair> validObjects;
		WD_getHoleInfo(roi_scanLines, lineSegPara, filterParam, growParam, valieCommonNumRatio, segTrees_v, segTrees_h, validObjects);
		if (validObjects.size() > 0)
		{
			SSG_intPair a_hvPair = validObjects[0];
			if (validObjects.size() > 1)
			{
				for (int j = 1; j < (int)validObjects.size(); j++)
				{
					if (a_hvPair.idx < validObjects[j].idx)
						a_hvPair = validObjects[j];
				}
			}
			//生成Contour
			std::vector<SVzNL3DPoint> cluster_pointArray;
			int vTreeIdx = a_hvPair.data_0;
			int hTreeIdx = a_hvPair.data_1;
			for (int m = 0; m < (int)segTrees_v[vTreeIdx].treeNodes.size(); m++)
			{
				SWD_segFeature& a_seg = segTrees_v[vTreeIdx].treeNodes[m];
				if (roi_scanLines[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx == 0)
				{
					roi_scanLines[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = vTreeIdx + 1; // 0x01;
					SVzNL3DPoint a_pos = roi_scanLines[a_seg.lineIdx][a_seg.endPtIdx].pt3D;
					cluster_pointArray.push_back(a_pos);
				}
				if (roi_scanLines[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx == 0)
				{
					roi_scanLines[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = vTreeIdx + 1; // 0x01;
					SVzNL3DPoint a_pos = roi_scanLines[a_seg.lineIdx][a_seg.startPtIdx].pt3D;
					cluster_pointArray.push_back(a_pos);
				}
			}
			for (int m = 0; m < (int)segTrees_h[hTreeIdx].treeNodes.size(); m++)
			{
				SWD_segFeature& a_seg = segTrees_h[hTreeIdx].treeNodes[m];
				if (roi_scanLines[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx == 0)
				{
					roi_scanLines[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02;
					SVzNL3DPoint a_pos = roi_scanLines[a_seg.startPtIdx][a_seg.lineIdx].pt3D;
					cluster_pointArray.push_back(a_pos);
				}
				if (roi_scanLines[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx == 0)
				{
					roi_scanLines[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02;
					SVzNL3DPoint a_pos = roi_scanLines[a_seg.endPtIdx][a_seg.lineIdx].pt3D;
					cluster_pointArray.push_back(a_pos);
				}
			}
			//计算重心
#if 0
			SVzNL3DPoint center;
			double radius;
			double err = fitCircleByLeastSquare(cluster_pointArray, center, radius);
#endif
			if (cluster_pointArray.size() > 0)
			{
				SVzNL3DPoint center = { 0, 0, 0 };
				int ptSize = (int)cluster_pointArray.size();
				for (int m = 0; m < ptSize; m++)
				{
					center.x += cluster_pointArray[m].x;
					center.y += cluster_pointArray[m].y;
					center.z += cluster_pointArray[m].z;
				}
				center.x = center.x / ptSize;
				center.y = center.y / ptSize;
				center.z = center.z / ptSize;
				//求在平面上的垂足
				double t = -(center.x * norm_A + center.y * norm_B + center.z * norm_C + norm_D);
				SVzNL3DPoint realCenter;
				realCenter.x = center.x + t * norm_A;
				realCenter.y = center.y + t * norm_B;
				realCenter.z = center.z + t * norm_C;
				WD_HolePositionInfo a_hole;
				a_hole.center = realCenter;
				a_hole.normDir = { norm_A, norm_B, norm_C };
				holePositioning.push_back(a_hole);
			}
		}

	}
	return;
}

SSG_ROIRectD _getListROI(std::vector< SVzNL3DPosition>& listData)
{
	if (listData.size() == 0)
		return { 0,0,0,0 };
	SSG_ROIRectD roi = { listData[0].pt3D.x, listData[0].pt3D.x, listData[0].pt3D.y, listData[0].pt3D.y };
	for (int i = 0; i < (int)listData.size(); i++)
	{
		roi.left = roi.left > listData[i].pt3D.x ? listData[i].pt3D.x : roi.left;
		roi.right = roi.right < listData[i].pt3D.x ? listData[i].pt3D.x : roi.right;
		roi.top = roi.top > listData[i].pt3D.y ? listData[i].pt3D.y : roi.top;
		roi.bottom = roi.bottom < listData[i].pt3D.y ? listData[i].pt3D.y : roi.bottom;
	}
	return roi;
}

double _getListMeanZ(std::vector< SVzNL3DPosition>& listData, SVzNLRangeD& zRange)
{
	if (listData.size() == 0)
		return 0;
	double meanZ = 0;
	zRange.max = -1;
	zRange.min = 0;
	for (int i = 0; i < (int)listData.size(); i++)
	{
		meanZ += listData[i].pt3D.z;
		if (zRange.max < 0)
		{
			zRange.max = listData[i].pt3D.z;
			zRange.min = listData[i].pt3D.z;
		}
		else
		{
			zRange.max = zRange.max < listData[i].pt3D.z ? listData[i].pt3D.z : zRange.max;
			zRange.min = zRange.min > listData[i].pt3D.z ? listData[i].pt3D.z : zRange.min;
		}
	}
	meanZ = meanZ / (double)listData.size();
	return meanZ;
}

//砂轮盘中心孔定位
WD_HolePositionInfo sx_getDiscHolePose(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SSG_cornerParam cornerPara,
	int* errCode)
{
	*errCode = 0;
	WD_HolePositionInfo resultPose;
	resultPose.center = { 0, 0, 0 };
	resultPose.normDir = { 0, 0, 0 };

	int lineNum = (int)scanLines.size();
	if (lineNum == 0)
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return resultPose;
	}

	int linePtNum = (int)scanLines[0].size();

	//判断数据格式是否为grid。算法只能处理grid数据格式
	bool isGridData = true;
	for (int line = 0; line < lineNum; line++)
	{
		if (linePtNum != (int)scanLines[line].size())
		{
			isGridData = false;
			break;
		}
	}
	if (false == isGridData)//数据不是网格格式
	{
		*errCode = SG_ERR_NOT_GRID_FORMAT;
		return resultPose;
	}

	//产生水平扫描数据
	std::vector< std::vector<SVzNL3DPosition>> scanLines_h;
	scanLines_h.resize(linePtNum);
	for (int i = 0; i < linePtNum; i++)
		scanLines_h[i].resize(lineNum);
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
			scanLines_h[j][line] = scanLines[line][j];
			scanLines_h[j][line].pt3D.x = scanLines[line][j].pt3D.y;
			scanLines_h[j][line].pt3D.y = scanLines[line][j].pt3D.x;
		}
	}
	for (int line = 0; line < linePtNum; line++)
	{
		for (int j = 0, j_max = (int)scanLines_h[line].size(); j < j_max; j++)
			scanLines_h[line][j].nPointIdx = j;
	}

	//算法流程：
	//1、检查垂直方向数据并去除
	//2、聚类
	//3、保留最前面目标
	//4、提取孔
	//5、拟合
	//6、计算中间坐标
	//内部参数
	SSG_cornerParam removeVertialPara = cornerPara;
	removeVertialPara.scale = 3.0;
	removeVertialPara.cornerTh = 60;

	std::vector<std::vector<int>> flags;
	flags.resize(lineNum);
	for (int i = 0; i < lineNum; i++)
	{
		flags[i].resize(linePtNum);
		std::fill(flags[i].begin(), flags[i].end(), 0);
	}
	std::vector<std::vector<int>> zVertivalFlags;
	for (int line = 0; line < lineNum; line++)
	{
		if (line == 700)
			int kkk = 1;
		std::vector<int> line_verticalFlags;
		wd_getXYVertialFeature_dirAngleMethod(
			scanLines[line],
			line,
			removeVertialPara,
			line_verticalFlags
		);
		zVertivalFlags.push_back(line_verticalFlags);

		for (int i = 0; i < (int)line_verticalFlags.size(); i++)
		{
			if (line_verticalFlags[i] > 0)
				flags[line][i] = 1;
		}
	}

	std::vector<std::vector<int>> zVertivalFlags_h;
	for (int line = 0; line < linePtNum; line++)
	{
		if (line == 1177)
			int kkk = 1;
		std::vector<int> line_verticalFlags;
		wd_getXYVertialFeature_dirAngleMethod(
			scanLines_h[line],
			line,
			removeVertialPara,
			line_verticalFlags
		);
		zVertivalFlags_h.push_back(line_verticalFlags);

		for (int i = 0; i < (int)line_verticalFlags.size(); i++)
		{
			if (line_verticalFlags[i] > 0)
				flags[i][line] = 1;
		}
	}
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			if (flags[line][j] > 0)
			{
				scanLines[line][j].pt3D.z = 0;
				scanLines_h[j][line].pt3D.z = 0;
			}
		}
	}
	//迭代一次
	SSG_lineSegParam lineSegPara;
	lineSegPara.distScale = 5.0;
	lineSegPara.segGapTh_y = 5.0;
	lineSegPara.segGapTh_z = 5.0;
	const int minSegLen = 5;
	for (int line = 0; line < lineNum; line++)
	{
		std::vector<SSG_RUN> segs;
		wd_getLineDataIntervals(
			scanLines[line],
			lineSegPara,
			segs);
		for (int i = 0; i < (int)segs.size(); i++)
		{
			if (segs[i].len <= minSegLen)
			{
				int idx0 = segs[i].start;
				for (int j = 0; j < segs[i].len; j++)
					flags[line][idx0 + j] = 1;
			}
		}
	}
	for (int line = 0; line < linePtNum; line++)
	{
		std::vector<SSG_RUN> segs;
		wd_getLineDataIntervals(
			scanLines_h[line],
			lineSegPara,
			segs);
		for (int i = 0; i < (int)segs.size(); i++)
		{
			if (segs[i].len <= minSegLen)
			{
				int idx0 = segs[i].start;
				for (int j = 0; j < segs[i].len; j++)
					flags[idx0 + j][line] = 1;
			}
		}
	}

	//标注
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
			scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
	}
	//将垂直线段去除
	std::vector< SVzNL3DPosition> validPoints;
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			if (flags[line][j] > 0)
				scanLines[line][j].pt3D.z = 0;

			if (scanLines[line][j].pt3D.z > 1e-4)
			{
				SVzNL3DPosition a_vldPt;
				a_vldPt.pt3D = scanLines[line][j].pt3D;
				a_vldPt.nPointIdx = (line << 16) | (j & 0xffff);
				validPoints.push_back(a_vldPt);
			}
		}
	}

	//聚类
	//内部参数
	//double minObjSize_w = 150;
	//double minObjSize_h = 150;

	int clusterCheckWin = 5;
	double clusterDist = 1.5;
	int distType = 1;  //0 - 2d distance; 1- 3d distance 
	std::vector<std::vector< SVzNL3DPosition>> objClusters;  //result
	wd_pointClustering_speedUp(
		validPoints,
		lineNum, linePtNum, clusterCheckWin, //搜索窗口
		clusterDist,
		distType,
		objClusters  //result
	);

	//保留最前面的目标
	const double topPlateMinW = 150.0;
	const double topPlateMinH = 150.0;
	const double topPlateMaxZRange = 100.0;
	std::vector<double> objMeanZ;
	std::vector<SVzNLRangeD> objZRange;
	objMeanZ.resize(objClusters.size());
	objZRange.resize(objClusters.size());
	for (int i = 0; i < (int)objClusters.size(); i++)
	{
		SSG_ROIRectD a_roi = _getListROI(objClusters[i]);
		double w = a_roi.right - a_roi.left;
		double h = a_roi.bottom - a_roi.top;
		if ((w > topPlateMinW) && (h > topPlateMinH))
		{
			SVzNLRangeD zRange;
			double meanZ = _getListMeanZ(objClusters[i], zRange);
			objMeanZ[i] = meanZ;
			objZRange[i] = zRange;
		}
		else
		{
			objMeanZ[i] = 0;
			objZRange[i].max = -1.0;
			objZRange[i].min = 0.0;
		}
	}

	//选出z最小的目标作为顶部轮廓
	int objIdx = -1;
	double minMeanZ = DBL_MAX;
	for (int i = 0; i < (int)objClusters.size(); i++)
	{
		if ((objMeanZ[i] > 1e-4) && (objZRange[i].max > 0))
		{
			double range = objZRange[i].max - objZRange[i].min;
			if (range < topPlateMaxZRange)
			{
				if (minMeanZ > objMeanZ[i])
				{
					minMeanZ = objMeanZ[i];
					objIdx = i;
				}
			}
		}
	}
	if (objIdx < 0)
	{
		*errCode = SG_ERR_ZERO_OBJECTS;
		return resultPose;
	}

	std::vector< SVzNL3DPosition>& topCluster = objClusters[objIdx];
	//标注
	//重新将flags设置为目标的mask
	for (int i = 0; i < lineNum; i++)
		std::fill(flags[i].begin(), flags[i].end(), -1);
	for (int i = 0; i < (int)topCluster.size(); i++)
	{
		int line = topCluster[i].nPointIdx >> 16;
		int ptIdx = topCluster[i].nPointIdx & 0x0000FFFF;
		scanLines[line][ptIdx].nPointIdx = 2;
		flags[line][ptIdx] = i;  //indexing
	}

	//使用PCA方法计算法向量
	SVzNL3DPoint vec_normal, vec_centroid;

	computePlaneNormalByPCA(
		topCluster,
		vec_normal,
		vec_centroid);

	if (vec_normal.z < 0)
		vec_normal = { -vec_normal.x, -vec_normal.y, -vec_normal.z };
	resultPose.center = vec_centroid;
	resultPose.normDir = vec_normal;
	return resultPose;
}

//砂轮盘架子中心定位
SSG_pointPose sx_getDiscRackCenterPosition(
	std::vector< std::vector<SVzNL3DPosition>>& src_scanLines,
	const SSG_cornerParam cornerPara,
	int* errCode)
{
	*errCode = 0;
	SSG_pointPose resultPose;
	resultPose.point = { 0, 0, 0 };
	resultPose.pose_x = { 0, 0, 0 };
	resultPose.pose_y = { 0, 0, 0 };
	resultPose.pose_z = { 0, 0, 0 };

	int lineNum = (int)src_scanLines.size();
	if (lineNum == 0)
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return resultPose;
	}

	int linePtNum = (int)src_scanLines[0].size();

	//判断数据格式是否为grid。算法只能处理grid数据格式
	bool isGridData = true;
	for (int line = 0; line < lineNum; line++)
	{
		if (linePtNum != (int)src_scanLines[line].size())
		{
			isGridData = false;
			break;
		}
	}
	if (false == isGridData)//数据不是网格格式
	{
		*errCode = SG_ERR_NOT_GRID_FORMAT;
		return resultPose;
	}

	//产生扫描数据副本和水平扫描数据
	std::vector< std::vector<SVzNL3DPosition>> scanLines;
	scanLines.resize(lineNum);
	std::vector< std::vector<SVzNL3DPosition>> scanLines_h;
	scanLines_h.resize(linePtNum);
	for (int i = 0; i < linePtNum; i++)
		scanLines_h[i].resize(lineNum);
	for (int line = 0; line < lineNum; line++)
	{
		scanLines[line].insert(scanLines[line].end(), src_scanLines[line].begin(), src_scanLines[line].end());
		for (int j = 0; j < linePtNum; j++)
		{
			scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
			src_scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
			scanLines_h[j][line] = scanLines[line][j];
			scanLines_h[j][line].pt3D.x = scanLines[line][j].pt3D.y;
			scanLines_h[j][line].pt3D.y = scanLines[line][j].pt3D.x;
		}
	}
	for (int line = 0; line < linePtNum; line++)
	{
		for (int j = 0, j_max = (int)scanLines_h[line].size(); j < j_max; j++)
			scanLines_h[line][j].nPointIdx = j;
	}

	//算法流程：
	//1、检查垂直方向数据并去除
	//2、聚类
	//3、保留最前面目标
	//4、提取孔
	//5、拟合
	//6、计算中间坐标
	//内部参数
	SSG_cornerParam removeVertialPara = cornerPara;
	removeVertialPara.scale = 3.0;
	removeVertialPara.cornerTh = 60;

	std::vector<std::vector<int>> flags;
	flags.resize(lineNum);
	for (int i = 0; i < lineNum; i++)
	{
		flags[i].resize(linePtNum);
		std::fill(flags[i].begin(), flags[i].end(), 0);
	}
	std::vector<std::vector<int>> zVertivalFlags;
	for (int line = 0; line < lineNum; line++)
	{
		if (line == 700)
			int kkk = 1;
		std::vector<int> line_verticalFlags;
		wd_getXYVertialFeature_dirAngleMethod(
			scanLines[line],
			line,
			removeVertialPara,
			line_verticalFlags
		);
		zVertivalFlags.push_back(line_verticalFlags);

		for (int i = 0; i < (int)line_verticalFlags.size(); i++)
		{
			if (line_verticalFlags[i] > 0)
				flags[line][i] = 1;
		}
	}

	std::vector<std::vector<int>> zVertivalFlags_h;
	for (int line = 0; line < linePtNum; line++)
	{
		if (line == 1177)
			int kkk = 1;
		std::vector<int> line_verticalFlags;
		wd_getXYVertialFeature_dirAngleMethod(
			scanLines_h[line],
			line,
			removeVertialPara,
			line_verticalFlags
		);
		zVertivalFlags_h.push_back(line_verticalFlags);

		for (int i = 0; i < (int)line_verticalFlags.size(); i++)
		{
			if (line_verticalFlags[i] > 0)
				flags[i][line] = 1;
		}
	}
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			if (flags[line][j] > 0)
			{
				scanLines[line][j].pt3D.z = 0;
				scanLines_h[j][line].pt3D.z = 0;
			}
		}
	}
	//迭代一次
	SSG_lineSegParam lineSegPara;
	lineSegPara.distScale = 5.0;
	lineSegPara.segGapTh_y = 5.0;
	lineSegPara.segGapTh_z = 5.0;
	const int minSegLen = 5;
	for (int line = 0; line < lineNum; line++)
	{
		std::vector<SSG_RUN> segs;
		wd_getLineDataIntervals(
			scanLines[line],
			lineSegPara,
			segs);
		for (int i = 0; i < (int)segs.size(); i++)
		{
			if (segs[i].len <= minSegLen)
			{
				int idx0 = segs[i].start;
				for (int j = 0; j < segs[i].len; j++)
					flags[line][idx0 + j] = 1;
			}
		}
	}
	for (int line = 0; line < linePtNum; line++)
	{
		std::vector<SSG_RUN> segs;
		wd_getLineDataIntervals(
			scanLines_h[line],
			lineSegPara,
			segs);
		for (int i = 0; i < (int)segs.size(); i++)
		{
			if (segs[i].len <= minSegLen)
			{
				int idx0 = segs[i].start;
				for (int j = 0; j < segs[i].len; j++)
					flags[idx0 + j][line] = 1;
			}
		}
	}

	//标注
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
			scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
	}
	//将垂直线段去除
	std::vector< SVzNL3DPosition> validPoints;
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			if (flags[line][j] > 0)
				scanLines[line][j].pt3D.z = 0;

			if (scanLines[line][j].pt3D.z > 1e-4)
			{
				SVzNL3DPosition a_vldPt;
				a_vldPt.pt3D = scanLines[line][j].pt3D;
				a_vldPt.nPointIdx = (line << 16) | (j & 0xffff);
				validPoints.push_back(a_vldPt);
			}
		}
	}

	//聚类
	int clusterCheckWin = 5;
	double clusterDist = 5.0;
	int distType = 1;  //0 - 2d distance; 1- 3d distance 
	std::vector<std::vector< SVzNL3DPosition>> objClusters;  //result
	wd_pointClustering_speedUp(
		validPoints,
		lineNum, linePtNum, clusterCheckWin, //搜索窗口
		clusterDist,
		distType,
		objClusters  //result
	);

	//保留最前面的目标
	const double topPlateMinW = 800.0;
	const double topPlateMinH = 800.0;
	const double topPlateMaxZRange = 100.0;
	std::vector<double> objMeanZ;
	std::vector<SVzNLRangeD> objZRange;
	objMeanZ.resize(objClusters.size());
	objZRange.resize(objClusters.size());
	for (int i = 0; i < (int)objClusters.size(); i++)
	{
		SSG_ROIRectD a_roi = _getListROI(objClusters[i]);
		double w = a_roi.right - a_roi.left;
		double h = a_roi.bottom - a_roi.top;
		if ((w > topPlateMinW) && (h > topPlateMinH))
		{
			SVzNLRangeD zRange;
			double meanZ = _getListMeanZ(objClusters[i], zRange);
			objMeanZ[i] = meanZ;
			objZRange[i] = zRange;
		}
		else
		{
			objMeanZ[i] = 0;
			objZRange[i].max = -1.0;
			objZRange[i].min = 0.0;
		}
	}

	//选出z最小的目标作为顶部轮廓
	int objIdx = -1;
	double minMeanZ = DBL_MAX;
	for (int i = 0; i < (int)objClusters.size(); i++)
	{
		if ((objMeanZ[i] > 1e-4) && (objZRange[i].max > 0))
		{
			double range = objZRange[i].max - objZRange[i].min;
			if (range < topPlateMaxZRange)
			{
				if (minMeanZ > objMeanZ[i])
				{
					minMeanZ = objMeanZ[i];
					objIdx = i;
				}
			}
		}
	}
	if (objIdx < 0)
	{
		*errCode = SG_ERR_ZERO_OBJECTS;
		return resultPose;
	}

	std::vector< SVzNL3DPosition>& topCluster = objClusters[objIdx];
	//标注
	//重新将flags设置为目标的mask
	for (int i = 0; i < lineNum; i++)
		std::fill(flags[i].begin(), flags[i].end(), -1);
	for (int i = 0; i < (int)topCluster.size(); i++)
	{
		int line = topCluster[i].nPointIdx >> 16;
		int ptIdx = topCluster[i].nPointIdx & 0x0000FFFF;
		scanLines[line][ptIdx].nPointIdx = 2;
		flags[line][ptIdx] = i;  //indexing
	}

	//拟合平面(此处不使用PCA方法，原因是PCA是整体优化，噪点会影响结果）
	std::vector<cv::Point3d> Points3ds;
	for (int i = 0; i < (int)objClusters[objIdx].size(); i++)
	{
		cv::Point3d a_pt = cv::Point3d(objClusters[objIdx][i].pt3D.x, objClusters[objIdx][i].pt3D.y, objClusters[objIdx][i].pt3D.z);
		Points3ds.push_back(a_pt);
	}
	//计算面参数: z = Ax + By + C
	//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
	std::vector<cv::Point3d> out_inliers;
	Plane res = ransacFitPlane(Points3ds, out_inliers);
	if (res.C < 0)
	{
		res.A = -res.A;
		res.B = -res.B;
		res.C = -res.C;
		res.D = -res.D;
	}

	//计算投影向量
	SVzNL3DPoint vec_1 = { res.A, res.B, res.C };
	SVzNL3DPoint vec_2 = { 0, 0, 1.0 };
	SSG_planeCalibPara poseR = wd_computeRTMatrix(vec_1, vec_2);

	//投影
	double normDataPlane = sqrt(res.A * res.A + res.B * res.B + res.C * res.C);
	std::vector<SVzNL3DPosition> projectPosition3ds;
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			if (src_scanLines[line][j].pt3D.z > 1e-4)
			{
				SVzNL3DPosition a_vldPt;
				//a_vldPt.pt3D = scanLines[line][j].pt3D;
				a_vldPt.nPointIdx = (line << 16) | (j & 0xffff);
				a_vldPt.pt3D = wd_ptRotate(src_scanLines[line][j].pt3D, poseR.planeCalib);
				projectPosition3ds.push_back(a_vldPt);
			}
		}
	}
	std::vector<SVzNL3DPosition> projectCluster3ds;
	for (int i = 0; i < (int)topCluster.size(); i++)
	{
		SVzNL3DPosition a_pt;
		a_pt.nPointIdx = topCluster[i].nPointIdx;
		a_pt.pt3D = wd_ptRotate(topCluster[i].pt3D, poseR.planeCalib);
		projectCluster3ds.push_back(a_pt);
	}
	//计算meanZ和ROI
	SSG_ROIRectD topCluster_roi2D = _getListROI(topCluster);
	SVzNLRangeD topCluster_zRange;
	double meanZ = _getListMeanZ(topCluster, topCluster_zRange);
	//（1）取Cluster的ROI
	double xw = topCluster_roi2D.right - topCluster_roi2D.left;
	double yw = topCluster_roi2D.bottom - topCluster_roi2D.top;
	double minW = xw < yw ? yw : xw;
	double centerROI_w = (minW / 2) * sqrt(2.0) * 0.9;
	double center_x = (topCluster_roi2D.right + topCluster_roi2D.left) / 2;
	double center_y = (topCluster_roi2D.bottom + topCluster_roi2D.top) / 2;
	//取中央一个小的ROI，去掉边框。这样可以使用PCA方法计算条纹方向
	double sampleROI_L = center_x - centerROI_w / 2;
	double sampleROI_R = center_x + centerROI_w / 2;
	double sampleROI_T = center_y - centerROI_w / 2;
	double sampleROI_B = center_y + centerROI_w / 2;

	std::vector< SVzNL3DPosition> roi3DPoints;
	std::vector< SVzNL2DPoint> roiPointPostions;
	SVzNL3DPoint roiCentroid = { 0.0, 0.0, 0.0 };
	int sumCounter = 0;
	for (int i = 0; i < (int)projectCluster3ds.size(); i++)
	{
		SVzNL3DPosition& a_pt = projectCluster3ds[i];
		if ((a_pt.pt3D.x > sampleROI_L) && (a_pt.pt3D.x < sampleROI_R) &&
			(a_pt.pt3D.y > sampleROI_T) && (a_pt.pt3D.y < sampleROI_B))
		{
			roi3DPoints.push_back(a_pt);

			roiCentroid.x += a_pt.pt3D.x;
			roiCentroid.y += a_pt.pt3D.y;
			roiCentroid.z += a_pt.pt3D.z;
			sumCounter++;

			int line = a_pt.nPointIdx >> 16;
			int ptIdx = a_pt.nPointIdx & 0x0000FFFF;
			SVzNL2DPoint a_position = {line, ptIdx};
			roiPointPostions.push_back(a_position);
		}
	}
	if (sumCounter == 0)
	{
		*errCode = SX_ERR_ZERO_OBJECTS;
		return resultPose;
	}
	roiCentroid.x = roiCentroid.x / sumCounter;
	roiCentroid.y = roiCentroid.y / sumCounter;
	roiCentroid.z = roiCentroid.z / sumCounter;

	//聚类出其中一条格栅
	std::vector<std::vector< SVzNL3DPosition>> roiClusters;  //result
	wd_pointClustering_speedUp(
		roi3DPoints,
		lineNum, linePtNum, clusterCheckWin, //搜索窗口
		clusterDist,
		distType,
		roiClusters  //result
	);
	if (roiClusters.size() == 0)
	{
		*errCode = SX_ERR_ZERO_OBJECTS;
		return resultPose;
	}
	std::vector<SSG_ROIRectD> gridROIs;
	int centralGridIdx = -1;
	double minDist = DBL_MAX;
	for (int i = 0; i < (int)roiClusters.size(); i++)
	{
		SSG_ROIRectD a_roi = _getListROI(roiClusters[i]);
		gridROIs.push_back(a_roi);

		double cx = (a_roi.left + a_roi.right) / 2;
		double cy = (a_roi.top + a_roi.bottom) / 2;
		double dist = sqrt(pow(cx - roiCentroid.x, 2) + pow(cy - roiCentroid.y, 2));
		if (minDist > dist)
		{
			minDist = dist;
			centralGridIdx = i;
		}
	}
	//取中间条纹
	std::vector< SVzNL3DPosition>& centeralCluster = roiClusters[centralGridIdx];
	std::vector< SVzNL2DPointD> roiPoints;
	for (int i = 0; i < (int)centeralCluster.size(); i++)
	{
		SVzNL2DPointD a_pt = { centeralCluster[i].pt3D.x, centeralCluster[i].pt3D.y };
		roiPoints.push_back(a_pt);
	}
	SVzNL2DPointD roi_axis, roi_centroid;
	pca2D(roiPoints, roi_axis, roi_centroid);
	
	//(1)cluster继续生长
	//在未旋转的点云中继续生长（端面可能在去除水平点中被去除）
	SVzNL3DRangeD growingROI;
	growingROI.xRange.min = topCluster_roi2D.left - 100.0;
	growingROI.xRange.max = topCluster_roi2D.right + 100.0;
	growingROI.yRange.min = topCluster_roi2D.top - 100.0;
	growingROI.yRange.max = topCluster_roi2D.bottom + 100.0;
	growingROI.zRange.min = 100.0;
	growingROI.zRange.max = topCluster_zRange.max;
	std::vector< SVzNL3DPosition> added_points;
	clusterDist = 10.0;
	wd_clusterGrowing_speedUp(
		projectPosition3ds,
		projectCluster3ds,
		growingROI,  //聚类范围，用于加速
		lineNum, linePtNum, clusterCheckWin, //搜索窗口
		clusterDist,
		distType, //0 - 2d distance; 1- 3d distance 
		added_points
	);
	
	//标注
	for (int i = 0; i < (int)projectCluster3ds.size(); i++)
	{
		int line = projectCluster3ds[i].nPointIdx >> 16;
		int ptIdx = projectCluster3ds[i].nPointIdx & 0x0000FFFF;
		src_scanLines[line][ptIdx].nPointIdx = 2;
	}
	for (int i = 0; i < (int)roiPointPostions.size(); i++)
	{
		int line = roiPointPostions[i].x;
		int ptIdx = roiPointPostions[i].y;
		src_scanLines[line][ptIdx].nPointIdx = 3;
	}


	//搜索4个角点
	SVzNL2DPointD rotate_axis1 = wd_pt2dRotate(roi_axis, 45);
	double denominator = sqrt(pow(rotate_axis1.x, 2) + pow(rotate_axis1.y, 2));
	rotate_axis1 = { rotate_axis1.x/ denominator , rotate_axis1.y /denominator };

	SVzNL2DPointD rotate_axis2 = wd_pt2dRotate(roi_axis, -45);
	denominator = sqrt(pow(rotate_axis2.x, 2) + pow(rotate_axis2.y, 2));
	rotate_axis2 = { rotate_axis2.x / denominator , rotate_axis2.y / denominator };

	//生成过roi_centroid的两条直线
	//A = dy, B = −dx, C = dx⋅y0​−dy⋅x0
	double line1_A, line1_B, line1_C;
	line1_A = rotate_axis1.y; 
	line1_B = -rotate_axis1.x; 
	line1_C = roi_centroid.y * rotate_axis1.x - roi_centroid.x * rotate_axis1.y;
	double line2_A, line2_B, line2_C;
	line2_A = rotate_axis2.y;
	line2_B = -rotate_axis2.x;
	line2_C = roi_centroid.y * rotate_axis2.x - roi_centroid.x * rotate_axis2.y;
	//寻找四个顶点：距两条直线（line1和line2）距离最大的点
	SVzNL3DPosition corner1, corner2, corner3, corner4;
	double maxDist1 = 0.0, maxDist2 = DBL_MAX, maxDist3 = 0.0, maxDist4 = DBL_MAX;
	for (int i = 0; i < (int)projectCluster3ds.size(); i++)
	{
		SVzNL3DPosition& a_pt = projectCluster3ds[i];
		double dist1 = line1_A * a_pt.pt3D.x + line1_B * a_pt.pt3D.y + line1_C;
		double dist2 = line2_A * a_pt.pt3D.x + line2_B * a_pt.pt3D.y + line2_C;
		if (maxDist1 < dist1)  //距离最大值
		{
			maxDist1 = dist1;
			corner1 = a_pt;
		}
		if (maxDist2 > dist1)  //距离最小值, 负值
		{
			maxDist2 = dist1;
			corner2 = a_pt;
		}
		if (maxDist3 < dist2)  //距离最大值
		{
			maxDist3 = dist2;
			corner3 = a_pt;
		}
		if (maxDist4 > dist2)  //距离最小值, 负值
		{
			maxDist4 = dist2;
			corner4 = a_pt;
		}
	}
	//计算两条对角线
	double crossLine1_A, crossLine1_B, crossLine1_C;
	compute2ptLine(
		corner1.pt3D,
		corner2.pt3D,
		&crossLine1_A, &crossLine1_B, &crossLine1_C);
	double crossLine2_A, crossLine2_B, crossLine2_C;
	compute2ptLine(
		corner3.pt3D,
		corner4.pt3D,
		&crossLine2_A, &crossLine2_B, &crossLine2_C);
	//计算对角线交点
	SVzNL3DPoint rackCenter = computeLineCrossPt_abs(
		crossLine1_A, crossLine1_B, crossLine1_C,
		crossLine2_A, crossLine2_B, crossLine2_C);
	rackCenter.z = meanZ;
	//生成结果
	resultPose.point = rackCenter;
	resultPose.pose_x = { roi_axis.x, roi_axis.y, 0 };
	resultPose.pose_z = { 0.0, 0.0, 1.0 };
	//叉乘出y
	resultPose.pose_y = vec3_cross(resultPose.pose_z, resultPose.pose_x);

	//旋转回原坐标系
	resultPose.point = _ptRotate(resultPose.point, poseR.invRMatrix);
	resultPose.pose_x = _ptRotate(resultPose.pose_x, poseR.invRMatrix);
	resultPose.pose_y = _ptRotate(resultPose.pose_y, poseR.invRMatrix);
	resultPose.pose_z = _ptRotate(resultPose.pose_z, poseR.invRMatrix);
	return resultPose;
}

int _getMaxROI(std::vector<SSG_ROIRectD>& roiList)
{
	if (roiList.size() == 0)
		return -1;

	int maxId = 0;
	double maxSize = (roiList[0].right - roiList[0].left) * (roiList[0].bottom - roiList[0].top);
	for (int i = 1; i < (int)roiList.size(); i++)
	{
		double size = (roiList[i].right - roiList[i].left) * (roiList[i].bottom - roiList[i].top); 
		if (maxSize < size)
		{
			maxId = i;
			maxSize = size;
		}
	}
	return maxId;
}

void _getFlagIntervals_v(
	std::vector<std::vector<int>>& flags,
	int index,
	const int maxSkipping,
	std::vector<SSG_RUN>& segs)
{
	SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx
	int dataSize = (int)flags[index].size();
	for (int i = 0; i < dataSize; i++)
	{
		if (flags[index][i] >=0 )
		{
			if (a_run.len < 0)
			{
				a_run.start = i;
				a_run.len = 1;
				a_run.value = flags[index][i];
			}
			else
			{
				int gap = i - (a_run.start + a_run.len - 1);
				if( (flags[index][i] == a_run.value) && (gap <= maxSkipping))
				{
					a_run.len = i - a_run.start + 1;
				}
				else
				{
					segs.push_back(a_run);

					a_run.start = i;
					a_run.len = 1;
					a_run.value = a_run.value = flags[index][i];
				}
			}
		}
	}
	if (a_run.len > 0)
		segs.push_back(a_run);
}

void _getFlagIntervals_h(
	std::vector<std::vector<int>>& flags,
	int index,
	const int maxSkipping,
	std::vector<SSG_RUN>& segs)
{
	SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx
	int dataSize = (int)flags.size();
	for (int i = 0; i < dataSize; i++)
	{
		if (flags[i][index] >= 0)
		{
			if (a_run.len < 0)
			{
				a_run.start = i;
				a_run.len = 1;
				a_run.value = flags[i][index];
			}
			else
			{
				int gap = i - (a_run.start + a_run.len - 1);
				if ((flags[i][index] == a_run.value) && (gap <= maxSkipping))
				{
					a_run.len = i - a_run.start + 1;
				}
				else
				{
					segs.push_back(a_run);

					a_run.start = i;
					a_run.len = 1;
					a_run.value = a_run.value = flags[i][index];
				}
			}
		}
	}
	if (a_run.len > 0)
		segs.push_back(a_run);
}

void _computeAnlgeScanData(
	std::vector< SVzNL3DPosition>& clusterPoints,
	const double angleScale,
	const SVzNL3DPoint& centerPoint,
	std::vector<std::vector<SWD_polarPt>>& polarScanData)
{
	int dataSize = (int)clusterPoints.size();
	for (int i = 0; i < dataSize; i++)
	{
		int line = clusterPoints[i].nPointIdx >> 16;
		int ptIdx = clusterPoints[i].nPointIdx & 0x0000FFFF;
		SVzNL3DPoint& a_pt = clusterPoints[i].pt3D;

		double angle = atan2(a_pt.y - centerPoint.y, a_pt.x - centerPoint.x);
		angle = (angle / PI) * 180 + 180.0;
		double R = sqrt(pow(a_pt.y - centerPoint.y, 2) + pow(a_pt.x - centerPoint.x, 2));

		int angleLine = (int)(angle / angleScale + 0.5);
		angleLine = angleLine % (int)polarScanData.size();

		SWD_polarPt a_polarPt;
		a_polarPt.lineIdx = line;
		a_polarPt.ptIdx = ptIdx;
		a_polarPt.x = a_pt.x;
		a_polarPt.y = a_pt.y;
		a_polarPt.z = a_pt.z;
		a_polarPt.R = R;
		a_polarPt.angle = angle;
		polarScanData[angleLine].push_back(a_polarPt);
	}
}
bool compareByPolarScanR(const SWD_polarPt& a, const SWD_polarPt& b) {
	return a.R < b.R;
}
bool _compareByTireZValue(WD_HolePositionInfo& a, WD_HolePositionInfo& b)
{
	return a.center.z < b.center.z;
}
//山东本事机电轮胎中心定位
 void sx_getTireHolePose(
	 std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
	 const SSG_cornerParam cornerPara,
	 const SSG_planeCalibPara groundCalibPara, 
	 const WD_tireParam tireParam, 
	 std::vector<WD_HolePositionInfo>& tirePositions,
	 int* errCode)
{
	*errCode = 0;

	//生成数据副本，使用副本数据进行调平和后续处理
	int lineNum = (int)scanLinesInput.size();
	std::vector< std::vector<SVzNL3DPosition>> scanLines;
	scanLines.resize(lineNum);
	int linePtNum = (int)scanLinesInput[0].size();
	bool isGridData = true;
	for (int i = 0; i < lineNum; i++)
	{
		if (linePtNum != (int)scanLinesInput[i].size())
			isGridData = false;

		scanLines[i].resize(scanLinesInput[i].size());
		std::copy(scanLinesInput[i].begin(), scanLinesInput[i].end(), scanLines[i].begin()); // 使用std::copy算法

		for (int j = 0; j < (int)scanLinesInput[i].size(); j++)
			scanLinesInput[i][j].nPointIdx = 0;  //清零，用于debug时记录信息
	}
	if (false == isGridData)//数据不是网格格式
	{
		*errCode = SG_ERR_NOT_GRID_FORMAT;
		return;
	}

	double removeGroundHeight = groundCalibPara.planeHeight - tireParam.thickness * 0.1;
	for (int i = 0; i < lineNum; i++)
	{																					 //行处理
		//调平，去除地面
		wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, removeGroundHeight);
	}

	//产生水平扫描数据
	std::vector< std::vector<SVzNL3DPosition>> scanLines_h;
	scanLines_h.resize(linePtNum);
	for (int i = 0; i < linePtNum; i++)
		scanLines_h[i].resize(lineNum);
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
			scanLines_h[j][line] = scanLines[line][j];
			scanLines_h[j][line].pt3D.x = scanLines[line][j].pt3D.y;
			scanLines_h[j][line].pt3D.y = scanLines[line][j].pt3D.x;
		}
	}
	for (int line = 0; line < linePtNum; line++)
	{
		for (int j = 0, j_max = (int)scanLines_h[line].size(); j < j_max; j++)
			scanLines_h[line][j].nPointIdx = j;
	}

	//算法流程：
	//1、检查垂直方向数据并去除
	//2、聚类
	//3、保留最前面目标
	//4、提取孔
	//5、拟合
	//6、计算中间坐标
	//内部参数
	SSG_cornerParam removeVertialPara = cornerPara;
	removeVertialPara.scale = 10.0;
	removeVertialPara.cornerTh = 60;

	std::vector<std::vector<int>> flags;
	flags.resize(lineNum);
	for (int i = 0; i < lineNum; i++)
	{
		flags[i].resize(linePtNum);
		std::fill(flags[i].begin(), flags[i].end(), 0);
	}
	std::vector<std::vector<int>> zVertivalFlags;
	for (int line = 0; line < lineNum; line++)
	{
		if (line == 700)
			int kkk = 1;
		std::vector<int> line_verticalFlags;
		wd_getXYVertialFeature_dirAngleMethod(
			scanLines[line],
			line,
			removeVertialPara,
			line_verticalFlags
		);
		zVertivalFlags.push_back(line_verticalFlags);

		for (int i = 0; i < (int)line_verticalFlags.size(); i++)
		{
			if (line_verticalFlags[i] > 0)
				flags[line][i] = 1;
		}
	}

	std::vector<std::vector<int>> zVertivalFlags_h;
	for (int line = 0; line < linePtNum; line++)
	{
		if (line == 1177)
			int kkk = 1;
		std::vector<int> line_verticalFlags;
		wd_getXYVertialFeature_dirAngleMethod(
			scanLines_h[line],
			line,
			removeVertialPara,
			line_verticalFlags
		);
		zVertivalFlags_h.push_back(line_verticalFlags);

		for (int i = 0; i < (int)line_verticalFlags.size(); i++)
		{
			if (line_verticalFlags[i] > 0)
				flags[i][line] = 1;
		}
	}
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			if (flags[line][j] > 0)
			{
				scanLines[line][j].pt3D.z = 0;
				scanLines_h[j][line].pt3D.z = 0;
			}
		}
	}
	//迭代一次
	SSG_lineSegParam lineSegPara;
	lineSegPara.distScale = 5.0;
	lineSegPara.segGapTh_y = 5.0;
	lineSegPara.segGapTh_z = 5.0;
	const int minSegLen = 5;
	for (int line = 0; line < lineNum; line++)
	{
		std::vector<SSG_RUN> segs;
		wd_getLineDataIntervals(
			scanLines[line],
			lineSegPara,
			segs);
		for (int i = 0; i < (int)segs.size(); i++)
		{
			if (segs[i].len <= minSegLen)
			{
				int idx0 = segs[i].start;
				for (int j = 0; j < segs[i].len; j++)
					flags[line][idx0 + j] = 1;
			}
		}
	}
	for (int line = 0; line < linePtNum; line++)
	{
		std::vector<SSG_RUN> segs;
		wd_getLineDataIntervals(
			scanLines_h[line],
			lineSegPara,
			segs);
		for (int i = 0; i < (int)segs.size(); i++)
		{
			if (segs[i].len <= minSegLen)
			{
				int idx0 = segs[i].start;
				for (int j = 0; j < segs[i].len; j++)
					flags[idx0 + j][line] = 1;
			}
		}
	}

	//标注
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			scanLinesInput[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
			scanLines[line][j].nPointIdx = 0;
		}
	}
	//将垂直线段去除
	std::vector< SVzNL3DPosition> validPoints;
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			if (flags[line][j] > 0)
				scanLines[line][j].pt3D.z = 0;

			if (scanLines[line][j].pt3D.z > 1e-4)
			{
				SVzNL3DPosition a_vldPt;
				a_vldPt.pt3D = scanLines[line][j].pt3D;
				a_vldPt.nPointIdx = (line << 16) | (j & 0xffff);
				validPoints.push_back(a_vldPt);
			}
		}
	}

	//聚类
	int clusterCheckWin = 5;
	double clusterDist = 5.0;
	int distType = 1;  //0 - 2d distance; 1- 3d distance 
	std::vector<std::vector< SVzNL3DPosition>> objClusters;  //result
	wd_pointClustering_speedUp(
		validPoints,
		lineNum, linePtNum, clusterCheckWin, //搜索窗口
		clusterDist,
		distType,
		objClusters  //result
	);

	//保留最前面的目标
	double minObjSize = tireParam.diameter * 0.8;
	double maxObjSize = tireParam.diameter * 1.5;
	const double topPlateMaxZRange = 100.0;
	std::vector<double> objMeanZ;
	std::vector<SSG_ROIRectD> objROIs;
	std::vector<SVzNLRangeD> objZRange;
	objMeanZ.resize(objClusters.size());
	objROIs.resize(objClusters.size());
	objZRange.resize(objClusters.size());
	std::vector<int> validObjIndice;
	for (int i = 0; i < (int)objClusters.size(); i++)
	{
		SSG_ROIRectD a_roi = _getListROI(objClusters[i]);
		objROIs[i] = a_roi;
		double w = a_roi.right - a_roi.left;
		double h = a_roi.bottom - a_roi.top;
		if ( (w > minObjSize) && (h > minObjSize) && (w < maxObjSize) && (h < maxObjSize))
		{
			SVzNLRangeD zRange;
			double meanZ = _getListMeanZ(objClusters[i], zRange);
			objMeanZ[i] = meanZ;
			objZRange[i] = zRange;
			validObjIndice.push_back(i);
		}
		else
		{
			objMeanZ[i] = 0;
			objZRange[i].max = -1.0;
			objZRange[i].min = 0.0;
		}
	}

	if (validObjIndice.size() == 0)
	{
		*errCode = SG_ERR_ZERO_OBJECTS;
		return;
	}

	//标注
	//重新将flags设置为目标的mask
	for (int i = 0; i < lineNum; i++)
		std::fill(flags[i].begin(), flags[i].end(), -1);
	for (int m = 0; m < (int)validObjIndice.size(); m++)
	{
		int objIdx = validObjIndice[m];
		for (int i = 0; i < (int)objClusters[objIdx].size(); i++)
		{
			int line = objClusters[objIdx][i].nPointIdx >> 16;
			int ptIdx = objClusters[objIdx][i].nPointIdx & 0x0000FFFF;
			scanLinesInput[line][ptIdx].nPointIdx = 2;
			flags[line][ptIdx] = m;  //indexing
		}
	}
	//提取轮廓点
	
#if 1
	//按照圆扫描获取轮廓点
	//计算质心
	for (int m = 0; m < (int)validObjIndice.size(); m++)
	{
		int objIdx = validObjIndice[m];
		SVzNL3DPoint centroid = { 0, 0, 0 };
		int cluster_size = (int)objClusters[objIdx].size();
		for (int i = 0; i < cluster_size; i++)
		{
			int line = objClusters[objIdx][i].nPointIdx >> 16;
			int ptIdx = objClusters[objIdx][i].nPointIdx & 0x0000FFFF;
			centroid.x += scanLinesInput[line][ptIdx].pt3D.x;
			centroid.y += scanLinesInput[line][ptIdx].pt3D.y;
			centroid.z += scanLinesInput[line][ptIdx].pt3D.z;
		}
		centroid.x = centroid.x / cluster_size;
		centroid.y = centroid.y / cluster_size;
		centroid.z = centroid.z / cluster_size;

		//圆周扫描:0.25度间隔
		double angleScale = 1.0;
		std::vector<std::vector<SWD_polarPt>> polarScanData;
		int polarLines = (int)(360.0 / angleScale + 0.5);
		polarScanData.resize(polarLines);
		_computeAnlgeScanData(
			objClusters[objIdx],
			angleScale,
			centroid,
			polarScanData);

		//每个扫描角度按R从小到大排序
		for (int i = 0; i < polarLines; i++)
		{
			std::sort(polarScanData[i].begin(), polarScanData[i].end(), compareByPolarScanR);
		}

		//提取轮廓
		std::vector< SWD_polarPt> contourPoints;
		std::vector< SVzNL3DPosition> fittingPoints;
		for (int i = 0; i < polarLines; i++)
		{
			if (polarScanData[i].size() == 0)
				continue;

			contourPoints.push_back(polarScanData[i][0]);

			SVzNL3DPosition a_pt;
			a_pt.nPointIdx = 0;
			a_pt.pt3D = { polarScanData[i][0].x, polarScanData[i][0].y, polarScanData[i][0].z};
			fittingPoints.push_back(a_pt);
		}

		//标记
		for (int m = 0; m < (int)contourPoints.size(); m++)
		{
			int line = contourPoints[m].lineIdx;
			int ptIdx = contourPoints[m].ptIdx;
			scanLinesInput[line][ptIdx].nPointIdx = 3;
		}

		//提取中心点
		SVzNLRangeD holezRange;
		double meanZ = _getListMeanZ(fittingPoints, holezRange);
		//拟合圆心
		//圆最小二乘拟合
		SVzNL3DPoint center = { 0.0, 0.0, 0.0 };
		double radius = 0;
#if  0
		double errValue = fitCircleByLeastSquare_2(fittingPoints, center, radius);
#else
		double errValue = fitCircleRobust(fittingPoints, center, radius);
#endif
		if ((radius > tireParam.diameter * 0.15) && (radius < tireParam.diameter * 0.75))
		{
			WD_HolePositionInfo a_holeInfo;
			a_holeInfo.center = { center.x, center.y, meanZ };
			a_holeInfo.normDir = { 0.0, 0.0, 1.0 };
			a_holeInfo.holeR = radius;
			tirePositions.push_back(a_holeInfo);
		}
	}

#else
	int maxSkipping = 3;  //间隔小于3的空白视为连续，消除扫描拉条子的影响
	std::vector<std::vector<int>> contourMasks;
	contourMasks.resize(lineNum);
	for (int i = 0; i < lineNum; i++)
	{
		contourMasks[i].resize(linePtNum);
		std::fill(contourMasks[i].begin(), contourMasks[i].end(), -1);
	}
	//垂直方向
	for (int line = 0; line < lineNum; line++)
	{
		std::vector<SSG_RUN> flagSegs;
		_getFlagIntervals_v(
			flags,
			line,
			maxSkipping,
			flagSegs);
		for (int j = 0; j < (int)flagSegs.size(); j++)
		{
			int idx1 = flagSegs[j].start;
			int idx2 = flagSegs[j].start + flagSegs[j].len - 1;
			contourMasks[line][idx1] = flagSegs[j].value;
			contourMasks[line][idx2] = flagSegs[j].value;
		}
	}
	//水平方向
	for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++)
	{
		std::vector<SSG_RUN> flagSegs;
		_getFlagIntervals_h(
			flags,
			ptIdx,
			maxSkipping,
			flagSegs);
		for (int j = 0; j < (int)flagSegs.size(); j++)
		{
			int idx1 = flagSegs[j].start;
			int idx2 = flagSegs[j].start + flagSegs[j].len - 1;
			contourMasks[idx1][ptIdx] = flagSegs[j].value;
			contourMasks[idx2][ptIdx] = flagSegs[j].value;
		}
	}
	std::vector<std::vector< SVzNL3DPosition>> contourPoints;
	contourPoints.resize(validObjIndice.size());
	for (int line = 0; line < lineNum; line++)
	{
		for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++)
		{
			if (contourMasks[line][ptIdx] >= 0)
			{
				int cIdx = contourMasks[line][ptIdx];
				SVzNL3DPosition a_vldPt;
				a_vldPt.pt3D = scanLines[line][ptIdx].pt3D;
				a_vldPt.nPointIdx = (line << 16) | (ptIdx & 0xffff);
				contourPoints[cIdx].push_back(a_vldPt);
			}
		}
	}
	//标记
	for (int m = 0; m < (int)contourPoints.size(); m++)
	{
		for (int j = 0; j < contourPoints[m].size(); j++)
		{
			int line = contourPoints[m][j].nPointIdx >> 16;
			int ptIdx = contourPoints[m][j].nPointIdx & 0x0000FFFF;
			scanLinesInput[line][ptIdx].nPointIdx = m + 3;
		}
	}

	//逐个计算目标姿态
	clusterDist = 10.0;  //轮廓点聚类适当加大距离
	distType = 0;  //0 - 2d distance; 1- 3d distance 
	for (int m = 0; m < (int)contourPoints.size(); m++)
	{
		std::vector<std::vector< SVzNL3DPosition>> contourClusters;  //result
		wd_pointClustering_speedUp(
			contourPoints[m],
			lineNum, linePtNum, clusterCheckWin, //搜索窗口
			clusterDist,
			distType,
			contourClusters  //result
		);
		//计算ROI
		std::vector<SSG_ROIRectD> coutourROIs;
		for (int i = 0; i < (int)contourClusters.size(); i++)
		{
			SSG_ROIRectD a_roi = _getListROI(contourClusters[i]);
			coutourROIs.push_back(a_roi);
		}
		//去掉最大的目标
		int maxId = _getMaxROI(coutourROIs);
		if (maxId < 0)
			continue;
		double innerHole_w = coutourROIs[maxId].right - coutourROIs[maxId].left;
		double innerHole_h = coutourROIs[maxId].bottom - coutourROIs[maxId].top;
		while ((innerHole_w > minObjSize) || (innerHole_h > minObjSize))
		{
			coutourROIs[maxId].right = 0;
			coutourROIs[maxId].left = 0;
			coutourROIs[maxId].bottom = 0;
			coutourROIs[maxId].top = 0;
			//继续找最大的目标，若大于门限，视为目标
			maxId = _getMaxROI(coutourROIs);
			if (maxId < 0)
				continue;

			innerHole_w = coutourROIs[maxId].right - coutourROIs[maxId].left;
			innerHole_h = coutourROIs[maxId].bottom - coutourROIs[maxId].top;
		}

		if ((innerHole_w > tireParam.diameter * 0.4) && (innerHole_h > tireParam.diameter * 0.4))
		{
			//计算姿态
			SVzNLRangeD holezRange;
			double meanZ = _getListMeanZ(contourClusters[maxId], holezRange);
			//拟合圆心
			//圆最小二乘拟合
			SVzNL3DPoint center = { 0.0, 0.0, 0.0 };
			double radius = 0;
			double errValue = fitCircleByLeastSquare_2(	contourClusters[maxId],	center,	radius);
			if ((radius > tireParam.diameter * 0.2) && (radius < minObjSize * 0.5))
			{
				WD_HolePositionInfo a_holeInfo;
				a_holeInfo.center = { center.x, center.y, meanZ };
				a_holeInfo.normDir = { 0.0, 0.0, 1.0 };
				a_holeInfo.holeR = radius;
				tirePositions.push_back(a_holeInfo);
			}
		}
	}
#endif
	//按高度排序
	std::sort(tirePositions.begin(), tirePositions.end(), _compareByTireZValue);

	if (tirePositions.size() == 0)
	{
		*errCode = SG_ERR_ZERO_OBJECTS;
		return;
	}

	//旋转回原坐标系
	for (int i = 0; i < (int)tirePositions.size(); i++)
	{
		tirePositions[i].center = _ptRotate(tirePositions[i].center, groundCalibPara.invRMatrix);
		tirePositions[i].normDir = _ptRotate(tirePositions[i].normDir, groundCalibPara.invRMatrix);
	}

	return;
}

//模板检查
void _templateChecking(
	std::vector<std::vector<double>>& quantiData, 
	int ref_x, int ref_y, 
	std::vector<std::vector<int>>& templateMask, 
	std::vector<double>& circleZ, 
	std::vector<double>& centerZ)
{
	int templateX = (int)templateMask.size();
	int templateY = (int)templateMask[0].size();
	for (int x = 0; x < templateX; x++)
	{
		for (int y = 0; y < templateY; y++)
		{
			if (templateMask[x][y] == 0)
				continue;
			int maskX = x + ref_x;
			int maskY = y + ref_y;
			if (quantiData[maskX][maskY] < 1e-4)
				continue;

			if (templateMask[x][y] == 1)
				circleZ.push_back(quantiData[maskX][maskY]);
			else if (templateMask[x][y] == 2)
				centerZ.push_back(quantiData[maskX][maskY]);
		}
	}
	return;
}

bool validateVarPeak(std::vector<std::vector< SSG_2DValueI>>& varPeaks, SSG_2DValueI& seedPeak, const int searchRng)
{
	int startX = seedPeak.x - searchRng;
	if (startX < 0)
		startX = 0;
	int endX = seedPeak.x + searchRng;
	if (endX >= (int)varPeaks.size())
		endX = (int)varPeaks.size() - 1;

	int startY = seedPeak.y - searchRng;
	int endY = seedPeak.y + searchRng;
	bool isPeak = true;
	for (int x = startX; x <= endX; x++)
	{
		int linePkSize = (int)varPeaks[x].size();
		if (linePkSize == 0)
			continue;

		for (int m = 0; m < linePkSize; m++)
		{
			if (varPeaks[x][m].y < startY)
				continue;
			if (varPeaks[x][m].y > endY)
				break;

			if ((varPeaks[x][m].x == seedPeak.x) && (varPeaks[x][m].y == seedPeak.y))
				continue;

			//比较
			if (seedPeak.valueD > varPeaks[x][m].valueD)
			{
				isPeak = false;
				break;
			}
			else if (seedPeak.valueD < varPeaks[x][m].valueD)
				varPeaks[x][m].value = 1;
		}
		if (false == isPeak)
			break;
	}
	return isPeak;
}

//宁波海瑞马定子芯中心定位
void sx_getRotorCorePose(
	std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
	const SSG_cornerParam cornerPara,
	const SSG_planeCalibPara groundCalibPara,
	const WD_rotorAppParam rotorParam,
	std::vector<WD_HolePositionInfo>& rotorPositions,
	int* errCode)
{
	*errCode = 0;

	//生成数据副本，使用副本数据进行调平和后续处理
	int lineNum = (int)scanLinesInput.size();
	std::vector< std::vector<SVzNL3DPosition>> scanLines;
	scanLines.resize(lineNum);
	int linePtNum = (int)scanLinesInput[0].size();
	bool isGridData = true;
	for (int i = 0; i < lineNum; i++)
	{
		if (linePtNum != (int)scanLinesInput[i].size())
			isGridData = false;

		scanLines[i].resize(scanLinesInput[i].size());
		std::copy(scanLinesInput[i].begin(), scanLinesInput[i].end(), scanLines[i].begin()); // 使用std::copy算法

		for (int j = 0; j < (int)scanLinesInput[i].size(); j++)
			scanLinesInput[i][j].nPointIdx = 0;  //清零，用于debug时记录信息
	}
	if (false == isGridData)//数据不是网格格式
	{
		*errCode = SG_ERR_NOT_GRID_FORMAT;
		return;
	}

	//内部参数
	double removeGroundHeight = groundCalibPara.planeHeight - rotorParam.removeGroundHOffset;
	for (int i = 0; i < lineNum; i++)
	{																					 //行处理
		//调平，去除地面
		wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, removeGroundHeight);
	}

	//产生水平扫描数据
	std::vector< std::vector<SVzNL3DPosition>> scanLines_h;
	scanLines_h.resize(linePtNum);
	for (int i = 0; i < linePtNum; i++)
		scanLines_h[i].resize(lineNum);
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
			scanLines_h[j][line] = scanLines[line][j];
			scanLines_h[j][line].pt3D.x = scanLines[line][j].pt3D.y;
			scanLines_h[j][line].pt3D.y = scanLines[line][j].pt3D.x;
		}
	}
	for (int line = 0; line < linePtNum; line++)
	{
		for (int j = 0, j_max = (int)scanLines_h[line].size(); j < j_max; j++)
			scanLines_h[line][j].nPointIdx = j;
	}

	//算法流程：
	//1、检查垂直方向数据并去除
	//2、聚类
	//3、保留最前面目标
	//4、提取孔
	//5、拟合
	//6、计算中间坐标
	
	//内部参数
	double maxDistTh = 10.0;
	double minSegLen = 2.0;
	SSG_cornerParam removeVertialPara = cornerPara;
	removeVertialPara.scale = 5.0;
	removeVertialPara.cornerTh = 60;

	SWDRimEdgeParam rimPara_1 = {60, 25};
	SWDRimEdgeParam rimPara_2 = { 60, 25 };

	SVzNLRangeD rimWidth = { 1,20 };

	std::vector<std::vector<int>> flags;
	flags.resize(lineNum);
	for (int i = 0; i < lineNum; i++)
	{
		flags[i].resize(linePtNum);
		std::fill(flags[i].begin(), flags[i].end(), 0);
	}
	std::vector<std::vector<int>> zVertivalFlags;
	std::vector<std::vector<SSG_RUN>> rimFeatures;
	for (int line = 0; line < lineNum; line++)
	{
		if (line == 578)
			int kkk = 1;

		//根据z连续性分段
		std::vector<SSG_RUN> segs;
		wd_lineDataSegment_dist_2(
			scanLines[line],
			segs,
			maxDistTh,
			minSegLen
		);

		std::vector<int> line_verticalFlags;
		wd_getXYVertialFeature_perSeg_dirAngleMethod(
			scanLines[line],
			segs,
			line,
			removeVertialPara,
			line_verticalFlags
		);
		zVertivalFlags.push_back(line_verticalFlags);
		//提取边沿特征
		std::vector<SSG_RUN> line_rimFeatures;
		wd_getRimConvexFeature(
			scanLines[line],
			segs,
			line,
			rimPara_1,
			rimPara_2,
			rimWidth,
			line_rimFeatures
		);
		rimFeatures.push_back(line_rimFeatures);
		
		for (int i = 0; i < (int)line_verticalFlags.size(); i++)
		{
			if (line_verticalFlags[i] > 0)
				flags[line][i] = 1;
		}
		for (int i = 0; i < (int)line_rimFeatures.size(); i++)
		{
			SSG_RUN& a_rim = line_rimFeatures[i];
			for (int j = 0; j < a_rim.len; j++)
			{
				flags[line][j+a_rim.start] = 2;
			}
		}
	}

	std::vector<std::vector<int>> zVertivalFlags_h;
	std::vector<std::vector<SSG_RUN>> rimFeatures_h;
	for (int line = 0; line < linePtNum; line++)
	{
		if (line == 1177)
			int kkk = 1;

		//根据z连续性分段
		std::vector<SSG_RUN> segs;
		wd_lineDataSegment_dist_2(
			scanLines_h[line],
			segs,
			maxDistTh,
			minSegLen
		);

		std::vector<int> line_verticalFlags;
		wd_getXYVertialFeature_perSeg_dirAngleMethod(
			scanLines_h[line],
			segs,
			line,
			removeVertialPara,
			line_verticalFlags
		);
		zVertivalFlags_h.push_back(line_verticalFlags);

		//提取边沿特征
		std::vector<SSG_RUN> line_rimFeatures;
		wd_getRimConvexFeature(
			scanLines_h[line],
			segs,
			line,
			rimPara_1,
			rimPara_2,
			rimWidth,
			line_rimFeatures
		);
		rimFeatures_h.push_back(line_rimFeatures);
		
		for (int i = 0; i < (int)line_verticalFlags.size(); i++)
		{
			if (line_verticalFlags[i] > 0)
				flags[i][line] = 1;
		}
		for (int i = 0; i < (int)line_rimFeatures.size(); i++)
		{
			SSG_RUN& a_rim = line_rimFeatures[i];
			for (int j = 0; j < a_rim.len; j++)
			{
				if (flags[j+a_rim.start][line] == 2)
					flags[j + a_rim.start][line] = 4;
				else
					flags[j + a_rim.start][line] = 3;
			}
		}
	}
	//标注
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			if (flags[line][j] == 1)
			{
				scanLines[line][j].pt3D.z = 0;
				scanLines_h[j][line].pt3D.z = 0;
			}
		}
	}
	//迭代一次
	SSG_lineSegParam lineSegPara;
	lineSegPara.distScale = 5.0;
	lineSegPara.segGapTh_y = 5.0;
	lineSegPara.segGapTh_z = 5.0;
	int minSegSize = 1;
	for (int line = 0; line < lineNum; line++)
	{
		std::vector<SSG_RUN> segs;
		wd_getLineDataIntervals(
			scanLines[line],
			lineSegPara,
			segs);
		for (int i = 0; i < (int)segs.size(); i++)
		{
			if (segs[i].len <= minSegSize)
			{
				int idx0 = segs[i].start;
				for (int j = 0; j < segs[i].len; j++)
					flags[line][idx0 + j] = 1;
			}
		}
	}
	for (int line = 0; line < linePtNum; line++)
	{
		std::vector<SSG_RUN> segs;
		wd_getLineDataIntervals(
			scanLines_h[line],
			lineSegPara,
			segs);
		for (int i = 0; i < (int)segs.size(); i++)
		{
			if (segs[i].len <= minSegSize)
			{
				int idx0 = segs[i].start;
				for (int j = 0; j < segs[i].len; j++)
					flags[idx0 + j][line] = 1;
			}
		}
	}

	//标注
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			scanLinesInput[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
			scanLines[line][j].nPointIdx = 0;
		}
	}
	//将垂直线段去除
	std::vector< SVzNL3DPosition> validPoints;
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			if (flags[line][j] == 1)
			{
				scanLinesInput[line][j].pt3D.z = 0;
				scanLines[line][j].pt3D.z = 0;
			}
			else if (flags[line][j] > 0)
				scanLinesInput[line][j].nPointIdx = flags[line][j];

			if (scanLines[line][j].pt3D.z > 1e-4)
			{
				SVzNL3DPosition a_vldPt;
				a_vldPt.pt3D = scanLines[line][j].pt3D;
				a_vldPt.nPointIdx = (line << 16) | (j & 0xffff);
				validPoints.push_back(a_vldPt);
			}
		}
	}

	//聚类
	int clusterCheckWin = 5;
	double clusterDist = 5.0;
	int distType = 1;  //0 - 2d distance; 1- 3d distance 
	std::vector<std::vector< SVzNL3DPosition>> objClusters;  //result
	wd_pointClustering_speedUp(
		validPoints,
		lineNum, linePtNum, clusterCheckWin, //搜索窗口
		clusterDist,
		distType,
		objClusters  //result
	);

	//保留最前面的目标, 筐子
	double minObj_L = rotorParam.basket_L * 0.8;
	double maxObj_L = rotorParam.basket_L * 1.25;
	double minObj_W = rotorParam.basket_W * 0.8;
	double maxObj_W = rotorParam.basket_W * 1.25;
	const double topPlateMaxZRange = 100.0;
	std::vector<double> objMeanZ;
	std::vector<SSG_ROIRectD> objROIs;
	std::vector<SVzNLRangeD> objZRange;
	objMeanZ.resize(objClusters.size());
	objROIs.resize(objClusters.size());
	objZRange.resize(objClusters.size());
	std::vector<int> validObjIndice;
	for (int i = 0; i < (int)objClusters.size(); i++)
	{
		SSG_ROIRectD a_roi = _getListROI(objClusters[i]);
		objROIs[i] = a_roi;
		double w = a_roi.right - a_roi.left;
		double h = a_roi.bottom - a_roi.top;
		double len = w < h ? h : w;
		double width = w > h ? h : w;
		if ((len > minObj_L) && (width > minObj_W) && (len < maxObj_L) && (width < maxObj_W))
		{
			SVzNLRangeD zRange;
			double meanZ = _getListMeanZ(objClusters[i], zRange);
			objMeanZ[i] = meanZ;
			objZRange[i] = zRange;
			validObjIndice.push_back(i);
		}
		else
		{
			objMeanZ[i] = 0;
			objZRange[i].max = -1.0;
			objZRange[i].min = 0.0;
		}
	}

	if (validObjIndice.size() == 0)
	{
		*errCode = SG_ERR_ZERO_OBJECTS;
		return;
	}

	//最前面的目标为筐子
	int basketClusterIdx = validObjIndice[0];
	for (int i = 1; i < (int)validObjIndice.size(); i++)
	{
		int idx = validObjIndice[i];
		if (objMeanZ[basketClusterIdx] > objMeanZ[idx])
			basketClusterIdx = idx;
	}
	std::vector< SVzNL3DPosition>& basketCluster = objClusters[basketClusterIdx];
	//提取筐子的四个角点，用于确定目标所在的边

	//生成投影图，方便使用模板
	const double quantiScale = 1.0; //
	SSG_ROIRectD& basketROI = objROIs[basketClusterIdx];
	int quantiW = (basketROI.right - basketROI.left) / quantiScale + 1;
	if (quantiW % 2 == 1)
		quantiW += 1;
	int quantiH = (basketROI.bottom - basketROI.top) / quantiScale + 1;
	if (quantiH % 2 == 1)
		quantiH += 1;

	std::vector<std::vector<double>> quantiData;
	quantiData.resize(quantiW);
	for (int i = 0; i < quantiW; i++)
	{
		quantiData[i].resize(quantiH);
		std::fill(quantiData[i].begin(), quantiData[i].end(), -1);
	}
	//量化
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			if ((scanLines[line][j].pt3D.z > 1e-4) &&
				(scanLines[line][j].pt3D.x >= basketROI.left) && (scanLines[line][j].pt3D.x < basketROI.right) &&
				(scanLines[line][j].pt3D.y >= basketROI.top) && (scanLines[line][j].pt3D.y < basketROI.bottom))
			{
				int quantiX = (scanLines[line][j].pt3D.x - basketROI.left) / quantiScale;
				int quantiY = (scanLines[line][j].pt3D.y - basketROI.top) / quantiScale;
				quantiData[quantiX][quantiY] = scanLines[line][j].pt3D.z;
			}
		}
	}
#ifdef _DEBUG_OUTPUT
	//输出图像
	cv::Mat vis = vec2Float2VisMat(quantiData);
	cv::imwrite("quanti_visual.png", vis);
#endif

	//生成模板
	double template_minR = rotorParam.rotorDiameter/2.0 - 1.0;
	double template_maxR = rotorParam.rotorDiameter/2.0 + 1.0;
	double template_centerR = 10.0;
	int template_pixR = (int)(template_maxR / quantiScale + 1);
	int template_W = template_pixR * 2 + 1;

	double template_cx = (double)template_pixR * quantiScale;
	double template_cy = template_cx;
	std::vector<std::vector<int>> templateMask;
	templateMask.resize(template_W);
	for (int i = 0; i < template_W; i++)
		templateMask[i].resize(template_W);
	for (int i = 0; i < template_W; i++)
	{
		for (int j = 0; j < template_W; j++)
		{
			double x = (double)i * quantiScale;
			double y = (double)j * quantiScale;
			double dist = sqrt(pow(x - template_cx, 2) + pow(y - template_cy, 2));
			if ((dist >= template_minR) && (dist <= template_maxR))
				templateMask[i][j] = 1;
			else if (dist <= template_centerR)
				templateMask[i][j] = 2;
		}
	}
	if ((template_W >= quantiW) || (template_W >= quantiH))
	{
		*errCode = SG_ERR_ZERO_OBJECTS;
		return;
	}
#ifdef _DEBUG_OUTPUT
	//输出图像
	cv::Mat vis_template = vec2Int2VisMat(templateMask);
	cv::imwrite("template_visual.png", vis_template);
#endif
	//搜索目标
	std::vector<std::vector<SSG_meanVar>> statsCircle;
	std::vector<std::vector<SSG_meanVar>> statsCenter;
	statsCircle.resize(quantiW);
	statsCenter.resize(quantiW);
	for (int i = 0; i < quantiW; i++)
	{
		statsCircle[i].resize(quantiH);
		statsCenter[i].resize(quantiH);
	}

	int searchStart_X = template_pixR;
	int searchEnd_X = quantiW - 1 - template_pixR;
	int searchStart_Y = template_pixR;
	int searchEnd_Y = quantiH - 1 - template_pixR;
#if 0
	for (int x = searchStart_X; x < searchEnd_X; x++)
	{
		for (int y = searchStart_Y; y < searchEnd_Y; y++)
		{
			if ((x == 373) && (y == 72))
				int kkk = 1;
			std::vector<double> circleZ;
			std::vector<double> centerZ;
			//模板检查
			_templateChecking(
				quantiData,
				x - template_pixR, y - template_pixR,
				templateMask,
				circleZ,
				centerZ);
			//计算均值和方差
			SSG_meanVar satasInfo_circle = { 0.0, 0.0 };
			CalcStats(circleZ, satasInfo_circle);
			statsCircle[x][y] = satasInfo_circle;

			SSG_meanVar satasInfo_center = { 0.0, 0.0 };
			CalcStats(centerZ, satasInfo_center);
			statsCenter[x][y] = satasInfo_center;
		}
	}
#endif
#ifdef _DEBUG_OUTPUT
	//输出图像
	cv::Mat vis_circle = vec2Var2VisMat(statsCircle);
	cv::imwrite("var_circle_visual.png", vis_circle);
	cv::Mat vis_center = vec2Var2VisMat(statsCenter);
	cv::imwrite("var_center_visual.png", vis_center);
	cv::Mat vis_circleCenter = vec2VarAdd2VisMat(statsCircle, statsCenter);
	cv::imwrite("var_center_circle_visual.png", vis_circleCenter);
#endif

	//搜索方差极小值点, circle和center的方差累加后的极小值
	double rotorH_th = rotorParam.rotorH * 0.8;
	//两步搜索法：先搜索5x5的，再搜索模块大小
	std::vector<std::vector< SSG_2DValueI>> varPeaks_5x5;  //按行存储
	varPeaks_5x5.resize(quantiW);
	const SVzNL2DPoint searchIndice[24] = {
		{-2,-2}, {-1, -2}, {0, -2},  { 1, -2 }, {2, -2 },
		{-2,-1}, {-1, -1}, {0, -1},  { 1, -1 }, {2, -1 },
		{-2, 0}, {-1,  0}, {1,  0},  {2,  0 },
		{-2, 1}, {-1,  1}, {0,  1},  { 1,  1 }, {2,  1 },
		{-2, 2}, {-1,  2}, {0,  2},  { 1,  2 }, {2,  2 }
	};
	//使用一个辅助Flag来加速
	std::vector<std::vector<int>> peakFalseflags;
	peakFalseflags.resize(quantiW);
	for (int i = 0; i < quantiW; i++)
		peakFalseflags[i].resize(quantiH);
	for (int x = searchStart_X; x < searchEnd_X; x++)
	{
		for (int y = searchStart_Y; y < searchEnd_Y; y++)
		{
			if ((x == 373) && (y == 70))
				int kkk = 1;
			SSG_meanVar& curr_circle = statsCircle[x][y];
			SSG_meanVar& curr_center = statsCenter[x][y];
			if (peakFalseflags[x][y] > 0)
				continue;

			if ((curr_center.mean - curr_circle.mean) > rotorH_th)
			{
				bool isPeak = true;
				for (int m = 0; m < 24; m++)
				{
					const SVzNL2DPoint& an_idx = searchIndice[m];
					SSG_meanVar& chk_circle = statsCircle[x + an_idx.x][y+an_idx.y];
					if (curr_circle.var > chk_circle.var)
					{
						isPeak = false;
						break;
					}
				}
				if (true == isPeak)
				{
					for (int m = 0; m < 24; m++)
					{
						const SVzNL2DPoint& an_idx = searchIndice[m];
						peakFalseflags[x + an_idx.x][y + an_idx.y] = 1;
					}
					SSG_2DValueI a_peak;
					a_peak.x = x;
					a_peak.y = y;
					a_peak.value = 0;
					a_peak.valueD = curr_circle.var;
					a_peak.sideID = 0; //1-T, 2-B, 3-L, 4-R
					varPeaks_5x5[x].push_back(a_peak);
				}
			}
		}
	}
	//二次搜索
	std::vector< SSG_2DValueI> objVarPeaks;
	int searchRng = (int)( rotorParam.rotorDiameter / quantiScale + 0.5 );
	for (int x = searchStart_X; x < searchEnd_X; x++)
	{
		int pkSize = (int)varPeaks_5x5[x].size();
		if (pkSize > 0)
		{
			for (int m = 0; m < pkSize; m++)
			{
				SSG_2DValueI& a_peak = varPeaks_5x5[x][m];
				if (a_peak.value == 1)
					continue;

				bool isPeak = validateVarPeak(varPeaks_5x5, a_peak, searchRng);
				if (true == isPeak)
				{
					objVarPeaks.push_back(varPeaks_5x5[x][m]);
				}
			}
		}
	}
#ifdef _DEBUG_OUTPUT
	//输出图像
	cv::Mat vis_gray = vec2Float2VisMat(quantiData);
	cv::Mat vis_color;
	cv::cvtColor(vis_gray, vis_color, cv::COLOR_GRAY2BGR);
	int radius = 3;
	cv::Scalar color_all(0, 255, 0); // 颜色 (BGR顺序：蓝色, 绿色, 红色)
	for (int x = searchStart_X; x < searchEnd_X; x++)
	{
		int pkSize = (int)varPeaks_5x5[x].size();
		if (pkSize > 0)
		{
			for (int m = 0; m < pkSize; m++)
			{
				int px = varPeaks_5x5[x][m].x;
				int py = varPeaks_5x5[x][m].y;
				cv::circle(vis_color, cv::Point(py, px), radius, color_all, -1);
			}
		}
	}
	cv::Scalar color(0, 0, 255); // 颜色 (BGR顺序：蓝色, 绿色, 红色)
	for (int i = 0; i < (int)objVarPeaks.size(); i++)
	{
		int px = objVarPeaks[i].x;
		int py = objVarPeaks[i].y;
		// thickness = -1 表示实心
		cv::circle(vis_color, cv::Point(py, px), radius, color, -1);
	}

	cv::imwrite("quanti_obj_color.png", vis_color);
#endif

	//旋转回原坐标系

	for (int i = 0; i < (int)rotorPositions.size(); i++)
	{
		rotorPositions[i].center = _ptRotate(rotorPositions[i].center, groundCalibPara.invRMatrix);
		rotorPositions[i].normDir = _ptRotate(rotorPositions[i].normDir, groundCalibPara.invRMatrix);
	}

	return;
}

//北京玖瑞工件定位
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>& workpiecePositions,
	int* errCode)
{
	*errCode = 0;

	//生成数据副本，使用副本数据进行调平和后续处理
	int lineNum = (int)scanLinesInput.size();
	std::vector< std::vector<SVzNL3DPosition>> scanLines;
	scanLines.resize(lineNum);
	int linePtNum = (int)scanLinesInput[0].size();
	bool isGridData = true;
	for (int i = 0; i < lineNum; i++)
	{
		if (linePtNum != (int)scanLinesInput[i].size())
			isGridData = false;

		scanLines[i].resize(scanLinesInput[i].size());
		std::copy(scanLinesInput[i].begin(), scanLinesInput[i].end(), scanLines[i].begin()); // 使用std::copy算法

		for (int j = 0; j < (int)scanLinesInput[i].size(); j++)
			scanLinesInput[i][j].nPointIdx = 0;  //清零，用于debug时记录信息
	}
	if (false == isGridData)//数据不是网格格式
	{
		*errCode = SG_ERR_NOT_GRID_FORMAT;
		return;
	}

	double removeGroundHeight = -1;
	for (int i = 0; i < lineNum; i++)
	{																					 //行处理
		//调平，去除地面
		wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, removeGroundHeight);
	}

	//产生水平扫描数据
	std::vector< std::vector<SVzNL3DPosition>> scanLines_h;
	scanLines_h.resize(linePtNum);
	for (int i = 0; i < linePtNum; i++)
		scanLines_h[i].resize(lineNum);
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
			scanLines_h[j][line] = scanLines[line][j];
			scanLines_h[j][line].pt3D.x = scanLines[line][j].pt3D.y;
			scanLines_h[j][line].pt3D.y = scanLines[line][j].pt3D.x;
		}
	}
	for (int line = 0; line < linePtNum; line++)
	{
		for (int j = 0, j_max = (int)scanLines_h[line].size(); j < j_max; j++)
			scanLines_h[line][j].nPointIdx = j;
	}

	//算法流程：
	//1、检查垂直方向数据并去除
	//2、聚类
	//3、保留最前面目标
	//4、提取孔
	//5、拟合
	//6、计算中间坐标
	//内部参数
	SSG_cornerParam removeVertialPara = cornerPara;
	removeVertialPara.scale = 10.0;
	removeVertialPara.cornerTh = 60;

	std::vector<std::vector<int>> flags;
	flags.resize(lineNum);
	for (int i = 0; i < lineNum; i++)
	{
		flags[i].resize(linePtNum);
		std::fill(flags[i].begin(), flags[i].end(), 0);
	}
	std::vector<std::vector<int>> zVertivalFlags;
	for (int line = 0; line < lineNum; line++)
	{
		if (line == 700)
			int kkk = 1;
		std::vector<int> line_verticalFlags;
		wd_getXYVertialFeature_dirAngleMethod(
			scanLines[line],
			line,
			removeVertialPara,
			line_verticalFlags
		);
		zVertivalFlags.push_back(line_verticalFlags);

		for (int i = 0; i < (int)line_verticalFlags.size(); i++)
		{
			if (line_verticalFlags[i] > 0)
				flags[line][i] = 1;
		}
	}

	std::vector<std::vector<int>> zVertivalFlags_h;
	for (int line = 0; line < linePtNum; line++)
	{
		if (line == 1177)
			int kkk = 1;
		std::vector<int> line_verticalFlags;
		wd_getXYVertialFeature_dirAngleMethod(
			scanLines_h[line],
			line,
			removeVertialPara,
			line_verticalFlags
		);
		zVertivalFlags_h.push_back(line_verticalFlags);

		for (int i = 0; i < (int)line_verticalFlags.size(); i++)
		{
			if (line_verticalFlags[i] > 0)
				flags[i][line] = 1;
		}
	}
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			if (flags[line][j] > 0)
			{
				scanLines[line][j].pt3D.z = 0;
				scanLines_h[j][line].pt3D.z = 0;
			}
		}
	}
	//迭代一次
	SSG_lineSegParam lineSegPara;
	lineSegPara.distScale = 5.0;
	lineSegPara.segGapTh_y = 5.0;
	lineSegPara.segGapTh_z = 5.0;
	const int minSegLen = 5;
	for (int line = 0; line < lineNum; line++)
	{
		std::vector<SSG_RUN> segs;
		wd_getLineDataIntervals(
			scanLines[line],
			lineSegPara,
			segs);
		for (int i = 0; i < (int)segs.size(); i++)
		{
			if (segs[i].len <= minSegLen)
			{
				int idx0 = segs[i].start;
				for (int j = 0; j < segs[i].len; j++)
					flags[line][idx0 + j] = 1;
			}
		}
	}
	for (int line = 0; line < linePtNum; line++)
	{
		std::vector<SSG_RUN> segs;
		wd_getLineDataIntervals(
			scanLines_h[line],
			lineSegPara,
			segs);
		for (int i = 0; i < (int)segs.size(); i++)
		{
			if (segs[i].len <= minSegLen)
			{
				int idx0 = segs[i].start;
				for (int j = 0; j < segs[i].len; j++)
					flags[idx0 + j][line] = 1;
			}
		}
	}

	//标注
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			scanLinesInput[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
			scanLines[line][j].nPointIdx = 0;
		}
	}
	//将垂直线段去除
	std::vector< SVzNL3DPosition> validPoints;
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			if (flags[line][j] > 0)
				scanLines[line][j].pt3D.z = 0;

			if (scanLines[line][j].pt3D.z > 1e-4)
			{
				SVzNL3DPosition a_vldPt;
				a_vldPt.pt3D = scanLines[line][j].pt3D;
				a_vldPt.nPointIdx = (line << 16) | (j & 0xffff);
				validPoints.push_back(a_vldPt);
			}
		}
	}

	//聚类
	int clusterCheckWin = 5;
	double clusterDist = 5.0;
	int distType = 1;  //0 - 2d distance; 1- 3d distance 
	std::vector<std::vector< SVzNL3DPosition>> objClusters;  //result
	wd_pointClustering_speedUp(
		validPoints,
		lineNum, linePtNum, clusterCheckWin, //搜索窗口
		clusterDist,
		distType,
		objClusters  //result
	);

	//保留最前面的大小合适的目标
	double minObjSize = workpieceParam.width * 0.8;
	double maxObjSize = workpieceParam.len * 1.5;
	const double topPlateMaxZRange = 100.0;
	std::vector<double> objMeanZ;
	std::vector<SSG_ROIRectD> objROIs;
	std::vector<SVzNLRangeD> objZRange;
	objMeanZ.resize(objClusters.size());
	objROIs.resize(objClusters.size());
	objZRange.resize(objClusters.size());
	std::vector<int> validObjIndice;
	int topClusterId = -1;
	for (int i = 0; i < (int)objClusters.size(); i++)
	{
		SSG_ROIRectD a_roi = _getListROI(objClusters[i]);
		objROIs[i] = a_roi;
		double w = a_roi.right - a_roi.left;
		double h = a_roi.bottom - a_roi.top;
		if ((w > minObjSize) && (h > minObjSize) && (w < maxObjSize) && (h < maxObjSize))
		{
			SVzNLRangeD zRange;
			double meanZ = _getListMeanZ(objClusters[i], zRange);
			objMeanZ[i] = meanZ;
			objZRange[i] = zRange;
			validObjIndice.push_back(i);

			if (topClusterId < 0)
				topClusterId = i;
			else if (objMeanZ[topClusterId] > meanZ)
				topClusterId = i;
		}
		else
		{
			objMeanZ[i] = 0;
			objZRange[i].max = -1.0;
			objZRange[i].min = 0.0;
		}
	}

	if ( (validObjIndice.size() == 0) || (topClusterId < 0))
	{
		*errCode = SG_ERR_ZERO_OBJECTS;
		return;
	}

	//标注
	//重新将flags设置为目标的mask
	for (int i = 0; i < lineNum; i++)
		std::fill(flags[i].begin(), flags[i].end(), -1);

	for (int i = 0; i < (int)objClusters[topClusterId].size(); i++)
	{
		int line = objClusters[topClusterId][i].nPointIdx >> 16;
		int ptIdx = objClusters[topClusterId][i].nPointIdx & 0x0000FFFF;
		scanLinesInput[line][ptIdx].nPointIdx = 2;
		flags[line][ptIdx] = 1;  //indexing
	}

	//计算质心
	SVzNL3DPoint centroid = { 0, 0, 0 };
	int centroid_size = (int)objClusters[topClusterId].size();
	for (int i = 0; i < centroid_size; i++)
	{
		int line = objClusters[topClusterId][i].nPointIdx >> 16;
		int ptIdx = objClusters[topClusterId][i].nPointIdx & 0x0000FFFF;
		centroid.x += scanLinesInput[line][ptIdx].pt3D.x;
		centroid.y += scanLinesInput[line][ptIdx].pt3D.y;
		centroid.z += scanLinesInput[line][ptIdx].pt3D.z;
	}
	centroid.x = centroid.x / centroid_size;
	centroid.y = centroid.y / centroid_size;
	centroid.z = centroid.z / centroid_size;

	//圆周扫描:0.5度间隔
	double angleScale = 1.0;
	std::vector<std::vector<SWD_polarPt>> polarScanData;
	int polarLines = (int)(360.0 / angleScale + 0.5);
	polarScanData.resize(polarLines);
	_computeAnlgeScanData(
		objClusters[topClusterId],
		angleScale,
		centroid,
		polarScanData);

	//每个扫描角度按R从小到大排序
	for (int i = 0; i < polarLines; i++)
	{
		std::sort(polarScanData[i].begin(), polarScanData[i].end(), compareByPolarScanR);
	 }
	
	//提取平面点-计算法向
	double distToPlane_min = 19.5;
	double distToPlane_max = 20.5;
	std::vector<SWD_polarPt> planePolarPoints;
	std::vector<cv::Point3d> Points3ds;
	for (int i = 0; i < polarLines; i++)
	{
		double angle = angleScale * i;
		if ((angle < 30) || ((angle > 75) && (angle < 120)) || ((angle > 150) && (angle < 210)) ||
			((angle > 240) && (angle < 300)) || (angle > 330))  //避开水平和垂直区域
			continue;

		int lineSize = (int)polarScanData[i].size();
		if (lineSize == 0)
			continue;

		int minR = polarScanData[i][0].R;
		for (int j = 0; j < lineSize; j++)
		{
			double dist = polarScanData[i][j].R - minR;
			if ((dist >= distToPlane_min) && (dist <= distToPlane_max))
			{
				planePolarPoints.push_back(polarScanData[i][j]);
				cv::Point3d a_pt = cv::Point3d(polarScanData[i][j].x, polarScanData[i][j].y, polarScanData[i][j].z);
				Points3ds.push_back(a_pt);
			}
		}
	}
	//标注
	for (int i = 0; i < (int)planePolarPoints.size(); i++)
	{
		int line = planePolarPoints[i].lineIdx;
		int ptIdx = planePolarPoints[i].ptIdx;
		scanLinesInput[line][ptIdx].nPointIdx = 3;
	}
	//拟合
	//计算面参数: z = Ax + By + C
	//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
	std::vector<double> res;
	vzCaculateLaserPlane(Points3ds, res);

	//计算投影向量
	SVzNL3DPoint vec_1 = { res[0], res[1], res[2]};
	if (res[2] < 0)
		vec_1 = { -res[0], -res[1], -res[2] };
	SVzNL3DPoint vec_z = { 0, 0, 1.0 };
	SSG_planeCalibPara poseR = wd_computeRTMatrix(vec_1, vec_z);

	//投影
	std::vector<SVzNL3DPosition> projectPosition3ds;
	for (int i = 0; i < (int)objClusters[topClusterId].size(); i++)
	{
		SVzNL3DPosition a_vldPt;
		a_vldPt.nPointIdx = objClusters[topClusterId][i].nPointIdx;
		a_vldPt.pt3D = wd_ptRotate(objClusters[topClusterId][i].pt3D, poseR.planeCalib);
		projectPosition3ds.push_back(a_vldPt);
	}

	//
	//迭代：圆周扫描-提取轮廓-计算中心点和轴向
	// 
	//迭代计算质心
	centroid = { 0, 0, 0 };
	std::vector< SVzNL2DPointD> points_2d;
	points_2d.resize(projectPosition3ds.size());
	for (int i = 0; i < (int)projectPosition3ds.size(); i++)
	{
		centroid.x += projectPosition3ds[i].pt3D.x;
		centroid.y += projectPosition3ds[i].pt3D.y;
		centroid.z += projectPosition3ds[i].pt3D.z;
		points_2d[i] = { projectPosition3ds[i].pt3D.x ,  projectPosition3ds[i].pt3D.y };
	}
	centroid.x = centroid.x / centroid_size;
	centroid.y = centroid.y / centroid_size;
	centroid.z = centroid.z / centroid_size;

	//圆周扫描:0.5度间隔
	angleScale = 1.0;
	polarLines = (int)(360.0 / angleScale + 0.5);
	polarScanData.clear();
	polarScanData.resize(polarLines);
	_computeAnlgeScanData(
		projectPosition3ds,
		angleScale,
		centroid,
		polarScanData);
	//每个扫描角度按R从小到大排序
	for (int i = 0; i < polarLines; i++)
	{
		std::sort(polarScanData[i].begin(), polarScanData[i].end(), compareByPolarScanR);
	}

	//取轮廓点计算中心
	SVzNL3DPoint objCenter = { 0, 0, 0 };
	int centerSum = 0;
	for (int i = 0; i < polarLines; i++)
	{
		int lineSize = (int)polarScanData[i].size();
		if (lineSize == 0)
			continue;

		objCenter.x += polarScanData[i][0].x;
		objCenter.y += polarScanData[i][0].y;
		objCenter.z += polarScanData[i][0].z;
		centerSum++;
	}
	objCenter.x = objCenter.x / centerSum;
	objCenter.y = objCenter.y / centerSum;
	objCenter.z = objCenter.z / centerSum;

#if 0
	//计算长轴方向: 长轴短轴比最大
	int degree90 = (int)(90.0 / angleScale);
	int degree180 = (int)(180.0 / angleScale);
	int longAxisIndex = -1;
	double axisLenRatio = 0;
	std::vector<double> ratioBuff;
	ratioBuff.resize(polarLines / 2);
	std::vector<double> sumRBuff;
	sumRBuff.resize(polarLines / 2);
	for (int i = 0; i < polarLines/2; i++)
	{
		int idx_1 = i;
		int idx_2 = (i + degree180) % polarLines;
		int idx_3 = (i + degree90) % polarLines;
		int idx_4 = (idx_3 + degree180) % polarLines;
		if (idx_2 < polarLines)
		{
			if ((polarScanData[idx_1].size() > 0) && (polarScanData[idx_2].size() > 0) &&
				(polarScanData[idx_3].size() > 0) && (polarScanData[idx_4].size() > 0))
			{
				double sumR_long = polarScanData[idx_1].back().R + polarScanData[idx_2].back().R;
				double sumR_short = polarScanData[idx_3].back().R + polarScanData[idx_4].back().R;
				double ratio = sumR_long / sumR_short;
				sumRBuff[i] = sumR_long;
				ratioBuff[i] = ratio;
				if (longAxisIndex < 0)
				{
					longAxisIndex = idx_1;
					axisLenRatio = ratio;
				}
				else
				{
					if (axisLenRatio < ratio)
					{
						longAxisIndex = idx_1;
						axisLenRatio = ratio;
					}
				}
			}
		}
	}
	if(longAxisIndex < 0)
	{
		*errCode = SG_ERR_ZERO_OBJECTS;
		return;
	}

	double longAxisAngle = longAxisIndex * angleScale * PI / 180;
	SVzNL3DPoint vector_y = { cos(longAxisAngle), sin(longAxisAngle), 0 };
#else
	//PCA方法
	SVzNL2DPointD axis, roi_centroid;
	pca2D(points_2d, axis, roi_centroid);
	SVzNL3DPoint vector_y = { axis.x, axis.y, 0 };
#endif
	//叉乘出vector_x
	SVzNL3DPoint vector_x;
	vector_x.x = vector_y.y * vec_z.z - vec_z.y * vector_y.z;
	vector_x.y = vector_y.z * vec_z.x - vec_z.z * vector_y.x;
	vector_x.z = vector_y.x * vec_z.y - vec_z.x * vector_y.y;
	//生成结果
	SSG_pointPose a_pose;
	a_pose.point = _ptRotate(objCenter, poseR.invRMatrix);
	a_pose.pose_x = _ptRotate(vector_x, poseR.invRMatrix);
	a_pose.pose_y = _ptRotate(vector_y, poseR.invRMatrix);
	a_pose.pose_z = _ptRotate(vec_z, poseR.invRMatrix);

	//旋转回原坐标系
	a_pose.point = _ptRotate(a_pose.point, groundCalibPara.invRMatrix);
	a_pose.pose_x = _ptRotate(a_pose.pose_x, groundCalibPara.invRMatrix);
	a_pose.pose_y = _ptRotate(a_pose.pose_y, groundCalibPara.invRMatrix);
	a_pose.pose_z = _ptRotate(a_pose.pose_z, groundCalibPara.invRMatrix);
	workpiecePositions.push_back(a_pose);
	return;
}