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

#define _DEBUG_OUTPUT

//version 1.0.0 : base version release to customer
//version 1.1.0 : 海瑞马码垛规划版本
//version 1.2.0 : 添加了海瑞马锥形工件定位版本（不包含二次定位）
//version 1.3.0 : 添加了海瑞马转子钢芯定位
std::string	m_strVersion = "HybridPositioning 1.3.0";
const char* wd_hybridPositioningVersion(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);
}

SVzNL3DPosition _computeMinZPoint(std::vector<SVzNL3DPosition>& points)
{
	SVzNL3DPosition peak;
	peak.nPointIdx = 0;
	peak.pt3D = { 0.0, 0.0, 0.0 };
	for (int i = 0; i < (int)points.size(); i++)
	{
		if (points[i].pt3D.z < 1e-4)
			continue;

		if (peak.pt3D.z < 1e-4)
			peak = points[i];
		else if (peak.pt3D.z > points[i].pt3D.z)
			peak = points[i];

	}
	return peak;
}

SVzNL3DPoint _computeCentroid(std::vector<SVzNL3DPosition>& points)
{
	SVzNL3DPoint centroid = { 0.0, 0.0, 0.0 };
	int counter = 0;
	for (int i = 0; i < (int)points.size(); i++)
	{
		if (points[i].pt3D.z < 1e-4)
			continue;

		counter++;
		centroid.x += points[i].pt3D.x;
		centroid.y += points[i].pt3D.y;
		centroid.z += points[i].pt3D.z;
	}
	centroid.x = centroid.x / counter;
	centroid.y = centroid.y / counter;
	centroid.z = centroid.z / counter;
	return centroid;
}

int _get2DRegion(SVzNLPositionD& a_pt2D3D, std::vector<WD_objArea2D>& objROIs)
{
	for (int i = 0; i < (int)objROIs.size(); i++)
	{
		if ((a_pt2D3D.ptLeft2D.x >= objROIs[i].roi.left) && (a_pt2D3D.ptLeft2D.x <= objROIs[i].roi.right) &&
			(a_pt2D3D.ptLeft2D.y >= objROIs[i].roi.top) && (a_pt2D3D.ptLeft2D.y <= objROIs[i].roi.bottom))
			return i;
	}
	return -1;
}

bool _compareByZValue(SVzNL3DPosition& a, SVzNL3DPosition& b)
{
	return a.pt3D.z < b.pt3D.z;
}

