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;

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 SG_motorStatorPositionParam 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 compareByNeighbourDist(const SSG_motorStatorPosition& a, const SSG_motorStatorPosition& b)
{
	return a.obstacleDist > b.obstacleDist;
}
bool compareByNeighbourNum(const SSG_motorStatorPosition& a, const SSG_motorStatorPosition& b)
{
	return a.neighbourNum < b.neighbourNum;
}
typedef struct
{
	cv::RotatedRect fittingPara;
	SVzNL3DPoint objCenter;
}SG_fittingInfo;

void sg_motorStatorPosition(
	SVzNL3DLaserLine* laser3DPoints,
	int lineNum,
	const SG_motorStatorPositionParam positionParam,
	int* errCode,
	std::vector<SSG_motorStatorPosition>& resultOpPositions
)
{
	/// ͳ<><CDB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ұ<EFBFBD><D2B0>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֮<EFBFBD><D6AE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	SVzNL3DRangeD roi3D = sg_getScanDataROI(
		laser3DPoints,
		lineNum);
	SVzNLRangeD z_range = roi3D.zRange;

	//Z<><5A><EFBFBD><EFBFBD>ͳ<EFBFBD>ơ<EFBFBD><C6A1><EFBFBD><EFBFBD><EFBFBD>Ϊ1mm
	int zHistSize = (int)(z_range.max - z_range.min) + 1;
	std::vector<int> zHist;
	zHist.resize(zHistSize);
	int totalPtNum = 0;
	for (int line = 0; line < lineNum; line++)
	{
		for (int i = 0; i < laser3DPoints[line].nPositionCnt; i++)
		{
			SVzNL3DPosition* pt3D = &laser3DPoints[line].p3DPosition[i];
			if (pt3D->pt3D.z < 1e-4)
				continue;

			totalPtNum++;
			int zPos = (int)(pt3D->pt3D.z - z_range.min);
			zHist[zPos] ++;
		}
	}
	//ȡ<><C8A1><EFBFBD>Ӷ<EFBFBD><D3B6>棺ȡ5mmͳ<6D><CDB3><EFBFBD><EFBFBD><EFBFBD>䣬zHist<73><74><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>10%
	std::vector<int> sumHist;
	sumHist.resize(zHistSize);
	for (int i = 0; i < zHistSize; i++)
	{
		int data = 0;
		for (int j = i - 2; j < i + 2; j++)
		{
			if ((j >= 0) && (j < zHistSize))
				data += zHist[j];
		}
		sumHist[i] = data;
	}
	//ȡ<><C8A1>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD>ڴ<EFBFBD><DAB4><EFBFBD>10%<25>ļ<EFBFBD>ֵ<EFBFBD><D6B5>
	int dataTh = totalPtNum / 10;
	int maxPos = -1;
	for (int i = 1; i < zHistSize-1; i++)
	{
		int preData = sumHist[i-1];
		int data = sumHist[i];
		int nxtData = sumHist[i+1];
		if ((data > preData) && (data > nxtData) && (data > dataTh))
		{
			maxPos = i;
			break;
		}
	}
	if (maxPos < 0)
		return;

	//ȡ<><C8A1><EFBFBD>Ӷ<EFBFBD><D3B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	double zSliceTop = (double)maxPos - 10 + z_range.min;
	double zSliceBtm = (double)maxPos + 8 + z_range.min;

	//<2F><>XOY<4F><59><EFBFBD><EFBFBD>ͶӰ
	//<2F><><EFBFBD><EFBFBD><EFBFBD>任Mask<73><6B><EFBFBD><EFBFBD>1mmΪ<6D><CEAA><EFBFBD><EFBFBD><EFBFBD>߶<EFBFBD>
	int maskX = (int)(roi3D.xRange.max - roi3D.xRange.min) + 1;
	int maskY = (int)(roi3D.yRange.max - roi3D.yRange.min) + 1;
	cv::Mat zSliceData = cv::Mat::zeros(maskY, maskX, CV_8U);
	cv::Mat distTranformMask(maskY, maskX, CV_32FC1, 1e+6);  //<2F><><EFBFBD><EFBFBD><EFBFBD>任Mask<73><6B><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>Ϊһ<CEAA><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ1e+6
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ھ<EFBFBD><DABE><EFBFBD><EFBFBD>任<EFBFBD><E4BBBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	cv::Mat distTranformIndexing(maskY, maskX, CV_32SC2, cv::Vec2i(0, 0));  //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	for (int line = 0; line < lineNum; line++)
	{
		for (int i = 0; i < laser3DPoints[line].nPositionCnt; i++)
		{
			SVzNL3DPosition* pt3D = &laser3DPoints[line].p3DPosition[i];
			if ( (pt3D->pt3D.z < zSliceTop) || (pt3D->pt3D.z > zSliceBtm))
				continue;
			double x = pt3D->pt3D.x;
			double y = pt3D->pt3D.y;

			int px = (int)(x - roi3D.xRange.min);
			int py = (int)(y - roi3D.yRange.min);
			cv::Vec2i v2i = { line, i };
			zSliceData.at<uchar>(py, px) = (uchar)255;
			distTranformIndexing.at<cv::Vec2i>(py, px) = v2i;
			distTranformMask.at<float>(py, px) = 0;
		}
	}
	cv::Mat zSliceData_origin = zSliceData.clone();

	//<2F><><EFBFBD><EFBFBD><EFBFBD>任
	cv::Mat distTransform;
	sg_distanceTrans(distTranformMask, distTransform, 0);
