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>

std::string	m_strVersion = "1.1.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 lineIdx;
	int ptIdx;
	double R;
	double angle;
	double x;
	double y;
	double z;
}SWD_polarPt;

//逆时针旋转时 θ > 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;
}

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
	SSX_debugInfo* debug_conturs,
#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 linePtNum = (int)scanLines[0].size();
	bool isGridData = true;

	SSX_featureContour region[4];
	//自适应各种旋转角度
	{
		//垂直跳变特征提取
		std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_v_raw;
		for (int line = 0; line < lineNum; line++)
		{
			if (line == 202)
				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.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);
		//提取R极值点
		std::vector<int> rPeaks;
		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;
			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)
			{
				rPeaks.push_back(pi);
				polarRPeakPts.push_back(polarPoints[pi]);
			}
		}
		if (rPeaks.size() != 8)
		{
			*errCode = SX_ERR_INVLID_RPEAK_NUM;
			return workpieceCorners;
		}
		//SSX_featureContour region[4];
		//Left, top, right, bottom
		SSG_intPair peakPair[4];
		int pairIdx = 0;
		for (int i = 0; i < 8; i++)
		{
			if (polarRPeakPts[i].lineIdx < 0)
				continue;
			//和前面的目标检测
			int pre_idx = (i - 1 + 8) % 8;
			int nxt_idx = (i + 1 + 8) % 8;
			bool pre_isPair = false;
			if (rPeaks[pre_idx] >= 0)
			{
				int mid_idx;
				if (rPeaks[i] < rPeaks[pre_idx])
					mid_idx = ((contourPtSize + rPeaks[i] + rPeaks[pre_idx]) / 2) % contourPtSize;
				else
					mid_idx = (rPeaks[i] + rPeaks[pre_idx]) / 2;
				double mid_R = polarPoints[mid_idx].R;
				cv::Point2f midChord = cv::Point2f((polarRPeakPts[i].x + polarRPeakPts[pre_idx].x) / 2, (polarRPeakPts[i].y + polarRPeakPts[pre_idx].y) / 2);
				double meanR = sqrt(pow(midChord.x - center_x,2) + pow(midChord.y-center_y,2));
				double delta_R = abs(mid_R - meanR);
				if (delta_R < 10)
					pre_isPair = true;
			}
			bool nxt_isPair = false;
			if (rPeaks[nxt_idx] >= 0)
			{
				int mid_idx;
				if (rPeaks[nxt_idx] < rPeaks[i])
					mid_idx = ((contourPtSize + rPeaks[nxt_idx] + rPeaks[i]) / 2) % contourPtSize;
				else
					mid_idx = (rPeaks[i] + rPeaks[nxt_idx]) / 2;
				double mid_R = polarPoints[mid_idx].R;
				cv::Point2f midChord = cv::Point2f((polarRPeakPts[i].x + polarRPeakPts[nxt_idx].x) / 2, (polarRPeakPts[i].y + polarRPeakPts[nxt_idx].y) / 2);
				double meanR = sqrt(pow(midChord.x - center_x, 2) + pow(midChord.y - center_y, 2));
				double delta_R = abs(mid_R - meanR);
				if (delta_R < 10)
					nxt_isPair = true;
			}

			if ((true == pre_isPair) && (false == nxt_isPair))
			{
				peakPair[pairIdx].idx = pairIdx;
				peakPair[pairIdx].data_0 = pre_idx;
				peakPair[pairIdx].data_1 = i;
				pairIdx++;
				polarRPeakPts[pre_idx].lineIdx = -1;
				polarRPeakPts[i].lineIdx = -1;
			}
			else if ((false == pre_isPair) && (true == nxt_isPair))
			{
				peakPair[pairIdx].idx = pairIdx;
				peakPair[pairIdx].data_0 = i;
				peakPair[pairIdx].data_1 = nxt_idx;
				pairIdx++;
				polarRPeakPts[nxt_idx].lineIdx = -1;
				polarRPeakPts[i].lineIdx = -1;
			}
			else
			{
				*errCode = SX_ERR_INVLID_RPEAK_PAIR;
				return workpieceCorners;
			}
		}
		 //生成SSX_featureContour region[4]
		double centerAngle;
		int idx0 = peakPair[0].data_0;
		int idx1 = peakPair[0].data_1;
		if (polarRPeakPts[idx0].angle > polarRPeakPts[idx1].angle)
		{
			centerAngle = (360 + polarRPeakPts[idx0].angle + polarRPeakPts[idx1].angle) / 2;
			if (centerAngle >= 360)
				centerAngle = centerAngle - 360;
		}
		else
			centerAngle = (polarRPeakPts[idx0].angle + polarRPeakPts[idx1].angle) / 2;
		int LRTB[4];
		if ((centerAngle > 315) || (centerAngle <= 45)) //Left
		{
			LRTB[0] = 0;  //left
			LRTB[1] = 1;  //top
			LRTB[2] = 2;  //right
			LRTB[3] = 3;  //bottom
		}
		else if ((centerAngle > 45) && (centerAngle <= 135)) //Top
		{
			LRTB[0] = 1;  //top
			LRTB[1] = 2;  //right
			LRTB[2] = 3;  //bottom
			LRTB[3] = 0;  //left
		}
		else if ((centerAngle > 135) && (centerAngle <= 225)) //Right
		{
			LRTB[0] = 2;  //right
			LRTB[1] = 3;  //bottom
			LRTB[2] = 0;  //left
			LRTB[3] = 1;  //top		
		}
		else //Bottom
		{
			LRTB[0] = 3;  //bottom
			LRTB[1] = 0;  //left
			LRTB[2] = 1;  //top
			LRTB[3] = 2;  //right				
		}

		for (int i = 0; i < 4; i++)
		{
			int rgnIdx = LRTB[i];
			int idx_0 = peakPair[i].data_0;
			int idx_1 = peakPair[i].data_1;
			std::vector<SVzNL3DPoint> edge;
			std::vector<SVzNL3DPoint> edge_link1;
			std::vector<SVzNL3DPoint> edge_link2;
			int startIdx = rPeaks[idx_0];
			int endIdx = rPeaks[idx_1];
			if (startIdx < endIdx)
			{
				for (int m = startIdx + 1; m < endIdx; m++)
				{
					SVzNL3DPoint a_pt = { polarPoints[m].x, polarPoints[m].y, polarPoints[m].z };
					edge.push_back(a_pt);
				}
			}
			else  //分两段
			{
				for (int m = startIdx + 1; m < contourPtSize; m++)
				{
					SVzNL3DPoint a_pt = { polarPoints[m].x, polarPoints[m].y, polarPoints[m].z };
					edge.push_back(a_pt);
				}
				for (int m = 0; m < endIdx; m++)
				{
					SVzNL3DPoint a_pt = { polarPoints[m].x, polarPoints[m].y, polarPoints[m].z };
					edge.push_back(a_pt);
				}
			}
			//edge_link1
			for (int m = 1; m < contourPtSize; m++)
			{
				int idx = (startIdx - m + contourPtSize) % contourPtSize;
				SVzNL3DPoint a_pt = { polarPoints[idx].x, polarPoints[idx].y, polarPoints[idx].z };
				double dist = sqrt(pow(a_pt.x - polarPoints[startIdx].x, 2) + pow(a_pt.y - polarPoints[startIdx].y, 2));
				if (dist > workpieceParam.lineLen)
					break;
				edge_link1.push_back(a_pt);
			}
			//edge_link2
			for (int m = 1; m < contourPtSize; m++)
			{
				int idx = (endIdx + m) % contourPtSize;
				SVzNL3DPoint a_pt = { polarPoints[idx].x, polarPoints[idx].y, polarPoints[idx].z };
				double dist = sqrt(pow(a_pt.x - polarPoints[endIdx].x, 2) + pow(a_pt.y - polarPoints[endIdx].y, 2));
				if (dist > workpieceParam.lineLen)
					break;
				edge_link2.push_back(a_pt);
			}

			region[rgnIdx].rgnIdx = rgnIdx;
			region[rgnIdx].edge.insert(region[rgnIdx].edge.end(), edge.begin(), edge.end());
			region[rgnIdx].edgeLink_1.insert(region[rgnIdx].edgeLink_1.end(), edge_link1.begin(), edge_link1.end());
			region[rgnIdx].edgeLink_2.insert(region[rgnIdx].edgeLink_2.end(), edge_link2.begin(), edge_link2.end());
		}
	}