//生成圆周扫描数据
void _genPolarScanData(
	std::vector<SVzNL3DPosition>& points,
	const double angleScale,
	const SVzNL3DPoint polarCener,
	std::vector<std::vector<SWD_polarPt>>& polarScanData)
{
	int polarLines = (int)(360.0 / angleScale + 0.5);
	polarScanData.resize(polarLines);

	int dataSize = (int)points.size();
	for (int i = 0; i < dataSize; i++)
	{
		int line = points[i].nPointIdx >> 16;
		int ptIdx = points[i].nPointIdx & 0x0000FFFF;
		SVzNL3DPoint& a_pt = points[i].pt3D;

		double angle = atan2(a_pt.y - polarCener.y, a_pt.x - polarCener.x);
		angle = (angle / PI) * 180 + 180.0;
		double R = sqrt(pow(a_pt.y - polarCener.y, 2) + pow(a_pt.x - polarCener.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);
	}
}

//生成圆周扫描数据, 同心圆环结构
void _genPolarScanData_2(
	std::vector<SVzNL3DPosition>& points,
	const double radiusScale,
	const SVzNL3DPoint polarCener,
	std::vector<std::vector<SWD_polarPt>>& polarScanData)
{
	std::vector< SWD_polarPt> polarPoints;
	int dataSize = (int)points.size();
	double rMax = 0;
	for (int i = 0; i < dataSize; i++)
	{
		int line = points[i].nPointIdx >> 16;
		int ptIdx = points[i].nPointIdx & 0x0000FFFF;
		SVzNL3DPoint& a_pt = points[i].pt3D;

		double angle = atan2(a_pt.y - polarCener.y, a_pt.x - polarCener.x);
		angle = (angle / PI) * 180 + 180.0;
		double R = sqrt(pow(a_pt.y - polarCener.y, 2) + pow(a_pt.x - polarCener.x, 2));
		rMax = rMax < R ? R : rMax;
	
		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;

		polarPoints.push_back(a_polarPt);
	}

	int circleNum = (int)(rMax / radiusScale) + 1;
	polarScanData.resize(circleNum);
	for (int i = 0; i < (int)polarPoints.size(); i++)
	{
		double r = polarPoints[i].R;
		int idx = r / radiusScale;
		polarScanData[idx].push_back(polarPoints[i]);
	}
}

bool compareByPolarScanR(const SWD_polarPt& a, const SWD_polarPt& b) {
	return a.R < b.R;
}
bool compareByPolarScanAngle(const SWD_polarPt& a, const SWD_polarPt& b) {
	return a.angle < b.angle;
}

typedef struct
{
	int hScanFlag; //0: 垂直扫描; 1-水平扫描
	int lineIdx;
	SSG_RUN seg;
	int segValidPtNum;
	double segMeanZ;
	int LRFlag; //0:left; 1:right
	SVzNL3DPosition outsideEdgePoint;   //外侧端点
	SVzNL3DPosition centerPoint;
}SWD_indexingSeg;

void _seedSegClustering_speedUp(
	std::vector< SWD_indexingSeg>& a_cluster,
	std::vector< SWD_indexingSeg>& segs,
	std::vector<int>& flags,
	double clusterDist,
	int distType, //0 - 2d distance; 1- 3d distance 
	std::vector<std::vector<int>>& indexing2D,
	int lineNum, int ptSize, int clusterCheckWin) //搜索窗口
{
	int i = 0;
	while (1)
	{
		if (i >= a_cluster.size())
			break;

		SWD_indexingSeg& a_seed = a_cluster[i];
		int seed_line, seed_ptIdx;
		if (a_seed.hScanFlag == 0) //垂直
		{
			seed_line = a_seed.lineIdx;
			seed_ptIdx = a_seed.centerPoint.nPointIdx;
		}
		else
		{
			seed_line = a_seed.centerPoint.nPointIdx;
			seed_ptIdx = a_seed.lineIdx;
		}
		for (int line = seed_line - clusterCheckWin; line <= seed_line + clusterCheckWin; line++)
		{
			if ((line >= 0) && (line < lineNum))
			{
				for (int ptIdx = seed_ptIdx - clusterCheckWin; ptIdx <= seed_ptIdx + clusterCheckWin; ptIdx++)
				{
					if ((ptIdx >= 0) && (ptIdx < ptSize))
					{
						int backIdx = indexing2D[line][ptIdx];
						if (backIdx < 0)
							continue;
						if (flags[backIdx] < 0)
							continue;
						double dist;
						if (0 == distType)
							dist = sqrt(pow(a_seed.centerPoint.pt3D.x - segs[backIdx].centerPoint.pt3D.x, 2) + pow(a_seed.centerPoint.pt3D.y - segs[backIdx].centerPoint.pt3D.y, 2));
						else
							dist = sqrt(pow(a_seed.centerPoint.pt3D.x - segs[backIdx].centerPoint.pt3D.x, 2) + pow(a_seed.centerPoint.pt3D.y - segs[backIdx].centerPoint.pt3D.y, 2) + pow(a_seed.centerPoint.pt3D.z - segs[backIdx].centerPoint.pt3D.z, 2));
						if (dist < clusterDist)
						{
							a_cluster.push_back(segs[backIdx]);
							flags[backIdx] = -1;
						}
					}
				}
			}
		}
		i++;
	}
	return;
}

void _segsClustering_speedUp(
	std::vector< SWD_indexingSeg>& segs,
	int lineNum, int linePtSize, int clusterCheckWin, //搜索窗口
	double clusterDist,
	int distType, //0 - 2d distance; 1- 3d distance 
	std::vector<std::vector< SWD_indexingSeg>>& objClusters  //result
)
{
	int segSize = (int)segs.size();
	if (segSize == 0)
		return;

	std::vector<std::vector<int>> indexing2D;
	indexing2D.resize(lineNum);
	for (int i = 0; i < lineNum; i++)
	{
		indexing2D[i].resize(linePtSize);
		std::fill(indexing2D[i].begin(), indexing2D[i].end(), -1);
	}
	for (int i = 0; i < segSize; i++)
	{
		SWD_indexingSeg& a_seg = segs[i];
		int line, ptIdx;
		if (a_seg.hScanFlag == 0) //垂直
		{
			line = a_seg.lineIdx;
			ptIdx = a_seg.centerPoint.nPointIdx;
		}
		else
		{
			line = a_seg.centerPoint.nPointIdx;
			ptIdx = a_seg.lineIdx;
		}
		indexing2D[line][ptIdx] = i;
	}

	std::vector<int> flags;
	flags.resize(segSize);
	std::fill(flags.begin(), flags.end(), 0);

	int idx = 0;
	while (idx < (int)segs.size())
	{
		SWD_indexingSeg& a_seg = segs[idx];
		if (flags[idx] >= 0)
		{
			flags[idx] = -1;//防止重复被计算
			//新建一个cluster
			std::vector< SWD_indexingSeg> a_cluster;
			a_cluster.push_back(a_seg);
			//pts[0].nPointIdx = -1; 
			_seedSegClustering_speedUp(
				a_cluster,
				segs,
				flags,
				clusterDist,
				distType,
				indexing2D,
				lineNum, linePtSize, clusterCheckWin);
			if (a_cluster.size() >= 5)
				objClusters.push_back(a_cluster); //保存一个聚类
		}
		idx++;
	}
	return;
}


WD_workpieceInfo _computeWorkpiecePose(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SWDScanPosition centerPosition,
	const SWD_Cylinder workpieceParam,
	std::vector<SWDScanPosition>& outsideEdgePtIndice,
	std::vector<SWDScanPosition>& centerPtIndice)
{
	WD_workpieceInfo a_pose;
	memset(&a_pose, 0, sizeof(WD_workpieceInfo));

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

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

	//过滤掉凹段
	std::vector<std::vector<int>> flags;
	flags.resize(lineNum);
	for (int i = 0; i < lineNum; i++)
		flags[i].resize(linePtNum);

	//内部参数
	double topSeg_minWidth = 5.0; //
	double topSeg_maxWidth = 20.0;

	SSG_lineSegParam filterSegPara;
	filterSegPara.distScale = 0;
	filterSegPara.segGapTh_y = topSeg_maxWidth;
	filterSegPara.segGapTh_z = workpieceParam.height / 2;

	//垂直
	std::vector<std::vector<SWD_indexingSeg>> vScanLineSegs;
	for (int line = 0; line < lineNum; line++)
	{
		if (line == 83)
			int kkk = 1;
		//分段
		std::vector<SSG_RUN> simpleSegs;
		wd_getLineDataIntervals(scanLines[line], filterSegPara, simpleSegs);

		//过滤掉凹段
		std::vector<int> filterIndice;
		//若一个段首尾都比相邻段低，视为凹陷段
		for (int i = 1; i < (int)simpleSegs.size() - 1; i++)
		{
			int tail_1 = simpleSegs[i - 1].start + simpleSegs[i - 1].len - 1;
			int head_1 = simpleSegs[i].start;
			int tail_2 = simpleSegs[i].start + simpleSegs[i].len - 1;
			int head_2 = simpleSegs[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 > filterSegPara.segGapTh_z) && (z_diff_2 > filterSegPara.segGapTh_z))
				filterIndice.push_back(i);
		}
		for (int m = 0; m < (int)filterIndice.size(); m++)
		{
			int idx = filterIndice[m];
			int offset = simpleSegs[idx].start;
			for (int i = 0; i < simpleSegs[idx].len; i++)
				flags[line][offset + i] = 1;
			simpleSegs[idx].start = -1;  //invalid
		}
		//生成扩展信息
		std::vector<SWD_indexingSeg> exSegs;
		for (int m = 0; m < (int)simpleSegs.size(); m++)
		{
			if (simpleSegs[m].start < 0)
				continue;

			int start = simpleSegs[m].start;
			int end = simpleSegs[m].start + simpleSegs[m].len - 1;
			double meanZ = 0;
			int size = 0;
			int namedCenter = (start + end) / 2;
			int realCenter = -1;
			int minDist = -1;
			for (int n = start; n <= end; n++)
			{
				if (scanLines[line][n].pt3D.z > 1e-4)
				{
					meanZ += scanLines[line][n].pt3D.z;
					size++;

					int nDist = namedCenter > n ? (namedCenter - n) : (n - namedCenter);  //距离名义中心点的距离
					if (minDist < 0)
					{
						minDist = nDist;
						realCenter = n;
					}
					else if (minDist > nDist)
					{
						minDist = nDist;
						realCenter = n;
					}
				}
			}
			if (size > 0)
			{
				SWD_indexingSeg a_seg;
				memset(&a_seg, 0, sizeof(SWD_indexingSeg));
				a_seg.centerPoint.nPointIdx = realCenter;
				a_seg.centerPoint.pt3D = scanLines[line][realCenter].pt3D;
				a_seg.lineIdx = line;
				a_seg.seg = simpleSegs[m];
				a_seg.segMeanZ = meanZ / size;
				a_seg.segValidPtNum = size;
				exSegs.push_back(a_seg);
			}
		}
		if (exSegs.size() == 0)
			return a_pose; 
		//去除低的段：如果两个段的meanZ相差达到工件高度的一半，视为低的段
		int highestSeg = 0;
		for (int m = 1; m < (int)exSegs.size(); m++)
		{
			if (exSegs[highestSeg].segMeanZ > exSegs[m].segMeanZ)
				highestSeg = m;
		}

		double highestZ = exSegs[highestSeg].segMeanZ;
		std::vector<SWD_indexingSeg> validExSegs;
		for (int m = 0; m < (int)exSegs.size(); m++)
		{
			double zDiff = exSegs[m].segMeanZ - highestZ;
			if (zDiff < workpieceParam.height / 2)
			{
				validExSegs.push_back(exSegs[m]);
			}
		}
		vScanLineSegs.push_back(validExSegs);
	}

	//水平扫描
	std::vector<std::vector<SWD_indexingSeg>> hScanLineSegs;
	for (int line = 0; line < linePtNum; line++)
	{
		//分段
		std::vector<SSG_RUN> simpleSegs;
		wd_getLineDataIntervals(hLines[line], filterSegPara, simpleSegs);

		//过滤掉凹段
		std::vector<int> filterIndice;
		//若一个段首尾都比相邻段低，视为凹陷段
		for (int i = 1; i < (int)simpleSegs.size() - 1; i++)
		{
			int tail_1 = simpleSegs[i - 1].start + simpleSegs[i - 1].len - 1;
			int head_1 = simpleSegs[i].start;
			int tail_2 = simpleSegs[i].start + simpleSegs[i].len - 1;
			int head_2 = simpleSegs[i + 1].start;
			double z_diff_1 = hLines[line][head_1].pt3D.z - hLines[line][tail_1].pt3D.z;
			double z_diff_2 = hLines[line][tail_2].pt3D.z - hLines[line][head_2].pt3D.z;
			if ((z_diff_1 > filterSegPara.segGapTh_z) && (z_diff_2 > filterSegPara.segGapTh_z))
				filterIndice.push_back(i);
		}
		for (int m = 0; m < (int)filterIndice.size(); m++)
		{
			int idx = filterIndice[m];
			int offset = simpleSegs[idx].start;
			for (int i = 0; i < simpleSegs[idx].len; i++)
				flags[offset + i][line] = 1;
			simpleSegs[idx].start = -1; //invalidate
		}

		//生成扩展信息
		std::vector<SWD_indexingSeg> exSegs;
		for (int m = 0; m < (int)simpleSegs.size(); m++)
		{
			if (simpleSegs[m].start < 0)
				continue;

			int start = simpleSegs[m].start;
			int end = simpleSegs[m].start + simpleSegs[m].len - 1;
			double meanZ = 0;
			int size = 0;
			int namedCenter = (start + end) / 2;
			int realCenter = -1;
			int minDist = -1;
			for (int n = start; n <= end; n++)
			{
				if (hLines[line][n].pt3D.z > 1e-4)
				{
					meanZ += hLines[line][n].pt3D.z;
					size++;

					int nDist = namedCenter > n ? (namedCenter - n) : (n - namedCenter);  //距离名义中心点的距离
					if (minDist < 0)
					{
						minDist = nDist;
						realCenter = n;
					}
					else if (minDist > nDist)
					{
						minDist = nDist;
						realCenter = n;
					}
				}
			}
			if (size > 0)
			{
				SWD_indexingSeg a_seg;
				memset(&a_seg, 0, sizeof(SWD_indexingSeg));
				a_seg.centerPoint.nPointIdx = realCenter;
				a_seg.centerPoint.pt3D = scanLines[realCenter][line].pt3D;
				a_seg.lineIdx = line;
				a_seg.seg = simpleSegs[m];
				a_seg.segMeanZ = meanZ / size;
				a_seg.segValidPtNum = size;
				exSegs.push_back(a_seg);
			}
		}
		//去除低的段：如果两个段的meanZ相差达到工件高度的一半，视为低的段
		int highestSeg = 0;
		for (int m = 1; m < (int)exSegs.size(); m++)
		{
			if (exSegs[highestSeg].segMeanZ > exSegs[m].segMeanZ)
				highestSeg = m;
		}

		double highestZ = exSegs[highestSeg].segMeanZ;
		std::vector<SWD_indexingSeg> validExSegs;
		for (int m = 0; m < (int)exSegs.size(); m++)
		{
			double zDiff = exSegs[m].segMeanZ - highestZ;
			if (zDiff < workpieceParam.height / 2)
			{
				validExSegs.push_back(exSegs[m]);
			}
		}
		hScanLineSegs.push_back(validExSegs);
	}

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

	SVzNL3DPoint centerPoint = scanLines[centerPosition.lineIdx][centerPosition.ptIdx].pt3D;
	std::vector< SWD_indexingSeg> segsTop;
	//生成工件顶面上的点: 宽度大于一个门限（滤噪）；边界点不能是边界
	//垂直扫描处理
	for (int line = 0; line < lineNum; line++)
	{
		if (line == 83)
			int kkk = 1;
		int mostLeftSegId = -1;  //对于垂直扫描，实际上是最上面
		int mostRightSegId = -1;//对于垂直扫描，实际上是最下面
		for (int i = 0; i < (int)vScanLineSegs[line].size(); i++)
		{
			SWD_indexingSeg& exSeg = vScanLineSegs[line][i];

			int idx1 = exSeg.seg.start;
			int idx2 = exSeg.seg.start + exSeg.seg.len - 1;
			double width = abs(scanLines[line][idx1].pt3D.y - scanLines[line][idx2].pt3D.y);
			SVzNL3DPoint segCenter = exSeg.centerPoint.pt3D;
			double distToCircleCenter = sqrt(pow(centerPoint.x - segCenter.x, 2) + pow(centerPoint.y - segCenter.y, 2));
			if ( (width > topSeg_minWidth) && (width < topSeg_maxWidth) && (distToCircleCenter < workpieceParam.radius))
			{
				if ((mostLeftSegId < 0) && (idx2 < centerPosition.ptIdx) &&(idx1 > 0 ))
					mostLeftSegId = i;

				if( (idx1 > centerPosition.ptIdx) && (idx2 < linePtNum - 1))
					mostRightSegId = i;
			}
		}
		if (mostLeftSegId >= 0)
		{
			SWD_indexingSeg& left_seg = vScanLineSegs[line][mostLeftSegId];
			int edge2 = left_seg.seg.start + left_seg.seg.len - 1;
			if ((edge2 < centerPosition.ptIdx) && (left_seg.seg.start > 0)) //最左侧，不能是起点（如果是起点可能连接到其他工件上)
			{
				int outEdgeIdx = left_seg.seg.start;
				left_seg.hScanFlag = 0;
				left_seg.LRFlag = 0;
				left_seg.outsideEdgePoint.nPointIdx = outEdgeIdx;
				left_seg.outsideEdgePoint.pt3D = scanLines[line][outEdgeIdx].pt3D;
				segsTop.push_back(left_seg);
			}
		}
		if(mostRightSegId >= 0)
		{
			SWD_indexingSeg& right_seg = vScanLineSegs[line][mostRightSegId];
			int edge2 = right_seg.seg.start + right_seg.seg.len - 1;
			if ((edge2 > centerPosition.ptIdx) && (edge2 < linePtNum -1)) //最右侧，不能是终点（如果是终点可能连接到其他工件上)
			{
				int outEdgeIdx = right_seg.seg.start + right_seg.seg.len - 1;
				right_seg.hScanFlag = 0;
				right_seg.LRFlag = 1;
				right_seg.outsideEdgePoint.nPointIdx = outEdgeIdx;
				right_seg.outsideEdgePoint.pt3D = scanLines[line][outEdgeIdx].pt3D;
				segsTop.push_back(right_seg);
			}
		}
	}
	//水平水平处理
	for (int line = 0; line < linePtNum; line++)
	{
		int mostLeftSegId = -1;  //对于水平扫描，实际上是最左面
		int mostRightSegId = -1;//对于水平扫描，实际上是最右面
		for (int i = 0; i < (int)hScanLineSegs[line].size(); i++)
		{
			SWD_indexingSeg& a_seg = hScanLineSegs[line][i];

			int idx1 = a_seg.seg.start;
			int idx2 = a_seg.seg.start + a_seg.seg.len - 1;
			double width = abs(hLines[line][idx1].pt3D.y - hLines[line][idx2].pt3D.y);
			SVzNL3DPoint segCenter = a_seg.centerPoint.pt3D;
			double distToCircleCenter = sqrt(pow(centerPoint.x - segCenter.x, 2) + pow(centerPoint.y - segCenter.y, 2));
			if ( (width > topSeg_minWidth) && (width < topSeg_maxWidth) && (distToCircleCenter < workpieceParam.radius))
			{
				if ((mostLeftSegId < 0) && (idx2 < centerPosition.lineIdx) && (idx1 > 0))
					mostLeftSegId = i;

				if( (idx1 > centerPosition.lineIdx) && (idx2 < lineNum - 1))
					mostRightSegId = i;
			}
		}
		if (mostLeftSegId >= 0)
		{
			SWD_indexingSeg& left_seg = hScanLineSegs[line][mostLeftSegId];
			int edge2 = left_seg.seg.start + left_seg.seg.len - 1;
			if ((edge2 < centerPosition.lineIdx) && (left_seg.seg.start > 0)) //最左侧，不能是起点（如果是起点可能连接到其他工件上)
			{
				int outEdgeIdx = left_seg.seg.start;
				left_seg.hScanFlag = 1;
				left_seg.LRFlag = 0;
				left_seg.outsideEdgePoint.nPointIdx = outEdgeIdx;
				left_seg.outsideEdgePoint.pt3D = scanLines[outEdgeIdx][line].pt3D;
				segsTop.push_back(left_seg);
			}
		}
		if (mostRightSegId >= 0)
		{
			SWD_indexingSeg& right_seg = hScanLineSegs[line][mostRightSegId];
			int edge2 = right_seg.seg.start + right_seg.seg.len - 1;
			if ((edge2 > centerPosition.lineIdx) && (edge2 < lineNum - 1)) //最右侧，不能是终点（如果是终点可能连接到其他工件上)
			{
				int outEdgeIdx = right_seg.seg.start + right_seg.seg.len - 1;
				right_seg.hScanFlag = 1;
				right_seg.LRFlag = 1;
				right_seg.outsideEdgePoint.nPointIdx = outEdgeIdx;
				right_seg.outsideEdgePoint.pt3D = scanLines[outEdgeIdx][line].pt3D;
				segsTop.push_back(right_seg);
			}

		}
	}

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

	//判断是否为合格目标
	if (objClusters.size() == 0)
		return a_pose;
	
	std::vector<double> clusterMeanZ;
	clusterMeanZ.resize(objClusters.size());
	for (int i = 0; i < (int)objClusters.size(); i++)
	{
		std::vector< SWD_indexingSeg>& a_cluster = objClusters[i];
		int nodeSize = (int)a_cluster.size();
		double meanZ = 0;
		for (int m = 0; m < nodeSize; m++)
			meanZ += a_cluster[m].centerPoint.pt3D.z;
		meanZ = meanZ / nodeSize;
		clusterMeanZ[i] = meanZ;
	}
	//检测相互之间的高差
	bool isValid = true;
	for (int m = 0; m < (int)objClusters.size(); m++)
	{
		for (int n = m + 1; n < (int)objClusters.size(); n++)
		{
			double zDiff = abs(clusterMeanZ[m] - clusterMeanZ[n]);
			if (zDiff > workpieceParam.height / 2)
			{
				isValid = false;
				break;
			}
		}
		if (false == isValid)
			break;
	}
	if (false == isValid)
		return a_pose;


	//计算整个的meanZ, 作为工件的meanZ
	double objZ = 0;
	int totalNodeSize = 0;
	for (int i = 0; i < (int)objClusters.size(); i++)
	{
		std::vector< SWD_indexingSeg>& a_cluster = objClusters[i];
		int nodeSize = (int)a_cluster.size();
		totalNodeSize += nodeSize;
		objZ += nodeSize * clusterMeanZ[i];
	}
	objZ = objZ / totalNodeSize;

	//使用边缘点进行圆拟合计算中心点
	std::vector<SVzNL3DPoint> pointArray;
	for (int i = 0; i < (int)objClusters.size(); i++)
	{
		std::vector< SWD_indexingSeg>& a_cluster = objClusters[i];
		int nodeSize = (int)a_cluster.size();
		for (int m = 0; m < nodeSize; m++)
			pointArray.push_back(a_cluster[m].outsideEdgePoint.pt3D);
	}
	SVzNL3DPoint center;
	double radius;
	double err = fitCircleByLeastSquare(pointArray, center, radius);

	//迭代一次
	pointArray.clear();
	for (int i = 0; i < (int)objClusters.size(); i++)
	{
		std::vector< SWD_indexingSeg>& a_cluster = objClusters[i];
		int nodeSize = (int)a_cluster.size();
		for (int m = 0; m < nodeSize; m++)
		{
			SWD_indexingSeg& a_seg = a_cluster[m];
			double r1 = sqrt(pow(center.x - a_seg.outsideEdgePoint.pt3D.x, 2) + pow(center.y - a_seg.outsideEdgePoint.pt3D.y, 2));
			if (r1 < workpieceParam.radius * 1.05)
			{
				pointArray.push_back(a_cluster[m].outsideEdgePoint.pt3D);
				//生成标注需要的信息
				if (a_seg.hScanFlag == 0)
				{
					SWDScanPosition pos_center = { a_seg.lineIdx, a_seg.centerPoint.nPointIdx };
					centerPtIndice.push_back(pos_center);
					SWDScanPosition pos_edge = { a_seg.lineIdx, a_seg.outsideEdgePoint.nPointIdx };
					outsideEdgePtIndice.push_back(pos_edge);
				}
				else
				{
					SWDScanPosition pos_center = { a_seg.centerPoint.nPointIdx, a_seg.lineIdx };
					centerPtIndice.push_back(pos_center);
					SWDScanPosition pos_edge = { a_seg.outsideEdgePoint.nPointIdx, a_seg.lineIdx };
					outsideEdgePtIndice.push_back(pos_edge);
				}
			}
		}
	}

	err = fitCircleByLeastSquare(pointArray, center, radius);
	a_pose.center = { center.x, center.y, objZ };
	a_pose.value = radius;
	a_pose.z_dir = { 0.0, 0.0, 1.0 };
	a_pose.y_dir = { 0.0, 0.0, 0.0 };
	a_pose.x_dir = { 0.0, 0.0, 0.0 };
	return a_pose;
}

