algoLib/sourceCode/SG_bagPositioning.cpp

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

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

void sg_lineDataR_RGBD(SVzNLXYZRGBDLaserLine* a_line,
	const double* camPoseR,
	double groundH)
{
	lineDataRT_RGBD(a_line, camPoseR, groundH);
}

//扫描线处理，进行垂直方向的特征提取和生长
void sg_bagPositioning_lineProc(
	SVzNL3DLaserLine* a_line, 
	int lineIdx, 
	int* errCode, 
	std::vector<SSG_lineFeature>& all_vLineFeatures, 
	std::vector<std::vector<int>>& noisePts, 
	const SG_bagPositionParam bagParam)
{
	SSG_lineFeature a_line_features;
	a_line_features.lineIdx = lineIdx;
	//滤波，滤除异常点
	std::vector<SVzNL3DPosition> filterData;
	std::vector<int> lineNoisePts;
	sg_lineDataRemoveOutlier(a_line->p3DPosition, a_line->nPositionCnt, bagParam.filterParam, filterData, lineNoisePts);
	noisePts.push_back(lineNoisePts);
#if BAG_ALGO_USE_CORNER_FEATURE
	sg_getLineCornerFeature(
		filterData.data(), 
		(int)filterData.size(),
		lineIdx, 
		bagParam.cornerParam,  
		&a_line_features);
#else
	sg_getLineLVFeature(
		filterData.data(), 
		filterData.size(), 
		lineIdx, 
		bagParam.slopeParam, 
		bagParam.valleyPara, 
		&a_line_features);
#endif
	all_vLineFeatures.push_back(a_line_features);  //空行也加入，保证能按行号索引
	return;
}

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

bool compareByHeight(const SSG_2DValueI& a, const SSG_2DValueI& b) {
	return a.valueD > b.valueD;
}

bool compareByXValue(const SSG_peakRgnInfo& a, const SSG_peakRgnInfo& b) {
	return a.centerPos.x < b.centerPos.x;
}

//检查是否是孤立突起
bool _LRChkAbnormal(SVzNL3DLaserLine* laser3DPoints, int lineNum, int col, int y, int abnormalChkWin)
{
	bool abormalFlag = true;
	for (int x = col - 1; x >= col - abnormalChkWin; x--)
	{
		if (x >= 0)
		{
			if (laser3DPoints[x].p3DPosition[y].pt3D.z > 1e-4)
			{
				abormalFlag = false;
				break;
			}
		}
	}
	for (int x = col + 1; x <= col + abnormalChkWin; x++)
	{
		if (x < lineNum)
		{
			if (laser3DPoints[x].p3DPosition[y].pt3D.z > 1e-4)
			{
				abormalFlag = false;
				break;
			}
		}
	}
	return abormalFlag;
}
//检查是否是孤立突起
bool _TBChkAbnormal(SVzNL3DLaserLine* laser3DPoints, int ptNum, int x, int row, int abnormalChkWin)
{
	bool abormalFlag = true;
	for (int y = row - 1; y >= row - abnormalChkWin; y--)
	{
		if (y >= 0)
		{
			if (laser3DPoints[x].p3DPosition[y].pt3D.z > 1e-4)
			{
				abormalFlag = false;
				break;
			}
		}
	}
	for (int y = row + 1; y <= row + abnormalChkWin; y++)
	{
		if (y < ptNum)
		{
			if (laser3DPoints[x].p3DPosition[y].pt3D.z > 1e-4)
			{
				abormalFlag = false;
				break;
			}
		}
	}
	return abormalFlag;
}

void _updateObjROI(SSG_ROIRectD* objROI, SVzNL3DPosition* a_pt)
{
	if ( (objROI->right - objROI->left) < -1e-4)
	{
		objROI->left = a_pt->pt3D.x;
		objROI->right = a_pt->pt3D.x;
		objROI->top = a_pt->pt3D.y;
		objROI->bottom = a_pt->pt3D.y;
	}
	else
	{
		if(objROI->left > a_pt->pt3D.x)
			objROI->left = a_pt->pt3D.x;
		if(objROI->right < a_pt->pt3D.x)
			objROI->right = a_pt->pt3D.x;
		if(objROI->top > a_pt->pt3D.y)
			objROI->top = a_pt->pt3D.y;
		if(objROI->bottom < a_pt->pt3D.y)
			objROI->bottom = a_pt->pt3D.y;
	}
	return;
}

double _getPosMeanZ(
	SVzNL3DLaserLine * laser3DPoints,
	int lineNum, 
	SVzNL2DPoint pos, 
	int winSize)
{
	int ptNum = laser3DPoints[0].nPositionCnt;
	double meanZ = 0;
	double vldNum = 0;
	for (int i = -winSize; i <= winSize; i++)
	{
		for (int j = -winSize; j <= winSize; j++)
		{
			int x = pos.x + i;
			int y = pos.y + j;
			if ((x >= 0) && (x < ptNum) && (y >= 0) && (y < lineNum))
			{
				SVzNL3DPosition* a_pt = &laser3DPoints[x].p3DPosition[y];
				if (a_pt->pt3D.z > 1e-4)
				{
					meanZ += a_pt->pt3D.z;
					vldNum++;
				}
			}
		}
	}
	if (vldNum > 0)
		meanZ = meanZ / vldNum;
	return meanZ;
}

int _computeMaxEmptyLen(std::vector<int>& mask)
{
	int maxLen = 0;
	int emptyLen = 0;
	int preMaskValue = 0;
	for (int i = 0, i_max = (int)mask.size(); i < i_max; i++)
	{
		if (0 == mask[i])
		{
			emptyLen++;
			if (i == i_max - 1)
			{
				if (maxLen < emptyLen)
					maxLen = emptyLen;
			}
		}
		else
		{
			if (preMaskValue == 0)
			{
				if (maxLen < emptyLen)
					maxLen = emptyLen;
			}
			emptyLen = 0;
		}
		preMaskValue = mask[i];
	}
	return maxLen;
}

//Validate object
//1、边界检查。边界点连续左右5cm内没有feature，认为是空缺。空缺长度大于0.75W,无效目标
//2、中心高度检查。若bestPt（5x5)附近的高度低于区域平均高度，无效目标
//3、不能有一个内部的tree长度超过宽度的0.75W
//需要考虑边界是Edge的情况。Edge不在TreeInfo中
bool _validateObj_treeROI(
	SSG_ROIRectD objROI, 
	const SSG_ROIRectD globalROI, 
	std::vector< SSG_conotourPair>& contourPair_TB,
	std::vector< SSG_conotourPair>& contourPair_LR,
	std::vector<SSG_treeInfo>& allTreesInfo)
{
	double isSameSideTh = 100; //objROI的边与GlobalROI的边的距离在5cm内认为是同一条边
	double treeCombineTh = 30.0;  //两个Tree合并的距离门限
	double innerInvalidLenTh = 0.9;
	double sideInvalidLenTh = 0.8;
	double side_diff[4];
	side_diff[0] = objROI.left - globalROI.left;
	side_diff[1] = globalROI.right - objROI.right;
	side_diff[2] = objROI.top - globalROI.top;
	side_diff[3] = globalROI.bottom - objROI.bottom;

	bool sideIdScanEdge[4];
	for (int i = 0; i < 4; i++)
	{
		if (side_diff[i] < isSameSideTh)
			sideIdScanEdge[i] = true;
		else
			sideIdScanEdge[i] = false;
	}

	//1、边界检查。边界点连续左右5cm内没有feature，认为是空缺。空缺长度大于0.75W,无效目标
	//3、不能有一个内部的tree长度超过宽度的0.75W
	std::vector<SVzNLRangeD> topTreeRanges;
	std::vector<SVzNLRangeD> btmTreeRanges;
	std::vector<SVzNLRangeD> leftTreeRanges;
	std::vector<SVzNLRangeD> rightTreeRanges;
	std::vector<SSG_ROIRectD> innerTreeROI_v;
	std::vector<SSG_ROIRectD> innerTreeROI_h;
	for (int i = 0, i_max = (int)allTreesInfo.size(); i < i_max; i++)
	{
		SSG_treeInfo* a_treeInfo = &allTreesInfo[i];
		if (a_treeInfo->treeType == 0)
			continue;
		//检查tree的ROI与objROI是否有交叉或在objROI的内部
		if ((a_treeInfo->roi.right > objROI.left) && (objROI.right > a_treeInfo->roi.left) &&
			(a_treeInfo->roi.bottom > objROI.top) && (objROI.bottom > a_treeInfo->roi.top))  //有交叉
		{
			SSG_ROIRectD commonROI;
			commonROI.left = objROI.left < a_treeInfo->roi.left ? a_treeInfo->roi.left : objROI.left;
			commonROI.right = objROI.right > a_treeInfo->roi.right ? a_treeInfo->roi.right : objROI.right;
			commonROI.top = objROI.top < a_treeInfo->roi.top ? a_treeInfo->roi.top : objROI.top;
			commonROI.bottom = objROI.bottom > a_treeInfo->roi.bottom ? a_treeInfo->roi.bottom : objROI.bottom;
			if (a_treeInfo->vTreeFlag == 1)  //vTree为left-right方向
			{
				double top_diff = commonROI.top - objROI.top;
				double btm_diff = objROI.bottom - commonROI.bottom;
				SVzNLRangeD a_range;
				a_range.min = commonROI.left;
				a_range.max = commonROI.right;
				if (top_diff < isSameSideTh)
					topTreeRanges.push_back(a_range);
				else if (btm_diff < isSameSideTh)
					btmTreeRanges.push_back(a_range);
				else //在内部
				{
					innerTreeROI_v.push_back(commonROI);
				}
			}
			else
			{
				double left_diff = commonROI.left - objROI.left;
				double right_diff = objROI.right - commonROI.right;
				SVzNLRangeD a_range;
				a_range.min = commonROI.top;
				a_range.max = commonROI.bottom;
				if (left_diff < isSameSideTh)
					leftTreeRanges.push_back(a_range);
				else if (right_diff < isSameSideTh)
					rightTreeRanges.push_back(a_range);
				else //在内部
					innerTreeROI_h.push_back(commonROI);
			}
		}
		else  //外部
		{
			if (a_treeInfo->vTreeFlag == 1)  //vTree为left-right方向
			{
				if ((a_treeInfo->roi.right > objROI.left) && (objROI.right > a_treeInfo->roi.left)) //LR方向有公共部分
				{
					SVzNLRangeD a_range;
					a_range.min = objROI.left < a_treeInfo->roi.left ? a_treeInfo->roi.left : objROI.left;
					a_range.max = objROI.right > a_treeInfo->roi.right ? a_treeInfo->roi.right : objROI.right;
					double up_diff = objROI.top - a_treeInfo->roi.bottom;
					double down_diff = a_treeInfo->roi.top - objROI.bottom;
					if ( (a_treeInfo->roi.bottom < objROI.top) && (up_diff < isSameSideTh))  //在上部外边
						topTreeRanges.push_back(a_range);
					else if ( (a_treeInfo->roi.top > objROI.bottom) && (down_diff < isSameSideTh)) //在下部外边
						btmTreeRanges.push_back(a_range);
				}
			}
			else
			{
				if ((a_treeInfo->roi.bottom > objROI.top) && (objROI.bottom > a_treeInfo->roi.top)) //TB方向有公共部分
				{
					SVzNLRangeD a_range;
					a_range.min = objROI.top < a_treeInfo->roi.top ? a_treeInfo->roi.top : objROI.top;
					a_range.max = objROI.bottom > a_treeInfo->roi.bottom ? a_treeInfo->roi.bottom : objROI.bottom;
					double left_diff = objROI.left - a_treeInfo->roi.right;
					double right_diff = a_treeInfo->roi.left - objROI.right;
					if ((a_treeInfo->roi.right < objROI.left) && (left_diff < isSameSideTh))  //在左部外边
						leftTreeRanges.push_back(a_range);
					else if ((a_treeInfo->roi.left > objROI.right) && (right_diff < isSameSideTh)) //在右部外边
						rightTreeRanges.push_back(a_range);
				}
			}
		}
	}
	//将内部Tree合并
	std::vector<SVzNLRangeD> innerTreeRange_v;
	int treeROI_size = (int)innerTreeROI_v.size();
	for (int i = 0; i < treeROI_size; i++)
	{
		SSG_ROIRectD* a_roi = &innerTreeROI_v[i];
		if (a_roi->right < a_roi->left)
			continue;
		SVzNLRangeD a_range = { a_roi->left, a_roi->right };
		for (int j = i+1; j < treeROI_size; j++)
		{
			SSG_ROIRectD* chk_roi = &innerTreeROI_v[j];
			double LR_diff_1 = abs(chk_roi->right - a_roi->left);
			double LR_diff_2 = abs(a_roi->right - chk_roi->left);
			if ((a_roi->bottom > chk_roi->top) && (chk_roi->bottom > a_roi->top) &&
				((LR_diff_1 < treeCombineTh) || (LR_diff_2 < treeCombineTh)))  //合并
			{
				a_range.min = a_range.min > chk_roi->left ? chk_roi->left : a_range.min;
				a_range.max = a_range.max < chk_roi->right ? chk_roi->right : a_range.max;
				chk_roi->right = chk_roi->left - 1;
			}
		}
		innerTreeRange_v.push_back(a_range);
	}
	std::vector<SVzNLRangeD> innerTreeRange_h;
	treeROI_size = (int)innerTreeROI_h.size();
	for (int i = 0; i < treeROI_size; i++)
	{
		SSG_ROIRectD* a_roi = &innerTreeROI_h[i];
		if (a_roi->bottom < a_roi->top)
			continue;
		SVzNLRangeD a_range = { a_roi->top, a_roi->bottom };
		for (int j = i+1; j < treeROI_size; j++)
		{
			SSG_ROIRectD* chk_roi = &innerTreeROI_h[j];
			double TB_diff_1 = abs(chk_roi->bottom - a_roi->top);
			double TB_diff_2 = abs(a_roi->bottom - chk_roi->top);
			if ((a_roi->right > chk_roi->left) && (chk_roi->right > a_roi->left) &&
				((TB_diff_1 < treeCombineTh) || (TB_diff_2 < treeCombineTh)))  //合并
			{
				a_range.min = a_range.min > chk_roi->top ? chk_roi->top : a_range.min;
				a_range.max = a_range.max < chk_roi->bottom ? chk_roi->bottom : a_range.max;
				chk_roi->bottom = chk_roi->top - 1;
			}
		}
		innerTreeRange_h.push_back(a_range);
	}
	//分析
	bool isValid = true;
	double obj_w = objROI.right - objROI.left;
	double obj_h = objROI.bottom - objROI.top;
	for (int i = 0; i < innerTreeRange_v.size(); i++)
	{
		double common_w = innerTreeRange_v[i].max - innerTreeRange_v[i].min;
		if(common_w > obj_w * innerInvalidLenTh)
			return false;
	}
	for (int i = 0; i < innerTreeRange_h.size(); i++)
	{
		double common_h = innerTreeRange_h[i].max - innerTreeRange_h[i].min;
		if (common_h > obj_h * innerInvalidLenTh)
			return false;
	}
	std::vector<int> emptyMask;
	int size = (int)((objROI.right - objROI.left) * 10 + 1);
	
	if (false == sideIdScanEdge[2])  //top
	{
		emptyMask.resize(size);
		for (int i = 0; i < size; i++)
			emptyMask[i] = 0;
		for (int i = 0; i < contourPair_TB.size(); i++)
		{
			double x = contourPair_TB[i].contourPt_0.edgePt.x;
			int x_idx = (int)((x - objROI.left) * 10);
			if ((x_idx >= 0) && (x_idx < size))
				emptyMask[x_idx] = 1;
		}
		for (int i = 0; i < topTreeRanges.size(); i++)
		{
			int start = (int)((topTreeRanges[i].min - objROI.left) * 10);
			int end = (int)((topTreeRanges[i].max - objROI.left) * 10);
			start = start < 0 ? 0 : start;
			end = end >= size ? (size - 1) : end;
			for (int j = start; j <= end; j++)
				emptyMask[j] = 1;
		}

		int maxEmptyLen = _computeMaxEmptyLen(emptyMask);
		double emptyLen = (double)maxEmptyLen / 10.0;
		if (emptyLen > obj_w * sideInvalidLenTh)
			return false;
	}
	if (false == sideIdScanEdge[3])//bottom
	{
		emptyMask.resize(size);
		for (int i = 0; i < size; i++)
			emptyMask[i] = 0;
		for (int i = 0; i < contourPair_TB.size(); i++)
		{
			double x = contourPair_TB[i].contourPt_1.edgePt.x;
			int x_idx = (int)((x - objROI.left) * 10);
			if ((x_idx >= 0) && (x_idx < size))
				emptyMask[x_idx] = 1;
		}
		for (int i = 0; i < btmTreeRanges.size(); i++)
		{
			int start = (int)((btmTreeRanges[i].min - objROI.left) * 10);
			int end = (int)((btmTreeRanges[i].max - objROI.left) * 10);
			start = start < 0 ? 0 : start;
			end = end >= size ? (size - 1) : end;
			for (int j = start; j <= end; j++)
				emptyMask[j] = 1;
		}
		int maxEmptyLen = _computeMaxEmptyLen(emptyMask);
		double emptyLen = (double)maxEmptyLen / 10.0;
		if (emptyLen > obj_w * sideInvalidLenTh)
			return false;
	}

	size = (int)((objROI.bottom - objROI.top) * 10 + 1);
	if (false == sideIdScanEdge[0])
	{
		emptyMask.resize(size);
		for (int i = 0; i < size; i++)
			emptyMask[i] = 0;
		for (int i = 0; i < contourPair_LR.size(); i++)
		{
			double y = contourPair_LR[i].contourPt_0.edgePt.y;
			int y_idx = (int)((y - objROI.top) * 10);
			if( (y_idx >= 0) &&(y_idx < size))
				emptyMask[y_idx] = 1;
		}
		for (int i = 0; i < leftTreeRanges.size(); i++)
		{
			int start = (int)((leftTreeRanges[i].min - objROI.top) * 10);
			int end = (int)((leftTreeRanges[i].max - objROI.top) * 10);
			start = start < 0 ? 0 : start;
			end = end >= size ? (size - 1) : end;
			for (int j = start; j <= end; j++)
				emptyMask[j] = 1;
		}
		int maxEmptyLen = _computeMaxEmptyLen(emptyMask);
		double emptyLen = (double)maxEmptyLen / 10.0;
		if (emptyLen > obj_h * sideInvalidLenTh)
			return false;
	}
	if (false == sideIdScanEdge[1])
	{
		emptyMask.resize(size);
		for (int i = 0; i < size; i++)
			emptyMask[i] = 0;
		for (int i = 0; i < contourPair_LR.size(); i++)
		{
			double y = contourPair_LR[i].contourPt_1.edgePt.y;
			int y_idx = (int)((y - objROI.top) * 10);
			if ((y_idx >= 0) && (y_idx < size))
				emptyMask[y_idx] = 1;
		}
		for (int i = 0; i < rightTreeRanges.size(); i++)
		{
			int start = (int)((rightTreeRanges[i].min - objROI.top) * 10);
			int end = (int)((rightTreeRanges[i].max - objROI.top) * 10);
			start = start < 0 ? 0 : start;
			end = end >= size ? (size - 1) : end;
			for (int j = start; j <= end; j++)
				emptyMask[j] = 1;
		}
		int maxEmptyLen = _computeMaxEmptyLen(emptyMask);
		double emptyLen = (double)maxEmptyLen / 10.0;
		if (emptyLen > obj_h * sideInvalidLenTh)
			return false;
	}
	return true;
}

