大致思路:定义起始种子点后通过当前点k邻域搜索最邻近点,调整邻近点法线朝向与当前种子点法线朝向同向,然后传播出去。
具体实现:(1)用vector模拟两个栈points和normals,存放点云的点和法线。首先将初始种子点压入这两个栈。
(2)设置一个和点云点数相同大小的flags用来标记点云中的点是否被生长过,并将初始种子点的标志位置为true。
(3)建立点云的kd树。
(4)分别从points和normals弹出最后一个值作为当前种子点,计算该点对应的法线,并求出其k邻域点。
(5)遍历该点的k邻域点,若该点未被标记过,判断该点与当前种子点法线朝向是否同向,如果反向则翻转法线方向。将该点压入栈中并将该点的标志位置为true。
(6)返回(4),直到栈为空。
/**
* @brief normal_flip 法线方向翻转
* @param cloud 点云
* @param cloud_normals 点云法线
* @param seed_index 种子点索引
*/
void normal_flip(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::Normal>::Ptr& cloud_normals, int seed_index)
{
std::vector<pcl::PointXYZ> points;
std::vector<pcl::Normal> normals;
points.push_back(cloud->points[seed_index]);
normals.push_back(cloud_normals->points[seed_index]);
std::vector<bool> flags(cloud->size(), false); //用来标记点云中的点是否被生长过
flags[seed_index] = true;
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
kdtree.setInputCloud(cloud);
int K = 20;
std::vector<int> pointsIdx(K); //索引
std::vector<float> pointsDistance(K); //距离
while (!normals.empty())
{
pcl::PointXYZ seed_point = points.back(); //种子点
pcl::Normal seed_normal = normals.back(); //种子点法线
points.pop_back();
normals.pop_back();
kdtree.nearestKSearch(seed_point, K, pointsIdx, pointsDistance); //k近邻搜索
Eigen::Vector3f v1(seed_normal.normal_x, seed_normal.normal_y, seed_normal.normal_z);
for (size_t i = 0; i < pointsIdx.size(); i++)
{
if (!flags[pointsIdx[i]]) //如果该点没有被生长到
{
Eigen::Vector3f v2(cloud_normals->points[pointsIdx[i]].normal_x,
cloud_normals->points[pointsIdx[i]].normal_y,
cloud_normals->points[pointsIdx[i]].normal_z);
if (v1.dot(v2) < 0) //如果该点法线方向与种子点法线方向相反(夹角为钝角),则翻转法线方向
{
cloud_normals->points[pointsIdx[i]].normal_x *= -1;
cloud_normals->points[pointsIdx[i]].normal_y *= -1;
cloud_normals->points[pointsIdx[i]].normal_z *= -1;
}
points.push_back(cloud->points[pointsIdx[i]]);
normals.push_back(cloud_normals->points[pointsIdx[i]]);
flags[pointsIdx[i]] = true; //标记该点已经被生长过
}
}
}
//std::vector<int> indexs;
//for (size_t i = 0; i < flags.size(); ++i)
//{
// if (flags[i])
// indexs.push_back(i);
//}
//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_flag(new pcl::PointCloud<pcl::PointXYZ>);
//pcl::copyPointCloud(*cloud, indexs, *cloud_flag);
//pcl::io::savePCDFile("cloud_flag.pcd", *cloud_flag);
}
效果展示:
pcl法线提取结果(法线朝向有内有外)
统一法线方向结果(法线朝向都朝外侧)
参考:无序点云的法线全局定向