#if 0 //老算法，需要扫描为水平-竖直方向
	{
		//垂直跳变特征提取
		std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_v;
		for (int line = 0; line < lineNum; line++)
		{
			if (line == 202)
				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.push_back(line_features);
		}

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

		//生成水平扫描
		std::vector<std::vector<SVzNL3DPosition>> hLines;
		hLines.resize(linePtNum);
		for (int i = 0; i < linePtNum; i++)
			hLines[i].resize(lineNum);
		for (int line = 0; line < lineNum; line++)
		{
			for (int j = 0; j < linePtNum; j++)
			{
				scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
				hLines[j][line] = scanLines[line][j];
				hLines[j][line].pt3D.x = scanLines[line][j].pt3D.y;
				hLines[j][line].pt3D.y = scanLines[line][j].pt3D.x;
			}
		}
		//水平arc特征提取
		std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_h;
		int lineNum_h = (int)hLines.size();
		for (int line = 0; line < lineNum_h; line++)
		{
			if (line == 416)
				int kkk = 1;
			std::vector<SVzNL3DPosition>& lineData = hLines[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[line][0],
				dataSize,
				line,
				groundCalibPara.planeHeight,
				cornerPara, //scale通常取bagH的1/4
				line_features);
			jumpFeatures_h.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[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; line++)
		{
			if (line == 650)
				int kkk = 1;
			bool isLastLine = false;
			if (line == lineNum_h - 1)
				isLastLine = true;
			std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_h[line];
			sg_lineFeaturesGrowing(
				line,
				isLastLine,
				a_lineJumpFeature,
				h_trees,
				growParam);
		}

		//tree信息
		std::vector<SSG_treeInfo> allTreesInfo; //不包含边界
		SSG_treeInfo a_nullTree;
		memset(&a_nullTree, 0, sizeof(SSG_treeInfo));
		allTreesInfo.push_back(a_nullTree); //保持存储位置与treeIdx相同位置，方便索引
		//标记，根据起点的生长树进行标注
		int hvTreeIdx = 1;
		for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++)
		{
			SSG_featureTree* a_vTree = &v_trees[i];
			sg_getTreeROI(a_vTree);
			//记录Tree的信息
			SSG_treeInfo a_treeInfo;
			a_treeInfo.vTreeFlag = 1;
			a_treeInfo.treeIdx = hvTreeIdx;
			a_treeInfo.treeType = a_vTree->treeType;
			a_treeInfo.sLineIdx = a_vTree->sLineIdx;
			a_treeInfo.eLineIdx = a_vTree->eLineIdx;
			a_treeInfo.roi = a_vTree->roi;
			allTreesInfo.push_back(a_treeInfo);

			std::vector<SVzNL3DPoint> a_weld_contour;
			//在原始点云上标记，同时有Mask上标记
			for (int j = 0, j_max = (int)a_vTree->treeNodes.size(); j < j_max; j++)
			{
				SSG_basicFeature1D* a_feature = &a_vTree->treeNodes[j];
				if (scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.z > 1e-4)//虚假目标过滤后点会置0
				{
					int existEdgeId = scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx >> 16;
					if (existEdgeId == 0)
					{
						scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx = a_feature->featureType;
						scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx &= 0xffff;
						scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx += hvTreeIdx << 16;
					}
					a_weld_contour.push_back(scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D);
				}
			}
			hvTreeIdx++;
		}
		int hTreeStart = hvTreeIdx;
		////标注:水平特征
		for (int i = 0, i_max = (int)h_trees.size(); i < i_max; i++)
		{
			SSG_featureTree* a_hTree = &h_trees[i];
			sg_getTreeROI(a_hTree);
			//记录Tree的信息
			SSG_treeInfo a_treeInfo;
			a_treeInfo.vTreeFlag = 0;
			a_treeInfo.treeIdx = hvTreeIdx;
			a_treeInfo.treeType = a_hTree->treeType;
			a_treeInfo.sLineIdx = a_hTree->sLineIdx;
			a_treeInfo.eLineIdx = a_hTree->eLineIdx;
			a_treeInfo.roi.left = a_hTree->roi.top;  //水平扫描xy是交换的
			a_treeInfo.roi.right = a_hTree->roi.bottom;
			a_treeInfo.roi.top = a_hTree->roi.left;
			a_treeInfo.roi.bottom = a_hTree->roi.right;
			allTreesInfo.push_back(a_treeInfo);

			std::vector<SVzNL3DPoint> a_weld_contour;
			//在原始点云上标记，同时有Mask上标记
			for (int j = 0, j_max = (int)a_hTree->treeNodes.size(); j < j_max; j++)
			{
				SSG_basicFeature1D* a_feature = &a_hTree->treeNodes[j];
				if (scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.z > 1e-4)//虚假目标过滤后点会置0
				{
					int existEdgeId = scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx >> 16;
					if (existEdgeId == 0)
					{
						scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += a_feature->featureType << 4;
						scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx &= 0xffff;
						scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += hvTreeIdx << 16;
					}
					a_weld_contour.push_back(scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D);
				}
			}
			hvTreeIdx++;
		}
		int hvTreeSize = hvTreeIdx;

		if (v_trees.size() < 2)
		{
			*errCode = SX_ERR_INVLD_VTREE_NUM;
			return workpieceCorners;
		}
		if (h_trees.size() < 2)
		{
			*errCode = SX_ERR_INVLD_HTREE_NUM;
			return workpieceCorners;
		}

		//寻找vTree的最上和最下
		int vTree_T = 0;
		int vTree_B = 0;
		for (int i = 1, i_max = (int)v_trees.size(); i < i_max; i++)
		{
			if (v_trees[i].roi.top < v_trees[vTree_T].roi.top)
				vTree_T = i;
			if (v_trees[i].roi.bottom > v_trees[vTree_B].roi.bottom)
				vTree_B = i;
		}

		//寻找hTree的最左和最右
		int hTree_L = 0;
		int hTree_R = 0;
		for (int i = 1, i_max = (int)h_trees.size(); i < i_max; i++)
		{
			//水平扫描xy是交换的，左右对应ROI的topBottom
			if (h_trees[i].roi.top < h_trees[hTree_L].roi.top)
				hTree_L = i;
			if (h_trees[i].roi.bottom > h_trees[hTree_R].roi.bottom)
				hTree_R = i;
		}

		region[0].rgnIdx = 0; //Left
		_getEdgeContour(&h_trees[hTree_L], region[0].edge, scanLines, false);
		//寻找对应的两边
		SVzNL3DPoint firstPt = region[0].edge[0];
		SVzNL3DPoint lastPt = region[0].edge.back();
		int idx0 = _getPointClosestContour(v_trees, true, firstPt, scanLines, true);
		if (idx0 < 0)
		{
			*errCode = SX_ERR_INVLD_CLOSES_PT;
			return workpieceCorners;
		}
		_getEdgeLinkingContour(&v_trees[idx0], true, firstPt, region[0].edgeLink_1, scanLines, true, workpieceParam.lineLen);
		int idx1 = _getPointClosestContour(v_trees, true, lastPt, scanLines, true);
		if (idx1 < 0)
		{
			*errCode = SX_ERR_INVLD_CLOSES_PT;
			return workpieceCorners;
		}
		_getEdgeLinkingContour(&v_trees[idx1], true, lastPt, region[0].edgeLink_2, scanLines, true, workpieceParam.lineLen);
		if ((region[0].edgeLink_1.size() < 5) || (region[0].edgeLink_2.size() < 5))
		{
			*errCode = SX_ERR_INVLD_EDGE_LINK_NUM;
			return workpieceCorners;
		}

		region[1].rgnIdx = 1; //Top
		_getEdgeContour(&v_trees[vTree_T], region[1].edge, scanLines, true);
		//寻找对应的两边
		firstPt = region[1].edge[0];
		lastPt = region[1].edge.back();
		idx0 = _getPointClosestContour(h_trees, false, firstPt, scanLines, true);
		if (idx0 < 0)
		{
			*errCode = SX_ERR_INVLD_CLOSES_PT;
			return workpieceCorners;
		}
		_getEdgeLinkingContour(&h_trees[idx0], false, firstPt, region[1].edgeLink_1, scanLines, true, workpieceParam.lineLen);
		idx1 = _getPointClosestContour(h_trees, false, lastPt, scanLines, true);
		if (idx1 < 0)
		{
			*errCode = SX_ERR_INVLD_CLOSES_PT;
			return workpieceCorners;
		}
		_getEdgeLinkingContour(&h_trees[idx1], false, lastPt, region[1].edgeLink_2, scanLines, true, workpieceParam.lineLen);
		if ((region[1].edgeLink_1.size() < 5) || (region[1].edgeLink_2.size() < 5))
		{
			*errCode = SX_ERR_INVLD_EDGE_LINK_NUM;
			return workpieceCorners;
		}
		region[2].rgnIdx = 2; //Right
		_getEdgeContour(&h_trees[hTree_R], region[2].edge, scanLines, false);
		//寻找对应的两边
		firstPt = region[2].edge[0];
		lastPt = region[2].edge.back();
		idx0 = _getPointClosestContour(v_trees, true, firstPt, scanLines, false);
		if (idx0 < 0)
		{
			*errCode = SX_ERR_INVLD_CLOSES_PT;
			return workpieceCorners;
		}
		_getEdgeLinkingContour(&v_trees[idx0], true, firstPt, region[2].edgeLink_1, scanLines, false, workpieceParam.lineLen);
		idx1 = _getPointClosestContour(v_trees, true, lastPt, scanLines, false);
		if (idx1 < 0)
		{
			*errCode = SX_ERR_INVLD_CLOSES_PT;
			return workpieceCorners;
		}
		_getEdgeLinkingContour(&v_trees[idx1], true, lastPt, region[2].edgeLink_2, scanLines, false, workpieceParam.lineLen);
		if ((region[2].edgeLink_1.size() < 5) || (region[2].edgeLink_2.size() < 5))
		{
			*errCode = SX_ERR_INVLD_EDGE_LINK_NUM;
			return workpieceCorners;
		}
		region[3].rgnIdx = 3; //Bottom
		_getEdgeContour(&v_trees[vTree_B], region[3].edge, scanLines, true);
		//寻找对应的两边
		firstPt = region[3].edge[0];
		lastPt = region[3].edge.back();
		idx0 = _getPointClosestContour(h_trees, false, firstPt, scanLines, true);
		if (idx0 < 0)
		{
			*errCode = SX_ERR_INVLD_CLOSES_PT;
			return workpieceCorners;
		}
		_getEdgeLinkingContour(&h_trees[idx0], false, firstPt, region[3].edgeLink_1, scanLines, false, workpieceParam.lineLen);
		idx1 = _getPointClosestContour(h_trees, false, lastPt, scanLines, true);
		if (idx1 < 0)
		{
			*errCode = SX_ERR_INVLD_CLOSES_PT;
			return workpieceCorners;
		}
		_getEdgeLinkingContour(&h_trees[idx1], false, lastPt, region[3].edgeLink_2, scanLines, false, workpieceParam.lineLen);
		if ((region[3].edgeLink_1.size() < 5) || (region[3].edgeLink_2.size() < 5))
		{
			*errCode = SX_ERR_INVLD_EDGE_LINK_NUM;
			return workpieceCorners;
		}
	}