typedef struct
{
	int u;
	int v;
	SVzNL3DPosition* ptPtr;
}SSG_pt2DIndexing;
SSG_peakRgnInfo _getRgnObjInfo(
	SVzNL3DLaserLine* laser3DPoints,
	int lineNum,
	std::vector<int>& edgeFlag,  //edge标志，属于本Rgn的EdgeId被置为1 
	std::vector< SSG_conotourPair>& contourPair_TB,
	std::vector< SSG_conotourPair>& contourPair_LR,
	std::vector<SSG_treeInfo>& allTreesInfo,
	int rgnBlockFlag,
	const int peakRgnId,
	const SSG_ROIRectD globalROI,
	const SG_bagPositionParam algoParam, 
	bool labelBlockedObj, 
	int* objBlockedFlag)
{
	*objBlockedFlag = 0;
	SSG_peakRgnInfo a_peakRgn;
	memset(&a_peakRgn, 0, sizeof(SSG_peakRgnInfo));

	std::vector< SSG_2DValueI> rgnContour;
	for (int i = 0; i < contourPair_TB.size(); i++)
	{
		SSG_conotourPair& a_ptPair = contourPair_TB[i];
		SSG_2DValueI a_filterPt = { a_ptPair.contourPt_0.x, a_ptPair.contourPt_0.y, a_ptPair.contourPt_0.type, a_ptPair.contourPt_0.edgePt.z };
		rgnContour.push_back(a_filterPt);
		a_filterPt = { a_ptPair.contourPt_1.x, a_ptPair.contourPt_1.y, a_ptPair.contourPt_1.type, a_ptPair.contourPt_1.edgePt.z };
		rgnContour.push_back(a_filterPt);
	}
	for (int i = 0; i < contourPair_LR.size(); i++)
	{
		SSG_conotourPair& a_ptPair = contourPair_LR[i];
		SSG_2DValueI a_filterPt = { a_ptPair.contourPt_0.x, a_ptPair.contourPt_0.y, a_ptPair.contourPt_0.type, a_ptPair.contourPt_0.edgePt.z };
		rgnContour.push_back(a_filterPt);
		a_filterPt = { a_ptPair.contourPt_1.x, a_ptPair.contourPt_1.y, a_ptPair.contourPt_1.type, a_ptPair.contourPt_1.edgePt.z };
		rgnContour.push_back(a_filterPt);
	}

	//最小外接矩
// 生成一些带有噪声的数据，用于拟合矩形
	std::vector<cv::Point2f> points;
	SVzNLRect roi = { -1,-1,-1,-1 };
	for (int m = 0; m < rgnContour.size(); m++)
	{
		//生成ROI
		if (roi.left < 0)
			roi = { rgnContour[m].x , rgnContour[m].x , rgnContour[m].y, rgnContour[m].y };
		else
		{
			if (roi.left > rgnContour[m].x)
				roi.left = rgnContour[m].x;
			if (roi.right < rgnContour[m].x)
				roi.right = rgnContour[m].x;
			if (roi.top > rgnContour[m].y)
				roi.top = rgnContour[m].y;
			if (roi.bottom < rgnContour[m].y)
				roi.bottom = rgnContour[m].y;
		}

		int line = rgnContour[m].x;
		float x = (float)laser3DPoints[line].p3DPosition[rgnContour[m].y].pt3D.x;
		float y = (float)laser3DPoints[line].p3DPosition[rgnContour[m].y].pt3D.y;
		points.push_back(cv::Point2f(x, y));
	}
	if (points.size() == 0)
		return a_peakRgn;

	// 最小外接矩形
	cv::RotatedRect rect = minAreaRect(points);
	cv::Point2f vertices[4];
	rect.points(vertices);
	double width = rect.size.width;  //投影的宽和高， 对应袋子的长和宽
	double height = rect.size.height;
	if (width < height)
	{
		double tmp = height;
		height = width;
		width = tmp;
	}
	//计算姿态角vertices[0]是旋转点
	double dist_v0v3 = sqrt(pow(vertices[0].x - vertices[3].x, 2) + pow(vertices[0].y - vertices[3].y, 2));
	double width_diff = abs(dist_v0v3 - width);
	double pose_yaw;
	if (CV_VERSION == "3.2.0")
	{
		if (width_diff < 10.0)//width方向
		{
			pose_yaw = -rect.angle;
		}
		else //长度方向
		{
			pose_yaw = -rect.angle - 90;
			if (pose_yaw < -90)
				pose_yaw = 180 + pose_yaw;
		}
	}
	else  //if (CV_VERSION == "4.8.0")
	{
		if (width_diff < 10.0)  //width方向
		{
			pose_yaw = -rect.angle;
		}
		else//长度方向
		{
			pose_yaw = -rect.angle + 90;
			if (pose_yaw > 90)
				pose_yaw = pose_yaw - 180;
		}
	}


	//计算vertices[0]-vertices[3]的距离， 

	//
#if 0	 
	//检查目标可能的宽高。以最接近的边为基准
	double chkWidth, chkHeight;
	double diff_W_bL = abs(width - algoParam.bagParam.bagL);
	double diff_W_bW = abs(width - algoParam.bagParam.bagW);
	double diff_H_bL = abs(height - algoParam.bagParam.bagL);
	double diff_H_bW = abs(height - algoParam.bagParam.bagW);
	if ((diff_W_bL < diff_W_bW) && (diff_W_bL < diff_H_bL) && (diff_W_bL < diff_H_bW))
	{
		chkWidth = width;
		chkHeight = height;
	}
	else if ((diff_W_bW < diff_W_bL) && (diff_W_bW < diff_H_bL) && (diff_W_bW < diff_H_bW))
	{
		chkWidth = height;
		chkHeight = width;
	}
	else if ((diff_H_bL < diff_W_bL) && (diff_H_bL < diff_W_bW) && (diff_H_bL < diff_H_bW))
	{
		chkWidth = height;
		chkHeight = width;
	}
	else
	{
		chkWidth = width;
		chkHeight = height;
	}
#else
	double chkWidth, chkHeight;
	chkWidth = width;
	chkHeight = height;
#endif

	if ((rgnBlockFlag == 1) && ((chkWidth < algoParam.bagParam.bagL * 0.8) || (chkHeight < algoParam.bagParam.bagW * 0.8)))
		return a_peakRgn;

	if ((chkWidth < algoParam.bagParam.bagL * 0.7) || (chkHeight < algoParam.bagParam.bagW * 0.7) ||
		(chkWidth > algoParam.bagParam.bagL * 1.3) || (chkHeight > algoParam.bagParam.bagW * 1.3))  //尺寸过滤，投影的宽和高， 对应袋子的长和宽
	{
		return a_peakRgn;
	}

	int pkRgnId = peakRgnId; 
	//取中心点
	cv::Point2f center = rect.center;
	//以vertices[0]为中心旋转，将最小外接矩与坐标系平行。vertices[0]-vertices[3]为X轴
	double angle = -rect.angle;
	double sinTheta = sin(PI * angle / 180);
	double cosTheta = cos(PI * angle / 180);
	cv::Point2f v[4];
	v[0].x = 0; v[0].y = 0;
	v[1] = _rotate2D(cv::Point2f(vertices[1].x - vertices[0].x, vertices[1].y - vertices[0].y), sinTheta, cosTheta);
	v[2] = _rotate2D(cv::Point2f(vertices[2].x - vertices[0].x, vertices[2].y - vertices[0].y), sinTheta, cosTheta);
	v[3] = _rotate2D(cv::Point2f(vertices[3].x - vertices[0].x, vertices[3].y - vertices[0].y), sinTheta, cosTheta);

	//寻找与中心点最近的3D点, 同时标注寻找到的面
	SVzNL2DPoint bestPt2D = { -1, -1 };
	double minDist = -1;
	SVzNL3DPosition* best_pt = NULL;
	std::vector<SVzNL3DPosition*> all_rgnPts;
	//记录新的边界点进行边界点属性继承
	std::vector<SSG_pt2DIndexing> top_side;
	top_side.resize(roi.right - roi.left + 1);
	std::vector<SSG_pt2DIndexing> bottom_side;
	bottom_side.resize(roi.right - roi.left + 1);
	std::vector<SSG_pt2DIndexing> left_side;
	left_side.resize(roi.bottom - roi.top + 1);
	std::vector<SSG_pt2DIndexing> right_side;
	right_side.resize(roi.bottom - roi.top + 1);

	int blockedPtNum = 0;
	//统计已有目标的数据，计算遮挡面积
	//统计在目标区域内的不属于目标的JUMP点
	SSG_ROIRectD objROI = { 0,-1,0,0 };  //目标ROI（XOY平面）
	double objMeanZ = 0;  //计算rgn的平均z高度，用于判断目标是否为合格目标
	int rgnPtNum = 0;
	int innerJumpPtNum = 0;
	for (int m = roi.left; m <= roi.right; m++)
	{
		for (int n = roi.top; n <= roi.bottom; n++)
		{
if ((m == 226) && (n == 742))
	int kkk = 1;

			SVzNL3DPosition* a_pt = &(laser3DPoints[m].p3DPosition[n]);
			if (a_pt->pt3D.z > 1e-4)
			{
				double dist = sqrt(pow(a_pt->pt3D.x - center.x, 2) + pow(a_pt->pt3D.y - center.y, 2));
				if (minDist < 0)
				{
					bestPt2D = { m, n };
					minDist = dist;
					best_pt = a_pt;
				}
				else
				{
					if (minDist > dist)
					{
						bestPt2D = { m, n };
						minDist = dist;
						best_pt = a_pt;
					}
				}
				cv::Point2f r_pt = _rotate2D(cv::Point2f((float)(a_pt->pt3D.x - vertices[0].x), (float)(a_pt->pt3D.y - vertices[0].y)), sinTheta, cosTheta);
				if ((r_pt.x >= 0) && (r_pt.x <= rect.size.width) && (r_pt.y <= 0) && (r_pt.y >= -rect.size.height))
				{
					int existId = (a_pt->nPointIdx >> 8) & 0xff;
					if (existId > 0)
						blockedPtNum++;
					//统计ROI
					_updateObjROI(&objROI, a_pt);
					objMeanZ += a_pt->pt3D.z;  //计算rgn的平均z高度，用于判断目标是否为合格目标
					rgnPtNum ++;
					//点在最小外接矩内,记录
					all_rgnPts.push_back(a_pt);
					//a_pt->nPointIdx += pkRgnId;

					//记录新的边界点进行边界点属性继承
					int TB_side_id = m - roi.left;
					if (NULL == top_side[TB_side_id].ptPtr)
					{
						top_side[TB_side_id].ptPtr = a_pt;
						top_side[TB_side_id].u = m;
						top_side[TB_side_id].v = n;
					}
					if (NULL == bottom_side[TB_side_id].ptPtr)
					{
						bottom_side[TB_side_id].ptPtr = a_pt;
						bottom_side[TB_side_id].u = m;
						bottom_side[TB_side_id].v = n;
					}
					else if (bottom_side[TB_side_id].v < n)
					{
						bottom_side[TB_side_id].ptPtr = a_pt;
						bottom_side[TB_side_id].u = m;
						bottom_side[TB_side_id].v = n;
					}

					int LR_side_id = n - roi.top;
					if (NULL == left_side[LR_side_id].ptPtr)
					{
						left_side[LR_side_id].ptPtr = a_pt;
						left_side[LR_side_id].u = m;
						left_side[LR_side_id].v = n;
					}
					else if (left_side[LR_side_id].u > m)
					{
						left_side[LR_side_id].ptPtr = a_pt;
						left_side[LR_side_id].u = m;
						left_side[LR_side_id].v = n;
					}

					if (NULL == right_side[LR_side_id].ptPtr)
					{
						right_side[LR_side_id].ptPtr = a_pt;
						right_side[LR_side_id].u = m;
						right_side[LR_side_id].v = n;
					}
					else if (right_side[LR_side_id].u < m)
					{
						right_side[LR_side_id].ptPtr = a_pt;
						right_side[LR_side_id].u = m;
						right_side[LR_side_id].v = n;
					}

					//检查JUMP点
					const double inner_distTh = 100;
					int treeId = a_pt->nPointIdx >> 16;
					int vType = (a_pt->nPointIdx) & 0x00ff;//屏蔽rgnID
					int hType = vType >> 4;
					vType &= 0x0f;
					if ((LINE_FEATURE_L_JUMP_H2L == vType) || (LINE_FEATURE_L_JUMP_L2H == vType) ||
						(LINE_FEATURE_L_JUMP_H2L == hType) || (LINE_FEATURE_L_JUMP_L2H == hType))
					{
						if (0 == edgeFlag[treeId])
						{
							double dist_1 = abs(r_pt.x);
							double dist_2 = abs(r_pt.x - rect.size.width);
							double dist_3 = abs(r_pt.y);
							double dist_4 = abs(-rect.size.height - r_pt.y);
							if ((dist_1 > inner_distTh) && (dist_2 > inner_distTh) && (dist_3 > inner_distTh) && (dist_4 > inner_distTh))
								innerJumpPtNum++;
						}
					}
				}
			}
		}
	}
	if(rgnPtNum > 0)
		objMeanZ = objMeanZ / rgnPtNum;  //计算rgn的平均z高度，用于判断目标是否为合格目标
	
	//迭代检查：记录检查到的Jump点，判断Jump点在ROI内部的数量。大于20为
	if(innerJumpPtNum > 30)
		return a_peakRgn;

	if (best_pt)
	{
		//遮挡检查
		//五花垛遮挡面积为1/3以上，取1/4为门限 
		if (blockedPtNum > (all_rgnPts.size() / 3))  //被遮挡
			*objBlockedFlag = 1;
	
		//Validate object
		//1、边界检查。边界点连续左右5cm内没有feature，认为是空缺。空缺长度大于0.75W,无效目标
		//2、中心高度检查。若bestPt（5x5)附近的高度低于区域平均高度，无效目标
		//3、不能有一个内部的tree长度超过宽度的0.75W
		double cneterMeanZ = _getPosMeanZ(laser3DPoints, lineNum, bestPt2D, 3);
		if(cneterMeanZ > objMeanZ+50)
			return a_peakRgn;

		bool isValid = _validateObj_treeROI(
			objROI, 
			globalROI, 
			contourPair_TB,
			contourPair_LR,
			allTreesInfo);
		if(false == isValid)
			return a_peakRgn;

		if( (*objBlockedFlag == 0) || (true == labelBlockedObj))
		{
			for (int m = 0; m < all_rgnPts.size(); m++)
			{
				int existId = (all_rgnPts[m]->nPointIdx >> 8) & 0xff; //中间8位是rgnID，低8位是featureType
				existId += pkRgnId;
				if (existId >= 255)
					existId = existId % 255 + 1;
				all_rgnPts[m]->nPointIdx += existId << 8;  //Rgn标注
			}
		}

		//边界属性继承
		for (int m = 0; m < rgnContour.size(); m++)
		{
			SSG_2DValueI* a_contour = &rgnContour[m];
			if (a_contour->sideID == 1)  //top
			{
				int top_id = a_contour->x - roi.left;
				if (top_side[top_id].ptPtr)
				{
					top_side[top_id].ptPtr->nPointIdx &= 0xfffffff0;
					top_side[top_id].ptPtr->nPointIdx += (a_contour->value) & 0xf;
				}
			}
			else if (a_contour->sideID == 2) //bottom
			{
				int btm_id = a_contour->x - roi.left;
				if (bottom_side[btm_id].ptPtr)
				{
					bottom_side[btm_id].ptPtr->nPointIdx &= 0xfffffff0;
					bottom_side[btm_id].ptPtr->nPointIdx += (a_contour->value)&0xf;
				}
			}
			else if (a_contour->sideID == 3)  //left
			{
				int left_id = a_contour->y - roi.top;
				if (left_side[left_id].ptPtr)
				{
					left_side[left_id].ptPtr->nPointIdx &= 0xffffff0f;
					left_side[left_id].ptPtr->nPointIdx += ((a_contour->value)&0xf) << 4;
				}
			}
			else if (a_contour->sideID == 4) //right
			{
				int right_id = a_contour->y - roi.top;
				if (right_side[right_id].ptPtr)
				{
					right_side[right_id].ptPtr->nPointIdx &= 0xffffff0f;
					right_side[right_id].ptPtr->nPointIdx += ((a_contour->value) & 0xf) << 4;
				}
			}
		}

		a_peakRgn.objSize.dWidth = width;
		a_peakRgn.objSize.dHeight = height;
		a_peakRgn.pkRgnIdx = peakRgnId;
		a_peakRgn.pos2D = bestPt2D;
		a_peakRgn.centerPos = { best_pt->pt3D.x, best_pt->pt3D.y, best_pt->pt3D.z, 0, 0, pose_yaw };
	}
	return a_peakRgn;
}

bool _checkValueMatch(double data1, double data2, const SVzNLRangeD matchTh)
{
	if ( (data1 > data2+matchTh.min) && (data1 < data2 + matchTh.max))
		return true;
	else
		return false;
}

inline bool _chkInRange(SVzNLRangeD range, double value)
{
	if ((value >= range.min) && (value <= range.max))
		return true;
	else
		return false;
}

inline bool _chkInRange_2(double range_min, double range_max, double value)
{
	if ((value >= range_min) && (value <= range_max))
		return true;
	else
		return false;
}

//按可信度寻找。首先寻找可信度最高的，然后寻找中等可信度的，最后寻找低可信度的
int _getBestMatch(
	std::vector<SSG_matchPair>& matchPairs,
	double nominalValue,
	const SVzNLRangeD matchTh,
	bool isTB,
	double seed_pos)
{
	int id = -1;
	SVzNLRangeD matchRng = {0, 0};
	int matchPts = 0;
	//首先比较高可信度
	for (int i = 0; i < matchPairs.size(); i++)
	{
		if ( (matchPairs[i].matchValue > nominalValue +matchTh.min) && (matchPairs[i].matchValue < nominalValue + matchTh.max) && (matchPairs[i].matchType == 2))
		{
			if (id < 0)
			{
				id = i;
				if (true == isTB)
				{
					matchRng.min = matchPairs[i].roi.left;
					matchRng.max = matchPairs[i].roi.right;
				}
				else
				{
					matchRng.min = matchPairs[i].roi.top;
					matchRng.max = matchPairs[i].roi.bottom;
				}
				matchPts = matchPairs[i].matchPts;
			}
			else
			{
				SVzNLRangeD currRng;
				if (true == isTB)
				{
					currRng.min = matchPairs[i].roi.left;
					currRng.max = matchPairs[i].roi.right;
				}
				else
				{
					currRng.min = matchPairs[i].roi.top;
					currRng.max = matchPairs[i].roi.bottom;
				}

				bool bestInRange = _chkInRange(matchRng, seed_pos);
				bool currInRange = _chkInRange(currRng, seed_pos);
				if( (false == bestInRange) && (true == currInRange))
				{
					matchRng = currRng;
					id = i;
					matchPts = matchPairs[i].matchPts;
				}
				else if( ((false == bestInRange) && (false == currInRange)) || ((true == bestInRange) && (true == currInRange)))
				{
					if (matchPts < matchPairs[i].matchPts)
					{
						matchRng = currRng;
						id = i;
						matchPts = matchPairs[i].matchPts;
					}
				}
			}
		}
	}
	//if (id < 0)
	{
		//比较中可信度
		for (int i = 0; i < matchPairs.size(); i++)
		{
			if ((matchPairs[i].matchValue > nominalValue + matchTh.min) && (matchPairs[i].matchValue < nominalValue + matchTh.max) && (matchPairs[i].matchType == 1))
			{
				if (id < 0)
				{
					id = i;
					if (true == isTB)
					{
						matchRng.min = matchPairs[i].roi.left;
						matchRng.max = matchPairs[i].roi.right;
					}
					else
					{
						matchRng.min = matchPairs[i].roi.top;
						matchRng.max = matchPairs[i].roi.bottom;
					}
					matchPts = matchPairs[i].matchPts;
				}
				else
				{
					SVzNLRangeD currRng;
					if (true == isTB)
					{
						currRng.min = matchPairs[i].roi.left;
						currRng.max = matchPairs[i].roi.right;
					}
					else
					{
						currRng.min = matchPairs[i].roi.top;
						currRng.max = matchPairs[i].roi.bottom;
					}

					bool bestInRange = _chkInRange(matchRng, seed_pos);
					bool currInRange = _chkInRange(currRng, seed_pos);
					if ((false == bestInRange) && (true == currInRange))
					{
						matchRng = currRng;
						id = i;
						matchPts = matchPairs[i].matchPts;
					}
					else  if (((false == bestInRange) && (false == currInRange)) || ((true == bestInRange) && (true == currInRange)))
					{
						if (matchPts < matchPairs[i].matchPts)
						{
							matchRng = currRng;
							id = i;
							matchPts = matchPairs[i].matchPts;
						}
					}
				}
			}
		}
	}
	//if (id < 0)
	{
		//比较低可信度
		for (int i = 0; i < matchPairs.size(); i++)
		{
			if ((matchPairs[i].matchValue > nominalValue + matchTh.min) && (matchPairs[i].matchValue < nominalValue + matchTh.max) && (matchPairs[i].matchType == 0))
			{
				if (id < 0)
				{
					id = i;
					if (true == isTB)
					{
						matchRng.min = matchPairs[i].roi.left;
						matchRng.max = matchPairs[i].roi.right;
					}
					else
					{
						matchRng.min = matchPairs[i].roi.top;
						matchRng.max = matchPairs[i].roi.bottom;
					}
					matchPts = matchPairs[i].matchPts;
				}
				else
				{
					SVzNLRangeD currRng;
					if (true == isTB)
					{
						currRng.min = matchPairs[i].roi.left;
						currRng.max = matchPairs[i].roi.right;
					}
					else
					{
						currRng.min = matchPairs[i].roi.top;
						currRng.max = matchPairs[i].roi.bottom;
					}

					bool bestInRange = _chkInRange(matchRng, seed_pos);
					bool currInRange = _chkInRange(currRng, seed_pos);
					if ((false == bestInRange) && (true == currInRange))
					{
						matchRng = currRng;
						id = i;
						matchPts = matchPairs[i].matchPts;
					}
					else  if (((false == bestInRange) && (false == currInRange)) || ((true == bestInRange) && (true == currInRange)))
					{
						if (matchPts < matchPairs[i].matchPts)
						{
							matchRng = currRng;
							id = i;
							matchPts = matchPairs[i].matchPts;
						}
					}
				}
			}
		}
	}
	return id;
}

SSG_ROIRectD combineROI(SSG_ROIRectD roi_1, SSG_ROIRectD roi_2)
{
	SSG_ROIRectD result;
	result.left = roi_1.left < roi_2.left ? roi_1.left : roi_2.left;
	result.right = roi_1.right > roi_2.right ? roi_1.right : roi_2.right;
	result.top = roi_1.top < roi_2.top ? roi_1.top : roi_2.top;
	result.bottom = roi_1.bottom > roi_2.bottom ? roi_1.bottom : roi_2.bottom;
	return result;
}

void _combineValidPairs(
	std::vector<SSG_matchPair>& matchPairs,
	SSG_intPair seed_pair,
	SVzNLRangeD chkRange,
	bool isTBDir,
	const int vTreeStart,
	const int hTreeStart,
	double namedValue,
	const SVzNLRangeD matchTh,
	bool TBL_LRW,
	double bagW,
	std::vector<SSG_intPair>& LR_idPairs)
{
	for (int i = 0; i < matchPairs.size(); i++)
	{
		SSG_matchPair& a_matchPair = matchPairs[i];
		if (true == isTBDir)
		{
			if ((a_matchPair.id1 >= hTreeStart) || (a_matchPair.id2 >= hTreeStart))
				continue;
		}
		else
		{
			if ( ((a_matchPair.id1 >= vTreeStart) && (a_matchPair.id1 < hTreeStart)) ||
				 ((a_matchPair.id2 >= vTreeStart) && (a_matchPair.id2 < hTreeStart)))
				continue;
		}
		if ((a_matchPair.minMatchValue > namedValue + matchTh.min) && (a_matchPair.maxMatchValue < namedValue + matchTh.max) )  //不能使用matchValue，这个是平均值
		{
			if ( ((a_matchPair.id1 == seed_pair.data_0) && (a_matchPair.id2 != seed_pair.data_1))||
				((a_matchPair.id1 != seed_pair.data_0) && (a_matchPair.id2 == seed_pair.data_1)))
			{
				SSG_ROIRectD rstROI = combineROI(matchPairs[seed_pair.idx].roi, matchPairs[i].roi);
				double rst_width;
				if (TBL_LRW == true)
					rst_width = rstROI.right - rstROI.left;
				else
					rst_width = rstROI.bottom - rstROI.top;
				if (rst_width < bagW * 1.3)
				{
					if ((true == isTBDir) && (a_matchPair.roi.left >= chkRange.min-matchTh.min) && (a_matchPair.roi.right <= chkRange.max + matchTh.max))
					{
						SSG_intPair a_newPair = { a_matchPair.id1, a_matchPair.id2 };
						LR_idPairs.push_back(a_newPair);
					}
					else if ((false == isTBDir) && (a_matchPair.roi.top >= chkRange.min - matchTh.min) && (a_matchPair.roi.bottom <= chkRange.max + matchTh.max))
					{
						SSG_intPair a_newPair = { a_matchPair.id1, a_matchPair.id2 };
						LR_idPairs.push_back(a_newPair);
					}
				}
			}
#if 1  //2025.10.4:添加ROI比较
			else if ((a_matchPair.id1 != seed_pair.data_0) && (a_matchPair.id2 != seed_pair.data_1))
			{
				//compare ROI
				bool roiValid = false;
				if (true == isTBDir)
				{
					double T_diff = abs(a_matchPair.roi.top - matchPairs[seed_pair.idx].roi.top);
					double B_diff = abs(a_matchPair.roi.bottom - matchPairs[seed_pair.idx].roi.bottom);
					if ((a_matchPair.roi.left >= chkRange.min) && (a_matchPair.roi.right <= chkRange.max) &&
						(T_diff < matchTh.max) && (B_diff < matchTh.max))
					{
						roiValid = true;
					}
				}
				else
				{
					double L_diff = abs(a_matchPair.roi.left - matchPairs[seed_pair.idx].roi.left);
					double R_diff = abs(a_matchPair.roi.right - matchPairs[seed_pair.idx].roi.right);
					if ((a_matchPair.roi.left >= chkRange.min) && (a_matchPair.roi.right <= chkRange.max) &&
						(L_diff < matchTh.max) && (R_diff < matchTh.max))
					{
						roiValid = true;
					}
				}
				if (roiValid == true)
				{
					SSG_ROIRectD rstROI = combineROI(matchPairs[seed_pair.idx].roi, matchPairs[i].roi);
					double rst_width;
					if (TBL_LRW == true)
						rst_width = rstROI.right - rstROI.left;
					else
						rst_width = rstROI.bottom - rstROI.top;
					if (rst_width < bagW * 1.3)
					{
						if ((true == isTBDir) && (a_matchPair.roi.left >= chkRange.min - matchTh.min) && (a_matchPair.roi.right <= chkRange.max + matchTh.max))
						{
							SSG_intPair a_newPair = { a_matchPair.id1, a_matchPair.id2 };
							LR_idPairs.push_back(a_newPair);
						}
						else if ((false == isTBDir) && (a_matchPair.roi.top >= chkRange.min - matchTh.min) && (a_matchPair.roi.bottom <= chkRange.max + matchTh.max))
						{
							SSG_intPair a_newPair = { a_matchPair.id1, a_matchPair.id2 };
							LR_idPairs.push_back(a_newPair);
						}
					}

				}
			}
#endif		
		}
	}
	return ;
}

/// <summary>
/// 建立全匹配表
/// </summary>
void _getMatchTable(
	std::vector< SSG_lineConotours>& contours_1,
	std::vector< SSG_lineConotours>& contours_2,
	int maxEdgeId_1,
	int maxEdgeId_2,
	bool isVScan,
	const int vTreeStart,
	const int hTreeStart,
	std::vector<SSG_edgeMatchInfo>& matchTable,
	std::vector< SSG_matchPair>& match_pairs,
	std::vector<SSG_conotourPair>& contourPairs,
	int* match_size)
{
	*match_size = 0;
	//分段计算平均宽度和平均高度以及分段ROI
	//全匹配（将水平所有分段的所有可能距离
	int matchSize = (maxEdgeId_1 + 1) * (maxEdgeId_2 + 1);
	matchTable.resize(matchSize);
	for (int m = 0; m < matchSize; m++)
		matchTable[m].roi.right = -1;  //初始化ROI
	int size_1 = (int)contours_1.size();
	int size_2 = (int)contours_2.size();
	int m = 0;
	int n = 0;
	int matchNum = 0;
	while (1)
	{
		SSG_lineConotours* a_contour_1 = &contours_1[m];
		SSG_lineConotours* a_contour_2 = &contours_2[n];

		if (a_contour_1->lineIdx < a_contour_2->lineIdx)
			m++;
		else if (a_contour_1->lineIdx > a_contour_2->lineIdx)
			n++;
		else  //m==n，同一条扫描线
		{
			std::vector<SSG_contourPtInfo>& lineContours_1 = a_contour_1->contourPts;
			std::vector<SSG_contourPtInfo>& lineContours_2 = a_contour_2->contourPts;
			for (int j = 0; j < lineContours_1.size(); j++)
			{
				if (((true == isVScan) && (lineContours_1[j].edgeId >= hTreeStart)) ||  //tree类型需要和scan方向对应
					((false == isVScan) && (lineContours_1[j].edgeId >= vTreeStart) && (lineContours_1[j].edgeId < hTreeStart)))
					continue; 
				for (int k = 0; k < lineContours_2.size(); k++)
				{
					if (((true == isVScan) && (lineContours_2[k].edgeId >= hTreeStart)) ||  //tree类型需要和scan方向对应
						((false == isVScan) && (lineContours_2[k].edgeId >= vTreeStart) && (lineContours_2[k].edgeId < hTreeStart)))
						continue;
					//得到一个匹配
					SSG_edgeMatchInfo* a_match = &matchTable[lineContours_1[j].edgeId * (maxEdgeId_2 + 1) + lineContours_2[k].edgeId];
					if (a_match->matchNum == 0)
					{
						SSG_matchPair a_pair = { lineContours_1[j].edgeId , lineContours_2[k].edgeId, 0, 0,  0};
						match_pairs.push_back(a_pair);
					}

					double dist = lineContours_1[j].scanDist + lineContours_2[k].scanDist;

					SSG_conotourPair a_pair;
					a_pair.lineIdx = a_contour_1->lineIdx;
					a_pair.edgeId_0 = lineContours_1[j].edgeId;
					a_pair.edgeId_1 = lineContours_2[k].edgeId;
					a_pair.contourPt_0 = lineContours_1[j];
					a_pair.contourPt_1 = lineContours_2[k];
					a_pair.ptPairDist = dist;
					contourPairs.push_back(a_pair);

if ((lineContours_1[j].edgeId == 29))
	int kkk = 1;
					//可信度分析。将JUMP和Ending的匹配置为高可信度
					bool reliable_1 = false;
					bool reliable_2 = false;
					if ((LINE_FEATURE_L_JUMP_H2L == lineContours_1[j].type) || (LINE_FEATURE_L_JUMP_L2H == lineContours_1[j].type) ||
						(LINE_FEATURE_LINE_ENDING_0 == lineContours_1[j].type) || (LINE_FEATURE_LINE_ENDING_1 == lineContours_1[j].type) ||
						(LINE_FEATURE_RGN_EDGE == lineContours_1[j].type) || (LINE_FEATURE_CORNER_V == lineContours_1[j].type))
						reliable_1 = true;

					if ((LINE_FEATURE_L_JUMP_H2L == lineContours_2[k].type) || (LINE_FEATURE_L_JUMP_L2H == lineContours_2[k].type) ||
						(LINE_FEATURE_LINE_ENDING_0 == lineContours_2[k].type) || (LINE_FEATURE_LINE_ENDING_1 == lineContours_2[k].type) ||
						(LINE_FEATURE_RGN_EDGE == lineContours_2[k].type) || (LINE_FEATURE_CORNER_V == lineContours_2[k].type))
						reliable_2 = true;

					if ((true == reliable_1) && (true == reliable_2))
						a_match->level2_num++;
					else if((true == reliable_1) || (true == reliable_2))
						a_match->level1_num ++;


					a_match->meanDist += dist;
					a_match->varDist += dist * dist;

					if (a_match->matchNum == 0)
					{
						a_match->minDist = dist;
						a_match->maxDist = dist;
					}
					else
					{
						a_match->minDist = a_match->minDist > dist ? dist : a_match->minDist;
						a_match->maxDist = a_match->maxDist < dist ? dist : a_match->maxDist;
					}
					a_match->matchNum++;
					if (a_match->roi.right < a_match->roi.left)
					{
						a_match->roi = { lineContours_1[j].edgePt.x, lineContours_1[j].edgePt.x, lineContours_1[j].edgePt.y, lineContours_1[j].edgePt.y };
						a_match->roi.left = a_match->roi.left > lineContours_2[k].edgePt.x ? lineContours_2[k].edgePt.x : a_match->roi.left;
						a_match->roi.right = a_match->roi.right < lineContours_2[k].edgePt.x ? lineContours_2[k].edgePt.x : a_match->roi.right;
						a_match->roi.top = a_match->roi.top > lineContours_2[k].edgePt.y ? lineContours_2[k].edgePt.y : a_match->roi.top;
						a_match->roi.bottom = a_match->roi.bottom < lineContours_2[k].edgePt.y ? lineContours_2[k].edgePt.y : a_match->roi.bottom;
					}
					else
					{
						a_match->roi.left = a_match->roi.left > lineContours_1[j].edgePt.x ? lineContours_1[j].edgePt.x : a_match->roi.left;
						a_match->roi.right = a_match->roi.right < lineContours_1[j].edgePt.x ? lineContours_1[j].edgePt.x : a_match->roi.right;
						a_match->roi.top = a_match->roi.top > lineContours_1[j].edgePt.y ? lineContours_1[j].edgePt.y : a_match->roi.top;
						a_match->roi.bottom = a_match->roi.bottom < lineContours_1[j].edgePt.y ? lineContours_1[j].edgePt.y : a_match->roi.bottom;

						a_match->roi.left = a_match->roi.left > lineContours_2[k].edgePt.x ? lineContours_2[k].edgePt.x : a_match->roi.left;
						a_match->roi.right = a_match->roi.right < lineContours_2[k].edgePt.x ? lineContours_2[k].edgePt.x : a_match->roi.right;
						a_match->roi.top = a_match->roi.top > lineContours_2[k].edgePt.y ? lineContours_2[k].edgePt.y : a_match->roi.top;
						a_match->roi.bottom = a_match->roi.bottom < lineContours_2[k].edgePt.y ? lineContours_2[k].edgePt.y : a_match->roi.bottom;
					}
					matchNum++;
					if (matchNum > 10000)
						int kkk = 1;
				}
			}
			m++;
			n++;
		}
		if ((m >= size_1) || (n >= size_2))
			break;
	}
	*match_size = matchNum;
	return;
}

