algoLib/sourceCode/SG_clustering.cpp

#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include <vector>
#ifdef _WIN32
#include <corecrt_math_defines.h>
#endif
#include <cmath>

void _seedClustering(
	std::vector< SVzNL3DPosition>& a_cluster,
	std::vector< SVzNL3DPosition>& pts,
	std::vector<int> flags,
	double clusterDist)
{
	int i = 0;
	while (1)
	{
		if (i >= a_cluster.size())
			break;

		SVzNL3DPosition a_seed = a_cluster[i];
		for (int j = 0, j_max = (int)pts.size(); j < j_max; j++)
		{
			if (flags[j] < 0)
				continue;
			double dist_x = abs(a_seed.pt3D.x - pts[j].pt3D.x);
			double dist_y = abs(a_seed.pt3D.y - pts[j].pt3D.y);
			if ( (dist_x < clusterDist) && (dist_y < clusterDist))
			{
				a_cluster.push_back(pts[j]);
				flags[j] = -1;
			}
		}
		i++;
	}
	return;
}

void _seedClustering_speedUp(
	std::vector< SVzNL3DPosition>& a_cluster,
	std::vector< SVzNL3DPosition>& pts,
	std::vector<int>& flags,
	double clusterDist,
	std::vector<std::vector<int>>& indexing2D,
	int lineNum, int ptSize, int clusterCheckWin) //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
	int i = 0;
	while (1)
	{
		if (i >= a_cluster.size())
			break;

		SVzNL3DPosition a_seed = a_cluster[i];
		int seed_line = a_seed.nPointIdx >> 16;
		int seed_ptIdx =a_seed.nPointIdx & 0x0000FFFF;
		for (int line = seed_line - clusterCheckWin; line <= seed_line + clusterCheckWin; line++)
		{
			if ((line >= 0) && (line < lineNum))
			{
				for (int ptIdx = seed_ptIdx - clusterCheckWin; ptIdx <= seed_ptIdx + clusterCheckWin; ptIdx++)
				{
					if ((ptIdx >= 0) && (ptIdx < ptSize))
					{
						int backIdx = indexing2D[line][ptIdx];
						if (backIdx < 0)
							continue;
						if (flags[backIdx] < 0)
							continue;
						double dist = sqrt(pow(a_seed.pt3D.x - pts[backIdx].pt3D.x,2) + pow(a_seed.pt3D.y - pts[backIdx].pt3D.y,2));
						if (dist < clusterDist) 
						{
							a_cluster.push_back(pts[backIdx]);
							flags[backIdx] = -1;
						}
					}
				}
			}
		}
		i++;
	}
	return;
}

void sg_pointClustering(
	std::vector< SVzNL3DPosition>& pts, 
	double clusterDist, 
	std::vector<std::vector< SVzNL3DPosition>>& objClusters  //result
)
{
	int ptSize = (int)pts.size();
	if (ptSize == 0)
		return;

	std::vector<int> flags;
	flags.resize(ptSize);
	std::fill(flags.begin(), flags.end(), 0);

	while(pts.size() > 0)
	{
		SVzNL3DPosition a_pt = pts[0];
		flags[0] = -1;//<2F><>ֹ<EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		//<2F>½<EFBFBD>һ<EFBFBD><D2BB>cluster
		std::vector< SVzNL3DPosition> a_cluster;
		a_cluster.push_back(a_pt);
		//pts[0].nPointIdx = -1; 
		_seedClustering(
			a_cluster,
			pts,
			flags,
			clusterDist);
		objClusters.push_back(a_cluster); //<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		//<2F><>pts<74>д<EFBFBD><D0B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD>ȥ<EFBFBD><C8A5>
		int m_max = (int)pts.size();
		for (int m = m_max - 1; m >= 0; m--)  //<2F>Ӻ<EFBFBD><D3BA><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɾ<EFBFBD><C9BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӱ<EFBFBD><D3B0>ѭ<EFBFBD><D1AD>
		{
			if (flags[m] < 0)
			{
				pts.erase(pts.begin() + m);
				flags.erase(flags.begin() + m);
			}
		}
	}
	return;
}

