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

#define ALGO_USE_PATTERN_MATCH 0

void sg_lineDataR(SVzNL3DLaserLine* a_line,
	const double* camPoseR,
	double groundH)
{
	lineDataRT(a_line, camPoseR, groundH);
}

#if 0
//扫描线处理，进行垂直方向的特征提取和生长
void sg_sieveNodeDetection_lineProc(
	SVzNL3DLaserLine* a_line,
	int lineIdx,
	int* errCode,
	std::vector<std::vector< SSG_featureSemiCircle>>& all_vLineFeatures,
	std::vector<std::vector<int>>& noisePts,
	const SSG_sieveNodeDetectionParam sieveDetectParam)
{
	std::vector< SSG_featureSemiCircle> a_line_features;
	//滤波，滤除异常点
	std::vector<SVzNL3DPosition> filterData;
	std::vector<int> lineNoisePts;
	sg_lineDataRemoveOutlier(a_line->p3DPosition, a_line->nPositionCnt, sieveDetectParam.filterParam, filterData, lineNoisePts);
	noisePts.push_back(lineNoisePts);

	sg_getLineUpperSemiCircleFeature(
		filterData.data(),
		filterData.size(),
		lineIdx,
		sieveDetectParam.sieveDiameter,
		sieveDetectParam.slopeParam,
		a_line_features);

	all_vLineFeatures.push_back(a_line_features);  //空行也加入，保证能按行号索引
	return;
}
#endif

int _checkFeatureSplit(
	SSG_featureSemiCircle& a_feaurue, 
	std::vector< SSG_featureSemiCircle>& chk_line_feature,
	double splitMinDist,
	double splitMaxDist)  //在此距离内为有效分叉
{
	int split = -1;
	
	for (int i = 0, i_max = chk_line_feature.size(); i < i_max; i++)
	{
		if (i < i_max - 1)
		{
			if ((chk_line_feature[i].midPt.y < a_feaurue.midPt.y) && (chk_line_feature[i + 1].midPt.y > a_feaurue.midPt.y))
			{
				double dist_1 = abs(chk_line_feature[i].midPt.y - a_feaurue.midPt.y);
				double dist_2 = abs(chk_line_feature[i+1].midPt.y - a_feaurue.midPt.y);
				if ((dist_1 > splitMinDist) && (dist_1 < splitMaxDist) && (dist_2 > splitMinDist)&& (dist_2 < splitMaxDist))
				{
					split = i;
					break;
				}
			}
		}
	}
	return split;
}

bool compareByWidth(const SSG_featureSemiCircle& a, const SSG_featureSemiCircle& b) {
	return a.width < b.width;
}

SVzNL3DPoint _getMeanPt(
	std::vector<SSG_featureSemiCircle>& nodes, 
	SVzNL3DLaserLine* laser3DPoints)
{
	int nodeSize = nodes.size();
	int num = 0;
	SVzNL3DPoint meanPt = { 0.0,0.0,0.0 };
	for (int i = 0; i < nodeSize; i++)
	{
		SSG_featureSemiCircle& a_node = nodes[i];
		int lineIdx = a_node.lineIdx;
		for (int j = a_node.startPtIdx; j <= a_node.endPtIdx; j++)
		{
			if (laser3DPoints[lineIdx].p3DPosition[j].pt3D.z > 1e-4)
			{
				num++;
				meanPt.x += laser3DPoints[lineIdx].p3DPosition[j].pt3D.x;
				meanPt.y += laser3DPoints[lineIdx].p3DPosition[j].pt3D.y;
				meanPt.z += laser3DPoints[lineIdx].p3DPosition[j].pt3D.z;
			}
		}
	}
	if (num > 0)
	{
		meanPt.x = meanPt.x / num;
		meanPt.y = meanPt.y / num;
		meanPt.z = meanPt.z / num;
	}
	return meanPt;
}