SSG_peakRgnInfo _getPeakRgn(
	SVzNL3DLaserLine* laser3DPoints,
	int lineNum, 
	int treeSize, 
	//std::vector< SSG_lineConotours>& topContour,
	//std::vector< SSG_lineConotours>& bottomContour,
	//std::vector< SSG_lineConotours>& leftContour,
	//std::vector< SSG_lineConotours>& rightContour, 
	std::vector<SSG_conotourPair>& TB_contourPairs,
	std::vector<SSG_conotourPair>& LR_contourPairs,
	std::vector<SSG_intPair>& TB_idPairs,
	std::vector<SSG_intPair>& LR_idPairs,
	std::vector<SSG_treeInfo>& allTreesInfo,
	const SSG_ROIRectD globalROI, 
	SVzNLRangeD TB_range,
	SVzNLRangeD LR_range,
	const SG_bagPositionParam algoParam, 
	const int peakRgnId,
	int* lowLevelChkFlag)
{
	SSG_peakRgnInfo a_pkRgn;
	memset(&a_pkRgn, 0, sizeof(SSG_peakRgnInfo));
	if ((TB_idPairs.size() > 0) && (LR_idPairs.size() > 0) )
	{
		std::vector<int> edgeFlag;
		edgeFlag.resize(treeSize);
		for (int i = 0; i < TB_idPairs.size(); i++)
		{
			edgeFlag[TB_idPairs[i].data_0] = 1;
			edgeFlag[TB_idPairs[i].data_1] = 1;
		}
		for (int i = 0; i < LR_idPairs.size(); i++)
		{
			edgeFlag[LR_idPairs[i].data_0] = 1;
			edgeFlag[LR_idPairs[i].data_1] = 1;
		}

		//生成contourPT
		//同时检查是否在下一层
		int lowLevelFlag_T = 0;
		int lowLevelFlag_B = 0;
		
		std::vector< SSG_conotourPair> result_contourTB;
		sg_getPairingContourPts(
			TB_contourPairs,
			TB_idPairs,
			result_contourTB,
			LR_range,
			true,
			&lowLevelFlag_T,
			&lowLevelFlag_B);

		int lowLevelFlag_L = 0;
		int lowLevelFlag_R = 0;
		std::vector< SSG_conotourPair> result_contourLR;
		sg_getPairingContourPts(
			LR_contourPairs,
			LR_idPairs,
			result_contourLR,
			TB_range,
			false,
			&lowLevelFlag_L,
			&lowLevelFlag_R);

		lowLevelFlag_B <<= 1;
		lowLevelFlag_L <<= 2;
		lowLevelFlag_R <<= 3;
		int lowLevelFlag = lowLevelFlag_T + lowLevelFlag_B + lowLevelFlag_L + lowLevelFlag_R;
		if (lowLevelFlag > 0)
			*lowLevelChkFlag = lowLevelFlag;

		int blockedFlag = 0;
		bool labelBlockedObj = true;
		a_pkRgn = _getRgnObjInfo(
			laser3DPoints,
			lineNum,
			edgeFlag,
			result_contourTB,
			result_contourLR,
			allTreesInfo,
			0,
			peakRgnId,
			globalROI,
			algoParam,
			labelBlockedObj,
			&blockedFlag);
		if ((a_pkRgn.pkRgnIdx > 0) && (0 == blockedFlag))
			return a_pkRgn;
		else
			a_pkRgn.pkRgnIdx = 0;  //invalidate
	}
	return a_pkRgn;
}

void _refineContourPairs(SSG_matchPair* valid_match, std::vector<SSG_conotourPair>& all_contourPairs)
{
	double sigma_th = 3 * valid_match->varValue;
	if (sigma_th < valid_match->matchValue * 0.1)  //10%
		sigma_th = valid_match->matchValue * 0.1;
	double dist_th_L = valid_match->matchValue - sigma_th;
	double dist_th_H = valid_match->matchValue + sigma_th;
	valid_match->roi.right = -1;
	valid_match->roi.left = 0;
	for (int i = 0, i_max = (int)all_contourPairs.size(); i < i_max; i++)
	{
		if ((all_contourPairs[i].edgeId_0 == valid_match->id1) && (all_contourPairs[i].edgeId_1 == valid_match->id2))
		{
			if ((all_contourPairs[i].ptPairDist < dist_th_L) || (all_contourPairs[i].ptPairDist > dist_th_H))
			{
				all_contourPairs[i].edgeId_0 = 0;
				all_contourPairs[i].edgeId_1 = 0;
			}
			else
			{
				if (valid_match->roi.right < valid_match->roi.left)
				{
					valid_match->roi = { all_contourPairs[i].contourPt_0.edgePt.x, all_contourPairs[i].contourPt_0.edgePt.x, all_contourPairs[i].contourPt_0.edgePt.y, all_contourPairs[i].contourPt_0.edgePt.y };
					valid_match->roi.left = valid_match->roi.left > all_contourPairs[i].contourPt_1.edgePt.x ? all_contourPairs[i].contourPt_1.edgePt.x : valid_match->roi.left;
					valid_match->roi.right = valid_match->roi.right < all_contourPairs[i].contourPt_1.edgePt.x ? all_contourPairs[i].contourPt_1.edgePt.x : valid_match->roi.right;
					valid_match->roi.top = valid_match->roi.top > all_contourPairs[i].contourPt_1.edgePt.y ? all_contourPairs[i].contourPt_1.edgePt.y : valid_match->roi.top;
					valid_match->roi.bottom = valid_match->roi.bottom < all_contourPairs[i].contourPt_1.edgePt.y ? all_contourPairs[i].contourPt_1.edgePt.y : valid_match->roi.bottom;
				}
				else
				{
					valid_match->roi.left = valid_match->roi.left > all_contourPairs[i].contourPt_0.edgePt.x ? all_contourPairs[i].contourPt_0.edgePt.x : valid_match->roi.left;
					valid_match->roi.right = valid_match->roi.right < all_contourPairs[i].contourPt_0.edgePt.x ? all_contourPairs[i].contourPt_0.edgePt.x : valid_match->roi.right;
					valid_match->roi.top = valid_match->roi.top > all_contourPairs[i].contourPt_0.edgePt.y ? all_contourPairs[i].contourPt_0.edgePt.y : valid_match->roi.top;
					valid_match->roi.bottom = valid_match->roi.bottom < all_contourPairs[i].contourPt_0.edgePt.y ? all_contourPairs[i].contourPt_0.edgePt.y : valid_match->roi.bottom;

					valid_match->roi.left = valid_match->roi.left > all_contourPairs[i].contourPt_1.edgePt.x ? all_contourPairs[i].contourPt_1.edgePt.x : valid_match->roi.left;
					valid_match->roi.right = valid_match->roi.right < all_contourPairs[i].contourPt_1.edgePt.x ? all_contourPairs[i].contourPt_1.edgePt.x : valid_match->roi.right;
					valid_match->roi.top = valid_match->roi.top > all_contourPairs[i].contourPt_1.edgePt.y ? all_contourPairs[i].contourPt_1.edgePt.y : valid_match->roi.top;
					valid_match->roi.bottom = valid_match->roi.bottom < all_contourPairs[i].contourPt_1.edgePt.y ? all_contourPairs[i].contourPt_1.edgePt.y : valid_match->roi.bottom;
				}
			}
		}
	}
}

//最大相似匹配
//按可信度进行匹配
	SSG_peakRgnInfo _maxLikelihoodMatch(
	SVzNL3DLaserLine* laser3DPoints,
	int lineNum,
	int treeSize,
	SSG_2DValueI a_peak,
	//std::vector< SSG_lineConotours>& topContour,
	//std::vector< SSG_lineConotours>& bottomContour,
	//std::vector< SSG_lineConotours>& leftContour,
	//std::vector< SSG_lineConotours>& rightContour,
	std::vector<SSG_edgeMatchInfo>& matchTable_TB,
	std::vector< SSG_matchPair>& TB_pairs,
	std::vector<SSG_conotourPair>& TB_contourPairs,
	int TB_matchNum,
	int maxEdgeId_btm,
	std::vector<SSG_edgeMatchInfo>& matchTable_LR,
	std::vector< SSG_matchPair>& LR_pairs,
	std::vector<SSG_conotourPair>& LR_contourPairs,
	int LR_matchNum,
	int maxEdgeId_right,
	std::vector<SSG_treeInfo>& allTreesInfo,
	const int vTreeStart,
	const int hTreeStart,
	const SSG_ROIRectD globalROI,
	const SG_bagPositionParam algoParam,
	const int peakRgnId,
	int* lowLevelChkFlag)
{
	*lowLevelChkFlag = 0;
	double pk_x = laser3DPoints[a_peak.x].p3DPosition[a_peak.y].pt3D.x;
	double pk_y = laser3DPoints[a_peak.x].p3DPosition[a_peak.y].pt3D.y;
	//计算每个匹配的平均距离:TB
	//double vld_matchNum_k = 0.1;
	for (int i = 0; i < TB_pairs.size(); i++)
	{
		SSG_matchPair* a_pair = &TB_pairs[i];
		SSG_edgeMatchInfo* a_match = &matchTable_TB[a_pair->id1 * (maxEdgeId_btm + 1) + a_pair->id2];
		if (a_match->matchNum >= 10) //TB_matchNum * vld_matchNum_k)
		{
			a_match->meanDist = a_match->meanDist / a_match->matchNum;
			a_match->varDist = a_match->varDist / a_match->matchNum - a_match->meanDist * a_match->meanDist;
			a_match->varDist = sqrt(a_match->varDist);
			if (a_match->level2_num > a_match->matchNum / 2)
				a_pair->matchType = 2; //高可信度
			else if (a_match->level1_num > a_match->matchNum / 2)
				a_pair->matchType = 1; //中可信度
			a_pair->matchPts = a_match->matchNum;
			a_pair->matchValue = a_match->meanDist;
			a_pair->varValue = a_match->varDist;
			a_pair->minMatchValue = a_match->minDist;
			a_pair->maxMatchValue = a_match->maxDist;
			a_pair->roi = a_match->roi;
		}
		else
			a_pair->matchValue = -1.0;
	}
	///去除无效匹配
	int TB_size = (int)TB_pairs.size();
	for (int i = TB_size - 1; i >= 0; i--)
	{
		if ( (TB_pairs[i].matchValue < 0) || (TB_pairs[i].id1 == TB_pairs[i].id2))
			TB_pairs.erase(TB_pairs.begin() + i);
	}
	TB_size = (int)TB_pairs.size();
	//计算每个匹配的平均距离:LR
	for (int i = 0; i < LR_pairs.size(); i++)
	{
		SSG_matchPair* a_pair = &LR_pairs[i];
		SSG_edgeMatchInfo* a_match = &matchTable_LR[a_pair->id1 * (maxEdgeId_right + 1) + a_pair->id2];
		if (a_match->matchNum >= 10)//LR_matchNum * vld_matchNum_k)
		{
			a_match->meanDist = a_match->meanDist / a_match->matchNum;
			a_match->varDist = a_match->varDist / a_match->matchNum - a_match->meanDist * a_match->meanDist;
			a_match->varDist = sqrt(a_match->varDist);
			if (a_match->level2_num > a_match->matchNum / 2)
				a_pair->matchType = 2; //高可信度
			else if (a_match->level1_num > a_match->matchNum / 2)
				a_pair->matchType = 1; //高可信度
			a_pair->matchPts = a_match->matchNum;
			a_pair->matchValue = a_match->meanDist;
			a_pair->varValue = a_match->varDist;
			a_pair->minMatchValue = a_match->minDist;
			a_pair->maxMatchValue = a_match->maxDist;
			a_pair->roi = a_match->roi;
		}
		else
			a_pair->matchValue = -1.0;
	}
	///去除无效匹配
	int LR_size = (int)LR_pairs.size();
	for (int i = LR_size - 1; i >= 0; i--)
	{
		if (( LR_pairs[i].matchValue < 0) || (LR_pairs[i].id1 == LR_pairs[i].id2))
			LR_pairs.erase(LR_pairs.begin() + i);
	}
	LR_size = (int)LR_pairs.size();
	//四种情况：（1）TB一个， LR一个，（2）TB一个，LR多个，（3）TB多个，LR一个（4）TB多个，LR多个
	int edgeId_top = -1, edgeId_btm = -1, edgeId_left = -1, edgeId_right = -1;
	SVzNLRangeD L_th = {-80, algoParam.bagParam.bagL * 0.25};
	SVzNLRangeD W_th = { -algoParam.bagParam.bagW * 0.25, 100 }; 
	SVzNLRangeD combine_th = {-50, 50};
	if ((TB_size == 1) && (LR_size == 1)) //一对一匹配
	{
		SSG_peakRgnInfo a_pkRgn;
		memset(&a_pkRgn, 0, sizeof(SSG_peakRgnInfo));

		SSG_matchPair* match_TB = &TB_pairs[0];
		SSG_matchPair* match_LR = &LR_pairs[0];

		bool TB_bagL = _checkValueMatch(match_TB->matchValue, algoParam.bagParam.bagL, L_th);
		bool TB_bagW = _checkValueMatch(match_TB->matchValue, algoParam.bagParam.bagW, W_th);
		bool LR_bagL = _checkValueMatch(match_LR->matchValue, algoParam.bagParam.bagL, L_th);
		bool LR_bagW = _checkValueMatch(match_LR->matchValue, algoParam.bagParam.bagW, W_th);
		if (((true == TB_bagL) && (true == LR_bagW)) || ((true == TB_bagW) && (true == LR_bagL)))
		{
			//此处添加点的过滤，然后重新更新ROI
			_refineContourPairs(match_TB, TB_contourPairs);
			_refineContourPairs(match_LR, LR_contourPairs);

			SVzNLRangeD TB_range = { match_TB->roi.top, match_TB->roi.bottom };
			SVzNLRangeD LR_range = { match_LR->roi.left, match_LR->roi.right };

			std::vector<SSG_intPair> TB_idPairs;
			std::vector<SSG_intPair> LR_idPairs;
			TB_idPairs.push_back({ match_TB->id1 , match_TB->id2 });
			LR_idPairs.push_back({ match_LR->id1 , match_LR->id2 });

			a_pkRgn = _getPeakRgn(
				laser3DPoints,
				lineNum,
				treeSize,
				TB_contourPairs,
				LR_contourPairs,
				TB_idPairs,
				LR_idPairs,
				allTreesInfo,
				globalROI,
				TB_range,
				LR_range,
				algoParam,
				peakRgnId,
				lowLevelChkFlag);
		}
		return a_pkRgn;
	}
	else if ((TB_size == 1) && (LR_size > 1))//一对多匹配
	{
		SSG_peakRgnInfo a_pkRgn;
		memset(&a_pkRgn, 0, sizeof(SSG_peakRgnInfo));

		SSG_matchPair* match_TB = &TB_pairs[0];
		bool TB_bagL = _checkValueMatch(match_TB->matchValue, algoParam.bagParam.bagL, L_th);
		bool TB_bagW = _checkValueMatch(match_TB->matchValue, algoParam.bagParam.bagW, W_th);

		int LR_id = -1;
		
		if (true == TB_bagL)
			LR_id = _getBestMatch(LR_pairs, algoParam.bagParam.bagW, W_th, false, pk_y);
		else if (true == TB_bagW)
			LR_id = _getBestMatch(LR_pairs, algoParam.bagParam.bagL, L_th, false, pk_y);
		if (LR_id >= 0)
		{
			//此处添加点的过滤，然后重新更新ROI
			_refineContourPairs(match_TB, TB_contourPairs);
			_refineContourPairs(&LR_pairs[LR_id], LR_contourPairs);

			edgeId_top = match_TB->id1;
			edgeId_btm = match_TB->id2;
			edgeId_left = LR_pairs[LR_id].id1;
			edgeId_right = LR_pairs[LR_id].id2;
			//合并匹配对
			SVzNLRangeD TB_range = { match_TB->roi.top, match_TB->roi.bottom };
			SVzNLRangeD LR_range = { LR_pairs[LR_id].roi.left, LR_pairs[LR_id].roi.right };
			std::vector<SSG_intPair> TB_idPairs;
			std::vector<SSG_intPair> LR_idPairs;
			SSG_intPair seed_TBPair = { edgeId_top , edgeId_btm, 0 };
			TB_idPairs.push_back(seed_TBPair);
			SSG_intPair seed_LRPair = { edgeId_left , edgeId_right, LR_id };
			LR_idPairs.push_back(seed_LRPair);
			_combineValidPairs(
				LR_pairs, 
				seed_LRPair, 
				TB_range, 
				false, 
				vTreeStart,
				hTreeStart,
				LR_pairs[LR_id].matchValue, 
				combine_th, 
				TB_bagL,
				algoParam.bagParam.bagW,
				LR_idPairs);
			
			a_pkRgn = _getPeakRgn(
				laser3DPoints,
				lineNum,
				treeSize,
				TB_contourPairs,
				LR_contourPairs,
				TB_idPairs,
				LR_idPairs,
				allTreesInfo,
				globalROI,
				TB_range,
				LR_range,
				algoParam,
				peakRgnId,
				lowLevelChkFlag);
		}
		return a_pkRgn;
	}
	else if ((TB_size > 1) && (LR_size == 1))//多对一匹配
	{
		SSG_peakRgnInfo a_pkRgn;
		memset(&a_pkRgn, 0, sizeof(SSG_peakRgnInfo));

		SSG_matchPair* match_LR = &LR_pairs[0];
		bool LR_bagL = _checkValueMatch(match_LR->matchValue, algoParam.bagParam.bagL, L_th);
		bool LR_bagW = _checkValueMatch(match_LR->matchValue, algoParam.bagParam.bagW, W_th);

		int TB_id = -1;
		if (true == LR_bagL)
			TB_id = _getBestMatch(TB_pairs, algoParam.bagParam.bagW, W_th, true, pk_x);
		else if (true == LR_bagW)
			TB_id = _getBestMatch(TB_pairs, algoParam.bagParam.bagL, L_th, true, pk_x);
		if (TB_id >= 0)
		{
			//此处添加点的过滤，然后重新更新ROI
			_refineContourPairs(&TB_pairs[TB_id], TB_contourPairs);
			_refineContourPairs(match_LR, LR_contourPairs);

			edgeId_top = TB_pairs[TB_id].id1;
			edgeId_btm = TB_pairs[TB_id].id2;
			edgeId_left = match_LR->id1;
			edgeId_right = match_LR->id2;

			//合并匹配对
			SVzNLRangeD TB_range = { TB_pairs[TB_id].roi.top, TB_pairs[TB_id].roi.bottom };
			SVzNLRangeD LR_range = { match_LR->roi.left, match_LR->roi.right };
			std::vector<SSG_intPair> TB_idPairs;
			std::vector<SSG_intPair> LR_idPairs;
			SSG_intPair seed_TBPair = { edgeId_top , edgeId_btm, TB_id };
			TB_idPairs.push_back(seed_TBPair);
			SSG_intPair seed_LRPair = { edgeId_left , edgeId_right, 0 };
			LR_idPairs.push_back(seed_LRPair);
			_combineValidPairs(
				TB_pairs, 
				seed_TBPair, 
				LR_range, 
				true, 
				vTreeStart,
				hTreeStart,
				TB_pairs[TB_id].matchValue, 
				combine_th, 
				LR_bagW,
				algoParam.bagParam.bagW,
				TB_idPairs);

			a_pkRgn = _getPeakRgn(
				laser3DPoints,
				lineNum,
				treeSize,
				TB_contourPairs,
				LR_contourPairs,
				TB_idPairs,
				LR_idPairs,
				allTreesInfo,
				globalROI,
				TB_range,
				LR_range,
				algoParam,
				peakRgnId,
				lowLevelChkFlag);
		}
		return a_pkRgn;
	}
	else if ((TB_size > 1) && (LR_size > 1))//多对多匹配
	{
		SSG_peakRgnInfo a_pkRgn;
		memset(&a_pkRgn, 0, sizeof(SSG_peakRgnInfo));

		int TB_bagL = _getBestMatch(TB_pairs, algoParam.bagParam.bagL, L_th, true, pk_x);
		int TB_bagW = _getBestMatch(TB_pairs, algoParam.bagParam.bagW, W_th, true, pk_x);
		if ((TB_bagL >= 0) && (TB_bagW >= 0))
		{
			SVzNLRangeD L_th_1 = { L_th.min, L_th.max / 2 };
			SVzNLRangeD W_th_1 = { W_th.min, W_th.max / 2 };
			int TB_bagL_1 = _getBestMatch(TB_pairs, algoParam.bagParam.bagL, L_th_1, true, pk_x);
			int TB_bagW_1 = _getBestMatch(TB_pairs, algoParam.bagParam.bagW, W_th_1, true, pk_x);
			if (((TB_bagL_1 >= 0) && (TB_bagW_1 < 0)) || ((TB_bagL_1 < 0) && (TB_bagW_1 >= 0)))
			{
				TB_bagL = TB_bagL_1;
				TB_bagW = TB_bagW_1;
			}
		}
		int LR_bagL = _getBestMatch(LR_pairs, algoParam.bagParam.bagL, L_th, false, pk_y);
		int LR_bagW = _getBestMatch(LR_pairs, algoParam.bagParam.bagW, W_th, false, pk_y);
		if ((LR_bagL >= 0) && (LR_bagW >= 0))
		{
			SVzNLRangeD L_th_1 = { L_th.min, L_th.max / 2 };
			SVzNLRangeD W_th_1 = { W_th.min, W_th.max / 2 };
			int LR_bagL_1 = _getBestMatch(LR_pairs, algoParam.bagParam.bagL, L_th_1, false, pk_y);
			int LR_bagW_1 = _getBestMatch(LR_pairs, algoParam.bagParam.bagW, W_th_1, false, pk_y);
			if (((LR_bagL_1 >= 0) && (LR_bagW_1 < 0)) || ((LR_bagL_1 < 0) && (LR_bagW_1 >= 0)))
			{
				LR_bagL = LR_bagL_1;
				LR_bagW = LR_bagW_1;
			}
		}

		//迭代处理
		if ((TB_bagL >= 0) && (TB_bagW >= 0))
		{
			if ((LR_bagL >= 0) && (LR_bagW < 0))
				TB_bagL = -1;
			else if((LR_bagL < 0) && (LR_bagW >= 0))
				TB_bagW = -1;
			else
			{
				//ROI比较
				bool pkInRoi_TBbagL = false;
				bool pkInRoi_TBbagW = false;
				if ((pk_x > TB_pairs[TB_bagL].roi.left) && (pk_x < TB_pairs[TB_bagL].roi.right))
					pkInRoi_TBbagL = true;
				if ((pk_x > TB_pairs[TB_bagW].roi.left) && (pk_x < TB_pairs[TB_bagW].roi.right))
					pkInRoi_TBbagW = true;
				if ((pkInRoi_TBbagL == true) && (pkInRoi_TBbagW == false))
					TB_bagW = -1;
				else if ((pkInRoi_TBbagL == false) && (pkInRoi_TBbagW == true))
					TB_bagL = -1;
#if 0
				//优先级比较
				if ((TB_bagL >= 0) && (TB_bagW >= 0))
				{
					if (TB_pairs[TB_bagL].matchType > TB_pairs[TB_bagW].matchType)
						TB_bagW = -1;
					else if (TB_pairs[TB_bagL].matchType < TB_pairs[TB_bagW].matchType)
						TB_bagL = -1;
				}
#endif
			}
		}
		if ((LR_bagL >= 0) && (LR_bagW >= 0))
		{
			if ((TB_bagL >= 0) && (TB_bagW < 0))
				LR_bagL = -1;
			else if ((TB_bagL < 0) && (TB_bagW >= 0))
				LR_bagW = -1;
			else
			{
				//ROI比较
				bool pkInRoi_LRbagL = false;
				bool pkInRoi_LRbagW = false;
				if ((pk_y > LR_pairs[LR_bagL].roi.top) && (pk_y < LR_pairs[LR_bagL].roi.bottom))
					pkInRoi_LRbagL = true;
				if ((pk_y > LR_pairs[LR_bagW].roi.top) && (pk_y < LR_pairs[LR_bagW].roi.bottom))
					pkInRoi_LRbagW = true;
				if ((pkInRoi_LRbagL == true) && (pkInRoi_LRbagW == false))
					LR_bagW = -1;
				else if ((pkInRoi_LRbagL == false) && (pkInRoi_LRbagW == true))
					LR_bagL = -1;
#if 0
				//优先级比较
				if ((LR_bagL >= 0) && (LR_bagW >= 0))
				{
					if (LR_pairs[LR_bagL].matchType > LR_pairs[LR_bagW].matchType)
						LR_bagW = -1;
					else if (LR_pairs[LR_bagL].matchType < LR_pairs[LR_bagW].matchType)
						LR_bagL = -1;
				}
#endif
			}

		}

		if ((TB_bagL >= 0) && (TB_bagW >= 0) && (LR_bagL >= 0) && (LR_bagW >= 0))
		{
			//比较TB_bagL - LR_bagW; TB_bagW - LR_bagL两对的ROI
			SSG_ROIRectD roi_TBL_LRW = combineROI(TB_pairs[TB_bagL].roi, LR_pairs[LR_bagW].roi);
			double diff_x_value = roi_TBL_LRW.right - roi_TBL_LRW.left;
			bool roi_valid_1 = true;
			if (diff_x_value > algoParam.bagParam.bagW * 1.3)
				roi_valid_1 = false;

			SSG_ROIRectD roi_TBW_LRL = combineROI(TB_pairs[TB_bagW].roi, LR_pairs[LR_bagL].roi);
			double diff_y_value = roi_TBW_LRL.bottom - roi_TBW_LRL.top;
			bool roi_valid_2 = true;
			if (diff_y_value > algoParam.bagParam.bagW * 1.3)
				roi_valid_2 = false;

			if ((true == roi_valid_1) && (false == roi_valid_2))
			{
				TB_bagW = -1;
				LR_bagL = -1;
			}
			else if ((false == roi_valid_1) && (true == roi_valid_2))
			{
				TB_bagL = -1;
				LR_bagW = -1;
			}
		}

		if ((TB_bagL >= 0) && (LR_bagW >= 0) && ((TB_bagW < 0) || (LR_bagL < 0)))
		{
			//此处添加点的过滤，然后重新更新ROI
			_refineContourPairs(&TB_pairs[TB_bagL], TB_contourPairs);
			_refineContourPairs(&LR_pairs[LR_bagW], LR_contourPairs);

			edgeId_top = TB_pairs[TB_bagL].id1;
			edgeId_btm = TB_pairs[TB_bagL].id2;
			edgeId_left = LR_pairs[LR_bagW].id1;
			edgeId_right = LR_pairs[LR_bagW].id2;

			//合并匹配对
			SVzNLRangeD TB_range = { TB_pairs[TB_bagL].roi.top, TB_pairs[TB_bagL].roi.bottom };
			SVzNLRangeD LR_range = { LR_pairs[LR_bagW].roi.left, LR_pairs[LR_bagW].roi.right };
			std::vector<SSG_intPair> TB_idPairs;
			std::vector<SSG_intPair> LR_idPairs;
			SSG_intPair seed_TBPair = { edgeId_top , edgeId_btm, TB_bagL };
			TB_idPairs.push_back(seed_TBPair);
			SSG_intPair seed_LRPair = { edgeId_left , edgeId_right, LR_bagW };
			LR_idPairs.push_back(seed_LRPair);
#if 1  //此处合并需要小心。只有小于一定宽度才需要合并（防止最小外接矩形有误差），否则会引起副作用，将不是目标的点合并进来

			bool TBL_LRW = true;
			_combineValidPairs(
				TB_pairs, 
				seed_TBPair, 
				LR_range, 
				true, 
				vTreeStart,
				hTreeStart,
				TB_pairs[TB_bagL].matchValue, 
				combine_th, 
				TBL_LRW,
				algoParam.bagParam.bagW,
				TB_idPairs	);
			_combineValidPairs(LR_pairs, 
				seed_LRPair, 
				TB_range, 
				false, 
				vTreeStart,
				hTreeStart,
				LR_pairs[LR_bagW].matchValue, 
				combine_th, 
				TBL_LRW,
				algoParam.bagParam.bagW,
				LR_idPairs);
#endif

			a_pkRgn = _getPeakRgn(
				laser3DPoints,
				lineNum,
				treeSize,
				TB_contourPairs,
				LR_contourPairs,
				TB_idPairs,
				LR_idPairs,
				allTreesInfo,
				globalROI,
				TB_range,
				LR_range,
				algoParam,
				peakRgnId,
				lowLevelChkFlag);
		}
		else if ((TB_bagW >= 0) && (LR_bagL >= 0) && ((TB_bagL < 0) || (LR_bagW < 0)))
		{
			//此处添加点的过滤，然后重新更新ROI
			_refineContourPairs(&TB_pairs[TB_bagW], TB_contourPairs);
			_refineContourPairs(&LR_pairs[LR_bagL], LR_contourPairs);

			edgeId_top = TB_pairs[TB_bagW].id1;
			edgeId_btm = TB_pairs[TB_bagW].id2;
			edgeId_left = LR_pairs[LR_bagL].id1;
			edgeId_right = LR_pairs[LR_bagL].id2;

			//合并匹配对
			SVzNLRangeD TB_range = { TB_pairs[TB_bagW].roi.top, TB_pairs[TB_bagW].roi.bottom };
			SVzNLRangeD LR_range = { LR_pairs[LR_bagL].roi.left, LR_pairs[LR_bagL].roi.right };
			std::vector<SSG_intPair> TB_idPairs;
			std::vector<SSG_intPair> LR_idPairs;
			SSG_intPair seed_TBPair = { edgeId_top , edgeId_btm, TB_bagW };
			TB_idPairs.push_back(seed_TBPair);
			SSG_intPair seed_LRPair = { edgeId_left , edgeId_right, LR_bagL };
			LR_idPairs.push_back(seed_LRPair);
			bool TBL_LRW = false;
			_combineValidPairs(TB_pairs, 
				seed_TBPair, 
				LR_range, 
				true, 
				vTreeStart,
				hTreeStart,
				TB_pairs[TB_bagW].matchValue, 
				combine_th, 
				TBL_LRW,
				algoParam.bagParam.bagW,
				TB_idPairs);
			_combineValidPairs(
				LR_pairs, 
				seed_LRPair, 
				TB_range, 
				false, 
				vTreeStart,
				hTreeStart,
				LR_pairs[LR_bagL].matchValue, 
				combine_th, 
				TBL_LRW,
				algoParam.bagParam.bagW,
				LR_idPairs);

			a_pkRgn = _getPeakRgn(
				laser3DPoints,
				lineNum,
				treeSize,
				TB_contourPairs,
				LR_contourPairs,
				TB_idPairs,
				LR_idPairs,
				allTreesInfo,
				globalROI,
				TB_range,
				LR_range,
				algoParam,
				peakRgnId,
				lowLevelChkFlag);
		}
		else if ((TB_bagL >= 0) && (LR_bagW >= 0) && (TB_bagW >= 0) && (LR_bagL >= 0))
		{
			//此处添加点的过滤，然后重新更新ROI
			_refineContourPairs(&TB_pairs[TB_bagL], TB_contourPairs);
			_refineContourPairs(&LR_pairs[LR_bagW], LR_contourPairs);

			//首先匹配TB_bagL和LR_bagW
			edgeId_top = TB_pairs[TB_bagL].id1;
			edgeId_btm = TB_pairs[TB_bagL].id2;
			edgeId_left = LR_pairs[LR_bagW].id1;
			edgeId_right = LR_pairs[LR_bagW].id2;
			//合并匹配对
			SVzNLRangeD TB_range = { TB_pairs[TB_bagL].roi.top, TB_pairs[TB_bagL].roi.bottom };
			SVzNLRangeD LR_range = { LR_pairs[LR_bagW].roi.left, LR_pairs[LR_bagW].roi.right };
			std::vector<SSG_intPair> TB_idPairs;
			std::vector<SSG_intPair> LR_idPairs;
			SSG_intPair seed_TBPair = { edgeId_top , edgeId_btm, TB_bagL };
			TB_idPairs.push_back(seed_TBPair);
			SSG_intPair seed_LRPair = { edgeId_left , edgeId_right, LR_bagW };
			LR_idPairs.push_back(seed_LRPair);
			bool TBL_LRW = true;
			_combineValidPairs(
				TB_pairs, 
				seed_TBPair, 
				LR_range, 
				true, 
				vTreeStart,
				hTreeStart,
				TB_pairs[TB_bagL].matchValue, 
				combine_th, 
				TBL_LRW,
				algoParam.bagParam.bagW,
				TB_idPairs);
			_combineValidPairs(
				LR_pairs, 
				seed_LRPair, 
				TB_range, 
				false, 
				vTreeStart,
				hTreeStart,
				LR_pairs[LR_bagW].matchValue, 
				combine_th, 
				TBL_LRW,
				algoParam.bagParam.bagW,
				LR_idPairs);

			a_pkRgn = _getPeakRgn(
				laser3DPoints,
				lineNum,
				treeSize,
				TB_contourPairs,
				LR_contourPairs,
				TB_idPairs,
				LR_idPairs,
				allTreesInfo,
				globalROI,
				TB_range,
				LR_range,
				algoParam,
				peakRgnId,
				lowLevelChkFlag);

			//此处添加点的过滤，然后重新更新ROI
			_refineContourPairs(&TB_pairs[TB_bagW], TB_contourPairs);
			_refineContourPairs(&LR_pairs[LR_bagL], LR_contourPairs);
			//再匹配TB_bagW和LR_bagL
			edgeId_top = TB_pairs[TB_bagW].id1;
			edgeId_btm = TB_pairs[TB_bagW].id2;
			edgeId_left = LR_pairs[LR_bagL].id1;
			edgeId_right = LR_pairs[LR_bagL].id2;
			//合并匹配对
			TB_range = { TB_pairs[TB_bagW].roi.top, TB_pairs[TB_bagW].roi.bottom };
			LR_range = { LR_pairs[LR_bagL].roi.left, LR_pairs[LR_bagL].roi.right };
			std::vector<SSG_intPair> TB_idPairs_1;
			std::vector<SSG_intPair> LR_idPairs_1;
			seed_TBPair = { edgeId_top , edgeId_btm, TB_bagW };
			TB_idPairs_1.push_back(seed_TBPair);
			seed_LRPair = { edgeId_left , edgeId_right, LR_bagL };
			LR_idPairs_1.push_back(seed_LRPair);
			TBL_LRW = false;
			_combineValidPairs(
				TB_pairs, 
				seed_TBPair, 
				LR_range, 
				true, 
				vTreeStart,
				hTreeStart,
				TB_pairs[TB_bagW].matchValue, 
				combine_th, 
				TBL_LRW,
				algoParam.bagParam.bagW,
				TB_idPairs_1);
			_combineValidPairs(
				LR_pairs, 
				seed_LRPair, 
				TB_range, 
				false, 
				vTreeStart,
				hTreeStart,
				LR_pairs[LR_bagL].matchValue, 
				combine_th, 
				TBL_LRW,
				algoParam.bagParam.bagW,
				LR_idPairs_1);

			SSG_peakRgnInfo a_pkRgn_1;
			memset(&a_pkRgn_1, 0, sizeof(SSG_peakRgnInfo));
			a_pkRgn_1 = _getPeakRgn(
				laser3DPoints,
				lineNum,
				treeSize,
				TB_contourPairs,
				LR_contourPairs,
				TB_idPairs_1,
				LR_idPairs_1,
				allTreesInfo,
				globalROI,
				TB_range,
				LR_range,
				algoParam,
				peakRgnId,
				lowLevelChkFlag);

			if ((a_pkRgn.pkRgnIdx == 0) && (a_pkRgn_1.pkRgnIdx > 0))
				a_pkRgn = a_pkRgn_1;
			else if ((a_pkRgn.pkRgnIdx > 0) && (a_pkRgn_1.pkRgnIdx > 0))
				a_pkRgn.pkRgnIdx = 0;
		}
		return a_pkRgn;
	}
	SSG_peakRgnInfo a_pkRgn;
	memset(&a_pkRgn, 0, sizeof(SSG_peakRgnInfo));
	return a_pkRgn;
}

