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

typedef struct
{
	int id;
	double dist;
	double angle;
}_linkInfo;
typedef struct
{
	int objId;
	SVzNL3DPoint ptPos;
	_linkInfo link[8]; //R, RT, T, LT, L, LB, B, RB
}NodeOctLink;

typedef struct
{
	SSG_ROIRectD roi;
	int* qrCode;
}DMCodeInfo;

//将ply格式的数据恢复成扫描行的数据形式，从而方面按行进行处理
void wd_getScanLines(
	std::vector<SVzNL3DPoint>& scanData, 
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	int scan_rows)
{
	std::vector<SVzNL3DPosition> a_line;
	for (int i = 0, i_max = scanData.size(); i < i_max; i++)
	{
		int idx = i % scan_rows;
		if (0 == idx)
		{
			//新的一行
			if (a_line.size() > 0)
			{
				scanLines.push_back(a_line);
				a_line.clear();
			}
		}
		SVzNL3DPoint a_pt = scanData[i];
		SVzNL3DPosition a_idxPt;
		a_idxPt.nPointIdx = 0;
		a_idxPt.pt3D = a_pt;
		a_line.push_back(a_idxPt);
	}
	if(a_line.size() > 0)
		scanLines.push_back(a_line);
	return;
}

SSG_planeCalibPara wd_getBaseCalibPara(
	SVzNL3DLaserLine* laser3DPoints,
	int lineNum)
{
	return sg_getPlaneCalibPara(laser3DPoints, lineNum);
}

void wd_lineDataR(std::vector< SVzNL3DPosition>& a_line,
	const double* camPoseR,
	double groundH)
{
	lineDataRT_vector(a_line, camPoseR, groundH);
}

SVzNL3DPoint _getObjCenter(SSG_featureTree& a_tree)
{
	SVzNL3DPoint centerPt = { 0, 0, 0 };
	int nodeNum = 0;
	for (int i = 0; i < a_tree.treeNodes.size(); i++)
	{
		centerPt.x += a_tree.treeNodes[i].jumpPos.x;
		centerPt.y += a_tree.treeNodes[i].jumpPos.y;
		centerPt.z += a_tree.treeNodes[i].jumpPos.z;
		nodeNum++;
	}
	if (nodeNum > 0)
	{
		centerPt.x = centerPt.x / nodeNum;
		centerPt.y = centerPt.y / nodeNum;
		centerPt.z = centerPt.z / nodeNum;
	}
	return centerPt;
}

_linkInfo compareBestLinl(_linkInfo link_1, _linkInfo link_2, double bestDist)
{
	double dist_diff1 = abs(link_1.dist - bestDist);
	double dist_diff2 = abs(link_2.dist - bestDist);
	if (dist_diff1 < dist_diff2)
		return link_1;
	else
		return link_2;
}