//ʹ<><CAB9>SVzNL3DPosition<6F><6E>nPointIdx<64><78>ʾ2D<32><44>Ϣ<EFBFBD><CFA2><EFBFBD><EFBFBD>16λLine<6E><65> <20><>16λptIdx<64><78>
//<2F><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD><D4BC><EFBFBD>
void wd_pointClustering_speedUp(
	std::vector< SVzNL3DPosition>& pts,
	int lineNum, int linePtSize, int clusterCheckWin, //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	double clusterDist,
	std::vector<std::vector< SVzNL3DPosition>>& objClusters  //result
)
{
	std::vector<std::vector<int>> indexing2D;
	indexing2D.resize(lineNum);
	for (int i = 0; i < lineNum; i++)
	{
		indexing2D[i].resize(linePtSize);
		std::fill(indexing2D[i].begin(), indexing2D[i].end(), -1);
	}
	for (int i = 0; i < (int)pts.size(); i++)
	{
		int line = pts[i].nPointIdx >> 16;
		int ptIdx = pts[i].nPointIdx & 0x0000FFFF;
		indexing2D[line][ptIdx] = i;
	}

	int ptSize = (int)pts.size();
	if (ptSize == 0)
		return;

	std::vector<int> flags;
	flags.resize(ptSize);
	std::fill(flags.begin(), flags.end(), 0);

	int idx = 0;
	while (idx < (int)pts.size())
	{
		SVzNL3DPosition a_pt = pts[idx];
		if (flags[idx] >= 0)
		{
			flags[idx] = -1;//<2F><>ֹ<EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
			//<2F>½<EFBFBD>һ<EFBFBD><D2BB>cluster
			std::vector< SVzNL3DPosition> a_cluster;
			a_cluster.push_back(a_pt);
			//pts[0].nPointIdx = -1; 
			_seedClustering_speedUp(
				a_cluster,
				pts,
				flags,
				clusterDist,
				indexing2D,
				lineNum, linePtSize, clusterCheckWin);
			objClusters.push_back(a_cluster); //<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		}
		idx++;
	}
	return;
}

//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><C4BE><EFBFBD>
void wd_gridPointClustering(
	std::vector<std::vector<SSG_featureClusteringInfo>>& featureMask,
	std::vector<std::vector<SVzNL3DPoint>>& feature3DInfo,
	int clusterCheckWin, //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	SSG_treeGrowParam growParam,//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	int clusterID, //<2F><>ǰCluster<65><72>ID
	std::vector< SVzNL2DPoint>& a_cluster,  //result
	SVzNL3DRangeD& clusterRoi  //roi3D
)
{
	int i = 0;
	int lineNum = (int)featureMask.size();
	int linePtNum = (int)featureMask[0].size();
	featureMask[a_cluster[0].x][a_cluster[0].y].flag = 1;  //<2F><>ֹ<EFBFBD><D6B9>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD>
	while (1)
	{
		if (i >= a_cluster.size())
			break;

		SVzNL2DPoint a_seedPos = a_cluster[i];
		if ((a_seedPos.x == 752) && (a_seedPos.y == 2399))
			int kkk = 1;

		SSG_featureClusteringInfo& a_seed = featureMask[a_seedPos.x][a_seedPos.y];
		SVzNL3DPoint& seedValue = feature3DInfo[a_seedPos.x][a_seedPos.y];
		if (0 == a_seed.clusterID)  //clusterID == 0, δ<><CEB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		{
			for (int i = -clusterCheckWin; i <= clusterCheckWin; i++)
			{
				for (int j = -clusterCheckWin; j <= clusterCheckWin; j++)
				{
					int y = j + a_seedPos.y;
					int x = i + a_seedPos.x;
					if ((x == 645) && (y == 2240))
						int kkk = 1;

					if ((x >= 0) && (x < lineNum) && (y >= 0) && (y < linePtNum))
					{
						SSG_featureClusteringInfo& chk_seed = featureMask[x][y];
						if ((chk_seed.featurType ==0) || (chk_seed.clusterID > 0))  //ֻ<><D6BB><EFBFBD><EFBFBD>δ<EFBFBD><CEB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
							continue;

						SVzNL3DPoint& chkValue = feature3DInfo[x][y];
						double y_diff = abs(seedValue.y - chkValue.y);
						double z_diff = abs(seedValue.z - chkValue.z);
						double x_diff = abs(seedValue.x - chkValue.x);
						if ((y_diff < growParam.yDeviation_max) && (z_diff < growParam.zDeviation_max) &&
							(x_diff < growParam.maxSkipDistance))
						{
							if (0 == chk_seed.flag)//<2F><>ֹ<EFBFBD><D6B9><EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD>
							{
								chk_seed.flag = 1;
								SVzNL2DPoint new_seed = { x, y };
								a_cluster.push_back(new_seed);
							}
						}
					}

				}
			}
		}
		a_seed.clusterID = clusterID;
		//<2F><><EFBFBD><EFBFBD>ROI
		clusterRoi.xRange.min = clusterRoi.xRange.min > seedValue.x ? seedValue.x : clusterRoi.xRange.min;
		clusterRoi.xRange.max = clusterRoi.xRange.max < seedValue.x ? seedValue.x : clusterRoi.xRange.max;
		clusterRoi.yRange.min = clusterRoi.yRange.min > seedValue.y ? seedValue.y : clusterRoi.yRange.min;
		clusterRoi.yRange.max = clusterRoi.yRange.max < seedValue.y ? seedValue.y : clusterRoi.yRange.max;
		clusterRoi.zRange.min = clusterRoi.zRange.min > seedValue.z ? seedValue.z : clusterRoi.zRange.min;
		clusterRoi.zRange.max = clusterRoi.zRange.max < seedValue.z ? seedValue.z : clusterRoi.zRange.max;
		i++;
	}
	return;
}