SSG_2DValueI _backIndexingPeakPos(SSG_2DValueI dt_pk, cv::Mat& distTranformIndexing )
{
	//矩形扫描寻找最近点
	for (int i = 0; i < distTranformIndexing.rows; i++)
	{
		for (int j = -i; j <= i; j++)
		{
			int py = dt_pk.y - i;
			int px = dt_pk.x + j;
			if ((px >= 0) && (px < distTranformIndexing.cols) && (py >= 0) && (py < distTranformIndexing.rows))
			{
				cv::Vec2i v2i = distTranformIndexing.at<cv::Vec2i>(py, px);
				if ((v2i[0] > 0) || (v2i[1] > 0))
					return { v2i[0] , v2i[1] };
			}
			py = dt_pk.y + i;
			if ((px >= 0) && (px < distTranformIndexing.cols) && (py >= 0) && (py < distTranformIndexing.rows))
			{
				cv::Vec2i v2i = distTranformIndexing.at<cv::Vec2i>(py, px);
				if ((v2i[0] > 0) || (v2i[1] > 0))
					return { v2i[0] , v2i[1] };
			}
		}
		for (int j = -i + 1; j < i; j++)
		{
			int px = dt_pk.x - i;
			int py = dt_pk.y + j;
			if ((px >= 0) && (px < distTranformIndexing.cols) && (py >= 0) && (py < distTranformIndexing.rows))
			{
				cv::Vec2i v2i = distTranformIndexing.at<cv::Vec2i>(py, px);
				if ((v2i[0] > 0) || (v2i[1] > 0))
					return { v2i[0] , v2i[1] };
			}
			px = dt_pk.x + i;
			if ((px >= 0) && (px < distTranformIndexing.cols) && (py >= 0) && (py < distTranformIndexing.rows))
			{
				cv::Vec2i v2i = distTranformIndexing.at<cv::Vec2i>(py, px);
				if ((v2i[0] > 0) || (v2i[1] > 0))
					return { v2i[0] , v2i[1] };
			}
		}
	}
	return { 0 , 0 };
}
///数据输入必须是grid格式，以进行水平方向和垂直方向的处理
///（1）寻找边界点
///（2）从最高点开始进行区域生长
///  -每次生长记录生长圈。分析生长圈，确定生长停止点
void sg_getBagPosition(
	SVzNL3DLaserLine* laser3DPoints, 
	int lineNum, 
	//std::vector<SSG_lineFeature>& all_vLineFeatures, 
	//std::vector<std::vector<int>>& noisePts, 
	const SG_bagPositionParam algoParam,
	const SSG_planeCalibPara poseCalibPara,
	std::vector<SSG_peakRgnInfo>& objOps)
{
	///噪点过滤
	//垂直方向过滤
	for (int i = 0; i < lineNum; i++)
	{
		sg_lineDataRemoveOutlier_changeOriginData(
			laser3DPoints[i].p3DPosition,
			laser3DPoints[i].nPositionCnt,
			algoParam.filterParam);
	}
	//水平方向过滤
	int hLineNum = laser3DPoints->nPositionCnt;  //Grid格式，所有扫描线的点数是一样的
	//生成水平扫描数据
	std::vector<std::vector<SVzNL3DPosition>> filterHLines;
	filterHLines.resize(hLineNum);
	for (int i = 0; i < hLineNum; i++)
		filterHLines[i].resize(lineNum);
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < hLineNum; j++)
		{
			filterHLines[j][line] = laser3DPoints[line].p3DPosition[j];
			filterHLines[j][line].pt3D.x = laser3DPoints[line].p3DPosition[j].pt3D.y;
			filterHLines[j][line].pt3D.y = laser3DPoints[line].p3DPosition[j].pt3D.x;
		}
	}
	for (int hLine = 0; hLine < hLineNum; hLine++)
	{
		//滤波，滤除异常点
		std::vector<SVzNL3DPosition> filterData;
		std::vector<int> lineNoise;
		sg_lineDataRemoveOutlier(
			(SVzNL3DPosition*)filterHLines[hLine].data(),
			(int)filterHLines[hLine].size(),
			algoParam.filterParam,
			filterData,
			lineNoise);
		for (int j = 0; j < lineNoise.size(); j++)
		{
			int lineIdx = lineNoise[j];
			laser3DPoints[lineIdx].p3DPosition[hLine].pt3D.z = 0;
		}
	}
	
	//虚假目标过滤(量化，区域标注)
	// 统计初始整个视野大小
	SVzNL3DRangeD oriRoi3D = sg_getScanDataROI(
		laser3DPoints,
		lineNum);
	SVzNLRangeD x_range = oriRoi3D.xRange;
	SVzNLRangeD y_range = oriRoi3D.yRange;

	//量化尺度，以1mm为量化尺度
	const double scale = 1.0;
	int maskX = (int)(x_range.max - x_range.min) + 1;
	if (maskX % 2 > 0)
		maskX++;
	int maskY = (int)(y_range.max - y_range.min) + 1;
	if (maskY % 2 > 0)
		maskY++;
	if ((maskX < 16) || (maskY < 16))
		return;
	const int x_skip = 2;
	const int y_skip = 2;
	maskY = maskY + y_skip * 2;
	maskX = maskX + x_skip * 2;
	//生成标注Mask，进行目标标注，根据标注结果进行虚假目标滤除
	cv::Mat bwImg = cv::Mat::zeros(maskY, maskX, CV_8UC1);//rows, cols
	//此处进行量化，并对量化产生的空白点进行插值
	const double inerPolateDistTh = 20; //2cm，两间距离大于2cm不插值
	for (int line = 0; line < lineNum; line++)
	{
		int pre_x = -1, pre_y = -1;
		SVzNL3DPosition* prePt = NULL;
		for (int i = 0; i < laser3DPoints[line].nPositionCnt; i++)
		{
			SVzNL3DPosition* pt3D = &laser3DPoints[line].p3DPosition[i];
			if (pt3D->pt3D.z < 1e-4)
				continue;
			double x = pt3D->pt3D.x;
			double y = pt3D->pt3D.y;
			int px = (int)(x - x_range.min) + x_skip;
			int py = (int)(y - y_range.min) + y_skip;
			bwImg.at<uchar>(py, px) = 1;
			//垂直插值
			if (prePt)
			{
				//计算距离，超过一定距离则不插值
				double dist = sqrt(pow(pt3D->pt3D.x - prePt->pt3D.x, 2) +
					pow(pt3D->pt3D.y - prePt->pt3D.y, 2) +
					pow(pt3D->pt3D.z - prePt->pt3D.z, 2));
				if (dist < inerPolateDistTh)
				{
					std::vector<SVzNL2DPoint> interPts;
					drawLine(
						pre_x,
						pre_y,
						px,
						py,
						interPts);
					for (int m = 0, m_max = (int)interPts.size(); m < m_max; m++)
						bwImg.at<uchar>(interPts[m].y, interPts[m].x) = 1;
				}
			}
			prePt = pt3D;
			pre_x = px;
			pre_y = py;
		}
	}
	//水平插值
	int pixWin = (int)(inerPolateDistTh / 1.0);
	for (int y = 0; y < maskY; y++)
	{
		int pre_x = -1;
		for (int x = 0; x < maskX; x++)
		{
			uchar value = bwImg.at<uchar>(y, x);
			if (value > 0)
			{
				if (pre_x >= 0)
				{
					//插值
					int x_diff = x - pre_x;
					if ((x_diff > 1) && (x_diff < pixWin))
					{
						for (int m = pre_x + 1; m < x; m++)
							bwImg.at<uchar>(y, m) = 1;
					}
				}
				pre_x = x;
			}
		}
	}
	//目标过滤
	cv::Mat labImg;
	std::vector<SSG_Region> labelRgns;
	SG_TwoPassLabel(bwImg, labImg, labelRgns, 8);
#if OUTPUT_DEBUG
	cv::Mat oriMaskImage;
	cv::normalize(labImg, oriMaskImage, 0, 255, cv::NORM_MINMAX, CV_8U);
	cv::imwrite("originMask.png", oriMaskImage);
#endif

	int vldRgnSize = (int)((algoParam.bagParam.bagL * algoParam.bagParam.bagW / 2) / (scale * scale));
	for (int rgnid = 0, rgn_max = (int)labelRgns.size(); rgnid < rgn_max; rgnid++)
	{
		SSG_Region* a_rgn = &labelRgns[rgnid];
		if (a_rgn->ptCounter < vldRgnSize)
		{
			//将所在区域的点无效
			for (int ry = a_rgn->roi.top; ry <= a_rgn->roi.bottom; ry++)
			{
				for (int rx = a_rgn->roi.left; rx <= a_rgn->roi.right; rx++)
				{
					if (labImg.at<int>(ry, rx) == a_rgn->labelID)
						labImg.at<int>(ry, rx) = 0; //invalid
				}
			}
		}
	}
	//将无效rgn的点无效
	for (int line = 0; line < lineNum; line++)
	{
		for (int i = 0; i < laser3DPoints[line].nPositionCnt; i++)
		{
			SVzNL3DPosition* pt3D = &laser3DPoints[line].p3DPosition[i];
			if (pt3D->pt3D.z < 1e-4)
				continue;
			double x = pt3D->pt3D.x;
			double y = pt3D->pt3D.y;
			int px = (int)(x - x_range.min) + x_skip;
			int py = (int)(y - y_range.min) + y_skip;
			if (labImg.at<int>(py, px) == 0)
				pt3D->pt3D.z = 0;
		}
	}
#if OUTPUT_DEBUG
	cv::Mat filterMaskImage;
	//cv::normalize(distTranformMask, maskImage, 0, 255, cv::NORM_MINMAX, CV_8U);
	cv::normalize(labImg, filterMaskImage, 0, 255, cv::NORM_MINMAX, CV_8U);
	cv::imwrite("filterMask.png", filterMaskImage);