//建立Node间的上下左右的Link关系
void _createNodeLinks(
	std::vector< SVzNL3DPosition>& a_cluster,
	std::vector< NodeOctLink>& clusterNodeLinks,
	double row_space,
	double col_space
)
{
	int dirInvTbl[8] = { 4, 5, 6, 7, 0, 1, 2, 3 };
	double diaDist = sqrt(pow(row_space, 2) + pow(col_space, 2));
	int nodeSize = a_cluster.size();
	for (int i = 0; i < nodeSize; i++)
	{
		SVzNL3DPosition& a_node = a_cluster[i];
		NodeOctLink& a_nodeLink = clusterNodeLinks[i];
		_linkInfo link[8];
		for (int j = 0; j < 8; j++)
		{
			link[j].id = -1;
			link[j].dist = 0.0;
			link[j].angle = 0.0;
		}
		for (int j = i+1; j < nodeSize; j++)
		{
			SVzNL3DPosition& chk_node = a_cluster[j];
			NodeOctLink& chk_nodeLink = clusterNodeLinks[j];
			if (chk_node.nPointIdx < 0)
				continue;

			double dist = sqrt(pow(a_node.pt3D.x - chk_node.pt3D.x, 2) + pow(a_node.pt3D.y - chk_node.pt3D.y, 2));
			if (dist < diaDist * 2) //粗过滤
			{
				double angle = atan2(a_node.pt3D.y - chk_node.pt3D.y, chk_node.pt3D.x - a_node.pt3D.x); //图像坐标系y方向与欧氏坐标系相么
				angle = angle * 180.0 / PI;  //转为角度
				if (angle < 0)
					angle += 360;  //转成0-360度

				_linkInfo a_link = { j, dist, angle };
				if ((angle > 345) || (angle < 15))  //R:0
				{
					if (dist < col_space * 1.5)
					{
						if (link[0].id < 0)
							link[0] = a_link;
						else
							link[0] = compareBestLinl(link[0], a_link, col_space);
					}
				}
				else if ((angle > 30) && (angle < 60)) //RT:1
				{
					if (dist < diaDist * 1.5)
					{
						if (link[1].id < 0)
							link[1] = a_link;
						else
							link[1] = compareBestLinl(link[1], a_link, diaDist);
					}
				}
				else if ((angle > 75) && (angle < 105)) //T:2
				{
					if (dist < row_space * 1.5)
					{
						if (link[2].id < 0)
							link[2] = a_link;
						else
							link[2] = compareBestLinl(link[2], a_link, row_space);
					}
				}
				else if ((angle > 120) && (angle < 150)) //LT:3
				{
					if (dist < diaDist * 1.5)
					{
						if (link[3].id < 0)
							link[3] = a_link;
						else
							link[3] = compareBestLinl(link[3], a_link, diaDist);
					}
				}
				else if ((angle > 165) && (angle < 195)) //L:4
				{
					if (dist < col_space * 1.5)
					{
						if (link[4].id < 0)
							link[4] = a_link;
						else
							link[4] = compareBestLinl(link[4], a_link, col_space);
					}
				}
				else if ((angle > 210) && (angle < 240)) //LB:5
				{
					if (dist < diaDist * 1.5)
					{
						if (link[5].id < 0)
							link[5] = a_link;
						else
							link[5] = compareBestLinl(link[5], a_link, diaDist);
					}
				}
				else if ((angle > 255) && (angle < 285)) //B:6
				{
					if (dist < row_space * 1.5)
					{
						if (link[6].id < 0)
							link[6] = a_link;
						else
							link[6] = compareBestLinl(link[6], a_link, row_space);
					}
				}
				else if ((angle > 300) && (angle < 330)) //RB:7
				{
					if (dist < diaDist * 1.5)
					{
						if (link[7].id < 0)
							link[7] = a_link;
						else
							link[7] = compareBestLinl(link[7], a_link, diaDist);
					}
				}
			}
		}

		for (int j = 0; j < 8; j++)
		{
			if (link[j].id >= 0)
			{
				a_nodeLink.link[j] = link[j];
				int linkId = link[j].id;
				int dir_inv = dirInvTbl[j];
				if (clusterNodeLinks[linkId].link[dir_inv].id < 0)
				{
					clusterNodeLinks[linkId].link[dir_inv].id = a_nodeLink.objId;
					clusterNodeLinks[linkId].link[dir_inv].dist = link[j].dist;
					clusterNodeLinks[linkId].link[dir_inv].angle = link[j].angle + 180;
					if (clusterNodeLinks[linkId].link[dir_inv].angle >= 360)
						clusterNodeLinks[linkId].link[dir_inv].angle = clusterNodeLinks[linkId].link[dir_inv].angle - 360;
				}
			}
		}
	}
}

//
std::vector<int> _LTseedSearchRight(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks)
{
	NodeOctLink a_seed = startNode;
	std::vector<int> result;
	result.push_back(a_seed.objId);
	while (1)
	{
		if (a_seed.link[0].id < 0)
			break;
		a_seed = clusterNodeLinks[a_seed.link[0].id];

		if ((a_seed.link[1].id < 0) && (a_seed.link[2].id < 0) && (a_seed.link[3].id < 0))
			result.push_back(a_seed.objId);
		else
			break;
	}
	return result;
}
std::vector<int> _LTseedSearchBottom(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks)
{
	NodeOctLink a_seed = startNode;
	std::vector<int> result;
	result.push_back(a_seed.objId);
	while (1)
	{
		if (a_seed.link[6].id < 0)
			break;
		a_seed = clusterNodeLinks[a_seed.link[6].id];

		if ((a_seed.link[3].id < 0) && (a_seed.link[4].id < 0) && (a_seed.link[5].id < 0))
			result.push_back(a_seed.objId);
		else
			break;
	}
	return result;
}
int _foundDM_LTcorner(std::vector< NodeOctLink>& clusterNodeLinks, int rows, int cols)
{
	for (int i = 0, i_max = clusterNodeLinks.size(); i < i_max; i++)
	{
		//寻找种子向右向下
		NodeOctLink a_seed = clusterNodeLinks[i];
		if ((a_seed.link[0].id >= 0) && (a_seed.link[6].id >= 0) &&
			(a_seed.link[1].id < 0)  && (a_seed.link[2].id < 0) &&
			(a_seed.link[3].id < 0) && (a_seed.link[4].id < 0) &&
			(a_seed.link[5].id < 0))  //合格的种子
		{
			std::vector<int> R_result = _LTseedSearchRight(a_seed, clusterNodeLinks);
			std::vector<int> B_result = _LTseedSearchBottom(a_seed, clusterNodeLinks);
			if (((int)R_result.size() == cols) && ((int)B_result.size() == rows))  //找到LT角点
				return i;
		}
	}
	return -1;
}

