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

//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一个小的修正， 不影响结果
std::string	m_strVersion = "1.4.4";
const char* wd_workpieceHolePositioningVersion(void)
{
	return m_strVersion.c_str();
}

//相机水平安装计算地面调平参数。
//相机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;
}

//搜索最接近distance的目标
int distanceSearchObject(SVzNL3DPoint seed, std::vector<SWD_HoleInfo>& holes, double distance, double distDeviation)
{
	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);
		if (minDistDiff > distDiff)
		{
			minDistDiff = distDiff;
			minDistIndex = i;
		}
	}
	if ((minDistIndex >= 0) && (minDistDiff < distDeviation))
		result = minDistIndex;
	return result;
}

//搜索最接近distance且角度为angle的目标, 以角度为优先
int angleConditionDistanceSearch(
	SVzNL3DPoint seed, SVzNL3DPoint angleSide,
	std::vector<SWD_HoleInfo>& holes,
	double distance, double distDeviation,
	SVzNLRangeD angleRange)
{
	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);
		if (distDiff < distDeviation)
		{
			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)
				{
					minAngleDeviateion = angleDiff;
					minAngleIdx = idx;
				}
			}
		}
		result = minAngleIdx;
	}
	return result;
}

double _getMeanZ(std::vector<std::vector<double>>& quantiValue, SVzNL3DPoint seed, SVzNLRect& roi2D, double rectR)
{
	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;
	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)
				{
					zSum += quantiValue[qx][qy];
					hist++;
				}
			}
		}
	}
	if (hist == 0)
		return 0;
	else
		return (zSum / hist);
}

void _updateROI(SSG_ROIRectD& roi, SVzNL3DPoint& a_pt)
{
	if (a_pt.z > 1E-4)
	{
		if (roi.left < 0)
		{
			roi.left = a_pt.x;
			roi.right = a_pt.x;
			roi.left = a_pt.y;
			roi.right = a_pt.y;
		}
		else
		{
			if (roi.left > a_pt.x)
				roi.left = a_pt.x;
			if (roi.right < a_pt.x)
				roi.right = a_pt.x;
			if (roi.top > a_pt.y)
				roi.top = a_pt.y;
			if (roi.bottom < a_pt.y)
				roi.bottom = a_pt.y;
		}
	}
	return;
}

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

void _updateRenge(SVzNLRange& range, int idx)
{
	if (range.nMin < 0)
	{
		range.nMin = idx;
		range.nMax = idx;
	}
	else
	{
		if (range.nMin > idx)
			range.nMin = idx;
		if (range.nMax < idx)
			range.nMax = idx;
	}
	return;
}

bool _checkExist(int id, std::vector<int>& buff)
{
	for (int i = 0; i < (int)buff.size(); i++)
	{
		if (id == buff[i])
			return true;
	}
	return false;
}

void _searchNeighbours(
	int selfId, int chkExtening,
	int sLineIdx, int eLineIdx, 
	SVzNLRange ptIdxRange, 
	std::vector<std::vector<int>>& treeMask,
	std::vector<int>& neighbours)
{
	int lineNum = (int)treeMask.size();
	int ptNum = treeMask[0].size();
	for (int line = sLineIdx - chkExtening; line <= eLineIdx+ chkExtening; line++)
	{
		if ((line >= 0) && (line < lineNum))
		{
			for (int ptIdx = ptIdxRange.nMin - chkExtening; ptIdx <= ptIdxRange.nMax + chkExtening; ptIdx++)
			{
				if ((ptIdx >= 0) && (ptIdx < ptNum))
				{
					if ((treeMask[line][ptIdx] >= 0) && (treeMask[line][ptIdx] != selfId))
					{
						bool isExist = _checkExist(treeMask[line][ptIdx], neighbours);
						if (false == isExist)
							neighbours.push_back(treeMask[line][ptIdx]);
					}
				}
			}
		}
	}
	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;
}