#endif

	///开始数据处理
	//垂直数据处理
	std::vector<SSG_lineFeature> all_vLineFeatures;
	for (int i = 0; i < lineNum; i++)
	{
		if (i == 202)
			int k = 1;
		SSG_lineFeature a_line_features;
		a_line_features.lineIdx = i;
#if BAG_ALGO_USE_CORNER_FEATURE
		sg_getLineCornerFeature(
			laser3DPoints[i].p3DPosition,
			laser3DPoints[i].nPositionCnt,
			i,
			algoParam.cornerParam,
			&a_line_features);
#else
		sg_getLineLVFeature(
			filterData.data(),
			filterData.size(),
			lineIdx,
			bagParam.slopeParam,
			bagParam.valleyPara,
			&a_line_features);
#endif
		all_vLineFeatures.push_back(a_line_features);  //空行也加入，保证能按行号索引
	}

	//在虚假目标过滤后重新生成水平扫描数据
	std::vector<std::vector<SVzNL3DPosition>> hLines;
	hLines.resize(hLineNum);
	for (int i = 0; i < hLineNum; i++)
		hLines[i].resize(lineNum);
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < hLineNum; j++)
		{
			laser3DPoints[line].p3DPosition[j].nPointIdx = 0;;
			hLines[j][line] = laser3DPoints[line].p3DPosition[j];
			hLines[j][line].pt3D.x = laser3DPoints[line].p3DPosition[j].pt3D.y;
			hLines[j][line].pt3D.y = laser3DPoints[line].p3DPosition[j].pt3D.x;
		}
	}
	std::vector<SSG_lineFeature> all_hLineFeatures;
	//逐行提取特征
	for (int hLine = 0; hLine < hLineNum; hLine++)
	{
if (hLine == 14)
	int kkk = 1;
		std::vector<SVzNL3DPosition> smoothData;
		sg_lineSegSmoothing(
			hLines[hLine],
			algoParam.cornerParam.minEndingGap, //分段的Y间隔。大于此间隔，为新的分段
			algoParam.cornerParam.minEndingGap_z,//分段的Z间隔。大于此间隔，为新的分段
			5,
			smoothData);
		//sg_lineDataSmoothing(hLines[hLine], 5, smoothData);
		SSG_lineFeature a_hLine_featrues;
		a_hLine_featrues.lineIdx = hLine;
#if BAG_ALGO_USE_CORNER_FEATURE
		sg_getLineCornerFeature(
			smoothData.data(), 
			(int)smoothData.size(),
			hLine, 
			algoParam.cornerParam, 
			&a_hLine_featrues);
#else
		sg_getLineLVFeature(
			smoothData.data(), 
			smoothData.size(), 
			hLine, 
			algoParam.slopeParam, 
			algoParam.valleyPara, 
			&a_hLine_featrues);
#endif
		//if ((a_hLine_featrues.features.size() > 0) || (a_hLine_featrues.endings.size() > 0))
			all_hLineFeatures.push_back(a_hLine_featrues);//空行也加入，保证能按行号索引
	}

	/// 统计整个视野大小，在滤噪之后进行
	SVzNL3DRangeD roi3D = sg_getScanDataROI(
		laser3DPoints,
		lineNum);
	x_range = roi3D.xRange;
	y_range = roi3D.yRange;

	SSG_ROIRectD globalROI;
	globalROI.left = roi3D.xRange.min;
	globalROI.right = roi3D.xRange.max;
	globalROI.top = roi3D.yRange.min;
	globalROI.bottom = roi3D.yRange.max;

	//垂直方向特征生长（相机扫描方向）
	std::vector<SSG_featureTree> v_trees;
	std::vector<SSG_2DValueI> v_edgePts_0;
	std::vector<SSG_2DValueI> v_edgePts_1;
	sg_LVFeatureGrowing(
		all_vLineFeatures, 
		v_trees, 
		algoParam.bagParam,
		algoParam.growParam, 
		v_edgePts_0,
		v_edgePts_1);
	//对tree的node的连续性进行插值，保证无空缺
	for (int treeId = 0, tree_max = (int)v_trees.size(); treeId < tree_max; treeId++)
	{
		SSG_featureTree& a_vTree = v_trees[treeId];
		if (a_vTree.treeNodes.size() > 0)
		{
			//从后往前处理，这个插值的点插入后不影响继续处理的下标
			for (int nid = (int)a_vTree.treeNodes.size() - 2; nid >= 0; nid--)
			{
				SSG_basicFeature1D curr_node = a_vTree.treeNodes[nid];
				SSG_basicFeature1D nxt_node = a_vTree.treeNodes[nid+1];
				int line_diff = curr_node.jumpPos2D.x - nxt_node.jumpPos2D.x;
				if (line_diff < 0)
					line_diff = -line_diff;
				if (line_diff > 1)  //插值
				{
					double interp_y = (curr_node.jumpPos.y + nxt_node.jumpPos.y) / 2.0;
					double interp_z = (curr_node.jumpPos.z + nxt_node.jumpPos.z) / 2.0;
					int interp_py = (curr_node.jumpPos2D.y + nxt_node.jumpPos2D.y) / 2;
					std::vector<SSG_basicFeature1D> interp_nodes;
					int line_min, line_max;
					double x_min, x_max;
					bool normalOrder;
					if (curr_node.jumpPos2D.x < nxt_node.jumpPos2D.x)
					{
						line_min = curr_node.jumpPos2D.x;
						line_max = nxt_node.jumpPos2D.x;
						x_min = curr_node.jumpPos.x;
						x_max = nxt_node.jumpPos.x;
						normalOrder = true;
					}
					else
					{
						line_min = nxt_node.jumpPos2D.x;
						line_max = curr_node.jumpPos2D.x;
						x_min = nxt_node.jumpPos.x;
						x_max = curr_node.jumpPos.x;
						normalOrder = false;
					}
					for (int ix = line_min + 1; ix < line_max; ix++)
					{
						SSG_basicFeature1D interp_node;
						interp_node.featureType = curr_node.featureType;
						interp_node.jumpPos2D = { ix, interp_py };
						double k1 = (double)(ix - line_min) / (double)line_diff;
						double k0 = 1.0 - k1;
						double interp_x = x_min * k0 + x_max * k1;
						SVzNL3DPosition* a_pt = &laser3DPoints[interp_node.jumpPos2D.x].p3DPosition[interp_node.jumpPos2D.y];
						if (a_pt->pt3D.z < 1e-4)
						{
							a_pt->pt3D.z = interp_z;
							if (a_pt->pt3D.x < 1e-4)
							{
								a_pt->pt3D.x = interp_x;
								a_pt->pt3D.y = interp_y;
							}
						}
						interp_node.jumpPos = a_pt->pt3D;
						if (true == normalOrder)
							interp_nodes.push_back(interp_node);
						else
							interp_nodes.insert(interp_nodes.begin(), interp_node);
					}
					a_vTree.treeNodes.insert(a_vTree.treeNodes.begin() + nid+1, interp_nodes.begin(), interp_nodes.end());
				}
			}
		}
	}

	//水平方向特征生长
	std::vector<SSG_featureTree> h_trees;
	std::vector<SSG_2DValueI> h_edgePts_0;
	std::vector<SSG_2DValueI> h_edgePts_1;
	sg_LVFeatureGrowing(
		all_hLineFeatures, 
		h_trees, 
		algoParam.bagParam, 
		algoParam.growParam, 
		h_edgePts_0,
		h_edgePts_1);
	//对tree的node的连续性进行插值，保证无空缺
	for (int treeId = 0, tree_max = (int)h_trees.size(); treeId < tree_max; treeId++)
	{
		SSG_featureTree& a_hTree = h_trees[treeId];
		if (a_hTree.treeNodes.size() > 0)
		{
			//从后往前处理，这个插值的点插入后不影响继续处理的下标
			for (int nid = (int)a_hTree.treeNodes.size() - 2; nid >= 0; nid--)
			{
				SSG_basicFeature1D curr_node = a_hTree.treeNodes[nid];
				SSG_basicFeature1D nxt_node = a_hTree.treeNodes[nid + 1];
				int line_diff = curr_node.jumpPos2D.x - nxt_node.jumpPos2D.x;
				if (line_diff < 0)
					line_diff = -line_diff;
				if (line_diff > 1)  //插值
				{
					double interp_x = (curr_node.jumpPos.y + nxt_node.jumpPos.y) / 2.0;
					double interp_z = (curr_node.jumpPos.z + nxt_node.jumpPos.z) / 2.0;
					int interp_px = (curr_node.jumpPos2D.y + nxt_node.jumpPos2D.y) / 2;
					std::vector<SSG_basicFeature1D> interp_nodes;
					int hLine_min, hLine_max;
					double y_min, y_max;
					bool normalOrder;
					if (curr_node.jumpPos2D.x < nxt_node.jumpPos2D.x)
					{
						hLine_min = curr_node.jumpPos2D.x;
						hLine_max = nxt_node.jumpPos2D.x;
						y_min = curr_node.jumpPos.x;
						y_max = nxt_node.jumpPos.x;
						normalOrder = true;
					}
					else
					{
						hLine_min = nxt_node.jumpPos2D.x;
						hLine_max = curr_node.jumpPos2D.x;
						y_min = nxt_node.jumpPos.x;
						y_max = curr_node.jumpPos.x;
						normalOrder = false;
					}
					for (int iy = hLine_min + 1; iy < hLine_max; iy++)
					{
						SSG_basicFeature1D interp_node;
						interp_node.featureType = curr_node.featureType;
						interp_node.jumpPos2D = { iy, interp_px };
						double k1 = (double)(iy - hLine_min) / (double)line_diff;
						double k0 = 1.0 - k1;
						double interp_y = y_min * k0 + y_max * k1;
						SVzNL3DPosition* a_pt = &laser3DPoints[interp_node.jumpPos2D.y].p3DPosition[interp_node.jumpPos2D.x];
						if (a_pt->pt3D.z < 1e-4)
						{
							a_pt->pt3D.z = interp_z;
							if (a_pt->pt3D.x < 1e-4)
							{
								a_pt->pt3D.x = interp_x;
								a_pt->pt3D.y = interp_y;
							}
						}
						interp_node.jumpPos = { a_pt->pt3D.y, a_pt->pt3D.x, a_pt->pt3D.z };
						if (true == normalOrder)
							interp_nodes.push_back(interp_node);
						else
							interp_nodes.insert(interp_nodes.begin(), interp_node);
					}
					a_hTree.treeNodes.insert(a_hTree.treeNodes.begin() + nid+1, interp_nodes.begin(), interp_nodes.end());
				}
			}
		}
	}
	//对端点进行生长
	std::vector<SSG_featureTree> vEdge_0_trees;
	std::vector<SSG_featureTree> vEdge_1_trees;
	std::vector<SSG_featureTree> hEdge_0_trees;
	std::vector<SSG_featureTree> hEdge_1_trees;
	//端点特征生长
	if (algoParam.supportRotate == 0)
	{
		sg_getEndingGrowingTrees(
			v_edgePts_0,
			laser3DPoints,
			true, //isVScan,
			LINE_FEATURE_LINE_ENDING_0,
			vEdge_0_trees,
			algoParam.growParam);
		sg_getEndingGrowingTrees(
			v_edgePts_1,
			laser3DPoints,
			true, //isVScan,
			LINE_FEATURE_LINE_ENDING_1,
			vEdge_1_trees,
			algoParam.growParam);
		sg_getEndingGrowingTrees(
			h_edgePts_0,
			laser3DPoints,
			false, //isVScan,
			LINE_FEATURE_LINE_ENDING_0,
			hEdge_0_trees,
			algoParam.growParam);
		sg_getEndingGrowingTrees(
			h_edgePts_1,
			laser3DPoints,
			false, //isVScan,
			LINE_FEATURE_LINE_ENDING_1,
			hEdge_1_trees,
			algoParam.growParam);
	}
	else
	{
		sg_getEndingGrowingTrees_angleCheck(
			v_edgePts_0,
			laser3DPoints,
			true, //isVScan,
			LINE_FEATURE_LINE_ENDING_0,
			vEdge_0_trees,
			algoParam.growParam,
			algoParam.cornerParam.scale);
		sg_getEndingGrowingTrees_angleCheck(
			v_edgePts_1,
			laser3DPoints,
			true, //isVScan,
			LINE_FEATURE_LINE_ENDING_1,
			vEdge_1_trees,
			algoParam.growParam,
			algoParam.cornerParam.scale);
		sg_getEndingGrowingTrees_angleCheck(
			h_edgePts_0,
			laser3DPoints,
			false, //isVScan,
			LINE_FEATURE_LINE_ENDING_0,
			hEdge_0_trees,
			algoParam.growParam,
			algoParam.cornerParam.scale);
		sg_getEndingGrowingTrees_angleCheck(
			h_edgePts_1,
			laser3DPoints,
			false, //isVScan,
			LINE_FEATURE_LINE_ENDING_1,
			hEdge_1_trees,
			algoParam.growParam,
			algoParam.cornerParam.scale);
	}

	cv::Mat featureMask = cv::Mat::zeros(hLineNum, lineNum, CV_32SC4);
	//距离变换Mask，以1mm为量化尺度
	maskX = (int)(x_range.max - x_range.min) + 1;
	maskY = (int)(y_range.max - y_range.min) + 1;
	if ((maskX < 16) || (maskY < 16))
		return;
	maskY = maskY + y_skip * 2;
	maskX = maskX + x_skip * 2;
	cv::Mat distTranformMask(maskY, maskX, CV_32FC1, 0.0f);  //距离变换Mask，初始化为一个极大值1e+6
	//标记坐标索引，用于距离变换后回找坐标
	cv::Mat distTranformIndexing(maskY, maskX, CV_32SC2, cv::Vec2i(0,0));  //坐标索引
	//此处进行量化，并对量化产生的空白点进行插值
	for (int line = 0; line < lineNum; line++)
	{
		int pre_x = -1, pre_y = -1;
		SVzNL3DPosition* prePt = NULL;
		for (int i = 0; i < laser3DPoints[line].nPositionCnt; i++)
		{
			SVzNL3DPosition* pt3D = &laser3DPoints[line].p3DPosition[i];
			if (pt3D->pt3D.z < 1e-4)
				continue;
			double x = pt3D->pt3D.x;
			double y = pt3D->pt3D.y;
			int px = (int)(x - x_range.min) + x_skip;
			int py = (int)(y - y_range.min) + y_skip;
			
			cv::Vec2i v2i_exist = distTranformIndexing.at<cv::Vec2i>(py, px);
#if 0
			if ((v2i_exist[0] > 0) || (v2i_exist[1] > 0))  //多个点重复投影到同一个点上，只保留一个有效点
			{
				pt3D->pt3D.z = 0; //invalidate
			}
			else
#endif
			{
				cv::Vec2i v2i = { line, i };
				distTranformIndexing.at<cv::Vec2i>(py, px) = v2i;
				distTranformMask.at<float>(py, px) = 1e+6;
				//垂直插值
				if (prePt)
				{
					//计算距离，超过一定距离则不插值
					double dist = sqrt(pow(pt3D->pt3D.x - prePt->pt3D.x, 2) +
						pow(pt3D->pt3D.y - prePt->pt3D.y, 2) +
						pow(pt3D->pt3D.z - prePt->pt3D.z, 2));
					if (dist < inerPolateDistTh)
					{
						std::vector<SVzNL2DPoint> interPts;
						drawLine(
							pre_x,
							pre_y,
							px,
							py,
							interPts);
						for (int m = 0, m_max = (int)interPts.size(); m < m_max; m++)
							distTranformMask.at<float>(interPts[m].y, interPts[m].x) = 1e+6;
					}
				}
				prePt = pt3D;
				pre_x = px;
				pre_y = py;
			}
		}
	}
	//水平插值
	pixWin = (int)(inerPolateDistTh / 1.0);
	for(int y = 0; y < maskY; y ++)
	{
		int pre_x = -1;
		for (int x = 0; x < maskX; x++)
		{
			double value = distTranformMask.at<float>(y, x);
			if (value > 1e-4)
			{
				if (pre_x >= 0)
				{
					//插值
					int x_diff = x - pre_x;
					if ((x_diff > 1) && (x_diff < pixWin))
					{
						for(int m = pre_x + 1; m < x; m ++)
							distTranformMask.at<float>(y, m) = 1e+6;
					}
				}
				pre_x = x;
			}
		}
	}

	std::vector<SSG_treeInfo> allTreesInfo; //不包含边界
	//标注:垂直
	SSG_treeInfo a_nullTree;
	memset(&a_nullTree, 0, sizeof(SSG_treeInfo));
	allTreesInfo.push_back(a_nullTree); //保持存储位置与treeIdx相同位置，方便索引
	//标注垂直边界
	//起点
#if 0  //将所有起点归于同一棵树
	a_nullTree.treeIdx = 1;
	allTreesInfo.push_back(a_nullTree); //保持存储位置与treeIdx相同位置，方便索引
	for (int i = 0, i_max = v_edgePts_0.size(); i < i_max; i++)
	{
		SSG_2DValueI* an_edge = &v_edgePts_0[i];
		if (laser3DPoints[an_edge->x].p3DPosition[an_edge->y].pt3D.z > 1e-4) //虚假目标过滤后点会置0
		{
			int vType = laser3DPoints[an_edge->x].p3DPosition[an_edge->y].nPointIdx & 0x0f;
			if (vType == 0)
			{
				laser3DPoints[an_edge->x].p3DPosition[an_edge->y].nPointIdx = an_edge->value;
				laser3DPoints[an_edge->x].p3DPosition[an_edge->y].nPointIdx &= 0xffff;
				laser3DPoints[an_edge->x].p3DPosition[an_edge->y].nPointIdx += a_nullTree.treeIdx << 16;
				featureMask.at<cv::Vec4i>(an_edge->y, an_edge->x)[0] = a_nullTree.treeIdx; //edgeID
				featureMask.at<cv::Vec4i>(an_edge->y, an_edge->x)[1] = an_edge->value;
				featureMask.at<cv::Vec4i>(an_edge->y, an_edge->x)[2] = 1;  //vscan

				int px = (int)(laser3DPoints[an_edge->x].p3DPosition[an_edge->y].pt3D.x - x_range.min);
				int py = (int)(laser3DPoints[an_edge->x].p3DPosition[an_edge->y].pt3D.y - y_range.min);
				distTranformMask.at<float>(py, px) = 0;
			}
		}
	}
#else //根据起点的生长树进行标注
	int treeID = 1;
	for (int i = 0, i_max = (int)vEdge_0_trees.size(); i < i_max; i++)
	{
		SSG_featureTree* a_vEdgeTree = &vEdge_0_trees[i];

		//记录Tree的信息
		SSG_treeInfo a_treeInfo;
		a_treeInfo.vTreeFlag = 1;
		a_treeInfo.treeIdx = treeID;
		a_treeInfo.treeType = a_vEdgeTree->treeType;
		a_treeInfo.sLineIdx = a_vEdgeTree->sLineIdx;
		a_treeInfo.eLineIdx = a_vEdgeTree->eLineIdx;
		a_treeInfo.roi = a_vEdgeTree->roi;
		allTreesInfo.push_back(a_treeInfo);
		//在原始点云上标记，同时有Mask上标记
		for (int j = 0, j_max = (int)a_vEdgeTree->treeNodes.size(); j < j_max; j++)
		{
			SSG_basicFeature1D* a_feature = &a_vEdgeTree->treeNodes[j];
			if (laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.z > 1e-4)//虚假目标过滤后点会置0
			{
				laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].nPointIdx = a_feature->featureType;
				laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].nPointIdx &= 0xffff;
				laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].nPointIdx += treeID << 16;
				featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[0] = treeID; //edgeID
				featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[1] = a_vEdgeTree->treeType;
				featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[2] = 1;  //vscan

				int px = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.x - x_range.min) + x_skip;
				int py = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.y - y_range.min) + y_skip;
				distTranformMask.at<float>(py, px) = 0;
			}
		}
		treeID++;
	}
#endif
	//终点
#if 0 //将所有起点归于同一棵树
	a_nullTree.treeIdx = 2;
	allTreesInfo.push_back(a_nullTree); //保持存储位置与treeIdx相同位置，方便索引
	for (int i = 0, i_max = v_edgePts_1.size(); i < i_max; i++)
	{
		SSG_2DValueI* an_edge = &v_edgePts_1[i];
		if (laser3DPoints[an_edge->x].p3DPosition[an_edge->y].pt3D.z > 1e-4)//虚假目标过滤后点会置0
		{
			int vType = laser3DPoints[an_edge->x].p3DPosition[an_edge->y].nPointIdx & 0x0f;
			if (vType == 0)
			{
				laser3DPoints[an_edge->x].p3DPosition[an_edge->y].nPointIdx = an_edge->value;
				laser3DPoints[an_edge->x].p3DPosition[an_edge->y].nPointIdx &= 0xffff;
				laser3DPoints[an_edge->x].p3DPosition[an_edge->y].nPointIdx += a_nullTree.treeIdx << 16;
				featureMask.at<cv::Vec4i>(an_edge->y, an_edge->x)[0] = a_nullTree.treeIdx; //edgeID
				featureMask.at<cv::Vec4i>(an_edge->y, an_edge->x)[1] = an_edge->value;
				featureMask.at<cv::Vec4i>(an_edge->y, an_edge->x)[2] = 1;  //vscan

				int px = (int)(laser3DPoints[an_edge->x].p3DPosition[an_edge->y].pt3D.x - x_range.min);
				int py = (int)(laser3DPoints[an_edge->x].p3DPosition[an_edge->y].pt3D.y - y_range.min);
				distTranformMask.at<float>(py, px) = 0;
			}
		}
	}
#else //根据起点的生长树进行标注
	for (int i = 0, i_max = (int)vEdge_1_trees.size(); i < i_max; i++)
	{
		SSG_featureTree* a_vEdgeTree = &vEdge_1_trees[i];

		//记录Tree的信息
		SSG_treeInfo a_treeInfo;
		a_treeInfo.vTreeFlag = 1;
		a_treeInfo.treeIdx = treeID;
		a_treeInfo.treeType = a_vEdgeTree->treeType;
		a_treeInfo.sLineIdx = a_vEdgeTree->sLineIdx;
		a_treeInfo.eLineIdx = a_vEdgeTree->eLineIdx;
		a_treeInfo.roi = a_vEdgeTree->roi;
		allTreesInfo.push_back(a_treeInfo);
		//在原始点云上标记，同时有Mask上标记
		for (int j = 0, j_max = (int)a_vEdgeTree->treeNodes.size(); j < j_max; j++)
		{
			SSG_basicFeature1D* a_feature = &a_vEdgeTree->treeNodes[j];
			if (laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.z > 1e-4)//虚假目标过滤后点会置0
			{
				laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].nPointIdx = a_feature->featureType;
				laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].nPointIdx &= 0xffff;
				laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].nPointIdx += treeID << 16;
				featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[0] = treeID; //edgeID
				featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[1] = a_vEdgeTree->treeType;
				featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[2] = 1;  //vscan

				int px = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.x - x_range.min)+ x_skip;
				int py = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.y - y_range.min) + y_skip;
				distTranformMask.at<float>(py, px) = 0;
			}
		}
		treeID++;
	}
#endif
	//标注水平边界
	//起点
#if 0 //将所有起点归于同一棵树
	a_nullTree.treeIdx = 3;
	allTreesInfo.push_back(a_nullTree); //保持存储位置与treeIdx相同位置，方便索引
	for (int i = 0, i_max = h_edgePts_0.size(); i < i_max; i++)
	{
		SSG_2DValueI* an_edge = &h_edgePts_0[i];
		if (laser3DPoints[an_edge->y].p3DPosition[an_edge->x].pt3D.z > 1e-4)//虚假目标过滤后点会置0
		{
			int hType = laser3DPoints[an_edge->y].p3DPosition[an_edge->x].nPointIdx & 0xf0;
			if (hType == 0)  //ending的优先级最低。
			{
				laser3DPoints[an_edge->y].p3DPosition[an_edge->x].nPointIdx += an_edge->value << 4;
				laser3DPoints[an_edge->y].p3DPosition[an_edge->x].nPointIdx &= 0xffff;
				laser3DPoints[an_edge->y].p3DPosition[an_edge->x].nPointIdx += a_nullTree.treeIdx << 16;
				featureMask.at<cv::Vec4i>(an_edge->x, an_edge->y)[0] = a_nullTree.treeIdx; //edgeID
				featureMask.at<cv::Vec4i>(an_edge->x, an_edge->y)[1] += an_edge->value << 4;
				featureMask.at<cv::Vec4i>(an_edge->x, an_edge->y)[2] = 0;  //hscan

				int px = (int)(laser3DPoints[an_edge->y].p3DPosition[an_edge->x].pt3D.x - x_range.min);
				int py = (int)(laser3DPoints[an_edge->y].p3DPosition[an_edge->x].pt3D.y - y_range.min);
				distTranformMask.at<float>(py, px) = 0;
			}
		}
	}
#else //根据起点的生长树进行标注
	for (int i = 0, i_max = (int)hEdge_0_trees.size(); i < i_max; i++)
	{
		SSG_featureTree* a_hEdgeTree = &hEdge_0_trees[i];
		//记录Tree的信息
		SSG_treeInfo a_treeInfo;
		a_treeInfo.vTreeFlag = 0;
		a_treeInfo.treeIdx = treeID;
		a_treeInfo.treeType = a_hEdgeTree->treeType;
		a_treeInfo.sLineIdx = a_hEdgeTree->sLineIdx;
		a_treeInfo.eLineIdx = a_hEdgeTree->eLineIdx;
		a_treeInfo.roi.left = a_hEdgeTree->roi.top;  //水平扫描xy是交换的
		a_treeInfo.roi.right = a_hEdgeTree->roi.bottom;
		a_treeInfo.roi.top = a_hEdgeTree->roi.left;
		a_treeInfo.roi.bottom = a_hEdgeTree->roi.right;
		allTreesInfo.push_back(a_treeInfo);
		//在原始点云上标记，同时有Mask上标记
		for (int j = 0, j_max = (int)a_hEdgeTree->treeNodes.size(); j < j_max; j++)
		{
			SSG_basicFeature1D* a_feature = &a_hEdgeTree->treeNodes[j];
			if (laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.z > 1e-4)//虚假目标过滤后点会置0
			{
				int existEdgeId = laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].nPointIdx >> 16;
				if (existEdgeId == 0)
				{
					laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].nPointIdx += a_feature->featureType << 4;
					laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].nPointIdx &= 0xffff;
					laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].nPointIdx += treeID << 16;
					featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[0] = treeID;
					featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[1] += a_hEdgeTree->treeType << 4;
					featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[2] = 2;//hsan flag

					int px = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.x - x_range.min) + x_skip;
					int py = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.y - y_range.min) + y_skip;
					distTranformMask.at<float>(py, px) = 0;
				}
			}
		}
		treeID++;
	}
#endif
	//终点
#if 0 //将所有起点归于同一棵树
	a_nullTree.treeIdx = 4;
	allTreesInfo.push_back(a_nullTree); //保持存储位置与treeIdx相同位置，方便索引
	for (int i = 0, i_max = h_edgePts_1.size(); i < i_max; i++)
	{
		SSG_2DValueI* an_edge = &h_edgePts_1[i];
		if (laser3DPoints[an_edge->y].p3DPosition[an_edge->x].pt3D.z > 1e-4)//虚假目标过滤后点会置0
		{
			int hType = laser3DPoints[an_edge->y].p3DPosition[an_edge->x].nPointIdx & 0xf0;
			if (hType == 0)  //ending的优先级最低。
			{
				laser3DPoints[an_edge->y].p3DPosition[an_edge->x].nPointIdx += an_edge->value << 4;
				laser3DPoints[an_edge->y].p3DPosition[an_edge->x].nPointIdx &= 0xffff;
				laser3DPoints[an_edge->y].p3DPosition[an_edge->x].nPointIdx += a_nullTree.treeIdx << 16;
				featureMask.at<cv::Vec4i>(an_edge->x, an_edge->y)[0] = a_nullTree.treeIdx; //edgeID
				featureMask.at<cv::Vec4i>(an_edge->x, an_edge->y)[1] += an_edge->value << 4;
				featureMask.at<cv::Vec4i>(an_edge->x, an_edge->y)[2] = 0;  //hscan

				int px = (int)(laser3DPoints[an_edge->y].p3DPosition[an_edge->x].pt3D.x - x_range.min);
				int py = (int)(laser3DPoints[an_edge->y].p3DPosition[an_edge->x].pt3D.y - y_range.min);
				distTranformMask.at<float>(py, px) = 0;
			}
		}
	}
#else //根据起点的生长树进行标注
	for (int i = 0, i_max = (int)hEdge_1_trees.size(); i < i_max; i++)
	{
		SSG_featureTree* a_hEdgeTree = &hEdge_1_trees[i];
		//记录Tree的信息
		SSG_treeInfo a_treeInfo;
		a_treeInfo.vTreeFlag = 0;
		a_treeInfo.treeIdx = treeID;
		a_treeInfo.treeType = a_hEdgeTree->treeType;
		a_treeInfo.sLineIdx = a_hEdgeTree->sLineIdx;
		a_treeInfo.eLineIdx = a_hEdgeTree->eLineIdx;
		a_treeInfo.roi.left = a_hEdgeTree->roi.top;  //水平扫描xy是交换的
		a_treeInfo.roi.right = a_hEdgeTree->roi.bottom;
		a_treeInfo.roi.top = a_hEdgeTree->roi.left;
		a_treeInfo.roi.bottom = a_hEdgeTree->roi.right;
		allTreesInfo.push_back(a_treeInfo);
		//在原始点云上标记，同时有Mask上标记
		for (int j = 0, j_max = (int)a_hEdgeTree->treeNodes.size(); j < j_max; j++)
		{
			SSG_basicFeature1D* a_feature = &a_hEdgeTree->treeNodes[j];
			if (laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.z > 1e-4)//虚假目标过滤后点会置0
			{
				int existEdgeId = laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].nPointIdx >> 16;
				if (existEdgeId == 0)
				{
					laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].nPointIdx += a_feature->featureType << 4;
					laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].nPointIdx &= 0xffff;
					laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].nPointIdx += treeID << 16;
					featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[0] = treeID;
					featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[1] += a_hEdgeTree->treeType << 4;
					featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[2] = 2;//hsan flag

					int px = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.x - x_range.min) + x_skip;
					int py = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.y - y_range.min) + y_skip;
					distTranformMask.at<float>(py, px) = 0;

				}
			}
		}
		treeID++;
	}
