#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 : 增加了定位盘中心完整姿态输出
std::string	m_strVersion = "1.2.1";
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_segFeature& 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;
	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,
	std::vector<SVzNL3DPoint>& 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;
			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))
			{
				a_pt.x = x;
				a_pt.y = y;
				a_pt.z = z;
				projectPoints.push_back(a_pt);
			}
		}
	}
}

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

void zCutPointClouds(
	std::vector<SVzNL3DPoint>& projectPoints, 
	SVzNLRangeD& zRange, 
	std::vector<SVzNL3DPoint>& cutLayerPoints)
{
	int ptNum = (int)projectPoints.size();
	for (int i = 0; i < ptNum; i++)
	{
		if ((projectPoints[i].z >= zRange.min) && (projectPoints[i].z <= zRange.max))
			cutLayerPoints.push_back(projectPoints[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 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< 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_segFeature>> arcFeatures;
	for (int line = 0; line < lineNum; line++)
	{
		if (line == 329)
			int kkk = 1;
		std::vector<SVzNL3DPosition>& lineData = data_lines[line];

		//滤波，滤除异常点
		sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
		std::vector<SWD_segFeature> line_ringArcs;
		int dataSize = (int)lineData.size();
		SVzNLRangeD arcWidth;
		arcWidth.min = rodDiameter / 2;
		arcWidth.max = rodDiameter * 1.5;
		//提取Arc特征
		wd_getRingArcFeature(
			lineData,
			line, //当前扫描线序号
			cornerPara,
			arcWidth, //环宽度，以半径为基准，对应60度角
			line_ringArcs  //环
		);
		arcFeatures.push_back(line_ringArcs);
	}
	//特征生长
	std::vector<SWD_segFeatureTree> growTrees;
	wd_getSegFeatureGrowingTrees(
		arcFeatures,
		growTrees,
		growParam);

	if (growTrees.size() == 0)
	{
		*errCode = SG_ERR_NOT_GRID_FORMAT;
		return;
	}
	int objNum = (int)growTrees.size();

	//置标志，用于debug
	for (int i = 0; i < objNum; i++)
	{
		int nodeNum = (int)growTrees[i].treeNodes.size();
		for (int j = 0; j < nodeNum; j++)
		{
			int lineIdx, ptIdx;
			if (false == isHorizonScan)
			{
				lineIdx = growTrees[i].treeNodes[j].lineIdx;
				for (int m = growTrees[i].treeNodes[j].startPtIdx; m <= growTrees[i].treeNodes[j].endPtIdx; m++)
				{
					ptIdx = m;
					scanLines[lineIdx][ptIdx].nPointIdx = 1;
				}
			}
			else
			{
				ptIdx = growTrees[i].treeNodes[j].lineIdx;
				for (int m = growTrees[i].treeNodes[j].startPtIdx; m <= growTrees[i].treeNodes[j].endPtIdx; m++)
				{
					lineIdx = m;
					scanLines[lineIdx][ptIdx].nPointIdx = 1;
				}
			}
		}
	}
	
	//逐个目标处理
	for (int i = 0; i < objNum; i++)
	{
		//空间直线拟合
		std::vector<SVzNL3DPoint> fitPoints;
		std::vector<SVzNL2DPoint> fit2DPos;
		int nodeSize = (int)growTrees[i].treeNodes.size();
		for (int j = 0; j < nodeSize; j++)
		{
			SVzNL2DPoint arcPos;
			SVzNL3DPoint a_pt = getArcPeak_parabolaFitting(data_lines, growTrees[i].treeNodes[j], arcPos);
			//SVzNL3DPoint a_pt = getArcPeak(data_lines, growTrees[i].treeNodes[j], arcPos);
			fitPoints.push_back(a_pt);
			fit2DPos.push_back(arcPos);
		}
		if (fitPoints.size() < 27)
			continue;
		//去除头尾各5个点，防止在端部和根部扫描时的数据有干扰
		fitPoints.erase(fitPoints.begin(), fitPoints.begin() + 10);
		fit2DPos.erase(fit2DPos.begin(), fit2DPos.begin() + 10);
		fitPoints.erase(fitPoints.end() - 5, fitPoints.end());
		fit2DPos.erase(fit2DPos.end() - 5, fit2DPos.end());
		//置标志
		for (int j = 0; j < (int)fit2DPos.size(); j++)
		{
			int lineIdx, ptIdx;
			if (false == isHorizonScan)
			{
				lineIdx = fit2DPos[j].x;
				ptIdx = fit2DPos[j].y;
			}
			else
			{
				lineIdx = fit2DPos[j].y;
				ptIdx = fit2DPos[j].x;
			}
			scanLines[lineIdx][ptIdx].nPointIdx = 2;
		}

		//拟合
		SVzNL3DPoint P0_center, P1_dir;
		bool result = fitLine3DLeastSquares(fitPoints, P0_center, P1_dir);
		if (false == result)
			continue;
		//投影
		//计算旋转向量
		SVzNL3DPoint vector1 = P1_dir;
		SVzNL3DPoint vector2 = { 0, 0, -1.0 };
		SSG_planeCalibPara rotatePara = wd_computeRTMatrix(	vector1, vector2);
		//
		SVzNL3DPoint P0_rotate = _ptRotate(P0_center, rotatePara.planeCalib);
		SSG_ROIRectD roi_xoy;
		roi_xoy.left = P0_rotate.x - rodDiameter * 2; //2D范围
		roi_xoy.right = P0_rotate.x + rodDiameter * 2; //2D范围
		roi_xoy.top = P0_rotate.y - rodDiameter * 2; //2D范围
		roi_xoy.bottom = P0_rotate.y + rodDiameter * 2; //2D范围

#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
		std::vector< SVzNL3DPoint> roiProjectionData;
		xoyROIProjection(data_lines, rotatePara.planeCalib, roi_xoy, roiProjectionData);
		//取端面
		SVzNLRangeD zRange = getZRange(roiProjectionData);
		SVzNLRangeD cutZRange;
		cutZRange.min = zRange.min;
		cutZRange.max = zRange.min + 5.0;  //5mm的端面
		std::vector<SVzNL3DPoint> surfacePoints;
		zCutPointClouds(roiProjectionData, 	cutZRange, surfacePoints);
		//计算中心点
		SVzNL3DPoint projectionCenter;// = getXoYCentroid(surfacePoints);
		SVzNL3DRangeD roi3D = wd_getPointCloudROI(surfacePoints);
		projectionCenter.x = (roi3D.xRange.min + roi3D.xRange.max) / 2;
		projectionCenter.y = (roi3D.yRange.min + roi3D.yRange.max) / 2;
		projectionCenter.z = zRange.min;
		//旋转回原坐标系
		SVzNL3DPoint surfaceCenter = _ptRotate(projectionCenter, rotatePara.invRMatrix);
		//生成Rod信息
		SSX_rodPoseInfo a_rod;
		a_rod.center = surfaceCenter;
		a_rod.axialDir = P1_dir;
		screwInfo.push_back(a_rod);
	}
	if (true == isHorizonScan)
	{
		int objNum = (int)screwInfo.size();
		for (int i = 0; i < objNum; i++)
		{
			double tmp = screwInfo[i].center.x;
			screwInfo[i].center.x = screwInfo[i].center.y;
			screwInfo[i].center.y = tmp;
			tmp = screwInfo[i].axialDir.x;
			screwInfo[i].axialDir.x = screwInfo[i].axialDir.y;
			screwInfo[i].axialDir.y = tmp;
			//screwInfo[i].rotateAngle += 90;
		}
	}
	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;
}

//计算定位盘中心点位姿
SSX_pointPoseInfo sx_getLocationPlatePose(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SSG_cornerParam cornerPara,
	int* errCode)
{
	*errCode = 0;
	SSX_pointPoseInfo resultPose;
	resultPose.center = { 0, 0, 0 };
	resultPose.normalDir = { 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;
	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);
	}
	//计算面参数: z = Ax + By + C
	//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
	//std::vector<cv::Point3f> out_inliers;
	//Plane res = ransacFitPlane(	Points3ds,	out_inliers	);
	Plane res = robustFitPlane(Points3ds);

	//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);

	//投影
	std::vector<cv::Point3f> projectPoints3ds;
	projectPoints3ds.resize(Points3ds.size());
	double sum_x = 0, sum_y = 0;
	for (int i = 0; i < (int)Points3ds.size(); i++)
	{
		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);
		sum_x += x;
		sum_y += y;
	}

	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();

	//计算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;
	resultPose.center = { center_x, center_y, center_z };
	resultPose.normalDir = { 0, 0, -1.0 };
	resultPose.yDir = { 0, -1.0, 0 };
	resultPose.xDir = { 1.0, 0, 0 };
	//旋转回去
	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 };

	return resultPose;
}

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 == 424)
			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;
}

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 = 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（会转义使用）
		}
	}
	//置标志，用于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);
		if (lenDiff < 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;
}

//筑裕钢结构钢筋焊缝定位
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,
	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);
	}

	//寻找水平和垂直方向的钢筋
	//生成水平扫描数据
	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 = 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（会转义使用）
		}
	}
	//置标志，用于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;
		}
	}
}