void wd_getHoleInfo(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SSG_lineSegParam lineSegPara,
	const SSG_outlierFilterParam filterParam,
	const SSG_treeGrowParam growParam,
	const double valieCommonNumRatio,
	std::vector<SWD_segFeatureTree>& segTrees_v,
	std::vector<SWD_segFeatureTree>& segTrees_h,
	std::vector<SSG_intPair>& validObjects
)
{
	int lineNum = (int)scanLines.size();
	int linePtNum = (int)scanLines[0].size();

	std::vector<std::vector<int>> pointMask;
	pointMask.resize(lineNum);

	//提取空白线段特征（孔特征）
	std::vector<std::vector<SWD_segFeature>> holeGaps;
	//提取线段端点特征
	for (int line = 0; line < lineNum; line++)
	{
		if (line == 1047)
			int kkk = 1;

		std::vector<SVzNL3DPosition>& lineData = scanLines[line];
		pointMask[line].resize(lineData.size());
		std::fill(pointMask[line].begin(), pointMask[line].end(), 0);//初始化为0
		//滤波，滤除异常点
		sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);

		std::vector<SSG_RUN> segs;
		wd_getLineDataNullIntervals(lineData, lineSegPara, segs);

		//将seg端点作为边缘点。做了地面调平后，垂直孔的内侧在XY平面上均为边缘点。
		std::vector<SWD_segFeature> line_gaps;
		for (int i = 0, i_max = (int)segs.size(); i < i_max; i++)
		{
			int ptIdx_1 = segs[i].start;
			int ptIdx_2 = segs[i].start + segs[i].len - 1;
			SWD_segFeature a_gap;
			a_gap.lineIdx = line;
			a_gap.startPtIdx = ptIdx_1;
			a_gap.endPtIdx = ptIdx_2;
			a_gap.startPt = lineData[ptIdx_1].pt3D;
			a_gap.endPt = lineData[ptIdx_2].pt3D;
			a_gap.featureValue = abs(a_gap.startPt.y - a_gap.endPt.y);
			line_gaps.push_back(a_gap);
		}
		holeGaps.push_back(line_gaps);
	}
	//特征生长
	wd_getSegFeatureGrowingTrees_2(holeGaps, segTrees_v, growParam);

	//生成水平扫描
	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特征提取
	std::vector<std::vector<SWD_segFeature>> holeGaps_h;
	for (int line = 0; line < linePtNum; line++)
	{
		if (line == 974)
			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_RUN> segs;
		wd_getLineDataNullIntervals(lineData, lineSegPara, segs);

		//将seg端点作为边缘点。做了地面调平后，垂直孔的内侧在XY平面上均为边缘点。
		std::vector<SWD_segFeature> line_gaps;
		for (int i = 0, i_max = (int)segs.size(); i < i_max; i++)
		{
			int ptIdx_1 = segs[i].start;
			int ptIdx_2 = segs[i].start + segs[i].len - 1;
			SWD_segFeature a_gap;
			a_gap.lineIdx = line;
			a_gap.startPtIdx = ptIdx_1;
			a_gap.endPtIdx = ptIdx_2;
			a_gap.startPt = lineData[ptIdx_1].pt3D;
			a_gap.endPt = lineData[ptIdx_2].pt3D;
			a_gap.featureValue = abs(a_gap.startPt.y - a_gap.endPt.y);
			line_gaps.push_back(a_gap);
		}
		holeGaps_h.push_back(line_gaps);
	}
	//特征生长
	wd_getSegFeatureGrowingTrees_2(holeGaps_h, segTrees_h, growParam);

	//创建Tree所在孔洞的Mask
	std::vector<std::vector<int>> treeMask_v;
	treeMask_v.resize(lineNum);
	std::vector<std::vector<int>> treeMask_h;
	treeMask_h.resize(lineNum);
	for (int i = 0; i < lineNum; i++)
	{
		treeMask_v[i].resize(linePtNum);
		std::fill(treeMask_v[i].begin(), treeMask_v[i].end(), -1);
		treeMask_h[i].resize(linePtNum);
		std::fill(treeMask_h[i].begin(), treeMask_h[i].end(), -1);
	}
	//标注
	std::vector<SSG_treeInfo> treeInfo_v;
	treeInfo_v.resize(segTrees_v.size());
	for (int i = 0; i < (int)segTrees_v.size(); i++)
	{
		SWD_segFeatureTree& a_tree = segTrees_v[i];
		treeInfo_v[i].treeIdx = i;
		treeInfo_v[i].sLineIdx = a_tree.sLineIdx;
		treeInfo_v[i].eLineIdx = a_tree.eLineIdx;
		treeInfo_v[i].vTreeFlag = 0;
		treeInfo_v[i].treeType = 0;
		treeInfo_v[i].roi = { -1, -1, -1, -1 };
		int nullPtSize = 0;
		SSG_ROIRectD roi = { -1, -1, -1, -1 };
		SVzNLRange ptIdxRange = { -1, -1 };
		if (a_tree.treeNodes.size() > 0)
		{
			for (int m = 0; m < (int)a_tree.treeNodes.size(); m++)
			{
				SWD_segFeature& a_seg = a_tree.treeNodes[m];
				for (int n = a_seg.startPtIdx; n <= a_seg.endPtIdx; n++)
					treeMask_v[a_seg.lineIdx][n] = i;

				nullPtSize += a_seg.endPtIdx - a_seg.startPtIdx + 1;
				_updateROI(roi, scanLines[a_seg.lineIdx][a_seg.startPtIdx].pt3D);
				_updateROI(roi, scanLines[a_seg.lineIdx][a_seg.endPtIdx].pt3D);
				_updateRenge(ptIdxRange, a_seg.startPtIdx);
				_updateRenge(ptIdxRange, a_seg.endPtIdx);
				//scanLinesInput[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = 0x01;
				//scanLinesInput[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = 0x01;
			}
			treeInfo_v[i].treeType = nullPtSize;
			treeInfo_v[i].roi = roi;
			treeInfo_v[i].ptIdxRange = ptIdxRange;
		}
	}
	std::vector<SSG_treeInfo> treeInfo_h;
	treeInfo_h.resize(segTrees_h.size());
	for (int i = 0; i < (int)segTrees_h.size(); i++)
	{
		SWD_segFeatureTree& a_tree = segTrees_h[i];
		treeInfo_h[i].treeIdx = i;
		treeInfo_h[i].sLineIdx = a_tree.sLineIdx;
		treeInfo_h[i].eLineIdx = a_tree.eLineIdx;
		treeInfo_h[i].vTreeFlag = 0;
		treeInfo_h[i].treeType = 0;
		treeInfo_h[i].roi = { -1, -1, -1, -1 };
		int nullPtSize = 0;
		SSG_ROIRectD roi = { -1, -1, -1, -1 };
		SVzNLRange ptIdxRange = { -1, -1 };
		if (a_tree.treeNodes.size() > 0)
		{
			for (int m = 0; m < (int)a_tree.treeNodes.size(); m++)
			{
				SWD_segFeature& a_seg = a_tree.treeNodes[m];
				for (int n = a_seg.startPtIdx; n <= a_seg.endPtIdx; n++)
					treeMask_h[n][a_seg.lineIdx] = i;

				nullPtSize += a_seg.endPtIdx - a_seg.startPtIdx + 1;
				_updateROI(roi, scanLines[a_seg.startPtIdx][a_seg.lineIdx].pt3D);
				_updateROI(roi, scanLines[a_seg.endPtIdx][a_seg.lineIdx].pt3D);
				_updateRenge(ptIdxRange, a_seg.startPtIdx);
				_updateRenge(ptIdxRange, a_seg.endPtIdx);
				//scanLinesInput[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx |= 0x02;
				//scanLinesInput[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx |= 0x02;
			}
			treeInfo_h[i].treeType = nullPtSize;
			treeInfo_h[i].roi = roi;
			treeInfo_h[i].ptIdxRange = ptIdxRange;
		}
	}
	//水平和垂直目标合并
	int vTreeSize = (int)segTrees_v.size();
	int hTreeSize = (int)segTrees_h.size();
	std::vector<std::vector<int>> treeHVInfo;  //统计垂直和水平的tree的位置信息
	treeHVInfo.resize(vTreeSize);
	for (int i = 0; i < vTreeSize; i++)
	{
		treeHVInfo[i].resize(hTreeSize);
		std::fill(treeHVInfo[i].begin(), treeHVInfo[i].end(), 0);
	}
	for (int line = 0; line < lineNum; line++)
	{
		for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++)
		{
			int idx_v = treeMask_v[line][ptIdx];
			int idx_h = treeMask_h[line][ptIdx];
			if ((idx_v >= 0) && (idx_h >= 0))
				treeHVInfo[idx_v][idx_h]++;
		}
	}

	//生成候选目标
	std::vector<SSG_intPair> objects;
	for (int i = 0; i < vTreeSize; i++)
	{
		SWD_segFeatureTree& a_tree = segTrees_v[i];
		if ((a_tree.sLineIdx <= 1047) && (a_tree.eLineIdx >= 1047))
			int kkk = 1;

		int totalSize_v = treeInfo_v[i].treeType;
		int commonSize = 0;
		int hTreeIdx = -1;
		for (int j = 0; j < hTreeSize; j++)
		{
			if (commonSize < treeHVInfo[i][j])
			{
				hTreeIdx = j;
				commonSize = treeHVInfo[i][j];
			}
		}
		if (hTreeIdx >= 0)
		{
			int totalSize_h = treeInfo_h[hTreeIdx].treeType;
			int sizeV_th = (int)((double)totalSize_v * valieCommonNumRatio);
			int sizeH_th = (int)((double)totalSize_h * valieCommonNumRatio);
			if ((commonSize > sizeH_th) && (commonSize > sizeV_th))
			{
				SSG_intPair a_obj;
				a_obj.data_0 = i;
				a_obj.data_1 = hTreeIdx;
				a_obj.idx = commonSize;
				objects.push_back(a_obj);
				treeInfo_v[i].data = commonSize;
				treeInfo_h[hTreeIdx].data = commonSize;
			}
		}
	}

	//滤除相邻。每个目标保留一个
	for (int i = 0; i < (int)objects.size(); i++)
	{
		int vTreeIdx = objects[i].data_0;
		if (treeInfo_v[vTreeIdx].vTreeFlag < 0)
			continue;

		std::vector<int> neighbours;
		_searchNeighbours(vTreeIdx, 3,
			treeInfo_v[vTreeIdx].sLineIdx, treeInfo_v[vTreeIdx].eLineIdx,
			treeInfo_v[vTreeIdx].ptIdxRange, treeMask_v, neighbours);

		int bestIdx = vTreeIdx;
		int maxValue = treeInfo_v[vTreeIdx].data;
		if (neighbours.size() > 0)
		{
			for (int j = 0; j < (int)neighbours.size(); j++)
			{
				int idx = neighbours[j];
				if (maxValue < treeInfo_v[idx].data)
				{
					maxValue = treeInfo_v[idx].data;
					bestIdx = idx;
				}
			}
			if (bestIdx != vTreeIdx)
				treeInfo_v[vTreeIdx].vTreeFlag = -1;
			for (int j = 0; j < (int)neighbours.size(); j++)
			{
				int idx = neighbours[j];
				if (bestIdx != idx)
					treeInfo_v[idx].vTreeFlag = -1;
			}
		}
	}

	for (int i = 0; i < (int)objects.size(); i++)
	{
		int vTreeIdx = objects[i].data_0;
		if (treeInfo_v[vTreeIdx].vTreeFlag < 0)
			continue;

		validObjects.push_back(objects[i]);
	}
}