#endif

	for (int i = 0; i < 4; i++)
	{
		if ((i == 0) || (i == 2))
		{
			//Left:防止垂直直线，使用x=ky+b
			std::vector<SVzNL3DPoint> transPts;
			for (int m = 0, m_max = (int)region[i].edge.size(); m < m_max; m++)
			{
				SVzNL3DPoint a_pt;
				a_pt.x = region[i].edge[m].y;
				a_pt.y = region[i].edge[m].x;
				a_pt.z = region[i].edge[m].z;
				transPts.push_back(a_pt);
			}
			//拟合测量
			double edge_x_k, edge_x_b;
			lineFitting(transPts, &edge_x_k, &edge_x_b);
			//计算拟合直线端点
			SVzNL3DPoint end_0 = transPts[0];
			SVzNL3DPoint end_1 = transPts.back();
			SVzNL2DPointD foot_0 = sx_getFootPoint(end_0.x, end_0.y, edge_x_k, edge_x_b);
			SVzNL2DPointD foot_1 = sx_getFootPoint(end_1.x, end_1.y, edge_x_k, edge_x_b);
			region[i].edge_ends[0] = { foot_0.y, foot_0.x, groundCalibPara.planeHeight };
			region[i].edge_ends[1] = { foot_1.y, foot_1.x, groundCalibPara.planeHeight };
			//两侧Linking直线使用 y=kx+b
			double edge_link1_k, edge_link1_b;
			lineFitting(region[i].edgeLink_1, &edge_link1_k, &edge_link1_b);
			end_0 = region[i].edgeLink_1[0];
			end_1 = region[i].edgeLink_1.back();
			foot_0 = sx_getFootPoint(end_0.x, end_0.y, edge_link1_k, edge_link1_b);
			foot_1 = sx_getFootPoint(end_1.x, end_1.y, edge_link1_k, edge_link1_b);
			region[i].edge_link1_ends[0] = { foot_0.x, foot_0.y, groundCalibPara.planeHeight };
			region[i].edge_link1_ends[1] = { foot_1.x, foot_1.y, groundCalibPara.planeHeight };

			double edge_link2_k, edge_link2_b;
			lineFitting(region[i].edgeLink_2, &edge_link2_k, &edge_link2_b);
			end_0 = region[i].edgeLink_2[0];
			end_1 = region[i].edgeLink_2.back();
			foot_0 = sx_getFootPoint(end_0.x, end_0.y, edge_link2_k, edge_link2_b);
			foot_1 = sx_getFootPoint(end_1.x, end_1.y, edge_link2_k, edge_link2_b);
			region[i].edge_link2_ends[0] = { foot_0.x, foot_0.y, groundCalibPara.planeHeight };
			region[i].edge_link2_ends[1] = { foot_1.x, foot_1.y, groundCalibPara.planeHeight };

			//计算交点
			end_0 = region[i].edge[0];
			end_1 = region[i].edge.back();
			SVzNL3DPoint crossPt[3];
			crossPt[0].x = (edge_x_k * edge_link1_b + edge_x_b) / (1.0 - edge_x_k * edge_link1_k);
			crossPt[0].y = edge_link1_k * crossPt[0].x + edge_link1_b;
			crossPt[0].z = groundCalibPara.planeHeight;
			crossPt[2].x = (edge_x_k * edge_link2_b + edge_x_b) / (1.0 - edge_x_k * edge_link2_k);
			crossPt[2].y = edge_link2_k * crossPt[2].x + edge_link2_b;
			crossPt[2].z = groundCalibPara.planeHeight;
			crossPt[1].x = (crossPt[0].x + crossPt[2].x) / 2;
			crossPt[1].y = (crossPt[0].y + crossPt[2].y) / 2;
			crossPt[1].z = groundCalibPara.planeHeight;
			if (i == 0)
			{
				for (int m = 0; m < 3; m++)
					workpieceCorners.corner_L[m] = crossPt[m];
			}
			else
			{
				for (int m = 0; m < 3; m++)
					workpieceCorners.corner_R[m] = crossPt[m];
			}
		}
		else
		{

			//拟合测量
			double edge_k, edge_b;
			lineFitting(region[i].edge, &edge_k, &edge_b);
			SVzNL3DPoint end_0 = region[i].edge[0];
			SVzNL3DPoint end_1 = region[i].edge.back();
			SVzNL2DPointD foot_0 = sx_getFootPoint(end_0.x, end_0.y, edge_k, edge_b);
			SVzNL2DPointD foot_1 = sx_getFootPoint(end_1.x, end_1.y, edge_k, edge_b);
			region[i].edge_ends[0] = { foot_0.x, foot_0.y, groundCalibPara.planeHeight };
			region[i].edge_ends[1] = { foot_1.x, foot_1.y, groundCalibPara.planeHeight };
			//防止垂直直线，使用x=ky+b
			std::vector<SVzNL3DPoint> transPts_link1;
			for (int m = 0, m_max = (int)region[i].edgeLink_1.size(); m < m_max; m++)
			{
				SVzNL3DPoint a_pt;
				a_pt.x = region[i].edgeLink_1[m].y;
				a_pt.y = region[i].edgeLink_1[m].x;
				a_pt.z = region[i].edgeLink_1[m].z;
				transPts_link1.push_back(a_pt);
			}
			double edge_link1_kx, edge_link1_bx;
			lineFitting(transPts_link1, &edge_link1_kx, &edge_link1_bx);
			//计算拟合直线端点
			end_0 = transPts_link1[0];
			end_1 = transPts_link1.back();
			foot_0 = sx_getFootPoint(end_0.x, end_0.y, edge_link1_kx, edge_link1_bx);
			foot_1 = sx_getFootPoint(end_1.x, end_1.y, edge_link1_kx, edge_link1_bx);
			region[i].edge_link1_ends[0] = { foot_0.y, foot_0.x, groundCalibPara.planeHeight };
			region[i].edge_link1_ends[1] = { foot_1.y, foot_1.x, groundCalibPara.planeHeight };
			//两侧Linking直线使用 y=kx+b
			std::vector<SVzNL3DPoint> transPts_link2;
			for (int m = 0, m_max = (int)region[i].edgeLink_2.size(); m < m_max; m++)
			{
				SVzNL3DPoint a_pt;
				a_pt.x = region[i].edgeLink_2[m].y;
				a_pt.y = region[i].edgeLink_2[m].x;
				a_pt.z = region[i].edgeLink_2[m].z;
				transPts_link2.push_back(a_pt);
			}
			double edge_link2_kx, edge_link2_bx;
			lineFitting(transPts_link2, &edge_link2_kx, &edge_link2_bx);
			end_0 = transPts_link2[0];
			end_1 = transPts_link2.back();
			foot_0 = sx_getFootPoint(end_0.x, end_0.y, edge_link2_kx, edge_link2_bx);
			foot_1 = sx_getFootPoint(end_1.x, end_1.y, edge_link2_kx, edge_link2_bx);
			region[i].edge_link2_ends[0] = { foot_0.y, foot_0.x, groundCalibPara.planeHeight };
			region[i].edge_link2_ends[1] = { foot_1.y, foot_1.x, groundCalibPara.planeHeight };

			//计算交点
			end_0 = region[i].edge[0];
			end_1 = region[i].edge.back();
			SVzNL3DPoint crossPt[3];
			crossPt[0].x = (edge_link1_kx * edge_b + edge_link1_bx) / (1.0 - edge_link1_kx * edge_k);
			crossPt[0].y = edge_k * crossPt[0].x + edge_b;
			crossPt[0].z = groundCalibPara.planeHeight;
			crossPt[2].x = (edge_link2_kx * edge_b + edge_link2_bx) / (1.0 - edge_link2_kx * edge_k);
			crossPt[2].y = edge_k * crossPt[2].x + edge_b;
			crossPt[2].z = groundCalibPara.planeHeight;
			crossPt[1].x = (crossPt[0].x + crossPt[2].x) / 2;
			crossPt[1].y = (crossPt[0].y + crossPt[2].y) / 2;
			crossPt[1].z = groundCalibPara.planeHeight;
			if (i == 1)
			{
				for (int m = 0; m < 3; m++)
					workpieceCorners.corner_T[m] = crossPt[m];
			}
			else
			{
				for (int m = 0; m < 3; m++)
					workpieceCorners.corner_B[m] = crossPt[m];
			}
		}
	}
	



