#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/common.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
using namespace std;
int main()
{
//------------------------------读取点云---------------------------
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
if (pcl::io::loadPCDFile<pcl::PointXYZI>("E://data//CSite1_orig-utm.pcd", *cloud) == -1)
{
PCL_ERROR("Could not read file\n");
}
//------------------------------计算法线----------------------------
pcl::NormalEstimationOMP<pcl::PointXYZI, pcl::Normal> n;//OMP加速
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
pcl::search::KdTree<pcl::PointXYZI>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZI>());
n.setNumberOfThreads(10);
n.setViewPoint(0, 0, 0); // 设置视点，默认为（0，0，0）
n.setInputCloud(cloud);
n.setSearchMethod(tree);
n.setKSearch(10); // 点云法向计算时，需要所搜的近邻点大小
//n.setRadiusSearch(0.03); // 半径搜素
n.compute(*normals); // 开始进行法向计
// ------------------------计算给定点位的坡度-------------------------
Eigen::Vector3f orZDir(0, 0, 1); // Z轴正方向
std::vector<double>pointSlope(normals->points.size(), 0); // 给定点位的坡度
for (int i = 0; i < normals->points.size(); ++i)
{
auto normal = normals->points[i].getNormalVector3fMap();
// 法向量定向，使其全部朝向Z轴正方向
if (normal.norm() == 0.0)
{
normal = orZDir;
}
else if (normal.dot(orZDir) < 0.0)
{
normal *= -1.0;
}
pointSlope[i] = pcl::getAngle3D(normal, orZDir, false);
// 这一步是为了将坡度计算结果可视化出来，所以将坡度存储到强度字段
cloud->points[i].intensity = pointSlope[i];
}
//-----------------------强度（坡度）渲染并可视化-----------------------
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI>fildColor(cloud, "intensity");
viewer->setBackgroundColor(0, 0, 0);
viewer->setWindowName("点云按坡度显示");
viewer->addText("Point clouds are shown by intensity", 50, 50, 0, 1, 0, "v1_text");
viewer->addPointCloud<pcl::PointXYZI>(cloud, fildColor, "sample cloud");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
while (!viewer->wasStopped())
{
viewer->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
return 0;
}