typedef struct
{
	std::vector<SWDScanPosition> mappings;//每个2D点可能对应多个3D点
}mapping2DTo3D;

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_HRM_RotorCorePositioning(
	std::vector< std::vector<SVzNLPositionD>>& scanLinesInput,
	std::vector<WD_objArea2D>& objROIs,
	const SSG_planeCalibPara groundCalibPara,
	const SWD_Cylinder workpieceParam,  //圆柱形工件的标称半径和高度
	std::vector< WD_workpieceInfo>& workpiecePositions,
#ifdef _OUTPUT_DEBUG_DATA
	std::vector<std::vector<SVzNL3DPosition>>& debugScanLines,
#endif
	int* errCode)
{
	*errCode = 0;

	if (objROIs.size() == 0)
	{
		*errCode = SX_ERR_ZERO_2D_OBJECTS;
		return;
	}

	int lineNum = (int)scanLinesInput.size();
	int linePtNum = (int)scanLinesInput[0].size();
	int maxU = 0; //2D图像的最大Col
	int maxV = 0; //2D图像的最大Row
	for (int line = 0; line < lineNum; line++)
	{
		for (int ptIdx = 0; ptIdx < (int)scanLinesInput[line].size(); ptIdx++)
		{
			if (scanLinesInput[line][ptIdx].pt3D.z < 1e-4)
				continue;

			maxU = maxU < scanLinesInput[line][ptIdx].ptLeft2D.x ? scanLinesInput[line][ptIdx].ptLeft2D.x : maxU;
			maxV = maxV < scanLinesInput[line][ptIdx].ptLeft2D.y ? scanLinesInput[line][ptIdx].ptLeft2D.y : maxV;
		}
	}

	//生成图像与3d的对应表
	const int imgCols = maxU+1;
	const int imgRows = maxV+1;
	std::vector<std::vector<mapping2DTo3D>> mappingTable;  //按图像大小建立。
	mappingTable.resize(imgCols);  //与扫描线方向对应
	for (int i = 0; i < imgCols; i++)
		mappingTable[i].resize(imgRows);

	std::vector<std::vector<SVzNL3DPosition>> scanLines;
	scanLines.resize(lineNum);
	for (int line = 0; line < lineNum; line++)
	{
		scanLines[line].resize(linePtNum);
		for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++)
		{
			scanLinesInput[line][ptIdx].nPointIdx = ptIdx;
			scanLines[line][ptIdx].pt3D = scanLinesInput[line][ptIdx].pt3D;

			if (scanLinesInput[line][ptIdx].pt3D.z < 1e-4)
				continue;

			SWDScanPosition a_pos;
			a_pos.lineIdx = line;
			a_pos.ptIdx = ptIdx;
			int u = scanLinesInput[line][ptIdx].ptLeft2D.x;
			int v = scanLinesInput[line][ptIdx].ptLeft2D.y;
			mappingTable[u][v].mappings.push_back(a_pos);
		}
	}
	//进行调平处理
	for (int i = 0; i < lineNum; i++)
	{
		//调平，去除地面
		wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1);
	}

	//产生水平扫描数据
	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_h[j][line].nPointIdx = line;
			scanLines_h[j][line].pt3D = { scanLines[line][j].pt3D.y, scanLines[line][j].pt3D.x, scanLines[line][j].pt3D.z };
		}
	}

	//去除垂直段
	SSG_cornerParam removeVertialPara;
	memset(&removeVertialPara, 0, sizeof(SSG_cornerParam));
	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（会转义使用）
	}
	//将垂直线段去除
	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;
		}
	}

	int objNum =  (int)objROIs.size();
	std::vector<WD_workpieceInfo> allWorkpiece;
	for (int idx = 0; idx < objNum; idx++)
	{
		WD_objArea2D& obj_roi = objROIs[idx];
		double cx = (obj_roi.roi.left + obj_roi.roi.right) / 2;
		double cy = (obj_roi.roi.top + obj_roi.roi.bottom) / 2;
		double radius = cx - obj_roi.roi.left;
		radius = radius * 1.414;

		int L = (int)(cx - radius);
		int R = (int)(cx + radius + 0.5);
		int T = (int)(cy - radius);
		int B = (int)(cy + radius + 0.5);
		int nCx = (int)cx;
		int nCy = (int)cy;

		//统计ROI中的扫描线和PtIdx范围
		SVzNLRange roiLineIndice = { INT_MAX, 0 };
		SVzNLRange roiPtIndice = { INT_MAX, 0 };
		for (int x = L; x <= R; x++)
		{
			for (int y = T; y <= B; y++)
			{
				mapping2DTo3D& a_mapping = mappingTable[x][y];
				for (int m = 0; m < (int)a_mapping.mappings.size(); m++)
				{
					int lineIdx = a_mapping.mappings[m].lineIdx;
					int ptIdx = a_mapping.mappings[m].ptIdx;
					if (scanLines[lineIdx][ptIdx].pt3D.z > 1e-4)
					{
						scanLinesInput[lineIdx][ptIdx].nPointIdx &= 0xffff0000;
						scanLinesInput[lineIdx][ptIdx].nPointIdx |= (idx + 1);
						roiLineIndice.nMin = roiLineIndice.nMin > lineIdx ? lineIdx : roiLineIndice.nMin;
						roiLineIndice.nMax = roiLineIndice.nMax < lineIdx ? lineIdx : roiLineIndice.nMax;
						roiPtIndice.nMin = roiPtIndice.nMin > ptIdx ? ptIdx : roiPtIndice.nMin;
						roiPtIndice.nMax = roiPtIndice.nMax < ptIdx ? ptIdx : roiPtIndice.nMax;
					}
				}
			}
		}

		//生成ROI中的扫描数据
		SWDScanPosition centerPosition = { 0, 0 };
		int minDist = -1;
		int roiLines = roiLineIndice.nMax - roiLineIndice.nMin + 1;
		int roiLinePtNum = roiPtIndice.nMax - roiPtIndice.nMin + 1;
		std::vector< std::vector<SVzNL3DPosition>> roiScanLines;
		roiScanLines.resize(roiLines);
		for (int line = 0; line < roiLines; line++)
			roiScanLines[line].resize(roiLinePtNum);
		for (int x = L; x <= R; x++)
		{
			for (int y = T; y <= B; y++)
			{
				mapping2DTo3D& a_mapping = mappingTable[x][y];
				for (int m = 0; m < (int)a_mapping.mappings.size(); m++)
				{
					int lineIdx = a_mapping.mappings[m].lineIdx;
					int ptIdx = a_mapping.mappings[m].ptIdx;
					if ((lineIdx == 275) && (ptIdx == 929))
						int kkk = 1;
					if (scanLines[lineIdx][ptIdx].pt3D.z > 1e-4)
					{
						scanLines[lineIdx][ptIdx].nPointIdx &= 0xffff0000;
						scanLines[lineIdx][ptIdx].nPointIdx |= idx + 1;
						int roiLineIdx = lineIdx - roiLineIndice.nMin;
						int roiPtIdx = ptIdx - roiPtIndice.nMin;
						roiScanLines[roiLineIdx][roiPtIdx].nPointIdx = 0;
						roiScanLines[roiLineIdx][roiPtIdx].pt3D = scanLines[lineIdx][ptIdx].pt3D;
						//搜索中心点
						int dist = abs(x - nCx) + abs(y - nCy);
						if (minDist < 0)
						{
							minDist = dist;
							centerPosition = { roiLineIdx, roiPtIdx };
						}
						else if (minDist > dist)
						{
							minDist = dist;
							centerPosition = { roiLineIdx, roiPtIdx };
						}
					}
				}
			}
		}

		//判断工件种类，计算工件姿态
		std::vector<SWDScanPosition> outsideEdgePtIndice;
		std::vector<SWDScanPosition> centerPtIndice;
		WD_workpieceInfo a_workpiece = _computeWorkpiecePose(
			roiScanLines,
			centerPosition,
			workpieceParam,
			outsideEdgePtIndice,
			centerPtIndice);
	
#ifdef _OUTPUT_DEBUG_DATA
		if (a_workpiece.center.z > 1e-4)
		{
			//标注
			for (int i = 0; i < (int)outsideEdgePtIndice.size(); i++)
			{
				int line = outsideEdgePtIndice[i].lineIdx + roiLineIndice.nMin;
				int ptIdx = outsideEdgePtIndice[i].ptIdx + roiPtIndice.nMin;
				scanLines[line][ptIdx].nPointIdx |= 0x10000;
			}
			for (int i = 0; i < (int)centerPtIndice.size(); i++)
			{
				int line = centerPtIndice[i].lineIdx + roiLineIndice.nMin;
				int ptIdx = centerPtIndice[i].ptIdx + roiPtIndice.nMin;
				scanLines[line][ptIdx].nPointIdx |= 0x20000;
			}
		}
#endif

		if (a_workpiece.center.z > 1e-4)
			allWorkpiece.push_back(a_workpiece);

	}
	if (allWorkpiece.size() == 0)
		return;

	//排序
	//搜索最高目标
	double highest_z = allWorkpiece[0].center.z;
	for (int i = 1; i < (int)allWorkpiece.size(); i++)
		highest_z = highest_z > allWorkpiece[i].center.z ? allWorkpiece[i].center.z : highest_z;
	//z方向排序 
	std::vector< WD_workpieceInfo> zSortWorkpiece;
	double topLayerTh = highest_z + workpieceParam.height * 0.75;
	for (int i = 0; i < (int)allWorkpiece.size(); 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, workpieceParam.radius);
		workpiecePositions.insert(workpiecePositions.end(), firstLine.begin(), firstLine.end());
		zSortWorkpiece.clear();
		zSortWorkpiece.insert(zSortWorkpiece.end(), restWorkpiece.begin(), restWorkpiece.end());
	}

	//旋转回去
	int workpieceNum = (int)workpiecePositions.size();
	for (int i = 0; i < workpieceNum; i++)
	{
		SVzNL3DPoint rpt;
		rpt =  wd_ptRotate(workpiecePositions[i].center, groundCalibPara.invRMatrix);
		workpiecePositions[i].center = rpt;
		rpt = wd_ptRotate(workpiecePositions[i].y_dir, groundCalibPara.invRMatrix);
		workpiecePositions[i].y_dir = rpt;
		rpt = wd_ptRotate(workpiecePositions[i].z_dir, groundCalibPara.invRMatrix);
		workpiecePositions[i].z_dir = rpt;
		rpt = wd_ptRotate(workpiecePositions[i].x_dir, groundCalibPara.invRMatrix);
		workpiecePositions[i].x_dir = rpt;
	}

