二维格网聚类.md

将三维点云投影到XOZ或YOZ平面进行二维格网划分后聚类

作者水平有限,随便看看吧

/**
	* findGirdRound 遍历点云,计算二维格网的范围
  * @pt 为当前遍历点
  * @minX x轴上点云最小x值; maxX x轴上点云最大x值 其他同理
  */
bool findGirdRound(const PointCoordinateType *pt, float *minX, float *minY, float minZ,
			float *maxX, float *maxY, float *maxZ)
{
	if (pt[2] > minZ)
	{
		*minX = *minX > pt[0] ? pt[0] : *minX;
		*minY = *minY > pt[1] ? pt[1] : *minY;
		*maxX = *maxX < pt[0] ? pt[0] : *maxX;
		*maxY = *maxY < pt[1] ? pt[1] : *maxY;
		*maxZ = *maxZ < pt[2] ? pt[2] : *maxZ;
	}
	return true;
}
 
/**
	* findGirdRound 遍历点云,计算二维格网
	* @pt 为当前遍历点
	* @minX x轴上点云最小x值 maxX x轴上点云最大x值 其他同理
	* @mViewForward 为当前GL窗口的前视图向量
	*	@gridPoint 二维格网的数据结构
	* @rowNum,colNum二维格网行列值
  */
bool createGirdForeach(const PointCoordinateType *pt, 
            QMultiHash<int, Vector3> *gridPoint, 
			Vector3 mViewForward, float minX, float minY, 
			float minZ, int rowNum, int colNum)
{
	if (pt[2] > minZ) {
		Vector3 Pt3d;
		Pt3d.x = pt[0];
		Pt3d.y = pt[1];
		Pt3d.z = pt[2];
 
		//计算点云在二维格网里的行、列索引
		int Row2d = int((Pt3d.z - minZ) / GRID_SIZE) + 1;
		int Col2d;
		if (abs(mViewForward.x) > abs(mViewForward.y))
		{//如果前视角向量靠近X轴 ,则在YOZ平面上格网划分
			Col2d = int((Pt3d.y - minY) / GRID_SIZE) + 1;
		}
		else if (abs(mViewForward.x) < abs(mViewForward.y))
		{//如果前视角向量靠近Y轴 ,则在XOZ平面上格网划分
			Col2d = int((Pt3d.x - minX) / GRID_SIZE) + 1;
		}
		//计算出二维格网项的索引
		int index2d = (Row2d - 1)*colNum + Col2d;
		gridPoint->insert(index2d, Pt3d);
	}
	return true;
}
 
//二维格网聚类
void TreeGridCluster2D(QMultiHash<int, Vector3> Grid, QMultiHash<int, Vector3>* result,
			int rowNum, int colNum)
{
	//非空格网id
	QList<int> grid2dIdList;
	for (QMultiHash<int, Vector3>::iterator iter = Grid.begin(); iter != Grid.end(); iter++)
	{
		if (!grid2dIdList.contains(iter.key()))
		{
			grid2dIdList.append(iter.key());
		}
	}
	//存放所有的聚类体
	QList<QList<int>> ClusterList;
 
	//遍历非空格网
	for (QMultiHash<int, Vector3>::iterator iter = Grid.begin(); iter != Grid.end(); iter++)
	{
		//判断是否已有聚类体包含当前格网
		bool isContain = false;
		for (int iClusteres = 0; iClusteres < ClusterList.size(); iClusteres++)
		{
			if (ClusterList.at(iClusteres).contains(iter.key()))
			{
				isContain = true;
				break;
			}
		}
		if (isContain)
		{//如果包含 则跳过
			continue;
		}
		//当前聚类体
		QList<int> tempCluster;
		//当前遍历的格网当作 当前聚类体的种子格网
		tempCluster.append(iter.key());
		for (int iTmp = 0; iTmp < tempCluster.size(); ++iTmp)
		{
			//当前中心格网
			int mobileCenterId = tempCluster.at(iTmp);
			//计算中心格网行列
			int Rid = mobileCenterId / colNum + 1;
			int Cid = mobileCenterId % colNum;
			if (Cid == 0)
			{
				Rid = Rid - 1;
				Cid = colNum;
			}
			//暂存中心格网的非空邻域格网id
			QList<int> tempId;
			for (int r = Rid - 1; r <= Rid + 1; ++r)
			{
				for (int c = Cid - 1; c <= Cid + 1; ++c)
				{
					//计算当前邻域格网id
					int gridid = (r - 1)* colNum + c;
					//是否已有聚类体包含当前邻域格网
					bool isContainTmp = false;
					for (int iClusteres = 0; iClusteres < ClusterList.size(); iClusteres++)
					{
						if (ClusterList.at(iClusteres).contains(gridid))
						{
							isContainTmp = true;
							break;
						}
					}
					if (grid2dIdList.contains(gridid) && (!tempCluster.contains(gridid)) && !isContainTmp)
					{
						tempId.append(gridid);
					}
				}
			}
			//满足聚类条件(中心格网的非空邻域格网数>=3),则添加到 当前聚类体
			if (tempId.size() > 2)
			{
				for (int i = 0; i < tempId.size(); i++)
				{
					tempCluster.append(tempId.at(i));
					grid2dIdList.removeOne(tempId.at(i));
				}
			}
		}
		//把当前聚类体追加到已有聚类体List中
		ClusterList.append(tempCluster);
	}
	if (ClusterList.size()==0)
	{
		return;
	}
	//找到格网数量最大的聚类体
	int maxClusterIndex = 0;
	int maxClusterSize = ClusterList.at(0).size();
 
	for (int i = 1; i < ClusterList.size(); i++)
	{
		if (ClusterList.at(i).size() > maxClusterSize)
		{
			maxClusterSize = ClusterList.at(i).size();
			maxClusterIndex = i;
		}
	}
	//返回结果集
	for (int n = 0; n < ClusterList.at(maxClusterIndex).size(); ++n)
	{
		QList<Vector3>& list = Grid.values(ClusterList.at(maxClusterIndex).at(n));
		for (int m = 0; m < list.size(); ++m)
		{
			result->insert(ClusterList.at(maxClusterIndex).at(n), list.at(m));
		}
	}
}
 
void run()
{
	float minX = 9999, minY = 9999, maxX = -9999, maxY = -9999, maxZ = -9999;
 
	xs3d::pointAction findGrid = std::bind(findGirdRound, std::placeholders::_1, &minX, &minY, branchPoint.z, &maxX, &maxY, &maxZ);
	vRef[0]->forEach(findGrid, false);
 
	QMultiHash<int,Vector3> Grid;
	//判断是xoz还是yoz
	int colNum = 0, rowNum = (int)((maxZ - branchPoint.z) / GRID_SIZE) + 1;
	if (abs(mViewForwardBox.x)>abs(mViewForwardBox.y ))
	{
		colNum = int((maxY - minY) / GRID_SIZE) + 1;
	}
	else if (abs(mViewForwardBox.x) < abs(mViewForwardBox.y))
	{
		colNum = int((maxX - minX) / GRID_SIZE) + 1;
	}
	//二维格网划分
	xs3d::pointAction createGrid = std::bind(createGirdForeach, std::placeholders::_1, &Grid, mViewForwardBox, minX, minY, branchPoint.z, rowNum, colNum);
	vRef[0]->forEach(createGrid, false);
	//结果集
	QMultiHash<int, Vector3> result;
	TreeGridCluster2D(Grid, &result, rowNum, colNum);
}