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

#define DEBUG_OUT_IMAGE	1

typedef struct
{
	int objID;
	cv::Point2f pos;
	double angle;
}SSG_objAngleInfo;
typedef struct
{
	bool isSide;
	double sideDist;
}SSG_objSideInfo;
typedef struct
{
	int objID;
	cv::Point2f objPos;
	SVzNL3DPoint objPos3D;
	std::vector< SSG_objAngleInfo> neighbours;
	SSG_objSideInfo sideInfo[4];  //对应L，R，T，P
}SSG_statorNeighbourInfo;

typedef struct
{
	int ID;
	int linkNum;
	int neighborID[6]; 
}SSG_hexagonNeighbour;

typedef struct
{
	cv::RotatedRect fittingPara;
	SVzNL3DPoint objCenter;
}SG_fittingInfo;

std::string	m_strVersion = "1.0.0";
const char* wd_particleSegVersion(void)
{
	return m_strVersion.c_str();
}

//计算一个平面调平参数。
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
SSG_planeCalibPara wd_getBaseCalibPara(
	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);
}

SSG_objSideInfo _getSideX(
	int cx, 
	int cy, 
	cv::Mat& zSliceData, 
	bool dirLeft)
{
	int edge_x = -1;
	SSG_objSideInfo sideInfo;
	sideInfo.isSide = false;
	sideInfo.sideDist = -1;

	if (true == dirLeft)
	{
		for (int m = cx; m >= 0; m--)
		{
			if (zSliceData.at<uchar>(cy, m) > 0)
				edge_x = m;
		}
	}
	else
	{
		for (int m = cx; m < zSliceData.cols; m++)
		{
			if (zSliceData.at<uchar>(cy, m) > 0)
				edge_x = m;
		}
	}
	if(edge_x >= 0)
	{
		sideInfo.isSide = true;
		sideInfo.sideDist = edge_x;
	}
	return sideInfo;
}

SSG_objSideInfo _getSideY(
	int cx,
	int cy,
	cv::Mat& zSliceData,
	bool dirUp)
{
	int edge_y = -1;
	SSG_objSideInfo sideInfo;
	sideInfo.isSide = false;
	sideInfo.sideDist = -1;

	if (true == dirUp)
	{
		for (int m = cy; m >= 0; m--)
		{
			if (zSliceData.at<uchar>(m, cx) > 0)
				edge_y = m;
		}
	}
	else
	{
		for (int m = cy; m < zSliceData.rows; m++)
		{
			if (zSliceData.at<uchar>(m, cx) > 0)
				edge_y = m;
		}
	}
	if (edge_y >= 0)
	{
		sideInfo.isSide = true;
		sideInfo.sideDist = edge_y;
	}
	sideInfo.isSide = true;
	return sideInfo;
}

void _getNeighbouringInfo(
	const SWD_statorParam positionParam,
	cv::Mat& zSliceData,  //用于寻找边界（边框） 
	std::vector<SG_fittingInfo>& Objects, //目标位置
	std::vector< SSG_statorNeighbourInfo>& neighbouringInfo //邻接关系
)
{
	double searchWin = positionParam.statorOuterD * 2.5; 
	int objNum = Objects.size();
	neighbouringInfo.resize(objNum);
	for (int i = 0; i < objNum; i++)
	{
		SG_fittingInfo* a_obj = &Objects[i];
		SSG_statorNeighbourInfo* obj_info = &neighbouringInfo[i];
		obj_info->objID = i;
		obj_info->objPos3D = a_obj->objCenter;
		obj_info->objPos = a_obj->fittingPara.center;
		bool checkSide[4];
		for (int m = 0; m < 4; m++)
			checkSide[m] = true;
		for (int j = 0; j < objNum; j++)
		{
			if (j != i)
			{
				//计算中心点距离
				double dist = sqrt(pow(a_obj->fittingPara.center.x - Objects[j].fittingPara.center.x, 2) + pow(a_obj->fittingPara.center.y - Objects[j].fittingPara.center.y, 2));
				if (dist < searchWin)
				{
					SSG_objAngleInfo a_neighbour;
					a_neighbour.objID = j;
					a_neighbour.pos = Objects[j].fittingPara.center;
					//计算角度
					double angle = atan2(Objects[j].fittingPara.center.y - a_obj->fittingPara.center.y, Objects[j].fittingPara.center.x - a_obj->fittingPara.center.x);
					angle = angle * 180.0 / PI; //转变成角度
					if (angle < 0)
						angle += 360; //转变成0-360度范围
					a_neighbour.angle = angle;
					if ((angle > 345) || (angle < 15))
						checkSide[0] = false;
					else if ((angle > 75) && (angle < 105))
						checkSide[1] = false;
					else if ((angle > 165) && (angle < 195))
						checkSide[2] = false;
					else if ((angle > 255) && (angle < 285))
						checkSide[3] = false;
					obj_info->neighbours.push_back(a_neighbour);
				}
			}
		}
		//分析neighbour情况，判断是否要检查side情况
		int cx = (int)a_obj->fittingPara.center.x;
		int cy = (int)a_obj->fittingPara.center.y;
		if (false == checkSide[0])
		{
			obj_info->sideInfo[0].isSide = false;
			obj_info->sideInfo[0].sideDist = -1;
		}
		else
		{
			SSG_objSideInfo sideInfo = _getSideX(cx, cy, zSliceData, true);
			obj_info->sideInfo[0] = sideInfo;
		}

		if (false == checkSide[1])
		{
			obj_info->sideInfo[1].isSide = false;
			obj_info->sideInfo[1].sideDist = -1;
		}
		else
		{
			SSG_objSideInfo sideInfo = _getSideX(cx, cy, zSliceData, false);
			obj_info->sideInfo[1] = sideInfo;
		}

		if (false == checkSide[2])
		{
			obj_info->sideInfo[2].isSide = false;
			obj_info->sideInfo[2].sideDist = -1;
		}
		else
		{
			SSG_objSideInfo sideInfo = _getSideY(cx, cy, zSliceData, true);
			obj_info->sideInfo[2] = sideInfo;
		}

		if (false == checkSide[3])
		{
			obj_info->sideInfo[3].isSide = false;
			obj_info->sideInfo[3].sideDist = -1;
		}
		else
		{
			SSG_objSideInfo sideInfo = _getSideY(cx, cy, zSliceData, false);
			obj_info->sideInfo[3] = sideInfo;
		}
	}
	return;
}

void _getPtsApart120(cv::Point2f center, double R, double angle, cv::Point2f* testPts)
{
	double theta_0 = angle * PI / 180;
	double theta_120 = angle + 120;
	if (theta_120 > 360)
		theta_120 = theta_120 - 360;
	theta_120 = theta_120 * PI / 180;
	double theta_240 = angle + 240;
	if (theta_240 > 360)
		theta_240 = theta_240 - 360;
	theta_240 = theta_240 *PI / 180;

	testPts[0].x = center.x + R * cos(theta_0);
	testPts[0].y = center.y - R * sin(theta_0);
	testPts[1].x = center.x + R * cos(theta_120);
	testPts[1].y = center.y - R * sin(theta_120);
	testPts[2].x = center.x + R * cos(theta_240);
	testPts[2].y = center.y - R * sin(theta_240);
}

