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

//version 1.0.0 : base version release to customer
//version 1.1.0 : 添加了地面调平和棒材定位
//version 1.1.1 : 初始发布给客户的版本
//version 1.2.0 : 配天螺杆测量增加了定位盘中心测量功能
//version 1.2.1 : 增加了定位盘中心完整姿态输出
//version 1.2.2 : 调整了定位盘中心姿态定义：法向（Z向）：向右；Y：向下
//version 1.2.3 : 根据定向盘轮廓的直线段确定向右的向量
//version 1.2.4 : 根据定向盘左上点和右下点确定姿态向量
//version 1.2.5 : 添加筑裕钢筋焊缝提取API
//version 1.2.6 : 定位盘位姿计算由RANSAC调整为迭代拟合，测试精度是否有提高
//version 1.2.7 : (1)根据实际点云修正了螺杆定位算法（2）定位盘位姿计算改回RANSAC
//version 1.2.8 : 修正螺杆定位算法中的一个Bug
//version 1.2.9 : 修正螺杆定位算法中轴向拟合计算时方向确定的问题，保证投影后的点云与投影前的点云的Z变化趋势一致
//version 1.2.10 : 改进螺杆定位取点算法
//version 1.2.11 : 根据现场数据调整特征生长参数
//version 1.3.0 : 新的定位盘中心测量功能
//version 1.3.1 : 定位盘中心测量功能添加了噪声过滤
//version 1.3.2 : 矩森棒材抓取修正一个错误码返回错误，测试了地面调平API
std::string	m_strVersion = "1.3.2";
const char* wd_rodAndBarDetectionVersion(void)
{
	return m_strVersion.c_str();
}

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

//相机姿态调平，并去除地面
void sx_rodPosition_lineDataR(
	std::vector< SVzNL3DPosition>& a_line,
	const double* camPoseR,
	double groundH)
{
	lineDataRT_vector(a_line, camPoseR, groundH);
}

SVzNL3DPoint _translatePoint(SVzNL3DPoint point, const 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;
}


SVzNL3DPoint getArcPeak(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines, 
	SWD_segFeature & a_arcFeature,
	SVzNL2DPoint& arcPos)
{
	SVzNL3DPoint arcPeak = scanLines[a_arcFeature.lineIdx][a_arcFeature.startPtIdx].pt3D;
	for (int i = a_arcFeature.startPtIdx+1; i <= a_arcFeature.endPtIdx; i++)
	{
		if (scanLines[a_arcFeature.lineIdx][i].pt3D.z > 1e-4) //跳开空点
		{
			if (arcPeak.z > scanLines[a_arcFeature.lineIdx][i].pt3D.z)
			{
				arcPeak = scanLines[a_arcFeature.lineIdx][i].pt3D;
				arcPos = { a_arcFeature.lineIdx , i };
			}
		}
	}
	return arcPeak;
}

SVzNL3DPoint getArcPeak_parabolaFitting(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	SWD_rodArcFeature& a_arcFeature,
	SVzNL2DPoint& arcPos)
{
	std::vector<cv::Point2d> points;
	for (int i = a_arcFeature.startPtIdx + 1; i <= a_arcFeature.endPtIdx; i++)
	{
		if (scanLines[a_arcFeature.lineIdx][i].pt3D.z > 1e-4) //跳开空点
		{
			cv::Point2d a_pt2D;
			if (scanLines[a_arcFeature.lineIdx][i].pt3D.z > 1e-4)
			{
				a_pt2D.x = scanLines[a_arcFeature.lineIdx][i].pt3D.y;
				a_pt2D.y = scanLines[a_arcFeature.lineIdx][i].pt3D.z;
				points.push_back(a_pt2D);
			}
		}
	}
	double a, b, c, mse, max_err;
	//抛物线最小二乘拟合 y = ax ^ 2 + bx + c
	bool result = leastSquareParabolaFitEigen(
		points,
		a, b, c,
		mse, max_err);
	double yP = -b / (2 * a);
	//寻找与yP最近的点作为Peak点
	SVzNL3DPoint arcPeak = {0, 0, 0};
	double minDist = -1;
	for (int i = a_arcFeature.startPtIdx + 1; i <= a_arcFeature.endPtIdx; i++)
	{
		if (scanLines[a_arcFeature.lineIdx][i].pt3D.z > 1e-4) //跳开空点
		{
			double dist = abs(scanLines[a_arcFeature.lineIdx][i].pt3D.y - yP);
			if (minDist < 0)
			{
				minDist = dist;
				arcPeak = scanLines[a_arcFeature.lineIdx][i].pt3D;
				arcPos = { a_arcFeature.lineIdx , i };
			}
			else
			{
				if(minDist > dist)
				{
					minDist = dist;
					arcPeak = scanLines[a_arcFeature.lineIdx][i].pt3D;
					arcPos = { a_arcFeature.lineIdx , i };
				}
			}
		}
	}
	return arcPeak;
}

//投影，提取ROI内的数据
void xoyROIProjection(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const double* rtMatrix,
	SSG_ROIRectD& roi_xoy,
	int objClusterId,
	std::vector<std::vector<int>>& clusterIdMask,
	std::vector<SWD3DPointPostion>& projectPoints
	)
{
	int lineNum = (int)scanLines.size();
	for (int line = 0; line < lineNum; line++)
	{
		std::vector<SVzNL3DPosition>& a_line = scanLines[line];
		int ptNum = (int)a_line.size();
		for (int i = 0; i < (int)a_line.size(); i++)
		{
			SVzNL3DPoint a_pt = a_line[i].pt3D;
			if (a_pt.z < 1e-4)
				continue;
			int cId = clusterIdMask[line][i];
			if (cId != objClusterId)
				continue;

			double x = a_pt.x * rtMatrix[0] + a_pt.y * rtMatrix[1] + a_pt.z * rtMatrix[2];
			double y = a_pt.x * rtMatrix[3] + a_pt.y * rtMatrix[4] + a_pt.z * rtMatrix[5];
			double z = a_pt.x * rtMatrix[6] + a_pt.y * rtMatrix[7] + a_pt.z * rtMatrix[8];
			if ((x >= roi_xoy.left) && (x <= roi_xoy.right) &&
				(y >= roi_xoy.top) && (y <= roi_xoy.bottom))
			{
				SWD3DPointPostion projectPt;
				projectPt.lineIdx = line;
				projectPt.ptIdx = i;
				projectPt.point.x = x;
				projectPt.point.y = y;
				projectPt.point.z = z;
				projectPoints.push_back(projectPt);
			}
		}
	}
}

SVzNLRangeD getZRange(std::vector<SWD3DPointPostion>& projectPoints)
{
	int ptNum = (int)projectPoints.size();
	SVzNLRangeD zRange;
	zRange.min = DBL_MAX;
	zRange.max = -DBL_MAX;
	for (int i = 0; i < ptNum; i++)
	{
		zRange.min = zRange.min > projectPoints[i].point.z ? projectPoints[i].point.z : zRange.min;
		zRange.max = zRange.max < projectPoints[i].point.z ? projectPoints[i].point.z : zRange.max;
	}
	return zRange;
}

void zCutPointClouds(
	std::vector<SWD3DPointPostion>& projectPoints,
	SVzNLRangeD& zRange, 
	std::vector<SWD3DPointPostion>& cutLayerPoints,
	std::vector<std::vector<int>>& addrMapping)
{
	int ptNum = (int)projectPoints.size();
	for (int i = 0; i < ptNum; i++)
	{
		if ((projectPoints[i].point.z >= zRange.min) && (projectPoints[i].point.z <= zRange.max))
		{
			cutLayerPoints.push_back(projectPoints[i]);
			int lineIdx = projectPoints[i].lineIdx;
			int ptIdx = projectPoints[i].ptIdx;
			addrMapping[lineIdx][ptIdx] = i;
		}
	}
}

SVzNL3DPoint getXoYCentroid(std::vector<SVzNL3DPoint>& points)
{
	int ptNum = (int)points.size();
	SVzNL3DPoint centroid = { 0.0, 0.0, 0.0 };
	if (ptNum == 0)
		return centroid;
	for (int i = 0; i < ptNum; i++)
	{
		centroid.x += points[i].x;
		centroid.y += points[i].y;
	}
	centroid.x = centroid.x / ptNum;
	centroid.y = centroid.y / ptNum;
	return centroid;
}

SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9])
{
	SVzNL3DPoint _r_pt;
	_r_pt.x = pt3D.x * matrix3d[0] + pt3D.y * matrix3d[1] + pt3D.z * matrix3d[2];
	_r_pt.y = pt3D.x * matrix3d[3] + pt3D.y * matrix3d[4] + pt3D.z * matrix3d[5];
	_r_pt.z = pt3D.x * matrix3d[6] + pt3D.y * matrix3d[7] + pt3D.z * matrix3d[8];
	return _r_pt;
}


void rodAarcFeatueDetection(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SSG_cornerParam cornerPara,
	const SSG_outlierFilterParam filterParam,
	const double rodDiameter,
	std::vector<std::vector<SWD_rodArcFeature>>& arcFeatures)
{
	int lineNum = (int)scanLines.size();
	int linePtNum = (int)scanLines[0].size();
	for (int line = 0; line < lineNum; line++)
	{
		if (line == 413)
			int kkk = 1;
		std::vector<SVzNL3DPosition>& lineData = scanLines[line];

		// Filter outliers from line data
		sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
		// Extract rod arc features
		std::vector<SWD_rodArcFeature> line_rodArcs;
		wd_getRodArcFeature_segmentPeakCornerMethod(
			lineData,
			line,
			rodDiameter/4,
			rodDiameter / 8,
			rodDiameter / 2,
			cornerPara,
			line_rodArcs  //环
		);
		//wd_getRodArcFeature_peakCornerMethod(lineData, line, rodDiameter / 2, cornerPara, line_rodArcs);
		arcFeatures.push_back(line_rodArcs);
	}
	return;
}

//所有点计算点云ROI: vecotr格式, （不过滤z小于0的点）
SVzNL3DRangeD _getPointCloudROI(std::vector<SWD3DPointPostion>& scanData)
{
	SVzNL3DRangeD roi;
	roi.xRange = { 0, -1 };
	roi.yRange = { 0, -1 };
	roi.zRange = { 0, -1 };

	int nPositionCnt = (int)scanData.size();
	for (int i = 0; i < nPositionCnt; i++)
	{
		SWD3DPointPostion& pt3D = scanData[i];
		if (roi.xRange.max < roi.xRange.min)
		{
			roi.xRange.min = pt3D.point.x;
			roi.xRange.max = pt3D.point.x;
		}
		else
		{
			if (roi.xRange.min > pt3D.point.x)
				roi.xRange.min = pt3D.point.x;
			if (roi.xRange.max < pt3D.point.x)
				roi.xRange.max = pt3D.point.x;
		}
		//y
		if (roi.yRange.max < roi.yRange.min)
		{
			roi.yRange.min = pt3D.point.y;
			roi.yRange.max = pt3D.point.y;
		}
		else
		{
			if (roi.yRange.min > pt3D.point.y)
				roi.yRange.min = pt3D.point.y;
			if (roi.yRange.max < pt3D.point.y)
				roi.yRange.max = pt3D.point.y;
		}
		//z
		if (roi.zRange.max < roi.zRange.min)
		{
			roi.zRange.min = pt3D.point.z;
			roi.zRange.max = pt3D.point.z;
		}
		else
		{
			if (roi.zRange.min > pt3D.point.z)
				roi.zRange.min = pt3D.point.z;
			if (roi.zRange.max < pt3D.point.z)
				roi.zRange.max = pt3D.point.z;
		}
	}

	return roi;
}

//计算螺杆端部中心点位姿
 void sx_hexHeadScrewMeasure(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	bool isHorizonScan,  //true:激光线平行槽道；false:激光线垂直槽道
	const SSG_cornerParam cornerPara,
	const SSG_outlierFilterParam filterParam,
	const SSG_treeGrowParam growParam,
	double rodDiameter,
	std::vector<SSX_rodPoseInfo>& screwInfo,
	int* errCode)
{
	*errCode = 0;
	int lineNum = (int)scanLines.size();
	if (lineNum == 0)
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return;
	}

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

	//判断数据格式是否为grid。算法只能处理grid数据格式
	bool isGridData = true;
	for (int line = 0; line < lineNum; line++)
	{
		if (linePtNum != (int)scanLines[line].size())
		{
			isGridData = false;
			break;
		}
	}
	if (false == isGridData)//数据不是网格格式
	{
		*errCode = SG_ERR_NOT_GRID_FORMAT;
		return;
	}

	//聚类
	std::vector<SVzNL3DPosition> clusterDataSrc;
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			if (scanLines[line][j].pt3D.z > 1e-4)
			{
				SVzNL3DPosition a_pt;
				a_pt.nPointIdx = (line << 16) | (j & 0xffff);
				a_pt.pt3D = scanLines[line][j].pt3D;
				clusterDataSrc.push_back(a_pt);
			}
		}
	}
	int clusterCheckWin = 5;
	int clusterDist = sqrt(pow(growParam.zDeviation_max, 2) + pow(growParam.maxSkipDistance, 2) + pow(growParam.yDeviation_max, 2));
	std::vector<std::vector< SVzNL3DPosition>> objClusters;
	wd_pointClustering_speedUp(
		clusterDataSrc,
		lineNum, linePtNum, clusterCheckWin, //搜索窗口
		clusterDist,
		1, //0 - 2d distance; 1- 3d distance 
		objClusters  //result
	);
	//生成ClusterMask
	int clusterSize = (int)objClusters.size();
	std::vector<std::vector<int>> clusterIdMask;
	clusterIdMask.resize(lineNum);
	for (int i = 0; i < lineNum; i++)
	{
		clusterIdMask[i].resize(linePtNum);
		std::fill(clusterIdMask[i].begin(), clusterIdMask[i].end(), -1);
	}
	for (int i = 0; i < clusterSize; i++)
	{
		for (int j = 0; j < (int)objClusters[i].size(); j++)
		{
			int line = objClusters[i][j].nPointIdx >> 16;
			int ptIdx = objClusters[i][j].nPointIdx & 0x0000FFFF;
			clusterIdMask[line][ptIdx] = i;
		}
	}

	//生成扫描数据副本
	std::vector< std::vector<SVzNL3DPosition>> data_lines;
	if (false == isHorizonScan)
	{
		data_lines.resize(lineNum);
		for (int line = 0; line < lineNum; line++)
		{
			data_lines[line].insert(data_lines[line].end(), scanLines[line].begin(), scanLines[line].end());
			for (int j = 0, j_max = (int)data_lines[line].size(); j < j_max; j++)
			{
				data_lines[line][j].nPointIdx = j;
				scanLines[line][j].nPointIdx = 0; //转义复用
			}
		}
	}
	else
	{
		data_lines.resize(linePtNum);
		for (int i = 0; i < linePtNum; i++)
			data_lines[i].resize(lineNum);
		for (int line = 0; line < lineNum; line++)
		{
			for (int j = 0; j < linePtNum; j++)
			{
				scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
				data_lines[j][line] = scanLines[line][j];
				data_lines[j][line].pt3D.x = scanLines[line][j].pt3D.y;
				data_lines[j][line].pt3D.y = scanLines[line][j].pt3D.x;
			}

		}

		lineNum = linePtNum;
		linePtNum = (int)data_lines[0].size();
		for (int line = 0; line < lineNum; line++)
		{
			for (int j = 0, j_max = (int)data_lines[line].size(); j < j_max; j++)
				data_lines[line][j].nPointIdx = j;
		}
	}

	std::vector<std::vector<SWD_rodArcFeature>> arcFeatures;
	rodAarcFeatueDetection(
		data_lines,
		cornerPara,
		filterParam,
		rodDiameter,
		arcFeatures);

	//特征生长
	std::vector<SWD_rodArcFeatureTree> rodArcTrees;
	wd_getRodArcFeatureGrowingTrees(arcFeatures, rodArcTrees, growParam);

	if (rodArcTrees.size() == 0)
	{
		*errCode = SG_ERR_NOT_GRID_FORMAT;
		return;
	}
	int objNum = (int)rodArcTrees.size();
	
	//内部参数：nodeSize小于此的认为无效
	int validNodeSizeTH = 30;

	//置标志，用于debug
	for (int i = 0; i < objNum; i++)
	{
		int nodeNum = (int)rodArcTrees[i].treeNodes.size();
		for (int j = 0; j < nodeNum; j++)
		{
			int lineIdx, ptIdx;
			if (false == isHorizonScan)
			{
				lineIdx = rodArcTrees[i].treeNodes[j].lineIdx;
				for (int m = rodArcTrees[i].treeNodes[j].startPtIdx; m <= rodArcTrees[i].treeNodes[j].endPtIdx; m++)
				{
					ptIdx = m;
					if( (m == rodArcTrees[i].treeNodes[j].startPtIdx) || (m == rodArcTrees[i].treeNodes[j].endPtIdx) ||
						(m == rodArcTrees[i].treeNodes[j].peakPtIdx))
						scanLines[lineIdx][ptIdx].nPointIdx = 2;
					else
						scanLines[lineIdx][ptIdx].nPointIdx = 1;
				}
			}
			else
			{
				ptIdx = rodArcTrees[i].treeNodes[j].lineIdx;
				for (int m = rodArcTrees[i].treeNodes[j].startPtIdx; m <= rodArcTrees[i].treeNodes[j].endPtIdx; m++)
				{
					lineIdx = m;
					if ((m == rodArcTrees[i].treeNodes[j].startPtIdx) || (m == rodArcTrees[i].treeNodes[j].endPtIdx) ||
						(m == rodArcTrees[i].treeNodes[j].peakPtIdx))
						scanLines[lineIdx][ptIdx].nPointIdx = 2;
					else
						scanLines[lineIdx][ptIdx].nPointIdx = 1;
				}
			}
		}
	}
	
	//确定目标，以及目标所有的类
	//计算ROI，z最小的生长树为目标
	std::vector<SVzNL3DRangeD> objROIs;
	std::vector<int> objCluster;
	for (int i = 0; i < objNum; i++)
	{
		// Initialize min and max values
		// Calculate X, Y and Z ranges
		SVzNL3DRangeD a_roi3D;
		a_roi3D.xRange.min = DBL_MAX; a_roi3D.xRange.max = -DBL_MAX;
		a_roi3D.yRange.min = DBL_MAX; a_roi3D.yRange.max = -DBL_MAX;
		a_roi3D.zRange.min = DBL_MAX; a_roi3D.zRange.max = -DBL_MAX;

		std::vector<int> clusterHist;
		clusterHist.resize(clusterSize);
		std::fill(clusterHist.begin(), clusterHist.end(), 0);

		int nodeNum = (int)rodArcTrees[i].treeNodes.size();
		for (int j = 0; j < nodeNum; j++)
		{
			SVzNL3DPoint peak;
			int lineIdx, ptIdx;
			if (false == isHorizonScan)
			{
				int lineIdx = rodArcTrees[i].treeNodes[j].lineIdx;
				int ptIdx = rodArcTrees[i].treeNodes[j].peakPtIdx;
				peak = scanLines[lineIdx][ptIdx].pt3D;
				int clusterId = clusterIdMask[lineIdx][ptIdx];
				if (clusterId >= 0)
					clusterHist[clusterId]++;
			}
			else
			{
				int ptIdx = rodArcTrees[i].treeNodes[j].lineIdx;
				int lineIdx = rodArcTrees[i].treeNodes[j].peakPtIdx;
				peak = scanLines[lineIdx][ptIdx].pt3D;
				int clusterId = clusterIdMask[lineIdx][ptIdx];
				if (clusterId >= 0)
					clusterHist[clusterId]++;
			}
			if (peak.z > 1e-4)
			{
				a_roi3D.xRange.min = std::min(a_roi3D.xRange.min, peak.x);
				a_roi3D.xRange.max = std::max(a_roi3D.xRange.max, peak.x);
				a_roi3D.yRange.min = std::min(a_roi3D.yRange.min, peak.y);
				a_roi3D.yRange.max = std::max(a_roi3D.yRange.max, peak.y);
				a_roi3D.zRange.min = std::min(a_roi3D.zRange.min, peak.z);
				a_roi3D.zRange.max = std::max(a_roi3D.zRange.max, peak.z);
			}
		}
		int objClusterId = 0;
		for (int m = 1; m < clusterSize; m++)
		{
			if (clusterHist[objClusterId] < clusterHist[m])
				objClusterId = m;
		}
		objROIs.push_back(a_roi3D);
		objCluster.push_back(objClusterId);
	}

	//选择Z最小的作为目标
	int objIdx = -1;
	for (int i = 0; i < objNum; i++)
	{
		int nodeNum = (int)rodArcTrees[i].treeNodes.size();
		if (nodeNum < validNodeSizeTH)
			continue;

		if (objIdx < 0)
			objIdx = i;
		else if (objROIs[objIdx].zRange.min > objROIs[i].zRange.min)
			objIdx = i;
	}

	if(objIdx < 0)
	{
		*errCode = SX_ERR_ZERO_OBJECTS;
		return;
	}

	int resultObjClusterId = objCluster[objIdx];
	//提取轴向
	//空间直线拟合
	std::vector<SVzNL3DPoint> fitPoints;
	std::vector<SVzNL2DPoint> fit2DPos;
	int nodeSize = (int)rodArcTrees[objIdx].treeNodes.size();
	for (int j = 0; j < nodeSize; j++)
	{
		SVzNL2DPoint arcPos;
		SVzNL3DPoint a_pt = getArcPeak_parabolaFitting(data_lines, rodArcTrees[objIdx].treeNodes[j], arcPos);
		if (false == isHorizonScan)
		{
			fitPoints.push_back(a_pt);
			fit2DPos.push_back(arcPos);
		}
		else
		{
			arcPos = { arcPos.y, arcPos.x };
			a_pt = {a_pt.y, a_pt.x, a_pt.z};
			fitPoints.push_back(a_pt);
			fit2DPos.push_back(arcPos);
		}
	}
	if (fitPoints.size() < 20)
	{
		*errCode = SX_ERR_ZERO_OBJECTS;
		return;
	}

	//去除头50mm， 尾5个点，防止在端部和根部扫描时的数据有干扰
	SVzNL3DPoint pt_0 = fitPoints[0];
	SVzNL3DPoint pt_last = fitPoints.back();
	double headLenToRemove = 40;
	if (pt_0.z < pt_last.z)
	{
		int eraseIdx = -1;
		for (int i = 1; i < (int)fitPoints.size(); i++)
		{
			double dist = sqrt(pow(fitPoints[i].x - pt_0.x, 2) + pow(fitPoints[i].y - pt_0.y, 2) + pow(fitPoints[i].z - pt_0.z, 2));
			if (dist > headLenToRemove)
			{
				eraseIdx = i;
				break;
			}
		}
		if(eraseIdx < 0)
		{
			*errCode = SX_ERR_ZERO_OBJECTS;
			return;
		}
		fitPoints.erase(fitPoints.begin(), fitPoints.begin() + eraseIdx);
		fit2DPos.erase(fit2DPos.begin(), fit2DPos.begin() + eraseIdx);
		fitPoints.erase(fitPoints.end() - 15, fitPoints.end());
		fit2DPos.erase(fit2DPos.end() - 15, fit2DPos.end());
	}
	else
	{
		int eraseIdx = -1;
		for (int i = (int)fitPoints.size()-1; i >= 0; i--)
		{
			double dist = sqrt(pow(fitPoints[i].x - pt_last.x, 2) + pow(fitPoints[i].y - pt_last.y, 2) + pow(fitPoints[i].z - pt_last.z, 2));
			if (dist > headLenToRemove)
			{
				eraseIdx = i;
				break;
			}
		}
		if (eraseIdx < 0)
		{
			*errCode = SX_ERR_ZERO_OBJECTS;
			return;
		}
		int eraseNum = (int)fitPoints.size() - eraseIdx;
		fitPoints.erase(fitPoints.end() - eraseNum, fitPoints.end());
		fit2DPos.erase(fit2DPos.end() - eraseNum, fit2DPos.end());
		fitPoints.erase(fitPoints.begin(), fitPoints.begin() + 15);
		fit2DPos.erase(fit2DPos.begin(), fit2DPos.begin() + 15);
	}
	//置标志
	for (int j = 0; j < (int)fit2DPos.size(); j++)
	{
		int lineIdx, ptIdx;
		lineIdx = fit2DPos[j].x;
		ptIdx = fit2DPos[j].y;
		scanLines[lineIdx][ptIdx].nPointIdx = 4;
	}

	//拟合
	SVzNL3DPoint P0_center, P1_dir;
	bool result = fitLine3DLeastSquares(fitPoints, P0_center, P1_dir);
	if (false == result)
	{
		*errCode = SX_ERR_ZERO_OBJECTS;
		return;
	}
	//投影
	//计算旋转向量
	SVzNL3DPoint vector1 = P1_dir;
	SVzNL3DPoint vector2 = { 0, 0, 1.0 };
	SSG_planeCalibPara rotatePara = wd_computeRTMatrix(	vector1, vector2);
