algoLib/sourceCode/BQ_workpieceCornerExtraction.cpp

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

//version 1.1.0 : base version release to customer, output corner coordinate
//version 1.2.0 : add position length output
//version 1.2.1 : fix bugs for ver1.2.0
//version 1.3.0 : <20>㷨<EFBFBD><E3B7A8>ͬһbrach<63>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˾<EFBFBD><CBBE><EFBFBD><EFBFBD>ж<EFBFBD>
std::string	m_strVersion = "1.3.0";
const char* wd_BQWorkpieceCornerVersion(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 sx_BQ_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 sx_BQ_lineDataR(
	std::vector< SVzNL3DPosition>& a_line,
	const double* camPoseR,
	double groundH)
{
	lineDataRT_vector(a_line, camPoseR, groundH);
}

SVzNL3DPoint _translatePoint(SVzNL3DPoint point, double rMatrix[9])
{
	SVzNL3DPoint result;
	double x = point.x * rMatrix[0] + point.y * rMatrix[1] + point.z * rMatrix[2];
	double y = point.x * rMatrix[3] + point.y * rMatrix[4] + point.z * rMatrix[5];
	double z = point.x * rMatrix[6] + point.y * rMatrix[7] + point.z * rMatrix[8];
	result.x = x;
	result.y = y;
	result.z = z;
	return result;
}

//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ROI
void sg_getTreeROI(SSG_featureTree* a_tree)
{
	if (a_tree->treeNodes.size() == 0)
	{
		a_tree->roi.left = 0;
		a_tree->roi.right = 0;
		a_tree->roi.top = 0;
		a_tree->roi.bottom = 0;
	}
	else
	{
		a_tree->roi.left = a_tree->treeNodes[0].jumpPos.x;
		a_tree->roi.right = a_tree->treeNodes[0].jumpPos.x;
		a_tree->roi.top = a_tree->treeNodes[0].jumpPos.y;
		a_tree->roi.bottom = a_tree->treeNodes[0].jumpPos.y;
		for (int i = 1, i_max = a_tree->treeNodes.size(); i < i_max; i++)
		{
			if (a_tree->roi.left > a_tree->treeNodes[i].jumpPos.x)
				a_tree->roi.left = a_tree->treeNodes[i].jumpPos.x;
			if (a_tree->roi.right < a_tree->treeNodes[i].jumpPos.x)
				a_tree->roi.right = a_tree->treeNodes[i].jumpPos.x;
			if (a_tree->roi.top > a_tree->treeNodes[i].jumpPos.y)
				a_tree->roi.top = a_tree->treeNodes[i].jumpPos.y;
			if (a_tree->roi.bottom < a_tree->treeNodes[i].jumpPos.y)
				a_tree->roi.bottom = a_tree->treeNodes[i].jumpPos.y;
		}
	}
	return;
}

void _getEdgeContour(SSG_featureTree* a_tree, std::vector<SVzNL3DPoint>& contour, std::vector< std::vector<SVzNL3DPosition>>& scanLines, bool isVScan)
{
	for (int j = 0, j_max = (int)a_tree->treeNodes.size(); j < j_max; j++)
	{
		SSG_basicFeature1D* a_feature = &a_tree->treeNodes[j];
		SVzNL3DPoint a_pt;
		if (true == isVScan)
			a_pt = scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D;
		else
			a_pt = scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D;
		if (a_pt.z > 1e-4)//<2F><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>˺<EFBFBD><CBBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0
		{
			contour.push_back(a_pt);
		}
	}
}

int _getPointClosestContour(std::vector<SSG_featureTree> trees, bool isVscanTrees, SVzNL3DPoint seedPt, std::vector< std::vector<SVzNL3DPosition>>& scanLines, bool fromHead)
{
	double minDist = -1.0;
	int idx = -1;
	for (int i = 0, i_max = (int)trees.size(); i < i_max; i++)
	{
		SSG_basicFeature1D a_feature;
		if (true == fromHead)
			a_feature = trees[i].treeNodes[0];
		else
			a_feature = trees[i].treeNodes.back();

		SVzNL3DPoint a_pt;
		if (true == isVscanTrees)
			a_pt = scanLines[a_feature.jumpPos2D.x][a_feature.jumpPos2D.y].pt3D;
		else
			a_pt = scanLines[a_feature.jumpPos2D.y][a_feature.jumpPos2D.x].pt3D;

		double dist = sqrt(pow(a_pt.x - seedPt.x, 2) + pow(a_pt.y - seedPt.y, 2));
		if (minDist < 0)
		{
			minDist = dist;
			idx = i;
		}
		else
		{
			if(dist < minDist)
			{
				minDist = dist;
				idx = i;
			}
		}
	}
	return idx;
}

void _getEdgeLinkingContour(SSG_featureTree* a_tree, bool isVScanTree, SVzNL3DPoint seedPt, std::vector<SVzNL3DPoint>& contour, std::vector< std::vector<SVzNL3DPosition>>& scanLines, bool fromHead, double lineLen)
{
	for (int i = 0, i_max = (int)a_tree->treeNodes.size(); i < i_max; i++)
	{
		int idx = i;
		if (false == fromHead)
			idx = i_max - 1 - i;
		SSG_basicFeature1D* a_feature = &a_tree->treeNodes[idx];

		SVzNL3DPoint a_pt;
		if (true == isVScanTree)
			a_pt = scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D;
		else
			a_pt = scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D;
		if (a_pt.z > 1e-4)//<2F><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>˺<EFBFBD><CBBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0
		{
			double dist = sqrt(pow(a_pt.x - seedPt.x, 2) + pow(a_pt.y - seedPt.y, 2));
			if (dist > lineLen)
				break;
			contour.push_back(a_pt);
		}
	}
}

typedef struct
{
	int rgnIdx;
	std::vector<SVzNL3DPoint> edge;
	SVzNL3DPoint edge_ends[2];
	std::vector<SVzNL3DPoint> edgeLink_1;
	SVzNL3DPoint edge_link1_ends[2];
	std::vector<SVzNL3DPoint> edgeLink_2;
	SVzNL3DPoint edge_link2_ends[2];
}SSX_featureContour;

typedef struct
{
	double angle;
	SVzNL3DPoint corner[3];
	double line_a, line_b, line_c; //<2F>ߵĴ<DFB5><C4B4>߷<EFBFBD><DFB7><EFBFBD> ax+by+c = 0
}SWD_branchInfo;

//<2F><>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>תʱ <20><> > 0 <20><>˳ʱ<CBB3><CAB1><EFBFBD><EFBFBD>תʱ <20><> < 0 
cv::Point2f _rotate2D(cv::Point2f pt, double sinTheta, double cosTheta)
{
	return (cv::Point2f((float)(pt.x * cosTheta - pt.y * sinTheta), (float)(pt.x * sinTheta + pt.y * cosTheta)));
}

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

int _counterLinePtNum(std::vector<SVzNL3DPosition>& lineData)
{
	int ptNum = 0;
	for (int i = 0, i_max = (int)lineData.size(); i < i_max; i++)
	{
		if ( (abs(lineData[i].pt3D.z) > 1e-4) || 
			(abs(lineData[i].pt3D.x > 1e-4)) ||
			(abs(lineData[i].pt3D.y > 1e-4)))
			ptNum++;
	}
	return ptNum;
}

//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֧<EFBFBD><D6A7>Ϣ
int _getBranchInfo(
	int validStartLine,   //<2F><>ʼɨ<CABC><C9A8><EFBFBD>߽<EFBFBD>
	int validEndLine, //<2F><><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD>߽<EFBFBD>
	bool partialScan, //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ұ<EFBFBD><D2B0><EFBFBD>ܸ<EFBFBD><DCB8>ǹ<EFBFBD><C7B9><EFBFBD>ȫ<EFBFBD><C8AB>ʱ<EFBFBD><CAB1>partialScanΪtrue,
	std::vector<SWD_polarPt>& polarPoints,
	SWD_polarPt branchCorner,
	SWD_polarPeakInfo branchCornerInfo,
	SWD_branchInfo* resultBranchInfo,
	std::vector<SVzNL3DPoint>& LinePts,
	std::vector< SVzNL3DPoint>& edgePt1,
	std::vector< SVzNL3DPoint>& edgePt2
	)
{
	int contourPtSize = (int)polarPoints.size();
	std::vector<SWD_polarPt> branchContourPts;

	int validStartLineWin = validStartLine + 3;
	int validEndLineWin = validEndLine - 3; 
	if (validEndLineWin < 0)
		validEndLineWin = 0;
	int cornerWin = 2;
	int LineDir, contourDir, startIdx, endingIdx;
	if (branchCornerInfo.cornerDir == 2)//<2F><>ʱ<EFBFBD><CAB1>
	{
		startIdx = branchCorner.cptIndex - cornerWin;  //<2F>˴<EFBFBD><CBB4><EFBFBD>corner<65><72>Χ<EFBFBD>ĵ㲻<C4B5><E3B2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		if (startIdx < 0)
			startIdx += contourPtSize;
		LineDir = -1;
		contourDir = 1;
		endingIdx = branchCornerInfo.L1_ptIndex;
	}
	else  //˳ʱ<CBB3><CAB1>
	{
		startIdx = (branchCorner.cptIndex + cornerWin) % contourPtSize;
		LineDir = 1;
		contourDir = -1;
		endingIdx = branchCornerInfo.L2_ptIndex;
	}

	int ptIdx = startIdx;
	while (1)
	{
		SVzNL3DPoint a_pt = { polarPoints[ptIdx].x, polarPoints[ptIdx].y, polarPoints[ptIdx].z };
		LinePts.push_back(a_pt);
		if (ptIdx == endingIdx)
			break;
		ptIdx += LineDir;
		if (ptIdx < 0)
			ptIdx += contourPtSize;
		else
			ptIdx = ptIdx % contourPtSize;
	}
	if (LinePts.size() < 5)
		return -1;

	//<2F><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD>߽类<DFBD>г<EFBFBD>һ<EFBFBD><D2BB><EFBFBD>ϱߺ<CFB1>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ľǵ<C4BD><C7B5><EFBFBD><EFBFBD>ܲ<EFBFBD><DCB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ľǵ㣬<C7B5><E3A3AC>Ҫ<EFBFBD><D2AA><EFBFBD>м<EFBFBD><D0BC><EFBFBD>
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ľǵ㣺ȡcorner<65><72><EFBFBD><EFBFBD>5<EFBFBD><35><EFBFBD>㣬<EFBFBD><E3A3AC><EFBFBD>б߽磬<DFBD><E7A3AC><EFBFBD><EFBFBD>
	int chkWin = 5;
	bool toRefine = false;
	for (int i = -chkWin; i <= chkWin; i++)
	{
		int idx = branchCorner.cptIndex + i;
		if (idx < 0)
			idx += contourPtSize;
		else
			idx = idx % contourPtSize;
		if ((partialScan == true) &&
			((polarPoints[idx].lineIdx == validStartLine) || (polarPoints[idx].lineIdx == validEndLine)))
		{
			toRefine = true;
			break;
		}
	}
	if (true == toRefine)
	{
		SVzNL3DPoint pt1 = LinePts[0];
		SVzNL3DPoint pt2 = LinePts.back();
		double aa, bb, cc;
		compute2ptLine(	pt1, pt2, &aa, &bb, &cc);
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ľǵ<C4BD>
		double maxH = 0;
		int maxHPos = 0;
		for (int i = 0; i < (int)LinePts.size(); i++)
		{
			double H = computePtDistToLine(LinePts[i].x, LinePts[i].y, aa, bb, cc);
			if (maxH < H)
			{
				maxH = H;
				maxHPos = i;
			}
		}
		//<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
		int recEnding = maxHPos * 2;
		if (recEnding >= LinePts.size())
			recEnding = (int)LinePts.size() - 1;
		pt1 = LinePts[0];
		pt2 = LinePts[recEnding];
		compute2ptLine(pt1, pt2, &aa, &bb, &cc);
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ľǵ<C4BD>
		maxH = 0;
		maxHPos = 0;
		for (int i = 0; i < recEnding; i++)
		{
			double H = computePtDistToLine(LinePts[i].x, LinePts[i].y, aa, bb, cc);
			if (maxH < H)
			{
				maxH = H;
				maxHPos = i;
			}
		}

		int cptIdx = startIdx + LineDir * maxHPos;
		if (cptIdx < 0)
			cptIdx += contourPtSize;
		else
			cptIdx = cptIdx % contourPtSize;
		branchCorner = polarPoints[cptIdx];	
		if (branchCornerInfo.cornerDir == 2)//<2F><>ʱ<EFBFBD><CAB1>
		{
			startIdx = branchCorner.cptIndex - cornerWin;  //<2F>˴<EFBFBD><CBB4><EFBFBD>corner<65><72>Χ<EFBFBD>ĵ㲻<C4B5><E3B2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
			if (startIdx < 0)
				startIdx += contourPtSize;
		}
		else  //˳ʱ<CBB3><CAB1>
			startIdx = (branchCorner.cptIndex + cornerWin) % contourPtSize;

		LinePts.clear();
		ptIdx = startIdx;
		while (1)
		{
			SVzNL3DPoint a_pt = { polarPoints[ptIdx].x, polarPoints[ptIdx].y, polarPoints[ptIdx].z };
			LinePts.push_back(a_pt);
			if (ptIdx == endingIdx)
				break;
			ptIdx += LineDir;
			if (ptIdx < 0)
				ptIdx += contourPtSize;
			else
				ptIdx = ptIdx % contourPtSize;
		}
		if (LinePts.size() < 5)
			return -1;
	}

	//<2F><><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1>: ax+by+c = 0
	double _a = 0, _b = 0, _c = 0;
	lineFitting_abc(LinePts, &_a, &_b, &_c);
	SVzNL3DPoint endingPt0 = LinePts.back();
	//<2F><><EFBFBD><EFBFBD><EFBFBD>˵㴹<CBB5><E3B4B9>
	SVzNL2DPointD footPt = sx_getFootPoint_abc(endingPt0.x, endingPt0.y, _a, _b, _c);
	//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	double minDist = -1;
	for (int i = cornerWin; i < contourPtSize; i++)  //<2F>ܿ<EFBFBD><DCBF>ǵ<EFBFBD><C7B5><EFBFBD>Χ
	{
		int ptIdx = i * contourDir + branchCorner.cptIndex;
		if (ptIdx < 0)
			ptIdx += contourPtSize;
		else
			ptIdx = ptIdx % contourPtSize;
		SWD_polarPt a_pt = polarPoints[ptIdx];
		SVzNL2DPointD contourFoot = sx_getFootPoint_abc(a_pt.x, a_pt.y, _a, _b, _c);
		double dist = sqrt(pow(footPt.x - contourFoot.x, 2) + pow(footPt.y - contourFoot.y, 2));
		if (minDist < 0)
			minDist = dist;
		else
			minDist = minDist > dist ? dist : minDist;

		if ((dist < 10) &&(dist > (minDist + 1.0)))
			break;
		branchContourPts.push_back(a_pt);
	}
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD>б߽<D0B1><DFBD>㡣<EFBFBD><E3A1A3><EFBFBD><EFBFBD><EFBFBD>б߽<D0B1><DFBD>㣬<EFBFBD><E3A3AC>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	int contourPtNum = (int)branchContourPts.size();
	bool hasSidePt = false;
	for (int m = 0; m < contourPtNum; m++)
	{
		if ((partialScan == true) &&
			((branchContourPts[m].lineIdx == validStartLine) ||
				(branchContourPts[m].lineIdx == validEndLine)))
		{
			branchContourPts[m].z = -1.0;  //label
			hasSidePt = true;
		}
	}
	//<2F>ֿ<EFBFBD>branch<63><68><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	if (false == hasSidePt)
	{
		double maxDist = -1;
		int maxPos = -1;
		//<2F>ҵ<EFBFBD><D2B5>յ<EFBFBD>:<3A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD>˵<EFBFBD><CBB5>ľ<EFBFBD><C4BE>룬<EFBFBD><EBA3AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>Ϊ<EFBFBD>յ<EFBFBD>
		for (int m = 0; m < contourPtNum; m++)
		{
			double dist = sqrt(pow(branchContourPts[m].x - endingPt0.x, 2) +
								pow(branchContourPts[m].y - endingPt0.y, 2));
			if (maxDist < 0)
			{
				maxDist = dist;
				maxPos = m;
			}
			else
			{
				if (maxDist < dist)
				{
					maxDist = dist;
					maxPos = m;
				}
			}
		}
		//<2F>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD>
		int start0 = 0;
		int end0 = maxPos - cornerWin;
		
		for (int m = start0; m <= end0; m++)
		{
			SVzNL3DPoint a_pt = { branchContourPts[m].x, branchContourPts[m].y, branchContourPts[m].z };
			edgePt1.push_back(a_pt);
		}

		int start1 = maxPos + cornerWin;
		int end1 = contourPtNum - 1;
		
		for (int m = start1; m <= end1; m++)
		{
			SVzNL3DPoint a_pt = { branchContourPts[m].x, branchContourPts[m].y, branchContourPts[m].z };
			edgePt2.push_back(a_pt);
		}

		//<2F><><EFBFBD><EFBFBD><EFBFBD>󽻵<EFBFBD>
		double edge1_a, edge1_b, edge1_c;
		lineFitting_abc(edgePt1, &edge1_a, &edge1_b, &edge1_c);
		double edge2_a, edge2_b, edge2_c;
		lineFitting_abc(edgePt2, &edge2_a, &edge2_b, &edge2_c);

		//<2F><><EFBFBD>㽻<EFBFBD><E3BDBB>
		SWD_branchInfo a_branchInfo;
		a_branchInfo.corner[0] = computeLineCrossPt_abs(_a, _b, _c, edge1_a, edge1_b, edge1_c);
		a_branchInfo.corner[0].z = computeMeanZ(edgePt1);
		a_branchInfo.corner[2] = computeLineCrossPt_abs(edge1_a, edge1_b, edge1_c, edge2_a, edge2_b, edge2_c);
		a_branchInfo.corner[2].z = a_branchInfo.corner[0].z;
		a_branchInfo.corner[1].x = (a_branchInfo.corner[0].x + a_branchInfo.corner[2].x) / 2;
		a_branchInfo.corner[1].y = (a_branchInfo.corner[0].y + a_branchInfo.corner[2].y) / 2;
		a_branchInfo.corner[1].z = a_branchInfo.corner[0].z;
		//
		a_branchInfo.angle = branchCorner.angle;

		a_branchInfo.line_a = edge1_b;
		a_branchInfo.line_b = -edge1_a;
		a_branchInfo.line_c = -(a_branchInfo.line_a * a_branchInfo.corner[1].x + a_branchInfo.line_b * a_branchInfo.corner[1].y);
		*resultBranchInfo = a_branchInfo;
	}
	else
	{
		//<2F><><EFBFBD>߽<EFBFBD>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD>ʣ<EFBFBD>µ<EFBFBD><C2B5><EFBFBD><EFBFBD>ν<EFBFBD><CEBD>д<EFBFBD><D0B4><EFBFBD>
		std::vector< SVzNL3DPoint> edgePt1;
		for (int i = 0; i < contourPtNum; i++)
		{
			SVzNL3DPoint a_pt = { branchContourPts[i].x, branchContourPts[i].y, branchContourPts[i].z };
			if ((partialScan == true) &&
				((branchContourPts[i].lineIdx <= validStartLineWin)|| (branchContourPts[i].lineIdx >= validEndLineWin)))
			{
				break;
			}
			edgePt1.push_back(a_pt);
		}
		std::vector< SVzNL3DPoint> edgePt2;
		for (int i = contourPtNum - 1; i >= 0; i--)
		{
			SVzNL3DPoint a_pt = { branchContourPts[i].x, branchContourPts[i].y, branchContourPts[i].z };
			if ((partialScan == true) &&
				((branchContourPts[i].lineIdx <= validStartLineWin) || (branchContourPts[i].lineIdx >= validEndLineWin)))
				break;
			edgePt2.insert(edgePt2.begin(), a_pt);
		}
		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD>ϵĴ<CFB5><C4B4>㣬ȡ<E3A3AC><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ġ<EFBFBD><C4A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵĴ<C4B5><C4B4><EFBFBD>
		int edge1PtNum = (int)edgePt1.size();
		int edge2PtNum = (int)edgePt2.size();
		if ((edge1PtNum == 0) || (edge2PtNum == 0))
		{
			return -1;
		}
		SVzNL2DPointD foot = { 0.0,0.0 };
		for (int i = 0; i < edge1PtNum; i++)
		{
			SVzNL2DPointD a_foot = sx_getFootPoint_abc(edgePt1[i].x, edgePt1[i].y, _a, _b, _c);
			foot.x += a_foot.x;
			foot.y += a_foot.y;
		}
		foot.x = foot.x / edge1PtNum;
		foot.y = foot.y / edge1PtNum;
		double edge1_a, edge1_b, edge1_c;
		edge1_a = _b;
		edge1_b = -_a;
		edge1_c = _a * foot.y - _b * foot.x;
		//<2F><><EFBFBD><EFBFBD><EFBFBD>ڶ<EFBFBD><DAB6><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD>ϵĴ<CFB5><C4B4>㣬ȡ<E3A3AC><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ġ<EFBFBD><C4A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵĴ<C4B5><C4B4><EFBFBD>
		foot.x = 0;
		foot.y = 0;
		for (int i = 0; i < edge2PtNum; i++)
		{
			SVzNL2DPointD a_foot = sx_getFootPoint_abc(edgePt2[i].x, edgePt2[i].y, edge1_a, edge1_b, edge1_c);
			foot.x += a_foot.x;
			foot.y += a_foot.y;
		}
		foot.x = foot.x / edge2PtNum;
		foot.y = foot.y / edge2PtNum;
		double edge2_a, edge2_b, edge2_c;
		edge2_a = _a;
		edge2_b = _b;
		edge2_c = -_a * foot.x - _b * foot.y;
		//<2F><><EFBFBD>㽻<EFBFBD><E3BDBB>
		SWD_branchInfo a_branchInfo;
		a_branchInfo.corner[0] = computeLineCrossPt_abs(_a, _b, _c, edge1_a, edge1_b, edge1_c);
		a_branchInfo.corner[0].z = computeMeanZ(edgePt1);
		a_branchInfo.corner[2] = computeLineCrossPt_abs(edge1_a, edge1_b, edge1_c, edge2_a, edge2_b, edge2_c);
		a_branchInfo.corner[2].z = a_branchInfo.corner[0].z;
		a_branchInfo.corner[1].x = (a_branchInfo.corner[0].x + a_branchInfo.corner[2].x) / 2;
		a_branchInfo.corner[1].y = (a_branchInfo.corner[0].y + a_branchInfo.corner[2].y) / 2;
		a_branchInfo.corner[1].z = a_branchInfo.corner[0].z;
		//
		a_branchInfo.angle = branchCorner.angle;
		a_branchInfo.line_a = _a;
		a_branchInfo.line_b = _b;
		a_branchInfo.line_c = -(_a * a_branchInfo.corner[1].x + _b * a_branchInfo.corner[1].y);
		*resultBranchInfo = a_branchInfo;
	}
	return 0;
}


SVzNL3DPoint computeEdgeCross(double angle, std::vector<SWD_polarPt>& polarPoints, double a, double b, double c)
{
	double minDist = -1;
	int angleIdx = -1;
	int ptSize = (int)polarPoints.size();
	for (int i = 0; i < ptSize; i++)
	{
		double diff = computeAngleDiff(angle, polarPoints[i].angle);
		if (diff < 30)  //<2F><><EFBFBD><EFBFBD>60<36>ȷ<EFBFBD>Χ
		{
			double H = computePtDistToLine(polarPoints[i].x, polarPoints[i].y, a, b, c);
			if (minDist < 0)
			{
				minDist = H;
				angleIdx = i;
			}
			else
			{
				if (minDist > H)
				{
					minDist = H;
					angleIdx = i;
				}
			}
		}
	}
	SVzNL3DPoint a_pt = { polarPoints[angleIdx].x, polarPoints[angleIdx].y, polarPoints[angleIdx].z };
	return a_pt;
}


bool checkSameBranch(SWD_polarPt& corner_1, SWD_polarPt& corner_2, 
	std::vector<SWD_polarPt>& polarPoints,
	double center_x, double center_y)
{
	int size = (int)polarPoints.size();
	int midPtIdx;
	if (corner_2.cptIndex > corner_1.cptIndex)
		midPtIdx = (corner_1.cptIndex + corner_2.cptIndex) / 2;
	else
	{
		midPtIdx = (size - corner_1.cptIndex + corner_2.cptIndex) / 2 + corner_1.cptIndex;
		if (midPtIdx >= size)
			midPtIdx = midPtIdx - size;
	}

	SWD_polarPt& midPt = polarPoints[midPtIdx];
	double a1, b1, c1;
	compute2ptLine_2(center_x, center_y, midPt.x, midPt.y, &a1, &b1, &c1);
	double a2, b2, c2;
	compute2ptLine_2(corner_1.x, corner_1.y, corner_2.x, corner_2.y, &a2, &b2, &c2);
	//<2F><><EFBFBD>㽻<EFBFBD><E3BDBB>
	SVzNL3DPoint crossPt = computeLineCrossPt_abs(a1, b1, c1, a2, b2, c2);
	double len = sqrt(pow(crossPt.x - center_x, 2) + pow(crossPt.y - center_y, 2));
	double dist_diff = midPt.R / len;
	if ( (dist_diff > 0.95) &&(dist_diff < 1.05))
		return true;
	else
		return false;

}


SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SSG_cornerParam cornerPara,
	const SSG_outlierFilterParam filterParam,
	SSG_treeGrowParam growParam,
	SSG_planeCalibPara groundCalibPara,
	SSX_BQworkpiecePara workpieceParam,
#if _OUTPUT_DEBUG_DATA
	std::vector<SSX_debugInfo>& debug_contours,
#endif
	int* errCode)
{
	*errCode = 0;
	SSX_BQworkpieceResult workpieceCorners;
	memset(&workpieceCorners, 0, sizeof(SSX_BQworkpieceResult));
	int lineNum = (int)scanLines.size();
	if (lineNum == 0)
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return workpieceCorners;
	}

	//<2F><><EFBFBD><EFBFBD>ʼ<EFBFBD>ͽ<EFBFBD><CDBD><EFBFBD><EFBFBD>Ŀհ<C4BF>ɨ<EFBFBD><C9A8><EFBFBD><EFBFBD>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD>߽<EFBFBD>
	int validStartLine = -1;
	for (int i = 0; i < lineNum; i++)
	{
		int linePtNum = _counterLinePtNum(scanLines[i]);
		if (linePtNum > 0)
		{
			validStartLine = i;
			break;
		}
	}
	int validEndLine = -1;
	for (int i = lineNum - 1; i >= 0; i--)
	{
		int linePtNum = _counterLinePtNum(scanLines[i]);
		if (linePtNum > 0)
		{
			validEndLine = i;
			break;
		}
	}
	if ( (validStartLine < 0) || (validEndLine < 0))
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return workpieceCorners;
	}

	int linePtNum = (int)scanLines[0].size();
	bool isGridData = true;

	//<2F><><EFBFBD><EFBFBD>Ӧ<EFBFBD><D3A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD>Ƕ<EFBFBD>
	{
		//<2F><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ
		std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_v_raw;
		for (int line = 0; line < lineNum; line++)
		{
			if (line == 250)
				int kkk = 1;

			std::vector<SVzNL3DPosition>& lineData = scanLines[line];
			if (linePtNum != (int)lineData.size())
				isGridData = false;

			//<2F>˲<EFBFBD><CBB2><EFBFBD><EFBFBD>˳<EFBFBD><CBB3>쳣<EFBFBD><ECB3A3>
			sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);

			std::vector<SSG_basicFeature1D> line_features;
			int dataSize = (int)lineData.size();
			sg_getLineCornerFeature_BQ(
				&lineData[0],
				dataSize,
				line,
				groundCalibPara.planeHeight,
				cornerPara, //scaleͨ<65><CDA8>ȡbagH<67><48>1/4
				line_features);
			jumpFeatures_v_raw.push_back(line_features);
		}

		if (false == isGridData)//<2F><><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ
		{
			*errCode = SG_ERR_NOT_GRID_FORMAT;
			return workpieceCorners;
		}

		//<2F><><EFBFBD><EFBFBD>ˮƽɨ<C6BD><C9A8>
		std::vector<std::vector<SVzNL3DPosition>> hLines_raw;
		hLines_raw.resize(linePtNum);
		for (int i = 0; i < linePtNum; i++)
			hLines_raw[i].resize(lineNum);
		for (int line = 0; line < lineNum; line++)
		{
			for (int j = 0; j < linePtNum; j++)
			{
				scanLines[line][j].nPointIdx = 0; //<2F><>ԭʼ<D4AD><CABC><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>ʹ<EFBFBD>ã<EFBFBD>
				hLines_raw[j][line] = scanLines[line][j];
				hLines_raw[j][line].pt3D.x = scanLines[line][j].pt3D.y;
				hLines_raw[j][line].pt3D.y = scanLines[line][j].pt3D.x;
			}
		}
		//ˮƽarc<72><63><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ
		std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_h_raw;
		int lineNum_h_raw = (int)hLines_raw.size();
		for (int line = 0; line < lineNum_h_raw; line++)
		{
			if (line == 416)
				int kkk = 1;
			std::vector<SVzNL3DPosition>& lineData = hLines_raw[line];
			//<2F>˲<EFBFBD><CBB2><EFBFBD><EFBFBD>˳<EFBFBD><CBB3>쳣<EFBFBD><ECB3A3>
			int ptNum = (int)lineData.size();
			sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, filterParam);

			std::vector<SSG_basicFeature1D> line_features;
			int dataSize = (int)lineData.size();
			sg_getLineCornerFeature_BQ(
				&hLines_raw[line][0],
				dataSize,
				line,
				groundCalibPara.planeHeight,
				cornerPara, //scaleͨ<65><CDA8>ȡbagH<67><48>1/4
				line_features);
			jumpFeatures_h_raw.push_back(line_features);
		}

		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD><EFBFBD>
		//<2F><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߷<EFBFBD><DFB7><EFBFBD><EFBFBD><EFBFBD>
		std::vector<SSG_featureTree> v_trees;
		for (int line = 0; line < lineNum; line++)
		{
			bool isLastLine = false;
			if (line == lineNum - 1)
				isLastLine = true;
			std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_v_raw[line];
			if (a_lineJumpFeature.size() > 0)
				int kkk = 1;
			if (line == 202)
				int kkk = 1;
			sg_lineFeaturesGrowing(
				line,
				isLastLine,
				a_lineJumpFeature,
				v_trees,
				growParam);
		}

		//ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		std::vector<SSG_featureTree> h_trees;
		for (int line = 0; line < lineNum_h_raw; line++)
		{
			if (line == 650)
				int kkk = 1;
			bool isLastLine = false;
			if (line == lineNum_h_raw - 1)
				isLastLine = true;
			std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_h_raw[line];
			sg_lineFeaturesGrowing(
				line,
				isLastLine,
				a_lineJumpFeature,
				h_trees,
				growParam);
		}

		std::vector<SWD_polarPt> polarPoints;