SVzNL2DPoint _getNearestScanPt(
	std::vector<SSG_featureSemiCircle>& nodes,
	SVzNL3DLaserLine* laser3DPoints,
	SVzNL3DPoint objPt)
{
	int nodeSize = nodes.size();
	SVzNL2DPoint bestPos = { -1, -1 };
	double minDist = -1;
	for (int i = 0; i < nodeSize; i++)
	{
		SSG_featureSemiCircle& a_node = nodes[i];
		int lineIdx = a_node.lineIdx;
		for (int j = a_node.startPtIdx; j <= a_node.endPtIdx; j++)
		{
			if (laser3DPoints[lineIdx].p3DPosition[j].pt3D.z > 1e-4)
			{
				SVzNL3DPoint& a_pt = laser3DPoints[lineIdx].p3DPosition[j].pt3D;
				double dist = sqrt(pow(a_pt.x - objPt.x, 2) + pow(a_pt.y - objPt.y, 2));
				if (minDist < 0)
				{
					minDist = dist;
					bestPos = { a_node.lineIdx, j };
				}
				else
				{
					if (minDist > dist)
					{
						minDist = dist;
						bestPos = { a_node.lineIdx, j };
					}
				}
			}
		}
	}
	return bestPos;
}

//搜索目标在排序目标中的位置。返回的是排序目标前一位的序号，-1为最后一个
int getObjPostPos(std::vector<int>& sortedTrees, int treeID, std::vector<SSG_semiCircleFeatureTree>& trees, bool HPos)
{
	SSG_semiCircleFeatureTree& obj_tree = trees[treeID];

	for (int i = 0, i_max = sortedTrees.size(); i < i_max; i++)
	{
		int a_tree_id = sortedTrees[i];
		SSG_semiCircleFeatureTree& a_tree = trees[a_tree_id];
		if (((obj_tree.centerPt.x < a_tree.centerPt.x) && (true == HPos)) ||
			((obj_tree.centerPt.y < a_tree.centerPt.y) && (false == HPos)))
		{
			return i;
		}
	}
	return -1;  //最后一个
}

typedef struct
{
	SVzNL2DPoint nodePos;
	int LT_link;
	int RT_link;
	int LB_link;
	int RB_link;
	double LT_dist;
	double RT_dist;
	double RB_dist;
	double LB_dist;
}NodeQuadLink;

int getObjPostPos(std::vector<int>& sortedLine, int nodeID, std::vector<NodeQuadLink>& nodeLink, bool HPos)
{
	SVzNL2DPoint& nodePos = nodeLink[nodeID].nodePos;
	for (int i = 0, i_max = sortedLine.size(); i < i_max; i++)
	{
		int a_node_id = sortedLine[i];
		SVzNL2DPoint& sorting_node_pos = nodeLink[a_node_id].nodePos;
		if (((nodePos.x < sorting_node_pos.x) && (true == HPos)) ||
			((nodePos.y < sorting_node_pos.y) && (false == HPos)))
		{
			return i;
		}
	}
	return -1;  //最后一个
}


