alpha-shape计算二维点云边界--c++实现
alpha-shape原理看过很久了,今天花点时间实现一下:原理参考:博客平面点云边界提取算法研究[D].长沙理工大学,2017.《综合机载与车载LiDAR数据的建筑物三维重建》注意:个人原创,转载注明出处,如果感觉有帮助,点个赞,有疑问,欢迎留言交流。show the codes:// ashape.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。//#include&l
·
alpha-shape原理看过很久了,今天花点时间实现一下:
原理参考:
博客
平面点云边界提取算法研究[D].长沙理工大学,2017.
《综合机载与车载LiDAR数据的建筑物三维重建》
《基于体元的机载LiDAR建筑物提取》
注意:个人原创,转载注明出处,如果感觉有帮助,点个赞,有疑问,欢迎留言交流。
show the codes:
// ashape.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//
#include<pcl\common\common.h>
#include <iostream>
#include<pcl/point_cloud.h>
#include<pcl/kdtree/kdtree_flann.h>
#include<pcl/io/pcd_io.h>
double get2DDist(pcl::PointXYZ p1, pcl::PointXYZ p2)
{
double dist = std::pow(((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y)), 0.5);
return dist;
}
int main()
{
std::string file_name("Plane_0002.pcd");
pcl::PointCloud<pcl::PointXYZ> ::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>());
pcl::io::loadPCDFile<pcl::PointXYZ>(file_name, *cloud_in);
//投影到二维平面
for (auto& point : *cloud_in)
{
point.z = 0.0;
}
pcl::io::savePCDFile<pcl::PointXYZ>("二维平面.pcd", *cloud_in);
std::vector<int> boundary_bool(cloud_in->size(), 0);
double r = 1.8;
pcl::KdTreeFLANN<pcl::PointXYZ>kdtree;
kdtree.setInputCloud(cloud_in);
std::vector<int> indices(0,0);
std::vector<float>dist(0, 0.0);
for (size_t i = 0; i < cloud_in->size(); ++i)
{
pcl::PointXYZ p = cloud_in->points[i];
kdtree.radiusSearch(cloud_in->points[i], 2*r, indices,dist ,100);
//indices[0]对应的点云即为查询点本身
for (size_t j = 1; j < indices.size(); ++j)
{
pcl::PointXYZ p1 = cloud_in->points[indices[j]];
double s_2 = std::pow((p.x - p1.x), 2) +
std::pow((p.y - p1.y), 2);
double h = std::pow((r * r / s_2 - 0.25), 0.5);
double x2 = p.x + 0.5 * (p1.x - p.x) - h * (p1.y - p.y);
double y2 = p.y + 0.5 * (p1.y - p.y) - h * (p.x - p1.x);
double x3 = p.x + 0.5 * (p1.x - p.x) + h * (p1.y - p.y);
double y3 = p.y + 0.5 * (p1.y - p.y) + h * (p.x - p1.x);
pcl::PointXYZ p2(x2, y2, 0.0);
pcl::PointXYZ p3(x3, y3, 0.0);
//计算邻域内除了p1之外的点到p2,p3的距离
std::vector<double>distp2,distp3;
std::vector<int>distp2_bool(0, 0), distp3_bool(0, 0);
int count = 0;
for (size_t k = 1; k < indices.size(); ++k)
{
pcl::PointXYZ p_other = cloud_in->points[indices[k]];
if (k != j)
{
++count;
double distance_p2 = get2DDist(p_other, p2);
double distance_p3 = get2DDist(p_other, p3);
//比较距离与r的大小
if (distance_p2 > r)
{
distp2_bool.push_back(1);
}
if (distance_p3 > r)
{
distp3_bool.push_back(1);
}
}
}
//如果邻域内所有点到p2,或者p3的距离均大于r,则有distp2_bool.size()==count
//则表明p是边界点,退出循环,不用再计算邻域内点距离了
if (count == distp2_bool.size() || count == distp3_bool.size())
{
boundary_bool[i] = 1;
break;
}
}
}
pcl::PointCloud<pcl::PointXYZ> boundary_cloud;
for (size_t it=0;it< boundary_bool.size();++it)
{
if (boundary_bool[it] == 1)
{
boundary_cloud.push_back(cloud_in->points[it]);
}
}
boundary_cloud.height = boundary_cloud.size();
boundary_cloud.width = 1;
boundary_cloud.resize(boundary_cloud.height * boundary_cloud.width);
if (boundary_cloud.size())
{
pcl::io::savePCDFile<pcl::PointXYZ>("边界点云.pcd", boundary_cloud);
}
std::cout << "Hello World!\n";
}
实验效果:(红色为提取的边界点)
结论:该方法提取边界效率较慢(可以看出来三层循环,速度慢),效果还行(中间有一部分错误边界点)
更多推荐
已为社区贡献3条内容
所有评论(0)