#if 0
		for (int line = 0; line < lineNum; line++)
		{
			std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_v_raw[line];
			for (int pi = 0, pi_max = (int)a_lineJumpFeature.size(); pi < pi_max; pi++)
			{
				int lineIdx = a_lineJumpFeature[pi].jumpPos2D.x;
				int ptIdx = a_lineJumpFeature[pi].jumpPos2D.y;
				if (scanLines[lineIdx][ptIdx].nPointIdx >= 0)
				{
					SWD_polarPt a_polarPt;
					a_polarPt.lineIdx = lineIdx;
					a_polarPt.ptIdx = ptIdx;
					a_polarPt.R = 0;
					a_polarPt.angle = 0;
					a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x;
					a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y;
					a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z;
					polarPoints.push_back(a_polarPt);
					scanLines[lineIdx][ptIdx].nPointIdx = -1;
				}
			}
		}
		for (int line = 0; line < lineNum_h_raw; line++)
		{
			std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_h_raw[line];
			for (int pi = 0, pi_max = (int)a_lineJumpFeature.size(); pi < pi_max; pi++)
			{
				int lineIdx = a_lineJumpFeature[pi].jumpPos2D.y;
				int ptIdx = a_lineJumpFeature[pi].jumpPos2D.x;
				if (scanLines[lineIdx][ptIdx].nPointIdx >= 0)
				{
					SWD_polarPt a_polarPt;
					a_polarPt.lineIdx = lineIdx;
					a_polarPt.ptIdx = ptIdx;
					a_polarPt.R = 0;
					a_polarPt.angle = 0;
					a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x;
					a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y;
					a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z;
					polarPoints.push_back(a_polarPt);
					scanLines[lineIdx][ptIdx].nPointIdx = -1;
				}
			}
		}
