algoLib/sourceCode/motorStatorPosition.cpp

#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];  //<2F><>ӦL<D3A6><4C>R<EFBFBD><52>T<EFBFBD><54>P
}SSG_statorNeighbourInfo;

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

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

//version 1.0.0 : base version release to customer
//version 1.1.0 : opencv<63><76><EFBFBD><EFBFBD>480<38>汾<EFBFBD><E6B1BE><EFBFBD><EFBFBD>Ϊ320<32>汾 
std::string	m_strVersion = "1.1.0";
const char* wd_motorStatorVersion(void)
{
	return m_strVersion.c_str();
}

//<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>п<EFBFBD><D0BF><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD>Ͳο<CDB2><CEBF><EFBFBD>ƽƽ<C6BD>棬<EFBFBD><E6A3AC><EFBFBD><EFBFBD><EFBFBD>ߵ<EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD>ƽ
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD>淨<EFBFBD><E6B7A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>
SSG_planeCalibPara wd_getBaseCalibPara(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
	return sg_getPlaneCalibPara2(scanLines);
}

//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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,  //<2F><><EFBFBD><EFBFBD>Ѱ<EFBFBD>ұ߽磨<DFBD>߿<EFBFBD><DFBF><EFBFBD> 
	std::vector<SG_fittingInfo>& Objects, //Ŀ<><C4BF>λ<EFBFBD><CEBB>
	std::vector< SSG_statorNeighbourInfo>& neighbouringInfo //<2F>ڽӹ<DABD>ϵ
)
{
	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)
			{
				//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD><C4B5><EFBFBD><EFBFBD><EFBFBD>
				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;
					//<2F><><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD>
					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; //ת<><D7AA><EFBFBD>ɽǶ<C9BD>
					if (angle < 0)
						angle += 360; //ת<><D7AA><EFBFBD><EFBFBD>0-360<36>ȷ<EFBFBD>Χ
					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);
				}
			}
		}
		//<2F><><EFBFBD><EFBFBD>neighbour<75><72><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><D0B6>Ƿ<EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD>side<64><65><EFBFBD><EFBFBD>
		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;//<2F><><EFBFBD>߽<EFBFBD>
			dist = dist - 10; //<2F>ڱ߽紦<DFBD><E7B4A6>Ҫ<EFBFBD><D2AA><EFBFBD>߽<EFBFBD><DFBD><EFBFBD>ȥ<EFBFBD>ں񡣴˴<F1A1A3B4><CBB4>ں<EFBFBD>ʹ<EFBFBD><CAB9>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;//<2F><><EFBFBD>߽<EFBFBD>
			dist = dist - 10; //<2F>ڱ߽紦<DFBD><E7B4A6>Ҫ<EFBFBD><D2AA><EFBFBD>߽<EFBFBD><DFBD><EFBFBD>ȥ<EFBFBD>ں񡣴˴<F1A1A3B4><CBB4>ں<EFBFBD>ʹ<EFBFBD><CAB9>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;//<2F><><EFBFBD>߽<EFBFBD>
			dist = dist - 10; //<2F>ڱ߽紦<DFBD><E7B4A6>Ҫ<EFBFBD><D2AA><EFBFBD>߽<EFBFBD><DFBD><EFBFBD>ȥ<EFBFBD>ں񡣴˴<F1A1A3B4><CBB4>ں<EFBFBD>ʹ<EFBFBD><CAB9>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;//<2F><><EFBFBD>߽<EFBFBD>
			dist = dist - 10; //<2F>ڱ߽紦<DFBD><E7B4A6>Ҫ<EFBFBD><D2AA><EFBFBD>߽<EFBFBD><DFBD><EFBFBD>ȥ<EFBFBD>ں񡣴˴<F1A1A3B4><CBB4>ں<EFBFBD>ʹ<EFBFBD><CAB9>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)
{
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȷ<EFBFBD><C8B7>ץȡ<D7A5><C8A1>
	//Բ<><D4B2>ɨ<EFBFBD><C9A8><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD>
	double searchStepping = 0.5; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ0.5<EFBFBD><EFBFBD>
	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);
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD>룬<EFBFBD><EBA3AC><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD>ͱ߽<CDB1>ͳһ<CDB3><D2BB>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		double min_dist = _getMinDist(neighbouringInfo, testPts);
		//<2F><>ȡ<EFBFBD><C8A1>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>Ӧ<EFBFBD>ĽǶ<C4BD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD>
		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;
}