#endif
	//treeIndex 5-8预留给迭代后目标的边界：5-上边界， 6-下边界，7-左边界，8-右边界
	int hvTreeIdx = treeID;
	int vTreeStart = treeID;
	for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++)
	{
		SSG_featureTree* a_vTree = &v_trees[i];
		
		//记录Tree的信息
		SSG_treeInfo a_treeInfo;
		a_treeInfo.vTreeFlag = 1;
		a_treeInfo.treeIdx = hvTreeIdx;
		a_treeInfo.treeType = a_vTree->treeType;
		a_treeInfo.sLineIdx = a_vTree->sLineIdx;
		a_treeInfo.eLineIdx = a_vTree->eLineIdx;
		a_treeInfo.roi = a_vTree->roi;
		allTreesInfo.push_back(a_treeInfo);
		//在原始点云上标记，同时有Mask上标记
		for (int j = 0, j_max = (int)a_vTree->treeNodes.size(); j < j_max; j++)
		{
			SSG_basicFeature1D* a_feature = &a_vTree->treeNodes[j];
			if (laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.z > 1e-4)//虚假目标过滤后点会置0
			{
				int existEdgeId = laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].nPointIdx >> 16;
				if (existEdgeId == 0)
				{
					laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].nPointIdx = a_feature->featureType;
					laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].nPointIdx &= 0xffff;
					laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].nPointIdx += hvTreeIdx << 16;
					featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[0] = hvTreeIdx; //edgeID
					featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[1] = a_vTree->treeType;
					featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[2] = 1;  //vscan

					int px = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.x - x_range.min) + x_skip;
					int py = (int)(laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].pt3D.y - y_range.min) + y_skip;
					distTranformMask.at<float>(py, px) = 0;
				}
			}
		}
		hvTreeIdx++;
	}
	int hTreeStart = hvTreeIdx;
	////标注:水平特征
	for (int i = 0, i_max = (int)h_trees.size(); i < i_max; i++)
	{
		SSG_featureTree* a_hTree = &h_trees[i];
		//记录Tree的信息
		SSG_treeInfo a_treeInfo;
		a_treeInfo.vTreeFlag = 0;
		a_treeInfo.treeIdx = hvTreeIdx;
		a_treeInfo.treeType = a_hTree->treeType;
		a_treeInfo.sLineIdx = a_hTree->sLineIdx;
		a_treeInfo.eLineIdx = a_hTree->eLineIdx;
		a_treeInfo.roi.left = a_hTree->roi.top;  //水平扫描xy是交换的
		a_treeInfo.roi.right = a_hTree->roi.bottom;
		a_treeInfo.roi.top = a_hTree->roi.left;
		a_treeInfo.roi.bottom = a_hTree->roi.right;
		allTreesInfo.push_back(a_treeInfo);
		//在原始点云上标记，同时有Mask上标记
		for (int j = 0, j_max = (int)a_hTree->treeNodes.size(); j < j_max; j++)
		{
			SSG_basicFeature1D* a_feature = &a_hTree->treeNodes[j];
			if (laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.z > 1e-4)//虚假目标过滤后点会置0
			{
				int existEdgeId = laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].nPointIdx >> 16;
				if (existEdgeId == 0)
				{
					laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].nPointIdx += a_feature->featureType << 4;
					laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].nPointIdx &= 0xffff;
					laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].nPointIdx += hvTreeIdx << 16;
					featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[0] = hvTreeIdx;
					featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[1] += a_hTree->treeType << 4;
					featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[2] = 2;//hsan flag

					int px = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.x - x_range.min) + x_skip;
					int py = (int)(laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].pt3D.y - y_range.min) + y_skip;
					distTranformMask.at<float>(py, px) = 0;
				}
			}
		}
		hvTreeIdx++;
	}
	int hvTreeSize = hvTreeIdx;

#if 0
	//边界检查：垂直边界的点所有的线上不允许有水平边界；水平边界的点所有的线上不允许有垂直边界
	v_edgePts_0.insert(v_edgePts_0.end(), v_edgePts_1.begin(), v_edgePts_1.end());
	h_edgePts_0.insert(h_edgePts_0.end(), h_edgePts_1.begin(), h_edgePts_1.end());
	int abnormalChkWin = 3;
	for (int i = 0, i_max = v_edgePts_0.size(); i < i_max; i++)
	{
		SSG_2DValueI* an_edge = &v_edgePts_0[i];
		int col = an_edge->x;
if (col == 586)
	int kkk = 1;
		if (LINE_FEATURE_LINE_ENDING_0 == an_edge->value)
		{
			int y = an_edge->y+1;
			while (y < hLineNum)
			{
				SVzNL3DPosition* a_pt = &laser3DPoints[col].p3DPosition[y];
				if (a_pt->pt3D.z < 1e-4)
					break;
				int hType = (a_pt->nPointIdx >> 4) & 0x0f;
				if ((LINE_FEATURE_LINE_ENDING_0 == hType) || (LINE_FEATURE_LINE_ENDING_1 == hType))
				{
					//检查是否是孤立突起
					bool abormalFlag = _LRChkAbnormal(laser3DPoints, lineNum, col, y, abnormalChkWin);
					if (true == abormalFlag)
					{
						a_pt->nPointIdx &= 0xffffff0f;;
						featureMask.at<cv::Vec4i>(y, col)[0] = 0; //edgeID
						featureMask.at<cv::Vec4i>(y, col)[1] &= 0xffffff0f;
						featureMask.at<cv::Vec4i>(y, col)[2] = 0;  //vscan
						y++;
					}
					else
						break;
				}
				else
					break;
			}
		}
		else if (LINE_FEATURE_LINE_ENDING_1 == an_edge->value)
		{
			int y = an_edge->y - 1;
			while (y >= 0)
			{
				SVzNL3DPosition* a_pt = &laser3DPoints[col].p3DPosition[y];
				if (a_pt->pt3D.z < 1e-4)
					break;
				int hType = (a_pt->nPointIdx >> 4) & 0x0f;
				if ((LINE_FEATURE_LINE_ENDING_0 == hType) || (LINE_FEATURE_LINE_ENDING_1 == hType))
				{
					bool abormalFlag = _LRChkAbnormal(laser3DPoints, lineNum, col, y, abnormalChkWin);
					if (true == abormalFlag)
					{
						a_pt->nPointIdx &= 0xffffff0f;;
						featureMask.at<cv::Vec4i>(y, col)[0] = 0; //edgeID
						featureMask.at<cv::Vec4i>(y, col)[1] &= 0xffffff0f;
						featureMask.at<cv::Vec4i>(y, col)[2] = 0;  //vscan
						y--;
					}
					else
						break;
				}
				else
					break;
			}
		}
	}
	for (int i = 0, i_max = h_edgePts_0.size(); i < i_max; i++)
	{
		SSG_2DValueI* an_edge = &h_edgePts_0[i];
		int row = an_edge->x;
		if (LINE_FEATURE_LINE_ENDING_0 == an_edge->value)
		{
			int x = an_edge->y + 1;
			while (x < lineNum)
			{
				SVzNL3DPosition* a_pt = &laser3DPoints[x].p3DPosition[row];
				if (a_pt->pt3D.z < 1e-4)
					break;
				int vType = (a_pt->nPointIdx ) & 0x0f;
				if ((LINE_FEATURE_LINE_ENDING_0 == vType) || (LINE_FEATURE_LINE_ENDING_1 == vType))
				{
					bool abnormalFlag = _TBChkAbnormal(laser3DPoints, hLineNum, x, row, abnormalChkWin);
					if (true == abnormalFlag)
					{
						a_pt->nPointIdx &= 0xfffffff0;;
						featureMask.at<cv::Vec4i>(row, x)[0] = 0; //edgeID
						featureMask.at<cv::Vec4i>(row, x)[1] &= 0xfffffff0;
						featureMask.at<cv::Vec4i>(row, x)[2] = 0;  //vscan
						x++;
					}
					else
						break;
				}
				else
					break;
			}
		}
		else if (LINE_FEATURE_LINE_ENDING_1 == an_edge->value)
		{
			int x = an_edge->y - 1;
			while (x >= 0)
			{
				SVzNL3DPosition* a_pt = &laser3DPoints[x].p3DPosition[row];
				if (a_pt->pt3D.z < 1e-4)
					break;
				int vType = (a_pt->nPointIdx) & 0x0f;
				if ((LINE_FEATURE_LINE_ENDING_0 == vType) || (LINE_FEATURE_LINE_ENDING_1 == vType))
				{
					bool abnormalFlag = _TBChkAbnormal(laser3DPoints, hLineNum, x, row, abnormalChkWin);
					if (true == abnormalFlag)
					{
						a_pt->nPointIdx &= 0xfffffff0;;
						featureMask.at<cv::Vec4i>(row, x)[0] = 0; //edgeID
						featureMask.at<cv::Vec4i>(row, x)[1] &= 0xfffffff0;
						featureMask.at<cv::Vec4i>(row, x)[2] = 0;  //vscan
						x--;
					}
					else
						break;
				}
				else
					break;
			}
		}
	}
#endif


	//计算大概的行距和点距
	double x_scale = (x_range.max - x_range.min) / lineNum;
	double y_scale = (y_range.max - y_range.min) / hLineNum;
	//计算种子点最小区域:袋子宽度的1/2略小的的矩形区域


	/// 寻找区域最高点
#if 0
	SSG_localPkParam searchWin;
	searchWin.seachW_lines = (int)((algoParam.bagParam.bagW * 0.4) / x_scale);
	searchWin.searchW_pts = (int)((algoParam.bagParam.bagW * 0.4) / y_scale);
	std::vector<SSG_2DValueI> peaks;
	sg_getLocalPeaks(laser3DPoints, lineNum, peaks, searchWin);
	//按照高度排序
	std::sort(peaks.begin(), peaks.end(), compareByHeight);
	for (int i = 0, i_max = peaks.size(); i < i_max; i++)
		featureMask.at<cv::Vec4i>(peaks[i].y, peaks[i].x)[3] = 1;  //peak flag
#else
	cv::Mat distTransform;
	sg_distanceTrans(distTranformMask, distTransform, 0);
#if OUTPUT_DEBUG  //debug	
	cv::Mat maskImage;
	cv::normalize(distTranformMask, maskImage, 0, 255, cv::NORM_MINMAX, CV_8U);
	cv::imwrite("distTransformMask.png", maskImage);
	cv::Mat dtImage;
	cv::normalize(distTransform, dtImage, 0, 255, cv::NORM_MINMAX, CV_8U);
	cv::imwrite("distTransform.png", dtImage);
#endif

	SSG_localPkParam searchWin;
	searchWin.seachW_lines = (int)(algoParam.bagParam.bagW * 0.4);
	searchWin.searchW_pts = (int)(algoParam.bagParam.bagW * 0.4);
	std::vector<SSG_2DValueI> dt_peaks;
	sg_getLocalPeaks_distTransform(distTransform, dt_peaks, searchWin);
	//获取Peaks
	int invlidDistToEdge = 50; //距离边缘近的Peak是非法点。
	double minPeakValue = algoParam.bagParam.bagW / 8; 
	std::vector<SSG_2DValueI> peaks;
	for (int i = 0; i < dt_peaks.size(); i++)
	{
		//边界处的Peak为不合格Peak，去除
		int x_diff_0 = dt_peaks[i].x;  //距左边距离，单位为mm（量化尺度为1mm）
		int x_diff_1 = distTransform.cols - dt_peaks[i].x;//距右边距离
		int y_diff_0 = dt_peaks[i].y;//距上边距离
		int y_diff_1 = distTransform.rows - dt_peaks[i].y;//距下边距离
		if ((x_diff_0 < invlidDistToEdge) || (x_diff_1 < invlidDistToEdge) ||
			(y_diff_0 < invlidDistToEdge) || (y_diff_1 < invlidDistToEdge) ||
			(dt_peaks[i].valueD < minPeakValue))
			continue;

		//在distTranformIndexing中回找坐标
		double pkValue = dt_peaks[i].valueD;
		SSG_2DValueI a_peak = _backIndexingPeakPos(dt_peaks[i], distTranformIndexing);
		a_peak.valueD = pkValue; // laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y].pt3D.z;
		peaks.push_back(a_peak);
	}
	//按照高度排序
	std::sort(peaks.begin(), peaks.end(), compareByHeight);
	for (int i = 0, i_max = (int)peaks.size(); i < i_max; i++)
		featureMask.at<cv::Vec4i>(peaks[i].y, peaks[i].x)[3] = 1;  //peak flag

#if 0
	//使用真实的z值代替距离变换值
	for (int i = 0, i_max = peaks.size(); i < i_max; i++)
	{
		peaks[i].valueD = laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y].pt3D.z;
	}
#endif
#endif

	/// 以区域最高点作为种子进行区域生长
	std::vector<SSG_peakRgnInfo> peakRgns;
	int peakRgnId = 1;
	for (int i = 0, i_max = (int)peaks.size(); i < i_max; i++)
	{
if (i == 3)
	int kkk = 1;

		SVzNL3DPosition* pk_pt = &(laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y]);
		int pkRgnId = (pk_pt->nPointIdx >> 8) & 0xff;
		if (pkRgnId > 0)
			continue;

		//生长
		//进行水平和垂直方向扫描得到边界点 
		std::vector< SSG_lineConotours> topContour;
		std::vector< SSG_lineConotours> bottomContour;
		std::vector< SSG_lineConotours> leftContour;
		std::vector< SSG_lineConotours> rightContour;
		int maxEdgeId_top = 0, maxEdgeId_btm = 0, maxEdgeId_left = 0, maxEdgeId_right = 0;
		sg_peakXYScan(
			laser3DPoints, 
			lineNum, 
			featureMask, 
			peaks[i], 
			algoParam.growParam, 
			algoParam.bagParam,
			false,
			topContour, 
			bottomContour, 
			leftContour, 
			rightContour, 
			&maxEdgeId_top, 
			&maxEdgeId_btm, 
			&maxEdgeId_left, 
			&maxEdgeId_right);
		
		
		int vldSide = 0;
		if (leftContour.size() > 30)
			vldSide ++;
		if (rightContour.size() > 30)
			vldSide++;
		if (topContour.size() > 30)
			vldSide++;
		if (bottomContour.size() > 30)
			vldSide++;

		int invldSide = 0;
		if (leftContour.size() < 10)
			invldSide++;
		if (rightContour.size() < 10)
			invldSide++;
		if (topContour.size() < 10)
			invldSide++;
		if (bottomContour.size() < 10)
			invldSide++;

		if ( (vldSide < 3) || (invldSide > 0))
			continue;

		//全匹配：对于褶皱较多的袋子，需要全匹配寻找到正确的边
		std::vector<SSG_edgeMatchInfo> matchTable_TB;
		std::vector< SSG_matchPair> TB_pairs;
		std::vector<SSG_conotourPair> TB_contourPairs;
		int TB_matchNum = 0;
		_getMatchTable(
			topContour,
			bottomContour,
			maxEdgeId_top,
			maxEdgeId_btm,
			true, //isVScan,
			vTreeStart,
			hTreeStart,
			matchTable_TB,
			TB_pairs,
			TB_contourPairs,
			&TB_matchNum);
		if (TB_matchNum < 25)
			continue;

		std::vector< SSG_matchPair> LR_pairs;
		std::vector<SSG_edgeMatchInfo> matchTable_LR;
		std::vector<SSG_conotourPair> LR_contourPairs;
		int LR_matchNum = 0;
		_getMatchTable(
			leftContour,
			rightContour,
			maxEdgeId_left,
			maxEdgeId_right,
			false, //isHScan,
			vTreeStart,
			hTreeStart,
			matchTable_LR,
			LR_pairs,
			LR_contourPairs,
			&LR_matchNum);

		if (LR_matchNum < 25)
			continue;

		int lowLevelChkFlag = 0;
		SSG_peakRgnInfo a_pkRgn = _maxLikelihoodMatch(
			laser3DPoints,
			lineNum,
			hvTreeSize,
			peaks[i],
			matchTable_TB,
			TB_pairs,
			TB_contourPairs,
			TB_matchNum,
			maxEdgeId_btm,
			matchTable_LR,
			LR_pairs,
			LR_contourPairs,
			LR_matchNum,
			maxEdgeId_right,
			allTreesInfo,
			vTreeStart,
			hTreeStart,
			globalROI,
			algoParam,
			peakRgnId,
			&lowLevelChkFlag);
		if (a_pkRgn.pkRgnIdx > 0) 
		{
			peakRgns.push_back(a_pkRgn);
			peakRgnId++;
		}
	}
#if 1
	///迭代处理！！！！保证没有大于袋子的目标未处理
	///对于剩下的目标，如果没有检出的使用推理方法进行。
	///扫描时检测保证水平或垂直方向的宽度是否满足袋子尺寸，然后在另一个方向进行推理方法检测
	while (1)
	{
		std::vector<SSG_peakRgnInfo> iter_objs;
		//将没有处理的Peak点保留
		std::vector<SSG_2DValueI> residualPeaks;
		for (int i = 0, i_max = (int)peaks.size(); i < i_max; i++)
		{
			SVzNL3DPosition* pk_pt = &(laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y]);
			int pkRgnId = (pk_pt->nPointIdx >> 8) & 0xff;
			if (pkRgnId == 0)
			{
				residualPeaks.push_back(peaks[i]);
			}
		}
		if (residualPeaks.size() == 0)
			break;

		bool rgnPtAsEdge = true;
		for (int ri = 0; ri < residualPeaks.size(); ri++)
		{
			SVzNL3DPosition* pk_pt = &(laser3DPoints[residualPeaks[ri].x].p3DPosition[residualPeaks[ri].y]);
			int pkRgnId = (pk_pt->nPointIdx >> 8) & 0xff;
			if (pkRgnId > 0)
				continue;
			//生长
			//进行水平和垂直方向扫描得到边界点 
			std::vector< SSG_lineConotours> resi_topContour;
			std::vector< SSG_lineConotours> resi_bottomContour;
			std::vector< SSG_lineConotours> resi_leftContour;
			std::vector< SSG_lineConotours> resi_rightContour;
			int resi_maxEdgeId_top = 0, resi_maxEdgeId_btm = 0, resi_maxEdgeId_left = 0, resi_maxEdgeId_right = 0;
			sg_peakXYScan(
				laser3DPoints, 
				lineNum, 
				featureMask, 
				residualPeaks[ri], 
				algoParam.growParam, 
				algoParam.bagParam,
				true,
				resi_topContour, 
				resi_bottomContour, 
				resi_leftContour, 
				resi_rightContour,
				&resi_maxEdgeId_top, 
				&resi_maxEdgeId_btm, 
				&resi_maxEdgeId_left, 
				&resi_maxEdgeId_right);

			if ((resi_topContour.size() == 0) || (resi_bottomContour.size() == 0) || (resi_leftContour.size() == 0) || (resi_rightContour.size() == 0))
				continue;

			//分段计算平均宽度和平均高度以及分段ROI
			//全匹配（将水平所有分段的所有可能距离
			std::vector<SSG_edgeMatchInfo> matchTable_TB;
			std::vector< SSG_matchPair> TB_pairs;
			std::vector<SSG_conotourPair> TB_contourPairs;
			int TB_matchNum = 0;
			_getMatchTable(
				resi_topContour,
				resi_bottomContour,
				resi_maxEdgeId_top,
				resi_maxEdgeId_btm,
				true, //isVScan,
				vTreeStart,
				hTreeStart,
				matchTable_TB,
				TB_pairs,
				TB_contourPairs,
				&TB_matchNum);

			if (TB_matchNum < 25)
				continue;

			std::vector< SSG_matchPair> LR_pairs;
			std::vector<SSG_edgeMatchInfo> matchTable_LR;
			std::vector<SSG_conotourPair> LR_contourPairs;
			int LR_matchNum = 0;
			_getMatchTable(
				resi_leftContour,
				resi_rightContour,
				resi_maxEdgeId_left,
				resi_maxEdgeId_right,
				false, //isHScan,
				vTreeStart,
				hTreeStart,
				matchTable_LR,
				LR_pairs,
				LR_contourPairs,
				&LR_matchNum);

			if (LR_matchNum < 25)
				continue;

			int lowLevelChkFlag = 0;
			SSG_peakRgnInfo a_pkRgn = _maxLikelihoodMatch(
				laser3DPoints,
				lineNum,
				hvTreeSize,
				peaks[ri],
				matchTable_TB,
				TB_pairs,
				TB_contourPairs,
				TB_matchNum,
				resi_maxEdgeId_btm,
				matchTable_LR,
				LR_pairs,
				LR_contourPairs,
				LR_matchNum,
				resi_maxEdgeId_right,
				allTreesInfo,
				vTreeStart,
				hTreeStart,
				globalROI,
				algoParam,
				peakRgnId,
				&lowLevelChkFlag);
#if 0
			if (lowLevelChkFlag > 0)
			{
				//lowLevelFlag_T + lowLevelFlag_B<<1 + lowLevelFlag_L<<2 + lowLevelFlag_R<<3;
				if (lowLevelChkFlag & 0x01)  //Top
				{

				}
				if (lowLevelChkFlag & 0x02)  //Bottom
				{
				}
				if (lowLevelChkFlag & 0x04)  //Left
				{
				}
				if (lowLevelChkFlag & 0x08)  //Rigjt
				{
				}
			}
#endif
			if (a_pkRgn.pkRgnIdx > 0)
			{
				iter_objs.push_back(a_pkRgn);
				peakRgnId++;
			}
		}
		if (iter_objs.size() == 0)
			break;

		peakRgns.insert(peakRgns.end(), iter_objs.begin(), iter_objs.end());
		//为下一次迭代准备
		peaks.clear();
		peaks.insert(peaks.end(), residualPeaks.begin(), residualPeaks.end());  
	}

	///将剩余的小目标检出，用于碰撞检测
	///不能有未知的未处理的区域，以防止未知的碰撞
	///记录所有的大于1/4L*1/4W的目标 
	std::vector<SSG_2DValueI> smallObjPeaks;  //记录0.25L * 0.25W的目标，用于碰撞检查
	//将最后没有处理的Peak点保留
	std::vector<SSG_2DValueI> residualPeaks;
	for (int i = 0, i_max = (int)peaks.size(); i < i_max; i++)
	{
		SVzNL3DPosition* pk_pt = &(laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y]);
		int pkRgnId = (pk_pt->nPointIdx >> 8) & 0xff;
		if (pkRgnId == 0)
		{
			residualPeaks.push_back(peaks[i]);
		}
	}
	if(residualPeaks.size() > 0)
	{
		bool rgnPtAsEdge = true;
		for (int ri = 0; ri < residualPeaks.size(); ri++)
		{
			//生长
			//进行水平和垂直方向扫描得到边界点 
			std::vector< SSG_lineConotours> resi_topContour;
			std::vector< SSG_lineConotours> resi_bottomContour;
			std::vector< SSG_lineConotours> resi_leftContour;
			std::vector< SSG_lineConotours> resi_rightContour;
			int resi_maxEdgeId_top = 0, resi_maxEdgeId_btm = 0, resi_maxEdgeId_left = 0, resi_maxEdgeId_right = 0;
			sg_peakXYScan(
				laser3DPoints, 
				lineNum, 
				featureMask, 
				residualPeaks[ri], 
				algoParam.growParam,
				algoParam.bagParam,
				true,
				resi_topContour, 
				resi_bottomContour, 
				resi_leftContour, 
				resi_rightContour,
				&resi_maxEdgeId_top, 
				&resi_maxEdgeId_btm, 
				&resi_maxEdgeId_left, 
				&resi_maxEdgeId_right);
			//计算ROI
			//保留长宽都大于门限的
			SSG_ROIRectD objROI = { 0, -1, 0, 0 };
			for (int n = 0; n < resi_topContour.size(); n++)
			{
				std::vector<SSG_contourPtInfo>& a_line_contourPts = resi_topContour[n].contourPts;
				for(int m = 0; m < a_line_contourPts.size(); m ++)
				{
					if (objROI.right < objROI.left)
					{
						objROI.left = a_line_contourPts[m].edgePt.x;
						objROI.right = a_line_contourPts[m].edgePt.x;
						objROI.top = a_line_contourPts[m].edgePt.y;
						objROI.bottom = a_line_contourPts[m].edgePt.y;
					}
					else
					{
						objROI.left = objROI.left > a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.left;
						objROI.right = objROI.right < a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.right;
						objROI.top = objROI.top > a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.top;
						objROI.bottom = objROI.bottom < a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.bottom;
					}
				}
			}
			for (int n = 0; n < resi_bottomContour.size(); n++)
			{
				std::vector<SSG_contourPtInfo>& a_line_contourPts = resi_bottomContour[n].contourPts;
				for (int m = 0; m < a_line_contourPts.size(); m++)
				{
					if (objROI.right < objROI.left)
					{
						objROI.left = a_line_contourPts[m].edgePt.x;
						objROI.right = a_line_contourPts[m].edgePt.x;
						objROI.top = a_line_contourPts[m].edgePt.y;
						objROI.bottom = a_line_contourPts[m].edgePt.y;
					}
					else
					{
						objROI.left = objROI.left > a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.left;
						objROI.right = objROI.right < a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.right;
						objROI.top = objROI.top > a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.top;
						objROI.bottom = objROI.bottom < a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.bottom;
					}
				}
			}
			for (int n = 0; n < resi_leftContour.size(); n++)
			{
				std::vector<SSG_contourPtInfo>& a_line_contourPts = resi_leftContour[n].contourPts;
				for (int m = 0; m < a_line_contourPts.size(); m++)
				{
					if (objROI.right < objROI.left)
					{
						objROI.left = a_line_contourPts[m].edgePt.x;
						objROI.right = a_line_contourPts[m].edgePt.x;
						objROI.top = a_line_contourPts[m].edgePt.y;
						objROI.bottom = a_line_contourPts[m].edgePt.y;
					}
					else
					{
						objROI.left = objROI.left > a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.left;
						objROI.right = objROI.right < a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.right;
						objROI.top = objROI.top > a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.top;
						objROI.bottom = objROI.bottom < a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.bottom;
					}
				}
			}
			for (int n = 0; n < resi_rightContour.size(); n++)
			{
				std::vector<SSG_contourPtInfo>& a_line_contourPts = resi_rightContour[n].contourPts;
				for (int m = 0; m < a_line_contourPts.size(); m++)
				{
					if (objROI.right < objROI.left)
					{
						objROI.left = a_line_contourPts[m].edgePt.x;
						objROI.right = a_line_contourPts[m].edgePt.x;
						objROI.top = a_line_contourPts[m].edgePt.y;
						objROI.bottom = a_line_contourPts[m].edgePt.y;
					}
					else
					{
						objROI.left = objROI.left > a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.left;
						objROI.right = objROI.right < a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.right;
						objROI.top = objROI.top > a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.top;
						objROI.bottom = objROI.bottom < a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.bottom;
					}
				}
			}
			//检查ROI大小
			double obj_L = objROI.right - objROI.left;
			double obj_W = objROI.bottom - objROI.top;
			if (obj_L < obj_W)
			{
				double tmp_value = obj_W;
				obj_W = obj_L;
				obj_L = tmp_value;
			}
			if ((obj_L > algoParam.bagParam.bagL * 0.25) && (obj_W > algoParam.bagParam.bagW * 0.25))
			{
				smallObjPeaks.push_back(residualPeaks[ri]);
			}
		}
	}