#else
		for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++)
		{
			SSG_featureTree* a_vTree = &v_trees[i];
			//<2F><>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD><CFB1>ǣ<EFBFBD>ͬʱ<CDAC><CAB1>Mask<73>ϱ<EFBFBD><CFB1><EFBFBD>
			for (int j = 0, j_max = (int)a_vTree->treeNodes.size(); j < j_max; j++)
			{
				int lineIdx = a_vTree->treeNodes[j].jumpPos2D.x;
				int ptIdx = a_vTree->treeNodes[j].jumpPos2D.y;
				if (scanLines[lineIdx][ptIdx].nPointIdx >= 0)
				{
					SWD_polarPt a_polarPt;
					a_polarPt.lineIdx = lineIdx;
					a_polarPt.ptIdx = ptIdx;
					a_polarPt.R = 0;
					a_polarPt.angle = 0;
					a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x;
					a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y;
					a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z;
					polarPoints.push_back(a_polarPt);
					scanLines[lineIdx][ptIdx].nPointIdx = -1;
				}
			}
		}
		for (int i = 0, i_max = (int)h_trees.size(); i < i_max; i++)
		{
			SSG_featureTree* a_hTree = &h_trees[i];
			//<2F><>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD><CFB1>ǣ<EFBFBD>ͬʱ<CDAC><CAB1>Mask<73>ϱ<EFBFBD><CFB1><EFBFBD>
			for (int j = 0, j_max = (int)a_hTree->treeNodes.size(); j < j_max; j++)
			{
				int lineIdx = a_hTree->treeNodes[j].jumpPos2D.y;
				int ptIdx = a_hTree->treeNodes[j].jumpPos2D.x;
				if (scanLines[lineIdx][ptIdx].nPointIdx >= 0)
				{
					SWD_polarPt a_polarPt;
					a_polarPt.lineIdx = lineIdx;
					a_polarPt.ptIdx = ptIdx;
					a_polarPt.cptIndex = -1;
					a_polarPt.R = 0;
					a_polarPt.angle = 0;
					a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x;
					a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y;
					a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z;
					polarPoints.push_back(a_polarPt);
					scanLines[lineIdx][ptIdx].nPointIdx = -1;
				}
			}
		}