double _getMinDist(
	SSG_statorNeighbourInfo& neighbouringInfo, 
	cv::Point2f* testPts
	)
{
	double min_dist = -1;
	for (int i = 0; i < 3; i++)
	{
		cv::Point2f* a_pt = &testPts[i];
		for (int j = 0; j < neighbouringInfo.neighbours.size(); j++)
		{
			cv::Point2f* n_pt = &(neighbouringInfo.neighbours[j].pos);
			double dist = sqrt(pow(a_pt->x - n_pt->x, 2) + pow(a_pt->y - n_pt->y, 2));
			if (min_dist < 0)
				min_dist = dist;
			else if (min_dist > dist)
				min_dist = dist;
		}
		if (true == neighbouringInfo.sideInfo[0].isSide)
		{
			double dist = a_pt->x - neighbouringInfo.sideInfo[0].sideDist;//外边界
			dist = dist - 10; //内边界处需要外边界减去壁厚。此处壁厚使用10mm
			if (dist < 0)
				dist = 0;
			if (min_dist < 0)
				min_dist = dist;
			else if (min_dist > dist)
				min_dist = dist;
		}
		if (true == neighbouringInfo.sideInfo[1].isSide)
		{
			double dist = neighbouringInfo.sideInfo[1].sideDist - a_pt->x;//外边界
			dist = dist - 10; //内边界处需要外边界减去壁厚。此处壁厚使用10mm
			if (dist < 0)
				dist = 0;
			if (min_dist < 0)
				min_dist = dist;
			else if (min_dist > dist)
				min_dist = dist;
		}
		if (true == neighbouringInfo.sideInfo[2].isSide)
		{
			double dist = a_pt->y - neighbouringInfo.sideInfo[2].sideDist;//外边界
			dist = dist - 10; //内边界处需要外边界减去壁厚。此处壁厚使用10mm
			if (dist < 0)
				dist = 0;
			if (min_dist < 0)
				min_dist = dist;
			else if (min_dist > dist)
				min_dist = dist;
		}
		if (true == neighbouringInfo.sideInfo[3].isSide)
		{
			double dist = neighbouringInfo.sideInfo[3].sideDist - a_pt->y;//外边界
			dist = dist - 10; //内边界处需要外边界减去壁厚。此处壁厚使用10mm
			if (dist < 0)
				dist = 0;
			if (min_dist < 0)
				min_dist = dist;
			else if (min_dist > dist)
				min_dist = dist;
		}
	}
	return min_dist;
}

void _computeGripperPose(
	SSG_statorNeighbourInfo& neighbouringInfo, 
	double gripperR,
	double* opAngle,
	double* obstacleDist)
{
	//根据相邻目标数量确定抓取点
	//圆周扫描获取距离相邻目标最小距离最大的点
	double searchStepping = 0.5; //搜索精度为0.5度
	int steps = (int)(360.0 / searchStepping);
	double max_dist = -1;
	double max_angle = -1;
	for (int i = 0; i < steps; i++)
	{
		double angle = i * searchStepping;
		cv::Point2f testPts[3];
		_getPtsApart120(neighbouringInfo.objPos, gripperR, angle, testPts);
		//计算最小距离，相邻目标和边界统一在一起计算
		double min_dist = _getMinDist(neighbouringInfo, testPts);
		//获取最小距离的最大值，对应的角度为操作角度
		if (max_dist < 0)
		{
			max_dist = min_dist;
			max_angle = angle;
		}
		else if(max_dist < min_dist)
		{
			max_dist = min_dist;
			max_angle = angle;
		}
	}
	*opAngle = max_angle;
	*obstacleDist = max_dist;
	return;
}

bool compareByNeighbourNumber(const SSG_hexagonNeighbour& a, const SSG_hexagonNeighbour& b)
{
	return a.linkNum < b.linkNum;
}

//圆周扫描定子环
//以scanCenter为扫描中心，圆周扫描从scanR1到scanR2的圆形区域
void circileScan(
	cv::Mat& scanImg, //扫描对象
	int scanCenterX, int scanCenterY,  //扫描中心
	double scanR1, double scanR2,  //扫描区域
	std::vector<SVzNL2DPoint>& peakPts  //扫描结果
)
{
	//以1度间隔扫描
	double scale = 1.0;
	for (int i = 0; i < 360; i++)
	{
		double angle = (double)i * scale * PI / 180;
		double sinTheta = sin(angle);
		double cosTheta = cos(angle);
		int x0 = scanCenterX + (int)(scanR1 * cosTheta);
		int y0 = scanCenterY - (int)(scanR1 * sinTheta);
		int x1 = scanCenterX + (int)(scanR2 * cosTheta);
		int y1 = scanCenterY - (int)(scanR2 * sinTheta);
		
		//Bresenham算法
		std::vector<SVzNL2DPoint> pts;
		drawLine(x0, y0, x1, y1, pts);
		//搜索峰值
		float peakValue = 0;
		SVzNL2DPoint peakPos = { 0,0 };
		for (int m = 0, m_max = (int)pts.size(); m < m_max; m++)
		{
			if (peakValue < scanImg.at<float>(pts[m].y, pts[m].x))
			{
				peakValue = scanImg.at<float>(pts[m].y, pts[m].x);
				peakPos = pts[m];
			}
		}
		peakPts.push_back(peakPos);
	}
}

//对定子外圈抓取的三爪抓手计算旋转位置。
//三爪对称，分别在30,150,270度。根据抓取角度，计算最小旋转角度
double _computeGrasperRotateAngle(double grasperAngle)
{
	double rotateAngle = 0;
	if ((grasperAngle >= 330) || (grasperAngle < 90))  //丫第一个枝位置30度
	{
		if (grasperAngle < 90)
			rotateAngle = grasperAngle - 30;
		else
			rotateAngle = grasperAngle - 360 - 30;
	}
	else if ((grasperAngle >= 90) && (grasperAngle < 210))//丫第二个枝位置150度
		rotateAngle = grasperAngle - 150;
	else
		rotateAngle = grasperAngle - 270;
	return rotateAngle;
}

//计算两个角的间隔（绝对值），结果在0-180范围
double _computeAbsAngleInterval(double angle_0, double angle_1)
{
	double diff = angle_0 < angle_1 ? (angle_1 - angle_0) : (angle_0 - angle_1);
	if (diff > 180)
		diff = 360 - diff;
	return diff;
}