#if 1
	std::vector< SVzNL3DPoint> verifyData;
	for (int m = 0; m < (int)fitPoints.size(); m++)
	{
		SVzNL3DPoint rPt = _ptRotate(fitPoints[m], rotatePara.planeCalib);
		verifyData.push_back(rPt);
	}
#endif
	bool dirInverting = false;
	if (((fitPoints[0].z < fitPoints.back().z) && (verifyData[0].z > verifyData.back().z)) ||
		((fitPoints[0].z > fitPoints.back().z) && (verifyData[0].z < verifyData.back().z)))
		dirInverting = true;
	if (true == dirInverting)
	{
		P1_dir = { -P1_dir.x, -P1_dir.y, -P1_dir.z };
		vector1 = P1_dir;
		rotatePara = wd_computeRTMatrix(vector1, vector2);
	}
	verifyData.clear();
	for (int m = 0; m < (int)fitPoints.size(); m++)
	{
		SVzNL3DPoint rPt = _ptRotate(fitPoints[m], rotatePara.planeCalib);
		verifyData.push_back(rPt);
	}
	if (((fitPoints[0].z < fitPoints.back().z) && (verifyData[0].z > verifyData.back().z)) ||
		((fitPoints[0].z > fitPoints.back().z) && (verifyData[0].z < verifyData.back().z)))
		dirInverting = true;
	else
		dirInverting = false;

	//
	SVzNL3DPoint test_center = { P0_center.x, P0_center.y + rodDiameter / 2,  P0_center.z - 100 };
	SVzNL3DPoint P0_rotate = _ptRotate(P0_center, rotatePara.planeCalib);
	SVzNL3DPoint test_center_rotate = _ptRotate(test_center, rotatePara.planeCalib);
	SSG_ROIRectD roi_xoy;
	roi_xoy.left = P0_rotate.x - rodDiameter* 2.0; //2D范围
	roi_xoy.right = P0_rotate.x + rodDiameter * 2.0; //2D范围
	roi_xoy.top = P0_rotate.y - rodDiameter * 2.0; //2D范围
	roi_xoy.bottom = P0_rotate.y + rodDiameter * 2.0; //2D范围

	std::vector< SWD3DPointPostion> roiProjectionData;
	xoyROIProjection(scanLines, rotatePara.planeCalib, roi_xoy, resultObjClusterId, clusterIdMask, roiProjectionData);

	//取端面
	SVzNLRangeD zRange = getZRange(roiProjectionData);
	SVzNLRangeD cutZRange;
	if (false == dirInverting)
	{
		cutZRange.min = zRange.min;
		cutZRange.max = zRange.min + 10.0;  //5mm的端面
	}
	else
	{
		cutZRange.max = zRange.max;
		cutZRange.min = zRange.max - 10.0;  //5mm的端面
	}
	std::vector<SWD3DPointPostion> surfacePoints;
	std::vector<std::vector<int>>addrMapping;
	addrMapping.resize(scanLines.size());
	for (int i = 0; i < (int)scanLines.size(); i++)
	{
		addrMapping[i].resize(scanLines[i].size());
		std::fill(addrMapping[i].begin(), addrMapping[i].end(), -1);
	}
	zCutPointClouds(roiProjectionData, 	cutZRange, surfacePoints, addrMapping);
	//计算中心点
	SWD3DPointPostion projectionCenter;// = getXoYCentroid(surfacePoints);
	SVzNL3DRangeD roi3D = _getPointCloudROI(surfacePoints);
	//计算XY平面上的质心
	double sum_x = 0, sum_y = 0;
	int sum_size = 0;
	for (int i = 0; i < (int)surfacePoints.size(); i++)
	{
		if (surfacePoints[i].point.z > 1e-4)
		{
			sum_x += surfacePoints[i].point.x;
			sum_y += surfacePoints[i].point.y;
			sum_size++;
		}
	}
	if(sum_size == 0)
	{
		*errCode = SX_ERR_ZERO_OBJECTS;
		return;
	}
	projectionCenter.lineIdx = -1;
	projectionCenter.ptIdx = -1;
	projectionCenter.point.x = sum_x / sum_size; // (roi3D.xRange.min + roi3D.xRange.max) / 2;
	projectionCenter.point.y = sum_y / sum_size; // (roi3D.yRange.min + roi3D.yRange.max) / 2;
	projectionCenter.point.z = zRange.min;
	//迭代搜索：搜索projectionCenter为中心5mm内z最大的的点为中心点
	double searchR = 5.0;
	int centerIdx = -1;
	double maxZ = -1;
	for (int i = 0; i < (int)surfacePoints.size(); i++)
	{
		double dist = sqrt(pow(surfacePoints[i].point.x - projectionCenter.point.x, 2) + pow(surfacePoints[i].point.y - projectionCenter.point.y, 2));
		if (dist < searchR)
		{
			if (centerIdx < 0)
			{
				centerIdx = i;
				maxZ = surfacePoints[i].point.z;
			}
			else
			{
			
				if (((false == dirInverting) && (surfacePoints[centerIdx].point.z < surfacePoints[i].point.z)) ||
					((true == dirInverting) && (surfacePoints[centerIdx].point.z > surfacePoints[i].point.z)))
				{
					centerIdx = i;
					maxZ = surfacePoints[i].point.z;
				}
			}
		}
	}
	if(centerIdx < 0)
	{
		*errCode = SX_ERR_ZERO_OBJECTS;
		return;
	}
	int centerIdx_test = addrMapping[surfacePoints[centerIdx].lineIdx][surfacePoints[centerIdx].ptIdx];
	//迭代一次
	projectionCenter.lineIdx = surfacePoints[centerIdx].lineIdx;
	projectionCenter.ptIdx = surfacePoints[centerIdx].ptIdx;
	int sLine = projectionCenter.lineIdx - 5;
	if (sLine < 0)
		sLine = 0;
	int eLine = projectionCenter.lineIdx + 5;
	if (eLine >= (int)scanLines.size())
		eLine = (int)scanLines.size() - 1;
	int sPtIdx = projectionCenter.ptIdx - 5;
	if (sPtIdx < 0)
		sPtIdx = 0;
	int ePtIdx = projectionCenter.ptIdx + 5;
	if (ePtIdx >= (int)scanLines[0].size())
		ePtIdx = (int)scanLines[0].size() - 1;

	int objLine = -1;
	int objPtIdx = -1;
	maxZ = -1;
	for (int line = sLine; line <= eLine; line++)
	{
		for (int ptIdx = sPtIdx; ptIdx <= ePtIdx; ptIdx++)
		{
			int idx_center = addrMapping[line][ptIdx];
			if (idx_center < 0)
				continue;

			int sL = line - 1;
			if (sL < 0)
				sL = 0;
			int eL = line + 1;
			if (eL >= (int)scanLines.size())
				eL = (int)scanLines.size() - 1;
			int sPt = ptIdx - 1;
			if (sPt < 0)
				sPt = 0;
			int ePt = ptIdx + 1;
			if (ePt >= (int)scanLines[0].size())
				ePt = (int)scanLines[0].size() - 1;
			int size = 0;
			double sumZ = 0;
			for (int i = sL; i <= eL; i++)
			{
				for (int j = sPt; j <= ePt; j++)
				{
					int idx = addrMapping[i][j];
					if (idx >= 0)
					{
						if (roiProjectionData[idx].point.z > 1e-4)
						{
							sumZ += roiProjectionData[idx].point.z;
							size++;
						}
					}
				}
			}
			if (size > 0)
			{
				sumZ = sumZ / size;
				if (maxZ < 0)
				{
					maxZ = sumZ;
					objLine = line;
					objPtIdx = ptIdx;
				}
				else if (((false == dirInverting) && (maxZ < sumZ)) || ((true == dirInverting) && (maxZ > sumZ)))
				{
					maxZ = sumZ;
					objLine = line;
					objPtIdx = ptIdx;
				}

			}
		}
	}
	if( (objLine >=0)&&(objPtIdx >=0))
		centerIdx = addrMapping[objLine][objPtIdx];
	//旋转回原坐标系
	SVzNL3DPoint surfaceCenter = _ptRotate(roiProjectionData[centerIdx].point, rotatePara.invRMatrix);
	//生成Rod信息
	SSX_rodPoseInfo a_rod;
	a_rod.center = surfaceCenter;
	a_rod.axialDir = P1_dir;
	screwInfo.push_back(a_rod);
	return;
}

double _getListMeanZ(std::vector< SVzNL3DPosition>& listData)
{
	if (listData.size() == 0)
		return 0;
	double meanZ = 0;
	for (int i = 0; i < (int)listData.size(); i++)
		meanZ += listData[i].pt3D.z;
	meanZ = meanZ / (double)listData.size();
	return meanZ;
}

SSG_ROIRectD _getListROI(std::vector< SVzNL3DPosition>& listData)
{
	if (listData.size() == 0)
		return { 0,0,0,0 };
	SSG_ROIRectD roi = { listData[0].pt3D.x, listData[0].pt3D.x, listData[0].pt3D.y, listData[0].pt3D.y};
	for (int i = 0; i < (int)listData.size(); i++)
	{
		roi.left = roi.left > listData[i].pt3D.x ? listData[i].pt3D.x : roi.left;
		roi.right = roi.right < listData[i].pt3D.x ? listData[i].pt3D.x : roi.right;
		roi.top = roi.top > listData[i].pt3D.y ? listData[i].pt3D.y : roi.top;
		roi.bottom = roi.bottom < listData[i].pt3D.y ? listData[i].pt3D.y : roi.bottom;
	}
	return roi;
}

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

//计算封闭序列（首尾相连）在XOY平面内前向角和后向角以及拐角。 前向角：序号+； 后向角：序号-
void _computeClosedPntListDirCorners(std::vector<SWD_polarPt>& polarPoints, double scale, std::vector< SSG_dirCornerAngle>& dirCornerAngles)
{
	int pntSize = (int)polarPoints.size();
	//std::fill(dirCornerAngles.begin(), dirCornerAngles.end(), SSG_dirCornerAngle{ 0, 0, 0, 0, 0, 0 });
	dirCornerAngles.resize(pntSize);
	for (int i = 0; i < pntSize; i++)
	{
		if (i == 68)
			int kkk = 1;
		memset(&dirCornerAngles[i], 0, sizeof(SSG_dirCornerAngle));
		SWD_polarPt& a_polarPt = polarPoints[i];
		//前向寻找
		int minus_i = -1;
		for (int loop = i - 1; loop >= i - pntSize; loop--)
		{
			int j = loop >= 0 ? loop : (loop + pntSize);
			double dist = sqrt(pow(polarPoints[i].x - polarPoints[j].x, 2) +
				pow(polarPoints[i].y - polarPoints[j].y, 2));
			if (dist >= scale)
			{
				minus_i = j;
				break;
			}
		}
		//后向寻找
		int plus_i = -1;
		for (int loop = i + 1; loop < i + pntSize; loop++)
		{
			int j = loop % pntSize;
			double dist = sqrt(pow(polarPoints[i].x - polarPoints[j].x, 2) +
				pow(polarPoints[i].y - polarPoints[j].y, 2));
			if (dist >= scale)
			{
				plus_i = j;
				break;
			}
		}
		//计算拐角
		if ((minus_i >= 0) && (plus_i >= 0))
		{
			double backwardAngle = atan2(polarPoints[i].y - polarPoints[minus_i].y, polarPoints[i].x - polarPoints[minus_i].x) * 180.0 / PI;
			double forwardAngle = atan2(polarPoints[plus_i].y - polarPoints[i].y, polarPoints[plus_i].x - polarPoints[i].x) * 180.0 / PI;
			dirCornerAngles[i].forwardAngle = forwardAngle;
			dirCornerAngles[i].backwardAngle = backwardAngle;

			double corner = forwardAngle - backwardAngle;
			if (corner < -180)
				corner += 360;
			else if (corner > 180)
				corner = corner - 360;

			dirCornerAngles[i].flag = 0;
			//过滤掉由于极角和方向角相近产生的轮廓点位序错误导致的corner错误
			double polarAngle = dirCornerAngles[i].point.angle;
			double angleDiff1 = abs(polarAngle - dirCornerAngles[i].forwardAngle);
			if (angleDiff1 > 180)
				angleDiff1 = 360 - angleDiff1;
			double angleDiff2 = abs(polarAngle - dirCornerAngles[i].backwardAngle);
			if (angleDiff2 > 180)
				angleDiff2 = 360 - angleDiff2;
			if ((angleDiff1 < 10) || (angleDiff2 < 10))  //过滤掉边与极线方向相近导致的波动
			{
				if (dirCornerAngles[i].corner > 160)  //乱序产生角度错误
				{
					dirCornerAngles[i].flag = -1;
				}
			}

			dirCornerAngles[i].corner = corner;  //图像坐标系与正常坐标系y方向相反，所以有“-”号
			dirCornerAngles[i].pntIdx = i;
			dirCornerAngles[i].forward_pntIdx = plus_i;
			dirCornerAngles[i].backward_pntIdx = minus_i;
			dirCornerAngles[i].point = polarPoints[i];
		}
	}
}

//提取corner极值（较早实现函数可以使用此函数进行代码优化）
void _searchPlusCornerPeaks(
	std::vector< SSG_dirCornerAngle>& corners,
	double cutAngleTh,
	std::vector< SSG_dirCornerAngle>& cornerPlusPeaks
)
{
	std::vector<SSG_dirCornerAngle> peakCorners;
	int cornerSize = (int)corners.size();
	//搜索拐角极值
	int _state = 0;
	int pre_i = -1;
	int sEdgePtIdx = -1;
	int eEdgePtIdx = -1;
	SSG_dirCornerAngle* pre_data = NULL;
	for (int i = 0, i_max = cornerSize; i < i_max; i++)
	{
		if (i == 451)
			int kkk = 1;
		SSG_dirCornerAngle* curr_data = &corners[i];
		if (curr_data->flag < 0)
			continue;

		if (NULL == pre_data)
		{
			sEdgePtIdx = i;
			eEdgePtIdx = i;
			pre_data = curr_data;
			pre_i = i;
			continue;
		}

		eEdgePtIdx = i;
		double cornerDiff = curr_data->corner - pre_data->corner;
		switch (_state)
		{
		case 0:  //初态
			if (cornerDiff < 0) //下降
			{
				_state = 2;
			}
			else if (cornerDiff > 0) //上升
			{
				_state = 1;
			}
			break;
		case 1: //上升
			if (cornerDiff < 0)   //下降
			{
				if (pre_data->corner > cutAngleTh)  //截角门限，滤除点波动导致的角度变化
					peakCorners.push_back(*pre_data);
				_state = 2;
			}
			break;
		case 2: //下降
			if (cornerDiff > 0)   //  上升
				_state = 1;
			break;
		default:
			_state = 0;
			break;
		}
		pre_data = curr_data;
		pre_i = i;
	}
	//注意：最后一个不处理，为基座位置

	//极小值点（峰顶）
	//极值比较，在尺度窗口下寻找局部极值点
	for (int i = 0, i_max = (int)peakCorners.size(); i < i_max; i++)
	{
		bool isPeak = true;
		SSG_dirCornerAngle& a_dirAngle = peakCorners[i];
		int curr_dist1 = a_dirAngle.pntIdx - a_dirAngle.backward_pntIdx;
		if (curr_dist1 < 0)
			curr_dist1 += cornerSize;
		int curr_dist2 = a_dirAngle.forward_pntIdx - a_dirAngle.pntIdx;
		if (curr_dist2 < 0)
			curr_dist2 += cornerSize;

		//minus方向寻找
		int minus_i = i;
		while (1)
		{
			minus_i--;
			if (minus_i < 0)
				minus_i += i_max;
			SSG_dirCornerAngle& minus_dirAngle = peakCorners[minus_i];
			int pntDist_0 = a_dirAngle.pntIdx - minus_dirAngle.pntIdx;
			if (pntDist_0 < 0)
				pntDist_0 += cornerSize;
			if (pntDist_0 > curr_dist1)
				break;

			if (a_dirAngle.corner < minus_dirAngle.corner)
			{
				isPeak = false;
				break;
			}
		}
		//plus方向寻找
		int plus_i = i;
		while (1)
		{
			plus_i++;
			if (plus_i >= i_max)
				plus_i = 0;
			SSG_dirCornerAngle& plus_dirAngle = peakCorners[plus_i];
			int pntDist_0 = plus_dirAngle.pntIdx - a_dirAngle.pntIdx;
			if (pntDist_0 < 0)
				pntDist_0 += cornerSize;
			if (pntDist_0 > curr_dist2)
				break;

			if (a_dirAngle.corner < plus_dirAngle.corner)
			{
				isPeak = false;
				break;
			}
		}
		if (true == isPeak)
		{
			double corner_curr = a_dirAngle.corner;
			double corner_1 = corners[a_dirAngle.forward_pntIdx].corner;
			double corner_2 = corners[a_dirAngle.backward_pntIdx].corner;
			double diff_1 = corner_curr - corner_1;
			double diff_2 = corner_curr - corner_2;
			if ((diff_1 > corner_curr / 4) && (diff_2 > corner_curr / 4))
			{
				double polarAngle = a_dirAngle.point.angle;
				double angleDiff1 = abs(polarAngle - a_dirAngle.forwardAngle);
				if (angleDiff1 > 180)
					angleDiff1 = 360 - angleDiff1;
				double angleDiff2 = abs(polarAngle - a_dirAngle.backwardAngle);
				if (angleDiff2 > 180)
					angleDiff2 = 360 - angleDiff2;
				if ((angleDiff1 < 10) || (angleDiff2 < 10))  //过滤掉边与极线方向相近导致的波动
				{
					if (a_dirAngle.corner < 160)  //乱序产生角度错误
					{
						a_dirAngle.flag = 0;
						cornerPlusPeaks.push_back(a_dirAngle);
					}
				}
				else
				{
					a_dirAngle.flag = 0;
					cornerPlusPeaks.push_back(a_dirAngle);
				}
			}
		}
	}
}