//<2F><>ɨ<EFBFBD><C9A8><EFBFBD><EFBFBD><EFBFBD>ݵľ<DDB5><C4BE><EFBFBD>
//SSG_intPair<69><72>Ա<EFBFBD><D4B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: 
//		data_0: flag
//		data_1: valid point flag
//		idx: cluster ID
void wd_gridPointClustering_2(
	std::vector<std::vector<SVzNL3DPosition>>& gridData,
	std::vector<std::vector<SSG_intPair>>& pointMaskInfo, //<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD>ʱ<EFBFBD>ظ<EFBFBD>ѡ<EFBFBD><D1A1>
	int clusterCheckWin, //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	SSG_treeGrowParam growParam,//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	int clusterID, //<2F><>ǰCluster<65><72>ID
	std::vector< SVzNL2DPoint>& a_cluster,  //result
	SVzNL3DRangeD& clusterRoi  //roi3D
)
{
	int i = 0;
	int lineNum = (int)gridData.size();
	int linePtNum = (int)gridData[0].size();
	pointMaskInfo[a_cluster[0].x][a_cluster[0].y].data_0 = 1;  //<2F><>ֹ<EFBFBD><D6B9>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD>
	while (1)
	{
		if (i >= a_cluster.size())
			break;

		SVzNL2DPoint a_seedPos = a_cluster[i];
		if ((a_seedPos.x == 390) && (a_seedPos.y == 949))
			int kkk = 1;

		SSG_intPair& a_seed = pointMaskInfo[a_seedPos.x][a_seedPos.y];
		SVzNL3DPosition& seedValue = gridData[a_seedPos.x][a_seedPos.y];
		if (0 == a_seed.idx)  //clusterID == 0, δ<><CEB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		{
			for (int i = -clusterCheckWin; i <= clusterCheckWin; i++)
			{
				for (int j = -clusterCheckWin; j <= clusterCheckWin; j++)
				{
					int y = j + a_seedPos.y;
					int x = i + a_seedPos.x;
					if ((x == 390) && (y == 949))
						int kkk = 1;

					if ((x >= 0) && (x < lineNum) && (y >= 0) && (y < linePtNum))
					{
						SSG_intPair& chk_seed = pointMaskInfo[x][y];
						if ((chk_seed.data_1 == 0) || (chk_seed.idx > 0))  //idx:clusterID, data_1:valid point
							continue;

						SVzNL3DPosition& chkValue = gridData[x][y];
						double y_diff = abs(seedValue.pt3D.y - chkValue.pt3D.y);
						double z_diff = abs(seedValue.pt3D.z - chkValue.pt3D.z);
						double x_diff = abs(seedValue.pt3D.x - chkValue.pt3D.x);
						if ((y_diff < growParam.yDeviation_max) && (z_diff < growParam.zDeviation_max) &&
							(x_diff < growParam.maxSkipDistance))
						{
							if (0 == chk_seed.data_0)//<2F><>ֹ<EFBFBD><D6B9><EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD>
							{
								chk_seed.data_0 = 1;
								SVzNL2DPoint new_seed = { x, y };
								a_cluster.push_back(new_seed);
							}
						}
					}

				}
			}
		}
		a_seed.idx = clusterID;
		//<2F><><EFBFBD><EFBFBD>ROI
		clusterRoi.xRange.min = clusterRoi.xRange.min > seedValue.pt3D.x ? seedValue.pt3D.x : clusterRoi.xRange.min;
		clusterRoi.xRange.max = clusterRoi.xRange.max < seedValue.pt3D.x ? seedValue.pt3D.x : clusterRoi.xRange.max;
		clusterRoi.yRange.min = clusterRoi.yRange.min > seedValue.pt3D.y ? seedValue.pt3D.y : clusterRoi.yRange.min;
		clusterRoi.yRange.max = clusterRoi.yRange.max < seedValue.pt3D.y ? seedValue.pt3D.y : clusterRoi.yRange.max;
		clusterRoi.zRange.min = clusterRoi.zRange.min > seedValue.pt3D.z ? seedValue.pt3D.z : clusterRoi.zRange.min;
		clusterRoi.zRange.max = clusterRoi.zRange.max < seedValue.pt3D.z ? seedValue.pt3D.z : clusterRoi.zRange.max;
		i++;
	}
	return;
}