//计算角度与相邻目标的角度间隔，以其中最小值为结果
double _computeObjAngleInterval(double angle_0, std::vector<double>& linkObjAngles)
{
	if (linkObjAngles.size() == 0)
		return -1;
	double minInterval = _computeAbsAngleInterval(angle_0, linkObjAngles[0]);
	for (int i = 1, i_max = (int)linkObjAngles.size(); i < i_max; i++)
	{
		double angleInterval = _computeAbsAngleInterval(angle_0, linkObjAngles[i]);
		if (minInterval > angleInterval)
			minInterval = angleInterval;
	}
	return minInterval;
}

//计算目标最佳抓取角度。抓手为三爪设计，间隔120度。
//扫描与相邻目标最大的角度间隔点。此位置为最佳抓取位置
double _scanBestGrasperAngle(std::vector<double>& linkObjAngles, double* angleDistance)
{
	*angleDistance = 0;
	if (linkObjAngles.size() == 0)
		return -1;
	//以0.1度间隔扫描120度范围
	double maxAngleDist = -1;
	double bestAngleIdx = -1;
	//粗定位，精确到1度
	for (int i = 0; i < 120; i++)
	{
		double angle_0 = (double)i;
		double angle_1 = angle_0 + 120;
		if (angle_1 >= 360)
			angle_1 = angle_1 - 360;
		double angle_2 = angle_0 + 240;
		if (angle_2 >= 360)
			angle_2 = angle_2 - 360;

		double angleInterval_0 = _computeObjAngleInterval(angle_0, linkObjAngles);
		double angleInterval_1 = _computeObjAngleInterval(angle_1, linkObjAngles);
		double angleInterval_2 = _computeObjAngleInterval(angle_2, linkObjAngles);
		double angleInterval = angleInterval_0 < angleInterval_1 ? angleInterval_0 : angleInterval_1;
		angleInterval = angleInterval < angleInterval_2 ? angleInterval : angleInterval_2;

		if (maxAngleDist < 0)
		{
			maxAngleDist = angleInterval;
			bestAngleIdx = (double)i;
		}
		else
		{
			if (maxAngleDist < angleInterval)
			{
				maxAngleDist = angleInterval;
				bestAngleIdx = (double)i;
			}
		}
	}
	if (bestAngleIdx < 0)
		return -1;
	//精定位， 精确到0.1度
	maxAngleDist = -1;
	double bestAngle = bestAngleIdx;
	for (int i = -10; i <= 10; i++)
	{
		double angle_0 = (double)i * 0.1 + bestAngleIdx;
		double angle_1 = angle_0 + 120;
		if (angle_1 >= 360)
			angle_1 = angle_1 - 360;
		double angle_2 = angle_0 + 240;
		if (angle_2 >= 360)
			angle_2 = angle_2 - 360;

		double angleInterval_0 = _computeObjAngleInterval(angle_0, linkObjAngles);
		double angleInterval_1 = _computeObjAngleInterval(angle_1, linkObjAngles);
		double angleInterval_2 = _computeObjAngleInterval(angle_2, linkObjAngles);
		double angleInterval = angleInterval_0 < angleInterval_1 ? angleInterval_0 : angleInterval_1;
		angleInterval = angleInterval < angleInterval_2 ? angleInterval : angleInterval_2;

		if (maxAngleDist < 0)
		{
			maxAngleDist = angleInterval;
			bestAngle = angle_0;
		}
		else
		{
			if (maxAngleDist < angleInterval)
			{
				maxAngleDist = angleInterval;
				bestAngle = angle_0;
			}
		}
	}
	*angleDistance = maxAngleDist;
	if (bestAngle < 0)
		bestAngle = bestAngle + 360;

	return bestAngle;
}

bool computeOuterGraspPoint(
	int objID,
	double outerR,
	std::vector< SSG_peakRgnInfo>& objects,
	std::vector< SSG_hexagonNeighbour>& objLinkings,
	SWD_statorOuterGrasper& graspPoint,
	double* angleDistance
)
{
	//R = 1.155outR
	SSG_hexagonNeighbour& a_link = objLinkings[objID];
	std::vector<int> linkIDs;
	for (int i = 0; i < 6; i++)
	{
		if (a_link.neighborID[i] >= 0)
		{
			linkIDs.push_back(a_link.neighborID[i]);
		}
	}
	if (linkIDs.size() != a_link.linkNum)
		return false;

	//计算抓取位置
	if (a_link.linkNum == 0)  //默认抓取位置
	{
		graspPoint.objID = objID;
		graspPoint.grasperAngle = 0;
		graspPoint.rotateAngle = 0;
		graspPoint.graspR = 1.2 * outerR;
		*angleDistance = 120;
		return true;
	}
	else if (a_link.linkNum == 1) //与目标连线方向偏离60度
	{
		int linkID = linkIDs[0];
		SSG_peakRgnInfo& obj = objects[objID];
		SSG_peakRgnInfo& nObj = objects[linkID];
		double dy = nObj.pos2D.y - obj.pos2D.y;
		double dx = nObj.pos2D.x - obj.pos2D.x;
		double dist = sqrt(pow(dx, 2) + pow(dy, 2));
		double angle = atan2(dy, dx) * 180.0 / PI + 180;  //0-360度
		double grasperAngle = angle + 60;
		if (grasperAngle >= 360)
			grasperAngle = grasperAngle - 360;
		double rotateAngle = _computeGrasperRotateAngle(grasperAngle);
		graspPoint.grasperAngle = grasperAngle;
		graspPoint.rotateAngle = rotateAngle;
		graspPoint.graspR = 1.2 * outerR;
		graspPoint.objID = objID;
		*angleDistance = 60;
		return true;
	}
	else if ( (a_link.linkNum == 2) || (a_link.linkNum == 3)) //目标有两个或3个相邻目标
	{
		std::vector<double> linkObjAngles;
		for (int i = 0, i_max = (int)linkIDs.size(); i < i_max; i++)
		{
			int linkID = linkIDs[i];
			SSG_peakRgnInfo& obj = objects[objID];
			SSG_peakRgnInfo& nObj = objects[linkID];
			double dy = nObj.pos2D.y - obj.pos2D.y;
			double dx = nObj.pos2D.x - obj.pos2D.x;
			double dist = sqrt(pow(dx, 2) + pow(dy, 2));
			double angle = atan2(dy, dx) * 180.0 / PI + 180;  //0-360度
			linkObjAngles.push_back(angle);
		}
		double grasperAngle = _scanBestGrasperAngle(linkObjAngles, angleDistance);
		double rotateAngle = _computeGrasperRotateAngle(grasperAngle);
		graspPoint.grasperAngle = grasperAngle;
		graspPoint.rotateAngle = rotateAngle;
		graspPoint.graspR = 1.155 * outerR;
		graspPoint.objID = objID;
		return true;
	}
	else
		return false;
}

//切割
void cloudPointsFilter_zRange(std::vector< std::vector<SVzNL3DPosition>>& scanLines, SVzNLRangeD zRange, std::vector< std::vector<SVzNL3DPosition>>& cutData)
{
	int lineNum = (int)scanLines.size();
	cutData.resize(lineNum);
	for (int line = 0; line < lineNum; line++)
	{
		int ptNum = (int)scanLines[line].size();
		cutData[line].resize(ptNum);
		for (int i = 0; i < ptNum; i++)
		{
			SVzNL3DPosition a_pt = scanLines[line][i];
			if ((a_pt.pt3D.z < zRange.min) || (a_pt.pt3D.z > zRange.max))
				a_pt.pt3D.z = 0;
			cutData[line][i] = a_pt;
		}
	}
}