#endif
		//<2F><><EFBFBD>㼸<EFBFBD><E3BCB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		int contourPtSize = (int)polarPoints.size();
		if (contourPtSize == 0)
		{
			*errCode = SX_ERR_ZERO_CONTOUR_PT;
			return workpieceCorners;
		}
		double center_x = 0;
		double center_y = 0;
		for (int pi = 0; pi < contourPtSize; pi++)
		{
			center_x += polarPoints[pi].x;
			center_y += polarPoints[pi].y;
		}
		center_x = center_x / (double)contourPtSize;
		center_y = center_y / (double)contourPtSize;
		//<2F><><EFBFBD>㼫<EFBFBD><E3BCAB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>R<EFBFBD><52>Theta
		for (int pi = 0; pi < contourPtSize; pi++)
		{
			double angle = atan2(polarPoints[pi].y - center_y, polarPoints[pi].x - center_x);
			angle = (angle / PI) * 180 +180.0;
			double R = sqrt(pow(polarPoints[pi].y - center_y, 2) + pow(polarPoints[pi].x - center_x, 2));
			polarPoints[pi].R = R;
			polarPoints[pi].angle = angle;
		}
		//<2F><><EFBFBD>Ƕȴ<C7B6>С<EFBFBD><D0A1><EFBFBD><EFBFBD>
		std::sort(polarPoints.begin(), polarPoints.end(), compareByAngle);
		for (int pi = 0; pi < contourPtSize; pi++)
			polarPoints[pi].cptIndex = pi;  // index

		//<2F><>ȡR<C8A1><52>ֵ<EFBFBD><D6B5>
		double minR = -1, maxR = -1; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>R<EFBFBD><52><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD>з<EFBFBD>֧<EFBFBD><D6A7>minR<6E><52>maxR<78><52><EFBFBD><EFBFBD>Сʱ<D0A1><CAB1>ΪԲ<CEAA>λ<EFBFBD>8<EFBFBD><38><EFBFBD>Σ<EFBFBD>û<EFBFBD>з<EFBFBD>֧
		int minRPos = -1;
		std::vector<SWD_polarPt> polarRPeakPts;
		int winSize = contourPtSize / 36;  //+-10<31>ȷ<EFBFBD>Χ
		if (winSize < 5)
			winSize = 5;
		for (int pi = 0; pi < contourPtSize; pi++)
		{
			double currR = polarPoints[pi].R;
			if (minR < 0)
			{
				minR = currR;
				maxR = currR;
				minRPos = pi;
			}
			else
			{
				minRPos = minR > currR ? pi : minRPos;
				minR = minR > currR ? currR : minR;
				maxR = maxR < currR ? currR : maxR;
			}
			bool isPeak = true;
			for (int k = -winSize; k <= winSize; k++)
			{
				int idx = (pi + k + contourPtSize) % contourPtSize; //Ͳ<>νṹ
				if (polarPoints[idx].R > currR)
				{
					isPeak = false;
					break;
				}
			}
			if (true == isPeak)
				polarRPeakPts.push_back(polarPoints[pi]);
		}

		double ratio_MaxMin = maxR / minR;
		bool hasBranch = ratio_MaxMin < 1.25 ? false : true;

		std::vector<SWD_polarPt> validPolarRPeakPts;
		std::vector<SWD_polarPeakInfo> polarPeakInfo;
		int pkId = 0;
		//<2F><><EFBFBD><EFBFBD>Բ<EFBFBD><D4B2><EFBFBD>εļ<CEB5>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD>ƣ<EFBFBD>Բ<EFBFBD><D4B2><EFBFBD><EFBFBD>Ҳ<EFBFBD><D2B2><EFBFBD>γɼ<CEB3>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD>ݼ<EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>L=ֱ<>߶γ<DFB6><CEB3>ȹ<EFBFBD><C8B9>ɵ<EFBFBD><C9B5>Ž<EFBFBD><C5BD>ж<EFBFBD>
		double arcAngleChkLen = 100; //<2F><><EFBFBD><EFBFBD>Բ<EFBFBD><D4B2><EFBFBD>Žǵij߶<C4B3>
		for (int i = 0, i_max = (int)polarRPeakPts.size(); i < i_max; i++)
		{
			int ptidx = polarRPeakPts[i].cptIndex;
			double px, py, pz;
			px = polarRPeakPts[i].x;
			py = polarRPeakPts[i].y;

			int LL1 = -1;
			int halfLL1 = -1;
			for (int j = ptidx - 1; j > -contourPtSize; j--)
			{
				int idx = (j + contourPtSize) % contourPtSize; //Ͳ<>νṹ
				double cx = polarPoints[idx].x;
				double cy = polarPoints[idx].y;
				double len = sqrt(pow(px - cx, 2) + pow(py - cy, 2));
				if (len < arcAngleChkLen)
					halfLL1 = idx;
				if (len > (workpieceParam.lineLen))
				{
					LL1 = idx;
					break;
				}
			}

			int LL2 = -1;
			int halfLL2 = -1;
			for (int j = ptidx + 1; j < contourPtSize*2; j++)
			{
				int idx = j % contourPtSize; //Ͳ<>νṹ
				double cx = polarPoints[idx].x;
				double cy = polarPoints[idx].y;
				double len = sqrt(pow(px - cx, 2) + pow(py - cy, 2));
				if (len < arcAngleChkLen)
					halfLL2 = idx;
				if (len > (workpieceParam.lineLen))
				{
					LL2 = idx;
					break;
				}
			}

			if ((LL1 >= 0) && (LL2 >= 0))
			{
				double len1 = sqrt(pow(px - polarPoints[halfLL1].x, 2) + pow(py - polarPoints[halfLL1].y, 2));
				double len2 = sqrt(pow(px - polarPoints[halfLL2].x, 2) + pow(py - polarPoints[halfLL2].y, 2));
				double len3 = sqrt(pow(polarPoints[halfLL1].x - polarPoints[halfLL2].x, 2) +
									pow(polarPoints[halfLL1].y - polarPoints[halfLL2].y, 2));
				double cosTheta = (len1 * len1 + len2 * len2 - len3 * len3) / (2 * len1 * len2);
				double theta = acos(cosTheta) * 180.0 / PI;
				if (theta < 150)
				{
					SWD_polarPeakInfo a_pkInfo;
					a_pkInfo.cptIndex = ptidx;
					a_pkInfo.L1_ptIndex = LL1;
					a_pkInfo.L2_ptIndex = LL2;
					a_pkInfo.cornerAngle = theta;
					a_pkInfo.cornerDir = 0;
					polarRPeakPts[i].cptIndex = ptidx;
					polarRPeakPts[i].pkId = pkId;
					pkId++;
					validPolarRPeakPts.push_back(polarRPeakPts[i]);
					polarPeakInfo.push_back(a_pkInfo);
				}
			}
		}

		int workpieceType = -1;
		bool partialScan = false; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ұ<EFBFBD><D2B0><EFBFBD>ܸ<EFBFBD><DCB8>ǹ<EFBFBD><C7B9><EFBFBD>ȫ<EFBFBD><C8AB>ʱ<EFBFBD><CAB1>partialScanΪtrue
		std::vector< SWD_branchInfo> branchInfo;
		if (true == hasBranch)
		{
			//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֧<EFBFBD><D6A7>90<39>ȷ<EFBFBD>Χ<EFBFBD><CEA7>Ϊͬһ<CDAC><D2BB>֧<EFBFBD><D6A7><EFBFBD><EFBFBD><EFBFBD>ݷ<EFBFBD>֧<EFBFBD><D6A7><EFBFBD><EFBFBD>ȷ<EFBFBD><C8B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
			int pkSize = (int)validPolarRPeakPts.size();
			for (int m = 0; m < pkSize; m++)
			{
				if (validPolarRPeakPts[m].cptIndex < 0)
					continue;

				//if (polarPeakInfo[m].cornerDir == 2) //<2F><>ʱ<EFBFBD>롣
				{
					int nxtIdx = (m + 1)%pkSize;

					bool isSameBranch = checkSameBranch(validPolarRPeakPts[m], validPolarRPeakPts[nxtIdx],
						polarPoints, center_x, center_y);
					double angleDiff = computeAngleDiff(validPolarRPeakPts[nxtIdx].angle, validPolarRPeakPts[m].angle);
					if (angleDiff < 0)
						angleDiff += 360;
					if ( (angleDiff < 90) && (true == isSameBranch))  //Ϊͬһbranch<63><68><EFBFBD>ϲ<EFBFBD><CFB2><EFBFBD>ֻ<EFBFBD><D6BB><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
					{
						validPolarRPeakPts[nxtIdx].pkId = validPolarRPeakPts[m].pkId; //pair
						validPolarRPeakPts[nxtIdx].cptIndex = -1;
					}
				}
			}
			
			std::vector<std::vector<SWD_polarPt>> branchPeaks; //ÿ<><C3BF>branch<63><68><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>
			branchPeaks.resize(pkId);
			std::vector<std::vector<SWD_polarPeakInfo>> branchPeakInfo;
			branchPeakInfo.resize(pkId);
			for (int m = 0; m < pkSize; m++)
			{
				if (validPolarRPeakPts[m].cptIndex < 0)
					validPolarRPeakPts[m].cptIndex = polarPeakInfo[m].cptIndex; //<2F>ָ<EFBFBD>
				int pkId = validPolarRPeakPts[m].pkId;
				branchPeaks[pkId].push_back(validPolarRPeakPts[m]);
				branchPeakInfo[pkId].push_back(polarPeakInfo[m]);
			}
			for (int m = pkSize - 1; m >= 0; m--)
			{
				if (branchPeaks[m].size() == 0)
				{
					branchPeaks.erase(branchPeaks.begin() + m);
					branchPeakInfo.erase(branchPeakInfo.begin() + m);
				}
			}
			int branchNum = (int)branchPeaks.size();
			if (branchNum == 2)
			{
				workpieceType = 3;  //<2F>ڵ<EFBFBD>3

				SVzNL3DPoint pt1 = { polarPoints[minRPos].x, polarPoints[minRPos].y, 0 };
				SVzNL3DPoint pt2 = { center_x, center_y, 0 };
				double aux_a, aux_b, aux_c;
				compute2ptLine(pt1, pt2, &aux_a, &aux_b, &aux_c);
				double counterAngle = polarPoints[minRPos].angle + 180;
				if (counterAngle > 360)
					counterAngle = counterAngle - 360;
				SVzNL3DPoint a_cross = computeEdgeCross(counterAngle, polarPoints, aux_a, aux_b, aux_c);
				double refine_cx = (pt1.x + a_cross.x) / 2;
				double refine_cy = (pt1.y + a_cross.y) / 2;
				//ȷ<><C8B7>cornerDir
				for (int m = 0; m < branchNum; m++)
				{
					for (int n = 0; n < branchPeaks[m].size(); n++)
					{
						int cptIdx = branchPeakInfo[m][n].cptIndex;
						int LL1 = branchPeakInfo[m][n].L1_ptIndex;
						int LL2 = branchPeakInfo[m][n].L2_ptIndex;
						double angle = atan2(polarPoints[cptIdx].y - refine_cy, polarPoints[cptIdx].x - refine_cx);						
						angle = (angle / PI) * 180 + 180.0;
						double LL1_angle  = atan2(polarPoints[LL1].y - refine_cy, polarPoints[LL1].x - refine_cx);
						LL1_angle = (LL1_angle / PI) * 180 + 180.0;
						double LL2_angle = atan2(polarPoints[LL2].y - refine_cy, polarPoints[LL2].x - refine_cx);
						LL2_angle = (LL2_angle / PI) * 180 + 180.0;

						double L1_angle = computeAngleDiff(angle, LL1_angle);
						double L2_angle = computeAngleDiff(LL2_angle, angle); 
						if (L1_angle < L2_angle)
							branchPeakInfo[m][n].cornerDir = 2; //<2F><>ʱ<EFBFBD><CAB1>
						else
							branchPeakInfo[m][n].cornerDir = 1; //˳ʱ<CBB3><CAB1>
					}
				}
			}
			else if ((branchNum == 3) || (branchNum == 4))
			{
				if (branchNum == 3)
				{
					partialScan = true;
					workpieceType = 2;  //<2F>ڵ<EFBFBD>2
				}
				else
					workpieceType = 1;  //<2F>ڵ<EFBFBD>1
				//ȷ<><C8B7>cornerDir

				for (int m = 0; m < branchNum; m++)
				{
					for(int n = 0; n < branchPeaks[m].size(); n++)
					{
						int cptIdx = branchPeakInfo[m][n].cptIndex;
						int LL1 = branchPeakInfo[m][n].L1_ptIndex;
						int LL2 = branchPeakInfo[m][n].L2_ptIndex;
						double L1_angle = polarPoints[cptIdx].angle - polarPoints[LL1].angle;
						if (L1_angle < 0)
							L1_angle += 360;
						double L2_angle = polarPoints[LL2].angle - polarPoints[cptIdx].angle;
						if (L2_angle < 0)
							L2_angle += 360;
						if (L1_angle < L2_angle)
							branchPeakInfo[m][n].cornerDir = 2; //<2F><>ʱ<EFBFBD><CAB1>
						else
							branchPeakInfo[m][n].cornerDir = 1; //˳ʱ<CBB3><CAB1>
					}
				}
			}
			else
				workpieceType = 0;

			//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>branch<63><68><EFBFBD><EFBFBD>Ϣ
			for (int branchIdx = 0; branchIdx < branchNum; branchIdx++)
			{
				std::vector<SWD_polarPt>& a_branch = branchPeaks[branchIdx];
				std::vector<SWD_polarPeakInfo>& a_branchInfo = branchPeakInfo[branchIdx];
				//ȡ<>̶<EFBFBD><CCB6><EFBFBD><EFBFBD>ȴ<EFBFBD>ֱ<EFBFBD><D6B1>
				SWD_polarPt branchCorner;
				SWD_polarPeakInfo branchCornerInfo;
				if (a_branch.size() == 2)  //ȡ<>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD>ӽ<EFBFBD>90<39>ȵ<EFBFBD>
				{
					double diff1 = abs(a_branchInfo[0].cornerAngle - 90);
					double diff2 = abs(a_branchInfo[1].cornerAngle - 90);
					branchCorner = diff1 < diff2 ? a_branch[0]: a_branch[1];
					branchCornerInfo = diff1 < diff2 ? a_branchInfo[0] : a_branchInfo[1];
				}
				else
				{
					branchCorner = a_branch[0];
					branchCornerInfo = a_branchInfo[0];
				}
				
				SWD_branchInfo resultBranchInfo;
				std::vector<SVzNL3DPoint> branchLinePts; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾ<EFBFBD><CABE>debug
				std::vector< SVzNL3DPoint> branchEdgePt1;
				std::vector< SVzNL3DPoint> branchEdgePt2;
				int opOK = _getBranchInfo(
					validStartLine,   //<2F><>ʼɨ<CABC><C9A8><EFBFBD>߽<EFBFBD>
					validEndLine, //<2F><><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD>߽<EFBFBD>
					partialScan, //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ұ<EFBFBD><D2B0><EFBFBD>ܸ<EFBFBD><DCB8>ǹ<EFBFBD><C7B9><EFBFBD>ȫ<EFBFBD><C8AB>ʱ<EFBFBD><CAB1>partialScanΪtrue,
					polarPoints,
					branchCorner,
					branchCornerInfo,
					&resultBranchInfo,
					branchLinePts,
					branchEdgePt1,
					branchEdgePt2);
				if(opOK < 0)
				{
					*errCode = SX_ERR_ZERO_CONTOUR_PT;
					return workpieceCorners;
				}
				branchInfo.push_back(resultBranchInfo);
#if _OUTPUT_DEBUG_DATA
				SSX_debugInfo a_branchDebug;
				a_branchDebug.rgnIdx = (int)branchInfo.size();
				a_branchDebug.edge_size = (int)branchEdgePt1.size();
				a_branchDebug.edgeLink1_size = (int)branchLinePts.size();
				a_branchDebug.edgeLink2_size = (int)branchEdgePt2.size();
				a_branchDebug.edge = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * a_branchDebug.edge_size);
				a_branchDebug.edgeLink_1 = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * a_branchDebug.edgeLink1_size);
				a_branchDebug.edgeLink_2 = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * a_branchDebug.edgeLink2_size);

				for(int m = 0; m < a_branchDebug.edge_size; m ++)
					a_branchDebug.edge[m] = branchEdgePt1[m];
				for (int m = 0; m < a_branchDebug.edgeLink1_size; m++)
					a_branchDebug.edgeLink_1[m] = branchLinePts[m];
				for (int m = 0; m < a_branchDebug.edgeLink2_size; m++)
					a_branchDebug.edgeLink_2[m] = branchEdgePt2[m];
				debug_contours.push_back(a_branchDebug);