//ʹ<>þ<EFBFBD><C3BE>෽<EFBFBD><E0B7BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>8<EFBFBD><38>ͨ<EFBFBD><CDA8>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void wd_gridPointClustering_labelling(
	std::vector<std::vector<SSG_featureClusteringInfo>>& featureMask,
	std::vector<std::vector<SVzNL3DPoint>>& feature3DInfo,
	int clusterID, //<2F><>ǰCluster<65><72>ID
	std::vector< SVzNL2DPoint>& a_cluster,  //result
	SVzNLRect& clusterRoi  //roi2D
)
{
	int i = 0;
	int lineNum = (int)featureMask.size();
	int linePtNum = (int)featureMask[0].size();
	while (1)
	{
		if (i >= a_cluster.size())
			break;

		SVzNL2DPoint a_seedPos = a_cluster[i];
		if ((a_seedPos.x == 390) && (a_seedPos.y == 949))
			int kkk = 1;

		SSG_featureClusteringInfo& a_seed = featureMask[a_seedPos.x][a_seedPos.y];
		if (0 == a_seed.clusterID)  //clusterID == 0, δ<><CEB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		{
			//8<><38>ͨ
			for (int i = -1; i <= 1; i++)
			{
				for (int j = -1; j <= 1; j++)
				{
					int y = j + a_seedPos.y;
					int x = i + a_seedPos.x;
					if ((x == 390) && (y == 949))
						int kkk = 1;

					if ((x >= 0) && (x < lineNum) && (y >= 0) && (y < linePtNum))
					{
						SSG_featureClusteringInfo& chk_seed = featureMask[x][y];
						if ((chk_seed.featurType == 0) || (chk_seed.clusterID > 0))  //ֻ<><D6BB><EFBFBD><EFBFBD>δ<EFBFBD><CEB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
							continue;

						if (0 == chk_seed.flag)//<2F><>ֹ<EFBFBD><D6B9><EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD>
						{
							chk_seed.flag = 1;
							SVzNL2DPoint new_seed = { x, y };
							a_cluster.push_back(new_seed);
						}
					}

				}
			}
		}
		a_seed.clusterID = clusterID;
		//<2F><><EFBFBD><EFBFBD>ROI
		clusterRoi.left = clusterRoi.left > a_seedPos.x ? a_seedPos.x : clusterRoi.left;
		clusterRoi.right = clusterRoi.right < a_seedPos.x ? a_seedPos.x : clusterRoi.right;
		clusterRoi.top = clusterRoi.top > a_seedPos.y ? a_seedPos.y : clusterRoi.top;
		clusterRoi.bottom = clusterRoi.bottom < a_seedPos.y ? a_seedPos.y : clusterRoi.bottom;
		i++;
	}
	return;
}