#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_L[i], groundCalibPara.invRMatrix);
		workpieceCorners.corner_L[i] = rawObj;
		rawObj = _translatePoint(workpieceCorners.corner_R[i], groundCalibPara.invRMatrix);
		workpieceCorners.corner_R[i] = rawObj;
		rawObj = _translatePoint(workpieceCorners.corner_T[i], groundCalibPara.invRMatrix);
		workpieceCorners.corner_T[i] = rawObj;
		rawObj = _translatePoint(workpieceCorners.corner_B[i], groundCalibPara.invRMatrix);
		workpieceCorners.corner_B[i] = rawObj;
	}
#if _OUTPUT_DEBUG_DATA
	for (int i = 0; i < 4; i++)
	{
		rawObj = _translatePoint(region[i].edge_ends[0], groundCalibPara.invRMatrix);
		region[i].edge_ends[0] = rawObj;
		rawObj = _translatePoint(region[i].edge_ends[1], groundCalibPara.invRMatrix);
		region[i].edge_ends[1] = rawObj;
		rawObj = _translatePoint(region[i].edge_link1_ends[0], groundCalibPara.invRMatrix);
		region[i].edge_link1_ends[0] = rawObj;
		rawObj = _translatePoint(region[i].edge_link1_ends[1], groundCalibPara.invRMatrix);
		region[i].edge_link1_ends[1] = rawObj;
		rawObj = _translatePoint(region[i].edge_link2_ends[0], groundCalibPara.invRMatrix);
		region[i].edge_link2_ends[0] = rawObj;
		rawObj = _translatePoint(region[i].edge_link2_ends[1], groundCalibPara.invRMatrix);
		region[i].edge_link2_ends[1] = rawObj;
		for (int m = 0, m_max = (int)region[i].edge.size(); m < m_max; m++)
		{
			rawObj = _translatePoint(region[i].edge[m], groundCalibPara.invRMatrix);
			region[i].edge[m] = rawObj;
		}
		for (int m = 0, m_max = (int)region[i].edgeLink_1.size(); m < m_max; m++)
		{
			rawObj = _translatePoint(region[i].edgeLink_1[m], groundCalibPara.invRMatrix);
			region[i].edgeLink_1[m] = rawObj;;
		}
		for (int m = 0, m_max = (int)region[i].edgeLink_2.size(); m < m_max; m++)
		{
			rawObj = _translatePoint(region[i].edgeLink_2[m], groundCalibPara.invRMatrix);
			region[i].edgeLink_2[m] = rawObj;;
		}
	}