void _updateRoi3D(SVzNL3DRangeD& roi, SVzNL3DPoint& a_pt)
{
	if (a_pt.z > 1E-4)
	{
		if (roi.zRange.max < 0)
		{
			roi.xRange.min = a_pt.x;
			roi.xRange.max = a_pt.x;
			roi.yRange.min = a_pt.y;
			roi.yRange.max = a_pt.y;
			roi.zRange.min = a_pt.z;
			roi.zRange.max = a_pt.z;
		}
		else
		{
			if (roi.xRange.min > a_pt.x)
				roi.xRange.min = a_pt.x;
			if (roi.xRange.max < a_pt.x)
				roi.xRange.max = a_pt.x;
			if (roi.yRange.min > a_pt.y)
				roi.yRange.min = a_pt.y;
			if (roi.yRange.max < a_pt.y)
				roi.yRange.max = a_pt.y;
			if (roi.zRange.min > a_pt.z)
				roi.zRange.min = a_pt.z;
			if (roi.zRange.max < a_pt.z)
				roi.zRange.max = a_pt.z;
		}
	}
	return;
}

//计算定位盘中心点位姿
SSX_platePoseInfo sx_getLocationPlatePose(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SSG_cornerParam cornerPara,
	int* errCode)
{
	*errCode = 0;
	SSX_platePoseInfo resultPose;
	resultPose.center = { 0, 0, 0 };
	resultPose.normalDir = { 0, 0, 0 };
	resultPose.holeLT = { 0, 0, 0 };
	resultPose.holeRB = { 0, 0, 0 };
	resultPose.xDir = { 0, 0, 0 };
	resultPose.yDir = { 0, 0, 0 };
	int lineNum = (int)scanLines.size();
	if (lineNum == 0)
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return resultPose;
	}

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

	//判断数据格式是否为grid。算法只能处理grid数据格式
	bool isGridData = true;
	for (int line = 0; line < lineNum; line++)
	{
		if (linePtNum != (int)scanLines[line].size())
		{
			isGridData = false;
			break;
		}
	}
	if (false == isGridData)//数据不是网格格式
	{
		*errCode = SG_ERR_NOT_GRID_FORMAT;
		return resultPose;
	}

	//产生水平扫描数据
	std::vector< std::vector<SVzNL3DPosition>> scanLines_h;
	scanLines_h.resize(linePtNum);
	for (int i = 0; i < linePtNum; i++)
		scanLines_h[i].resize(lineNum);
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
			scanLines_h[j][line] = scanLines[line][j];
			scanLines_h[j][line].pt3D.x = scanLines[line][j].pt3D.y;
			scanLines_h[j][line].pt3D.y = scanLines[line][j].pt3D.x;
		}
	}
	for (int line = 0; line < linePtNum; line++)
	{
		for (int j = 0, j_max = (int)scanLines_h[line].size(); j < j_max; j++)
			scanLines_h[line][j].nPointIdx = j;
	}

	//获取定位盘外表面端点
	//垂直方向
	std::vector<std::vector<int>> flags;
	flags.resize(lineNum);
	for (int i = 0; i < lineNum; i++)
	{
		flags[i].resize(linePtNum);
		std::fill(flags[i].begin(), flags[i].end(), 0);
	}

	std::vector< SVzNL3DPosition> endingPts;
	for (int line = 0; line < lineNum; line++)
	{
		std::vector< SVzNL3DPosition> vldPts;
		std::vector<SSG_RUN_EX> segs;
		std::vector<int> backIndexing;
		backIndexing.resize(scanLines[line].size());
		wd_lineDataSegment_zDist(
			scanLines[line],
			vldPts,
			segs,
			backIndexing,
			cornerPara
		);
		for (int i = 0; i < (int)segs.size(); i++)
		{
			int idx_0 = segs[i].start;
			int idx_1 = segs[i].start + segs[i].len - 1;
			SVzNL3DPosition pt_0 = vldPts[idx_0];
			SVzNL3DPosition pt_1 = vldPts[idx_1];
			flags[line][pt_0.nPointIdx] = 1;
			flags[line][pt_1.nPointIdx] = 1;
			pt_0.nPointIdx = pt_0.nPointIdx & 0xffff | (line << 16);
			pt_1.nPointIdx = pt_1.nPointIdx & 0xffff | (line << 16);
			endingPts.push_back(pt_0);
			endingPts.push_back(pt_1);
		}
	}
	for (int line = 0; line < linePtNum; line++)
	{
		std::vector< SVzNL3DPosition> vldPts;
		std::vector<SSG_RUN_EX> segs;
		std::vector<int> backIndexing;
		backIndexing.resize(scanLines_h[line].size());
		wd_lineDataSegment_zDist(
			scanLines_h[line],
			vldPts,
			segs,
			backIndexing,
			cornerPara
		);
		for (int i = 0; i < (int)segs.size(); i++)
		{
			int idx_0 = segs[i].start;
			int idx_1 = segs[i].start + segs[i].len - 1;
			SVzNL3DPosition pt_0 = vldPts[idx_0];
			SVzNL3DPosition pt_1 = vldPts[idx_1];
			if (0 == flags[pt_0.nPointIdx][line])//不和垂直特征提取的点重复
			{
				flags[pt_0.nPointIdx][line] = 1;
				pt_0.pt3D = scanLines[pt_0.nPointIdx][line].pt3D;  //取原始点
				pt_0.nPointIdx = (pt_0.nPointIdx <<16) | (line & 0xffff);
				endingPts.push_back(pt_0);
			}
			if (0 == flags[pt_1.nPointIdx][line])  //不和垂直特征提取的点重复
			{
				flags[pt_1.nPointIdx][line] = 1;
				pt_1.pt3D = scanLines[pt_1.nPointIdx][line].pt3D;  //取原始点
				pt_1.nPointIdx = (pt_1.nPointIdx << 16) | (line & 0xffff);
				endingPts.push_back(pt_1);
			}
		}
	}
	//标注
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
			scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
	}
	for (int i = 0; i < (int)endingPts.size(); i++)
	{
		int line = endingPts[i].nPointIdx >> 16;
		int ptIdx = endingPts[i].nPointIdx & 0x0000FFFF;
		scanLines[line][ptIdx].nPointIdx = 1;
	}

	//聚类
	//内部参数
	int clusterCheckWin = 5;
	double clusterDist = 5.0;
	double topLayerThickness = 10.0;
	double centerZ_R = 20.0;
	int distType = 1;  //0 - 2d distance; 1- 3d distance 
	std::vector<std::vector< SVzNL3DPosition>> objClusters;  //result
	wd_pointClustering_speedUp(
		endingPts,
		lineNum, linePtNum, clusterCheckWin, //搜索窗口
		clusterDist,
		distType,
		objClusters  //result
	);

	//判断上表面外轮廓
	std::vector<double> objMeanZ;
	objMeanZ.resize(objClusters.size());
	std::vector< SSG_ROIRectD> objROIs;
	objROIs.resize(objClusters.size());
	//过滤出最小范围的Z
	double minMeanZ = -1;
	for (int i = 0; i < (int)objClusters.size(); i++)
	{
		SSG_ROIRectD a_roi = _getListROI(objClusters[i]);
		objROIs[i] = a_roi;
		double w = a_roi.right - a_roi.left;
		double h = a_roi.bottom - a_roi.top;

		double meanZ = _getListMeanZ(objClusters[i]);
		objMeanZ[i] = meanZ;
		if ( (meanZ >  1e-4) && (w > 150) && (h > 100))
		{
			if (minMeanZ < 0)
				minMeanZ = meanZ;
			else
				minMeanZ = minMeanZ > meanZ ? meanZ : minMeanZ;
		}
	}
	//选出ROI最大的目标作为顶部轮廓
	int objIdx = -1;
	double maxSize = 0;
	for (int i = 0; i < (int)objClusters.size(); i++)
	{
		if (objMeanZ[i] > 1e-4)
		{
			double zDist = objMeanZ[i] - minMeanZ;
			if (zDist < topLayerThickness) //同在顶层
			{
				double size = (objROIs[i].right - objROIs[i].left) * (objROIs[i].bottom - objROIs[i].top);
				if (maxSize < -1e-4)
				{
					maxSize = size;
					objIdx = i;
				}
				else
				{
					if (maxSize < size)
					{
						maxSize = size;
						objIdx = i;
					}
				}
			}
		}
	}

	if (objIdx < 0)
	{
		*errCode = SG_ERR_ZERO_OBJECTS;
		return resultPose;
	}
	//标注
	for (int i = 0; i < (int)objClusters[objIdx].size(); i++)
	{
		int line = objClusters[objIdx][i].nPointIdx >> 16;
		int ptIdx = objClusters[objIdx][i].nPointIdx & 0x0000FFFF;
		scanLines[line][ptIdx].nPointIdx = 2;
	}

	//拟合平面
	std::vector<cv::Point3f> Points3ds;
	std::vector<SWD_polarPt> polarPoints;
	for (int i = 0; i < (int)objClusters[objIdx].size(); i++)
	{
		cv::Point3f a_pt = cv::Point3f(objClusters[objIdx][i].pt3D.x, objClusters[objIdx][i].pt3D.y, objClusters[objIdx][i].pt3D.z);
		Points3ds.push_back(a_pt);
		SWD_polarPt a_polarPt;
		a_polarPt.lineIdx = objClusters[objIdx][i].nPointIdx >> 16;
		a_polarPt.ptIdx = objClusters[objIdx][i].nPointIdx & 0x0000FFFF;
		a_polarPt.R = 0;
		a_polarPt.angle = 0;
		a_polarPt.x = objClusters[objIdx][i].pt3D.x;
		a_polarPt.y = objClusters[objIdx][i].pt3D.y;
		a_polarPt.z = objClusters[objIdx][i].pt3D.z;
		polarPoints.push_back(a_polarPt);
	}
	//计算面参数: z = Ax + By + C
	//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
#if 1	
	std::vector<cv::Point3f> out_inliers;
	Plane res = ransacFitPlane(	Points3ds,	out_inliers	);
	if (res.C < 0)
	{
		res.A = -res.A;
		res.B = -res.B;
		res.C = -res.C;
		res.D = -res.D;
	}
#else
	Plane res = robustFitPlane(Points3ds);
#endif
	//std::vector<double> res;
	//vzCaculateLaserPlane(Points3ds, res);
	//计算投影向量
	SVzNL3DPoint vec_1 = {res.A, res.B, res.C};
	SVzNL3DPoint vec_2 = { 0, 0, 1.0 };
	SSG_planeCalibPara poseR = wd_computeRTMatrix(vec_1, vec_2);

	//投影
	double normDataPlane = sqrt(res.A * res.A + res.B * res.B + res.C * res.C);
	std::vector<cv::Point3f> projectPoints3ds;
	projectPoints3ds.resize(Points3ds.size());
	double sum_x = 0, sum_y = 0, sum_z = 0;
	double sumZConter = 0;
	for (int i = 0; i < (int)Points3ds.size(); i++)
	{
		double distToPlane = abs(res.A * Points3ds[i].x + res.B * Points3ds[i].y + res.C * Points3ds[i].z + res.D) / normDataPlane;
		double x = Points3ds[i].x * poseR.planeCalib[0] + Points3ds[i].y * poseR.planeCalib[1] + Points3ds[i].z * poseR.planeCalib[2];
		double y = Points3ds[i].x * poseR.planeCalib[3] + Points3ds[i].y * poseR.planeCalib[4] + Points3ds[i].z * poseR.planeCalib[5];
		double z = Points3ds[i].x * poseR.planeCalib[6] + Points3ds[i].y * poseR.planeCalib[7] + Points3ds[i].z * poseR.planeCalib[8];
		projectPoints3ds[i] = cv::Point3f(x, y, z);
		polarPoints[i].x = x;
		polarPoints[i].y = y;
		polarPoints[i].z = z;
		sum_x += x;
		sum_y += y;

		if (distToPlane < 2.0)
		{
			sum_z += z;
			sumZConter++;
		}
		
	}

	for (int i = 0; i < lineNum; i++)
	{
		if (i == 14)
			int kkk = 1;
		//行处理
		//调平，去除地面
		lineDataRT_vector(scanLines[i], poseR.planeCalib, -1);
	}

	//计算质心
	double center_x = sum_x / (double)Points3ds.size();
	double center_y = sum_y / (double)Points3ds.size();
	double center_z = sum_z / (double)sumZConter;
	//计算极坐标的R和Theta
	for (int pi = 0; pi < (int)polarPoints.size(); 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 < (int)polarPoints.size(); pi++)
		polarPoints[pi].cptIndex = pi;  // index

	//计算有序边缘点的角度变化 2026.03.06
	std::vector< SSG_dirCornerAngle> dirCornerAngles;
	double dirAngleScale = 20.0;
	_computeClosedPntListDirCorners(polarPoints, dirAngleScale, dirCornerAngles);
	//提取方向角拐点(正）极值
	std::vector< SSG_dirCornerAngle> cornerPlusPeaks;
	double minCutAngleTh = 30;
	_searchPlusCornerPeaks(dirCornerAngles, minCutAngleTh, cornerPlusPeaks);