#endif

	//目标排序：分层 -> 对最高层分行 -> 对最高层第一行从左到右排序
	if (peakRgns.size() > 0)
	{
		double maxHeight = peakRgns[0].centerPos.z;
		for (int i = 1; i < peakRgns.size(); i++)
		{
			if (maxHeight > peakRgns[i].centerPos.z)
				maxHeight = peakRgns[i].centerPos.z;
		}
		//取同高度层的目标
		std::vector<SSG_peakRgnInfo> level0_objs;
		for (int i = 0, i_max = (int)peakRgns.size(); i < i_max; i++)
		{
			double z_diff = peakRgns[i].centerPos.z - maxHeight;
			if (z_diff < algoParam.bagParam.bagH / 2)  //分层
			{
				level0_objs.push_back(peakRgns[i]);
			}
		}
		peakRgns.clear();
		peakRgns.insert(peakRgns.end(), level0_objs.begin(), level0_objs.end());
		int level0_size = (int)peakRgns.size();
		if (level0_size > 1)  //进一步排序，分行
		{
			//取Y最小的目标
			double minY = 0;
			double minY_idx = -1;
			for (int i = 0; i < level0_size; i++)
			{
				if (minY_idx < 0)
				{
					minY = peakRgns[i].centerPos.y;
					minY_idx = i;
				}
				else
				{
					if (minY > peakRgns[i].centerPos.y)
					{
						minY = peakRgns[i].centerPos.y;
						minY_idx = i;
					}
				}
			}
			std::vector<int> row_0_outlier;
			for (int i = 0; i < level0_size; i++)
			{
				double y_diff = peakRgns[i].centerPos.y - minY;
				if (y_diff < algoParam.bagParam.bagW / 2) //第一行
					objOps.push_back(peakRgns[i]);
				else
					row_0_outlier.push_back(i);  //将其它行的序号记录下来
			}
			//对第一行的目标按从左到右排序
			if (objOps.size() > 1)
			{
				std::sort(objOps.begin(), objOps.end(), compareByXValue);
			}
			for (int i = 0; i < row_0_outlier.size(); i++)
				objOps.push_back(peakRgns[row_0_outlier[i]]);
#if 0
			for (int i = level0_end + 1; i < peakRgns.size(); i++)
				objOps.push_back(peakRgns[i]);
#endif
		}
		else
			objOps.insert(objOps.end(), peakRgns.begin(), peakRgns.end());
	}
	//碰撞检测
	if ((objOps.size() > 0) && (smallObjPeaks.size() > 0)) 
	{
		SSG_peakRgnInfo* highest_obj = &objOps[0];
		double objZ = highest_obj->centerPos.z;
		for (int i = 0; i < smallObjPeaks.size(); i++)
		{
			SSG_2DValueI* a_samllPk = &smallObjPeaks[i];
			if (highest_obj->centerPos.z > a_samllPk->valueD + algoParam.bagParam.bagH / 2)
			{
				SVzNL3DPosition* smallPkPt = &laser3DPoints[a_samllPk->x].p3DPosition[a_samllPk->y];
				double dist = sqrt(pow(highest_obj->centerPos.x - smallPkPt->pt3D.x, 2) + pow(highest_obj->centerPos.y - smallPkPt->pt3D.y, 2));
				double dia_angle = sqrt(pow(highest_obj->objSize.dWidth, 2) + pow(highest_obj->objSize.dHeight, 2));
				//同层比较
				double z_diff = smallPkPt->pt3D.z - objZ;
				if (z_diff < algoParam.bagParam.bagH / 2)  //分层
				{
					if (dist < dia_angle / 2)
						objOps.clear(); //本次检测无效
				}
			}
		}
	}
	//将数据重新投射回原来的坐标系，以保持手眼标定结果正确
	for (int i = 0; i < lineNum; i++)
		sg_lineDataR(&laser3DPoints[i], poseCalibPara.invRMatrix, -1);
	//将检测结果重新投射回原来的坐标系
	double invMatrix[3][3];
	invMatrix[0][0] = poseCalibPara.invRMatrix[0];
	invMatrix[0][1] = poseCalibPara.invRMatrix[1];
	invMatrix[0][2] = poseCalibPara.invRMatrix[2];
	invMatrix[1][0] = poseCalibPara.invRMatrix[3];
	invMatrix[1][1] = poseCalibPara.invRMatrix[4];
	invMatrix[1][2] = poseCalibPara.invRMatrix[5];
	invMatrix[2][0] = poseCalibPara.invRMatrix[6];
	invMatrix[2][1] = poseCalibPara.invRMatrix[7];
	invMatrix[2][2] = poseCalibPara.invRMatrix[8];
	for (int i = 0, i_max = (int)objOps.size(); i < i_max; i++)
	{
		SSG_EulerAngles euAngle = { objOps[i].centerPos.x_roll, objOps[i].centerPos.y_pitch, objOps[i].centerPos.z_yaw};
		double pose[3][3];
		eulerToRotationMatrixZYX(euAngle, pose);
		double resultMatrix[3][3];
		for (int i = 0; i < 3; i++)
		{
			for (int j = 0; j < 3; j++)
			{
				resultMatrix[i][j] = 0;
				for (int m = 0; m < 3; m++)
					resultMatrix[i][j] += invMatrix[i][m] * pose[m][j];
			}
		}
		SSG_EulerAngles resultEuAngle = rotationMatrixToEulerZYX(resultMatrix);
		objOps[i].centerPos.z_yaw = resultEuAngle.yaw;
		double x =  objOps[i].centerPos.x * poseCalibPara.invRMatrix[0] + 
					objOps[i].centerPos.y * poseCalibPara.invRMatrix[1] + 
					objOps[i].centerPos.z * poseCalibPara.invRMatrix[2];
		double y =  objOps[i].centerPos.x * poseCalibPara.invRMatrix[3] + 
					objOps[i].centerPos.y * poseCalibPara.invRMatrix[4] + 
					objOps[i].centerPos.z * poseCalibPara.invRMatrix[5];
		double z =  objOps[i].centerPos.x * poseCalibPara.invRMatrix[6] + 
					objOps[i].centerPos.y * poseCalibPara.invRMatrix[7] + 
					objOps[i].centerPos.z * poseCalibPara.invRMatrix[8];
		objOps[i].centerPos.x = x;
		objOps[i].centerPos.y = y;
		objOps[i].centerPos.z = z;
	}
}

typedef struct
{
	int vldNum[RGN_HIST_SIZE];
	int totalNum[RGN_HIST_SIZE];
}_rgnHSVHistInfo;

HSV RGBtoHSV(int r, int g, int b) {
	double r_ = r / 255.0;
	double g_ = g / 255.0;
	double b_ = b / 255.0;
	double max_ = std::max(r_, std::max(g_, b_));
	double min_ = std::min(r_, std::min(g_, b_));
	double delta = max_ - min_;
	double h, s, v;
	v = max_;
	s = (max_ == 0) ? 0 : (delta / max_);
	if (delta == 0) //此处有修改。设定一个门限
	{
		h = -1; // 未定义，可以是任何值，通常设为0或NaN
	}
	else 
	{
		if (r_ == max_) {
			h = (g_ - b_) / delta;
		}
		else if (g_ == max_) {
			h = 2 + (b_ - r_) / delta;
		}
		else { // b_ == max_
			h = 4 + (r_ - g_) / delta;
		}
		h *= 60; // 将h的值标准化到[0,360]度
		if (h < 0) {
			h += 360; // 确保h在[0,360]范围内
		}
	}
	HSV hsv;
	hsv.h = h;
	hsv.s = s * 255;// 将s的值标准化到[0,255]
	hsv.v = v * 255;// 将v的值标准化到[0,255]
	return hsv;
}

bool isSameColor(HSV hsvPattern, HSV hsvPt, const SSG_hsvCmpParam colorCmpParam)
{
	if (((hsvPattern.h < 0) && (hsvPt.h >= 0)) || ((hsvPattern.h >= 0) && (hsvPt.h < 0)))
		return false;

	if ((hsvPattern.h < 0) && (hsvPt.h < 0))
		return true;

	double h_diff = hsvPt.h - hsvPt.h;
	if (h_diff < -180)
		h_diff = h_diff + 360;
	else if (h_diff > 180)
		h_diff = h_diff - 360;
	h_diff = abs(h_diff);
	double s_diff = abs(hsvPattern.s - hsvPt.s);
	if ( (h_diff < colorCmpParam.hueTh) && (s_diff < colorCmpParam.saturateTh))
		return true;
	else
		return false;
}

double cosSimilarity(const double* colorTemplate, double* sampleData)
{
	double sumA = 0, sumB = 0, sumAB = 0;
	for (int i = 0; i < RGN_HIST_SIZE; i++)
	{
		sumA += colorTemplate[i] * colorTemplate[i];
		sumAB += colorTemplate[i] * sampleData[i];
		sumB = sampleData[i] * sampleData[i];
	}
	sumA = sqrt(sumA);
	sumB = sqrt(sumB);
	sumA = sumA * sumB;
	if (sumA <= 1e-4)
		return 0;
	return (sumAB / sumA);
}

double cosSimilarityInv(const double* colorTemplate, double* sampleData)
{
	double sumA = 0, sumB = 0, sumAB = 0;
	for (int i = 0; i < RGN_HIST_SIZE; i++)
	{
		sumA += colorTemplate[i] * colorTemplate[i];
		sumAB += colorTemplate[i] * sampleData[RGN_HIST_SIZE - 1 - i];
		sumB = sampleData[RGN_HIST_SIZE - 1 - i] * sampleData[RGN_HIST_SIZE -1 - i];
	}
	sumA = sqrt(sumA);
	sumB = sqrt(sumB);
	sumA = sumA * sumB;
	if (sumA <= 1e-4)
		return 0;
	return (sumAB / sumA);
}

///数据输入必须是RGBD grid格式，以进行水平方向和垂直方向的处理
///（1）寻找边界点
///（2）从最高点开始进行区域生长
///  输出编织袋及袋子上下朝向
void sg_getBagPositionAndOrientation(
	SVzNLXYZRGBDLaserLine* laser3DPoints,
	int lineNum,
	//std::vector<SSG_lineFeature>& all_vLineFeatures, 
	//std::vector<std::vector<int>>& noisePts, 
	const SG_bagPositionParam algoParam,
	const SSG_planeCalibPara poseCalibPara,
	const SSG_hsvCmpParam colorCmpParam,
	const RGB rgbColorPattern,  //用于区分袋子方向的颜色
	const double frontColorTemplate[RGN_HIST_SIZE],
	const double backColorTemplate[RGN_HIST_SIZE],
	std::vector<SSG_peakOrienRgnInfo>& objOps,
#if OUTPUT_DEBUG
	std::vector<std::vector< SVzNL3DPosition>>& bagPositionCloudPts,
#endif
	int* errCode)
{
	*errCode = 0;
	if (lineNum == 0)
	{
		*errCode = SG_ERR_3D_DATA_INVLD;
		return;
	}
	//检查是否为栅格格式
	//将RGBD点云分离出对应的RGB和点云，RGB转成HSV
	std::vector<SVzNL3DLaserLine> cloudPts;
	std::vector<std::vector<HSV>> hsvPts;
	bool dataValid = true;
	int linePtNum = laser3DPoints[0].nPointCnt;
	for (int line = 0; line < lineNum; line++)
	{
		if (laser3DPoints[line].nPointCnt > 0)
		{
			std::vector<HSV> line_hsv;
			line_hsv.resize(laser3DPoints[line].nPointCnt);
			SVzNL3DPosition* p3DPoint = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * laser3DPoints[line].nPointCnt);
			memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * laser3DPoints[line].nPointCnt);
			for (int i = 0; i < laser3DPoints[line].nPointCnt; i++)
			{
				if ((line == 497) && (i >= 902))
					int kkk = 1;
				p3DPoint[i].pt3D.x = laser3DPoints[line].p3DPoint[i].x;
				p3DPoint[i].pt3D.y = laser3DPoints[line].p3DPoint[i].y;
				p3DPoint[i].pt3D.z = laser3DPoints[line].p3DPoint[i].z;
				p3DPoint[i].nPointIdx = i;

				unsigned int nRGB = laser3DPoints[line].p3DPoint[i].nRGB;

				int r = nRGB & 0xff;
				nRGB >>= 8;
				int g = nRGB & 0xff;
				nRGB >>= 8;
				int b = nRGB & 0xff;
				line_hsv[i] = RGBtoHSV(r,g,b);
			}

			SVzNL3DLaserLine a_line;
			a_line.nPositionCnt = laser3DPoints[line].nPointCnt;
			a_line.nTimeStamp = 0;
			a_line.p3DPosition = p3DPoint;
			cloudPts.push_back(a_line);
			hsvPts.push_back(line_hsv);
		}
		if (linePtNum != laser3DPoints[line].nPointCnt)
			dataValid = false;
	}

	if (false == dataValid)
	{
		*errCode = SG_ERR_3D_DATA_INVLD;
		//释放内存
		for (int i = 0, i_max = (int)cloudPts.size(); i < i_max; i++)
			free(cloudPts[i].p3DPosition);
		return;
	}

	//根据目标
	std::vector<SSG_peakRgnInfo> peakRgn;
	sg_getBagPosition(
		&cloudPts[0],
		(int)cloudPts.size(),
		algoParam,
		poseCalibPara,
		peakRgn);

	//释放内存
	for (int i = 0, i_max = (int)cloudPts.size(); i < i_max; i++)
	{
#if OUTPUT_DEBUG
		std::vector< SVzNL3DPosition> out_line;
		int nPtCounter = cloudPts[i].nPositionCnt;
		for (int j = 0; j < nPtCounter; j++)
			out_line.push_back(cloudPts[i].p3DPosition[j]);
		bagPositionCloudPts.push_back(out_line);
#endif
		free(cloudPts[i].p3DPosition);
		cloudPts[i].p3DPosition = NULL;
	}

	//将数据重新投射回原来的坐标系，以保持手眼标定结果正确
	for (int i = 0; i < lineNum; i++)
		sg_lineDataR_RGBD(&laser3DPoints[i], poseCalibPara.invRMatrix, -1);

	//将颜色属性转成HSV
	HSV hsvPattern = RGBtoHSV(rgbColorPattern.r, rgbColorPattern.g, rgbColorPattern.b);
	//进行颜色统计处理：取centerPos为中心的（0.8*Width, 0.8*Height）的范围内的点进行统计
	int objNum = (int)peakRgn.size();
	if (objNum > 0)
	{
		//以水平放置的矩阵为标准模型位置，即水平方向为长度，垂直方向为宽度
		std::vector<double> y_th;
		y_th.resize(objNum);
		std::vector<double> x_th;
		x_th.resize(objNum);
		std::vector<double> x_slice;
		x_slice.resize(objNum);
		for (int m = 0; m < objNum; m++)
		{
			y_th[m] = peakRgn[m].objSize.dHeight * 0.25;
			x_th[m] = peakRgn[m].objSize.dWidth * 0.45;
			x_slice[m] = peakRgn[m].objSize.dWidth / 16.0;
		}

		std::vector<SSG_ROIRectD> rgnROIs;
		rgnROIs.resize(objNum);
		for (int i = 0; i < objNum; i++)
		{
			//计算一个大的ROI，加速处理
			//以水平放置的矩阵为标准模型位置
			SVzNL2DPointD vec[4];
			vec[0].x = -peakRgn[i].objSize.dWidth / 2;
			vec[0].y = -peakRgn[i].objSize.dHeight / 2;
			vec[1].x =  peakRgn[i].objSize.dWidth / 2;
			vec[1].y = -peakRgn[i].objSize.dHeight / 2;
			vec[2].x =  peakRgn[i].objSize.dWidth / 2;
			vec[2].y =  peakRgn[i].objSize.dHeight / 2;
			vec[3].x = -peakRgn[i].objSize.dWidth / 2;
			vec[3].y =  peakRgn[i].objSize.dHeight / 2;
			//旋转
			const double deg2rad = PI / 180.0;
			const double yaw = peakRgn[i].centerPos.z_yaw * deg2rad;
			double cy = cos(yaw); double sy = sin(yaw);
			for (int m = 0; m < 4; m++)
			{
				double x = vec[m].x * cy + vec[m].y * sy;  //图像坐标系的Y与欧氏坐标系方向相反，
				double y = -vec[m].x * sy + vec[m].y * cy;
				vec[m].x = x + peakRgn[i].centerPos.x;
				vec[m].y = y + peakRgn[i].centerPos.y;
			}
			//生成ROI
			SSG_ROIRectD a_roi;
			a_roi.left = vec[0].x;
			a_roi.right = vec[0].x;
			a_roi.top = vec[0].y;
			a_roi.bottom = vec[0].y;
			for (int m = 1; m < 4; m++)
			{
				a_roi.left =	a_roi.left   > vec[m].x ? vec[m].x : a_roi.left;
				a_roi.right =	a_roi.right  < vec[m].x ? vec[m].x : a_roi.right;
				a_roi.top =		a_roi.top    > vec[m].y ? vec[m].y : a_roi.top;
				a_roi.bottom =	a_roi.bottom < vec[m].y ? vec[m].y : a_roi.bottom;
			}
			rgnROIs[i] = a_roi;
		}

		std::vector<_rgnHSVHistInfo> rgnHists; //统计量
		rgnHists.resize(objNum);
		//统计量清零
		for (int m = 0; m < objNum; m++)
		{
			for (int j = 0; j < 16; j++)
			{
				rgnHists[m].totalNum[j] = 0;
				rgnHists[m].vldNum[j] = 0;
			}
		}

		//对所有点进投射，注意此处使用原始点
		for (int line = 0; line < lineNum; line++)
		{
			for (int j = 0; j < linePtNum; j++)
			{
				SVzNLPointXYZRGBA* a_pt = &laser3DPoints[line].p3DPoint[j];
				if (a_pt->z < 1e-4)
					continue;
				if ((line == 497) && (j >= 902))
				{
					HSV a_hsv = hsvPts[line][j];
					int kkk = 1;
				}
				for (int m = 0; m < objNum; m++)
				{
					//检查点是否在ROI内
					if ((a_pt->x >= rgnROIs[m].left) && (a_pt->x <= rgnROIs[m].right) &&
						(a_pt->y >= rgnROIs[m].top) && (a_pt->y <= rgnROIs[m].bottom))
					{
						//计算投射点
						//反向旋转,逆时针角度
						const double deg2rad = PI / 180.0;
						double rotateAngle = - peakRgn[m].centerPos.z_yaw * deg2rad;
						double cy = cos(rotateAngle); double sy = sin(rotateAngle);
						double x = a_pt->x - peakRgn[m].centerPos.x;
						double y = a_pt->y - peakRgn[m].centerPos.y;
						double rx = x * cy + y * sy;
						double ry = -x * sy + y * cy;
						if ((rx >= -x_th[m]) && (rx <= x_th[m]) && (ry >= -y_th[m]) && (ry <= y_th[m]))
						{
							if (m == 0)
								int kkk = 1;
							else if (m == 1)
								int kkk = 1;
							else if (m == 2)
								int kkk = 1;
							else if (m == 3)
								int kkk = 1;
							double rx_abs = abs(rx);
							int hist_idx = (int)(rx_abs / x_slice[m]);
							if (hist_idx >= RGN_HIST_SIZE / 2)
								hist_idx = RGN_HIST_SIZE / 2 - 1;
							if (rx < 0)
								hist_idx = -hist_idx + RGN_HIST_SIZE / 2 - 1;
							else
								hist_idx = hist_idx + RGN_HIST_SIZE / 2;
							//判断属性
							bool isSame = isSameColor(hsvPattern, hsvPts[line][j], colorCmpParam);
							if (true == isSame)
								rgnHists[m].vldNum[hist_idx] ++;
							rgnHists[m].totalNum[hist_idx] ++;
						}
					}
				}
			}
		}


		for (int m = 0; m < objNum; m++)
		{
			double normalizeHist[RGN_HIST_SIZE];
			//生成归一化数据
			int totalNum=0, totalVldNum=0, upVldNum=0, dwnVldNum=0;
			for (int j = 0; j < RGN_HIST_SIZE; j++)
			{
				totalNum += rgnHists[m].totalNum[j];
				totalVldNum += rgnHists[m].vldNum[j];

				if (j < (RGN_HIST_SIZE / 2-2))
					dwnVldNum += rgnHists[m].vldNum[j];
				else if (j >= (RGN_HIST_SIZE / 2+2))
					upVldNum += rgnHists[m].vldNum[j];

				if (rgnHists[m].totalNum[j] > 0)
					normalizeHist[j] = (double)rgnHists[m].vldNum[j] / (double)rgnHists[m].totalNum[j];
				else
					normalizeHist[j] = 0;
			}

			SSG_peakOrienRgnInfo a_obj;
			a_obj.centerPos = peakRgn[m].centerPos;
			a_obj.objSize = peakRgn[m].objSize;
			a_obj.pkRgnIdx = peakRgn[m].pkRgnIdx;
			a_obj.pos2D = peakRgn[m].pos2D;
			a_obj.orienFlag = 0;  //未知正反
			//相似度比较
#if 0
			double frontSim = cosSimilarity(frontColorTemplate, normalizeHist);
			double frontSim_inv = cosSimilarityInv(frontColorTemplate, normalizeHist);
			double backSim = cosSimilarity(backColorTemplate, normalizeHist);
			double backSim_inv = cosSimilarityInv(backColorTemplate, normalizeHist);
			if ((frontSim > frontSim_inv) && (frontSim > backSim) && (frontSim > backSim_inv) && (frontSim > 0.5))
			{
				a_obj.orienFlag = 1;
			}
			else if ((frontSim_inv > frontSim) && (frontSim_inv > backSim) && (frontSim_inv > backSim_inv) && (frontSim_inv > 0.5))
			{
				a_obj.orienFlag = 1;
				a_obj.centerPos.z_yaw += 180;
				if (a_obj.centerPos.z_yaw >= 360)
					a_obj.centerPos.z_yaw = a_obj.centerPos.z_yaw - 360;
			}
			else if ((backSim > frontSim) && (backSim > frontSim_inv) && (backSim > backSim_inv) && (backSim > 0.5))
			{
				a_obj.orienFlag = 2;
			}
			else if ((backSim_inv > frontSim) && (backSim_inv > frontSim_inv) && (backSim_inv > backSim) && (backSim_inv > 0.5))
			{
				a_obj.orienFlag = 2;
				a_obj.centerPos.z_yaw += 180;
				if (a_obj.centerPos.z_yaw >= 360)
					a_obj.centerPos.z_yaw = a_obj.centerPos.z_yaw - 360;
			}
#else
			double vldPtRatio = (double)totalVldNum / (double)totalNum;
			bool isFront;
			if (true == colorCmpParam.frontVldPtGreater)
				isFront = vldPtRatio > colorCmpParam.FBVldPtRatioTh ? true : false;
			else
				isFront = vldPtRatio > colorCmpParam.FBVldPtRatioTh ? false : true;

			bool orienIsNormal;
			if (true == isFront)
			{
				if (true == colorCmpParam.front_upVldPtGreater)
					orienIsNormal = upVldNum > dwnVldNum ? true : false;
				else
					orienIsNormal = upVldNum > dwnVldNum ? false : true;
			}
			else
			{
				if (true == colorCmpParam.back_upVldPtGreater)
					orienIsNormal = upVldNum > dwnVldNum ? true : false;
				else
					orienIsNormal = upVldNum > dwnVldNum ? false : true;
			}

			if (true == isFront)
				a_obj.orienFlag = 1;
			else
				a_obj.orienFlag = 2;

			if (false == orienIsNormal)
			{
				a_obj.centerPos.z_yaw += 180;
				if (a_obj.centerPos.z_yaw >= 360)
					a_obj.centerPos.z_yaw = a_obj.centerPos.z_yaw - 360;
			}
#endif
			objOps.push_back(a_obj);
		}
	}

}