//工件孔定位-拓普发工件孔定位
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);
	}

	//生成量化数据，以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; //距离搜索的合格门限。小于此距离，认为搜索到的目标为有效
	std::vector< WD_workpieceInfo> allWorkpiece;
	for (int objIdx = 0; objIdx < objectSize; objIdx++)
	{
		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);
		if (idx1 < 0)
			continue;

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

		SWD_HoleInfo& p2 = holes[idx2];
		//搜索最接近distance且角度为angle的目标, 以角度为优先
		int idx3 = angleConditionDistanceSearch(
			p1.center, p0.center,
			holes,
			workpiecePara.holeDist_L, distDeviation,
			angleRange);
		if (idx3 < 0)
			continue;
		SWD_HoleInfo& p3 = holes[idx3];
		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);
		double z2 = _getMeanZ(quantiValue, center_p0p2, roi2D, rectR);
		double z3 = _getMeanZ(quantiValue, center_p1p3, roi2D, rectR);
		double z4 = _getMeanZ(quantiValue, center_p2p3, roi2D, rectR);
		
		p0.center.z = (z1 + z2) / 2;
		p1.center.z = (z1 + z3) / 2;
		p2.center.z = (z2 + z4) / 2;
		p3.center.z = (z3 + z4) / 2;
		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.z = a_pt.z + 20;  //法向，因为做过地面高平，所以法向只在z向
			a_workpiece.holesDir.push_back(a_pt);
		}
		a_workpiece.center = { (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 };

		SVzNL3DPoint y_dir;
		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 };
		double modLen = sqrt(pow(y_dir.x, 2) + pow(y_dir.y, 2));
		y_dir = { y_dir.x / modLen, y_dir.y / modLen, 0 };
		a_workpiece.y_dir = { y_dir.x * 20 + a_workpiece.center.x, y_dir.y * 20 + a_workpiece.center.y, a_workpiece.center.z };
		a_workpiece.z_dir = { a_workpiece.center.x, a_workpiece.center.y, a_workpiece.center.z + 20 };

		allWorkpiece.push_back(a_workpiece);
	}
	int workpieceNum = (int)allWorkpiece.size();
	if (workpieceNum == 0)
		return;

	//排序
	//z方向排序 
	std::vector< WD_workpieceInfo> zSortWorkpiece;
	double minZ = allWorkpiece[0].center.z;
	for (int i = 1; i < workpieceNum; i++)
	{
		if (minZ > allWorkpiece[i].center.z)
			minZ = allWorkpiece[i].center.z;
	}
	double topLayerTh = minZ + 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;

}

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::Point3f> 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::Point3f a_pt = cv::Point3f(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::Point3f a_pt = cv::Point3f(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::Point3f a_pt = cv::Point3f(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::Point3f a_pt = cv::Point3f(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::Point3f> out_inliers;
		Plane robustPlane = ransacFitPlane(
			Points3ds,
			out_inliers,
			dist_thresh,          // 内点距离阈值
			max_iter,                // 最大迭代
			stop_no_improve           // 连续多少次无提升就提前退出
		);
#endif
		res.resize(4);
		if (robustPlane.C > 0)
		{
			res[0] = -robustPlane.A;
			res[1] = -robustPlane.B;
			res[2] = -robustPlane.C;
			res[3] = -robustPlane.D;
		}
		else
		{
			res[0] = robustPlane.A;
			res[1] = robustPlane.B;
			res[2] = robustPlane.C;
			res[3] = robustPlane.D;
		}
#endif
		double normValue = sqrt(pow(res[0], 2) + pow(res[1], 2) + pow(res[2],2));
		double norm_A = res[0] / normValue;
		double norm_B = res[1] / normValue;
		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;
}