#if 1
	int cornerIdx_LT = -1;
	int cornerIdx_RB = -1;
	for (int i = 0; i < (int)cornerPlusPeaks.size(); i++)
	{
		if ((cornerPlusPeaks[i].point.angle > 0) && (cornerPlusPeaks[i].point.angle < 90))
			cornerIdx_LT = i;
		if ((cornerPlusPeaks[i].point.angle > 180) && (cornerPlusPeaks[i].point.angle < 270))
			cornerIdx_RB = i;
	}
	if ((cornerIdx_LT < 0) || (cornerIdx_RB < 0))
	{
		*errCode = SX_ERR_UNKNOWN_PLATE_DIR;
		return resultPose;
	}
	SSG_dirCornerAngle& corner_LT = cornerPlusPeaks[cornerIdx_LT];
	SSG_dirCornerAngle& corner_RB = cornerPlusPeaks[cornerIdx_RB];

	//Z切割
	//内部参数
	double zTh_min = center_z - 1.0;
	double zTh_max = center_z + 5.0;
	std::vector< std::vector<SVzNL3DPosition>> zCutLines;
	zCutLines.resize(scanLines.size());
	for (int line = 0; line < lineNum; line++)
	{
		//行处理
		zCutLines[line].resize(linePtNum);
		for (int j = 0; j < linePtNum; j++)
		{
			zCutLines[line][j] = scanLines[line][j];
			if ((scanLines[line][j].pt3D.z < zTh_min) || (scanLines[line][j].pt3D.z > zTh_max))
				zCutLines[line][j].pt3D = { 0.0, 0.0, 0.0 };
		}
	}
	//孔处理
	std::vector<SSG_intPair> validObjects;
	std::vector<SWD_segFeatureTree> holeSegTrees_v;
	std::vector<SWD_segFeatureTree> holeSegTrees_h;
	SSG_lineSegParam hole_lineSegPara;
	hole_lineSegPara.distScale = 3.0;
	hole_lineSegPara.segGapTh_y = 15.0;  //
	hole_lineSegPara.segGapTh_z = 0.0; //z方向间隔大于10mm认为是分段
	SSG_outlierFilterParam hole_filterParam;
	hole_filterParam.continuityTh = 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声
	hole_filterParam.outlierTh = 5;
	SSG_treeGrowParam hole_growParam;
	hole_growParam.maxLineSkipNum = 2;
	hole_growParam.yDeviation_max = 4.0;
	hole_growParam.maxSkipDistance = 2.0;
	hole_growParam.zDeviation_max = 10.0;// 
	hole_growParam.minLTypeTreeLen = 2.0; //mm
	hole_growParam.minVTypeTreeLen = 2.0; //mm
	double valieCommonNumRatio = 0.25;
	WD_getHoleInfo(
		zCutLines,
		hole_lineSegPara,
		hole_filterParam,
		hole_growParam,
		valieCommonNumRatio,
		holeSegTrees_v,
		holeSegTrees_h,
		validObjects
	);

	//寻找左上孔和右下孔
	for (int i = 0; i < lineNum; i++)
	{
		for (int j = 0; j < (int)scanLines[i].size(); j++)
			zCutLines[i][j].nPointIdx = 0;  //清零
	}
	//生成聚类信息， 
	std::vector<std::vector< SVzNL2DPoint>> clusters; //只记录位置
	std::vector<SVzNL3DRangeD> clustersRoi3D;
	for (int i = 0; i < (int)validObjects.size(); i++)
	{
		std::vector< SVzNL2DPoint> a_cluster;
		SVzNL3DRangeD a_roi3D = { {-1, -1}, {-1, -1}, {-1, -1 } };

		int vTreeIdx = validObjects[i].data_0;
		int hTreeIdx = validObjects[i].data_1;
		for (int m = 0; m < (int)holeSegTrees_v[vTreeIdx].treeNodes.size(); m++)
		{
			SWD_segFeature& a_seg = holeSegTrees_v[vTreeIdx].treeNodes[m];
			if (zCutLines[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx == 0)
			{
				zCutLines[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = vTreeIdx + 1; // 0x01;
				scanLines[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = vTreeIdx + 3; // 0x01;
				SVzNL2DPoint a_pos = { a_seg.lineIdx , a_seg.endPtIdx };
				a_cluster.push_back(a_pos);
				_updateRoi3D(a_roi3D, zCutLines[a_seg.lineIdx][a_seg.endPtIdx].pt3D);
			}
			if (zCutLines[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx == 0)
			{
				zCutLines[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = vTreeIdx + 1; // 0x01;
				scanLines[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = vTreeIdx + 3; // 0x01;
				SVzNL2DPoint a_pos = { a_seg.lineIdx , a_seg.startPtIdx };
				a_cluster.push_back(a_pos);
				_updateRoi3D(a_roi3D, zCutLines[a_seg.lineIdx][a_seg.startPtIdx].pt3D);
			}
		}
		for (int m = 0; m < (int)holeSegTrees_h[hTreeIdx].treeNodes.size(); m++)
		{
			SWD_segFeature& a_seg = holeSegTrees_h[hTreeIdx].treeNodes[m];
			if (zCutLines[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx == 0)
			{
				zCutLines[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx = hTreeIdx + 1; // 0x02;
				scanLines[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx = hTreeIdx + 4; // 0x02;
				SVzNL2DPoint a_pos = { a_seg.startPtIdx , a_seg.lineIdx };
				a_cluster.push_back(a_pos);
				_updateRoi3D(a_roi3D, zCutLines[a_seg.startPtIdx][a_seg.lineIdx].pt3D);
			}
			if (zCutLines[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx == 0)
			{
				zCutLines[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx = hTreeIdx + 1; // 0x02;
				scanLines[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx = hTreeIdx + 4; // 0x02;
				SVzNL2DPoint a_pos = { a_seg.endPtIdx , a_seg.lineIdx };
				a_cluster.push_back(a_pos);
				_updateRoi3D(a_roi3D, zCutLines[a_seg.endPtIdx][a_seg.lineIdx].pt3D);
			}
		}
		clusters.push_back(a_cluster);
		clustersRoi3D.push_back(a_roi3D);
	}

	//聚类结果分析:搜索距左上和右下点最后的目标
	//内部参数
	double minHoleSize = 10.0;
	double maxHoleSize = 24.0;
	double maxCornerHoleDistance = 40; //角点到孔的最大距离 
	int clusterSize = (int)clusters.size();
	int clusterIdx_LT = -1;
	double minDistLT = -1;
	int clusterIdx_RB = -1;
	double minDistRB = -1;
	for (int i = 0; i < clusterSize; i++)
	{
		SVzNL3DRangeD& a_roi = clustersRoi3D[i];
		double L = a_roi.xRange.max - a_roi.xRange.min;
		double W = a_roi.yRange.max - a_roi.yRange.min;
		double cx = (a_roi.xRange.max + a_roi.xRange.min) / 2;
		double cy = (a_roi.yRange.max + a_roi.yRange.min) / 2;

		if ((L > minHoleSize) && (L < maxHoleSize) && (W > minHoleSize) && (W < maxHoleSize))
		{
			double dist_1 = sqrt(pow(corner_LT.point.x - cx, 2) + pow(corner_LT.point.y - cy, 2));
			if ((minDistLT < 0) || (minDistLT > dist_1))
			{
				minDistLT = dist_1;
				clusterIdx_LT = i;
			}
			
			double dist_2 = sqrt(pow(corner_RB.point.x - cx, 2) + pow(corner_RB.point.y - cy, 2));
			if ((minDistRB < 0) || (minDistRB > dist_2))
			{
				minDistRB = dist_2;
				clusterIdx_RB = i;
			}
		}
	}
	if( (clusterIdx_LT < 0) || (clusterIdx_RB < 0) || (minDistLT > maxCornerHoleDistance) || (minDistRB > maxCornerHoleDistance))
	{
		*errCode = SX_ERR_UNKNOWN_PLATE_DIR;
		return resultPose;
	}

	//目标圆拟合
	std::vector<SVzNL3DPoint> pointArrayLT;
	int clusterPtSizeLT = (int)clusters[clusterIdx_LT].size();
	for (int i = 0; i < clusterPtSizeLT; i++)
	{
		SVzNL2DPoint a_pos = clusters[clusterIdx_LT][i];
		int lineIdx = a_pos.x;
		int ptIdx = a_pos.y;
		SVzNL3DPoint a_pt3d = scanLines[lineIdx][ptIdx].pt3D;
		pointArrayLT.push_back(a_pt3d);
	}
	//圆拟合
	SVzNL3DPoint centerLT;
	double radiusLT;
	double err = fitCircleByLeastSquare(pointArrayLT, centerLT, radiusLT);
	centerLT.z = center_z;

	std::vector<SVzNL3DPoint> pointArrayRB;
	int clusterPtSizeRB = (int)clusters[clusterIdx_RB].size();
	for (int i = 0; i < clusterPtSizeRB; i++)
	{
		SVzNL2DPoint a_pos = clusters[clusterIdx_RB][i];
		int lineIdx = a_pos.x;
		int ptIdx = a_pos.y;
		SVzNL3DPoint a_pt3d = scanLines[lineIdx][ptIdx].pt3D;
		pointArrayRB.push_back(a_pt3d);
	}
	//圆拟合
	SVzNL3DPoint centerRB;
	double radiusRB;
	err = fitCircleByLeastSquare(pointArrayRB, centerRB, radiusRB);
	centerRB.z = center_z;

	SVzNL3DPoint refVec = { (centerRB.x-centerLT.x)/2, (centerRB.y - centerLT.y)/2, center_z};
	double angleToHorizon = -46;  //两孔连线与水平线夹角46度
	double angleToCenter = 3; //两孔连线中点与定位盘中心夹角3度
	//逆时针旋转时 θ > 0 ；顺时针旋转时 θ < 0 
	SVzNL3DPoint rVec_1 = wd_rotate2D(refVec, angleToCenter);
	center_x = rVec_1.x + centerLT.x;
	center_y = rVec_1.y + centerLT.y;
	//center_z = center_z + 34.0; 
	SVzNL3DPoint xDir = wd_rotate2D(refVec, angleToHorizon);
	double normData = sqrt(pow(xDir.x, 2) + pow(xDir.y, 2));
	xDir.x = xDir.x / normData;
	xDir.y = xDir.y / normData;
	xDir.z = 0;

	resultPose.holeLT = centerLT;
	resultPose.holeRB = centerRB;

	center_z += 33.8; //定位盘中心点深33.8
#else
	//生成直线段数据
	int cornerIdx_0 = -1;
	int cornerIdx_1 = -1;
	for (int i = 0; i < (int)cornerPlusPeaks.size(); i++)
	{
		if ((cornerPlusPeaks[i].point.angle > 0) && (cornerPlusPeaks[i].point.angle < 90))
			cornerIdx_0 = i;
		if ((cornerPlusPeaks[i].point.angle > 90) && (cornerPlusPeaks[i].point.angle < 180))
			cornerIdx_1 = i;
	}
	if ((cornerIdx_0 < 0) && (cornerIdx_1 < 0))
	{
		if (objIdx < 0)
		{
			*errCode = SX_ERR_UNKNOWN_PLATE_DIR;
			return resultPose;
		}
	}
	std::vector<SVzNL3DPoint> lineFittingPts;
	double endingRemoveDist = 10.0;
	double validFittingLen = 100;
	if (cornerIdx_0 >= 0)
	{
		int startIdx = cornerPlusPeaks[cornerIdx_0].pntIdx;
		double sx = polarPoints[startIdx].x;
		double sy = polarPoints[startIdx].y;
		for (int m = startIdx; m < (int)polarPoints.size(); m++)
		{
			double dist = sqrt(pow(polarPoints[m].x - sx, 2) + pow(polarPoints[m].y - sy, 2));
			if ((dist >= endingRemoveDist) && (dist <= (validFittingLen + endingRemoveDist)))
			{
				SVzNL3DPoint a_fitPt = { polarPoints[m].x, polarPoints[m].y, polarPoints[m].z };
				lineFittingPts.push_back(a_fitPt);
			}
			if (dist > (validFittingLen + endingRemoveDist))
				break;
		}
	}
	else
	{
		int startIdx = cornerPlusPeaks[cornerIdx_1].pntIdx;
		double sx = polarPoints[startIdx].x;
		double sy = polarPoints[startIdx].y;
		for (int m = startIdx; m >= 0; m--)
		{
			double dist = sqrt(pow(polarPoints[m].x - sx, 2) + pow(polarPoints[m].y - sy, 2));
			if ((dist >= endingRemoveDist) && (dist <= (validFittingLen + endingRemoveDist)))
			{
				SVzNL3DPoint a_fitPt = { polarPoints[m].x, polarPoints[m].y, polarPoints[m].z };
				lineFittingPts.push_back(a_fitPt);
			}
			if (dist > (validFittingLen + endingRemoveDist))
				break;
		}
	}
	//直线拟合
	double _k, _b;
	lineFitting(lineFittingPts, &_k, &_b);
	double normData = sqrt(pow(_k, 2) + 1);
	SVzNL3DPoint xDir = { 1.0/normData, _k/normData, 0.0 };

	//计算4个点
	SVzNL2DPointD top_pt = { center_x, center_y - centerZ_R };
	SVzNL2DPointD bottom_pt = { center_x, center_y + centerZ_R };
	SVzNL2DPointD left_pt = { center_x - centerZ_R, center_y};
	SVzNL2DPointD roght_pt = { center_x + centerZ_R, center_y};
	//计算Z
	SVzNL3DPoint ptTop = { 0, 0, 0 };
	SVzNL3DPoint ptBtm = { 0, 0, 0 };
	SVzNL3DPoint ptLeft = { 0, 0, 0 };
	SVzNL3DPoint ptRight = { 0, 0, 0 };
	double minDistTop = -1, minDistBtm = -1, minDistLeft = -1, minDistRight = -1;
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			if (scanLines[line][j].pt3D.z > 1e-4)
			{
				//top
				if (minDistTop < 1e-4)
				{
					ptTop = scanLines[line][j].pt3D;
					minDistTop = sqrt(pow(top_pt.x - scanLines[line][j].pt3D.x, 2) + pow(top_pt.y - scanLines[line][j].pt3D.y, 2));
				}
				else
				{
					double dist = sqrt(pow(top_pt.x - scanLines[line][j].pt3D.x, 2) + pow(top_pt.y - scanLines[line][j].pt3D.y, 2));
					if (minDistTop > dist)
					{
						ptTop = scanLines[line][j].pt3D;
						minDistTop = dist;
					}
				}
				//bottom
				if (minDistBtm < 1e-4)
				{
					ptBtm = scanLines[line][j].pt3D;
					minDistBtm = sqrt(pow(bottom_pt.x - scanLines[line][j].pt3D.x, 2) + pow(bottom_pt.y - scanLines[line][j].pt3D.y, 2));
				}
				else
				{
					double dist = sqrt(pow(bottom_pt.x - scanLines[line][j].pt3D.x, 2) + pow(bottom_pt.y - scanLines[line][j].pt3D.y, 2));
					if (minDistBtm > dist)
					{
						ptBtm = scanLines[line][j].pt3D;
						minDistBtm = dist;
					}
				}
				//left
				if (minDistLeft < 1e-4)
				{
					ptLeft = scanLines[line][j].pt3D;
					minDistLeft = sqrt(pow(left_pt.x - scanLines[line][j].pt3D.x, 2) + pow(left_pt.y - scanLines[line][j].pt3D.y, 2));
				}
				else
				{
					double dist = sqrt(pow(left_pt.x - scanLines[line][j].pt3D.x, 2) + pow(left_pt.y - scanLines[line][j].pt3D.y, 2));
					if (minDistLeft > dist)
					{
						ptLeft = scanLines[line][j].pt3D;
						minDistLeft = dist;
					}
				}
				//right
				if (minDistRight < 1e-4)
				{
					ptRight = scanLines[line][j].pt3D;
					minDistRight = sqrt(pow(roght_pt.x - scanLines[line][j].pt3D.x, 2) + pow(roght_pt.y - scanLines[line][j].pt3D.y, 2));
				}
				else
				{
					double dist = sqrt(pow(roght_pt.x - scanLines[line][j].pt3D.x, 2) + pow(roght_pt.y - scanLines[line][j].pt3D.y, 2));
					if (minDistRight > dist)
					{
						ptRight = scanLines[line][j].pt3D;
						minDistRight = dist;
					}
				}
			}
		}
	}
	double center_z = (ptTop.z + ptBtm.z + ptLeft.z + ptRight.z) / 4;
#endif
	resultPose.center = { center_x, center_y, center_z  };
	resultPose.xDir = { 0.0, 0.0, -1.0 }; // { 1.0, 0.0, 0 };
	//resultPose.yDir		= { 0.0, 1.0, 0 };
	resultPose.normalDir = xDir; // { 1.0, 0.0, 0 };// { 0.0, 0.0, 1.0 };
	//叉乘出y;
	//向量叉乘
	resultPose.yDir = vec3_cross(resultPose.normalDir, resultPose.xDir);

	//旋转回去
	for (int i = 0; i < lineNum; i++)
	{
		//行处理
		//调平，去除地面
		lineDataRT_vector(scanLines[i], poseR.invRMatrix, -1);
	}
	double x = resultPose.center.x * poseR.invRMatrix[0] + resultPose.center.y * poseR.invRMatrix[1] + resultPose.center.z * poseR.invRMatrix[2];
	double y = resultPose.center.x * poseR.invRMatrix[3] + resultPose.center.y * poseR.invRMatrix[4] + resultPose.center.z * poseR.invRMatrix[5];
	double z = resultPose.center.x * poseR.invRMatrix[6] + resultPose.center.y * poseR.invRMatrix[7] + resultPose.center.z * poseR.invRMatrix[8];
	resultPose.center = { x, y, z };
	x = resultPose.normalDir.x * poseR.invRMatrix[0] + resultPose.normalDir.y * poseR.invRMatrix[1] + resultPose.normalDir.z * poseR.invRMatrix[2];
	y = resultPose.normalDir.x * poseR.invRMatrix[3] + resultPose.normalDir.y * poseR.invRMatrix[4] + resultPose.normalDir.z * poseR.invRMatrix[5];
	z = resultPose.normalDir.x * poseR.invRMatrix[6] + resultPose.normalDir.y * poseR.invRMatrix[7] + resultPose.normalDir.z * poseR.invRMatrix[8];
	resultPose.normalDir = { x, y, z };

	x = resultPose.xDir.x * poseR.invRMatrix[0] + resultPose.xDir.y * poseR.invRMatrix[1] + resultPose.xDir.z * poseR.invRMatrix[2];
	y = resultPose.xDir.x * poseR.invRMatrix[3] + resultPose.xDir.y * poseR.invRMatrix[4] + resultPose.xDir.z * poseR.invRMatrix[5];
	z = resultPose.xDir.x * poseR.invRMatrix[6] + resultPose.xDir.y * poseR.invRMatrix[7] + resultPose.xDir.z * poseR.invRMatrix[8];
	resultPose.xDir = { x, y, z };

	x = resultPose.yDir.x * poseR.invRMatrix[0] + resultPose.yDir.y * poseR.invRMatrix[1] + resultPose.yDir.z * poseR.invRMatrix[2];
	y = resultPose.yDir.x * poseR.invRMatrix[3] + resultPose.yDir.y * poseR.invRMatrix[4] + resultPose.yDir.z * poseR.invRMatrix[5];
	z = resultPose.yDir.x * poseR.invRMatrix[6] + resultPose.yDir.y * poseR.invRMatrix[7] + resultPose.yDir.z * poseR.invRMatrix[8];
	resultPose.yDir = { x, y, z };

	x = resultPose.holeLT.x * poseR.invRMatrix[0] + resultPose.holeLT.y * poseR.invRMatrix[1] + resultPose.holeLT.z * poseR.invRMatrix[2];
	y = resultPose.holeLT.x * poseR.invRMatrix[3] + resultPose.holeLT.y * poseR.invRMatrix[4] + resultPose.holeLT.z * poseR.invRMatrix[5];
	z = resultPose.holeLT.x * poseR.invRMatrix[6] + resultPose.holeLT.y * poseR.invRMatrix[7] + resultPose.holeLT.z * poseR.invRMatrix[8];
	resultPose.holeLT = { x, y, z };

	x = resultPose.holeRB.x * poseR.invRMatrix[0] + resultPose.holeRB.y * poseR.invRMatrix[1] + resultPose.holeRB.z * poseR.invRMatrix[2];
	y = resultPose.holeRB.x * poseR.invRMatrix[3] + resultPose.holeRB.y * poseR.invRMatrix[4] + resultPose.holeRB.z * poseR.invRMatrix[5];
	z = resultPose.holeRB.x * poseR.invRMatrix[6] + resultPose.holeRB.y * poseR.invRMatrix[7] + resultPose.holeRB.z * poseR.invRMatrix[8];
	resultPose.holeRB = { x, y, z };

	return resultPose;
}

//新的计算定位盘中心点位姿（去除了定位盘，只有定位板）
SSX_platePoseInfo sx_getLocationPlatePose_new(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SSG_cornerParam cornerPara,
	int* errCode)
{
	*errCode = 0;
	SSX_platePoseInfo resultPose;
	resultPose.center = { 0, 0, 0 };
	resultPose.normalDir = { 0, 0, 0 };
	resultPose.holeLT = { 0, 0, 0 };
	resultPose.holeRB = { 0, 0, 0 };
	resultPose.xDir = { 0, 0, 0 };
	resultPose.yDir = { 0, 0, 0 };
	int lineNum = (int)scanLines.size();
	if (lineNum == 0)
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return resultPose;
	}

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

	//判断数据格式是否为grid。算法只能处理grid数据格式
	bool isGridData = true;
	for (int line = 0; line < lineNum; line++)
	{
		if (linePtNum != (int)scanLines[line].size())
		{
			isGridData = false;
			break;
		}
	}
	if (false == isGridData)//数据不是网格格式
	{
		*errCode = SG_ERR_NOT_GRID_FORMAT;
		return resultPose;
	}

	//产生水平扫描数据
	std::vector< std::vector<SVzNL3DPosition>> scanLines_h;
	scanLines_h.resize(linePtNum);
	for (int i = 0; i < linePtNum; i++)
		scanLines_h[i].resize(lineNum);
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
			scanLines_h[j][line] = scanLines[line][j];
			scanLines_h[j][line].pt3D.x = scanLines[line][j].pt3D.y;
			scanLines_h[j][line].pt3D.y = scanLines[line][j].pt3D.x;
		}
	}
	for (int line = 0; line < linePtNum; line++)
	{
		for (int j = 0, j_max = (int)scanLines_h[line].size(); j < j_max; j++)
			scanLines_h[line][j].nPointIdx = j;
	}

	//算法流程：
	//1、检查垂直方向数据并去除
	//2、聚类
	//3、保留最前面目标
	//4、提取孔
	//5、拟合
	//6、计算中间坐标

	std::vector<std::vector<int>> flags;
	flags.resize(lineNum);
	for (int i = 0; i < lineNum; i++)
	{
		flags[i].resize(linePtNum);
		std::fill(flags[i].begin(), flags[i].end(), 0);
	}
	std::vector<std::vector<SSG_RUN>> zVertivalSegs;
	for (int line = 0; line < lineNum; line++)
	{
		if (line == 700)
			int kkk = 1;
		std::vector<SSG_RUN> line_verticalSegs;
		wd_getXYVertialFeature_peakCornerMethod(
			scanLines[line],
			line,
			cornerPara.scale,
			cornerPara,
			line_verticalSegs
		);
		zVertivalSegs.push_back(line_verticalSegs);

		for (int i = 0; i < (int)line_verticalSegs.size(); i++)
		{
			int idx_0 = line_verticalSegs[i].start;
			for (int j = 0; j < line_verticalSegs[i].len; j++)
				flags[line][idx_0 + j] = 1;
		}
	}

	std::vector<std::vector<SSG_RUN>> zVertivalSegs_h;
	for (int line = 0; line < linePtNum; line++)
	{
		if (line == 1177)
			int kkk = 1;
		std::vector<SSG_RUN> line_verticalSegs;
		wd_getXYVertialFeature_peakCornerMethod(
			scanLines_h[line],
			line,
			cornerPara.scale,
			cornerPara,
			line_verticalSegs
		);
		zVertivalSegs_h.push_back(line_verticalSegs);

		for (int i = 0; i < (int)line_verticalSegs.size(); i++)
		{
			int idx_0 = line_verticalSegs[i].start;
			for (int j = 0; j < line_verticalSegs[i].len; j++)
				flags[idx_0 + j][line] = 1;
		}
	}
	//标注
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
			scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
	}
	//将垂直线段去除
	std::vector< SVzNL3DPosition> validPoints;
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			if (flags[line][j] > 0)
				scanLines[line][j].pt3D.z = 0;

			if (scanLines[line][j].pt3D.z > 1e-4)
			{
				SVzNL3DPosition a_vldPt;
				a_vldPt.pt3D = scanLines[line][j].pt3D;
				a_vldPt.nPointIdx = (line << 16) | (j & 0xffff);
				validPoints.push_back(a_vldPt);
			}
		}
	}

	//聚类
	//内部参数
	double minObjSize_w = 150;
	double minObjSize_h = 150;

	int clusterCheckWin = 5;
	double clusterDist = 5.0;
	int distType = 1;  //0 - 2d distance; 1- 3d distance 
	std::vector<std::vector< SVzNL3DPosition>> objClusters;  //result
	wd_pointClustering_speedUp(
		validPoints,
		lineNum, linePtNum, clusterCheckWin, //搜索窗口
		clusterDist,
		distType,
		objClusters  //result
	);

	//保留最前面的目标
	const double topPlateMinW = 150;
	const double topPlateMinH = 150;
	std::vector<double> objMeanZ;
	objMeanZ.resize(objClusters.size());
	for (int i = 0; i < (int)objClusters.size(); i++)
	{
		SSG_ROIRectD a_roi = _getListROI(objClusters[i]);
		double w = a_roi.right - a_roi.left;
		double h = a_roi.bottom - a_roi.top;
		if ((w > topPlateMinW) && (h > topPlateMinH))
		{

			double meanZ = _getListMeanZ(objClusters[i]);
			objMeanZ[i] = meanZ;
		}
		else
			objMeanZ[i] = 0; 0;
	}

	//选出z最小的目标作为顶部轮廓
	int objIdx = -1;
	double minMeanZ = DBL_MAX;
	for (int i = 0; i < (int)objClusters.size(); i++)
	{
		if (objMeanZ[i] > 1e-4)
		{
			if (minMeanZ > objMeanZ[i])
			{
				minMeanZ = objMeanZ[i];
				objIdx = i;
			}
		}
	}
	if (objIdx < 0)
	{
		*errCode = SG_ERR_ZERO_OBJECTS;
		return resultPose;
	}

	std::vector< SVzNL3DPosition>& topCluster = objClusters[objIdx];
	//标注
	//重新将flags设置为目标的mask
	for (int i = 0; i < lineNum; i++)
		std::fill(flags[i].begin(), flags[i].end(), -1);
	for (int i = 0; i < (int)topCluster.size(); i++)
	{
		int line = topCluster[i].nPointIdx >> 16;
		int ptIdx = topCluster[i].nPointIdx & 0x0000FFFF;
		scanLines[line][ptIdx].nPointIdx = 2;
		flags[line][ptIdx] = i;  //indexing
	}


	//拟合平面
	std::vector<cv::Point3f> Points3ds;
	for (int i = 0; i < (int)topCluster.size(); i++)
	{
		cv::Point3f a_pt = cv::Point3f(topCluster[i].pt3D.x, topCluster[i].pt3D.y, topCluster[i].pt3D.z);
		Points3ds.push_back(a_pt);
	}
	//计算面参数: z = Ax + By + C
	//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
#if 1	
	std::vector<cv::Point3f> out_inliers;
	Plane res = ransacFitPlane(Points3ds, out_inliers);
	if (res.C < 0)
	{
		res.A = -res.A;
		res.B = -res.B;
		res.C = -res.C;
		res.D = -res.D;
	}
#else
	Plane res = robustFitPlane(Points3ds);
#endif

	//计算投影向量
	SVzNL3DPoint vec_1 = { res.A, res.B, res.C };
	SVzNL3DPoint vec_2 = { 0, 0, 1.0 };
	SSG_planeCalibPara poseR = wd_computeRTMatrix(vec_1, vec_2);

	//投影
	double normDataPlane = sqrt(res.A * res.A + res.B * res.B + res.C * res.C);
	std::vector<SVzNL3DPosition> projectPoints3ds;
	projectPoints3ds.resize(topCluster.size());
	double sum_x = 0, sum_y = 0, sum_z = 0;
	int sumZConter = 0;
	for (int i = 0; i < (int)topCluster.size(); i++)
	{
		double distToPlane = abs(res.A * topCluster[i].pt3D.x + res.B * topCluster[i].pt3D.y + res.C * topCluster[i].pt3D.z + res.D) / normDataPlane;
		double x = topCluster[i].pt3D.x * poseR.planeCalib[0] + topCluster[i].pt3D.y * poseR.planeCalib[1] + topCluster[i].pt3D.z * poseR.planeCalib[2];
		double y = topCluster[i].pt3D.x * poseR.planeCalib[3] + topCluster[i].pt3D.y * poseR.planeCalib[4] + topCluster[i].pt3D.z * poseR.planeCalib[5];
		double z = topCluster[i].pt3D.x * poseR.planeCalib[6] + topCluster[i].pt3D.y * poseR.planeCalib[7] + topCluster[i].pt3D.z * poseR.planeCalib[8];
		projectPoints3ds[i].nPointIdx = topCluster[i].nPointIdx;
		projectPoints3ds[i].pt3D = { x, y, z };
		sum_x += x;
		sum_y += y;
		if (distToPlane < 2.0)
		{
			sum_z += z;
			sumZConter++;
		}
	}
	if(sumZConter == 0)
	{
		*errCode = SG_ERR_ZERO_OBJECTS;
		return resultPose;
	}
	double meanZ = sum_z / sumZConter;

	//生成孔的边界点
	std::vector<std::vector<int>> endingMask;
	endingMask.resize(lineNum);
	for (int i = 0; i < lineNum; i++)
	{
		endingMask[i].resize(linePtNum);
		std::fill(endingMask[i].begin(), endingMask[i].end(), 0);
	}
	std::vector< SVzNL3DPosition> endingPts;
	//垂直
	double topZTh = meanZ + 10.0;
	for (int line = 0; line < lineNum; line++)
	{
		int preValid = -1;
		for (int j = 0; j < linePtNum; j++)
		{

			if (flags[line][j] >= 0)
			{
				if (preValid == 0)
				{
					if (endingMask[line][j] == 0) 
					{
						int idx = flags[line][j];
						if (projectPoints3ds[idx].pt3D.z < topZTh)
						{
							endingPts.push_back(projectPoints3ds[idx]);
							endingMask[line][j] = 1;
						}
					}
				}
				preValid = 1;
			}
			else
			{
				if (preValid == 1)
				{
					if (endingMask[line][j - 1] == 0)
					{
						int idx = flags[line][j - 1];
						if (projectPoints3ds[idx].pt3D.z < topZTh)
						{
							endingPts.push_back(projectPoints3ds[idx]);
							endingMask[line][j - 1] = 1;
						}
					}
				}
				preValid = 0;
			}
		}
	}
	//水平
	for (int j = 0; j < linePtNum; j++)
	{
		int preValid = -1;
		for (int line = 0; line < lineNum; line++)
		{
			if (flags[line][j] >= 0)
			{
				if (preValid == 0)
				{
					if (endingMask[line][j] == 0)
					{
						int idx = flags[line][j];
						if (projectPoints3ds[idx].pt3D.z < topZTh)
						{
							endingPts.push_back(projectPoints3ds[idx]);
							endingMask[line][j] = 1;
						}
					}

				}
				preValid = 1;
			}
			else
			{
				if (preValid == 1)
				{
					if (endingMask[line-1][j] == 0)
					{
						int idx = flags[line-1][j];
						if (projectPoints3ds[idx].pt3D.z < topZTh)
						{
							endingPts.push_back(projectPoints3ds[idx]);
							endingMask[line - 1][j] = 1;
						}
					}
				}
				preValid = 0;
			}
		}
	}
	//标注
	for (int i = 0; i < (int)endingPts.size(); i++)
	{
		int line = endingPts[i].nPointIdx >> 16;
		int ptIdx = endingPts[i].nPointIdx & 0x0000FFFF;
		scanLines[line][ptIdx].nPointIdx = 3;
	}

	//孔聚类
	clusterCheckWin = 10;
	clusterDist = 5.0;
	distType = 0;  //0 - 2d distance; 1- 3d distance 
	std::vector<std::vector< SVzNL3DPosition>> holeClusters;  //result
	wd_pointClustering_speedUp(
		endingPts,
		lineNum, linePtNum, clusterCheckWin, //搜索窗口
		clusterDist,
		distType,
		holeClusters  //result
	);

	//拟合
	std::vector< SVzNL3DPoint> holesCenter;
	holesCenter.resize(holeClusters.size());
	std::vector<double> holesR;
	holesR.resize(holeClusters.size());
	//目标圆拟合
	//圆拟合
	for (int i = 0; i < (int)holeClusters.size(); i++)
	{
		SVzNL3DPoint a_center;
		double a_radius;
		double err = fitCircleByLeastSquare_2(holeClusters[i], a_center, a_radius);
		holesCenter[i] = a_center;
		holesR[i] = a_radius;
	}
	//计算中心孔的姿态
	//目标过滤
	//首先挑出最大的孔
	const double bigHoleD = 40.0;//大孔直径
	const double smallHoleD = 5.0;//大孔直径
	std::vector<int> bigHoleIdx;
	std::vector<int> smallHoleIdx;
	for (int i = 0; i < (int)holeClusters.size(); i++)
	{
		if ( (holesR[i] > bigHoleD/2) && (holesR[i] < bigHoleD * 1.5))
			bigHoleIdx.push_back(i);
		else if ( (holesR[i] > smallHoleD/2) && (holesR[i] < smallHoleD * 1.5))
			smallHoleIdx.push_back(i);
	}
	if (bigHoleIdx.size() != 4)
	{
		*errCode = SG_ERR_ZERO_OBJECTS;
		return resultPose;
	}
	int holeL_idx = -1, holeR_idx = -1, holeT_idx = -1, holeB_idx = -1;
	for (int i = 0; i < (int)bigHoleIdx.size(); i++)
	{
		int idx = bigHoleIdx[i];
		if (holeL_idx < 0)
			holeL_idx = idx;
		else if (holesCenter[holeL_idx].x > holesCenter[idx].x)
			holeL_idx = idx;

		if (holeR_idx < 0)
			holeR_idx = idx;
		else if (holesCenter[holeR_idx].x < holesCenter[idx].x)
			holeR_idx = idx;

		if (holeT_idx < 0)
			holeT_idx = idx;
		else if (holesCenter[holeT_idx].y > holesCenter[idx].y)
			holeT_idx = idx;

		if (holeB_idx < 0)
			holeB_idx = idx;
		else if (holesCenter[holeB_idx].y < holesCenter[idx].y)
			holeB_idx = idx;
	}
	double ref_x = (holesCenter[holeL_idx].x + holesCenter[holeR_idx].x + holesCenter[holeT_idx].x + holesCenter[holeB_idx].x) / 4.0;
	double ref_y = (holesCenter[holeL_idx].y + holesCenter[holeR_idx].y + holesCenter[holeT_idx].y + holesCenter[holeB_idx].y) / 4.0;
	//寻找中间孔
	int centerHoleIdx = -1;
	double minDist = -1;
	for (int i = 0; i < (int)smallHoleIdx.size(); i++)
	{
		int idx = smallHoleIdx[i];
		double dist = sqrt(pow(holesCenter[idx].x - ref_x, 2) + pow(holesCenter[idx].y - ref_y, 2));
		if (centerHoleIdx < 0)
		{
			centerHoleIdx = idx;
			minDist = dist;
		}
		else if (minDist > dist)
		{
			centerHoleIdx = idx;
			minDist = dist;
		}
	}

	resultPose.center = { holesCenter[centerHoleIdx].x, holesCenter[centerHoleIdx].y, meanZ};
	resultPose.xDir = {0.0, 0.0, -1.0}; // { 1.0, 0.0, 0 };// { 0.0, 0.0, 1.0 };
	resultPose.normalDir = { holesCenter[holeR_idx].x - holesCenter[centerHoleIdx].x, holesCenter[holeR_idx].y - holesCenter[centerHoleIdx].y, 0.0 }; // { 1.0, 0.0, 0 };
	resultPose.normalDir = vec3_normalize(resultPose.normalDir);
	//resultPose.yDir		= { 0.0, 1.0, 0 };
	
	//叉乘出y;
	//向量叉乘
	resultPose.yDir = vec3_cross(resultPose.normalDir, resultPose.xDir);
	resultPose.holeLT = resultPose.center;
	resultPose.holeRB = resultPose.center;
#if 0
	//旋转回去
	for (int i = 0; i < lineNum; i++)
	{
		//行处理
		//调平，去除地面
		lineDataRT_vector(scanLines[i], poseR.invRMatrix, -1);
	}
#endif
	double x = resultPose.center.x * poseR.invRMatrix[0] + resultPose.center.y * poseR.invRMatrix[1] + resultPose.center.z * poseR.invRMatrix[2];
	double y = resultPose.center.x * poseR.invRMatrix[3] + resultPose.center.y * poseR.invRMatrix[4] + resultPose.center.z * poseR.invRMatrix[5];
	double z = resultPose.center.x * poseR.invRMatrix[6] + resultPose.center.y * poseR.invRMatrix[7] + resultPose.center.z * poseR.invRMatrix[8];
	resultPose.center = { x, y, z };
	x = resultPose.normalDir.x * poseR.invRMatrix[0] + resultPose.normalDir.y * poseR.invRMatrix[1] + resultPose.normalDir.z * poseR.invRMatrix[2];
	y = resultPose.normalDir.x * poseR.invRMatrix[3] + resultPose.normalDir.y * poseR.invRMatrix[4] + resultPose.normalDir.z * poseR.invRMatrix[5];
	z = resultPose.normalDir.x * poseR.invRMatrix[6] + resultPose.normalDir.y * poseR.invRMatrix[7] + resultPose.normalDir.z * poseR.invRMatrix[8];
	resultPose.normalDir = { x, y, z };

	x = resultPose.xDir.x * poseR.invRMatrix[0] + resultPose.xDir.y * poseR.invRMatrix[1] + resultPose.xDir.z * poseR.invRMatrix[2];
	y = resultPose.xDir.x * poseR.invRMatrix[3] + resultPose.xDir.y * poseR.invRMatrix[4] + resultPose.xDir.z * poseR.invRMatrix[5];
	z = resultPose.xDir.x * poseR.invRMatrix[6] + resultPose.xDir.y * poseR.invRMatrix[7] + resultPose.xDir.z * poseR.invRMatrix[8];
	resultPose.xDir = { x, y, z };

	x = resultPose.yDir.x * poseR.invRMatrix[0] + resultPose.yDir.y * poseR.invRMatrix[1] + resultPose.yDir.z * poseR.invRMatrix[2];
	y = resultPose.yDir.x * poseR.invRMatrix[3] + resultPose.yDir.y * poseR.invRMatrix[4] + resultPose.yDir.z * poseR.invRMatrix[5];
	z = resultPose.yDir.x * poseR.invRMatrix[6] + resultPose.yDir.y * poseR.invRMatrix[7] + resultPose.yDir.z * poseR.invRMatrix[8];
	resultPose.yDir = { x, y, z };

	x = resultPose.holeLT.x * poseR.invRMatrix[0] + resultPose.holeLT.y * poseR.invRMatrix[1] + resultPose.holeLT.z * poseR.invRMatrix[2];
	y = resultPose.holeLT.x * poseR.invRMatrix[3] + resultPose.holeLT.y * poseR.invRMatrix[4] + resultPose.holeLT.z * poseR.invRMatrix[5];
	z = resultPose.holeLT.x * poseR.invRMatrix[6] + resultPose.holeLT.y * poseR.invRMatrix[7] + resultPose.holeLT.z * poseR.invRMatrix[8];
	resultPose.holeLT = { x, y, z };

	x = resultPose.holeRB.x * poseR.invRMatrix[0] + resultPose.holeRB.y * poseR.invRMatrix[1] + resultPose.holeRB.z * poseR.invRMatrix[2];
	y = resultPose.holeRB.x * poseR.invRMatrix[3] + resultPose.holeRB.y * poseR.invRMatrix[4] + resultPose.holeRB.z * poseR.invRMatrix[5];
	z = resultPose.holeRB.x * poseR.invRMatrix[6] + resultPose.holeRB.y * poseR.invRMatrix[7] + resultPose.holeRB.z * poseR.invRMatrix[8];
	resultPose.holeRB = { x, y, z };


	return resultPose;
}

#if 0
void rodAarcFeatueDetection(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SSG_cornerParam cornerPara,
	const SSG_outlierFilterParam filterParam,
	const double rodDiameter,
	std::vector<std::vector<SWD_rodArcFeature>>& arcFeatures)
{
	int lineNum = (int)scanLines.size();
	int linePtNum = (int)scanLines[0].size();
	for (int line = 0; line < lineNum; line++)
	{
		if (line == 697)
			int kkk = 1;
		std::vector<SVzNL3DPosition>& lineData = scanLines[line];

		//滤波，滤除异常点
		sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
		//提取rodArc特征
		std::vector<SWD_rodArcFeature> line_rodArcs;
		wd_getRodArcFeature_peakCornerMethod(lineData, line, rodDiameter/2, cornerPara, line_rodArcs );
		arcFeatures.push_back(line_rodArcs);
	}
	return;
}
#endif

SVzNL3DPoint _exchangeXY(SVzNL3DPoint pt)
{
	SVzNL3DPoint result = {pt.y, pt.x, pt.z};
	return result;
}

void _computeRodInfo(
	SWD_rodArcFeatureTree& a_objTree,
	bool treeIsHorizon, 
	std::vector< SVzNL3DPoint>& fittingPoints, 
	SSX_rodPositionInfo& a_objRod)
{
	int nodeSize = a_objTree.treeNodes.size();
	//拟合
	double _a, _b, _c;
	lineFitting_abc(fittingPoints, &_a, &_b, &_c);
	int dataSize = (int)fittingPoints.size();
	SVzNL2DPointD foot1 = sx_getFootPoint_abc(fittingPoints[0].x, fittingPoints[0].y, _a, _b, _c);
	SVzNL2DPointD foot2 = sx_getFootPoint_abc(fittingPoints[dataSize - 1].x, fittingPoints[dataSize - 1].y, _a, _b, _c);
	double deltaZ = fittingPoints[dataSize - 1].z - fittingPoints[0].z;
	double len = sqrt(pow(foot1.x - foot2.x, 2) + pow(foot1.y - foot2.y, 2));
	//直线的轴向向量
	SVzNL3DPoint axialDir = { foot2.x - foot1.x, foot2.y - foot1.y, fittingPoints[dataSize - 1].z - fittingPoints[0].z };
	//归一化
	double normData = sqrt(pow(axialDir.x, 2) + pow(axialDir.y, 2) + pow(axialDir.z, 2));
	axialDir.x = axialDir.x / normData;
	axialDir.y = axialDir.y / normData;
	axialDir.z = axialDir.z / normData;
	//计算一个辅助平面（Y=0平面旋转一个角度）的法向
	double theta = atan2(foot2.y - foot1.y, foot2.x - foot1.x);
	double sinTheta = sin(theta);
	double cosTheta = cos(theta);
	SVzNL3DPoint tmpDir = { sinTheta, -cosTheta, 0 };
	//叉乘出棒材的法向
	SVzNL3DPoint normalDir = { axialDir.y * tmpDir.z - tmpDir.y * axialDir.z,
								axialDir.z * tmpDir.x - tmpDir.z * axialDir.x,
								axialDir.x * tmpDir.y - tmpDir.x * axialDir.y };


	//确定真正起点和终点：到直线距离小于5mm
	double tmpData = sqrt(_a * _a + _b * _b);
	_a = _a / tmpData;
	_b = _b / tmpData;
	_c = _c / tmpData;
	SVzNL3DPoint realStart, realEnd;
	bool foundStart = false;
	for (int j = 0; j < nodeSize; j++)
	{
		SVzNL3DPoint a_pt = a_objTree.treeNodes[j].peakPt;
		if (true == treeIsHorizon)
			a_pt = _exchangeXY(a_pt);
		double dist = abs(a_pt.x * _a + a_pt.y * _b + _c);
		if (dist < 5.0)
		{
			realStart = a_pt;
			foundStart = true;
			break;
		}
	}
	if (false == foundStart)
	{
		realStart = a_objTree.treeNodes[0].peakPt;
		if (true == treeIsHorizon)
			realStart = _exchangeXY(realStart);
	}

	bool foundEnd = false;
	for (int j = nodeSize - 1; j >= 0; j--)
	{
		SVzNL3DPoint a_pt = a_objTree.treeNodes[j].peakPt;
		if (true == treeIsHorizon)
			a_pt = _exchangeXY(a_pt);
		double dist = abs(a_pt.x * _a + a_pt.y * _b + _c);
		if (dist < 5.0)
		{
			realEnd = a_pt;
			foundEnd = true;
			break;
		}
	}
	if (false == foundEnd)
	{
		realEnd = a_objTree.treeNodes[nodeSize - 1].peakPt;
		if (true == treeIsHorizon)
			realEnd = _exchangeXY(realEnd);
	}

	SVzNL2DPointD foot_s = sx_getFootPoint_abc(realStart.x, realStart.y, _a, _b, _c);
	SVzNL2DPointD foot_e = sx_getFootPoint_abc(realEnd.x, realEnd.y, _a, _b, _c);
	double dist_s = sqrt(pow(foot_s.x - foot1.x, 2) + pow(foot_s.y - foot1.y, 2));
	double dist_e = sqrt(pow(foot_e.x - foot1.x, 2) + pow(foot_e.y - foot1.y, 2));
	//生成目标信息
	;
	a_objRod.startPt = { foot_s.x, foot_s.y, -(dist_s / len) * deltaZ + fittingPoints[0].z };
	a_objRod.endPt = { foot_e.x, foot_e.y, (dist_e / len) * deltaZ + fittingPoints[0].z };
	a_objRod.center = { (a_objRod.startPt.x + a_objRod.endPt.x) / 2,
					(a_objRod.startPt.y + a_objRod.endPt.y) / 2,
					(a_objRod.startPt.z + a_objRod.endPt.z) / 2 };
	a_objRod.axialDir = axialDir;
	a_objRod.normalDir = normalDir;

	return;
}

bool checkObjEixst(SSX_rodPositionInfo& a_objRod, std::vector<SSX_rodPositionInfo>& existObjs, const SSX_rodParam rodParam)
{
	double minDistance = -1;
	for (int i = 0; i < (int)existObjs.size(); i++)
	{
		double dist = sqrt(pow(a_objRod.center.x - existObjs[i].center.x, 2) +
			pow(a_objRod.center.y - existObjs[i].center.y, 2) +
			pow(a_objRod.center.z - existObjs[i].center.z, 2));
		if (minDistance < 0)
			minDistance = dist;
		else if (minDistance > dist)
			minDistance = dist;
	}
	if (minDistance < 0)
		return false;
	else if (minDistance < rodParam.diameter / 4) // R/2
		return true;
	else
		return false;
}

bool _commpareByCenterZ(SSX_rodPositionInfo& a, SSX_rodPositionInfo& b)
{
	return  (a.center.z < b.center.z);
}

void sx_rodPositioning(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SSG_planeCalibPara poseCalibPara,
	const SSG_cornerParam cornerPara,
	const SSG_outlierFilterParam filterParam,
	const SSG_treeGrowParam growParam,
	const SSX_rodParam rodParam,
	std::vector<SSX_rodPositionInfo>& rodInfo,
	int* errCode)
{
	*errCode = 0;
	int lineNum = (int)scanLines.size();
	if (lineNum == 0)
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return;
	}

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

	//判断数据格式是否为grid。算法只能处理grid数据格式
	bool isGridData = true;
	for (int line = 0; line < lineNum; line++)
	{
		if (linePtNum != (int)scanLines[line].size())
		{
			isGridData = false;
			break;
		}
	}
	if (false == isGridData)//数据不是网格格式
	{
		*errCode = SG_ERR_NOT_GRID_FORMAT;
		return;
	}

	for (int i = 0, i_max = (int)scanLines.size(); i < i_max; i++)
	{
		if (i == 14)
			int kkk = 1;
		//行处理
		//调平，去除地面
		double cuttingZ = -1;
		sx_rodPosition_lineDataR(scanLines[i], poseCalibPara.planeCalib, cuttingZ);
	}

	//生成水平扫描数据
	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<SWD_rodArcFeature>> arcFeatures_v;
	rodAarcFeatueDetection(	scanLines, cornerPara, filterParam, rodParam.diameter, arcFeatures_v);
	//特征生长
	std::vector<SWD_rodArcFeatureTree> rodArcTrees_v;
	wd_getRodArcFeatureGrowingTrees(arcFeatures_v, rodArcTrees_v, growParam);

	//水平方向
	std::vector<std::vector<SWD_rodArcFeature>> arcFeatures_h;
	rodAarcFeatueDetection(hLines_raw, cornerPara, filterParam, rodParam.diameter, arcFeatures_h);
	//特征生长
	std::vector<SWD_rodArcFeatureTree> rodArcTrees_h;
	wd_getRodArcFeatureGrowingTrees(arcFeatures_h, rodArcTrees_h, growParam);

	if ((rodArcTrees_v.size() == 0) && (rodArcTrees_h.size() == 0))
	{
		*errCode = SX_ERR_ZERO_OBJECTS;
		return;
	}


	int objNum_v = (int)rodArcTrees_v.size();
	int objNum_h = (int)rodArcTrees_h.size();
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
		}
	}
	//置标志，用于debug
	for (int i = 0; i < objNum_v; i++)
	{
		int nodeNum = (int)rodArcTrees_v[i].treeNodes.size();
		for (int j = 0; j < nodeNum; j++)
		{
			int lineIdx = rodArcTrees_v[i].treeNodes[j].lineIdx;
			int centerPtIdx = rodArcTrees_v[i].treeNodes[j].peakPtIdx;
			for (int m = rodArcTrees_v[i].treeNodes[j].startPtIdx; m <= rodArcTrees_v[i].treeNodes[j].endPtIdx; m++)
				scanLines[lineIdx][m].nPointIdx = 1;
			scanLines[lineIdx][centerPtIdx].nPointIdx |= 0x10;
		}
	}
	//置标志，用于debug
	for (int i = 0; i < objNum_h; i++)
	{
		int nodeNum = (int)rodArcTrees_h[i].treeNodes.size();
		for (int j = 0; j < nodeNum; j++)
		{
			int ptIdx = rodArcTrees_h[i].treeNodes[j].lineIdx;
			int centerLineIdx = rodArcTrees_h[i].treeNodes[j].peakPtIdx;
			for (int m = rodArcTrees_h[i].treeNodes[j].startPtIdx; m <= rodArcTrees_h[i].treeNodes[j].endPtIdx; m++)
				scanLines[m][ptIdx].nPointIdx |= 2;
			scanLines[centerLineIdx][ptIdx].nPointIdx |= 0x20;
		}
	}
	//目标判断
	//(1)长度过滤
	//垂直目标
	for (int i = 0; i < (int)rodArcTrees_v.size(); i++)
	{
		int nodeSize = rodArcTrees_v[i].treeNodes.size();
		SVzNL3DPoint startCenter = rodArcTrees_v[i].treeNodes[0].peakPt;
		SVzNL3DPoint endCenter = rodArcTrees_v[i].treeNodes[nodeSize-1].peakPt;
		double len = sqrt(pow(startCenter.x - endCenter.x, 2) +
						pow(startCenter.y - endCenter.y, 2) +
						pow(startCenter.z - endCenter.z, 2));
		double lenDiff = abs(len - rodParam.len);
		double lenDiff2 = abs(len - 2 * rodParam.len);
		if ( (lenDiff < rodParam.len* 0.15) || (lenDiff2 < rodParam.len * 0.15)) //validObj
		{
			//在XY平面内直线拟合
			//为了防止端部影响，跳过端面数据
			std::vector<SVzNL3DPoint> fittingPoints;
			for (int j = 0; j < nodeSize; j++)
			{
				double dist1 = sqrt(pow(rodArcTrees_v[i].treeNodes[j].peakPt.x - startCenter.x, 2) +
					pow(rodArcTrees_v[i].treeNodes[j].peakPt.y - startCenter.y, 2) +
					pow(rodArcTrees_v[i].treeNodes[j].peakPt.z - startCenter.z, 2));
				double dist2 = sqrt(pow(rodArcTrees_v[i].treeNodes[j].peakPt.x - endCenter.x, 2) +
					pow(rodArcTrees_v[i].treeNodes[j].peakPt.y - endCenter.y, 2) +
					pow(rodArcTrees_v[i].treeNodes[j].peakPt.z - endCenter.z, 2));
				if ((dist1 > rodParam.diameter / 2) && (dist2 > rodParam.diameter / 2))
					fittingPoints.push_back(rodArcTrees_v[i].treeNodes[j].peakPt);
			}
			if (fittingPoints.size() < 3)
				continue;
			SSX_rodPositionInfo a_objRod;
			_computeRodInfo(rodArcTrees_v[i], false, fittingPoints, a_objRod);
			rodInfo.push_back(a_objRod);
		}
	}
	//水平目标
	for (int i = 0; i < (int)rodArcTrees_h.size(); i++)
	{
		int nodeSize = rodArcTrees_h[i].treeNodes.size();
		SVzNL3DPoint startCenter = rodArcTrees_h[i].treeNodes[0].peakPt;
		startCenter = _exchangeXY(startCenter);
		SVzNL3DPoint endCenter = rodArcTrees_h[i].treeNodes[nodeSize - 1].peakPt;
		endCenter = _exchangeXY(endCenter);

		double len = sqrt(pow(startCenter.x - endCenter.x, 2) +
			pow(startCenter.y - endCenter.y, 2) +
			pow(startCenter.z - endCenter.z, 2));
		double lenDiff = abs(len - rodParam.len);
		if (lenDiff < rodParam.len * 0.15)  //validObj
		{
			//在XY平面内直线拟合
			//为了防止端部影响，跳过端面数据
			std::vector<SVzNL3DPoint> fittingPoints;
			for (int j = 0; j < nodeSize; j++)
			{
				SVzNL3DPoint a_pt = _exchangeXY(rodArcTrees_h[i].treeNodes[j].peakPt);
				double dist1 = sqrt(pow(a_pt.x - startCenter.x, 2) + pow(a_pt.y - startCenter.y, 2) + pow(a_pt.z - startCenter.z, 2));
				double dist2 = sqrt(pow(a_pt.x - endCenter.x, 2) + pow(a_pt.y - endCenter.y, 2) + pow(a_pt.z - endCenter.z, 2));
				if ((dist1 > rodParam.diameter / 2) && (dist2 > rodParam.diameter / 2))
					fittingPoints.push_back(a_pt);
			}
			if (fittingPoints.size() < 3)
				continue;

			SSX_rodPositionInfo a_objRod;
			_computeRodInfo(rodArcTrees_h[i], true, fittingPoints, a_objRod);
			//检查是否与垂直检测目标重叠
			bool isExist = checkObjEixst(a_objRod, rodInfo, rodParam);
			if(false == isExist)
				rodInfo.push_back(a_objRod);
		}
	}
	//（2）遮挡判断
	//按高度排序
	std::sort(rodInfo.begin(), rodInfo.end(), _commpareByCenterZ);

	//将数据重新投射回原来的坐标系，以保持手眼标定结果正确
	for (int i = 0; i < lineNum; i++)
		sx_rodPosition_lineDataR(scanLines[i], poseCalibPara.invRMatrix, -1);
	//将检测结果重新投射回原来的坐标系
	for (int i = 0; i < (int)rodInfo.size(); i++)
	{
		SSX_rodPositionInfo& a_rod = rodInfo[i];
		SVzNL3DPoint rawObj = _translatePoint(a_rod.center, poseCalibPara.invRMatrix);
		a_rod.center = rawObj;
		rawObj = _translatePoint(a_rod.axialDir, poseCalibPara.invRMatrix);
		a_rod.axialDir = rawObj;
		rawObj = _translatePoint(a_rod.normalDir, poseCalibPara.invRMatrix);
		a_rod.normalDir = rawObj;

		rawObj = _translatePoint(a_rod.startPt, poseCalibPara.invRMatrix);
		a_rod.startPt = rawObj;
		rawObj = _translatePoint(a_rod.endPt, poseCalibPara.invRMatrix);
		a_rod.endPt = rawObj;
	}
	return;
}