//Բ<><D4B2>ɨ<EFBFBD>趨<EFBFBD>ӻ<EFBFBD>
//<2F><>scanCenterΪɨ<CEAA><C9A8><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD>Բ<EFBFBD><D4B2>ɨ<EFBFBD><C9A8><EFBFBD><EFBFBD>scanR1<52><31>scanR2<52><32>Բ<EFBFBD><D4B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void circileScan(
	cv::Mat& scanImg, //ɨ<><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	int scanCenterX, int scanCenterY,  //ɨ<><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	double scanR1, double scanR2,  //ɨ<><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	std::vector<SVzNL2DPoint>& peakPts  //ɨ<><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
)
{
	//<2F><>1<EFBFBD>ȼ<EFBFBD><C8BC><EFBFBD>ɨ<EFBFBD><C9A8>
	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<61>㷨
		std::vector<SVzNL2DPoint> pts;
		drawLine(x0, y0, x1, y1, pts);
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
		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);
	}
}

//<2F>Զ<EFBFBD><D4B6><EFBFBD><EFBFBD><EFBFBD>Ȧץȡ<D7A5><C8A1><EFBFBD><EFBFBD>צץ<D7A6>ּ<EFBFBD><D6BC><EFBFBD><EFBFBD><EFBFBD>תλ<D7AA>á<EFBFBD>
//<2F><>צ<EFBFBD>Գƣ<D4B3><C6A3>ֱ<EFBFBD><D6B1><EFBFBD>30,150,270<37>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD>ץȡ<D7A5>Ƕȣ<C7B6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>ת<EFBFBD>Ƕ<EFBFBD>
double _computeGrasperRotateAngle(double grasperAngle)
{
	double rotateAngle = 0;
	if ((grasperAngle >= 330) || (grasperAngle < 90))  //Ѿ<><D1BE>һ<EFBFBD><D2BB>֦λ<D6A6><CEBB>30<33><30>
	{
		if (grasperAngle < 90)
			rotateAngle = grasperAngle - 30;
		else
			rotateAngle = grasperAngle - 360 - 30;
	}
	else if ((grasperAngle >= 90) && (grasperAngle < 210))//Ѿ<>ڶ<EFBFBD><DAB6><EFBFBD>֦λ<D6A6><CEBB>150<35><30>
		rotateAngle = grasperAngle - 150;
	else
		rotateAngle = grasperAngle - 270;
	return rotateAngle;
}

//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵļ<C7B5><C4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0-180<38><30>Χ
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;
}

//<2F><><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD>ĽǶȼ<C7B6><C8BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>СֵΪ<D6B5><CEAA><EFBFBD><EFBFBD>
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;
}