//对无点区聚类
void genCLusterMask(
	std::vector< std::vector<SVzNL3DPosition>>& clusterSrcData,
	bool maskType_null_vldPt,  //true: nullPt(z=0); false:valid Point
	std::vector<std::vector<SSG_featureClusteringInfo>>& featureInfoMask,
	std::vector<std::vector<SVzNL3DPoint>>& feature3DInfo)
{
	int lineNum = (int)clusterSrcData.size();
	if (lineNum == 0)
		return;
	int linePtNum = (int)clusterSrcData[0].size();

	featureInfoMask.resize(lineNum);
	feature3DInfo.resize(lineNum);
	for (int i = 0; i < lineNum; i++)
	{
		featureInfoMask[i].resize(linePtNum);
		feature3DInfo[i].resize(linePtNum);
	}
	//生成Mask
	for (int line = 0; line < lineNum; line++)
	{
		std::vector<SVzNL3DPosition>& lineData = clusterSrcData[line];
		for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++)
		{
			if( ((false == maskType_null_vldPt)&&(clusterSrcData[line][ptIdx].pt3D.z > 1e-4))||
				((true == maskType_null_vldPt) && (clusterSrcData[line][ptIdx].pt3D.z < 1e-4)))
			{
				SSG_featureClusteringInfo a_mask;
				memset(&a_mask, 0, sizeof(SSG_featureClusteringInfo));
				a_mask.featurType = 1;
				a_mask.lineIdx = line;
				a_mask.ptIdx = ptIdx;
				featureInfoMask[line][ptIdx] = a_mask;
				feature3DInfo[line][ptIdx] = clusterSrcData[line][ptIdx].pt3D;
			}
		}
	}
	return;
}

//聚类:对无效点的聚类实际上是连通域处理
void cloutPointsClustering_nullPt(
	std::vector< std::vector<SVzNL3DPosition>>& clusterSrcData,
	std::vector<std::vector<SSG_featureClusteringInfo>>& featureInfoMask,
	std::vector<std::vector<SVzNL3DPoint>>& feature3DInfo,
	std::vector<std::vector< SVzNL2DPoint>>& clusters, //只记录位置
	std::vector<SWD_clustersInfo>& clustersInfo)
{
	int lineNum = (int)clusterSrcData.size();
	if (lineNum == 0)
		return;
	int linePtNum = (int)clusterSrcData[0].size();
	//采用迭代思想，回归思路进行高效聚类
	int clusterID = 1;
	int clusterCheckWin = 5;
	for (int y = 0; y < linePtNum; y++)
	{
		for (int x = 0; x < lineNum; x++)
		{
			SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[x][y];
			if ((0 == a_featureInfo.featurType) || (a_featureInfo.clusterID > 0)) //非特征或已经处理
				continue;

			SVzNL3DPoint& a_feature3DValue = feature3DInfo[x][y];
			SVzNL3DRangeD a_clusterRoi3D;
			memset(&a_clusterRoi3D, 0, sizeof(SVzNL3DRangeD));
			SVzNLRect clusterRoi2D;
			clusterRoi2D.left = x;
			clusterRoi2D.right = x;
			clusterRoi2D.top = y;
			clusterRoi2D.bottom = y;

			SVzNL2DPoint a_seedPos = { x, y };
			std::vector< SVzNL2DPoint> a_cluster;
			a_cluster.push_back(a_seedPos);

			//使用聚类方法完成8连通连通域分析
			wd_gridPointClustering_labelling(
				featureInfoMask,
				feature3DInfo,
				clusterID, //当前Cluster的ID
				a_cluster,  //result
				clusterRoi2D  //roi2D
			);
			clusters.push_back(a_cluster);
			SWD_clustersInfo a_info;
			a_info.clusterIdx = clusterID;
			a_info.ptSize = (int)a_cluster.size();
			a_info.roi2D = clusterRoi2D;
			a_info.roi3D = a_clusterRoi3D;
			clustersInfo.push_back(a_info);
			clusterID++;
		}
	}
}

//聚类
void cloutPointsClustering(
	std::vector< std::vector<SVzNL3DPosition>>& clusterSrcData,
	std::vector<std::vector<SSG_featureClusteringInfo>>& featureInfoMask,
	std::vector<std::vector<SVzNL3DPoint>>& feature3DInfo,
	SSG_treeGrowParam growParam,
	std::vector<std::vector< SVzNL2DPoint>>& clusters, //只记录位置
	std::vector<SWD_clustersInfo>& clustersInfo)
{
	int lineNum = (int)clusterSrcData.size();
	if (lineNum == 0)
		return;
	int linePtNum = (int)clusterSrcData[0].size();
	//采用迭代思想，回归思路进行高效聚类
	int clusterID = 1;
	int clusterCheckWin = 5;
	for (int y = 0; y < linePtNum; y++)
	{
		for (int x = 0; x < lineNum; x++)
		{
			SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[x][y];
			if ((0 == a_featureInfo.featurType) || (a_featureInfo.clusterID > 0)) //非特征或已经处理
				continue;

			SVzNL3DPoint& a_feature3DValue = feature3DInfo[x][y];
			SVzNL3DRangeD a_clusterRoi;
			a_clusterRoi.xRange.min = a_feature3DValue.x;
			a_clusterRoi.xRange.max = a_feature3DValue.x;
			a_clusterRoi.yRange.min = a_feature3DValue.y;
			a_clusterRoi.yRange.max = a_feature3DValue.y;
			a_clusterRoi.zRange.min = a_feature3DValue.z;
			a_clusterRoi.zRange.max = a_feature3DValue.z;

			SVzNL2DPoint a_seedPos = { x, y };
			std::vector< SVzNL2DPoint> a_cluster;
			a_cluster.push_back(a_seedPos);
			wd_gridPointClustering(
				featureInfoMask,//int，记录特征标记和clusterID，附加一个flag
				feature3DInfo,//double,记录坐标信息
				clusterCheckWin, //搜索窗口
				growParam,//聚类条件
				clusterID, //当前Cluster的ID
				a_cluster,  //result
				a_clusterRoi
			);
			clusters.push_back(a_cluster);
			SWD_clustersInfo a_info;
			a_info.clusterIdx = clusterID;
			a_info.ptSize = (int)a_cluster.size();
			a_info.roi3D = a_clusterRoi;
			clustersInfo.push_back(a_info);
			clusterID++;
		}
	}
}