typedef struct
{
	int lineType;
	int treeIdx;
	double xyLine_a, xyLine_b, xyLine_c;
	SWD3DPointPostion line_start;
	SWD3DPointPostion line_end;
	std::vector<SWD3DPointPostion> linePts;
}_RodLineInfo;

double _compute2DDistance(SVzNL3DPoint& pt1, SVzNL3DPoint& pt2)
{
	return sqrt(pow(pt1.x - pt2.x, 2) + pow(pt1.y - pt2.y, 2));
}

double _compute2DDistance_indexingPt(SWD3DPointPostion& pt1, SWD3DPointPostion& pt2)
{
	return sqrt(pow(pt1.point.x - pt2.point.x, 2) + pow(pt1.point.y - pt2.point.y, 2));
}

SWD3DPointPostion _computeNearestPoint(_RodLineInfo& a_rod, SVzNL3DPoint& refPt, int* refPtIdx)
{
	SWD3DPointPostion nearestLinePt= { 0, 0, {0, 0, -1} };
	//线上
	double minDist = -1;
	int idx = -1;
	for (int i = 0; i < (int)a_rod.linePts.size(); i++)
	{
		SWD3DPointPostion a_linePt = a_rod.linePts[i];
		double dist = _compute2DDistance(a_linePt.point, refPt);
		if ((minDist < 0) || (minDist > dist))
		{
			minDist = dist;
			nearestLinePt = a_linePt;
			idx = i;
		}
	}
	*refPtIdx = idx;
	return nearestLinePt;
}