bool compareByXPos(const SSG_featureTree& a, const SSG_featureTree& b) {
	return (a.roi.left + a.roi.right) < (b.roi.left + b.roi.right);
}
void sg_sideBagPosition(
	SVzNL3DLaserLine* laser3DPoints,
	int lineNum,
	const SG_bagPositionParam algoParam,
	std::vector<SSG_sideBagInfo>& objOps)
{
	int hLineNum = laser3DPoints->nPositionCnt;  //Grid格式，所有扫描线的点数是一样的
	//生成水平扫描数据
	std::vector<std::vector<SVzNL3DPosition>> hLines;
	hLines.resize(hLineNum);
	for (int i = 0; i < hLineNum; i++)
		hLines[i].resize(lineNum);

	//生成水平扫描数据，同时计算点距均值
	double pntDist = 0.0;
	int nDistNum = 0;
	for (int line = 0; line < lineNum; line++)
	{
		SVzNL3DPosition  pre_pt = { 0, {0, 0, 0} };
		for (int j = 0; j < hLineNum; j++)
		{
			SVzNL3DPosition a_pt = laser3DPoints[line].p3DPosition[j];
			a_pt.pt3D.x = laser3DPoints[line].p3DPosition[j].pt3D.y;
			a_pt.pt3D.y = laser3DPoints[line].p3DPosition[j].pt3D.x;
			hLines[j][line] = a_pt;
			if (j > 0)
			{
				if ((a_pt.pt3D.z > 1e-4) && (pre_pt.pt3D.z > 1e-4))
				{
					double pt_dist = sqrt(pow(a_pt.pt3D.x - pre_pt.pt3D.x, 2) +
										pow(a_pt.pt3D.y - pre_pt.pt3D.y, 2) +
										pow(a_pt.pt3D.z - pre_pt.pt3D.z, 2));
					pntDist += pt_dist;
					nDistNum++;
				}
			}
			pre_pt = a_pt;
		}
	}



	SSG_outlierFilterParam filterParam = algoParam.filterParam;
	if (nDistNum > 0)
		filterParam.continuityTh = filterParam.continuityTh * (pntDist / (double)nDistNum);

	std::vector<std::vector< SSG_basicFeature1D>> all_linePkTop;
	std::vector<std::vector< SSG_basicFeature1D>> all_linePkBtm;
	std::vector<std::vector<int>> all_hLineNoises;
	//逐行提取特征
	for (int hLine = 0; hLine < hLineNum; hLine++)
	{
		if (hLine == 114)
			int kkk = 1;
		//滤除噪声
		//1、按距离分为一个个连续的段：距离门限
		//2、将点数小于门限的段视为噪声段滤除：噪声门限
		std::vector<SVzNL3DPosition> filterData;
		std::vector<int> lineNoise;
		sg_lineDataRemoveOutlier_ptDistMethod((SVzNL3DPosition*)hLines[hLine].data(), (int)hLines[hLine].size(), filterParam, filterData, lineNoise);
		all_hLineNoises.push_back(lineNoise);

		SSG_lineFeature a_hLine_featrues;
		a_hLine_featrues.lineIdx = hLine;
		//逐行提取最大值点和最小值点
		std::vector< SSG_basicFeature1D> localPkBtm;
		std::vector< SSG_basicFeature1D> localPkTop;
		sg_getLineLocalPeaks(
			filterData.data(),
			(int)filterData.size(),
			hLine,
			(algoParam.bagParam.bagH / 2),
			localPkBtm,
			localPkTop);
		
		//TBD：过滤虚假层间点
		
		//记录行特征
		all_linePkTop.push_back(localPkTop);//空行也加入，保证能按行号索引
		all_linePkBtm.push_back(localPkBtm);//空行也加入，保证能按行号索引
	}

	//噪声去除
	SSG_ROIRectD globalROI = { 0, -1, 0, 0 };
	for (int i = 0; i < all_hLineNoises.size(); i++)
	{
		std::vector<int>& lineNoise = all_hLineNoises[i];
		for (int j = 0; j < lineNoise.size(); j++)
		{
			int lineIdx = lineNoise[j];
			_updateObjROI(&globalROI, &laser3DPoints[lineIdx].p3DPosition[i]);
			laser3DPoints[lineIdx].p3DPosition[i].pt3D.z = 0;
		}
	}

	/// 统计整个视野大小，在滤噪之后进行
	SVzNL3DRangeD roi3D = sg_getScanDataROI(
		laser3DPoints,
		lineNum);
	SVzNLRangeD x_range = roi3D.xRange;
	SVzNLRangeD y_range = roi3D.yRange;

	//水平方向特征生长
	std::vector<SSG_featureTree> h_trees_top_all;
	std::vector<SSG_featureTree> h_trees_bottom;
	sg_peakFeatureGrowing(
		all_linePkTop,
		h_trees_top_all,
		algoParam.growParam);

	sg_peakFeatureGrowing(
		all_linePkBtm,
		h_trees_bottom,
		algoParam.growParam);

	//对top进行过滤
	std::vector<SSG_featureTree> h_trees_top;
	for (int i = 0, i_max = (int)h_trees_top_all.size(); i < i_max; i++)
	{
		double w = h_trees_top_all[i].roi.right - h_trees_top_all[i].roi.left;
		if (w > algoParam.bagParam.bagW / 2)
			h_trees_top.push_back(h_trees_top_all[i]);
	}

	if (0 == h_trees_top.size())
		return;

	//确定顶层位置
	double topLayer_height = std::numeric_limits<double>::max();  //一个极大值
	for (int i = 0, i_max = (int)h_trees_top.size(); i < i_max; i++)
	{
		double layer_h = (h_trees_top[i].roi.top + h_trees_top[i].roi.bottom) / 2;
		if (topLayer_height > layer_h)
			topLayer_height = layer_h;
	}

	///判断层间
	///寻找第一个位置最高的Tree，这个是编织袋的突出部，为一个纺织袋的中间
	std::vector<SSG_featureTree> hTree_top_first;
	for (int i = 0, i_max = (int)h_trees_top.size(); i < i_max; i++)
	{
		double layer_h = (h_trees_top[i].roi.top + h_trees_top[i].roi.bottom) / 2;
		double layer_h_diff = layer_h - topLayer_height;
		if(layer_h_diff < algoParam.bagParam.bagH / 2)
			hTree_top_first.push_back(h_trees_top[i]);
	}
	//没有找到顶层目标
	if (hTree_top_first.size() == 0)
		return;

	//按照从左到右排序
	std::sort(hTree_top_first.begin(), hTree_top_first.end(), compareByXPos);
	//寻找与topLayer_height最近的bottom
	std::vector<SSG_featureTree> hTree_btm_first;
	for (int i = 0, i_max = (int)h_trees_bottom.size(); i < i_max; i++)
	{
		double layer_h = (h_trees_bottom[i].roi.top + h_trees_bottom[i].roi.bottom) / 2;
		double layer_h_diff = layer_h - topLayer_height;
		if ( (layer_h_diff < algoParam.bagParam.bagH) && (layer_h > topLayer_height) )
			hTree_btm_first.push_back(h_trees_bottom[i]);
	}
	
	//标注
	int treeId = 1000;
	for (int i = 0, i_max = (int)hTree_top_first.size(); i < i_max; i++)
	{
		treeId++;
		SSG_featureTree& a_tree = hTree_top_first[i];
		for (int j = 0, j_max = (int)a_tree.treeNodes.size(); j < j_max; j++)
		{
			int lineIdx = a_tree.treeNodes[j].jumpPos2D.y;
			int ptIdx = a_tree.treeNodes[j].jumpPos2D.x;
			laser3DPoints[lineIdx].p3DPosition[ptIdx].nPointIdx = treeId << 16;
		}
	}
#if 1
	treeId = 2000;
	for (int i = 0, i_max = (int)hTree_btm_first.size(); i < i_max; i++)
	{
		treeId++;
		SSG_featureTree& a_tree = hTree_btm_first[i];
		for (int j = 0, j_max = (int)a_tree.treeNodes.size(); j < j_max; j++)
		{
			int lineIdx = a_tree.treeNodes[j].jumpPos2D.y;
			int ptIdx = a_tree.treeNodes[j].jumpPos2D.x;
			laser3DPoints[lineIdx].p3DPosition[ptIdx].nPointIdx = treeId << 16;
		}
	}
#endif
	//生成目标数据
	int objId = 1;
	for (int i = 0, i_max = (int)hTree_top_first.size(); i < i_max; i++)
	{
		SSG_featureTree& a_tree = hTree_top_first[i];
		double center_x = (a_tree.roi.left + a_tree.roi.right) / 2;
		std::vector< SVzNL3DPoint> pkPts;
		SVzNL3DPoint objPt = { 0,0,0 };  //与center_x最近的点作为目标点。
		for (int j = 0, j_max = (int)a_tree.treeNodes.size(); j < j_max; j++)
		{
			SVzNL3DPoint a_pt = a_tree.treeNodes[j].jumpPos;
			//a_pt.y = a_tree.treeNodes[j].jumpPos.x;
			//a_pt.x = a_tree.treeNodes[j].jumpPos.y;
			//a_pt.z = a_tree.treeNodes[j].jumpPos.z;
			pkPts.push_back(a_pt);
			//寻找与center_x最近的点。
			if (objPt.z < 1e-4)
			{
				objPt = a_tree.treeNodes[j].jumpPos;
			}
			else
			{
				double old_dist = abs(objPt.x - center_x);
				double curr_dist = abs(a_tree.treeNodes[j].jumpPos.x - center_x);
				if(old_dist > curr_dist)
					objPt = a_tree.treeNodes[j].jumpPos;
			}
		}
		//计算倾角
		double k = 0.0, b = 0.0;
		lineFitting(pkPts, &k, &b);
		double angle = 90 - atan(k) * 180 / PI;

		//计算抓取点
		SVzNL3DPoint pre_pt = { 0,0,0 };
		SVzNL3DPoint post_pt = { 0,0,0 };
		for (int m = 0, m_max = (int)hTree_btm_first.size(); m < m_max; m++)
		{
			SSG_featureTree& a_btm_tree = hTree_btm_first[m];
			int node_size = (int)a_btm_tree.treeNodes.size();
			if (node_size == 0)
				continue;

			if (a_btm_tree.roi.right < center_x)
			{
				if (pre_pt.z < 1e-4)
					pre_pt = a_btm_tree.treeNodes[node_size - 1].jumpPos;
				else if(pre_pt.x < a_btm_tree.treeNodes[node_size - 1].jumpPos.x)
					pre_pt = a_btm_tree.treeNodes[node_size - 1].jumpPos;
			}
			else if (a_btm_tree.roi.left > center_x)
			{
				if (post_pt.z < 1e-4)
					post_pt = a_btm_tree.treeNodes[0].jumpPos;
				else if (post_pt.x > a_btm_tree.treeNodes[0].jumpPos.x)
					post_pt = a_btm_tree.treeNodes[0].jumpPos;
			}
			else
			{
				for (int n = 0, n_max = (int)a_btm_tree.treeNodes.size(); n < n_max; n++)
				{
					SSG_basicFeature1D& a_node = a_btm_tree.treeNodes[n];
					if (a_node.jumpPos.x <= center_x)
					{
						if (pre_pt.z < 1e-4)
							pre_pt = a_node.jumpPos;
						else if (pre_pt.x < a_node.jumpPos.x)
							pre_pt = a_node.jumpPos;
					}
					else
					{
						if (post_pt.z < 1e-4)
							post_pt = a_node.jumpPos;
						else if (post_pt.x > a_node.jumpPos.x)
							post_pt = a_node.jumpPos;
					}
				}

			}
		}
		if ((pre_pt.z < 1e-4) || (post_pt.z < 1e-4))
			continue;

		//生成结果
		SSG_sideBagInfo a_sideBagInfo;
		a_sideBagInfo.objIdx = objId;
		objId++;
		a_sideBagInfo.objROI.top = a_tree.roi.left;
		a_sideBagInfo.objROI.bottom = a_tree.roi.right;
		a_sideBagInfo.objROI.left = a_tree.roi.top;
		a_sideBagInfo.objROI.right = a_tree.roi.bottom;
		//根据扫描到是否为长边判断是否为合格目标
		double L = a_tree.roi.right - a_tree.roi.left;
		if (L > (algoParam.bagParam.bagW + algoParam.bagParam.bagL) / 2)
			a_sideBagInfo.isValid = false;
		else
			a_sideBagInfo.isValid = true;
		a_sideBagInfo.objPos.x = objPt.y;
		a_sideBagInfo.objPos.y = objPt.x;
		a_sideBagInfo.objPos.z = objPt.z;
		//计算抓取点
		if (post_pt.x == pre_pt.x)
		{
			a_sideBagInfo.graspPos.y = (pre_pt.x + post_pt.x) / 2;
			a_sideBagInfo.graspPos.x = (pre_pt.y + post_pt.y) / 2;
			a_sideBagInfo.graspPos.z = (pre_pt.z + post_pt.z) / 2;
		}
		else
		{
			double k1 = (post_pt.x - center_x) / (post_pt.x - pre_pt.x);
			double k2 = 1.0 - k1;
			a_sideBagInfo.graspPos.y = pre_pt.x * k1 + post_pt.x * k2;
			a_sideBagInfo.graspPos.x = pre_pt.y * k1 + post_pt.y * k2;
			a_sideBagInfo.graspPos.z = pre_pt.z * k1 + post_pt.z * k2;
		}
		a_sideBagInfo.graspPos.z = (a_sideBagInfo.graspPos.z + objPt.z ) / 2;
		a_sideBagInfo.graspPos.x_roll = 0;
		a_sideBagInfo.graspPos.y_pitch = 0;
		a_sideBagInfo.graspPos.z_yaw = angle;

		objOps.push_back(a_sideBagInfo);
	}
	return;
}

//寻找侧扫时垛的基座（托盘）位置，用于确定整个垛的旋转角度
void sg_getSideBagStackBasePosition(
	SVzNL3DLaserLine* laser3DPoints,
	int lineNum,
	const SSG_stackBaseParam stackBaseParam,
	const SSG_treeGrowParam growParam,
	SSG_6DOF* stackBasePosition  //垛的托盘的中心位置和角度
)
{
	int hLineNum = laser3DPoints->nPositionCnt;  //Grid格式，所有扫描线的点数是一样的
	//生成水平扫描数据
	std::vector<std::vector<SVzNL3DPosition>> hLines;
	hLines.resize(hLineNum);
	for (int i = 0; i < hLineNum; i++)
		hLines[i].resize(lineNum);

	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < hLineNum; j++)
		{
			hLines[j][line] = laser3DPoints[line].p3DPosition[j];
			hLines[j][line].pt3D.x = laser3DPoints[line].p3DPosition[j].pt3D.y;
			hLines[j][line].pt3D.y = laser3DPoints[line].p3DPosition[j].pt3D.x;
		}
	}

	//逐条扫描线搜索z跳变特征
	std::vector<std::vector< SSG_basicFeature1D>> all_lineJumps;
	//逐行提取特征
	for (int hLine = 0; hLine < hLineNum; hLine++)
	{
		if (hLine == 761)
			int kkk = 1;

		SSG_lineFeature a_hLine_featrues;
		a_hLine_featrues.lineIdx = hLine;
		//逐行提取跳变点
		std::vector< SSG_basicFeature1D> jumps;
		sg_getLineDownJumps(
			hLines[hLine].data(),
			(int)hLines[hLine].size(),
			hLine,
			stackBaseParam.jumpTh,
			jumps);

		//记录行特征
		all_lineJumps.push_back(jumps);//空行也加入，保证能按行号索引
	}

	/// 统计整个视野大小，在滤噪之后进行
	SVzNL3DRangeD roi3D = sg_getScanDataROI(
		laser3DPoints,
		lineNum);
	SVzNLRangeD x_range = roi3D.xRange;
	SVzNLRangeD y_range = roi3D.yRange;

	//水平方向特征生长
	std::vector<SSG_featureTree> h_trees_jumps;
	sg_peakFeatureGrowing(
		all_lineJumps,
		h_trees_jumps,
		growParam);

#if 1
	if ( h_trees_jumps.size() < 2 )
		return;

	//根据长度进行进行配对
	std::vector<SSG_intPair> basePairs;
	std::vector<double> basePairYDist;
	for (int i = 0, i_max = (int)h_trees_jumps.size(); i < i_max; i++)
	{
		SSG_featureTree& tree_1 = h_trees_jumps[i];
		int nodeSize = (int)tree_1.treeNodes.size();
		if(nodeSize <2)
			continue;
		SSG_basicFeature1D& node1_L = tree_1.treeNodes[0];
		SSG_basicFeature1D& node1_R = tree_1.treeNodes[nodeSize - 1];
		double len1 = sqrt(pow(node1_R.jumpPos.x - node1_L.jumpPos.x, 2) + pow(node1_R.jumpPos.y - node1_L.jumpPos.y, 2));
		if ((len1 < stackBaseParam.baseHoleLen * 0.8) || (len1 > stackBaseParam.baseHoleLen * 1.2))
			continue;

		for (int j = i + 1; j < i_max; j++)
		{
			SSG_featureTree& tree_2 = h_trees_jumps[j];
			int nodeSize_2 = (int)tree_2.treeNodes.size();
			if (nodeSize_2 < 2)
				continue;
			SSG_basicFeature1D& node2_L = tree_2.treeNodes[0];
			SSG_basicFeature1D& node2_R = tree_2.treeNodes[nodeSize_2 - 1];
			double len2 = sqrt(pow(node2_R.jumpPos.x - node2_L.jumpPos.x, 2) + pow(node2_R.jumpPos.y - node2_L.jumpPos.y, 2));
			if ((len2 < stackBaseParam.baseHoleLen * 0.8) || (len2 > stackBaseParam.baseHoleLen * 1.2))
				continue;

			//不重叠
			if ((node1_R.jumpPos.x > node2_L.jumpPos.x) && (node2_R.jumpPos.x > node1_L.jumpPos.x))
				continue;

			if (node1_L.jumpPos.x < node2_L.jumpPos.x) //node1在node2的左边
			{
				double dist = abs(node1_R.jumpPos.x - node2_L.jumpPos.x);
				double yDist = abs(node1_R.jumpPos.y - node2_L.jumpPos.y);
				if ((dist > stackBaseParam.baseHoleDist * 0.8) && (dist < stackBaseParam.baseHoleDist * 1.2))
				{
					basePairs.push_back({i, j});
					basePairYDist.push_back(yDist);
				}
			}
			else //右边
			{
				double dist = abs(node2_R.jumpPos.x - node1_L.jumpPos.x);
				double yDist = abs(node2_R.jumpPos.y - node1_L.jumpPos.y);
				if ((dist > stackBaseParam.baseHoleDist * 0.8) && (dist < stackBaseParam.baseHoleDist * 1.2))
				{
					basePairs.push_back({ j, i });
					basePairYDist.push_back(yDist);
				}
			}
		}
	}
	if (0 == basePairs.size())
		return;

	SSG_intPair bestOne = basePairs[0];
	if (basePairs.size() > 1)
	{
		
		double minDist = basePairYDist[0];
		//挑选一个最佳的匹配
		for (int i = 1, i_max = (int)basePairs.size(); i < i_max; i++)
		{
			if (minDist > basePairYDist[i])
			{
				minDist = basePairYDist[i];
				bestOne = basePairs[i];
			}
		}
	}
	
	//计算相机相对的垛的旋转角
	// 不需要拟合空直线，分别拟合XY平面和xz平面的直线


#endif
	//标注
	int treeId = 1;
	SSG_featureTree& a_tree = h_trees_jumps[bestOne.data_0];
	for (int j = 0, j_max = (int)a_tree.treeNodes.size(); j < j_max; j++)
	{
		int lineIdx = a_tree.treeNodes[j].jumpPos2D.y;
		int ptIdx = a_tree.treeNodes[j].jumpPos2D.x;
		laser3DPoints[lineIdx].p3DPosition[ptIdx].nPointIdx = treeId << 16;
	}
	treeId = 2;
	a_tree = h_trees_jumps[bestOne.data_1];
	for (int j = 0, j_max = (int)a_tree.treeNodes.size(); j < j_max; j++)
	{
		int lineIdx = a_tree.treeNodes[j].jumpPos2D.y;
		int ptIdx = a_tree.treeNodes[j].jumpPos2D.x;
		laser3DPoints[lineIdx].p3DPosition[ptIdx].nPointIdx = treeId << 16;
	}

#if 0
	//生成目标数据
	int objId = 1;
	for (int i = 0, i_max = hTree_top_first.size(); i < i_max; i++)
	{
		SSG_featureTree& a_tree = hTree_top_first[i];
		double center_x = (a_tree.roi.left + a_tree.roi.right) / 2;
		std::vector< SVzNL3DPoint> pkPts;
		SVzNL3DPoint objPt = { 0,0,0 };  //与center_x最近的点作为目标点。
		for (int j = 0, j_max = a_tree.treeNodes.size(); j < j_max; j++)
		{
			SVzNL3DPoint a_pt = a_tree.treeNodes[j].jumpPos;
			//a_pt.y = a_tree.treeNodes[j].jumpPos.x;
			//a_pt.x = a_tree.treeNodes[j].jumpPos.y;
			//a_pt.z = a_tree.treeNodes[j].jumpPos.z;
			pkPts.push_back(a_pt);
			//寻找与center_x最近的点。
			if (objPt.z < 1e-4)
			{
				objPt = a_tree.treeNodes[j].jumpPos;
			}
			else
			{
				double old_dist = abs(objPt.x - center_x);
				double curr_dist = abs(a_tree.treeNodes[j].jumpPos.x - center_x);
				if (old_dist > curr_dist)
					objPt = a_tree.treeNodes[j].jumpPos;
			}
		}
		//计算倾角
		double k = 0.0, b = 0.0;
		lineFitting(pkPts, &k, &b);
		double angle = 90 - atan(k) * 180 / PI;

		//计算抓取点
		SVzNL3DPoint pre_pt = { 0,0,0 };
		SVzNL3DPoint post_pt = { 0,0,0 };
		for (int m = 0, m_max = hTree_btm_first.size(); m < m_max; m++)
		{
			SSG_featureTree& a_btm_tree = hTree_btm_first[m];
			int node_size = a_btm_tree.treeNodes.size();
			if (node_size == 0)
				continue;

			if (a_btm_tree.roi.right < center_x)
			{
				if (pre_pt.z < 1e-4)
					pre_pt = a_btm_tree.treeNodes[node_size - 1].jumpPos;
				else if (pre_pt.x < a_btm_tree.treeNodes[node_size - 1].jumpPos.x)
					pre_pt = a_btm_tree.treeNodes[node_size - 1].jumpPos;
			}
			else if (a_btm_tree.roi.left > center_x)
			{
				if (post_pt.z < 1e-4)
					post_pt = a_btm_tree.treeNodes[0].jumpPos;
				else if (post_pt.x > a_btm_tree.treeNodes[0].jumpPos.x)
					post_pt = a_btm_tree.treeNodes[0].jumpPos;
			}
			else
			{
				for (int n = 0, n_max = a_btm_tree.treeNodes.size(); n < n_max; n++)
				{
					SSG_basicFeature1D& a_node = a_btm_tree.treeNodes[n];
					if (a_node.jumpPos.x <= center_x)
					{
						if (pre_pt.z < 1e-4)
							pre_pt = a_node.jumpPos;
						else if (pre_pt.x < a_node.jumpPos.x)
							pre_pt = a_node.jumpPos;
					}
					else
					{
						if (post_pt.z < 1e-4)
							post_pt = a_node.jumpPos;
						else if (post_pt.x > a_node.jumpPos.x)
							post_pt = a_node.jumpPos;
					}
				}

			}
		}
		if ((pre_pt.z < 1e-4) || (post_pt.z < 1e-4))
			continue;

		//生成结果
		SSG_sideBagInfo a_sideBagInfo;
		a_sideBagInfo.objIdx = objId;
		objId++;
		a_sideBagInfo.objROI.top = a_tree.roi.left;
		a_sideBagInfo.objROI.bottom = a_tree.roi.right;
		a_sideBagInfo.objROI.left = a_tree.roi.top;
		a_sideBagInfo.objROI.right = a_tree.roi.bottom;
		//根据扫描到是否为长边判断是否为合格目标
		double L = a_tree.roi.right - a_tree.roi.left;
		if (L > (algoParam.bagParam.bagW + algoParam.bagParam.bagL) / 2)
			a_sideBagInfo.isValid = false;
		else
			a_sideBagInfo.isValid = true;
		a_sideBagInfo.objPos.x = objPt.y;
		a_sideBagInfo.objPos.y = objPt.x;
		a_sideBagInfo.objPos.z = objPt.z;
		//计算抓取点
		if (post_pt.x == pre_pt.x)
		{
			a_sideBagInfo.graspPos.y = (pre_pt.x + post_pt.x) / 2;
			a_sideBagInfo.graspPos.x = (pre_pt.y + post_pt.y) / 2;
			a_sideBagInfo.graspPos.z = (pre_pt.z + post_pt.z) / 2;
		}
		else
		{
			double k1 = (post_pt.x - center_x) / (post_pt.x - pre_pt.x);
			double k2 = 1.0 - k1;
			a_sideBagInfo.graspPos.y = pre_pt.x * k1 + post_pt.x * k2;
			a_sideBagInfo.graspPos.x = pre_pt.y * k1 + post_pt.y * k2;
			a_sideBagInfo.graspPos.z = pre_pt.z * k1 + post_pt.z * k2;
		}
		a_sideBagInfo.graspPos.z = (a_sideBagInfo.graspPos.z + objPt.z) / 2;
		a_sideBagInfo.graspPos.x_roll = 0;
		a_sideBagInfo.graspPos.y_pitch = 0;
		a_sideBagInfo.graspPos.z_yaw = angle;

		objOps.push_back(a_sideBagInfo);
	}
#endif
	return;
}

//计算一个平面调平参数。
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
SSG_planeCalibPara sg_getBagBaseCalibPara(
	SVzNL3DLaserLine* laser3DPoints,
	int lineNum)
{
	return sg_getPlaneCalibPara(laser3DPoints, lineNum);
}