#ifdef _OUTPUT_DEBUG_DATA
	debugScanLines.resize(scanLines.size());
	for (int line = 0; line < (int)scanLines.size(); line++)
	{
		debugScanLines[line].resize(scanLines[line].size());
		for (int j = 0; j < (int)scanLines[line].size(); j++)
			debugScanLines[line][j] = scanLines[line][j];
	}
#endif
	return;
}

void wd_HRM_TaperedWorkpiecePositioning(
	std::vector< std::vector<SVzNLPositionD>>& scanLinesInput,
	std::vector<WD_objArea2D>& objROIs,
	const SSG_planeCalibPara groundCalibPara,
	std::vector< WD_workpieceInfo>& workpiecePositions,
	int* errCode)
{
	*errCode = 0;

	if (objROIs.size() == 0)
	{
		*errCode = SX_ERR_ZERO_2D_OBJECTS;
		return;
	}

	std::vector<std::vector<SVzNL3DPosition>> rgnPoints;
	rgnPoints.resize(objROIs.size());
	for (int line = 0; line < (int)scanLinesInput.size(); line++)
	{
		for (int ptIdx = 0; ptIdx < (int)scanLinesInput[line].size(); ptIdx++)
		{
			scanLinesInput[line][ptIdx].nPointIdx = 0; //转义使用
			if (scanLinesInput[line][ptIdx].pt3D.z < 1e-4)
				continue;

			int rgnIdx = _get2DRegion(scanLinesInput[line][ptIdx], objROIs);
			if (rgnIdx >= 0)
			{
				SVzNL3DPosition a_rgnPt;
				a_rgnPt.nPointIdx = (line << 16) | (ptIdx & 0xffff);
				a_rgnPt.pt3D = scanLinesInput[line][ptIdx].pt3D;
				rgnPoints[rgnIdx].push_back(a_rgnPt);
				scanLinesInput[line][ptIdx].nPointIdx = rgnIdx + 1;
			}
		}
	}

	//统计每个region的最高点
	std::vector< SVzNL3DPosition> rgnPeaks;
	for (int i = 0; i < (int)rgnPoints.size(); i++)
	{
		SVzNL3DPosition peakPoint = _computeMinZPoint(rgnPoints[i]);
		SVzNL3DPosition a_peak;
		a_peak.nPointIdx = i;
		a_peak.pt3D = peakPoint.pt3D;
		rgnPeaks.push_back(a_peak);
	}

	//按高度排序 
	std::sort(rgnPeaks.begin(), rgnPeaks.end(), _compareByZValue);
	for (int i = 0; i < (int)rgnPeaks.size(); i++)
	{
		WD_workpieceInfo a_obj;
		memset(&a_obj, 0, sizeof(WD_workpieceInfo));
		a_obj.center = rgnPeaks[i].pt3D;
		workpiecePositions.push_back(a_obj);
	}
	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;
}