SWD3DPointPostion _computeGivenDistancePoint(_RodLineInfo& a_rod, SVzNL3DPoint& refPt, int refPtIdx, double givenDistance, int* ptPos, bool searchDir_forward)
{
	SWD3DPointPostion nearestLinePt = { 0, 0, {0, 0, -1} };
	//线上
	double minDist = -1;
	int position = -1;
	if (refPtIdx < 0)
	{
		for (int i = 0; i < (int)a_rod.linePts.size(); i++)
		{
			SWD3DPointPostion a_linePt = a_rod.linePts[i];
			double dist = _compute2DDistance(a_linePt.point, refPt);
			double distDiff = abs(dist - givenDistance);
			if ((minDist < 0) || (minDist > distDiff))
			{
				minDist = distDiff;
				nearestLinePt = a_linePt;
				position = i;
			}
		}
	}
	else
	{
		if (true == searchDir_forward)
		{
			for (int i = refPtIdx; i < (int)a_rod.linePts.size(); i++)
			{
				SWD3DPointPostion a_linePt = a_rod.linePts[i];
				double dist = _compute2DDistance(a_linePt.point, refPt);
				double distDiff = abs(dist - givenDistance);
				if ((minDist < 0) || (minDist > distDiff))
				{
					minDist = distDiff;
					nearestLinePt = a_linePt;
					position = i;
				}
			}
		}
		else
		{
			for (int i = refPtIdx; i >=  0; i--)
			{
				SWD3DPointPostion a_linePt = a_rod.linePts[i];
				double dist = _compute2DDistance(a_linePt.point, refPt);
				double distDiff = abs(dist - givenDistance);
				if ((minDist < 0) || (minDist > distDiff))
				{
					minDist = distDiff;
					nearestLinePt = a_linePt;
					position = i;
				}
			}
		}
	}
	*ptPos = position;
	return nearestLinePt;
}

bool _compareLineHeighth(_RodLineInfo& rod1, _RodLineInfo& rod2)
{
	//求交点
	SVzNL3DPoint crossPt = computeLineCrossPt_abs(
		rod1.xyLine_a, rod1.xyLine_b, rod1.xyLine_c,
		rod2.xyLine_a, rod2.xyLine_b, rod2.xyLine_c);
	//判断交点在线上还是线外
	//寻找离交点最近的点
	int posIdx = -1;
	double len_rod1 = _compute2DDistance_indexingPt(rod1.line_start, rod1.line_end);
	double cross_rod1_start = _compute2DDistance(rod1.line_start.point, crossPt);
	double cross_rod1_end = _compute2DDistance(rod1.line_end.point, crossPt);
	SWD3DPointPostion nearestLinePt_rod1;
	if ( (cross_rod1_start  < len_rod1) && (cross_rod1_end < len_rod1)) //线上
		nearestLinePt_rod1 = _computeNearestPoint(rod1, crossPt, &posIdx);
	else
	{
		if (cross_rod1_start < cross_rod1_end)
			nearestLinePt_rod1 = rod1.line_start;
		else
			nearestLinePt_rod1 = rod1.line_end;
	}
	if (nearestLinePt_rod1.point.z < 1e-4)
		return true;

	double len_rod2 = _compute2DDistance_indexingPt(rod2.line_start, rod2.line_end);
	double cross_rod2_start = _compute2DDistance(rod2.line_start.point, crossPt);
	double cross_rod2_end = _compute2DDistance(rod2.line_end.point, crossPt);
	SWD3DPointPostion nearestLinePt_rod2;
	if ((cross_rod2_start  < len_rod2) &&  (cross_rod2_end < len_rod2))//线上
		nearestLinePt_rod2 = _computeNearestPoint(rod2, crossPt, &posIdx);
	else
	{
		if (cross_rod2_start < cross_rod2_end)
			nearestLinePt_rod2 = rod2.line_start;
		else
			nearestLinePt_rod2 = rod2.line_end;
	}
	if (nearestLinePt_rod2.point.z < 1e-4)
		return true;

	//比较高度
	if (nearestLinePt_rod1.point.z < nearestLinePt_rod2.point.z)
		return true;
	else
		return false;
}

//寻找扫描线上距离直线最近的点
SVzNL3DPosition _computeLineNearestPoint(std::vector<SVzNL3DPosition>& lineData, int ptIdx_start, int ptIdx_end,  double line_a, double line_b, double line_c)
{
	SVzNL3DPosition nearestLinePt = { 0, {0, 0, 0} };
	double normData = sqrt(pow(line_a, 2) + pow(line_b, 2));
	line_a = line_a / normData;
	line_b = line_b / normData;
	line_c = line_c / normData;
	//线上
	double minDist = -1;
	for (int i = ptIdx_start; i <= ptIdx_end; i++)
	{
		SVzNL3DPosition a_linePt = lineData[i];
		double dist = abs(a_linePt.pt3D.x * line_a + a_linePt.pt3D.y * line_b + line_c);
		if ((minDist < 0) || (minDist > dist))
		{
			minDist = dist;
			a_linePt.nPointIdx = i;
			nearestLinePt = a_linePt;
		}
	}
	if( minDist > 2.0)
		nearestLinePt = { 0, {0, 0, 0} };
	return nearestLinePt;
}

//寻找水平扫描线上距离直线最近的点
SVzNL3DPosition _computeLineNearestPoint_H(std::vector<std::vector<SVzNL3DPosition>>& scanLines, int ptIdx, int line_start, int line_end, double line_a, double line_b, double line_c)
{
	SVzNL3DPosition nearestLinePt = { 0, {0, 0, 0} };
	double normData = sqrt(pow(line_a, 2) + pow(line_b, 2));
	line_a = line_a / normData;
	line_b = line_b / normData;
	line_c = line_c / normData;
	//线上
	double minDist = -1;
	for (int line = line_start; line <= line_end; line++)
	{
		SVzNL3DPosition a_linePt = scanLines[line][ptIdx];
		double dist = abs(a_linePt.pt3D.x * line_a + a_linePt.pt3D.y * line_b + line_c);
		if ((minDist < 0) || (minDist > dist))
		{
			minDist = dist;
			a_linePt.nPointIdx = line;
			nearestLinePt = a_linePt;
		}
	}
	if (minDist > 2.0)
		nearestLinePt = { 0, {0, 0, 0} };
	return nearestLinePt;
}