std::vector<int> _RTseedSearchLeft(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks)
{
	NodeOctLink a_seed = startNode;
	std::vector<int> result;
	result.push_back(a_seed.objId);
	while (1)
	{
		if (a_seed.link[4].id < 0)
			break;
		a_seed = clusterNodeLinks[a_seed.link[4].id];

		if ((a_seed.link[1].id < 0) && (a_seed.link[2].id < 0) && (a_seed.link[3].id < 0))
			result.push_back(a_seed.objId);
		else
			break;
	}
	return result;
}
std::vector<int> _RTseedSearchBottom(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks)
{
	NodeOctLink a_seed = startNode;
	std::vector<int> result;
	result.push_back(a_seed.objId);
	while (1)
	{
		if (a_seed.link[6].id < 0)
			break;
		a_seed = clusterNodeLinks[a_seed.link[6].id];

		if ((a_seed.link[1].id < 0) && (a_seed.link[0].id < 0) && (a_seed.link[7].id < 0))
			result.push_back(a_seed.objId);
		else
			break;
	}
	return result;
}
int _foundDM_RTcorner(std::vector< NodeOctLink>& clusterNodeLinks, int rows, int cols)
{
	for (int i = 0, i_max = clusterNodeLinks.size(); i < i_max; i++)
	{
		//寻找种子向右向下
		NodeOctLink a_seed = clusterNodeLinks[i];
		if ((a_seed.link[4].id >= 0) && (a_seed.link[6].id >= 0) &&
			(a_seed.link[0].id < 0) && (a_seed.link[1].id < 0) &&
			(a_seed.link[2].id < 0) && (a_seed.link[3].id < 0) &&
			(a_seed.link[7].id < 0))  //合格的种子
		{
			std::vector<int> L_result = _RTseedSearchLeft(a_seed, clusterNodeLinks);
			std::vector<int> B_result = _RTseedSearchBottom(a_seed, clusterNodeLinks);
			if (((int)L_result.size() == cols) && ((int)B_result.size() == rows))  //找到LT角点
				return i;
		}
	}
	return -1;
}

std::vector<int> _LBseedSearchRight(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks)
{
	NodeOctLink a_seed = startNode;
	std::vector<int> result;
	result.push_back(a_seed.objId);
	while (1)
	{
		if (a_seed.link[0].id < 0)
			break;
		a_seed = clusterNodeLinks[a_seed.link[0].id];

		if ((a_seed.link[5].id < 0) && (a_seed.link[6].id < 0) && (a_seed.link[7].id < 0))
			result.push_back(a_seed.objId);
		else
			break;
	}
	return result;
}
std::vector<int> _LBseedSearchTop(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks)
{
	NodeOctLink a_seed = startNode;
	std::vector<int> result;
	result.push_back(a_seed.objId);
	while (1)
	{
		if (a_seed.link[2].id < 0)
			break;
		a_seed = clusterNodeLinks[a_seed.link[2].id];

		if ((a_seed.link[3].id < 0) && (a_seed.link[4].id < 0) && (a_seed.link[5].id < 0))
			result.push_back(a_seed.objId);
		else
			break;
	}
	return result;
}
int _foundDM_LBcorner(std::vector< NodeOctLink>& clusterNodeLinks, int rows, int cols)
{
	for (int i = 0, i_max = clusterNodeLinks.size(); i < i_max; i++)
	{
		//寻找种子向右向下
		NodeOctLink a_seed = clusterNodeLinks[i];
		if ((a_seed.link[0].id >= 0) && (a_seed.link[2].id >= 0) &&
			(a_seed.link[3].id < 0) && (a_seed.link[4].id < 0) &&
			(a_seed.link[5].id < 0) && (a_seed.link[6].id < 0) &&
			(a_seed.link[7].id < 0))  //合格的种子
		{
			std::vector<int> L_result = _LBseedSearchRight(a_seed, clusterNodeLinks);
			std::vector<int> B_result = _LBseedSearchTop(a_seed, clusterNodeLinks);
			if (((int)L_result.size() == cols) && ((int)B_result.size() == rows))  //找到LT角点
				return i;
		}
	}
	return -1;
}