//<2F><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ץȡ<D7A5>Ƕȡ<C7B6>ץ<EFBFBD><D7A5>Ϊ<EFBFBD><CEAA>צ<EFBFBD><D7A6><EFBFBD>ƣ<EFBFBD><C6A3><EFBFBD><EFBFBD><EFBFBD>120<32>ȡ<EFBFBD>
//ɨ<><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĽǶȼ<C7B6><C8BC><EFBFBD><EFBFBD>㡣<EFBFBD><E3A1A3>λ<EFBFBD><CEBB>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>ץȡλ<C8A1><CEBB>
double _scanBestGrasperAngle(std::vector<double>& linkObjAngles, double* angleDistance)
{
	*angleDistance = 0;
	if (linkObjAngles.size() == 0)
		return -1;
	//<2F><>0.1<EFBFBD>ȼ<EFBFBD><EFBFBD><EFBFBD>ɨ<EFBFBD><EFBFBD>120<EFBFBD>ȷ<EFBFBD>Χ
	double maxAngleDist = -1;
	double bestAngleIdx = -1;
	//<2F>ֶ<EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>ȷ<EFBFBD><C8B7>1<EFBFBD><31>
	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;
	//<2F><><EFBFBD><EFBFBD>λ<EFBFBD><CEBB> <20><>ȷ<EFBFBD><C8B7>0.1<EFBFBD><EFBFBD>
	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;

	//<2F><><EFBFBD><EFBFBD>ץȡλ<C8A1><CEBB>
	if (a_link.linkNum == 0)  //Ĭ<><C4AC>ץȡλ<C8A1><CEBB>
	{
		graspPoint.objID = objID;
		graspPoint.grasperAngle = 0;
		graspPoint.rotateAngle = 0;
		graspPoint.graspR = 1.2 * outerR;
		*angleDistance = 120;
		return true;
	}
	else if (a_link.linkNum == 1) //<2F><>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>߷<EFBFBD><DFB7><EFBFBD>ƫ<EFBFBD><C6AB>60<36><30>
	{
		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<36><30>
		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)) //Ŀ<><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3<EFBFBD><33><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF>
	{
		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<36><30>
			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;
}

//<2F>и<EFBFBD>
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;
		}
	}
}

//<2F><><EFBFBD>޵<EFBFBD><DEB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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);
	}
	//<2F><><EFBFBD><EFBFBD>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;
}

//<2F><><EFBFBD><EFBFBD>:<3A><><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD>ľ<EFBFBD><C4BE><EFBFBD>ʵ<EFBFBD><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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, //ֻ<><D6BB>¼λ<C2BC><CEBB>
	std::vector<SWD_clustersInfo>& clustersInfo)
{
	int lineNum = (int)clusterSrcData.size();
	if (lineNum == 0)
		return;
	int linePtNum = (int)clusterSrcData[0].size();
	//<2F><><EFBFBD>õ<EFBFBD><C3B5><EFBFBD>˼<EFBFBD>룬<EFBFBD>ع<EFBFBD>˼·<CBBC><C2B7><EFBFBD>и<EFBFBD>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD>
	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)) //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD>
				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);

			//ʹ<>þ<EFBFBD><C3BE>෽<EFBFBD><E0B7BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>8<EFBFBD><38>ͨ<EFBFBD><CDA8>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
			wd_gridPointClustering_labelling(
				featureInfoMask,
				feature3DInfo,
				clusterID, //<2F><>ǰCluster<65><72>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++;
		}
	}
}

//<2F><><EFBFBD><EFBFBD>
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, //ֻ<><D6BB>¼λ<C2BC><CEBB>
	std::vector<SWD_clustersInfo>& clustersInfo)
{
	int lineNum = (int)clusterSrcData.size();
	if (lineNum == 0)
		return;
	int linePtNum = (int)clusterSrcData[0].size();
	//<2F><><EFBFBD>õ<EFBFBD><C3B5><EFBFBD>˼<EFBFBD>룬<EFBFBD>ع<EFBFBD>˼·<CBBC><C2B7><EFBFBD>и<EFBFBD>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD>
	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)) //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD>
				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<6E><74><EFBFBD><EFBFBD>¼<EFBFBD><C2BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǻ<EFBFBD>clusterID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>flag
				feature3DInfo,//double,<2C><>¼<EFBFBD><C2BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ
				clusterCheckWin, //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
				growParam,//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
				clusterID, //<2F><>ǰCluster<65><72>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++;
		}
	}
}

//<2F><><EFBFBD>ɵ<EFBFBD><C9B5><EFBFBD>Mask<73><6B> ʹ<><CAB9>SSG_clusterLabel<65>ṹ
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;
}

//<2F><><EFBFBD>ɵ<EFBFBD><C9B5><EFBFBD>Mask<73><6B> ʹ<><CAB9>SSG_clusterLabel<65>ṹ
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;
}