//逆时针旋转时 θ > 0 ；顺时针旋转时 θ < 0 
cv::Point2f _rotate2D(cv::Point2f pt, double sinTheta, double cosTheta)
{
	return (cv::Point2f((float)(pt.x * cosTheta - pt.y * sinTheta), (float)(pt.x * sinTheta + pt.y * cosTheta)));
}

//料筐码放：获取料筐尺寸、料筐姿态、料筐中心点坐标
#if 0
//检测层板边缘。层板小于料筐
WD_HRM_BinInfo wd_HRM_getBinSize(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SSG_cornerParam cornerPara,
	int* errCode)
{
	*errCode = 0;
	WD_HRM_BinInfo resultPose;
	memset(&resultPose, 0, sizeof(WD_HRM_BinInfo));

	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、拟合

	//内部参数
	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 = 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
	);

	//保留最大的目标
	std::vector<double> objMeanZ;
	std::vector<SVzNLRangeD> objZRange;
	objMeanZ.resize(objClusters.size());
	objZRange.resize(objClusters.size());
	int maxSizeId = -1;
	double maxSize = 0;
	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;
		double size = w * h;

		SVzNLRangeD zRange;
		double meanZ = _getListMeanZ(objClusters[i], zRange);
		objMeanZ[i] = meanZ;
		objZRange[i] = zRange;

		if (maxSize < size)
		{
			maxSize = size;
			maxSizeId = i;
		}

	}

	//迭代：消除底板和层板的边缘的扫描毛刺



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

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

	computePlaneNormalByPCA(
		bottomCluster,
		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.bottomNormal = vec_normal;


	return resultPose;
}
#else
//检测料筐边缘，以中心点为基准进行规划
WD_HRM_BinInfo wd_HRM_getBinSize(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SSG_planeCalibPara calibPara,
	const double binHeight, //料筐高度
	int* errCode)
{
	*errCode = 0;
	WD_HRM_BinInfo resultPose;
	memset(&resultPose, 0, sizeof(WD_HRM_BinInfo));

	//内部参数
	SVzNLRangeD binTopSliceRange = {calibPara.planeHeight- binHeight-5.0, calibPara.planeHeight - binHeight + 5.0};  //料筐高度Z层切范围

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

	//地面调平
	for (int i = 0; i < lineNum; i++)
		wd_lineDataR(scanLines[i], calibPara.planeCalib, -1);//调平

	//Z层切
	std::vector<SVzNL3DPosition> zSliceData;
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			scanLines[line][j].nPointIdx = 0;
			if ((scanLines[line][j].pt3D.z > binTopSliceRange.min) && (scanLines[line][j].pt3D.z < binTopSliceRange.max))
			{
				SVzNL3DPosition a_pt;
				a_pt.nPointIdx = (line << 16) | j & 0xffff;
				a_pt.pt3D = scanLines[line][j].pt3D;
				zSliceData.push_back(a_pt);
				scanLines[line][j].nPointIdx = 1;  //标注
			}
		}
	}

	SVzNLRangeD dataZRange;
	double zSliceZ = _getListMeanZ(zSliceData, dataZRange);
	//计算轮廓
	// 最小外接矩形
	std::vector<cv::Point2f> points;
	for (int i = 0; i < (int)zSliceData.size(); i++)
	{
		cv::Point2f a_pt = cv::Point2f(zSliceData[i].pt3D.x, zSliceData[i].pt3D.y);
		points.push_back(a_pt);
	}
	cv::RotatedRect rect = minAreaRect(points);
	cv::Point2f vertices[4];
	rect.points(vertices);
	double width = rect.size.width;  //投影的宽和高
	double height = rect.size.height;
	if (width < height)
	{
		double tmp = height;
		height = width;
		width = tmp;
	}
	//计算姿态角vertices[0]是旋转点
	double dist_v0v3 = sqrt(pow(vertices[0].x - vertices[3].x, 2) + pow(vertices[0].y - vertices[3].y, 2));
	double width_diff = abs(dist_v0v3 - width);
	double pose_yaw;
	if (CV_VERSION == "3.2.0")
	{
		if (width_diff < 10.0)//width方向
		{
			pose_yaw = -rect.angle;
		}
		else //长度方向
		{
			pose_yaw = -rect.angle - 90;
			if (pose_yaw < -90)
				pose_yaw = 180 + pose_yaw;
		}
	}
	else  //if (CV_VERSION == "4.8.0")
	{
		if (width_diff < 10.0)  //width方向
		{
			pose_yaw = -rect.angle;
		}
		else//长度方向
		{
			pose_yaw = -rect.angle + 90;
			if (pose_yaw > 90)
				pose_yaw = pose_yaw - 180;
		}
	}
	//生成料筐信息
	double binZ = calibPara.planeHeight - binHeight;
	double sinTheta = sin(-PI * pose_yaw / 180);
	double cosTheta = cos(-PI * pose_yaw / 180);
	resultPose.center = { rect.center.x, rect.center.y, binZ };
	resultPose.bottomNormal = { 0.0, 0.0, 1.0 };
	resultPose.x_dir = { cosTheta , sinTheta , 0.0 };
	resultPose.y_dir = vec3_cross(resultPose.bottomNormal, resultPose.x_dir);  //叉乘出y_dir
	resultPose.length = width;
	resultPose.width = height;
	resultPose.binTopZ = zSliceZ;
	resultPose.minRectVertex[0] = { vertices[0].x, vertices[0].y, binZ };
	resultPose.minRectVertex[1] = { vertices[1].x, vertices[1].y, binZ };
	resultPose.minRectVertex[2] = { vertices[2].x, vertices[2].y, binZ };
	resultPose.minRectVertex[3] = { vertices[3].x, vertices[3].y, binZ };
	
	return resultPose;
}
#endif