//计算两根垂直钢筋的焊点
SSX_weldSeamInfo _computeWeldPoint(
	_RodLineInfo& highestRod, _RodLineInfo& vRod, 
	const SSG_cornerParam cornerPara,
	const SSX_rodParam rodParam, 
	std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
	SSX_weldSeamInfo a_weldInfo;
	a_weldInfo.weldType = KeWD_WELD_UNKNOWN;

	int lineSize = (int)scanLines.size();
	int linePtSize = (int)scanLines[0].size();
	int pointChkWin = 20;
	//计算交点
	SVzNL3DPoint crossPt = computeLineCrossPt_abs(
		highestRod.xyLine_a, highestRod.xyLine_b, highestRod.xyLine_c,
		vRod.xyLine_a, vRod.xyLine_b, vRod.xyLine_c);
	int posIdx = -1;
	SWD3DPointPostion nearestLinePt_topRebar = _computeNearestPoint(highestRod, crossPt, &posIdx);
	double chkLen = rodParam.diameter * 2;
	int position = -1;
	SWD3DPointPostion chkStart = _computeGivenDistancePoint(vRod, crossPt, -1, chkLen, &position, true);

	//计算扫描线和点的范围
	int ptIdx_start, ptIdx_end;
	if (nearestLinePt_topRebar.ptIdx > chkStart.ptIdx)
	{
		ptIdx_start = chkStart.ptIdx;
		ptIdx_end = nearestLinePt_topRebar.ptIdx;
	}
	else
	{
		ptIdx_start = nearestLinePt_topRebar.ptIdx;
		ptIdx_end = chkStart.ptIdx;
	}
	int line_start, line_end;
	if (nearestLinePt_topRebar.lineIdx > chkStart.lineIdx)
	{
		line_start = chkStart.lineIdx;
		line_end = nearestLinePt_topRebar.lineIdx;
	}
	else
	{
		line_end = chkStart.lineIdx;
		line_start = nearestLinePt_topRebar.lineIdx;
	}

	double rotateAngle = 0;
	if (vRod.lineType == 1) //垂直扫描
	{
		if (nearestLinePt_topRebar.lineIdx > chkStart.lineIdx)
			rotateAngle = 45;
		else
			rotateAngle = -45;
	}
	else
	{
		if (nearestLinePt_topRebar.ptIdx > chkStart.ptIdx)
			rotateAngle = 45;
		else
			rotateAngle = -45;
	}

	//计算沿线上的点
	std::vector<SVzNL3DPosition> peakPoints;
	std::vector<SSG_intPair> peakPostions;

	if (vRod.lineType == 1) //垂直扫描
	{
		ptIdx_start = ptIdx_start - pointChkWin;
		if (ptIdx_start < 0)
			ptIdx_start = 0;
		ptIdx_end = ptIdx_end + pointChkWin;
		if (ptIdx_end >= linePtSize)
			ptIdx_end = linePtSize - 1;
		for (int line = line_start; line <= line_end; line++)
		{
			SVzNL3DPosition a_pkPt = _computeLineNearestPoint(scanLines[line], ptIdx_start, ptIdx_end, vRod.xyLine_a, vRod.xyLine_b, vRod.xyLine_c);
			if (a_pkPt.pt3D.z > 1e-4)
			{
				SSG_intPair a_pos;  //用于可视化显示
				a_pos.data_0 = line;
				a_pos.data_1 = a_pkPt.nPointIdx;
				a_pos.idx = (int)peakPoints.size();
				peakPostions.push_back(a_pos);

				a_pkPt.nPointIdx = (int)peakPoints.size();  
				double tmp = a_pkPt.pt3D.x;  //钢筋为水平方向，X和Y调换。因为计算方向角以Y方向计算
				a_pkPt.pt3D.x = a_pkPt.pt3D.y;
				a_pkPt.pt3D.y = tmp;
				peakPoints.push_back(a_pkPt);

			}
		}
	}
	else //水平扫描
	{
		line_start = line_start - pointChkWin;
		if (line_start < 0)
			line_start = 0;
		line_end = line_end + pointChkWin;
		if (line_end >= lineSize)
			line_end = lineSize - 1;

		for (int ptIdx = ptIdx_start; ptIdx <= ptIdx_end; ptIdx++)
		{
			SVzNL3DPosition a_pkPt = _computeLineNearestPoint_H(scanLines, ptIdx, line_start, line_end, vRod.xyLine_a, vRod.xyLine_b, vRod.xyLine_c);
			if (a_pkPt.pt3D.z > 1e-4)
			{
				SSG_intPair a_pos;  //用于可视化显示
				a_pos.data_0 = a_pkPt.nPointIdx;
				a_pos.data_1 = ptIdx;
				a_pos.idx = (int)peakPoints.size();
				peakPostions.push_back(a_pos);

				a_pkPt.nPointIdx = (int)peakPoints.size();
				peakPoints.push_back(a_pkPt);
			}
		}
	}
	std::vector< SSG_pntDirAngle> ptDirAngles;
	 wd_computeDirAngle_wholeLine(
		peakPoints,
		cornerPara,
		ptDirAngles
	);

	//寻找焊点：拐点最大的点
	std::vector< SSG_pntDirAngle> cornerPeakP;
	std::vector< SSG_pntDirAngle> cornerPeakM;
	wd_searchCornerPeaks(
		 ptDirAngles,
		 peakPoints,
		 cornerPara.cornerTh,
		 rodParam.diameter/2,
		 cornerPeakP,
		 cornerPeakM
	 );

	if (cornerPeakP.size() > 0)
	{
		int maxIdx = 0;
		for (int i = 1; i < (int)cornerPeakP.size(); i++)
		{
			if (cornerPeakP[i].corner > cornerPeakP[maxIdx].corner)
				maxIdx = i;
		}
		//生成结果 
		//double rotateAngle = (cornerPeakP[maxIdx].backwardAngle + cornerPeakP[maxIdx].forwardAngle) / 2 + 90;
		SVzNL3DPoint vec_axial = { highestRod.line_end.point.x - highestRod.line_start.point.x,
			highestRod.line_end.point.y - highestRod.line_start.point.y,
			highestRod.line_end.point.z - highestRod.line_start.point.z };
		SVzNL3DPoint vec_vRod = { vRod.line_end.point.x - vRod.line_start.point.x,
			vRod.line_end.point.y - vRod.line_start.point.y,
			vRod.line_end.point.z - vRod.line_start.point.z };
		SVzNL3DPoint v = vec3_cross(vec_axial, vec_vRod);

		SVzNL3DPoint v_dir = wd_rotateVectorInPlane(v, vec_axial, rotateAngle);
		if (v_dir.z > 0)
			v_dir = {-v_dir.x, -v_dir.y, -v_dir.z};
		v_dir = vec3_normalize(v_dir);

		int pkIdx = cornerPeakP[maxIdx].pntIdx;
		int obj_lineIdx = peakPostions[pkIdx].data_0;
		int obj_ptIdx = peakPostions[pkIdx].data_1;
		a_weldInfo.weldType = KeWD_WELD_POINT;
		a_weldInfo.center = scanLines[obj_lineIdx][obj_ptIdx].pt3D;
		a_weldInfo.startPt = a_weldInfo.center;
		a_weldInfo.endPt = a_weldInfo.center;
		a_weldInfo.axialDir = vec3_normalize(vec_axial);
		a_weldInfo.normalDir = v_dir; //法向量

		//标注
		for (int m = 0; m < (int)peakPostions.size(); m++)
		{
			int lineIdx = peakPostions[m].data_0;
			int centerPtIdx = peakPostions[m].data_1;
			scanLines[lineIdx][centerPtIdx].nPointIdx &= 0x0F;
			scanLines[lineIdx][centerPtIdx].nPointIdx |= 0x40;
		}
	}
	return a_weldInfo;
}

typedef struct
{
	EWD_weldType type;
	SWD3DPointPostion startPt;
	SWD3DPointPostion endPt;
	SVzNL3DPoint center;
}SSX_weldSlice;

//计算焊缝剖面特征
SSX_weldSlice _computeWeldSliceInfo(
	std::vector< SWD3DPointPostion>& weldSliceData,
	const SSX_rodParam rodParam,
	const SSG_cornerParam cornerPara,
	const double maxDistTh,
	bool dirForward
)
{
	double rodR = rodParam.diameter / 2;
	//提取段
	std::vector<SSG_RUN> segs;
	int minSegSize = 4;
	wd_lineDataSegment_dist(weldSliceData, segs, maxDistTh, minSegSize);
	if (segs.size() > 0)
	{
		SSG_RUN& base_seg = segs[0];
		int base_s = base_seg.start;
		int base_e = base_seg.len + base_s - 1;
		double base_w = abs(weldSliceData[base_s].point.y - weldSliceData[base_e].point.y);
		if (base_w > (rodR * 1.1))
		{
			std::vector< SVzNL3DPosition> line_data;
			line_data.resize(weldSliceData.size());
			SVzNL3DPosition zeroData = { 0, {0, 0, 0} };
			std::fill(line_data.begin(), line_data.end(), zeroData);
			for (int i = base_s; i <= base_e; i++)
			{
				line_data[i].nPointIdx = i;
				line_data[i].pt3D = weldSliceData[i].point;
			}
			//寻找拐点
			std::vector< SSG_pntDirAngle> ptDirAngles;
			wd_computeDirAngle_wholeLine(
				line_data,
				cornerPara,
				ptDirAngles
			);
			for(int i = 0; i < (int)ptDirAngles.size(); i ++)
			{
				if (ptDirAngles[i].type < 0)
					ptDirAngles[i].pntIdx = -1;
			}
			//寻找焊点：拐点最大的点
			std::vector< SSG_pntDirAngle> cornerPeakP;
			std::vector< SSG_pntDirAngle> cornerPeakM;
			wd_searchCornerPeaks(
				ptDirAngles,
				line_data,
				cornerPara.cornerTh,
				rodParam.diameter / 2,
				cornerPeakP,
				cornerPeakM
			);
			//if (true == dirForward)
			{
				if (cornerPeakP.size() > 0)
				{
					int maxIdx = 0;
					for (int i = 1; i < (int)cornerPeakP.size(); i++)
					{
						if (cornerPeakP[i].corner > cornerPeakP[maxIdx].corner)
							maxIdx = i;
					}
					int pkIdx = cornerPeakP[maxIdx].pntIdx;
					SSX_weldSlice a_slice;
					a_slice.type = KeWD_WELD_SEAM;
					a_slice.startPt = weldSliceData[pkIdx];
					a_slice.endPt = weldSliceData[pkIdx];
					a_slice.center = weldSliceData[pkIdx].point;
					return a_slice;
				}
			}
#if 0
			else
			{
				if (cornerPeakM.size() > 0)
				{
					int maxIdx = 0;
					for (int i = 1; i < (int)cornerPeakM.size(); i++)
					{
						if (cornerPeakM[i].corner < cornerPeakM[maxIdx].corner)
							maxIdx = i;
					}
					int pkIdx = cornerPeakM[maxIdx].pntIdx;
					SSX_weldSlice a_slice;
					a_slice.type = KeWD_WELD_SEAM;
					a_slice.startPt = weldSliceData[pkIdx];
					a_slice.endPt = weldSliceData[pkIdx];
					a_slice.center = weldSliceData[pkIdx].point;
					return a_slice;
				}
			}
#endif
		}
		else
		{
			if (segs.size() > 1)
			{
				//寻找除了钢筋外的最近点（大于0.5RodR）
				SWD3DPointPostion& refPt = weldSliceData[0];
				double minDist = -1;
				int minIdx = -1;
				for (int i = 1; i < (int)segs.size(); i++)
				{
					int seg_s = segs[i].start;
					int seg_e = segs[i].len + seg_s - 1;
					for (int j = seg_s; j <= seg_e; j++)
					{
						double dist = _compute2DDistance_indexingPt(refPt, weldSliceData[j]);
						if ((minDist < 0) || (minDist > dist))
						{
							minDist = dist;
							minIdx = j;
						}
					}
				}
				if (minIdx >= 0)
				{
					SWD3DPointPostion endPoint = weldSliceData[minIdx];
					int seg_s = segs[0].start;
					int seg_e = segs[0].len + seg_s - 1; // 寻找Z值最接近的
				
					double minZDiff = -1;
					int minZDiff_idx = -1;
					for (int i = seg_s; i <= seg_e; i++)
					{
						double zDiff = abs(endPoint.point.z - weldSliceData[i].point.z);
						if ((minZDiff < 0) || (minZDiff > zDiff))
						{
							minZDiff = zDiff;
							minZDiff_idx = i;
						}
					}
					if (minZDiff_idx >= 0)
					{
						SSX_weldSlice a_slice;
						a_slice.type = KeWD_WELD_SEAM_GAP;
						a_slice.startPt = weldSliceData[minZDiff_idx];
						a_slice.endPt = endPoint;
						a_slice.center = {(a_slice.startPt.point.x + a_slice.endPt.point.x)/2, 
							(a_slice.startPt.point.y + a_slice.endPt.point.y) / 2,
							(a_slice.startPt.point.z + a_slice.endPt.point.z) / 2};
						return a_slice;
					}
				}
			}
		}
	}
	SSX_weldSlice nullData;
	memset(&nullData, 0, sizeof(SSX_weldSlice));
	return nullData;
}

SSX_weldSeamInfo _computeWeldSeamInfo(
	std::vector<SSX_weldSlice>& seamData, 
	int rodScanType,
	const SSG_cornerParam& weldPointCornerParam,
	SVzNL3DPoint& v_zDir,
	double rotateAngle
)
{
	//拟合
	std::vector<SVzNL3DPoint> fitting_xy;
	std::vector<SVzNL3DPoint> fitting_yz;
	double mean_w = 0;
	for (int i = 0; i < (int)seamData.size(); i++)
	{
		fitting_xy.push_back(seamData[i].center);
		SVzNL3DPoint a_pt = { seamData[i].center.y, seamData[i].center.z, seamData[i].center.x };
		fitting_yz.push_back(a_pt);
		double w = abs(seamData[i].endPt.point.y - seamData[i].startPt.point.y);
		mean_w += w;
	}
	mean_w = mean_w / (double)seamData.size();

	double xyLine_a, xyLine_b, xyLine_c;
	lineFitting_abc(fitting_xy, &xyLine_a, &xyLine_b, &xyLine_c);
	//计算起点和终点
	SVzNL2DPointD foot_start_1 = sx_getFootPoint_abc(fitting_xy[0].x, fitting_xy[0].y, xyLine_a, xyLine_b, xyLine_c);
	SVzNL2DPointD foot_end_1 = sx_getFootPoint_abc(fitting_xy.back().x, fitting_xy.back().y, xyLine_a, xyLine_b, xyLine_c);

	double yzLine_a, yzLine_b, yzLine_c;
	lineFitting_abc(fitting_yz, &yzLine_a, &yzLine_b, &yzLine_c);
	//计算起点和终点
	SVzNL2DPointD foot_start_2 = sx_getFootPoint_abc(fitting_yz[0].x, fitting_yz[0].y, yzLine_a, yzLine_b, yzLine_c);
	SVzNL2DPointD foot_end_2 = sx_getFootPoint_abc(fitting_yz.back().x, fitting_yz.back().y, yzLine_a, yzLine_b, yzLine_c);

	SSX_weldSeamInfo a_weldSeam;
	a_weldSeam.weldType = mean_w > weldPointCornerParam.scale ? KeWD_WELD_SEAM_GAP : KeWD_WELD_SEAM;
	if (rodScanType == 1) //垂直
	{
		a_weldSeam.startPt = { foot_start_1.x, foot_start_1.y, foot_start_2.y };
		a_weldSeam.endPt = { foot_end_1.x, foot_end_1.y, foot_end_2.y };
		a_weldSeam.center = { (a_weldSeam.startPt.x + a_weldSeam.endPt.x) / 2,
			(a_weldSeam.startPt.y + a_weldSeam.endPt.y) / 2,
			(a_weldSeam.startPt.z + a_weldSeam.endPt.z) / 2 };
		a_weldSeam.axialDir = { (a_weldSeam.endPt.x - a_weldSeam.startPt.x) / 2,
			(a_weldSeam.endPt.y - a_weldSeam.startPt.y) / 2,
			(a_weldSeam.endPt.z - a_weldSeam.startPt.z) / 2 };
		a_weldSeam.axialDir = vec3_normalize(a_weldSeam.axialDir);
		SVzNL3DPoint v_dir = wd_rotateVectorInPlane(v_zDir, a_weldSeam.axialDir, rotateAngle);
		if (v_dir.z > 0)
			v_dir = { -v_dir.x, -v_dir.y, -v_dir.z };
		v_dir = vec3_normalize(v_dir);
		a_weldSeam.normalDir = v_dir;//法向量
		for (int i = 0; i < (int)seamData.size(); i++)
		{
			a_weldSeam.centerPts.push_back(seamData[i].center);
			SVzNL3DPoint pt_0 = seamData[i].startPt.point;
			SVzNL3DPoint pt_1 = seamData[i].endPt.point;
			a_weldSeam.edgePts_0.push_back(pt_0);
			a_weldSeam.edgePts_1.push_back(pt_1);
		}
	}
	else
	{
		a_weldSeam.startPt = { foot_start_1.y, foot_start_1.x, foot_start_2.y };
		a_weldSeam.endPt = { foot_end_1.y, foot_end_1.x, foot_end_2.y };
		a_weldSeam.center = { (a_weldSeam.startPt.x + a_weldSeam.endPt.x) / 2,
			(a_weldSeam.startPt.y + a_weldSeam.endPt.y) / 2,
			(a_weldSeam.startPt.z + a_weldSeam.endPt.z) / 2 };
		a_weldSeam.axialDir = { (a_weldSeam.endPt.x - a_weldSeam.startPt.x) / 2,
			(a_weldSeam.endPt.y - a_weldSeam.startPt.y) / 2,
			(a_weldSeam.endPt.z - a_weldSeam.startPt.z) / 2 };
		a_weldSeam.axialDir = vec3_normalize(a_weldSeam.axialDir);
		SVzNL3DPoint v_dir = wd_rotateVectorInPlane(v_zDir, a_weldSeam.axialDir, rotateAngle);
		if (v_dir.z > 0)
			v_dir = { -v_dir.x, -v_dir.y, -v_dir.z };
		v_dir = vec3_normalize(v_dir);
		a_weldSeam.normalDir = v_dir;//法向量
		for (int i = 0; i < (int)seamData.size(); i++)
		{
			SVzNL3DPoint a_pt = { seamData[i].center.y, seamData[i].center.x, seamData[i].center.z };
			a_weldSeam.centerPts.push_back(a_pt);
			SVzNL3DPoint pt_0 = { seamData[i].startPt.point.y, seamData[i].startPt.point.x, seamData[i].startPt.point.z };
			SVzNL3DPoint pt_1 = { seamData[i].endPt.point.y, seamData[i].endPt.point.x, seamData[i].endPt.point.z };
			a_weldSeam.edgePts_0.push_back(pt_0);
			a_weldSeam.edgePts_1.push_back(pt_1);
		}
	}
	return a_weldSeam;
}


void _computeRebarWeldSeam(
	_RodLineInfo& vRod, 
	SVzNL3DPoint& v_zDir,
	bool searchDir_forward,
	SVzNL3DPoint& crossPoint,
	int crossPtIdx, 
	std::vector<SSX_weldSeamInfo>& rebarWeldSeam,
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SSX_rodParam rodParam,
	const SSG_cornerParam weldPointCornerParam,
	const double weldSeanRange //焊缝距钢筋交叉点的范围（最大值）
)
{
	int lineSize = (int)scanLines.size();
	int linePtSize = (int)scanLines[0].size();
	//提取范围内的值
	int position_1 = -1;
	int position_2 = -1;
	SWD3DPointPostion chkPt_1 = _computeGivenDistancePoint(vRod, crossPoint, crossPtIdx, rodParam.diameter/2, &position_1, searchDir_forward);
	SWD3DPointPostion chkPt_2 = _computeGivenDistancePoint(vRod, crossPoint, crossPtIdx, weldSeanRange, &position_2, searchDir_forward);
	if ((position_1 < 0) || (position_2 < 0))
		return;

	std::vector<std::vector< SWD3DPointPostion>> checkingData_1;  //从Peak向左或向上
	std::vector<std::vector< SWD3DPointPostion>> checkingData_2; //从peak向右或向下
	int startIdx = position_1 < position_2 ? position_1 : position_2;
	int endIdx = position_1 < position_2 ? position_2 : position_1;
	for (int i = startIdx; i <= endIdx; i++)
	{
		std::vector< SWD3DPointPostion> a_line_1;
		std::vector< SWD3DPointPostion> a_line_2;
		SWD3DPointPostion& refPt = vRod.linePts[i];
		if (vRod.lineType == 1) //垂直
		{
			int lineIdx = refPt.lineIdx;
			for (int j = refPt.ptIdx; j >= 0; j--)
			{
				SVzNL3DPosition& a_pt = scanLines[lineIdx][j];
				if (a_pt.pt3D.z > 1e-4)
				{
					double xyDist = sqrt(pow(a_pt.pt3D.x - refPt.point.x, 2) + pow(a_pt.pt3D.y - refPt.point.y, 2));
					double zDiff = abs(a_pt.pt3D.z - refPt.point.z);
					if (xyDist > rodParam.diameter * 1.5)
						break;
					if (zDiff < rodParam.diameter)
					{
						SWD3DPointPostion a_chkPt;
						a_chkPt.lineIdx = lineIdx;
						a_chkPt.ptIdx = j;
						a_chkPt.point = a_pt.pt3D;
						a_line_1.push_back( a_chkPt);
					}
				}
			}
			//向后
			for (int j = refPt.ptIdx; j < linePtSize; j++)
			{
				SVzNL3DPosition& a_pt = scanLines[lineIdx][j];
				if (a_pt.pt3D.z > 1e-4)
				{
					double xyDist = sqrt(pow(a_pt.pt3D.x - refPt.point.x, 2) + pow(a_pt.pt3D.y - refPt.point.y, 2));
					double zDiff = abs(a_pt.pt3D.z - refPt.point.z);
					if (xyDist > rodParam.diameter * 1.5)
						break;
					if (zDiff < rodParam.diameter)
					{
						SWD3DPointPostion a_chkPt;
						a_chkPt.lineIdx = lineIdx;
						a_chkPt.ptIdx = j;
						a_chkPt.point = a_pt.pt3D;
						a_line_2.push_back(a_chkPt);
					}
				}
			}
		}
		else
		{
			int ptIdx = refPt.ptIdx;
			for (int j = refPt.lineIdx; j >= 0; j--)
			{
				SVzNL3DPosition& a_pt = scanLines[j][ptIdx];
				if (a_pt.pt3D.z > 1e-4)
				{
					double xyDist = sqrt(pow(a_pt.pt3D.x - refPt.point.x, 2) + pow(a_pt.pt3D.y - refPt.point.y, 2));
					double zDiff = abs(a_pt.pt3D.z - refPt.point.z);
					if (xyDist > rodParam.diameter * 1.5)
						break;
					if (zDiff < rodParam.diameter)
					{
						SWD3DPointPostion a_chkPt;
						a_chkPt.lineIdx = j;
						a_chkPt.ptIdx = ptIdx;
						a_chkPt.point.x = a_pt.pt3D.y;
						a_chkPt.point.y = a_pt.pt3D.x;
						a_chkPt.point.z = a_pt.pt3D.z;
						a_line_1.push_back(a_chkPt);
					}
				}
			}
			//向后
			for (int j = refPt.lineIdx; j < lineSize; j++)
			{
				SVzNL3DPosition& a_pt = scanLines[j][ptIdx];
				if (a_pt.pt3D.z > 1e-4)
				{
					double xyDist = sqrt(pow(a_pt.pt3D.x - refPt.point.x, 2) + pow(a_pt.pt3D.y - refPt.point.y, 2));
					double zDiff = abs(a_pt.pt3D.z - refPt.point.z);
					if (xyDist > rodParam.diameter * 1.5)
						break;
					if (zDiff < rodParam.diameter)
					{
						SWD3DPointPostion a_chkPt;
						a_chkPt.lineIdx = j;
						a_chkPt.ptIdx = ptIdx;
						a_chkPt.point.x = a_pt.pt3D.y;
						a_chkPt.point.y = a_pt.pt3D.x;
						a_chkPt.point.z = a_pt.pt3D.z;
						a_line_2.push_back(a_chkPt);
					}
				}
			}
		}
		checkingData_1.push_back(a_line_1);
		checkingData_2.push_back(a_line_2);
	}
	//宽度分析
	int dataLineSize = (int)checkingData_1.size();
	std::vector<double> lineW;
	int seam_start = -1;
	int seam_end = -1;
	for (int i = 0; i < dataLineSize; i++)
	{
		double w;
		if ((checkingData_1[i].size() == 0) || (checkingData_2[i].size() == 0))
			w = -1;
		else
			w = checkingData_2[i].back().point.y - checkingData_1[i].back().point.y;

		if (w > rodParam.diameter * 1.2)
		{
			if (seam_start < 0)
				seam_start = i;
			seam_end = i;
		}
		lineW.push_back(w);
	}
	//焊缝分析
	double segment_maxDistTh = 2.0;
	std::vector<SSX_weldSlice> seamData_1;
	std::vector<SSX_weldSlice> seamData_2;
	for (int i = seam_start; i <= seam_end; i++)
	{

		SSX_weldSlice a_slice_1 = _computeWeldSliceInfo(
			checkingData_1[i],
			rodParam,
			weldPointCornerParam,
			segment_maxDistTh,
			false
		);
		if (KeWD_WELD_UNKNOWN != a_slice_1.type)
			seamData_1.push_back(a_slice_1);

		SSX_weldSlice a_slice_2 = _computeWeldSliceInfo(
			checkingData_2[i],
			rodParam,
			weldPointCornerParam,
			segment_maxDistTh,
			true
		);
		if (KeWD_WELD_UNKNOWN != a_slice_2.type)
			seamData_2.push_back(a_slice_2);

	}

	//生成焊缝信息
	if (seamData_1.size() > 10)
	{
		double rotateAngle = vRod.lineType == 1 ? -45.0 : 45.0;
		SSX_weldSeamInfo a_weldSeam = _computeWeldSeamInfo(	seamData_1,	vRod.lineType, weldPointCornerParam, v_zDir, rotateAngle);
		rebarWeldSeam.push_back(a_weldSeam);
		//置标志，用于debug
		for (int i = 0; i < (int)seamData_1.size(); i++)
		{
			int lineIdx = seamData_1[i].startPt.lineIdx;
			int ptIdx = seamData_1[i].startPt.ptIdx;
			scanLines[lineIdx][ptIdx].nPointIdx |= 0x40;
			lineIdx = seamData_1[i].endPt.lineIdx;
			ptIdx = seamData_1[i].endPt.ptIdx;
			scanLines[lineIdx][ptIdx].nPointIdx |= 0x40;
		}
	}
	if (seamData_2.size() > 10)
	{
		double rotateAngle = vRod.lineType == 1 ? 45.0 : -45.0;
		SSX_weldSeamInfo a_weldSeam = _computeWeldSeamInfo(seamData_2, vRod.lineType,  weldPointCornerParam, v_zDir, rotateAngle);
		rebarWeldSeam.push_back(a_weldSeam);
		//置标志，用于debug
		for (int i = 0; i < (int)seamData_2.size(); i++)
		{
			int lineIdx = seamData_2[i].startPt.lineIdx;
			int ptIdx = seamData_2[i].startPt.ptIdx;
			scanLines[lineIdx][ptIdx].nPointIdx |= 0x40;
			lineIdx = seamData_2[i].endPt.lineIdx;
			ptIdx = seamData_2[i].endPt.ptIdx;
			scanLines[lineIdx][ptIdx].nPointIdx |= 0x40;
		}
	}
}