std::vector<int> _RBseedSearchLeft(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks)
{
	NodeOctLink a_seed = startNode;
	std::vector<int> result;
	result.push_back(a_seed.objId);
	while (1)
	{
		if (a_seed.link[4].id < 0)
			break;
		a_seed = clusterNodeLinks[a_seed.link[4].id];

		if ((a_seed.link[5].id < 0) && (a_seed.link[6].id < 0) && (a_seed.link[7].id < 0))
			result.push_back(a_seed.objId);
		else
			break;
	}
	return result;
}
std::vector<int> _RBseedSearchTop(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks)
{
	NodeOctLink a_seed = startNode;
	std::vector<int> result;
	result.push_back(a_seed.objId);
	while (1)
	{
		if (a_seed.link[2].id < 0)
			break;
		a_seed = clusterNodeLinks[a_seed.link[2].id];

		if ((a_seed.link[1].id < 0) && (a_seed.link[0].id < 0) && (a_seed.link[7].id < 0))
			result.push_back(a_seed.objId);
		else
			break;
	}
	return result;
}
int _foundDM_RBcorner(std::vector< NodeOctLink>& clusterNodeLinks, int rows, int cols)
{
	for (int i = 0, i_max = clusterNodeLinks.size(); i < i_max; i++)
	{
		//寻找种子向右向下
		NodeOctLink a_seed = clusterNodeLinks[i];
		if ((a_seed.ptPos.x > 19.2) && (a_seed.ptPos.y > -31))
			int kkk = 1;
		if ((a_seed.link[2].id >= 0) && (a_seed.link[4].id >= 0) &&
			(a_seed.link[0].id < 0) && (a_seed.link[1].id < 0) &&
			(a_seed.link[5].id < 0) && (a_seed.link[6].id < 0) &&
			(a_seed.link[7].id < 0))  //合格的种子
		{
			std::vector<int> L_result = _RBseedSearchLeft(a_seed, clusterNodeLinks);
			std::vector<int> B_result = _RBseedSearchTop(a_seed, clusterNodeLinks);
			if (((int)L_result.size() == cols) && ((int)B_result.size() == rows))  //找到LT角点
				return i;
		}
	}
	return -1;
}

typedef struct
{
	int idx;
	SVzNL2DPoint pt2D;
}DMPos;