void _recursiveSorting(
	int nodeId,
	std::vector<std::vector<int>>& hSorting,
	std::vector< int>& nodeProcFlag,
	std::vector< int>& nodeSortingLineID,
	std::vector< NodeQuadLink>& nodeLinks,
	bool HPos,
	int* maxLineId)
{
	std::vector<int> processBuff;
	processBuff.push_back(nodeId);
	while (processBuff.size() > 0)
	{
		int nodeId = processBuff.front();
		if (nodeProcFlag[nodeId] == 0)
		{
			int currLineId = nodeSortingLineID[nodeId];
			if (*maxLineId < currLineId)
				*maxLineId = currLineId;
			std::vector<int>& currLine = hSorting[currLineId];
			//将当前Node插入currLine
			int nodePos = getObjPostPos(currLine, nodeId, nodeLinks, HPos);
			if (nodePos < 0)
				currLine.push_back(nodeId);
			else
				currLine.insert(currLine.begin() + nodePos, nodeId);
			nodeProcFlag[nodeId] = 1;
			//检查Link
			NodeQuadLink& currLink = nodeLinks[nodeId];
			if ((currLink.LT_dist > 0) && (currLineId > 0))
			{
				int linkNode = currLink.LT_link;
				int lineId = currLineId - 1;
				nodeSortingLineID[linkNode] = lineId;
				processBuff.push_back(linkNode);
				currLink.LT_dist = -1.0;
				nodeLinks[linkNode].RB_dist = -1.0; //取消
			}
			if ((currLink.RT_dist > 0) && (currLineId > 0))
			{
				int linkNode = currLink.RT_link;
				int lineId = currLineId - 1;
				nodeSortingLineID[linkNode] = lineId;
				processBuff.push_back(linkNode);
				currLink.RT_dist = -1.0;
				nodeLinks[linkNode].LB_dist = -1.0; //取消
			}
			if ((currLink.LB_dist > 0) && (currLineId < hSorting.size() - 1))
			{
				int linkNode = currLink.LB_link;
				int lineId = currLineId + 1;
				nodeSortingLineID[linkNode] = lineId;
				processBuff.push_back(linkNode);
				currLink.LB_dist = -1.0;
				nodeLinks[linkNode].RT_dist = -1.0; //取消
			}
			if ((currLink.RB_dist > 0) && (currLineId < hSorting.size() - 1))
			{
				int linkNode = currLink.RB_link;
				int lineId = currLineId + 1;
				nodeSortingLineID[linkNode] = lineId;
				processBuff.push_back(linkNode);
				currLink.RB_dist = -1.0;
				nodeLinks[linkNode].LT_dist = -1.0; //取消
			}
		}
		processBuff.erase(processBuff.begin());
	}
}

void _lineFillNullNode(
	std::vector<SVzNL3DPosition>& sortingNodes_line,
	std::vector< NodeQuadLink>& nodeLinks,
	const double lineNodeDistMean
)
{
	for (int i = 0; i < sortingNodes_line.size() -1; i++)
	{
		SVzNL3DPosition node_1 = sortingNodes_line[i];
		if (node_1.nPointIdx < 0)
			continue;
		SVzNL3DPosition node_2 = sortingNodes_line[i + 1];
		double dist = sqrt(pow(node_1.pt3D.x - node_2.pt3D.x, 2) + pow(node_1.pt3D.y - node_2.pt3D.y, 2));
		int n = (int)(dist / lineNodeDistMean + 0.5) - 1;
		if (n > 0)
		{
			//判断遗漏几个目标
			double x_step = (node_2.pt3D.x - node_1.pt3D.x) / (n + 1);
			double y_step = (node_2.pt3D.y - node_1.pt3D.y) / (n + 1);
			double z_step = (node_2.pt3D.z - node_1.pt3D.z) / (n + 1);
			for (int m = 0; m < n; m++)
			{
				SVzNL3DPosition a_newNode;
				a_newNode.nPointIdx = -1;
				a_newNode.pt3D.x = x_step * (m + 1) + node_1.pt3D.x;
				a_newNode.pt3D.y = y_step * (m + 1) + node_1.pt3D.y;
				a_newNode.pt3D.z = z_step * (m + 1) + node_1.pt3D.z;
				sortingNodes_line.insert(sortingNodes_line.begin() + i + 1 + m, a_newNode);
			}
		}
	}
}

