激光点云区域生长

算法流程

（1）选中的点被添加到名为种子的集合中。（2）对于每一个种子点，找到它的邻近点。（3）算出每个相邻点的法线和当前种子点的法线之间的角度，如果角度小于阈值，则将当前点添加到当前区域。（4）然后计算每个邻居点的曲率值，如果曲率小于阈值，那么这个点被添加到种子中（5）将当前的种子从种子列表中移除（6）如果种子列表变成空的，这意味着该区域生长已完成，继续重复上述过程。

#pragma once
#include "ApiConfig.h"
#include <vector>
#include <deque>
#include <Eigen/Core>
#include <numeric>
#include <cmath>
#include "KdTree.h"
#include "PointCloud.h"

namespace EV
{
	namespace PointCloudAnalysis
	{
		///区域生长
		class API RegionGrowing
		{
		public:
			explicit RegionGrowing(PointCloud* pcd_, float minCluster_, float maxCluster_, int neighbourNum_, int threshold_,
				float curvatureThreshold_) :m_cloud(pcd_), m_minPtsPerCluster(minCluster_), m_maxPtsPerCluste(maxCluster_),
				m_thetaThreshold(threshold_), m_neighbourNumber(neighbourNum_) {};

			~RegionGrowing();

		public: 
			std::vector<std::vector<size_t>> Extract();
			
		private:
			void RegionGrowingProcess();

			void RegionGrowingClusters();

			int LabelForPoints(int initialSeed, int segmentNumber);

			void FindNeighbourPoints();

			std::pair<bool, bool> ValidatePoints(int point,int nebor);

		private:
			PointCloud* m_cloud;

			std::vector<float> m_cure;

			int m_minPtsPerCluster = 1;  //每个聚类的最小点数
			int m_maxPtsPerCluste = std::numeric_limits<int>::max();   // 每个聚类的最大点数
			float m_thetaThreshold = 30;  //法向量夹角阈值
			float m_curvatureThreshold = 0.05; //曲率阈值
			int m_neighbourNumber = 30; //邻域搜索点数

			std::vector<std::vector<int>> m_pointNeighbours; //近邻点集合
			std::vector<int> m_pointLabels; //点标签
			std::vector<int> m_numPtsInSegment; //分类标签

			std::vector<std::vector<size_t>> m_clusters; // 聚类容器

			int m_numberOfSegments = 0; //聚类个数

		};
	}
}

#include "RegionGrowing.h"

namespace EV 
{
	namespace PointCloudAnalysis
	{

		RegionGrowing::~RegionGrowing()
		{
		}

		void RegionGrowing::RegionGrowingClusters()
		{
			int numberOfSegments = m_numPtsInSegment.size();
			int numberOfPoints = m_cloud->m_points.size();

			m_clusters.resize(numberOfSegments);
			for (size_t i = 0; i < numberOfSegments; i++)
			{
				m_clusters[i].resize(m_numPtsInSegment[i], 0);
			}

			std::vector<int> counter;
			counter.resize(numberOfSegments, 0);
			for (size_t iPoint = 0; iPoint < numberOfPoints; iPoint++)
			{
				int segmentIndex = m_pointLabels[iPoint];
				if (segmentIndex != -1)
				{
					int pointIndex = counter[segmentIndex];
					m_clusters[segmentIndex][pointIndex] = iPoint;
					counter[segmentIndex] = pointIndex + 1;

				}
			}
			m_numberOfSegments = numberOfSegments;

		}

		std::vector<std::vector<size_t>> RegionGrowing::Extract()
		{
			if (m_cloud == nullptr)
				return std::vector<std::vector<size_t>>{};
			if (m_cloud->m_points.empty())
				return std::vector<std::vector<size_t>>{};

			FindNeighbourPoints();
			RegionGrowingProcess();
			RegionGrowingClusters();

			std::vector<std::vector<size_t>> allCluster;
			for (size_t i = 0; i < m_clusters.size(); i++)
			{
				if (m_minPtsPerCluster <= m_clusters[i].size() <= m_maxPtsPerCluste)
					allCluster.push_back(m_clusters[i]);
				else
				{
					for (const int& index: allCluster[i])
					{
						m_pointLabels[index] = -1;
					}
				}
			}
			m_clusters = allCluster;
			return allCluster;
		}