//生成点的Mask， 使用SSG_clusterLabel结构
void genValidPtLabelMask(
	std::vector< std::vector<SVzNL3DPoint>>& srcData, 
	std::vector<std::vector<SSG_clusterLabel>>& labelMask, 
	EWD_maskMode maskMode)
{
	int lineNum = (int)srcData.size();
	labelMask.resize(lineNum);
	for(int i = 0; i < lineNum; i ++)
	{
		int ptNum = (int)srcData[i].size();
		labelMask[i].resize(ptNum);
		std::fill(labelMask[i].begin(), labelMask[i].end(), SSG_clusterLabel{ 0,0,0 });
		for (int j = 0; j < ptNum; j++)
		{
			if (KeWD_Mask_ValidPt == maskMode)
			{
				if (srcData[i][j].z > 1e-4)
					labelMask[i][j].validFlag = 1;
			}
			else if(KeWD_Mask_NullPt == maskMode)
			{
				if (srcData[i][j].z < 1e-4)
					labelMask[i][j].validFlag = 1;
			}
		}
	}
	return;
}

//生成点的Mask， 使用SSG_clusterLabel结构
int countQuantiSize(
	std::vector< std::vector<SVzNL3DPoint>>& srcData,
	EWD_maskMode maskMode)
{
	int totalNum = 0;
	int lineNum = (int)srcData.size();
	for (int i = 0; i < lineNum; i++)
	{
		int ptNum = (int)srcData[i].size();
		for (int j = 0; j < ptNum; j++)
		{
			if (KeWD_Mask_ValidPt == maskMode)
			{
				if (srcData[i][j].z > 1e-4)
					totalNum ++;
			}
			else if (KeWD_Mask_NullPt == maskMode)
			{
				if (srcData[i][j].z < 1e-4)
					totalNum ++;
			}
		}
	}
	return totalNum;
}

//点云聚类
//SSG_intPair成员变量定义: 
//		data_0: flag
//		data_1: valid point flag
//		idx: cluster ID
void cloutPointsClustering_2(
	std::vector< std::vector<SVzNL3DPoint>>& clusterSrcData,
	std::vector<std::vector<SSG_clusterLabel>>& labelMask,
	std::vector<std::vector< SVzNL2DPoint>>& clusters, //只记录位置
	std::vector<SWD_clustersInfo>& clustersInfo)
{
	int lineNum = (int)clusterSrcData.size();
	if (lineNum == 0)
		return;
	int linePtNum = (int)clusterSrcData[0].size();

	//采用迭代思想，回归思路进行高效聚类
	int clusterID = 1;
	int clusterCheckWin = 5;
	for (int y = 0; y < linePtNum; y++)
	{
		for (int x = 0; x < lineNum; x++)
		{
			SSG_clusterLabel& a_seedInfo = labelMask[x][y];
			if ((0 == a_seedInfo.validFlag) || (a_seedInfo.clusterID > 0)) //非特征或已经处理
				continue;

			SVzNL3DPoint a_feature3DValue = clusterSrcData[x][y];
			SVzNL3DRangeD a_clusterRoi;
			a_clusterRoi.xRange.min = a_feature3DValue.x;
			a_clusterRoi.xRange.max = a_feature3DValue.x;
			a_clusterRoi.yRange.min = a_feature3DValue.y;
			a_clusterRoi.yRange.max = a_feature3DValue.y;
			a_clusterRoi.zRange.min = a_feature3DValue.z;
			a_clusterRoi.zRange.max = a_feature3DValue.z;

			SVzNL2DPoint a_seedPos = { x, y };
			std::vector< SVzNL2DPoint> a_cluster;
			a_cluster.push_back(a_seedPos);
			wd_gridPointClustering_labelling_2(
				clusterSrcData,
				labelMask,
				clusterID, //当前Cluster的ID
				a_cluster,  //result
				a_clusterRoi  //roi2D
			);
			clusters.push_back(a_cluster);
			SWD_clustersInfo a_info;
			a_info.clusterIdx = clusterID;
			a_info.ptSize = (int)a_cluster.size();
			a_info.roi3D = a_clusterRoi;
			clustersInfo.push_back(a_info);
			clusterID++;
		}
	}
}

//根据孔洞大小过滤合格目标
void clusterSizeMatching(
	std::vector<std::vector< SVzNL2DPoint>>& clusters,
	std::vector<SWD_clustersInfo>& clustersInfo,
	const SWD_statorParam statorParam,
	std::vector<int>& validClusterIndice //合格目标的序号。此处使用序号，避免数据拷贝
)
{
	double objSizeTh_min = statorParam.statorInnerD * 0.75;
	double objSizeTh_max = statorParam.statorInnerD * 1.25;
	int clusterNum = (int)clusters.size();
	for (int i = 0; i < clusterNum; i++)
	{
		double xw = clustersInfo[i].roi3D.xRange.max - clustersInfo[i].roi3D.xRange.min;
		double yw = clustersInfo[i].roi3D.yRange.max - clustersInfo[i].roi3D.yRange.min;
		if ((xw >= objSizeTh_min) && (xw <= objSizeTh_max) && (yw >= objSizeTh_min) && (yw <= objSizeTh_max))
			validClusterIndice.push_back(i);
	}
	return;
}

SSG_peakRgnInfo _computeObjInfo(int cluterIndex,
	std::vector<std::vector< SVzNL2DPoint>>& clusters,
	std::vector<SWD_clustersInfo>& clustersInfo,
	std::vector<std::vector<SVzNL3DPoint>>& srcData,
	const double quatiStatorR
)
{
	SSG_peakRgnInfo a_obj;
	memset(&a_obj, 0, sizeof(SSG_peakRgnInfo));
	a_obj.pkRgnIdx = -1;

	int counterNum = (int)clusters[cluterIndex].size();
	if (counterNum == 0)
		return a_obj;

	SVzNL3DPoint centerPos = { 0, 0, 0 };
	SVzNL2DPoint center2D = { 0, 0 };

	for (int i = 0; i < counterNum; i++)
	{
		SVzNL2DPoint& a_pt = clusters[cluterIndex][i];
		center2D.x += a_pt.x;
		center2D.y += a_pt.y;
		centerPos.x += srcData[a_pt.x][a_pt.y].x;
		centerPos.y += srcData[a_pt.x][a_pt.y].y;
	}

	a_obj.pkRgnIdx = cluterIndex;
	a_obj.pos2D.x = center2D.x / counterNum;
	a_obj.pos2D.y = center2D.y / counterNum;

	//计算Z：取周围的Z的最小值
	int winR = (int)quatiStatorR + 1;
	int xMax = srcData.size();
	int yMax = srcData[0].size();
	double cx = a_obj.pos2D.x;
	double cy = a_obj.pos2D.y;
	double minZ = DBL_MAX;
	for (int x = a_obj.pos2D.x - winR; x <= a_obj.pos2D.x + winR; x++)
	{
		for (int y = a_obj.pos2D.y - winR; y <= a_obj.pos2D.y + winR; y++)
		{
			if ((x >= 0) && (x < xMax) && (y >= 0) && (y < yMax))
			{
				double dx = x;
				double dy = y;
				double dist = sqrt(pow(dx - cx, 2) + pow(dy - cy, 2));
				if (dist < quatiStatorR)
				{
					if ((srcData[x][y].z > 1e-4) && (minZ > srcData[x][y].z))
					{
						minZ = srcData[x][y].z;
					}
				}
			}
		}
	}

	a_obj.centerPos.x = centerPos.x / counterNum;
	a_obj.centerPos.y = centerPos.y / counterNum;
	a_obj.centerPos.z = minZ;

	a_obj.objSize.dWidth = clustersInfo[cluterIndex].roi3D.xRange.max - clustersInfo[cluterIndex].roi3D.xRange.min;
	a_obj.objSize.dHeight = clustersInfo[cluterIndex].roi3D.yRange.max - clustersInfo[cluterIndex].roi3D.yRange.min;
	return a_obj;
}