#if DEBUG_OUT_IMAGE  //debug	
	cv::Mat dtImage;
	cv::normalize(distTranformMask, dtImage, 0, 255, cv::NORM_MINMAX, CV_8U);
	cv::imwrite("distTransform.png", dtImage);
	//cv::normalize(zSliceData, dtImage, 64, 255, cv::NORM_MINMAX, CV_8U);
	cv::imwrite("zSliceImage.png", zSliceData);
#endif

	//<2F><>zSliceData<74><61><EFBFBD>д<EFBFBD><D0B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD>䣨2<E4A3A8><32><EFBFBD><EFBFBD>ʴ<EFBFBD><CAB4>3<EFBFBD><33><EFBFBD><EFBFBD>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD>Ŀ<EFBFBD>꣨4<EAA3A8><34><EFBFBD><EFBFBD>Բ<EFBFBD><D4B2><EFBFBD>ϵõ<CFB5><C3B5><EFBFBD><EFBFBD>ĵ<EFBFBD>
	cv::Mat invertSliceData;
	cv::bitwise_not(zSliceData, invertSliceData);
#if DEBUG_OUT_IMAGE
	cv::imwrite("zSliceImage_invert.png", invertSliceData);
#endif
	//<2F><>ͨ<EFBFBD><CDA8>
	cv::Mat labels, centroids, stats;
	int nccomps = connectedComponentsWithStats(invertSliceData, labels, stats, centroids, 4);
	for (int i = 1; i < nccomps; i++)
	{
		int size = stats.at<int>(i, cv::CC_STAT_AREA);
		if (size < 20)
		{
			int roi_x = stats.at<int>(i, cv::CC_STAT_LEFT);
			int roi_y = stats.at<int>(i, cv::CC_STAT_TOP);
			int roi_w = stats.at<int>(i, cv::CC_STAT_WIDTH);
			int roi_h = stats.at<int>(i, cv::CC_STAT_HEIGHT);
			//<2F><>С<EFBFBD><D0A1>20<32><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
			for (int row = roi_y; row < roi_y+roi_h; row++)
			{
				for (int col = roi_x; col < roi_x+roi_w; col++)
				{
					if (labels.at<int>(row, col) == i)
						zSliceData.at<uchar>(row, col) = (uchar)255;
				}
			}
		}
	}
#if DEBUG_OUT_IMAGE
	cv::imwrite("zSliceImage_filled.png", zSliceData);
#endif

	//<2F><>ʴ
	cv::Mat test = cv::Mat::zeros(64, 64, CV_8UC1);
	cv::rectangle(test, cv::Rect(30, 30, 5, 5), 255, -1);

	cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
	cv::Mat zSliceData_erode;
	cv::erode(zSliceData, zSliceData_erode, element, cv::Point(-1,-1),2);
#if DEBUG_OUT_IMAGE
	cv::imwrite("zSliceImage_erode.png", zSliceData_erode);