		void RegionGrowing::RegionGrowingProcess()
		{
			if (m_cloud->m_points.empty())
				return;

			int numOfPts = m_cloud->m_points.size();

			m_pointLabels.resize(numOfPts, -1);

			if (!m_cloud->HasNormals())
			{
				m_cloud->EstimateNormals();
			}

			std::vector<Eigen::Matrix3d> cov_mat = m_cloud->m_covariances;

			m_cure.resize(numOfPts, 0.0);
			for (size_t i = 0; i < numOfPts; i++)
			{
				Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> es;  //矩阵特征值和特征向量
				es.compute(cov_mat[i]);
				//Eigen::JacobiSVD<Eigen::Matrix3Xd> svd(cov_mat[i], Eigen::ComputeThinU | Eigen::ComputeThinV);

				/*Eigen::MatrixXd U = svd.matrixU();
				Eigen::MatrixXd V = svd.matrixV();
				Eigen::VectorXd S = svd.singularValues();*/

				//const auto& eVec = es.eigenvectors();   //特征向量
				const auto& eValue = es.eigenvalues(); 
				//Eigen::Vector3d lsPlaneVectors = -eVec.col(0); 
				/*std::cout << "U" << U << std::endl;
				std::cout << "V" << U << std::endl;
				std::cout << "S" << U << std::endl;*/
				m_cure[i] = eValue(0) / (eValue(0) + eValue(1) + eValue(2));
			}

			std::vector<std::pair<float, int>> pointCurvatureIndex;
			pointCurvatureIndex.resize(numOfPts);
			for (size_t i = 0; i < numOfPts; i++)
			{
				pointCurvatureIndex[i] = { m_cure[i], i };
			}

			//曲率大小排序
			std::sort(pointCurvatureIndex.begin(), pointCurvatureIndex.end(), [](std::pair<float, int>& a, std::pair<float, int>& b)
			{
				return a.first < b.first;
			});

			int seed = pointCurvatureIndex.front().second;
			int segmentedPtsNum = 0;
			int numberOfSegments = 0;

			int seedCounter = 0;
			while (segmentedPtsNum < numOfPts)
			{
				int ptsInSegment = LabelForPoints(seed, numberOfSegments);
				segmentedPtsNum += ptsInSegment;

				m_numPtsInSegment.push_back(ptsInSegment);
				numberOfSegments++;

				for (size_t iSeed = seedCounter + 1; iSeed < numOfPts; iSeed++)
				{
					int index = pointCurvatureIndex[iSeed].second;
					if (m_pointLabels[index] == -1)
					{
						seed = index;
						seedCounter = iSeed;
						break;
					}
				}
			}
		}

		std::pair<bool, bool> RegionGrowing::ValidatePoints(int point, int nebor)
		{
			double cosineThreshold = std::cos(m_thetaThreshold * (3.1415926 / 180));

			Eigen::Vector3d currSeedNormal = m_cloud->m_normals[point];      // 当前种子点的法向量
			Eigen::Vector3d seedNeborNormal = m_cloud->m_normals[nebor];    // 种子点邻域点的法向量
			double dotNormal = std::fabs(seedNeborNormal.dot(currSeedNormal));

			bool isSeed = true;
			if (dotNormal < cosineThreshold)
				return {false, isSeed };

			if (m_cure[nebor] > m_curvatureThreshold)
				isSeed = false;

			return {true,isSeed };
		}

		void RegionGrowing::FindNeighbourPoints()
		{
			EV::PointCloudAnalysis::KdTree kdtree;
			kdtree.SetGeometry(*m_cloud);

			try
			{
				m_pointNeighbours.resize(m_cloud->m_points.size());
			}
			catch (const std::bad_alloc&)
			{
				return;
			}
			
			for (size_t i = 0; i < m_cloud->m_points.size(); i++)
			{
				std::vector<int> indices(m_neighbourNumber); //索引
				std::vector<double> dists(m_neighbourNumber); //距离平方
				if (kdtree.SearchKNN(m_cloud->m_points[i], m_neighbourNumber, indices, dists))
				{
					m_pointNeighbours[i] = indices;
				}
			}
		}


		int RegionGrowing::LabelForPoints(int initialSeed, int segmentNumber)
		{
			std::deque<int> seeds{ initialSeed };
			m_pointLabels[initialSeed] = segmentNumber;
			int numPtsInSegment = 1;
			
			while (seeds.size())
			{
				int currSeed = seeds[0];
				seeds.pop_front();
				int iNebor = 0;

				while (iNebor < m_neighbourNumber && iNebor < m_pointNeighbours[currSeed].size())
				{
					int index = m_pointNeighbours[currSeed][iNebor];
					if (m_pointLabels[index] != -1)
					{
						iNebor++;
						continue;
					}
					auto state = ValidatePoints(currSeed, index);
					if (!state.first)
					{
						iNebor++;
						continue;
					}

					m_pointLabels[index] = segmentNumber;
					numPtsInSegment++;
					if (state.second)
						seeds.push_back(index);

					iNebor++;
				}
				
			}

			return numPtsInSegment;
		}
	}
}