void computeBestObject(
	std::vector<SSG_peakRgnInfo>& objectRgns,
	const SWD_statorParam statorParam,
	SWD_statorGriperState* opState,
	std::vector<SWD_statorInnerGrasper>& resultObjPositions)
{
	int objNumber = (int)objectRgns.size();
	//计算抓取点
	//建立邻接关系,每个目标最多有6个邻接目标
	std::vector< int> objLinkings;
	objLinkings.resize(objNumber);//第一个不使用。目标ID从1开始，保持ID与数组下标一致
	for (int i = 0; i < objNumber; i++)
	{
		objLinkings[i] = 0;
	}
	double linkPixDistTh = statorParam.statorOuterD * 1.5;
	for (int i = 0; i < objNumber; i++)
	{
		SSG_peakRgnInfo& obj_0 = objectRgns[i];
		for (int j = i + 1; j < objNumber; j++)
		{
			SSG_peakRgnInfo& obj_1 = objectRgns[j];
			double dy = obj_1.pos2D.y - obj_0.pos2D.y;
			double dx = obj_1.pos2D.x - obj_0.pos2D.x;
			double dist = sqrt(pow(dx, 2) + pow(dy, 2));
			if (dist < linkPixDistTh)
			{
				objLinkings[i]++;
				objLinkings[j]++;
			}
		}
	}

	//确定最佳抓取点
	if (opState->refPos.z > 1e-4)  //根据上一次给定的位置进行抓取
	{
		int objID = -1;
		double minDist = -1;
		double refx = opState->refPos.x;
		double refy = opState->refPos.y;
		for (int i = 0; i < objNumber; i++) //以最近的目标作为抓取目标
		{
			double dist = sqrt(pow(objectRgns[i].centerPos.x - refx, 2) + pow(objectRgns[i].centerPos.y - refy, 2));
			if (minDist < 0)
			{
				minDist = dist;
				objID = i;
			}
			else
			{
				if (minDist > dist)
				{
					minDist = dist;
					objID = i;
				}
			}
		}
		SWD_statorInnerGrasper innerGrasper;
		innerGrasper.objID = objID;
		innerGrasper.opCenter = objectRgns[objID].centerPos;
		resultObjPositions.push_back(innerGrasper);
		//将其它目标列出，用于Debug，检查其它目标是否准确
		for (int i = 0; i < objNumber; i++)
		{
			if (i != objID)
			{
				innerGrasper.objID = i;
				innerGrasper.opCenter = objectRgns[i].centerPos;
				resultObjPositions.push_back(innerGrasper);
			}
		}
	}
	else
	{
		//统计相邻目标数量
		int minLinkNum = -1;
		int minLindID = -1;
		for (int i = 0; i < objNumber; i++)
		{

			if (minLinkNum < 0)
			{
				minLinkNum = objLinkings[i];
				minLindID = i;
			}
			else
			{
				if (minLinkNum > objLinkings[i])
				{
					minLinkNum = objLinkings[i];
					minLindID = i;
				}
			}
		}
		//将相邻目标数量最少的目标定义为抓取目标
		SWD_statorInnerGrasper innerGrasper;
		innerGrasper.objID = minLindID;
		innerGrasper.opCenter = objectRgns[minLindID].centerPos;
		resultObjPositions.push_back(innerGrasper);
		//将其它目标列出，用于Debug，检查其它目标是否准确
		for (int i = 0; i < objNumber; i++)
		{
			if (i != minLindID)
			{
				innerGrasper.objID = i;
				innerGrasper.opCenter = objectRgns[i].centerPos;
				resultObjPositions.push_back(innerGrasper);
			}
		}
	}
}

void getDistanceTransformMaskData(
	std::vector<std::vector<SVzNL3DPoint>>& quantiData,
	cv::Mat& projectionData//投影量化数据，初始化为一个极大值1e+6
)
{
	for (int row = 0; row < (int)quantiData.size(); row++)
	{
		for (int col = 0; col < (int)quantiData[row].size(); col++)
		{
			if(quantiData[row][col].z > 1e-4)
				projectionData.at<float>(row, col) = 1e+6;
		}
	}
	return;
}

//电机定子定位。
//输出：（1）定子目标（2）隔板抓取点
//算法逻辑：找到定子的高度->设置截取Z去掉底面->
// 投影（注意此时边框也同时投影）->距离变换->提取定子目标->
// 根据相邻和边框寻找最佳抓取目标和抓取点
//使用状态机记忆每一步操作。这种状态机适用于料筐中多层工件的按层抓取
void wd_motorStatorPosition(
	std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
	const SWD_statorParam statorParam,
	const SSG_planeCalibPara groundCalibPara,
	const SWD_statorPositonParam algoParam,
	SWD_statorGriperState* opState,  //操作状态机。指示当前状态
	int* errCode,
	std::vector<SWD_statorInnerGrasper>& resultObjPositions)
{
	int lineNum = (int)scanLinesInput.size();
	if (lineNum == 0)
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return;
	}

	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算法
	}
	if (false == isGridData)//数据不是网格格式
	{
		*errCode = SG_ERR_NOT_GRID_FORMAT;
		return;
	}
	for (int i = 0; i < lineNum; i++)
	{																					 //行处理
		//调平，去除地面
		wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1);
	}

	//噪点过滤
	wd_noiseFilter(scanLines, algoParam.filterParam, errCode);
	if (*errCode != 0)
		return;

	///开始数据处理
	double cutZ_topLevel = groundCalibPara.planeHeight - statorParam.plateThickness - statorParam.statorHeight * 1.5;
	double cutZ_btmLevel = groundCalibPara.planeHeight - statorParam.statorHeight * 0.5;
	//顶层数据
	std::vector< std::vector<SVzNL3DPosition>> cutData_topLevel;
	cutData_topLevel.resize(lineNum);
	//底层数据
	std::vector< std::vector<SVzNL3DPosition>> cutData_btmLevel;
	cutData_btmLevel.resize(lineNum);

	int ptNum_total = 0;
	int ptNum_topLevel = 0;
	int ptNum_btmLevel = 0;

	//使用cutZ_level进行切割
	for (int line = 0; line < lineNum; line++)
	{
		//top level
		cutData_topLevel[line].resize(linePtNum);
		std::fill(cutData_topLevel[line].begin(), cutData_topLevel[line].end(), SVzNL3DPosition{ 0,{0,0,0} });
		//bottom level
		cutData_btmLevel[line].resize(linePtNum);
		std::fill(cutData_btmLevel[line].begin(), cutData_btmLevel[line].end(), SVzNL3DPosition{ 0,{0,0,0} });
		for (int j = 0; j < linePtNum; j++)
		{
			if (scanLines[line][j].pt3D.z > 1e-4)
			{
				ptNum_total++;
				if (scanLines[line][j].pt3D.z < cutZ_topLevel)
				{
					cutData_topLevel[line][j] = scanLines[line][j];
					ptNum_topLevel++;
				}
				if (scanLines[line][j].pt3D.z < cutZ_btmLevel)
				{
					cutData_btmLevel[line][j] = scanLines[line][j];
					ptNum_btmLevel++;
				}
			}
		}
	}

	double quatiScale = 1.0;
	double quatiStatorR = (statorParam.statorOuterD / 2.0) / quatiScale;
	int quatiEdgeSkip = 8;
	double inerPolateDistTh = quatiScale * 3;
	SVzNL3DRangeD roi3D = sg_getScanDataROI_vector(scanLines);