//ʹ<>þ<EFBFBD><C3BE>෽<EFBFBD><E0B7BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>8<EFBFBD><38>ͨ<EFBFBD><CDA8>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void wd_gridPointClustering_labelling_2(
	std::vector<std::vector<SVzNL3DPoint>>& srcData,
	std::vector<std::vector<SSG_clusterLabel>>& labelMask,
	int clusterID, //<2F><>ǰCluster<65><72>ID
	std::vector< SVzNL2DPoint>& a_cluster,  //result
	SVzNL3DRangeD& clusterRoi  //roi3D
)
{
	int i = 0;
	int lineNum = (int)srcData.size();
	int linePtNum = (int)srcData[0].size();

	SVzNL2DPoint& inputSeed = a_cluster[0];
	labelMask[inputSeed.x][inputSeed.y].flag = 1; //<2F><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	while (1)
	{
		if (i >= a_cluster.size())
			break;

		SVzNL2DPoint a_seedPos = a_cluster[i];
		if ((a_seedPos.x == 390) && (a_seedPos.y == 949))
			int kkk = 1;

		SSG_clusterLabel& a_seed = labelMask[a_seedPos.x][a_seedPos.y];
		SVzNL3DPoint& a_feature3DValue = srcData[a_seedPos.x][a_seedPos.y];
		if (0 == a_seed.clusterID)  //clusterID == 0, δ<><CEB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		{
			//8<><38>ͨ
			for (int i = -1; i <= 1; i++)
			{
				for (int j = -1; j <= 1; j++)
				{
					int y = j + a_seedPos.y;
					int x = i + a_seedPos.x;
					if ((x == 390) && (y == 949))
						int kkk = 1;

					if ((x >= 0) && (x < lineNum) && (y >= 0) && (y < linePtNum))
					{
						SSG_clusterLabel& chk_seed = labelMask[x][y];
						if ((chk_seed.validFlag == 0) || (chk_seed.clusterID > 0))  //ֻ<><D6BB><EFBFBD><EFBFBD>δ<EFBFBD><CEB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
							continue;

						if (0 == chk_seed.flag)//<2F><>ֹ<EFBFBD><D6B9><EFBFBD>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD><EFBFBD>
						{
							chk_seed.flag = 1;
							SVzNL2DPoint new_seed = { x, y };
							a_cluster.push_back(new_seed);
						}
					}

				}
			}
		}
		a_seed.clusterID = clusterID;
		//<2F><><EFBFBD><EFBFBD>ROI
		clusterRoi.xRange.min = clusterRoi.xRange.min > a_feature3DValue.x ? a_feature3DValue.x : clusterRoi.xRange.min;
		clusterRoi.xRange.max = clusterRoi.xRange.max < a_feature3DValue.x ? a_feature3DValue.x : clusterRoi.xRange.max;
		clusterRoi.yRange.min = clusterRoi.yRange.min > a_feature3DValue.y ? a_feature3DValue.y : clusterRoi.yRange.min;
		clusterRoi.yRange.max = clusterRoi.yRange.max < a_feature3DValue.y ? a_feature3DValue.y : clusterRoi.yRange.max;
		clusterRoi.zRange.min = clusterRoi.zRange.min > a_feature3DValue.z ? a_feature3DValue.z : clusterRoi.zRange.min;
		clusterRoi.zRange.max = clusterRoi.zRange.max < a_feature3DValue.z ? a_feature3DValue.z : clusterRoi.zRange.max;
		i++;
	}
	return;
}