void _getDMPosLinks(std::vector< DMPos>& posLink, std::vector< NodeOctLink>& clusterNodeLinks)
{
	int i = 0;
	while (1)
	{
		if (i >= posLink.size())
			break;
		DMPos a_seed = posLink[i];
		NodeOctLink& a_node = clusterNodeLinks[a_seed.idx];
		if (a_node.objId >= 0)
		{
			if (a_node.link[0].id >= 0)
			{
				DMPos new_seed;
				new_seed.idx = a_node.link[0].id;
				new_seed.pt2D = { a_seed.pt2D.x + 1, a_seed.pt2D.y };
				if (clusterNodeLinks[new_seed.idx].objId >= 0)
				{
					clusterNodeLinks[new_seed.idx].link[4].id = -1; //标记已经处理
					posLink.push_back(new_seed);
				}
			}
			if (a_node.link[1].id >= 0)
			{
				DMPos new_seed;
				new_seed.idx = a_node.link[1].id;
				new_seed.pt2D = { a_seed.pt2D.x + 1, a_seed.pt2D.y - 1 };
				if (clusterNodeLinks[new_seed.idx].objId >= 0)
				{
					clusterNodeLinks[new_seed.idx].link[5].id = -1; //标记已经处理
					posLink.push_back(new_seed);
				}
			}
			if (a_node.link[2].id >= 0)
			{
				DMPos new_seed;
				new_seed.idx = a_node.link[2].id;
				new_seed.pt2D = { a_seed.pt2D.x, a_seed.pt2D.y - 1 };
				if (clusterNodeLinks[new_seed.idx].objId >= 0)
				{
					clusterNodeLinks[new_seed.idx].link[6].id = -1; //标记已经处理
					posLink.push_back(new_seed);
				}
			}
			if (a_node.link[3].id >= 0)
			{
				DMPos new_seed;
				new_seed.idx = a_node.link[3].id;
				new_seed.pt2D = { a_seed.pt2D.x - 1, a_seed.pt2D.y - 1 };
				clusterNodeLinks[new_seed.idx].link[7].id = -1; //标记已经处理
				posLink.push_back(new_seed);
			}
			if (a_node.link[4].id >= 0)
			{
				DMPos new_seed;
				new_seed.idx = a_node.link[4].id;
				new_seed.pt2D = { a_seed.pt2D.x - 1, a_seed.pt2D.y };
				if (clusterNodeLinks[new_seed.idx].objId >= 0)
				{
					clusterNodeLinks[new_seed.idx].link[0].id = -1; //标记已经处理
					posLink.push_back(new_seed);
				}
			}
			if (a_node.link[5].id >= 0)
			{
				DMPos new_seed;
				new_seed.idx = a_node.link[5].id;
				new_seed.pt2D = { a_seed.pt2D.x - 1, a_seed.pt2D.y + 1 };
				if (clusterNodeLinks[new_seed.idx].objId >= 0)
				{
					clusterNodeLinks[new_seed.idx].link[1].id = -1; //标记已经处理
					posLink.push_back(new_seed);
				}
			}
			if (a_node.link[6].id >= 0)
			{
				DMPos new_seed;
				new_seed.idx = a_node.link[6].id;
				new_seed.pt2D = { a_seed.pt2D.x, a_seed.pt2D.y + 1 };
				if (clusterNodeLinks[new_seed.idx].objId >= 0)
				{
					clusterNodeLinks[new_seed.idx].link[2].id = -1; //标记已经处理
					posLink.push_back(new_seed);
				}
			}
			if (a_node.link[7].id >= 0)
			{
				DMPos new_seed;
				new_seed.idx = a_node.link[7].id;
				new_seed.pt2D = { a_seed.pt2D.x + 1, a_seed.pt2D.y + 1 };
				if (clusterNodeLinks[new_seed.idx].objId >= 0)
				{
					clusterNodeLinks[new_seed.idx].link[3].id = -1; //标记已经处理
					posLink.push_back(new_seed);
				}
			}
			clusterNodeLinks[a_seed.idx].objId = -1; //标记已经处理
		}
		i++;
	}
	return;
}

SSG_ROIRectD _getClusterROI(std::vector< SVzNL3DPosition>& a_cluster)
{
	SSG_ROIRectD roi = {0,0,0,0};
	int nodeSize = a_cluster.size();
	if (nodeSize == 0)
		return roi;

	roi = { a_cluster [0].pt3D.x, a_cluster[0].pt3D.x, a_cluster[0].pt3D.y, a_cluster[0].pt3D.y};
	for (int i = 1; i < nodeSize; i++)
	{
		roi.left = roi.left > a_cluster[i].pt3D.x ? a_cluster[i].pt3D.x : roi.left;
		roi.right = roi.right < a_cluster[i].pt3D.x ? a_cluster[i].pt3D.x : roi.right;
		roi.top = roi.top > a_cluster[i].pt3D.y ? a_cluster[i].pt3D.y : roi.top;
		roi.bottom = roi.bottom < a_cluster[i].pt3D.y ? a_cluster[i].pt3D.y : roi.bottom;
	}
	return roi;
}