#if 0
	//对全图进行量化，作为层判断基础
	std::vector<std::vector<SVzNL3DPoint>> quantiData_all;
	std::vector<std::vector<SVzNL2DPoint>> backIndexing_all;
	pointCloud2DQuantization(
		scanLines,
		roi3D.xRange,
		roi3D.yRange,
		quatiScale,
		quatiEdgeSkip,
		inerPolateDistTh,  //插值阈值。大于此阈值的不进行量化插值
		quantiData_all,  //量化数据，初始化为一个极大值1e+6
		backIndexing_all //标记坐标索引，用于回找3D坐标
	);
	int totalQuantiSize = countQuantiSize(quantiData_all, KeWD_Mask_ValidPt);
#endif

	bool topLevelIsEmpty = false;
	if ((keSG_OPERATE_BTM_LAYER == opState->opState) ||
		(keSG_OPERATE_PLATE == opState->opState))
		topLevelIsEmpty = true;

	if ((keWD_OPERATE_Uknown == opState->opState) ||
		(keSG_OPERATE_TOP_LAYER == opState->opState)) //当状态机为未知状态或处于上层抓取时，对顶层进行量化
	{
		//对顶层进行量化
		//在量化图像上处理，否则空间处没有坐标，无法确定空间区域的面积，无法确定孔的大小
		std::vector<std::vector<SVzNL3DPoint>> quantiData_topLevel;
		std::vector<std::vector<SVzNL2DPoint>> backIndexing_topLevel;
		pointCloud2DQuantization(
			cutData_topLevel,
			roi3D.xRange,
			roi3D.yRange,
			quatiScale,
			quatiEdgeSkip,
			inerPolateDistTh,  //插值阈值。大于此阈值的不进行量化插值
			quantiData_topLevel,  //量化数据，初始化为一个极大值1e+6
			backIndexing_topLevel //标记坐标索引，用于回找3D坐标
		);
		int topLevelQuantiSize = countQuantiSize(quantiData_topLevel, KeWD_Mask_ValidPt);
		double ringArea = (pow(statorParam.statorOuterD, 2) - pow(statorParam.statorInnerD, 2)) * PI * 0.25;
		if (topLevelQuantiSize < (ringArea * 0.5))
		{
			topLevelIsEmpty = true;
		}

		//顶层有点云数据
		if (false == topLevelIsEmpty)
		{
			//生成点的Mask， 使用SSG_clusterLabel结构
			std::vector<std::vector<SSG_clusterLabel>> labelMask_topLevel;
			genValidPtLabelMask(quantiData_topLevel, labelMask_topLevel, KeWD_Mask_NullPt);  //目的：提取孔点
			//
			//使用聚类方法进行连通域标记
			std::vector<std::vector< SVzNL2DPoint>> clusters;
			std::vector<SWD_clustersInfo> clustersInfo;
			cloutPointsClustering_2(
				quantiData_topLevel,
				labelMask_topLevel,
				clusters, //只记录位置
				clustersInfo);

			if (clusters.size() == 1)  //如果只有一个目标，说明整个topLevel都是空点，上层为空
			{
				topLevelIsEmpty = true;
			}
			else
			{
				//过滤目标
				std::vector<int> validClusterIndice;//合格目标的序号。此处使用序号，避免数据拷贝
				//根据大小提取目标
				clusterSizeMatching(clusters, clustersInfo, statorParam, validClusterIndice); //合格目标的序号。此处使用序号，避免数据拷贝
				if (validClusterIndice.size() == 0)  //没有找到目标(此时有点云数据，因此报错）
				{
					*errCode = SX_ERR_ZERO_OBJ_TOPLAYER;
					return;
				}
				else
				{
					std::vector<SSG_peakRgnInfo> objectRgns;
					int objNum = (int)validClusterIndice.size();
					for (int i = 0; i < objNum; i++)
					{
						//计算目标位置
						SSG_peakRgnInfo a_obj = _computeObjInfo(
							validClusterIndice[i],
							clusters,
							clustersInfo,
							quantiData_topLevel,
							quatiStatorR
						);
						if (a_obj.pkRgnIdx < 0)
							continue;

						objectRgns.push_back(a_obj);
					}
					if (objectRgns.size() == 0)  //没有找到目标(此时有点云数据，因此报错）
					{
						*errCode = SX_ERR_ZERO_OBJ_TOPLAYER;
						return;
					}

					//生成结果
					computeBestObject(
						objectRgns,
						statorParam,
						opState,
						resultObjPositions
						);

					//更新状态机
					opState->refPos.x = resultObjPositions[0].opCenter.x;
					opState->refPos.y = resultObjPositions[0].opCenter.y;
					opState->refPos.z = resultObjPositions[0].opCenter.z;
					opState->opState = keSG_OPERATE_TOP_LAYER;
				}
			}
		}
	}

	//顶层为空，处理底层
	if (true == topLevelIsEmpty)  
	{
		//对底层进行处理
		//在量化图像上处理，否则空间处没有坐标，无法确定空间区域的面积，无法确定孔的大小
		std::vector<std::vector<SVzNL3DPoint>> quantiData_btmLevel;
		std::vector<std::vector<SVzNL2DPoint>> backIndexing_btmLevel;
		pointCloud2DQuantization(
			cutData_btmLevel,
			roi3D.xRange,
			roi3D.yRange,
			quatiScale,
			quatiEdgeSkip,
			inerPolateDistTh,  //插值阈值。大于此阈值的不进行量化插值
			quantiData_btmLevel,  //量化数据，初始化为一个极大值1e+6
			backIndexing_btmLevel //标记坐标索引，用于回找3D坐标
		);
		int bmtLevelQuantiSize = countQuantiSize(quantiData_btmLevel, KeWD_Mask_ValidPt);
		double ringArea = (pow(statorParam.statorOuterD, 2) - pow(statorParam.statorInnerD, 2)) * PI * 0.25;
		if (bmtLevelQuantiSize < (ringArea * 0.5))
		{
			//空框
			*errCode = SX_ERR_ZERO_OBJ_BTMLAYER;
			return;
		}

		//距离变换判断隔板
		if (opState->opState != keSG_OPERATE_BTM_LAYER)
		{
			int maskRow = quantiData_btmLevel.size();
			int maskCol = quantiData_btmLevel[0].size();
			cv::Mat projectionImg(maskRow, maskCol, CV_32FC1, 0.0f);  //距离变换Mask，初始化为一个极大值1e+6
			getDistanceTransformMaskData(quantiData_btmLevel, projectionImg ); // 投影量化数据，初始化为一个极大值1e + 6
			cv::Mat distTransform;
			sg_distanceTrans(projectionImg, distTransform, 0);
#if DEBUG_OUT_IMAGE  //debug	
			cv::Mat maskImage;
			cv::normalize(projectionImg, maskImage, 0, 255, cv::NORM_MINMAX, CV_8U);
			cv::imwrite("distTransformMask.png", maskImage);
			cv::Mat dtImage;
			cv::normalize(distTransform, dtImage, 0, 255, cv::NORM_MINMAX, CV_8U);
			cv::Mat dtImage_color;
			cv::cvtColor(dtImage, dtImage_color, cv::COLOR_GRAY2BGR);
			cv::imwrite("distTransform.png", dtImage_color);
#endif
			//取最大的距离变换值，若
			double distancePeakTh = statorParam.plateH < statorParam.plateW ? statorParam.plateH : statorParam.plateW;
			distancePeakTh = distancePeakTh / quatiScale;
			distancePeakTh = (distancePeakTh / 2) * 0.75;
			double maxDistTransformValue = 0;
			for (int y = 0; y < maskRow; y++)
			{
				for (int x = 0; x < maskCol; x++)
				{
					if (distTransform.at<float>(y, x) > maxDistTransformValue)
						maxDistTransformValue = distTransform.at<float>(y, x);
				}
			}
			if (maxDistTransformValue > distancePeakTh)
			{
				cv::Mat bwImg = cv::Mat::zeros(maskRow, maskCol, CV_8UC1);//rows, cols
				for (int row = 0; row < maskRow; row++)
				{
					for (int col = 0; col < maskCol; col++)
					{
						if (quantiData_btmLevel[row][col].z > 1e-4)
							bwImg.at<uchar>(row, col) = 1;
					}
				}
				//连通域标注
				cv::Mat labImg;
				std::vector<SSG_Region> labelRgns;
				SG_TwoPassLabel(bwImg, labImg, labelRgns, 8);
				//将最大目标的中心作为隔板抓取中心点
				SSG_Region maxRgn;
				memset(&maxRgn, 0, sizeof(SSG_Region));
				int maxId = -1;
				for (int i = 0; i < (int)labelRgns.size(); i++)
				{
					if (maxRgn.ptCounter < labelRgns[i].ptCounter)
					{
						maxRgn = labelRgns[i];
						maxId = i;
					}
				}
				if (maxId < 0)
				{
					//隔板提取错误
					*errCode = SX_ERR_GET_INVALID_PALTE;
					return;
				}

				double plate_cx = (double)(labelRgns[maxId].roi.right + labelRgns[maxId].roi.left) / 2;
				double plate_cy = (double)(labelRgns[maxId].roi.bottom + labelRgns[maxId].roi.top) / 2;
				int cx_i = (int)plate_cx;
				int cy_i = (int)plate_cy;
				double op_z = quantiData_btmLevel[cx_i][cy_i].z;
				plate_cx = (plate_cx - quatiEdgeSkip) * quatiScale + roi3D.xRange.min;
				plate_cy = (plate_cy - quatiEdgeSkip) * quatiScale + roi3D.yRange.min;

				SWD_statorInnerGrasper plateGrasper;
				plateGrasper.objID = 0;
				plateGrasper.opCenter = { plate_cx , plate_cy, op_z , 0,0,0 };
				resultObjPositions.push_back(plateGrasper);
				opState->opState = keSG_OPERATE_PLATE;
				opState->refPos = { 0,0,0 };
				return;
			}
		}

		//处理下层目标
		//生成点的Mask， 使用SSG_clusterLabel结构
		std::vector<std::vector<SSG_clusterLabel>> labelMask_btmLevel;
		genValidPtLabelMask(quantiData_btmLevel, labelMask_btmLevel, KeWD_Mask_NullPt);  //目的：提取孔点
		//使用聚类方法进行连通域标记
		std::vector<std::vector< SVzNL2DPoint>> clusters;
		std::vector<SWD_clustersInfo> clustersInfo;
		cloutPointsClustering_2(
			quantiData_btmLevel,
			labelMask_btmLevel,
			clusters, //只记录位置
			clustersInfo);

		if (clusters.size() == 1)  //如果只有一个目标，说明整个topLevel都是空点，上层为空
		{
			//空框
			*errCode = SX_ERR_ZERO_OBJ_BTMLAYER;
			return;
		}
		else
		{
			//过滤目标
			std::vector<int> validClusterIndice;//合格目标的序号。此处使用序号，避免数据拷贝
			//根据大小提取目标
			clusterSizeMatching(clusters, clustersInfo, statorParam, validClusterIndice); //合格目标的序号。此处使用序号，避免数据拷贝
			if (validClusterIndice.size() == 0)  //没有找到目标(此时有点云数据，因此报错）
			{
				*errCode = SX_ERR_ZERO_OBJ_TOPLAYER;
				return;
			}
			else
			{
				std::vector<SSG_peakRgnInfo> objectRgns;
				int objNum = (int)validClusterIndice.size();
				for (int i = 0; i < objNum; i++)
				{
					//计算目标位置
					SSG_peakRgnInfo a_obj = _computeObjInfo(
						validClusterIndice[i],
						clusters,
						clustersInfo,
						quantiData_btmLevel,
						quatiStatorR
					);
					if (a_obj.pkRgnIdx < 0)
						continue;

					objectRgns.push_back(a_obj);
				}
				if (objectRgns.size() == 0)  //没有找到目标(此时有点云数据，因此报错）
				{
					*errCode = SX_ERR_ZERO_OBJ_BTMLAYER;
					return;
				}

				//生成结果
				computeBestObject(
					objectRgns,
					statorParam,
					opState,
					resultObjPositions);

				//更新状态机
				opState->refPos.x = resultObjPositions[0].opCenter.x;
				opState->refPos.y = resultObjPositions[0].opCenter.y;
				opState->refPos.z = resultObjPositions[0].opCenter.z;
				opState->opState = keSG_OPERATE_BTM_LAYER;
			}
		}
	}
	return;
}