//<2F><><EFBFBD>ƾ<EFBFBD><C6BE><EFBFBD>
//SSG_intPair<69><72>Ա<EFBFBD><D4B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: 
//		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, //ֻ<><D6BB>¼λ<C2BC><CEBB>
	std::vector<SWD_clustersInfo>& clustersInfo)
{
	int lineNum = (int)clusterSrcData.size();
	if (lineNum == 0)
		return;
	int linePtNum = (int)clusterSrcData[0].size();

	//<2F><><EFBFBD>õ<EFBFBD><C3B5><EFBFBD>˼<EFBFBD>룬<EFBFBD>ع<EFBFBD>˼·<CBBC><C2B7><EFBFBD>и<EFBFBD>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD>
	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)) //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD>
				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, //<2F><>ǰCluster<65><72>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++;
		}
	}
}

//<2F><><EFBFBD>ݿ׶<DDBF><D7B6><EFBFBD>С<EFBFBD><D0A1><EFBFBD>˺ϸ<CBBA>Ŀ<EFBFBD><C4BF>
void clusterSizeMatching(
	std::vector<std::vector< SVzNL2DPoint>>& clusters,
	std::vector<SWD_clustersInfo>& clustersInfo,
	const SWD_statorParam statorParam,
	std::vector<int>& validClusterIndice //<2F>ϸ<EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>š<EFBFBD><C5A1>˴<EFBFBD>ʹ<EFBFBD><CAB9><EFBFBD><EFBFBD><EFBFBD>ţ<EFBFBD><C5A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݿ<EFBFBD><DDBF><EFBFBD>
)
{
	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;

	//<2F><><EFBFBD><EFBFBD>Z<EFBFBD><5A>ȡ<EFBFBD><C8A1>Χ<EFBFBD><CEA7>Z<EFBFBD><5A><EFBFBD><EFBFBD>Сֵ
	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();
	//<2F><><EFBFBD><EFBFBD>ץȡ<D7A5><C8A1>
	//<2F><><EFBFBD><EFBFBD><EFBFBD>ڽӹ<DABD>ϵ,ÿ<><C3BF>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>6<EFBFBD><36><EFBFBD>ڽ<EFBFBD>Ŀ<EFBFBD><C4BF>
	std::vector< int> objLinkings;
	objLinkings.resize(objNumber);//<2F><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>ʹ<EFBFBD>á<EFBFBD>Ŀ<EFBFBD><C4BF>ID<49><44>1<EFBFBD><31>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>±<EFBFBD>һ<EFBFBD><D2BB>
	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]++;
			}
		}
	}

	//ȷ<><C8B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ץȡ<D7A5><C8A1>
	if (opState->refPos.z > 1e-4)  //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>θ<EFBFBD><CEB8><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>ý<EFBFBD><C3BD><EFBFBD>ץȡ
	{
		int objID = -1;
		double minDist = -1;
		double refx = opState->refPos.x;
		double refy = opState->refPos.y;
		for (int i = 0; i < objNumber; i++) //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD>ΪץȡĿ<C8A1><C4BF>
		{
			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);
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD>г<EFBFBD><D0B3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Debug<75><67><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD>Ƿ<EFBFBD>׼ȷ
		for (int i = 0; i < objNumber; i++)
		{
			if (i != objID)
			{
				innerGrasper.objID = i;
				innerGrasper.opCenter = objectRgns[i].centerPos;
				resultObjPositions.push_back(innerGrasper);
			}
		}
	}
	else
	{
		//ͳ<><CDB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		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;
				}
			}
		}
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٵ<EFBFBD>Ŀ<EFBFBD>궨<EFBFBD><EAB6A8>ΪץȡĿ<C8A1><C4BF>
		SWD_statorInnerGrasper innerGrasper;
		innerGrasper.objID = minLindID;
		innerGrasper.opCenter = objectRgns[minLindID].centerPos;
		resultObjPositions.push_back(innerGrasper);
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD>г<EFBFBD><D0B3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Debug<75><67><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD>Ƿ<EFBFBD>׼ȷ
		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//ͶӰ<CDB6><D3B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD><DDA3><EFBFBD>ʼ<EFBFBD><CABC>Ϊһ<CEAA><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ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;
}