#endif			
			}
			workpieceCorners.workpieceType = workpieceType;
			if (workpieceType == 1) //4<><34>branch
			{
				for (int m = 0; m < 3; m++)
					workpieceCorners.corner_1[m] = branchInfo[0].corner[m];
				for (int m = 0; m < 3; m++)
					workpieceCorners.corner_2[m] = branchInfo[1].corner[m];
				for (int m = 0; m < 3; m++)
					workpieceCorners.corner_3[m] = branchInfo[2].corner[m];
				for (int m = 0; m < 3; m++)
					workpieceCorners.corner_4[m] = branchInfo[3].corner[m];
				//<2F><><EFBFBD><EFBFBD>ʣ<EFBFBD><CAA3><EFBFBD><EFBFBD>Ϣ
				double line1_a, line1_b, line1_c;
				compute2ptLine(
					workpieceCorners.corner_1[1], workpieceCorners.corner_3[1],
					&line1_a, &line1_b, &line1_c);
				double line2_a, line2_b, line2_c;
				compute2ptLine(
					workpieceCorners.corner_2[1], workpieceCorners.corner_4[1],
					&line2_a, &line2_b, &line2_c);
				workpieceCorners.center = computeLineCrossPt_abs(
					line1_a, line1_b, line1_c,
					line2_a, line2_b, line2_c);
				workpieceCorners.center.z = (workpieceCorners.corner_1[1].z + workpieceCorners.corner_2[1].z +
					workpieceCorners.corner_3[1].z + workpieceCorners.corner_4[1].z) / 4;
				//line1<65><31>ת45<34>ȷ<EFBFBD><C8B7><EFBFBD>
				double r45_line1_a, r45_line1_b, r45_line1_c;
				rotateLine45Deg(
					line1_a, line1_b, line1_c,
					workpieceCorners.center.x, workpieceCorners.center.y,
					&r45_line1_a, &r45_line1_b, &r45_line1_c);

				double r45_line2_a, r45_line2_b, r45_line2_c;
				rotateLine45Deg(
					line2_a, line2_b, line2_c,
					workpieceCorners.center.x, workpieceCorners.center.y,
					&r45_line2_a, &r45_line2_b, &r45_line2_c);

				double angle1 = getLineAngle(r45_line1_a, r45_line1_b, r45_line1_c);
				double angle2 = getLineAngle(r45_line2_a, r45_line2_b, r45_line2_c);
				if (angle1 < angle2)
				{
					SVzNL3DPoint a_cross = computeEdgeCross(angle1, polarPoints, r45_line1_a, r45_line1_b, r45_line1_c);
					workpieceCorners.len45_B1 = compute2DLen(workpieceCorners.center, a_cross);
					a_cross = computeEdgeCross((angle1+180), polarPoints, r45_line1_a, r45_line1_b, r45_line1_c);
					workpieceCorners.len225_A2 = compute2DLen(workpieceCorners.center, a_cross);
					a_cross = computeEdgeCross(angle2, polarPoints, r45_line2_a, r45_line2_b, r45_line2_c);
					workpieceCorners.len135_A1 = compute2DLen(workpieceCorners.center, a_cross);
					a_cross = computeEdgeCross((angle2+180), polarPoints, r45_line2_a, r45_line2_b, r45_line2_c);
					workpieceCorners.len315_B2 = compute2DLen(workpieceCorners.center, a_cross);
				}
				else
				{
					SVzNL3DPoint a_cross = computeEdgeCross(angle2, polarPoints, r45_line2_a, r45_line2_b, r45_line2_c);
					workpieceCorners.len45_B1 = compute2DLen(workpieceCorners.center, a_cross);
					a_cross = computeEdgeCross((angle2 + 180), polarPoints, r45_line2_a, r45_line2_b, r45_line2_c);
					workpieceCorners.len225_A2 = compute2DLen(workpieceCorners.center, a_cross);
					a_cross = computeEdgeCross(angle1, polarPoints, r45_line1_a, r45_line1_b, r45_line1_c);
					workpieceCorners.len135_A1 = compute2DLen(workpieceCorners.center, a_cross);
					a_cross = computeEdgeCross((angle1 + 180), polarPoints, r45_line1_a, r45_line1_b, r45_line1_c);
					workpieceCorners.len315_B2 = compute2DLen(workpieceCorners.center, a_cross);
				}
			}
			else if (workpieceType == 2) //3 <20><>branch
			{
				int startIdx = 0;
				for (int i = 0; i < 3; i++)
				{
					int nxtIdx = (i + 1) % 3;
					double diff = computeAngleDiff(branchInfo[i].angle, branchInfo[nxtIdx].angle);
					if (diff > 135)
					{
						startIdx = nxtIdx;
						break;
					}
				}

				double crossDir;
				for (int m = 0; m < 3; m++)
					workpieceCorners.corner_1[m] = branchInfo[startIdx].corner[m];
				startIdx = (startIdx + 1) % 3;
				crossDir = branchInfo[startIdx].angle;
				for (int m = 0; m < 3; m++)
					workpieceCorners.corner_2[m] = branchInfo[startIdx].corner[m];
				startIdx = (startIdx + 1) % 3;
				for (int m = 0; m < 3; m++)
					workpieceCorners.corner_3[m] = branchInfo[startIdx].corner[m];

				//<2F><><EFBFBD><EFBFBD>ʣ<EFBFBD><CAA3><EFBFBD><EFBFBD>Ϣ
				workpieceCorners.center.x = (workpieceCorners.corner_1[1].x + workpieceCorners.corner_3[1].x) / 2;
				workpieceCorners.center.y = (workpieceCorners.corner_1[1].y + workpieceCorners.corner_3[1].y) / 2;
				workpieceCorners.center.z = (workpieceCorners.corner_1[1].z + workpieceCorners.corner_3[1].z) / 2;

				double line_a, line_b, line_c;
				compute2ptLine(
					workpieceCorners.corner_2[1], workpieceCorners.center,
					&line_a, &line_b, &line_c);

				workpieceCorners.len45_B1 = compute2DLen(workpieceCorners.corner_1[1], workpieceCorners.center);
				workpieceCorners.len135_A1 = compute2DLen(workpieceCorners.corner_2[1], workpieceCorners.center);
				workpieceCorners.len225_A2 = compute2DLen(workpieceCorners.corner_3[1], workpieceCorners.center);
				SVzNL3DPoint a_cross = computeEdgeCross((crossDir+180), polarPoints, line_a, line_b, line_c);
				workpieceCorners.len315_B2 = compute2DLen(a_cross, workpieceCorners.center);
			}
			else
			{
				for (int m = 0; m < 3; m++)
					workpieceCorners.corner_1[m] = branchInfo[0].corner[m];
				for (int m = 0; m < 3; m++)
					workpieceCorners.corner_2[m] = branchInfo[1].corner[m];
				SVzNL3DPoint center = computeLineCrossPt_abs(
					branchInfo[0].line_a, branchInfo[0].line_b, branchInfo[0].line_c,
					branchInfo[1].line_a, branchInfo[1].line_b, branchInfo[1].line_c);
				workpieceCorners.center.x = center.x;
				workpieceCorners.center.y = center.y;
				workpieceCorners.center.z = (workpieceCorners.corner_1[1].z + workpieceCorners.corner_2[1].z) / 2;
				workpieceCorners.len45_B1 = compute2DLen(workpieceCorners.corner_1[1], workpieceCorners.center);
				workpieceCorners.len135_A1 = compute2DLen(workpieceCorners.corner_2[1], workpieceCorners.center);
				SVzNL3DPoint a_cross = computeEdgeCross(
					(branchInfo[0].angle + 180), polarPoints, 
					branchInfo[0].line_a, branchInfo[0].line_b, branchInfo[0].line_c);
				workpieceCorners.len225_A2 = compute2DLen(a_cross, workpieceCorners.center);
				a_cross = computeEdgeCross(
					(branchInfo[1].angle + 180), polarPoints,
					branchInfo[1].line_a, branchInfo[1].line_b, branchInfo[1].line_c);
				workpieceCorners.len315_B2 = compute2DLen(a_cross, workpieceCorners.center);
			}
		}
		else
		{
			workpieceType = 4;  //<2F>ڵ<EFBFBD>4
			partialScan = true;
			//<2F><><EFBFBD><EFBFBD>û<EFBFBD><C3BB><EFBFBD>ڱ߽<DAB1><DFBD><EFBFBD>Corner
			int polarPkNum = (int)validPolarRPeakPts.size();
			for (int pki = 0; pki < polarPkNum; pki++)
			{
				//<2F><><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD>߽<EFBFBD>
				int validStartLineWin = validStartLine + 3;
				int validEndLineWin = validEndLine - 3;
				if (validEndLineWin < 0)
					validEndLineWin = 0;
				bool isSide = false;
				int cptIdx = validPolarRPeakPts[pki].cptIndex;
				SVzNL3DPoint cpt = { validPolarRPeakPts[pki].x, validPolarRPeakPts[pki].y, validPolarRPeakPts[pki].z };
				std::vector<SVzNL3DPoint> edgePt_1;
				for (int i = 0; i < contourPtSize; i++)
				{
					int idx = (i + cptIdx)%contourPtSize;
					SVzNL3DPoint a_pt = { polarPoints[idx].x, polarPoints[idx].y, polarPoints[idx].z };
					double len = sqrt(pow(cpt.x - a_pt.x, 2) + pow(cpt.y - a_pt.y, 2));
					if (len > workpieceParam.lineLen)
						break;
					else
					{
						if ( (partialScan == true) &&
							((polarPoints[idx].lineIdx <= validStartLineWin) ||
							(polarPoints[idx].lineIdx >= validEndLineWin)))
							isSide = true;
						else
							edgePt_1.push_back(a_pt);
					}
				}

				std::vector<SVzNL3DPoint> edgePt_2;
				for (int i = 0; i < contourPtSize; i++)
				{
					int idx = cptIdx - i;
					if (idx < 0)
						idx += contourPtSize;

					SVzNL3DPoint a_pt = { polarPoints[idx].x, polarPoints[idx].y, polarPoints[idx].z };
					double len = sqrt(pow(cpt.x - a_pt.x, 2) + pow(cpt.y - a_pt.y, 2));
					if (len > workpieceParam.lineLen)
						break;
					else
					{
						if ((partialScan == true) &&
							((polarPoints[idx].lineIdx <= validStartLineWin) ||
							(polarPoints[idx].lineIdx >= validEndLineWin)))
							isSide = true;
						else
							edgePt_2.push_back(a_pt);
					}
				}
				if( (edgePt_1.size() < 10) || (edgePt_2.size() < 10))
				{
					*errCode = SX_ERR_ZERO_CONTOUR_PT;
					return workpieceCorners;
				}

				if (true == isSide)
				{
					//<2F><><EFBFBD>ϼ<EFBFBD><CFBC>㽻<EFBFBD>㣬<EFBFBD><E3A3AC><EFBFBD><EFBFBD>
					double edge1_a = 0, edge1_b = 0, edge1_c = 0;
					lineFitting_abc(edgePt_1, &edge1_a, &edge1_b, &edge1_c);
					double edge2_a = 0, edge2_b = 0, edge2_c = 0;
					lineFitting_abc(edgePt_2, &edge2_a, &edge2_b, &edge2_c);
					//<2F><><EFBFBD>㽻<EFBFBD><E3BDBB>
					SVzNL3DPoint crossPt = computeLineCrossPt_abs(
						edge1_a, edge1_b, edge1_c, edge2_a, edge2_b, edge2_c);
					validPolarRPeakPts[pki].x = crossPt.x;
					validPolarRPeakPts[pki].y = crossPt.y;
				}
			}
			if (polarPkNum != 8)
			{
				*errCode = SX_ERR_INVLID_RPEAK_NUM;
				return workpieceCorners;
			}
			//ȡ<><C8A1><EFBFBD>ഹֱ<E0B4B9><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
			//<2F><>1<EFBFBD><31>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>corner<65><72><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
			std::vector<double> sideLens;
			for (int i = 0; i < polarPkNum; i++)
			{
				int nxtIdx = (i + 1) % polarPkNum;
				double len = sqrt(pow(validPolarRPeakPts[i].x - validPolarRPeakPts[nxtIdx].x, 2) +
					pow(validPolarRPeakPts[i].y - validPolarRPeakPts[nxtIdx].y, 2));
				sideLens.push_back(len);
			}
			double minLen1 = -1;
			int minLen1_idx = -1;
			for (int i = 0; i < polarPkNum; i++)
			{
				if (minLen1 < 0)
				{
					minLen1 = sideLens[i];
					minLen1_idx = i;
				}
				else
				{
					if(minLen1 > sideLens[i])
					{
						minLen1 = sideLens[i];
						minLen1_idx = i;
					}
				}
			}
			double minLen2 = -1;
			int minLen2_idx = -1;
			for (int i = 0; i < polarPkNum; i++)
			{
				if (i == minLen1_idx)
					continue;

				if (minLen2 < 0)
				{
					minLen2 = sideLens[i];
					minLen2_idx = i;
				}
				else
				{
					if (minLen2 > sideLens[i])
					{
						minLen2 = sideLens[i];
						minLen2_idx = i;
					}
				}
			}
			if( (minLen1_idx < 0) || (minLen2_idx < 0))
			{
				*errCode = SX_ERR_INVLID_RPEAK_NUM;
				return workpieceCorners;
			}

			workpieceCorners.workpieceType = workpieceType;
			//<2F><><EFBFBD>㹤<EFBFBD><E3B9A4><EFBFBD><EFBFBD>Ϣ
			int tmpIdx = (minLen1_idx + 1) % polarPkNum;
			workpieceCorners.corner_1[0] = { validPolarRPeakPts[minLen1_idx].x, validPolarRPeakPts[minLen1_idx].y, validPolarRPeakPts[minLen1_idx].z };
			workpieceCorners.corner_1[2] = { validPolarRPeakPts[tmpIdx].x, validPolarRPeakPts[tmpIdx].y, validPolarRPeakPts[tmpIdx].z };
			workpieceCorners.corner_1[1].x = (workpieceCorners.corner_1[0].x + workpieceCorners.corner_1[2].x) / 2;
			workpieceCorners.corner_1[1].y = (workpieceCorners.corner_1[0].y + workpieceCorners.corner_1[2].y) / 2;
			workpieceCorners.corner_1[1].z = (workpieceCorners.corner_1[0].z + workpieceCorners.corner_1[2].z) / 2;
			//˳<><CBB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>cornerΪ<72><CEAA>һ<EFBFBD><D2BB>
			tmpIdx = (tmpIdx +1) % polarPkNum;
			int tmpIdx1 = (tmpIdx + 1) % polarPkNum;
			workpieceCorners.corner_2[0] = { validPolarRPeakPts[tmpIdx].x, validPolarRPeakPts[tmpIdx].y, validPolarRPeakPts[tmpIdx].z };
			workpieceCorners.corner_2[2] = { validPolarRPeakPts[tmpIdx1].x, validPolarRPeakPts[tmpIdx1].y, validPolarRPeakPts[tmpIdx1].z };
			workpieceCorners.corner_2[1].x = (workpieceCorners.corner_2[0].x + workpieceCorners.corner_2[2].x) / 2;
			workpieceCorners.corner_2[1].y = (workpieceCorners.corner_2[0].y + workpieceCorners.corner_2[2].y) / 2;
			workpieceCorners.corner_2[1].z = (workpieceCorners.corner_2[0].z + workpieceCorners.corner_2[2].z) / 2;
			//<2F>ڶ<EFBFBD><DAB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̵<EFBFBD><CCB5><EFBFBD><EFBFBD><EFBFBD>cornerΪ<72><CEAA>3<EFBFBD><33>corner
			tmpIdx = (minLen2_idx + 1) % polarPkNum;
			workpieceCorners.corner_3[0] = { validPolarRPeakPts[minLen2_idx].x, validPolarRPeakPts[minLen2_idx].y, validPolarRPeakPts[minLen2_idx].z };
			workpieceCorners.corner_3[2] = { validPolarRPeakPts[tmpIdx].x, validPolarRPeakPts[tmpIdx].y, validPolarRPeakPts[tmpIdx].z };
			workpieceCorners.corner_3[1].x = (workpieceCorners.corner_3[0].x + workpieceCorners.corner_3[2].x) / 2;
			workpieceCorners.corner_3[1].y = (workpieceCorners.corner_3[0].y + workpieceCorners.corner_3[2].y) / 2;
			workpieceCorners.corner_3[1].z = (workpieceCorners.corner_3[0].z + workpieceCorners.corner_3[2].z) / 2;
			//˳<><CBB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>cornerΪ<72><CEAA>һ<EFBFBD><D2BB>
			tmpIdx = (tmpIdx + 1) % polarPkNum;
			tmpIdx1 = (tmpIdx + 1) % polarPkNum;
			workpieceCorners.corner_4[0] = { validPolarRPeakPts[tmpIdx].x, validPolarRPeakPts[tmpIdx].y, validPolarRPeakPts[tmpIdx].z };
			workpieceCorners.corner_4[2] = { validPolarRPeakPts[tmpIdx1].x, validPolarRPeakPts[tmpIdx1].y, validPolarRPeakPts[tmpIdx1].z };
			workpieceCorners.corner_4[1].x = (workpieceCorners.corner_4[0].x + workpieceCorners.corner_4[2].x) / 2;
			workpieceCorners.corner_4[1].y = (workpieceCorners.corner_4[0].y + workpieceCorners.corner_4[2].y) / 2;
			workpieceCorners.corner_4[1].z = (workpieceCorners.corner_4[0].z + workpieceCorners.corner_4[2].z) / 2;
			//<2F><><EFBFBD><EFBFBD>ʣ<EFBFBD><CAA3><EFBFBD><EFBFBD>Ϣ
			double line1_a, line1_b, line1_c;
			compute2ptLine(
				workpieceCorners.corner_1[1], workpieceCorners.corner_3[1], 
				&line1_a, &line1_b, &line1_c);
			double line2_a, line2_b, line2_c;
			compute2ptLine(
				workpieceCorners.corner_2[1], workpieceCorners.corner_4[1], 
				&line2_a, &line2_b, &line2_c);
			workpieceCorners.center = computeLineCrossPt_abs(
				line1_a, line1_b, line1_c,
				line2_a, line2_b, line2_c);
			workpieceCorners.center.z = (workpieceCorners.corner_1[1].z + workpieceCorners.corner_2[1].z +
				workpieceCorners.corner_3[1].z + workpieceCorners.corner_4[1].z) / 4;
			workpieceCorners.len135_A1 = compute2DLen(workpieceCorners.corner_1[1], workpieceCorners.center);
			workpieceCorners.len225_A2 = compute2DLen(workpieceCorners.corner_2[1], workpieceCorners.center);
			workpieceCorners.len315_B2 = compute2DLen(workpieceCorners.corner_3[1], workpieceCorners.center);
			workpieceCorners.len45_B1 = compute2DLen(workpieceCorners.corner_4[1], workpieceCorners.center);
		}
	}