#endif
#endif

#if _OUTPUT_DEBUG_DATA
	if (debug_conturs)
	{
		for (int i = 0; i < 4; i++)
		{
			debug_conturs[i].rgnIdx = region[i].rgnIdx;
			debug_conturs[i].edge_ends[0] = region[i].edge_ends[0];
			debug_conturs[i].edge_ends[1] = region[i].edge_ends[1];
			debug_conturs[i].edge_link1_ends[0] = region[i].edge_link1_ends[0];
			debug_conturs[i].edge_link1_ends[1] = region[i].edge_link1_ends[1];
			debug_conturs[i].edge_link2_ends[0] = region[i].edge_link2_ends[0];
			debug_conturs[i].edge_link2_ends[1] = region[i].edge_link2_ends[1];
			debug_conturs[i].edge_size = (int)region[i].edge.size();
			debug_conturs[i].edge = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * (int)region[i].edge.size());
			for (int m = 0, m_max = (int)region[i].edge.size(); m < m_max; m++)
				debug_conturs[i].edge[m] = region[i].edge[m];
			debug_conturs[i].edgeLink1_size = (int)region[i].edgeLink_1.size();
			debug_conturs[i].edgeLink_1 = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * (int)region[i].edgeLink_1.size());
			for (int m = 0, m_max = (int)region[i].edgeLink_1.size(); m < m_max; m++)
				debug_conturs[i].edgeLink_1[m] = region[i].edgeLink_1[m];
			debug_conturs[i].edgeLink2_size = (int)region[i].edgeLink_2.size();
			debug_conturs[i].edgeLink_2 = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * (int)region[i].edgeLink_2.size());
			for (int m = 0, m_max = (int)region[i].edgeLink_2.size(); m < m_max; m++)
				debug_conturs[i].edgeLink_2[m] = region[i].edgeLink_2[m];
		}
	}
#endif

	workpieceCorners.workpieceType = 1;
	return workpieceCorners;
}