#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
std::string	m_strVersion = "1.2.0";
const char* wd_BQWorkpieceCornerVersion(void)
{
	return m_strVersion.c_str();
}

//计算一个平面调平参数。
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
SSG_planeCalibPara sx_BQ_getBaseCalibPara(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
	return sg_getPlaneCalibPara2(scanLines);
}

//相机姿态调平，并去除地面
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;
}

//获取生长树的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)//虚假目标过滤后点会置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)//虚假目标过滤后点会置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
{
	int pkId;
	int lineIdx;
	int ptIdx;
	int cptIndex;  //圆周扫描上的点序
	//double cornerAngle;  //以点为中心的两侧弦的夹角
	double R;
	double angle;
	double x;
	double y;
	double z;
}SWD_polarPt;

typedef struct
{
	int cptIndex;
	int L1_ptIndex;
	int L2_ptIndex;
	double cornerAngle;
	int cornerDir;
}SWD_polarPeakInfo;

typedef struct
{
	double angle;
	SVzNL3DPoint corner[3];
	double line_a, line_b, line_c; //边的垂线方程 ax+by+c = 0
}SWD_branchInfo;

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

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 (lineData[i].pt3D.z > 1e-4)
			ptNum++;
	}
	return ptNum;
}

//计算分支信息
int _getBranchInfo(
	int validStartLine,   //开始扫描边界
	int validEndLine, //结束扫描边界
	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 cornerWin = 2;
	int LineDir, contourDir, startIdx, endingIdx;
	if (branchCornerInfo.cornerDir == 2)//逆时针
	{
		startIdx = branchCorner.cptIndex - cornerWin;  //此处将corner周围的点不计算在内
		if (startIdx < 0)
			startIdx += contourPtSize;
		LineDir = -1;
		contourDir = 1;
		endingIdx = branchCornerInfo.L1_ptIndex;
	}
	else  //顺时针
	{
		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;

	//当直角由于扫描边界被切成一个上边很小的梯形时，检查出的角点可能不是真正的角点，需要进行检查
	//迭代计算真正的角点：取corner两边5个点，若有边界，检查
	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 ((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);
		//计算真正的角点
		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;
			}
		}
		int cptIdx = startIdx + LineDir * maxHPos;
		if (cptIdx < 0)
			cptIdx += contourPtSize;
		else
			cptIdx = cptIdx % contourPtSize;

		branchCorner = polarPoints[cptIdx];	
		if (branchCornerInfo.cornerDir == 2)//逆时针
		{
			startIdx = branchCorner.cptIndex - cornerWin;  //此处将corner周围的点不计算在内
			if (startIdx < 0)
				startIdx += contourPtSize;
		}
		else  //顺时针
			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;
	}

	//拟合直线: ax+by+c = 0
	double _a = 0, _b = 0, _c = 0;
	lineFitting_abc(LinePts, &_a, &_b, &_c);
	SVzNL3DPoint endingPt0 = LinePts.back();
	//计算端点垂足
	SVzNL2DPointD footPt = sx_getFootPoint_abc(endingPt0.x, endingPt0.y, _a, _b, _c);
	//提取其它轮廓点
	for (int i = cornerWin; i < contourPtSize; i++)  //避开角点周围
	{
		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 (dist < 1.0)
			break;
		branchContourPts.push_back(a_pt);
	}
	//检查轮廓点中有没有边界点。如果有边界点，需要进行推理
	int contourPtNum = (int)branchContourPts.size();
	bool hasSidePt = false;
	for (int m = 0; m < contourPtNum; m++)
	{
		if ((branchContourPts[m].lineIdx == validStartLine) ||
			(branchContourPts[m].lineIdx == validEndLine))
		{
			branchContourPts[m].z = -1.0;  //label
			hasSidePt = true;
		}
	}
	//分开branch的另两段
	if (false == hasSidePt)
	{
		double maxDist = -1;
		int maxPos = -1;
		//找到拐点:计算所有点与端点的距离，以最大值作为拐点
		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;
				}
			}
		}
		//分隔两段
		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);
		}

		//拟合求交点
		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);

		//计算交点
		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_a = _b;
		a_branchInfo.line_a = _c;
		*resultBranchInfo = a_branchInfo;
	}
	else
	{
		//将边界去除，剩下的两段进行处理
		std::vector< SVzNL3DPoint> edgePt1;
		for (int i = 0; i < contourPtNum; i++)
		{
			SVzNL3DPoint a_pt = { branchContourPts[i].x, branchContourPts[i].y, branchContourPts[i].z };
			if ((branchContourPts[i].lineIdx == validStartLine)|| (branchContourPts[i].lineIdx == validEndLine))
			{
				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 ((branchContourPts[i].lineIdx == validStartLine) || (branchContourPts[i].lineIdx == validEndLine))
				break;
			edgePt2.insert(edgePt2.begin(), a_pt);
		}
		//计算第一段在直线上的垂足，取垂足的质心。过质心的垂线
		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;
		//计算第二段在直线上的垂足，取垂足的质心。过质心的垂线
		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;
		//计算交点
		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)  //搜索60度范围
		{
			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;
}

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

	//将开始和结束的空白扫描线去除，获得扫描边界
	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;

	//自适应各种旋转角度
	{
		//垂直跳变特征提取
		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;

			//滤波，滤除异常点
			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通常取bagH的1/4
				line_features);
			jumpFeatures_v_raw.push_back(line_features);
		}

		if (false == isGridData)//数据不是网格格式
		{
			*errCode = SG_ERR_NOT_GRID_FORMAT;
			return workpieceCorners;
		}

		//生成水平扫描
		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; //将原始数据的序列清0（会转义使用）
				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特征提取
		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];
			//滤波，滤除异常点
			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通常取bagH的1/4
				line_features);
			jumpFeatures_h_raw.push_back(line_features);
		}

		//特征生长，用于滤除噪点
		//垂直方向特征生长（激光线方向）
		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);
		}

		//水平方向特征生长（扫描运动方向）
		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];
			//在原始点云上标记，同时有Mask上标记
			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];
			//在原始点云上标记，同时有Mask上标记
			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
		//计算几何中心
		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;
		//计算极坐标的R和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;
		}
		//按角度大小排序
		std::sort(polarPoints.begin(), polarPoints.end(), compareByAngle);
		for (int pi = 0; pi < contourPtSize; pi++)
			polarPoints[pi].cptIndex = pi;  // index

		//提取R极值点
		double minR = -1, maxR = -1; //计算最小和最大的R，用以区分有没有分支。minR和maxR相差小时，为圆形或8角形，没有分支
		std::vector<SWD_polarPt> polarRPeakPts;
		int winSize = contourPtSize / 36;  //+-10度范围
		if (winSize < 5)
			winSize = 5;
		for (int pi = 0; pi < contourPtSize; pi++)
		{
			double currR = polarPoints[pi].R;
			if (minR < 0)
			{
				minR = currR;
				maxR = currR;
			}
			else
			{
				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;
		//过滤圆弧段的极值：由于重心偏移，圆弧段也会形成极值。根据极值两边L=直线段长度构成的张角判断
		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;
			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 > workpieceParam.lineLen)
				{
					LL1 = idx;
					break;
				}
			}

			int LL2 = -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 > workpieceParam.lineLen)
				{
					LL2 = idx;
					break;
				}
			}

			if ((LL1 >= 0) && (LL2 >= 0))
			{
				double len1 = sqrt(pow(px - polarPoints[LL1].x, 2) + pow(py - polarPoints[LL1].y, 2));
				double len2 = sqrt(pow(px - polarPoints[LL2].x, 2) + pow(py - polarPoints[LL2].y, 2));
				double len3 = sqrt(pow(polarPoints[LL1].x - polarPoints[LL2].x, 2) +
									pow(polarPoints[LL1].y - polarPoints[LL2].y, 2));
				double cosTheta = (len1 * len1 + len2 * len2 - len3 * len3) / (2 * len1 * len2);
				double theta = acos(cosTheta) * 180.0 / PI;
				if (theta < 150)
				{
					double L1_angle = polarPoints[ptidx].angle - polarPoints[LL1].angle;
					if (L1_angle < 0)
						L1_angle += 360;
					double L2_angle = polarPoints[LL2].angle - polarPoints[ptidx].angle;
					if (L2_angle < 0)
						L2_angle += 360;

					SWD_polarPeakInfo a_pkInfo;
					a_pkInfo.cptIndex = ptidx;
					a_pkInfo.L1_ptIndex = LL1;
					a_pkInfo.L2_ptIndex = LL2;
					a_pkInfo.cornerAngle = theta;
					if(L1_angle < L2_angle)
						a_pkInfo.cornerDir = 2; //逆时针
					else
						a_pkInfo.cornerDir = 1; //顺时针

					polarRPeakPts[i].cptIndex = ptidx;
					polarRPeakPts[i].pkId = pkId;
					pkId++;
					validPolarRPeakPts.push_back(polarRPeakPts[i]);
					polarPeakInfo.push_back(a_pkInfo);
				}
			}
		}

		int workpieceType = -1;
		std::vector< SWD_branchInfo> branchInfo;
		if (true == hasBranch)
		{
			//计算分支，90度范围内为同一分支。根据分支数量确定工件类型
			int pkSize = (int)validPolarRPeakPts.size();
			for (int m = 0; m < pkSize; m++)
			{
				if (validPolarRPeakPts[m].cptIndex < 0)
					continue;

				if (polarPeakInfo[m].cornerDir == 2) //逆时针。
				{
					int nxtIdx = (m + 1)%pkSize;
					double angleDiff = validPolarRPeakPts[nxtIdx].angle - validPolarRPeakPts[m].angle;
					if (angleDiff < 0)
						angleDiff += 360;
					if (angleDiff < 90)  //为同一branch，合并，只保留一个
					{
						validPolarRPeakPts[nxtIdx].pkId = validPolarRPeakPts[m].pkId; //pair
						validPolarRPeakPts[nxtIdx].cptIndex = -1;
					}
				}
			}
			
			std::vector<std::vector<SWD_polarPt>> branchPeaks; //每个branch最多两个极值点
			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; //恢复
				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;  //节点3
			else if (branchNum == 3)
				workpieceType = 2;  //节点2
			else if (branchNum == 4)
				workpieceType = 1;  //节点1
			else
				workpieceType = 0;

			//计算各个branch的信息
			for (int branchIdx = 0; branchIdx < branchNum; branchIdx++)
			{
				std::vector<SWD_polarPt>& a_branch = branchPeaks[branchIdx];
				std::vector<SWD_polarPeakInfo>& a_branchInfo = branchPeakInfo[branchIdx];
				//取固定长度垂直边
				SWD_polarPt branchCorner;
				SWD_polarPeakInfo branchCornerInfo;
				if (a_branch.size() == 2)  //取角度最接近90度的
				{
					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; //用于显示和debug
				std::vector< SVzNL3DPoint> branchEdgePt1;
				std::vector< SVzNL3DPoint> branchEdgePt2;
				int opOK = _getBranchInfo(
					validStartLine,   //开始扫描边界
					validEndLine, //结束扫描边界
					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);
#endif			
				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);
			}
			workpieceCorners.workpieceType = workpieceType;
			if (workpieceType == 1) //4个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];
				//计算剩余信息
				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旋转45度方向
				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 个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];

				//计算剩余信息
				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;  //节点4
			//检查没有在边界的Corner
			int polarPkNum = (int)validPolarRPeakPts.size();
			for (int pki = 0; pki < polarPkNum; pki++)
			{
				//检查是否在扫描边界
				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 ((polarPoints[idx].lineIdx == validStartLine) ||
							(polarPoints[idx].lineIdx == validEndLine))
							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 ((polarPoints[idx].lineIdx == validStartLine) ||
							(polarPoints[idx].lineIdx == validEndLine))
							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 (false == isSide)
				{
					//拟合计算交点，修正
					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);
					//计算交点
					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;
			}
			//取互相垂直的两对
			//（1）取相邻corner距离最小的两段
			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;
			//计算工件信息
			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;
			//顺序的两个corner为下一段
			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;
			//第二个最短的两个corner为第3个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;
			//顺序的两个corner为下一段
			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;
			//计算剩余信息
			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
	//将数据重新投射回原来的坐标系，以保持手眼标定结果正确
	for (int i = 0; i < lineNum; i++)
		sx_BQ_lineDataR(scanLines[i], groundCalibPara.invRMatrix, -1);
	//将检测结果重新投射回原来的坐标系
	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;
}