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

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

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

void wd_noiseFilter(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines, 
	const SWD_PSM_algoParam algoParam,
	int* errCode)
{
	*errCode = 0;
	int lineNum = (int)scanLines.size();
	int nPointCnt = (int)scanLines[0].size();
	bool vldGrid = true;
	for (int i = 0; i < lineNum; i++)
	{
		if (nPointCnt != (int)scanLines[i].size())
			vldGrid = false;
		wd_vectorDataRemoveOutlier_overwrite(
			scanLines[i],
			algoParam.filterParam);
	}
	if (false == vldGrid)
	{
		*errCode = SG_ERR_3D_DATA_INVLD;
		return;
	}
	//水平方向过滤
	int hLineNum = nPointCnt;  //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] = scanLines[line][j];
			filterHLines[j][line].pt3D.x = scanLines[line][j].pt3D.y;
			filterHLines[j][line].pt3D.y = scanLines[line][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];
			scanLines[lineIdx][hLine].pt3D.z = 0;
		}
	}
	return;
}

//粒径检测
void wd_particleSizeMeasure(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SWD_paricleSizeParam particleSizeParam,
	const SSG_planeCalibPara groundCalibPara,
	const SWD_PSM_algoParam algoParam,
	std::vector<SWD_ParticlePosInfo>& particles,
	int* errCode)
{
	int lineNum = (int)scanLines.size();
	if (lineNum == 0)
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return;
	}
	//噪点过滤
	//垂直方向过滤
	wd_noiseFilter(scanLines, algoParam, errCode);
	if (*errCode != 0)
		return;

	///开始数据处理
	//提取特征：跳变、低于z阈值、V及L型
	//垂直数据处理
	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;
		wd_getLineCornerFeature_PSM(
			&scanLines[i][0],
			(int)scanLines[i].size(),
			i,
			groundCalibPara.planeHeight,
			algoParam.cornerParam,
			&a_line_features);

		all_vLineFeatures.push_back(a_line_features);  //空行也加入，保证能按行号索引
	}
	//生成水平扫描数据
	int nPointCnt = (int)scanLines[0].size();
	std::vector<std::vector<SVzNL3DPosition>> hLines;
	hLines.resize(nPointCnt);
	for (int i = 0; i < nPointCnt; i++)
		hLines[i].resize(lineNum);
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < nPointCnt; j++)
		{
			scanLines[line][j].nPointIdx = 0;;
			hLines[j][line] = scanLines[line][j];
			hLines[j][line].pt3D.x = scanLines[line][j].pt3D.y;
			hLines[j][line].pt3D.y = scanLines[line][j].pt3D.x;
		}
	}
	std::vector<SSG_lineFeature> all_hLineFeatures;
	//逐行提取特征
	for (int hLine = 0; hLine < nPointCnt; hLine++)
	{
		if (hLine == 14)
			int kkk = 1;

		SSG_lineFeature a_hLine_featrues;
		a_hLine_featrues.lineIdx = hLine;

		wd_getLineCornerFeature_PSM(
			&hLines[hLine][0],
			(int)hLines[hLine].size(),
			hLine,
			groundCalibPara.planeHeight,
			algoParam.cornerParam,
			&a_hLine_featrues);

		//if ((a_hLine_featrues.features.size() > 0) || (a_hLine_featrues.endings.size() > 0))
		all_hLineFeatures.push_back(a_hLine_featrues);//空行也加入，保证能按行号索引
	}

	/// 统计整个视野大小
	SWD_pointCloudPara pntCloudPara = wd_getPointCloudPara(scanLines);
	SVzNLRangeD x_range = pntCloudPara.xRange;
	SVzNLRangeD y_range = pntCloudPara.yRange;

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

	//垂直方向特征生长（相机扫描方向）
	std::vector<SSG_featureTree> v_feature_trees;
	std::vector<SSG_featureTree> v_ending_trees;
	//特征生长
	wd_getFeatureGrowingTrees_noTypeMatch(
		all_vLineFeatures,
		v_feature_trees,
		v_ending_trees,
		algoParam.growParam);

	//水平方向特征生长
	std::vector<SSG_featureTree> h_feature_trees;
	std::vector<SSG_featureTree> h_ending_trees;
	//特征生长
	wd_getFeatureGrowingTrees_noTypeMatch(
		all_hLineFeatures,
		h_feature_trees,
		h_ending_trees,
		algoParam.growParam);

	//生成Mask
	double scale;
	if ((pntCloudPara.scale_x < 0) || (pntCloudPara.scale_y < 0))
	{
		*errCode = SG_ERR_3D_DATA_INVLD;
		return;
	}
	if(pntCloudPara.scale_x < pntCloudPara.scale_y)
		scale = pntCloudPara.scale_x;
	else
		scale = pntCloudPara.scale_y;


	//距离变换Mask，以1mm为量化尺度
	double inerPolateDistTh = scale * 10; //插值门限， 两间距离大于此阈值不插值
	int edgeSkip = 2;
	int maskX = (int)(x_range.max - x_range.min)/scale + 1;
	int maskY = (int)(y_range.max - y_range.min)/scale + 1;
	if ((maskX < 16) || (maskY < 16))
		return;
	maskY = maskY + edgeSkip * 2;
	maskX = maskX + edgeSkip * 2;
	cv::Mat distTranformMask(maskY, maskX, CV_32FC1, 0.0f);  //距离变换Mask，初始化为一个极大值1e+6
	cv::Mat distTranformIndexing(maskY, maskX, CV_32SC2, cv::Vec2i(0, 0));  //坐标索引
	cv::Mat featureMask = cv::Mat::zeros(nPointCnt, lineNum, CV_32SC4);
	pointClout2DProjection(
		scanLines,
		x_range,
		y_range,
		scale,
		edgeSkip,
		inerPolateDistTh,  //插值阈值。大于此阈值的不进行量化插值
		distTranformMask,//投影量化数据，初始化为一个极大值1e+6
		distTranformIndexing  //标记坐标索引，用于回找3D坐标
	);

	std::vector<SSG_treeInfo> allTreesInfo; //不包含边界
	//标注:垂直
	SSG_treeInfo a_nullTree;
	memset(&a_nullTree, 0, sizeof(SSG_treeInfo));
	allTreesInfo.push_back(a_nullTree); //保持存储位置与treeIdx相同位置，方便索引
	//标注垂直边界
	int treeID = 1;
	for (int i = 0, i_max = (int)v_ending_trees.size(); i < i_max; i++)
	{
		SSG_featureTree* a_vEdgeTree = &v_ending_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 (scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.z > 1e-4)//虚假目标过滤后点会置0
			{
				scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx = a_feature->featureType;
				scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx &= 0xffff;
				scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx += 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)((scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.x - x_range.min)/scale) + edgeSkip;
				int py = (int)((scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.y - y_range.min)/scale) + edgeSkip;
				distTranformMask.at<float>(py, px) = 0;
			}
		}
		treeID++;
	}
	//标注水平边界
	for (int i = 0, i_max = (int)h_ending_trees.size(); i < i_max; i++)
	{
		SSG_featureTree* a_hEdgeTree = &h_ending_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 (scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.z > 1e-4)//虚假目标过滤后点会置0
			{
				int existEdgeId = scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx >> 16;
				if (existEdgeId == 0)
				{
					scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += a_feature->featureType << 4;
					scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx &= 0xffff;
					scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += 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)((scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.x - x_range.min)/scale) + edgeSkip;
					int py = (int)((scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.y - y_range.min)/scale) + edgeSkip;
					distTranformMask.at<float>(py, px) = 0;
				}
			}
		}
		treeID++;
	}
	//垂直特征标注
	int hvTreeIdx = treeID;
	int vTreeStart = treeID;
	for (int i = 0, i_max = (int)v_feature_trees.size(); i < i_max; i++)
	{
		SSG_featureTree* a_vTree = &v_feature_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 (scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.z > 1e-4)//虚假目标过滤后点会置0
			{
				int existEdgeId = scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx >> 16;
				if (existEdgeId == 0)
				{
					scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx = a_feature->featureType;
					scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx &= 0xffff;
					scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx += hvTreeIdx << 16;
					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)((scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.x - x_range.min)/scale) + edgeSkip;
					int py = (int)((scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.y - y_range.min)/scale) + edgeSkip;
					distTranformMask.at<float>(py, px) = 0;
				}
			}
		}
		hvTreeIdx++;
	}
	int hTreeStart = hvTreeIdx;
	//标注:水平特征
	for (int i = 0, i_max = (int)h_feature_trees.size(); i < i_max; i++)
	{
		SSG_featureTree* a_hTree = &h_feature_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 (scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.z > 1e-4)//虚假目标过滤后点会置0
			{
				int existEdgeId = scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx >> 16;
				if (existEdgeId == 0)
				{
					scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += a_feature->featureType << 4;
					scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx &= 0xffff;
					scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += hvTreeIdx << 16;
					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)((scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.x - x_range.min)/scale) + edgeSkip;
					int py = (int)((scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.y - y_range.min)/scale) + edgeSkip;
					distTranformMask.at<float>(py, px) = 0;
				}
			}
		}
		hvTreeIdx++;
	}
	int hvTreeSize = hvTreeIdx;
	double x_scale = pntCloudPara.scale_x;
	double y_scale = pntCloudPara.scale_y;

	//进行距离变换，然后使用分水岭算法进行分割
	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

	//寻找Peak。Peak确定后以Peak为种子点进行分水岭方法分割
	double minW = particleSizeParam.minSize.width;
	SSG_localPkParam searchWin;
	searchWin.seachW_lines = (int)(minW * 0.4/scale);
	searchWin.searchW_pts = (int)(minW * 0.4/scale);
	std::vector<SSG_2DValueI> dt_peaks;
	sg_getLocalPeaks_distTransform(distTransform, dt_peaks, searchWin);
	//以大小进行过滤
	double minDistTh = minW * 0.4 / scale;
	std::vector<SSG_2DValueI> filter_dt_peaks;
	for (int i = 0, i_max = (int)dt_peaks.size(); i < i_max; i++)
	{
		if (dt_peaks[i].valueD > minDistTh)
			filter_dt_peaks.push_back(dt_peaks[i]);
	}
	//以距离进行过滤：距离变换相当于最小内切圆。不同的目标所在的内切圆一定不相交，因此：R1+R2 < 圆心距离
	int filterSize = (int)filter_dt_peaks.size();
	for (int i = 0; i < filterSize; i++)
	{
		for (int j = i + 1; j < filterSize; j++)
		{

		}
	}
	
	//分水岭分割
	
	//扫描边界，建立相邻目标边界集合 
	//通过目标相邻边界判断是否合并相邻目标
	

	//获取最大最小值
	double minVal, maxVal;
	cv::Point minLoc, maxLoc;
	// 调用minMaxLoc函数
	cv::minMaxLoc(distTransform, &minVal, &maxVal, &minLoc, &maxLoc);

	//分水岭算法进行分割
	SWD_waterShedImage wsImg;
	wsImg.width = distTransform.cols;
	wsImg.height = distTransform.rows;
	wsImg.gray.resize(wsImg.height, std::vector<int>(wsImg.width));
	wsImg.markers.resize(wsImg.height, std::vector<int>(wsImg.width, 1));  // 初始化标记图为0
	int maxValue = (int)maxVal + 2;
	for (int i = 0; i < distTransform.rows; i++)
	{
		float* rowPtr = distTransform.ptr<float>(i);
		for (int j = 0; j < distTransform.cols; j++)
		{
			float disValue = rowPtr[j];
			if (disValue < 1e-4) //边界和背景
				wsImg.gray[i][j] = maxValue;
			else
			{
				wsImg.gray[i][j] = (int)(maxVal - disValue);
				wsImg.markers[i][j] = 0;
			}
		}
	}
	
	watershed(wsImg);

#if OUTPUT_DEBUG  //debug	
	cv::Mat waterShedResult(wsImg.height, wsImg.width, CV_8UC3);
	for (int i = 0; i < wsImg.height; ++i) {
		for (int j = 0; j < wsImg.width; ++j) {
			if (wsImg.markers[i][j] == -1) {  // 分水岭边界（红色）
				waterShedResult.at<cv::Vec3b>(i, j) = cv::Vec3b(0,0,255);
			}
			else {  // 区域（根据标记值生成不同颜色）
				int color_r = (wsImg.markers[i][j] * 50) % 256;
				int color_g = (color_r + 85) % 256;
				int color_b = (color_r + 170) % 256;
				waterShedResult.at<cv::Vec3b>(i, j) = cv::Vec3b(color_b, color_g, color_r);
			}
		}
	}
	cv::imwrite("watershed.png", waterShedResult);
#endif
#if 0
	





		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

		/// 以区域最高点作为种子进行区域生长
		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;
		}
#endif
	//水平和垂直提取特征
	
	//进行距离变换
	//分水岭分割
	//按z高度检测目标
}