#if 1
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ͷ<EFBFBD><CDB6><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD>Ա<EFBFBD><D4B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>۱궨<DBB1><EAB6A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȷ
	for (int i = 0; i < lineNum; i++)
		sx_BQ_lineDataR(scanLines[i], groundCalibPara.invRMatrix, -1);
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ͷ<EFBFBD><CDB6><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ
	SVzNL3DPoint rawObj;
	for (int i = 0; i < 3; i++)
	{
		rawObj = _translatePoint(workpieceCorners.corner_1[i], groundCalibPara.invRMatrix);
		workpieceCorners.corner_1[i] = rawObj;
		rawObj = _translatePoint(workpieceCorners.corner_2[i], groundCalibPara.invRMatrix);
		workpieceCorners.corner_2[i] = rawObj;
		rawObj = _translatePoint(workpieceCorners.corner_3[i], groundCalibPara.invRMatrix);
		workpieceCorners.corner_3[i] = rawObj;
		rawObj = _translatePoint(workpieceCorners.corner_4[i], groundCalibPara.invRMatrix);
		workpieceCorners.corner_4[i] = rawObj;
	}
	rawObj = _translatePoint(workpieceCorners.center, groundCalibPara.invRMatrix);
	workpieceCorners.center = rawObj;
#endif

#if _OUTPUT_DEBUG_DATA
	for (int i = 0; i < (int)debug_contours.size(); i++)
	{
		SSX_debugInfo& a_branchDebug = debug_contours[i];
		for (int m = 0; m < a_branchDebug.edge_size; m++)
		{
			rawObj = _translatePoint(a_branchDebug.edge[m], groundCalibPara.invRMatrix);
			a_branchDebug.edge[m] = rawObj;
		}
		for (int m = 0; m < a_branchDebug.edgeLink1_size; m++)
		{
			rawObj = _translatePoint(a_branchDebug.edgeLink_1[m], groundCalibPara.invRMatrix);
			a_branchDebug.edgeLink_1[m] = rawObj;
		}
		for (int m = 0; m < a_branchDebug.edgeLink2_size; m++)
		{
			rawObj = _translatePoint(a_branchDebug.edgeLink_2[m], groundCalibPara.invRMatrix);
			a_branchDebug.edgeLink_2[m] = rawObj;
		}
	}
#endif
	return workpieceCorners;
}