void sg_getSieveNodes(
	SVzNL3DLaserLine* laser3DPoints,
	int lineNum,
	const SSG_sieveNodeDetectionParam sieveDetectParam,
	std::vector<std::vector<SVzNL3DPoint>>& nodePos)
{
	const int hole_max_ptSize = 20;
	int errCode = 0;
	for (int i = 0; i < lineNum; i++)
	{
		if (i == 19)
			int kkk = 1;

		sg_lineDataRemoveOutlier_changeOriginData(
			laser3DPoints[i].p3DPosition, 
			laser3DPoints[i].nPositionCnt, 
			sieveDetectParam.filterParam);

		//将nPointIdx转义使用前清零
		for (int j = 0; j < laser3DPoints[i].nPositionCnt; j++)
			laser3DPoints[i].p3DPosition[j].nPointIdx = 0;
	}
	//孔洞检测，对小的孔洞需要合并
	int maskY = laser3DPoints[0].nPositionCnt;
	int maskX = lineNum;
	//生成孔洞标注Mask，进行目标标注
	cv::Mat bwImg = cv::Mat::zeros(maskY, maskX, CV_8UC1);//rows, cols
	for (int i = 0; i < lineNum; i++)
	{
		for (int j = 0; j < laser3DPoints[i].nPositionCnt; j++)
		{
			if(laser3DPoints[i].p3DPosition[j].pt3D.z < 1e-4)
				bwImg.at<uchar>(j, i) = 1;
		}
	}
	//孔洞目标标注
	cv::Mat labImg;
	std::vector<SSG_Region> labelRgns;
	SG_TwoPassLabel(bwImg, labImg, labelRgns, 8);
	//将孔洞目标进行标识
	cv::Mat holeMask = cv::Mat::zeros(maskY, maskX, CV_32SC1);//rows, cols
	for (int i = 0, i_max = labelRgns.size(); i < i_max; i++)
	{
		int rgnID = labelRgns[i].labelID;
		if (labelRgns[i].ptCounter < hole_max_ptSize)  //孔洞
		{
			for (int m = labelRgns[i].roi.left; m <= labelRgns[i].roi.right; m++)
			{
				for (int n = labelRgns[i].roi.top; n <= labelRgns[i].roi.bottom; n++)
				{
					if (rgnID == labImg.at<int>(n, m))
						holeMask.at<int>(n, m) = rgnID;
				}
			}
		}
	}
#if _OUTPUT_LINE_PROC_RESULT
	cv::Mat holeMaskImage;
	cv::normalize(holeMask, holeMaskImage, 0, 255, cv::NORM_MINMAX, CV_8U);
	cv::imwrite("holeMask.png", holeMaskImage);
#endif

	std::vector<std::vector< SSG_featureSemiCircle>> all_vLineFeatures;
	for (int i = 0; i < lineNum; i++)
	{
		std::vector< SSG_featureSemiCircle> a_line_features;
		sg_getLineUpperSemiCircleFeature(
			laser3DPoints[i].p3DPosition,
			laser3DPoints[i].nPositionCnt,
			i,
			sieveDetectParam.sieveDiameter,
			sieveDetectParam.slopeParam,
			a_line_features,
			holeMask);
		//将nPointIdx转义使用前清零
		for (int j = 0; j < laser3DPoints[i].nPositionCnt; j++)
			laser3DPoints[i].p3DPosition[j].nPointIdx = 0;
		all_vLineFeatures.push_back(a_line_features);  //空行也加入，保证能按行号索引
	}

	//根据筛网的特点去除无效的feature。当上下两个feature在下一条扫描线被合并成一个feature时，说明上一条扫描线的两个feature是无效feature。其相邻的feature均为无效feature
	for (int i = 0; i < lineNum; i++)
	{
		//与前一条扫描线比较,寻找开始
		std::vector< SSG_featureSemiCircle>& line_features = all_vLineFeatures[i];
		if (i > 0)
		{
			std::vector< SSG_featureSemiCircle>& pre_line_features = all_vLineFeatures[i-1];
			for (int j = 0, j_max = line_features.size(); j < j_max; j++)
			{
				int split = _checkFeatureSplit(line_features[j], pre_line_features, sieveDetectParam.sieveDiameter/2, sieveDetectParam.sieveHoleSize);
				if (split >= 0)
				{
					pre_line_features[split].flag = FEATURE_FLAG_INVLD_END;
					pre_line_features[split + 1].flag = FEATURE_FLAG_INVLD_END;
					line_features[j].flag = FEATURE_FLAG_VALID_START;
				}
			}
		}
		//与后一条扫描线比较,寻找结束
		if (i < lineNum - 1)
		{
			std::vector< SSG_featureSemiCircle>& post_line_features = all_vLineFeatures[i + 1];
			for (int j = 0, j_max = line_features.size(); j < j_max; j++)
			{
				int split = _checkFeatureSplit(line_features[j], post_line_features, sieveDetectParam.sieveDiameter/2, sieveDetectParam.sieveHoleSize);
				if (split >= 0)
				{
					post_line_features[split].flag = FEATURE_FLAG_INVLD_START;
					post_line_features[split + 1].flag = FEATURE_FLAG_INVLD_START;
					line_features[j].flag = FEATURE_FLAG_VALID_END;
				}
			}
		}
	}

	//feature生长。碰到无效feature生长停止。无效生长可以作为生长起点。其生长树上的所有feature均为无效feature
	std::vector<SSG_semiCircleFeatureTree> trees;
	std::vector<SSG_semiCircleFeatureTree> stopTrees;  //停止生长的树
	std::vector<SSG_semiCircleFeatureTree> invalidTrees;  //被移除的树，这些树可能将目标分成多个树，从而被移除。需要保存下来迭代分析
	for (int i = 0; i < lineNum; i++)
	{
		//与前一条扫描线比较,寻找开始
		std::vector< SSG_featureSemiCircle>& line_features = all_vLineFeatures[i];
		sg_getFeatureGrowingTrees_semiCircle(
			line_features,
			i,
			lineNum,
			trees,
			stopTrees,
			invalidTrees,
			sieveDetectParam.growParam);
	}
	//精确确定焊接点
	for (int i = 0, i_max = stopTrees.size(); i < i_max; i++)
	{
		//焊接定为为所有点的（x,y,z)的平均处（质心）。
		SSG_semiCircleFeatureTree& a_tree = stopTrees[i];
		a_tree.centerPt = _getMeanPt(a_tree.treeNodes, laser3DPoints);
		a_tree.centerPos = _getNearestScanPt(a_tree.treeNodes, laser3DPoints, a_tree.centerPt);
		//判断焊点是否被焊接过：以中间点的高度与左右两边的中间高度比较


	}


	//按行排序 
#if 1
	// (1)量化到2D,建立2D索引实现有序搜索
	SVzNL3DRangeD ROI = sg_getScanDataROI(laser3DPoints, lineNum);
	double scale_x = 1.0;
	double scale_y = 1.0;
	int rows = (int)((ROI.yRange.max - ROI.yRange.min) / scale_y) + 1;
	int cols = (int)((ROI.xRange.max - ROI.xRange.min) / scale_x) + 1;
	if ((rows == 0) || (cols == 0))
		return;
	cv::Mat objMask = cv::Mat(rows, cols, CV_32SC1, -1);//rows, cols
	for (int i = 0, i_max = stopTrees.size(); i < i_max; i++)
	{
		SVzNL3DPoint treeCenter = laser3DPoints[stopTrees[i].centerPos.x].p3DPosition[stopTrees[i].centerPos.y].pt3D;
		int px = (int)((treeCenter.x - ROI.xRange.min) / scale_x);
		int py = (int)((treeCenter.y - ROI.yRange.min) / scale_y);
		objMask.at<int>(py, px) = i;
	}
#endif

	std::vector< NodeQuadLink> nodeLinks(stopTrees.size(), { {0,0},-1,-1,-1,-1, -1.0, -1.0, -1.0, -1.0});
	double neighbourDistTh = sieveDetectParam.sieveHoleSize * 1.2; //以正方形为模型，边长为1，则对角线为1.414，取1.2为长度门限
	for (int i = 0, i_max = stopTrees.size(); i < i_max; i++)
	{
		SSG_semiCircleFeatureTree& tree_1 = stopTrees[i];
		NodeQuadLink& link_1 = nodeLinks[i];
		link_1.nodePos = tree_1.centerPos;
		for (int j = i + 1; j < i_max; j++)
		{
			SSG_semiCircleFeatureTree& tree_2 = stopTrees[j];
			NodeQuadLink& link_2 = nodeLinks[j];
			double dist = sqrt(pow(tree_1.centerPt.x - tree_2.centerPt.x, 2) + pow(tree_1.centerPt.y - tree_2.centerPt.y, 2));
			if (dist < neighbourDistTh)
			{
				if (tree_1.centerPt.x < tree_2.centerPt.x)  //left
				{
					if (tree_1.centerPt.y < tree_2.centerPt.y)  //top
					{
						//tree1:RB link is tree_2
						//tree2:LT link is tree_1
						if ( (link_1.RB_link >= 0) || (link_2.LT_link >= 0))
						{
							bool currIsBest = true;
							//判断最佳关系
							if ((link_1.RB_link >= 0) && (dist > link_1.RB_dist))
								currIsBest = false;
							if ((link_2.LT_link >= 0) && (dist > link_2.LT_dist))
								currIsBest = false;
							if (true == currIsBest)
							{
								if (link_1.RB_link >= 0)
								{
									nodeLinks[link_1.RB_link].LT_link = -1;
									nodeLinks[link_1.RB_link].LT_dist = 0;
								}
								link_1.RB_link = j;
								link_1.RB_dist = dist;
								if (link_2.LT_link >= 0)
								{
									nodeLinks[link_2.LT_link].RB_link = -1;
									nodeLinks[link_2.LT_link].RB_dist = 0;
								}
								link_2.LT_link = i;
								link_2.LT_dist = dist;
							}
						}
						else
						{
							link_1.RB_link = j;
							link_1.RB_dist = dist;
							link_2.LT_link = i;
							link_2.LT_dist = dist;
						}

					}
					else//bottom
					{
						//tree1:RT link is tree_2
						//tree2:LB link is tree_1
						if ((link_1.RT_link >= 0) || (link_2.LB_link >= 0))
						{
							bool currIsBest = true;
							//判断最佳关系
							if ((link_1.RT_link >= 0) && (dist > link_1.RT_dist))
								currIsBest = false;
							if ((link_2.LB_link >= 0) && (dist > link_2.LB_dist))
								currIsBest = false;
							if (true == currIsBest)
							{
								if (link_1.RT_link >= 0)
								{
									nodeLinks[link_1.RT_link].LB_link = -1;
									nodeLinks[link_1.RT_link].LB_dist = 0;
								}
								link_1.RT_link = j;
								link_1.RT_dist = dist;
								if (link_2.LB_link >= 0)
								{
									nodeLinks[link_2.LB_link].RT_link = -1;
									nodeLinks[link_2.LB_link].RT_dist = 0;
								}
								link_2.LB_link = i;
								link_2.LB_dist = dist;
							}
						}
						else
						{
							link_1.RT_link = j;
							link_1.RT_dist = dist;
							link_2.LB_link = i;
							link_2.LB_dist = dist;
						}
					}
				}
				else  //right
				{
					if (tree_1.centerPt.y < tree_2.centerPt.y)  //top
					{
						//tree1:LB link is tree_2
						//tree2:RT link is tree_1
						if ((link_1.LB_link >= 0) || (link_2.RT_link >= 0))
						{
							bool currIsBest = true;
							//判断最佳关系
							if ((link_1.LB_link >= 0) && (dist > link_1.LB_dist))
								currIsBest = false;
							if ((link_2.RT_link >= 0) && (dist > link_2.RT_dist))
								currIsBest = false;
							if (true == currIsBest)
							{
								if (link_1.LB_link >= 0)
								{
									nodeLinks[link_1.LB_link].RT_link = -1;
									nodeLinks[link_1.LB_link].RT_dist = 0;
								}
								link_1.LB_link = j;
								link_1.LB_dist = dist;
								if (link_2.RT_link >= 0)
								{
									nodeLinks[link_2.RT_link].LB_link = -1;
									nodeLinks[link_2.RT_link].LB_dist = 0;
								}
								link_2.RT_link = i;
								link_2.RT_dist = dist;
							}
						}
						else
						{
							link_1.LB_link = j;
							link_1.LB_dist = dist;
							link_2.RT_link = i;
							link_2.RT_dist = dist;
						}
					}
					else //bottom
					{
						//tree1:LT link is tree_2
						//tree2:RB link is tree_1
						if ((link_1.LT_link >= 0) || (link_2.RB_link >= 0))
						{
							bool currIsBest = true;
							//判断最佳关系
							if ((link_1.LT_link >= 0) && (dist > link_1.LT_dist))
								currIsBest = false;
							if ((link_2.RB_link >= 0) && (dist > link_2.RB_dist))
								currIsBest = false;
							if (true == currIsBest)
							{
								if (link_1.LT_link >= 0)
								{
									nodeLinks[link_1.LT_link].RB_link = -1;
									nodeLinks[link_1.LT_link].RB_dist = 0;
								}
								link_1.LT_link = j;
								link_1.LT_dist = dist;
								if (link_2.RB_link >= 0)
								{
									nodeLinks[link_2.RB_link].LT_link = -1;
									nodeLinks[link_2.RB_link].LT_dist = 0;
								}
								link_2.RB_link = i;
								link_2.RB_dist = dist;
							}
						}
						else
						{
							link_1.LT_link = j;
							link_1.LT_dist = dist;
							link_2.RB_link = i;
							link_2.RB_dist = dist;
						}
					}
				}
			}
		}
	}
	//（2）建立节点间的相互关系
	std::vector< int> nodeSortingLineID(stopTrees.size(), -1 );  //排序后的行位置
	std::vector< int> nodeProcFlag(stopTrees.size(), 0);  //已经处理标志，”1“已经处理
	std::vector<std::vector<int>> hSorting;
	hSorting.resize(stopTrees.size());
	int startSortingLine = -1;  //第一个检查的Node使用最中间的排序行，这样可以向上和向下添加行
	int gMaxLineId = 0;
	for (int y = 0; y < rows; y++)
	{
		for (int x = 0; x < cols; x++)
		{
			int nodeId = objMask.at<int>(y, x);
			if (nodeId >= 0)
			{
				if (nodeProcFlag[nodeId] > 0)
					continue;
				
				std::vector<int> processBuff;
				processBuff.push_back(nodeId);
				if (startSortingLine < 0)
					startSortingLine = 0;
				else
				{
					//确定行序号
					startSortingLine = 0;  //TBD

				}
				nodeSortingLineID[nodeId] = startSortingLine;
				int maxLineId = startSortingLine;
				_recursiveSorting(
					nodeId,
					hSorting,
					nodeProcFlag,
					nodeSortingLineID,
					nodeLinks,
					true,
					&maxLineId);
				if (gMaxLineId < maxLineId)
					gMaxLineId = maxLineId;
			}
		}
	}
	//计算行Node的距离均值
	double lineNodeDist = 0; 
	int lineNodeDistNum = 0;
	for (int i = 0, i_max = stopTrees.size(); i < i_max; i++)
	{
		NodeQuadLink& link_1 = nodeLinks[i];
		if (link_1.LB_link >= 0)
		{
			if (nodeLinks[link_1.LB_link].LT_link >= 0)
			{
				int id2 = nodeLinks[link_1.LB_link].LT_link;
				double dist = sqrt(pow(stopTrees[i].centerPt.x - stopTrees[id2].centerPt.x, 2) + pow(stopTrees[i].centerPt.y - stopTrees[id2].centerPt.y, 2));
				lineNodeDist += dist;
				lineNodeDistNum++;
			}
		}
		if (link_1.LT_link >= 0)
		{
			if (nodeLinks[link_1.LT_link].LB_link >= 0)
			{
				int id2 = nodeLinks[link_1.LT_link].LB_link;
				double dist = sqrt(pow(stopTrees[i].centerPt.x - stopTrees[id2].centerPt.x, 2) + pow(stopTrees[i].centerPt.y - stopTrees[id2].centerPt.y, 2));
				lineNodeDist += dist;
				lineNodeDistNum++;
			}
		}
		if (link_1.RB_link >= 0)
		{
			if (nodeLinks[link_1.RB_link].RT_link >= 0)
			{
				int id2 = nodeLinks[link_1.RB_link].RT_link;
				double dist = sqrt(pow(stopTrees[i].centerPt.x - stopTrees[id2].centerPt.x, 2) + pow(stopTrees[i].centerPt.y - stopTrees[id2].centerPt.y, 2));
				lineNodeDist += dist;
				lineNodeDistNum++;
			}
		}
		if (link_1.RT_link >= 0)
		{
			if (nodeLinks[link_1.RT_link].RB_link >= 0)
			{
				int id2 = nodeLinks[link_1.RT_link].RB_link;
				double dist = sqrt(pow(stopTrees[i].centerPt.x - stopTrees[id2].centerPt.x, 2) + pow(stopTrees[i].centerPt.y - stopTrees[id2].centerPt.y, 2));
				lineNodeDist += dist;
				lineNodeDistNum++;
			}
		}
	}
	if (lineNodeDistNum > 0)
		lineNodeDist = lineNodeDist / lineNodeDistNum;

	//生成目标坐标点,遗漏点检查
	std::vector<std::vector<SVzNL3DPosition>> sortingNodes;
	for (int i = 0; i <= gMaxLineId; i++)
	{
		std::vector<int>& a_sortingLine = hSorting[i];
		std::vector<SVzNL3DPosition> sortingNodes_line;
		for (int j = 0, j_max = a_sortingLine.size(); j < j_max; j++)
		{
			int nodeId = a_sortingLine[j];
			SVzNL3DPosition a_pt;
			a_pt.nPointIdx = nodeId;
			a_pt.pt3D = stopTrees[nodeId].centerPt;
			sortingNodes_line.push_back(a_pt);
		}
		//遗漏点检查
		_lineFillNullNode(	sortingNodes_line,	nodeLinks,	lineNodeDist );
		sortingNodes.push_back(sortingNodes_line);
	}

	//结果数据第一行和最后一行如果不完整，不使用
	if (sortingNodes.size() > 3)
	{
		if (sortingNodes[0].size() < sortingNodes[2].size())
			sortingNodes.erase(sortingNodes.begin());
	}

	//输出
	for (int i = 0, i_max = sortingNodes.size(); i < i_max; i++)
	{
		std::vector<SVzNL3DPosition>& sortingNodes_line = sortingNodes[i];
		if (sortingNodes_line.size() == 0)
			break;
		std::vector<SVzNL3DPoint> resultLine;
		for (int j = 0, j_max = sortingNodes_line.size(); j < j_max; j++)
			resultLine.push_back(sortingNodes_line[j].pt3D);
		nodePos.push_back(resultLine);
	}
#if _OUTPUT_LINE_PROC_RESULT
	//输出扫描线处理结果
	for (int i = 0, i_max = stopTrees.size(); i < i_max; i++)
	{
		std::vector< SSG_featureSemiCircle>& a_tree_features = stopTrees[i].treeNodes;
		for (int j = 0, j_max = a_tree_features.size(); j < j_max; j++)
		{
			SSG_featureSemiCircle& a_feature = a_tree_features[j];
			for (int m = a_feature.startPtIdx; m <= a_feature.endPtIdx; m++)
				laser3DPoints[a_feature.lineIdx].p3DPosition[m].nPointIdx = 0; //1; //此处nPointIdx转义
#if 0
			laser3DPoints[a_feature.lineIdx].p3DPosition[a_feature.midPtIdx].nPointIdx = 2;
			if(stopTrees[i].treeType == 0)
				laser3DPoints[stopTrees[i].centerPos.x].p3DPosition[stopTrees[i].centerPos.y].nPointIdx = 3;
			else
				laser3DPoints[stopTrees[i].centerPos.x].p3DPosition[stopTrees[i].centerPos.y].nPointIdx = 4;
#endif
				
		}
	}
#endif


}

//计算一个平面调平参数。
//以数据输入中ROI以内的点进行平面拟合，计算调平参数
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
SSG_planeCalibPara sg_getSieveBaseCalibPara(
	SVzNL3DLaserLine* laser3DPoints,
	int lineNum,
	std::vector<SVzNL3DRangeD>& ROIs)
{
	return sg_getPlaneCalibPara_ROIs(laser3DPoints, lineNum, ROIs);
}