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

std::string	m_strVersion = "1.0.0";
const char* wd_particleSegVersion(void)
{
	return m_strVersion.c_str();
}

//计算一个平面调平参数。
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
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::Mat dtImage_color;
	cv::cvtColor(dtImage, dtImage_color, cv::COLOR_GRAY2BGR);
	cv::imwrite("distTransform.png", dtImage_color);
#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++)
	{
		SSG_2DValueI& obj_0 = filter_dt_peaks[i];
		for (int j = i + 1; j < filterSize; j++)
		{
			SSG_2DValueI& obj_1 = filter_dt_peaks[j];
			double dist = sqrt(pow(obj_0.x - obj_1.x, 2) + pow(obj_0.y - obj_1.y, 2));
			double distTh = dist * 1.2;
			if ((obj_0.valueD + obj_1.valueD) > distTh)  //合并
			{
				if (obj_0.valueD < obj_1.valueD)
					obj_0.value = -1;
				else
					obj_1.value = -1;
			}
		}
	}

	//有效种子
	std::vector<SSG_2DValueI> vld_dt_peaks;
	for (int i = 0; i < filterSize; i++)
	{
		if (filter_dt_peaks[i].value < 0)
			continue;
		vld_dt_peaks.push_back(filter_dt_peaks[i]);
	}
#if OUTPUT_DEBUG  //debug	
	int dbg_seedNum = (int)vld_dt_peaks.size();
	for (int i = 0; i < dbg_seedNum; i++)
	{
		int dbg_px = vld_dt_peaks[i].x;
		int dbg_py = vld_dt_peaks[i].y;
		//dtImage_color.at<cv::Vec3b>(dbg_py, dbg_px) = cv::Vec3b(0, 0, 255);
		cv::circle(dtImage_color, cv::Point(dbg_px, dbg_py), 3, cv::Scalar(0, 0, 255), -1);
	}
	cv::imwrite("distTransform_seed.png", dtImage_color);
#endif

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

	//获取最大最小值
	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));  // 初始化标记图为1,背景
	int maxValue = (int)maxVal + 2;
	int maxLevel = (int)(maxVal - minVal);
	for (int i = 0; i < distTransform.rows; i++)
	{
		if (i == 758)
			int kkk = 1;
		float* rowPtr = distTransform.ptr<float>(i);
		for (int j = 0; j < distTransform.cols; j++)
		{
			if (j == 171)
				int kkk = 1;
			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;
			}
		}
	}
	int startMarkerID = 2;
	wd_seedWatershed(wsImg, vld_dt_peaks, maxLevel, startMarkerID);
#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 if (wsImg.markers[i][j] < 2)
			{
				waterShedResult.at<cv::Vec3b>(i, j) = cv::Vec3b(200, 200, 200);
			}
			else 
			{  // 区域（根据标记值生成不同颜色）

				int color_r = (wsImg.markers[i][j] * 97) % 256;
				int color_g = (wsImg.markers[i][j] * 73) % 256;
				int color_b = (wsImg.markers[i][j] * 59) % 256;
				waterShedResult.at<cv::Vec3b>(i, j) = cv::Vec3b(color_b, color_g, color_r);
			}
		}
	}
	cv::imwrite("watershed.png", waterShedResult);
#endif

	//生成分割后的目标
	//检查每个3D点在投影上的位置，生成目标的3D点
	std::vector<std::vector< SVzNL3DPoint>> segObjs;
	int maxMkID = startMarkerID + (int)vld_dt_peaks.size();
	segObjs.resize(maxMkID);
	for (int line = 0; line < lineNum; line++)
	{
		for (int i = 0; i < nPointCnt; i++)
		{
			SVzNL3DPosition* pt3D = &scanLines[line][i];
			pt3D->nPointIdx = 0;
			if (pt3D->pt3D.z < 1e-4)
				continue;

			double x = pt3D->pt3D.x;
			double y = pt3D->pt3D.y;
			int px = (int)(x - x_range.min) / scale + edgeSkip;
			int py = (int)(y - y_range.min) / scale + edgeSkip;

			int marker = wsImg.markers[py][px];
			if ((marker >= startMarkerID)&&( marker <= maxMkID))
			{
				pt3D->nPointIdx = marker;
				segObjs[marker].push_back(pt3D->pt3D);
			}
		}
	}

	//生成目标
	for (int i = startMarkerID; i < maxMkID; i++)
	{
		if (segObjs[i].size() < 10)
			continue;

		//获取投影,并统计最小Z和最大Z
		double minZ = -1;
		double maxZ = 0;
		std::vector<cv::Point2f> points;
		int ptSize = (int)segObjs[i].size();
		for (int m = 0; m < ptSize; m++)
		{
			float x = (float)segObjs[i][m].x;
			float y = (float)segObjs[i][m].y;
			points.push_back(cv::Point2f(x, y));
			if (minZ < 0)
			{
				minZ = segObjs[i][m].z;
				maxZ = segObjs[i][m].z;
			}
			else
			{
				if (minZ > segObjs[i][m].z) minZ = segObjs[i][m].z;
				if (maxZ < segObjs[i][m].z) maxZ = segObjs[i][m].z;
			}
		}
		if (points.size() == 0)
			continue;
		//最小外接矩
		// 最小外接矩形
		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;
		}
		SWD_ParticlePosInfo a_obj;
		a_obj.size.length = width;
		a_obj.size.width = height;
		a_obj.size.height = maxZ - minZ;
		for (int m = 0; m < 4; m++)
		{
			SVzNL3DPoint vPt_btm = { vertices[m].x, vertices[m].y, maxZ };
			SVzNL3DPoint vPt_top = { vertices[m].x, vertices[m].y, minZ };
			a_obj.vertix[m] = vPt_btm;
			a_obj.vertix[m + 4] = vPt_top;
		}
		particles.push_back(a_obj);
	}
}