void wd_QRcode3Ddetection(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const WD_QRcodeParam qrcode_param, 
	std::vector< SVzNL3DPosition>& outObjPoints,
	cv::Mat& dmCodeImg)
{
	std::vector<std::vector< SSG_basicFeature1D>> lineZMaxPts;
	int lineNum = scanLines.size();

	double scaleWin = qrcode_param.row_space / 5;
	double minPkHeighth = qrcode_param.pointHoleDepth / 2;
	double holeR = qrcode_param.pointHoleR;
	for (int line = 0; line < lineNum; line++)
	{
		std::vector< SSG_basicFeature1D> a_lineMax;
		sg_getFlatLineLocalPeaks_vector(
			scanLines[line],
			line,
			scaleWin,
			minPkHeighth,
			holeR,
			a_lineMax);
		lineZMaxPts.push_back(a_lineMax);
	}
	//特征生长
	SSG_treeGrowParam growParam;
	growParam.yDeviation_max = holeR;//生长时相邻特征最大的Y偏差
	growParam.zDeviation_max = holeR; //生长时相邻特征最大的Z偏差
	growParam.maxLineSkipNum = 5;  //生长时相邻特征的最大线间隔， -1时使用maxDkipDistance
	growParam.maxSkipDistance = -1; //若maxLineSkipNum为-1， 使用此参数.设为-1时，此参数无效
	growParam.minLTypeTreeLen = holeR/2;  //生长树最少的节点数目。小于此数目的生长树被移除
	growParam.minVTypeTreeLen = holeR/2;  //生长树最少的节点数目。小于此数目的生长树被移除

	std::vector<SSG_featureTree> trees;
	for (int line = 0; line < lineNum; line++)
	{
		std::vector< SSG_basicFeature1D>& a_lineMax = lineZMaxPts[line];
		if (a_lineMax.size() == 0)
			continue;

		//对feature进行生长
		bool isLastLine = false;
		if (line == lineNum - 1)
			isLastLine = true;
		sg_lineFeaturesGrowing(
			line,
			isLastLine,
			a_lineMax,
			trees,
			growParam);
	}

	//计算目标中心点，以此代表各个点
	std::vector< SVzNL3DPosition> objPoints;
	objPoints.resize(trees.size());
	for (int i = 0, i_max = trees.size(); i < i_max; i++)
	{
		SVzNL3DPosition a_obj;
		a_obj.nPointIdx = i;
		a_obj.pt3D = _getObjCenter(trees[i]);
		objPoints[i] = a_obj;
	}
	//将邻近的点合并
	int objPtSize = objPoints.size();
	for (int i = objPtSize - 1; i >= 0; i--)
	{
		SVzNL3DPosition a_pt = objPoints[i];
		bool isCombined = false;
		for (int j = i - 1; j >= 0; j--)
		{
			SVzNL3DPosition chk_pt = objPoints[j];
			double dist = sqrt(pow(a_pt.pt3D.x - chk_pt.pt3D.x, 2) + pow(a_pt.pt3D.y - chk_pt.pt3D.y, 2));
			if (dist < holeR * 2) //合并
			{
				objPoints[j].pt3D.x = (a_pt.pt3D.x + chk_pt.pt3D.x) / 2;
				objPoints[j].pt3D.y = (a_pt.pt3D.y + chk_pt.pt3D.y) / 2;
				objPoints[j].pt3D.z = (a_pt.pt3D.z + chk_pt.pt3D.z) / 2;
				isCombined = true;
				break;
			}	
		}
		if (true == isCombined)
			objPoints.erase(objPoints.begin() + i);
	}
	for (int i = 0, i_max = objPoints.size(); i < i_max; i++)
		outObjPoints.push_back(objPoints[i]);

	//聚类，分成一个个二维码区域
	double clusterDist = (qrcode_param.row_space + qrcode_param.col_space) * 2;  //使用棋盘格距离
	std::vector<std::vector< SVzNL3DPosition>> objClusters;
	sg_pointClustering(
		objPoints,
		clusterDist,
		objClusters  //result
	);

	std::vector<DMCodeInfo> qrCodeResult;
	for (int i = 0, i_max = objClusters.size(); i < i_max; i++)
	{
		std::vector< SVzNL3DPosition>& a_cluster = objClusters[i];
		int clustNodeSize = a_cluster.size();
		//构建相邻关系
		std::vector< NodeOctLink> clusterNodeLinks;
		clusterNodeLinks.resize(clustNodeSize);
		for (int m = 0; m < clustNodeSize; m++) //初始化
		{
			clusterNodeLinks[m].objId = m;
			clusterNodeLinks[m].ptPos = a_cluster[m].pt3D;
			for (int j = 0; j < 8; j++)
			{
				clusterNodeLinks[m].link[j].id = -1;
				clusterNodeLinks[m].link[j].dist = 0.0;
				clusterNodeLinks[m].link[j].angle = 0.0;
			}
		}
		_createNodeLinks(a_cluster,	clusterNodeLinks, qrcode_param.row_space, qrcode_param.col_space);
		//寻找DM二维码特征
		int cornerIdx = 0;
		int cornerNodeIdx = _foundDM_LTcorner(clusterNodeLinks, qrcode_param.rows, qrcode_param.cols);
		if (cornerNodeIdx < 0)
		{
			cornerIdx = 1;
			cornerNodeIdx = _foundDM_RTcorner(clusterNodeLinks, qrcode_param.rows, qrcode_param.cols);
			if (cornerNodeIdx < 0)
			{
				cornerIdx = 2;
				cornerNodeIdx = _foundDM_LBcorner(clusterNodeLinks, qrcode_param.rows, qrcode_param.cols);
				if (cornerNodeIdx < 0)
				{
					cornerIdx = 3;
					cornerNodeIdx = _foundDM_RBcorner(clusterNodeLinks, qrcode_param.rows, qrcode_param.cols);
				}
			}
		}
		if (cornerNodeIdx >= 0)
		{
			//提取二维码编码值
			int rows = qrcode_param.rows;
			int cols = qrcode_param.cols;

			DMPos startPos;
			startPos.idx = cornerNodeIdx;
			if (cornerIdx == 0)  //LT
				startPos.pt2D = { 0,0 };
			else if (cornerIdx == 1)  //RT
				startPos.pt2D = { 0,cols-1 };
			else if(cornerIdx == 2) //LB
				startPos.pt2D = { rows-1,0 };
			else //RB
				startPos.pt2D = { rows - 1, cols-1 };

			std::vector< DMPos> posLinks;
			posLinks.push_back(startPos);
			_getDMPosLinks(posLinks, clusterNodeLinks);
			
			int* codeBuff = (int*)malloc(sizeof(int) * rows * cols);
			memset(codeBuff, 0, sizeof(int) * qrcode_param.rows * qrcode_param.cols);
			for (int m = 0, m_max = posLinks.size(); m < m_max; m++)
			{
				int col = posLinks[m].pt2D.x;
				int row = posLinks[m].pt2D.y;
				codeBuff[row * cols + col] = 1;
			}
			//
			DMCodeInfo a_dmCode;
			a_dmCode.roi = _getClusterROI(a_cluster);
			a_dmCode.qrCode = codeBuff;
			qrCodeResult.push_back(a_dmCode);
		}
	}
	if (qrCodeResult.size() > 0)
	{
		SVzNL3DRangeD globalRoi = sg_getScanDataROI_vector(scanLines);
		//恢复二维码编码图像
		int dmNodeSize = 60;
		int dmLen = dmNodeSize * qrcode_param.cols;
		double scale = (qrCodeResult[0].roi.right - qrCodeResult[0].roi.left) / (double)dmLen;
		int skip = dmNodeSize;
		int img_cols = (int)((globalRoi.xRange.max - globalRoi.xRange.min) / scale);
		img_cols = ((img_cols + 3) / 4) * 4;  //4对齐
		int img_rows = (int)((globalRoi.yRange.max - globalRoi.yRange.min) / scale);
		img_rows = ((img_rows + 3) / 4) * 4;  //4对齐
		//加上空白边
		img_cols += skip * 2;
		img_rows += skip * 2;
		dmCodeImg = cv::Mat::ones(img_rows, img_cols, CV_8UC1); 
		for (int i = 0; i < qrCodeResult.size(); i++)
		{
			double x = qrCodeResult[i].roi.left;
			double y = qrCodeResult[i].roi.top;
			SVzNL2DPoint cornerPos;
			cornerPos.x = (int)((x-globalRoi.xRange.min) / scale) + skip;
			cornerPos.y = (int)((y-globalRoi.yRange.min) / scale) + skip;
			for (int row = 0; row < qrcode_param.rows; row++)
			{
				for (int col = 0; col < qrcode_param.cols; col++)
				{
					SVzNL2DPoint nodePos;
					nodePos.x = cornerPos.x + col * dmNodeSize;
					nodePos.y = cornerPos.y + row * dmNodeSize;
					if (qrCodeResult[i].qrCode[row * qrcode_param.cols + col] > 0)
					{
						for (int m = -dmNodeSize / 2; m < dmNodeSize / 2; m++)
						{
							for (int n = -dmNodeSize / 2; n < dmNodeSize / 2; n++)
								dmCodeImg.at<uchar>(nodePos.y + m, nodePos.x + n) = 0;
						}
					}
				}
			}
			free(qrCodeResult[i].qrCode);
			qrCodeResult[i].qrCode = NULL;
		}
	}
	return;
}