//筑裕钢结构钢筋焊缝定位
void sx_rebarWeldSeamPositioning(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SSG_planeCalibPara poseCalibPara,
	const SSG_cornerParam cornerPara,
	const SSG_outlierFilterParam filterParam,
	const SSG_treeGrowParam growParam,
	const SSX_rodParam rodParam,
	const double weldSeanRange, //焊缝距钢筋交叉点的范围（最大值）
	std::vector<SSX_weldSeamInfo>& weldSeamInfo,
	int* errCode)
{
	*errCode = 0;
	int lineNum = (int)scanLines.size();
	if (lineNum == 0)
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return;
	}

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

	//判断数据格式是否为grid。算法只能处理grid数据格式
	bool isGridData = true;
	for (int line = 0; line < lineNum; line++)
	{
		if (linePtNum != (int)scanLines[line].size())
		{
			isGridData = false;
			break;
		}
	}
	if (false == isGridData)//数据不是网格格式
	{
		*errCode = SG_ERR_NOT_GRID_FORMAT;
		return;
	}

	for (int i = 0, i_max = (int)scanLines.size(); i < i_max; i++)
	{
		if (i == 14)
			int kkk = 1;
		//行处理
		//调平，去除地面
		double cuttingZ = -1;
		sx_rodPosition_lineDataR(scanLines[i], poseCalibPara.planeCalib, cuttingZ);
	}

	//
	wd_noiseFilter(scanLines, filterParam, errCode);
	if (*errCode != 0)//数据不是网格格式
		return;

	//寻找水平和垂直方向的钢筋
	//生成水平扫描数据
	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;
		}
	}
#if 0
	for (int line = 0; line < linePtNum; line++)
	{
		//平滑
		sg_lineSegSmoothing(
			hLines_raw[line],
			double seg_y_deltaTh, //分段的Y间隔。大于此间隔，为新的分段
			double seg_z_deltaTh,//分段的Z间隔。大于此间隔，为新的分段
			int smoothWin,
			std::vector<SVzNL3DPosition>&output);
	}
#endif
	
	//在垂直方向上分别提取ARC特征，并进行特征生长
	std::vector<std::vector<SWD_rodArcFeature>> arcFeatures_v;
	rodAarcFeatueDetection(scanLines, cornerPara, filterParam, rodParam.diameter, arcFeatures_v);
	//特征生长
	std::vector<SWD_rodArcFeatureTree> rodArcTrees_v;
	wd_getRodArcFeatureGrowingTrees(arcFeatures_v, rodArcTrees_v, growParam);

	//水平方向
	std::vector<std::vector<SWD_rodArcFeature>> arcFeatures_h;
	rodAarcFeatueDetection(hLines_raw, cornerPara, filterParam, rodParam.diameter, arcFeatures_h);
	//特征生长
	std::vector<SWD_rodArcFeatureTree> rodArcTrees_h;
	wd_getRodArcFeatureGrowingTrees(arcFeatures_h, rodArcTrees_h, growParam);

	if ((rodArcTrees_v.size() == 0) && (rodArcTrees_h.size() == 0))
	{
		*errCode = SG_ERR_NOT_GRID_FORMAT;
		return;
	}

	int objNum_v = (int)rodArcTrees_v.size();
	int objNum_h = (int)rodArcTrees_h.size();
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
		}
	}
	//选择最高的目标
	std::vector<_RodLineInfo> rodLineInfo;
	for (int i = 0; i < objNum_v; i++)
	{
		_RodLineInfo a_rodInfo;
		int nodeNum = (int)rodArcTrees_v[i].treeNodes.size();
		for (int j = 0; j < nodeNum; j++)
		{
			int lineIdx = rodArcTrees_v[i].treeNodes[j].lineIdx;
			int centerPtIdx = rodArcTrees_v[i].treeNodes[j].peakPtIdx;
			SWD3DPointPostion a_pt;
			a_pt.lineIdx = lineIdx;
			a_pt.ptIdx = centerPtIdx;
			a_pt.point = scanLines[lineIdx][centerPtIdx].pt3D;
			if(a_pt.point.z > 1e-4)
				a_rodInfo.linePts.push_back(a_pt);
		}
		a_rodInfo.line_start = a_rodInfo.linePts[0];
		a_rodInfo.line_end = a_rodInfo.linePts.back();
		a_rodInfo.lineType = 1;
		a_rodInfo.treeIdx = i;
		indexingPtLineFitting_abc(a_rodInfo.linePts, &a_rodInfo.xyLine_a, &a_rodInfo.xyLine_b,	&a_rodInfo.xyLine_c);
		rodLineInfo.push_back(a_rodInfo);
	}
	for (int i = 0; i < objNum_h; i++)
	{
		_RodLineInfo a_rodInfo;
		int nodeNum = (int)rodArcTrees_h[i].treeNodes.size();
		for (int j = 0; j < nodeNum; j++)
		{
			int ptIdx = rodArcTrees_h[i].treeNodes[j].lineIdx;
			int centerLineIdx = rodArcTrees_h[i].treeNodes[j].peakPtIdx;
			SWD3DPointPostion a_pt;
			a_pt.lineIdx = centerLineIdx;
			a_pt.ptIdx = ptIdx;
			a_pt.point = scanLines[centerLineIdx][ptIdx].pt3D;
			if(a_pt.point.z > 1e-4)
				a_rodInfo.linePts.push_back(a_pt);
		}
		a_rodInfo.line_start = a_rodInfo.linePts[0];
		a_rodInfo.line_end = a_rodInfo.linePts.back();
		a_rodInfo.lineType = 2;
		a_rodInfo.treeIdx = i;
		indexingPtLineFitting_abc(a_rodInfo.linePts, &a_rodInfo.xyLine_a, &a_rodInfo.xyLine_b, &a_rodInfo.xyLine_c);
		rodLineInfo.push_back(a_rodInfo);
	}

	int totalRodLineSize = (int)rodLineInfo.size();
	//计算最高的线
	int highestLineIdx = -1;
	for (int i = 0; i < totalRodLineSize; i++)
	{
		bool isHighest = true;
		for (int j = i + 1; j < totalRodLineSize; j++)
		{
			bool isHigher = _compareLineHeighth(rodLineInfo[i], rodLineInfo[j]);
			if (false == isHigher)
			{
				isHighest = false;
				break;
			}
		}
		if (true == isHighest)
		{
			highestLineIdx = i;
			break;
		}
	}
	if(highestLineIdx < 0)
	{
		*errCode = SX_ERR_NO_HIGHEST_ROD;
		return;
	}

	_RodLineInfo& highestRod = rodLineInfo[highestLineIdx];
	//确定垂直的钢筋
	int vRodType;
	if (highestRod.lineType == 1)
		vRodType = 2;
	else
		vRodType = 1;

	//沿交点向两侧寻找
	//内部参数
	double validCrossRebar_distTh = rodParam.diameter * 1.5;

	double len_highestRod = _compute2DDistance_indexingPt(highestRod.line_start, highestRod.line_end);
	std::vector< _RodLineInfo> validVRod;
	std::vector<SWD3DPointPostion> crossPoints_highestRod;
	std::vector<SWD3DPointPostion> crossPoints_vRod;
	for (int i = 0; i < totalRodLineSize; i++)
	{
		if (rodLineInfo[i].lineType == vRodType)
		{
			_RodLineInfo& vRod = rodLineInfo[i];
			//求交点
			SVzNL3DPoint crossPt = computeLineCrossPt_abs(
				highestRod.xyLine_a, highestRod.xyLine_b, highestRod.xyLine_c,
				vRod.xyLine_a, vRod.xyLine_b, vRod.xyLine_c);
			//判断交点在线上还是线外
			//交点需要在最高的钢筋上，同时需要在另一个钢筋的线外
			double cross_highest_start = _compute2DDistance(highestRod.line_start.point, crossPt);
			double cross_highest_end = _compute2DDistance(highestRod.line_end.point, crossPt);
			if ((cross_highest_start < len_highestRod ) && ( cross_highest_end < len_highestRod ))
			{
				int posIdx = -1;
				SWD3DPointPostion nearestLinePt_highestRod = _computeNearestPoint(highestRod, crossPt, &posIdx);

				//寻找离交点最近的点
				double len_vRod = _compute2DDistance_indexingPt(vRod.line_start, vRod.line_end);
				double cross_vRod_start = _compute2DDistance(vRod.line_start.point, crossPt);
				double cross_vRod_end = _compute2DDistance(vRod.line_end.point, crossPt);
				if ( (cross_vRod_start > len_vRod) || (cross_vRod_end > len_vRod))  //线外
				{
					SWD3DPointPostion nearestLinePt_vRod;
					if (cross_vRod_start < cross_vRod_end)
						nearestLinePt_vRod = vRod.line_start;
					else
						nearestLinePt_vRod = vRod.line_end;

					double distance = _compute2DDistance_indexingPt(nearestLinePt_vRod, nearestLinePt_highestRod);
					double zDiff = nearestLinePt_vRod.point.z - nearestLinePt_highestRod.point.z;
					if ((distance < validCrossRebar_distTh) && (zDiff < validCrossRebar_distTh))
					{
						crossPoints_highestRod.push_back(nearestLinePt_highestRod);
						crossPoints_vRod.push_back(nearestLinePt_vRod);
						validVRod.push_back(vRod);
					}

				}
			}

		}
	}

	//置标志，用于debug
	if(highestRod.lineType == 1)  //vTree
	{
		int nodeNum = (int)rodArcTrees_v[highestRod.treeIdx].treeNodes.size();
		for (int j = 0; j < nodeNum; j++)
		{
			int lineIdx = rodArcTrees_v[highestRod.treeIdx].treeNodes[j].lineIdx;
			int centerPtIdx = rodArcTrees_v[highestRod.treeIdx].treeNodes[j].peakPtIdx;
			for (int m = rodArcTrees_v[highestRod.treeIdx].treeNodes[j].startPtIdx; m <= rodArcTrees_v[highestRod.treeIdx].treeNodes[j].endPtIdx; m++)
				scanLines[lineIdx][m].nPointIdx = 1;
			scanLines[lineIdx][centerPtIdx].nPointIdx |= 0x10;
		}
		for (int i = 0; i < (int)validVRod.size(); i++)
		{
			int treeIdx = validVRod[i].treeIdx;
			int nodeNum = (int)rodArcTrees_h[treeIdx].treeNodes.size();
			for (int j = 0; j < nodeNum; j++)
			{
				int ptIdx = rodArcTrees_h[treeIdx].treeNodes[j].lineIdx;
				int centerLineIdx = rodArcTrees_h[treeIdx].treeNodes[j].peakPtIdx;
				for (int m = rodArcTrees_h[treeIdx].treeNodes[j].startPtIdx; m <= rodArcTrees_h[treeIdx].treeNodes[j].endPtIdx; m++)
					scanLines[m][ptIdx].nPointIdx |= 2;
				scanLines[centerLineIdx][ptIdx].nPointIdx |= 0x20;
			}
		}
	}
	else
	{
		int nodeNum = (int)rodArcTrees_h[highestRod.treeIdx].treeNodes.size();
		for (int j = 0; j < nodeNum; j++)
		{
			int ptIdx = rodArcTrees_h[highestRod.treeIdx].treeNodes[j].lineIdx;
			int centerLineIdx = rodArcTrees_h[highestRod.treeIdx].treeNodes[j].peakPtIdx;
			for (int m = rodArcTrees_h[highestRod.treeIdx].treeNodes[j].startPtIdx; m <= rodArcTrees_h[highestRod.treeIdx].treeNodes[j].endPtIdx; m++)
				scanLines[m][ptIdx].nPointIdx |= 1;
			scanLines[centerLineIdx][ptIdx].nPointIdx |= 0x10;
		}
		//置标志，用于debug
		for (int i = 0; i < (int)validVRod.size(); i++)
		{
			int treeIdx = validVRod[i].treeIdx;
			int nodeNum = (int)rodArcTrees_v[treeIdx].treeNodes.size();
			for (int j = 0; j < nodeNum; j++)
			{
				int lineIdx = rodArcTrees_v[treeIdx].treeNodes[j].lineIdx;
				int centerPtIdx = rodArcTrees_v[treeIdx].treeNodes[j].peakPtIdx;
				for (int m = rodArcTrees_v[treeIdx].treeNodes[j].startPtIdx; m <= rodArcTrees_v[treeIdx].treeNodes[j].endPtIdx; m++)
					scanLines[lineIdx][m].nPointIdx = 2;
				scanLines[lineIdx][centerPtIdx].nPointIdx |= 0x20;
			}
		}
	}

	SSG_cornerParam weldPointCornerParam;
	memset(&weldPointCornerParam, 0, sizeof(SSG_cornerParam));
	weldPointCornerParam.cornerTh = 45;  //45度角
	weldPointCornerParam.scale = 2.0; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
	//计算焊点位置
	for (int i = 0; i < (int)validVRod.size(); i++)
	{
		//每一个与最高钢筋相交的钢筋都有一个焊点
		SSX_weldSeamInfo a_weldInfo = _computeWeldPoint(
			highestRod, validVRod[i],
			weldPointCornerParam,
			rodParam,
			scanLines);
		if (a_weldInfo.weldType != KeWD_WELD_UNKNOWN)
			weldSeamInfo.push_back(a_weldInfo);
	}

	std::vector<SWD3DPointPostion> crossPoints;
	std::vector<SVzNL3DPosition> vectorPositions;
	for (int i = 0; i < (int)validVRod.size(); i++)
	{
		//计算交点
		SVzNL3DPoint crossPt = computeLineCrossPt_abs(
			highestRod.xyLine_a, highestRod.xyLine_b, highestRod.xyLine_c,
			validVRod[i].xyLine_a, validVRod[i].xyLine_b, validVRod[i].xyLine_c);
		int posIdx = -1;
		SWD3DPointPostion nearestLinePt_topRebar = _computeNearestPoint(highestRod, crossPt, &posIdx);
		crossPoints.push_back(nearestLinePt_topRebar);

		
		//计算垂直向上的向量，用于计算焊缝方向
		SVzNL3DPoint vec_axial = { highestRod.line_end.point.x - highestRod.line_start.point.x,
								highestRod.line_end.point.y - highestRod.line_start.point.y,
								highestRod.line_end.point.z - highestRod.line_start.point.z };
		SVzNL3DPoint vec_vRod = { validVRod[i].line_end.point.x - validVRod[i].line_start.point.x,
								validVRod[i].line_end.point.y - validVRod[i].line_start.point.y,
								validVRod[i].line_end.point.z - validVRod[i].line_start.point.z };
		SVzNL3DPoint v = vec3_cross(vec_axial, vec_vRod);

		SVzNL3DPosition a_vectorPosition;
		a_vectorPosition.nPointIdx = posIdx;
		a_vectorPosition.pt3D = v;
		vectorPositions.push_back(a_vectorPosition);
		//计算下面钢筋的焊缝
		//SWD3DPointPostion nearestLinePt_btmRebar = _computeNearestPoint(validVRod[i], crossPt);
		std::vector<SSX_weldSeamInfo> a_rebarWeldSeam;
		_computeRebarWeldSeam(
			validVRod[i],
			v,
			true,
			crossPt,
			-1, 
			a_rebarWeldSeam,
			scanLines,
			rodParam,
			weldPointCornerParam,
			weldSeanRange //焊缝距钢筋交叉点的范围（最大值）
		);
		weldSeamInfo.insert(weldSeamInfo.end(), a_rebarWeldSeam.begin(), a_rebarWeldSeam.end());
	}

	for (int i = 0; i < (int)crossPoints.size(); i++)
	{
		if (vectorPositions[i].nPointIdx < 0)
			continue;
		for (int j = i + 1; j < (int)crossPoints.size(); j++)
		{
			if (vectorPositions[j].nPointIdx < 0)
				continue;

			double cross_dist = _compute2DDistance(crossPoints[i].point, crossPoints[j].point);
			if (cross_dist < rodParam.diameter)
				vectorPositions[j].nPointIdx = -1;
		}
	}
	for (int i = 0; i < (int)crossPoints.size(); i++)
	{
		if (vectorPositions[i].nPointIdx < 0)
			continue;

		//计算上面钢筋的焊缝
		std::vector<SSX_weldSeamInfo> a_rebarWeldSeam_0;
		_computeRebarWeldSeam(
			highestRod,
			vectorPositions[i].pt3D,
			true,
			crossPoints[i].point,
			vectorPositions[i].nPointIdx,
			a_rebarWeldSeam_0,
			scanLines,
			rodParam,
			weldPointCornerParam,
			weldSeanRange //焊缝距钢筋交叉点的范围（最大值）
		);
		weldSeamInfo.insert(weldSeamInfo.end(), a_rebarWeldSeam_0.begin(), a_rebarWeldSeam_0.end());
		//另一个方向
		std::vector<SSX_weldSeamInfo> a_rebarWeldSeam_1;
		_computeRebarWeldSeam(
			highestRod,
			vectorPositions[i].pt3D,
			false,
			crossPoints[i].point,
			vectorPositions[i].nPointIdx,
			a_rebarWeldSeam_1,
			scanLines,
			rodParam,
			weldPointCornerParam,
			weldSeanRange //焊缝距钢筋交叉点的范围（最大值）
		);
		weldSeamInfo.insert(weldSeamInfo.end(), a_rebarWeldSeam_1.begin(), a_rebarWeldSeam_1.end());
	}
}