//料筐码放：获取工件尺寸
WD_HRM_workpieceSizeInfo wd_HRM_getWorkpieceSize(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	std::vector<WD_HRM_workpieceSizeInfo>& standardWorkpieceSize, 
	const SSG_cornerParam cornerPara,
	const SSG_planeCalibPara calibPara,
	int* errCode)
{
	*errCode = 0;
	WD_HRM_workpieceSizeInfo resultInfo;
	memset(&resultInfo, 0, sizeof(WD_HRM_workpieceSizeInfo));

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

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

	//地面调平
	for (int i = 0; i < lineNum; i++)
		wd_lineDataR(scanLines[i], calibPara.planeCalib, -1);//调平

	//产生水平扫描数据
	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、拟合

	//内部参数
	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 = 10.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
	);

	//取最大的目标为层板
	std::vector<double> objMeanZ;
	std::vector<SVzNLRangeD> objZRange;
	std::vector< SSG_ROIRectD> objROIs;
	objMeanZ.resize(objClusters.size());
	objZRange.resize(objClusters.size());
	objROIs.resize(objClusters.size());
	int maxSizeId = -1;
	double maxSize = 0;
	for (int i = 0; i < (int)objClusters.size(); i++)
	{
		SSG_ROIRectD a_roi = _getListROI(objClusters[i]);
		objROIs[i] = a_roi;

		SVzNLRangeD zRange;
		double meanZ = _getListMeanZ(objClusters[i], zRange);
		objMeanZ[i] = meanZ;
		objZRange[i] = zRange;

		double w = a_roi.right - a_roi.left;
		double h = a_roi.bottom - a_roi.top;
		double size = w * h;
		if (maxSize < size)
		{
			maxSize = size;
			maxSizeId = i;
		}
	}

	//取层板ROI内的目标为工件
	SSG_ROIRectD& layerBoardROI = objROIs[maxSizeId];
	double layerBoardZ = objMeanZ[maxSizeId];
	int workpieceClusterId = -1;
	for (int i = 0; i< (int)objClusters.size(); i++)
	{
		if (i == maxSizeId)
			continue;

		SSG_ROIRectD& a_roi = objROIs[i];
		double obj_z = objMeanZ[i];
		if ((a_roi.left > layerBoardROI.left) && (a_roi.right < layerBoardROI.right) &&
			(a_roi.top > layerBoardROI.top) && (a_roi.bottom < layerBoardROI.bottom) && (obj_z < layerBoardZ))
		{
			if (workpieceClusterId < 0)
				workpieceClusterId = i;
			else if (objClusters[workpieceClusterId].size() < objClusters[i].size())
				workpieceClusterId = i;
		}
	}

	if(workpieceClusterId <0)
	{
		*errCode = SX_ERR_ZERO_OBJECTS;
		return resultInfo;
	}

	std::vector< SVzNL3DPosition>& layerBoardCluster = objClusters[maxSizeId];
	std::vector< SVzNL3DPosition>& workpieceCluster = objClusters[workpieceClusterId];
	double workpieceHeight = layerBoardZ - objZRange[workpieceClusterId].min;
	//标注
	for (int i = 0; i < (int)layerBoardCluster.size(); i++)
	{
		int line = layerBoardCluster[i].nPointIdx >> 16;
		int ptIdx = layerBoardCluster[i].nPointIdx & 0x0000FFFF;
		scanLines[line][ptIdx].nPointIdx = 1;
	}
	for (int i = 0; i < (int)workpieceCluster.size(); i++)
	{
		int line = workpieceCluster[i].nPointIdx >> 16;
		int ptIdx = workpieceCluster[i].nPointIdx & 0x0000FFFF;
		scanLines[line][ptIdx].nPointIdx = 2;
	}

	//计算工件的最小外接圆
	std::vector<cv::Point2f> points_2d;
	for (int i = 0; i < (int)workpieceCluster.size(); i++)
	{
		cv::Point2f a_pt = cv::Point2f(workpieceCluster[i].pt3D.x, workpieceCluster[i].pt3D.y);
		points_2d.push_back(a_pt);
	}
	cv::Point2f center;
	float r;
	cv::minEnclosingCircle(points_2d, center, r);

	//关联标准工件
	double bestError = 0;
	int bestId = -1;
	for (int i = 0; i < (int)standardWorkpieceSize.size(); i++)
	{
		double err = abs(standardWorkpieceSize[i].workpieceHeight - workpieceHeight) + abs(standardWorkpieceSize[i].workpieceRadius - r);
		if (bestId < 0)
		{
			bestId = i;
			bestError = err;
		}
		else if (bestError > err)
		{
			bestId = i;
			bestError = err;
		}
	}
	//生成投影结果
	resultInfo.center = { center.x, center.y, layerBoardZ };
	resultInfo.layerZValue = layerBoardZ;
	resultInfo.workpieceHeight = standardWorkpieceSize[bestId].workpieceHeight;
	resultInfo.workpieceRadius = standardWorkpieceSize[bestId].workpieceRadius;
	return resultInfo;
}