#endif
	//<2F><>ȡĿ<C8A1><C4BF>
	nccomps = connectedComponentsWithStats(zSliceData_erode, labels, stats, centroids, 4);
	//Ŀ<><C4BF><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	int obj_size_min = (int)(positionParam.statorInnerD * 0.75);
	int obj_size_max = (int)(positionParam.statorOuterD * 1.25);
	std::vector<SG_fittingInfo> fittingObs;
	for (int i = 1; i < nccomps; i++)
	{
		int roi_x = stats.at<int>(i, cv::CC_STAT_LEFT);
		int roi_y = stats.at<int>(i, cv::CC_STAT_TOP);
		int roi_w = stats.at<int>(i, cv::CC_STAT_WIDTH);
		int roi_h = stats.at<int>(i, cv::CC_STAT_HEIGHT);
		if( (roi_w > obj_size_min) && (roi_w < obj_size_max) && (roi_h > obj_size_min) && (roi_h < obj_size_max))
		{
			//<2F>ϸ<EFBFBD><CFB8><EFBFBD>Ŀ<EFBFBD><C4BF>
			//ȡ<><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
			std::vector<cv::Point > contourPts;
			for (int row = roi_y; row < roi_y + roi_h; row++)
			{
				for (int col = roi_x; col < roi_x + roi_w; col++)
				{
					if (labels.at<int>(row, col) == i)
						contourPts.push_back(cv::Point(col, row));
				}
			}
			cv::RotatedRect ellipse = cv::fitEllipse(contourPts);
			SG_fittingInfo a_fitting;
			a_fitting.fittingPara = ellipse;
			//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߶<EFBFBD>Z
			double z_sum = 0;
			double z_counter = 0;
			for (int m = 0; m < contourPts.size(); m++)
			{
				cv::Vec2i ptIndexing = distTranformIndexing.at<cv::Vec2i>(contourPts[m].y, contourPts[m].x);
				int line = ptIndexing[0];
				int ptIdx = ptIndexing[1];
				SVzNL3DPosition* pt3D = &laser3DPoints[line].p3DPosition[ptIdx];
				if (pt3D->pt3D.z < 1e-4)
					continue;
				z_sum += pt3D->pt3D.z;
				z_counter++;
			}
			if (z_counter > 0)
			{
				a_fitting.objCenter.x = ellipse.center.x + roi3D.xRange.min;
				a_fitting.objCenter.y = ellipse.center.y + roi3D.yRange.min;
				z_sum = z_sum / z_counter;
				a_fitting.objCenter.z = z_sum;
				a_fitting.fittingPara = ellipse;
				fittingObs.push_back(a_fitting);
			}

		}
	}
#if DEBUG_OUT_IMAGE
	cv::Mat fittingImage;
	cv::cvtColor(zSliceData_origin, fittingImage, cv::COLOR_GRAY2BGR);
	for(int i = 0; i < fittingObs.size(); i ++)
		cv::ellipse(fittingImage, fittingObs[i].fittingPara, cv::Scalar(0, 255, 0), 2); // <20><>ԭͼ<D4AD>ϻ<EFBFBD><CFBB><EFBFBD><EFBFBD><EFBFBD>Բ
	cv::imwrite("zSliceImage_fitting.png", fittingImage);
#endif
	//<2F><><EFBFBD><EFBFBD><EFBFBD>ڽӹ<DABD>ϵȷ<CFB5><C8B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ץȡ<D7A5><C8A1>
	std::vector< SSG_statorNeighbourInfo> neighbouringInfo;
	_getNeighbouringInfo(
		positionParam,
		zSliceData_origin,  //<2F><><EFBFBD><EFBFBD>Ѱ<EFBFBD>ұ߽磨<DFBD>߿<EFBFBD><DFBF><EFBFBD> 
		fittingObs, //Ŀ<><C4BF>λ<EFBFBD><CEBB>
		neighbouringInfo //<2F>ڽӹ<DABD>ϵ
	);

	//<2F><><EFBFBD><EFBFBD><EFBFBD>ڽӹ<DABD>ϵȷ<CFB5><C8B7>ץȡ<D7A5><C8A1>
	std::vector< SSG_motorStatorPosition> grabPoses; //С<>ڵ<EFBFBD><DAB5><EFBFBD>3<EFBFBD><33><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD>ץȡ<D7A5>㣨<EFBFBD><E3A3A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	std::vector< SSG_motorStatorPosition> grabPoses_2; //<2F><><EFBFBD><EFBFBD>3<EFBFBD><33><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD>ץȡ<D7A5>㣨<EFBFBD><E3A3A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	//grabPoses.resize(neighbouringInfo.size());
	for (int i = 0; i < neighbouringInfo.size(); i++)
	{
		double opAngle = -1;
		double obstacleDist = -1;
		_computeGripperPose(neighbouringInfo[i], positionParam.gripperR, &opAngle, &obstacleDist);
		SSG_motorStatorPosition opPos;
		memset(&opPos, 0, sizeof(SSG_motorStatorPosition));
		opPos.neighbourNum = neighbouringInfo[i].neighbours.size();
		opPos.obstacleDist = obstacleDist;
		opPos.opAngle = opAngle;
		opPos.opCenter = neighbouringInfo[i].objPos3D;
		if (opPos.neighbourNum <= 3)
			grabPoses.push_back(opPos);
		else
			grabPoses_2.push_back(opPos);
	}

	//ѡȡץȡ<D7A5>㣺<EFBFBD><E3A3BA>1<EFBFBD><31><EFBFBD>ܱ<EFBFBD><DCB1>ھ<EFBFBD><DABE><EFBFBD><EFBFBD>٣<EFBFBD>2<EFBFBD><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ߵľ<DFB5><C4BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	std::sort(grabPoses.begin(), grabPoses.end(),compareByNeighbourDist);
	std::sort(grabPoses_2.begin(), grabPoses_2.end(), compareByNeighbourNum);
	//<2F>ϲ<EFBFBD>
	grabPoses.insert(grabPoses.end(), grabPoses_2.begin(), grabPoses_2.end());
	return;
}