算法流程
(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;
}
}
}