void planningFromCenter_1D(int num, double interval, double centerValue, std::vector<double>& pos)
{
	pos.resize(num);
	double halfInterval = interval / 2;
	if (num % 2 == 1) //奇数
	{
		int centerIdx = num / 2;
		pos[centerIdx] = centerValue;
		int j = 1;
		for (int idx = centerIdx - 1; idx >= 0; idx--)
		{
			pos[idx] = centerValue - j * interval;
			j++;
		}
		j = 1;
		for (int idx = centerIdx + 1; idx < num; idx++)
		{
			pos[idx] = centerValue + j * interval;
			j++;
		}
	}
	else
	{
		int j = 0;
		int halfSize = num / 2;
		for (int idx = halfSize - 1; idx >= 0; idx--)
		{
			pos[idx] = centerValue - halfInterval - j * interval;
			j++;
		}
		j = 0;
		for (int idx = halfSize; idx < num; idx++)
		{
			pos[idx] = centerValue + halfInterval + j * interval;
			j++;
		}
	}
	return;
}

//料筐码放：码放位置规划
void wd_HRM_PlanBinPlacement(
	const WD_HRM_BinInfo binInfo,
	const SSG_size2D realBoardSize, //实际的托板大小
	const WD_HRM_workpieceSizeInfo workpieceInfo,
	const SSG_planeCalibPara calibPara,
	const double guardingInterval,  //工作与工件的保护间隔
	std::vector< WD_workpieceInfo>& planningPositions,
	int* out_rows,
	int* out_cols,
	int* isLastLayere)
{
	double diamter = workpieceInfo.workpieceRadius * 2 + guardingInterval;
	double L = realBoardSize.width;// binInfo.length - guardingToSide * 2 + guardingInterval;
	double W = realBoardSize.height; //binInfo.width - guardingToSide * 2 + guardingInterval;

	int cols = (int)(L / diamter);
	int rows = (int)(W / diamter);

	//以中心为基准进行位置计算
	std::vector<double> rowPos;
	planningFromCenter_1D(rows, diamter, 0, rowPos);
	std::vector<double> colPos;
	planningFromCenter_1D(cols, diamter, 0, colPos);

	int objNum = rows * cols;
	planningPositions.resize(objNum);
	for (int row = 0; row < rows; row++)
	{
		for(int col = 0; col < cols; col++)
		{
			WD_workpieceInfo a_pos;
			a_pos.center = { colPos[col], rowPos[row], workpieceInfo.layerZValue };
			a_pos.value = workpieceInfo.workpieceRadius;
			a_pos.workpieceType = 1;
			a_pos.z_dir = { 0, 0, 1.0 };
			a_pos.y_dir = { 0, 0, 0 };
			a_pos.x_dir = { 0, 0, 0 };
			planningPositions[row * cols + col] = a_pos;
		}
	}

	//旋转
	double cosTheta = binInfo.x_dir.x;
	double sinTheta = binInfo.x_dir.y;
	for (int i = 0; i < (int)planningPositions.size(); i++)
	{
		cv::Point2f a_pt2D = cv::Point2f(planningPositions[i].center.x, planningPositions[i].center.y);
		a_pt2D = _rotate2D(a_pt2D, sinTheta, cosTheta);
		planningPositions[i].center.x = a_pt2D.x + binInfo.center.x;
		planningPositions[i].center.y = a_pt2D.y + binInfo.center.y;
	}
	
	double resiH = workpieceInfo.layerZValue - binInfo.binTopZ - workpieceInfo.workpieceHeight;
	if (resiH < workpieceInfo.workpieceHeight)
		*isLastLayere = 1;
	else
		*isLastLayere = 0;
	*out_rows = rows;
	*out_cols = cols;

	//旋转回原坐标系
	for (int i = 0; i < (int)planningPositions.size(); i++)
	{
		planningPositions[i].center = wd_ptRotate(planningPositions[i].center, calibPara.invRMatrix);
		planningPositions[i].x_dir = wd_ptRotate(planningPositions[i].x_dir, calibPara.invRMatrix);
		planningPositions[i].y_dir = wd_ptRotate(planningPositions[i].y_dir, calibPara.invRMatrix);
		planningPositions[i].z_dir = wd_ptRotate(planningPositions[i].z_dir, calibPara.invRMatrix);
	}

	return;
}