//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӷ<EFBFBD>λ<EFBFBD><CEBB>
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD>꣨2<EAA3A8><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ץȡ<D7A5><C8A1>
//<2F>㷨<EFBFBD>߼<EFBFBD><DFBC><EFBFBD><EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD>ӵĸ߶<C4B8>-><3E><><EFBFBD>ý<EFBFBD>ȡZȥ<5A><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>->
// ͶӰ<CDB6><D3B0>ע<EFBFBD><D7A2><EFBFBD><EFBFBD>ʱ<EFBFBD>߿<EFBFBD>ҲͬʱͶӰ<CDB6><D3B0>-><3E><><EFBFBD><EFBFBD><EFBFBD>任-><3E><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF>->
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ںͱ߿<CDB1>Ѱ<EFBFBD><D1B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ץȡĿ<C8A1><C4BF><EFBFBD><EFBFBD>ץȡ<D7A5><C8A1>
//ʹ<><CAB9>״̬<D7B4><CCAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ÿһ<C3BF><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬<D7B4><CCAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ͽ<EFBFBD><CFBF>ж<EFBFBD><D0B6>㹤<EFBFBD><E3B9A4><EFBFBD>İ<EFBFBD><C4B0><EFBFBD>ץȡ
void wd_motorStatorPosition(
	std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
	const SWD_statorParam statorParam,
	const SSG_planeCalibPara groundCalibPara,
	const SWD_statorPositonParam algoParam,
	SWD_statorGriperState* opState,  //<2F><><EFBFBD><EFBFBD>״̬<D7B4><CCAC><EFBFBD><EFBFBD>ָʾ<D6B8><CABE>ǰ״̬
	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()); // ʹ<><CAB9>std::copy<70>㷨
	}
	if (false == isGridData)//<2F><><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ
	{
		*errCode = SG_ERR_NOT_GRID_FORMAT;
		return;
	}
	for (int i = 0; i < lineNum; i++)
	{																					 //<2F>д<EFBFBD><D0B4><EFBFBD>
		//<2F><>ƽ<EFBFBD><C6BD>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1);
	}

	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	wd_noiseFilter(scanLines, algoParam.filterParam, errCode);
	if (*errCode != 0)
		return;

	///<2F><>ʼ<EFBFBD><CABC><EFBFBD>ݴ<EFBFBD><DDB4><EFBFBD>
	double cutZ_topLevel = groundCalibPara.planeHeight - statorParam.plateThickness - statorParam.statorHeight * 1.5;
	double cutZ_btmLevel = groundCalibPara.planeHeight - statorParam.statorHeight * 0.5;
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	std::vector< std::vector<SVzNL3DPosition>> cutData_topLevel;
	cutData_topLevel.resize(lineNum);
	//<2F>ײ<EFBFBD><D7B2><EFBFBD><EFBFBD><EFBFBD>
	std::vector< std::vector<SVzNL3DPosition>> cutData_btmLevel;
	cutData_btmLevel.resize(lineNum);

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

	//ʹ<><CAB9>cutZ_level<65><6C><EFBFBD><EFBFBD><EFBFBD>и<EFBFBD>
	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
	//<2F><>ȫͼ<C8AB><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD>жϻ<D0B6><CFBB><EFBFBD>
	std::vector<std::vector<SVzNL3DPoint>> quantiData_all;
	std::vector<std::vector<SVzNL2DPoint>> backIndexing_all;
	pointCloud2DQuantization(
		scanLines,
		roi3D.xRange,
		roi3D.yRange,
		quatiScale,
		quatiEdgeSkip,
		inerPolateDistTh,  //<2F><>ֵ<EFBFBD><D6B5>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD>ڴ<EFBFBD><DAB4><EFBFBD>ֵ<EFBFBD>IJ<EFBFBD><C4B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
		quantiData_all,  //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD><DDA3><EFBFBD>ʼ<EFBFBD><CABC>Ϊһ<CEAA><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ1e+6
		backIndexing_all //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڻ<EFBFBD><DABB><EFBFBD>3D<33><44><EFBFBD><EFBFBD>
	);
	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)) //<2F><>״̬<D7B4><CCAC>Ϊδ֪״̬<D7B4><CCAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϲ<EFBFBD>ץȡʱ<C8A1><CAB1><EFBFBD>Զ<EFBFBD><D4B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	{
		//<2F>Զ<EFBFBD><D4B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͼ<EFBFBD><CDBC><EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ռ䴦û<E4B4A6><C3BB><EFBFBD><EFBFBD><EFBFBD>꣬<EFBFBD>޷<EFBFBD>ȷ<EFBFBD><C8B7><EFBFBD>ռ<EFBFBD><D5BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>޷<EFBFBD>ȷ<EFBFBD><C8B7><EFBFBD>׵Ĵ<D7B5>С
		std::vector<std::vector<SVzNL3DPoint>> quantiData_topLevel;
		std::vector<std::vector<SVzNL2DPoint>> backIndexing_topLevel;
		pointCloud2DQuantization(
			cutData_topLevel,
			roi3D.xRange,
			roi3D.yRange,
			quatiScale,
			quatiEdgeSkip,
			inerPolateDistTh,  //<2F><>ֵ<EFBFBD><D6B5>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD>ڴ<EFBFBD><DAB4><EFBFBD>ֵ<EFBFBD>IJ<EFBFBD><C4B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
			quantiData_topLevel,  //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD><DDA3><EFBFBD>ʼ<EFBFBD><CABC>Ϊһ<CEAA><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ1e+6
			backIndexing_topLevel //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڻ<EFBFBD><DABB><EFBFBD>3D<33><44><EFBFBD><EFBFBD>
		);
		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;
		}

		//<2F><><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		if (false == topLevelIsEmpty)
		{
			//<2F><><EFBFBD>ɵ<EFBFBD><C9B5><EFBFBD>Mask<73><6B> ʹ<><CAB9>SSG_clusterLabel<65>ṹ
			std::vector<std::vector<SSG_clusterLabel>> labelMask_topLevel;
			genValidPtLabelMask(quantiData_topLevel, labelMask_topLevel, KeWD_Mask_NullPt);  //Ŀ<>ģ<EFBFBD><C4A3><EFBFBD>ȡ<EFBFBD>׵<EFBFBD>
			//
			//ʹ<>þ<EFBFBD><C3BE>෽<EFBFBD><E0B7BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
			std::vector<std::vector< SVzNL2DPoint>> clusters;
			std::vector<SWD_clustersInfo> clustersInfo;
			cloutPointsClustering_2(
				quantiData_topLevel,
				labelMask_topLevel,
				clusters, //ֻ<><D6BB>¼λ<C2BC><CEBB>
				clustersInfo);

			if (clusters.size() == 1)  //<2F><><EFBFBD><EFBFBD>ֻ<EFBFBD><D6BB>һ<EFBFBD><D2BB>Ŀ<EFBFBD>꣬˵<EAA3AC><CBB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>topLevel<65><6C><EFBFBD>ǿյ㣬<D5B5>ϲ<EFBFBD>Ϊ<EFBFBD><CEAA>
			{
				topLevelIsEmpty = true;
			}
			else
			{
				//<2F><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF>
				std::vector<int> validClusterIndice;//<2F>ϸ<EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>š<EFBFBD><C5A1>˴<EFBFBD>ʹ<EFBFBD><CAB9><EFBFBD><EFBFBD><EFBFBD>ţ<EFBFBD><C5A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݿ<EFBFBD><DDBF><EFBFBD>
				//<2F><><EFBFBD>ݴ<EFBFBD>С<EFBFBD><D0A1>ȡĿ<C8A1><C4BF>
				clusterSizeMatching(clusters, clustersInfo, statorParam, validClusterIndice); //<2F>ϸ<EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>š<EFBFBD><C5A1>˴<EFBFBD>ʹ<EFBFBD><CAB9><EFBFBD><EFBFBD><EFBFBD>ţ<EFBFBD><C5A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݿ<EFBFBD><DDBF><EFBFBD>
				if (validClusterIndice.size() == 0)  //û<><C3BB><EFBFBD>ҵ<EFBFBD>Ŀ<EFBFBD><C4BF>(<28><>ʱ<EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD><DDA3><EFBFBD><EFBFBD>˱<EFBFBD><CBB1><EFBFBD><EFBFBD><EFBFBD>
				{
					*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++)
					{
						//<2F><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF>λ<EFBFBD><CEBB>
						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)  //û<><C3BB><EFBFBD>ҵ<EFBFBD>Ŀ<EFBFBD><C4BF>(<28><>ʱ<EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD><DDA3><EFBFBD><EFBFBD>˱<EFBFBD><CBB1><EFBFBD><EFBFBD><EFBFBD>
					{
						*errCode = SX_ERR_ZERO_OBJ_TOPLAYER;
						return;
					}

					//<2F><><EFBFBD>ɽ<EFBFBD><C9BD><EFBFBD>
					computeBestObject(
						objectRgns,
						statorParam,
						opState,
						resultObjPositions
						);

					//<2F><><EFBFBD><EFBFBD>״̬<D7B4><CCAC>
					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;
				}
			}
		}
	}

	//<2F><><EFBFBD><EFBFBD>Ϊ<EFBFBD>գ<EFBFBD><D5A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ײ<EFBFBD>
	if (true == topLevelIsEmpty)  
	{
		//<2F>Եײ<D4B5><D7B2><EFBFBD><EFBFBD>д<EFBFBD><D0B4><EFBFBD>
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͼ<EFBFBD><CDBC><EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ռ䴦û<E4B4A6><C3BB><EFBFBD><EFBFBD><EFBFBD>꣬<EFBFBD>޷<EFBFBD>ȷ<EFBFBD><C8B7><EFBFBD>ռ<EFBFBD><D5BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>޷<EFBFBD>ȷ<EFBFBD><C8B7><EFBFBD>׵Ĵ<D7B5>С
		std::vector<std::vector<SVzNL3DPoint>> quantiData_btmLevel;
		std::vector<std::vector<SVzNL2DPoint>> backIndexing_btmLevel;
		pointCloud2DQuantization(
			cutData_btmLevel,
			roi3D.xRange,
			roi3D.yRange,
			quatiScale,
			quatiEdgeSkip,
			inerPolateDistTh,  //<2F><>ֵ<EFBFBD><D6B5>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD>ڴ<EFBFBD><DAB4><EFBFBD>ֵ<EFBFBD>IJ<EFBFBD><C4B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
			quantiData_btmLevel,  //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD><DDA3><EFBFBD>ʼ<EFBFBD><CABC>Ϊһ<CEAA><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ1e+6
			backIndexing_btmLevel //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڻ<EFBFBD><DABB><EFBFBD>3D<33><44><EFBFBD><EFBFBD>
		);
		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))
		{
			//<2F>տ<EFBFBD>
			*errCode = SX_ERR_ZERO_OBJ_BTMLAYER;
			return;
		}

		//<2F><><EFBFBD><EFBFBD><EFBFBD>任<EFBFBD>жϸ<D0B6><CFB8><EFBFBD>
		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);  //<2F><><EFBFBD><EFBFBD><EFBFBD>任Mask<73><6B><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>Ϊһ<CEAA><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ1e+6
			getDistanceTransformMaskData(quantiData_btmLevel, projectionImg ); // ͶӰ<CDB6><D3B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD><DDA3><EFBFBD>ʼ<EFBFBD><CABC>Ϊһ<CEAA><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ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
			//ȡ<><C8A1><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><C4BE><EFBFBD><EFBFBD>任ֵ<E4BBBB><D6B5><EFBFBD><EFBFBD>
			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;
					}
				}
				//<2F><>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD>ע
				cv::Mat labImg;
				std::vector<SSG_Region> labelRgns;
				SG_TwoPassLabel(bwImg, labImg, labelRgns, 8);
				//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>ץȡ<D7A5><C8A1><EFBFBD>ĵ<EFBFBD>
				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)
				{
					//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>
					*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;
			}
		}

		//<2F><><EFBFBD><EFBFBD><EFBFBD>²<EFBFBD>Ŀ<EFBFBD><C4BF>
		//<2F><><EFBFBD>ɵ<EFBFBD><C9B5><EFBFBD>Mask<73><6B> ʹ<><CAB9>SSG_clusterLabel<65>ṹ
		std::vector<std::vector<SSG_clusterLabel>> labelMask_btmLevel;
		genValidPtLabelMask(quantiData_btmLevel, labelMask_btmLevel, KeWD_Mask_NullPt);  //Ŀ<>ģ<EFBFBD><C4A3><EFBFBD>ȡ<EFBFBD>׵<EFBFBD>
		//ʹ<>þ<EFBFBD><C3BE>෽<EFBFBD><E0B7BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		std::vector<std::vector< SVzNL2DPoint>> clusters;
		std::vector<SWD_clustersInfo> clustersInfo;
		cloutPointsClustering_2(
			quantiData_btmLevel,
			labelMask_btmLevel,
			clusters, //ֻ<><D6BB>¼λ<C2BC><CEBB>
			clustersInfo);

		if (clusters.size() == 1)  //<2F><><EFBFBD><EFBFBD>ֻ<EFBFBD><D6BB>һ<EFBFBD><D2BB>Ŀ<EFBFBD>꣬˵<EAA3AC><CBB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>topLevel<65><6C><EFBFBD>ǿյ㣬<D5B5>ϲ<EFBFBD>Ϊ<EFBFBD><CEAA>
		{
			//<2F>տ<EFBFBD>
			*errCode = SX_ERR_ZERO_OBJ_BTMLAYER;
			return;
		}
		else
		{
			//<2F><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF>
			std::vector<int> validClusterIndice;//<2F>ϸ<EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>š<EFBFBD><C5A1>˴<EFBFBD>ʹ<EFBFBD><CAB9><EFBFBD><EFBFBD><EFBFBD>ţ<EFBFBD><C5A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݿ<EFBFBD><DDBF><EFBFBD>
			//<2F><><EFBFBD>ݴ<EFBFBD>С<EFBFBD><D0A1>ȡĿ<C8A1><C4BF>
			clusterSizeMatching(clusters, clustersInfo, statorParam, validClusterIndice); //<2F>ϸ<EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>š<EFBFBD><C5A1>˴<EFBFBD>ʹ<EFBFBD><CAB9><EFBFBD><EFBFBD><EFBFBD>ţ<EFBFBD><C5A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݿ<EFBFBD><DDBF><EFBFBD>
			if (validClusterIndice.size() == 0)  //û<><C3BB><EFBFBD>ҵ<EFBFBD>Ŀ<EFBFBD><C4BF>(<28><>ʱ<EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD><DDA3><EFBFBD><EFBFBD>˱<EFBFBD><CBB1><EFBFBD><EFBFBD><EFBFBD>
			{
				*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++)
				{
					//<2F><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF>λ<EFBFBD><CEBB>
					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)  //û<><C3BB><EFBFBD>ҵ<EFBFBD>Ŀ<EFBFBD><C4BF>(<28><>ʱ<EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD><DDA3><EFBFBD><EFBFBD>˱<EFBFBD><CBB1><EFBFBD><EFBFBD><EFBFBD>
				{
					*errCode = SX_ERR_ZERO_OBJ_BTMLAYER;
					return;
				}

				//<2F><><EFBFBD>ɽ<EFBFBD><C9BD><EFBFBD>
				computeBestObject(
					objectRgns,
					statorParam,
					opState,
					resultObjPositions);

				//<2F><><EFBFBD><EFBFBD>״̬<D7B4><CCAC>
				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;
}