完善下列代码使得被切片点云的索引能保存到pclPointCloudpclPointXYZRGBPtr by_slicing_cloudnew pclPointCloudpclPointXYZRGB;stdvectorintsliceePointIndicespclPointCloudpclPointXYZRGBPtr &m_cloud float m_Delta = 004 float m_dPl
完善后的代码如下:
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/console/time.h>
#include <pcl/common/io.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <vector>
#include <omp.h>
typedef pcl::PointXYZRGB PointT;
std::vector<int> sliceePointIndices(pcl::PointCloud<PointT>::Ptr& m_cloud, float m_Delta = 0.04, float m_dPlatform = 0.4, bool m_OpenMap = true)
{
pcl::PointXYZRGB minPt, maxPt;
pcl::getMinMax3D(*m_cloud, minPt, maxPt);
pcl::console::TicToc time;
time.tic();
std::vector<int> idx;
if (m_OpenMap == true) {
// 并行获取各个切片
std::vector<int> point_idx(m_cloud->size(), -1);
#pragma omp parallel for num_threads(6)
for (int i = 0; i < m_cloud->size(); ++i) {
// 计算间隔数
int index = floor((m_cloud->points[i].x - minPt.x) / m_dPlatform);
// y最小值+间隔数*切片间距=下一个切片的最小值
float sliceMin = minPt.x + index * m_dPlatform;
// 在切片厚度范围内的点为同一层切片
if ((m_cloud->points[i].x >= sliceMin) && (m_cloud->points[i].x < sliceMin + m_Delta)) {
point_idx[i] = index;
}
}
for (int i = 0; i < point_idx.size(); ++i) {
if (point_idx[i] > -1) {
idx.push_back(i);
}
}
std::cout << "并行加速时的算法运行时间:" << time.toc() << " ms" << std::endl;
}
else {
// 循环切片,次数少于点的个数
for (int i = 0; i < m_cloud->size(); ++i) {
double minY = -0.25;
if (m_cloud->points[i].y >= minY && m_cloud->points[i].y <= maxPt.y && (m_cloud->points[i].x <= -0.001 || m_cloud->points[i].x >= 0.049)) {
int index = floor((m_cloud->points[i].x - minPt.x) / m_dPlatform);
float sliceMin = minPt.x + index * m_dPlatform;
if ((m_cloud->points[i].x >= sliceMin) && (m_cloud->points[i].x < sliceMin + m_Delta)) {
idx.push_back(i);
}
}
}
std::cout << "算法运行时间:" << time.toc() << " ms" << std::endl;
}
for (int i = 0; i < m_cloud->size(); ++i) {
double minY = -0.25;
double maxY = -0.168;
if ((m_cloud->points[i].y >= minY && m_cloud->points[i].y <= maxY) || (m_cloud->points[i].x >= -0.001 && m_cloud->points[i].x <= 0.049)) {
int index = floor((m_cloud->points[i].y - minPt.y) / m_dPlatform);
float sliceMin = minPt.y + index * m_dPlatform;
if ((m_cloud->points[i].y >= sliceMin) && (m_cloud->points[i].y < sliceMin + m_Delta)) {
idx.push_back(i);
}
}
}
std::cout << "算法运行时间:" << time.toc() << " ms" << std::endl;
return idx;
}
void qipian(pcl::PointCloud<PointT>::Ptr cloud1)
{
//--------------------------------设置相关参数------------------------------
float Delta = 0.0030; // 切片的厚度Delta
float dPlatform = 0.01; // 相邻切片之间的空隙
bool OpenMap = true; // 是否启动并行加速
// -------------------------------调用切片函数------------------------------
std::vector<int> idx = sliceePointIndices(cloud1, Delta, dPlatform, OpenMap);
pcl::PointCloud<PointT>::Ptr slicing_cloud(new pcl::PointCloud<PointT>);
pcl::copyPointCloud(*cloud1, idx, *slicing_cloud);
// 可视化
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
viewer->setWindowName("点云切片");
viewer->addPointCloud<PointT>(slicing_cloud, "slicing cloud");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "slicing cloud");
while (!viewer->wasStopped()) {
viewer->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
}
int main()
{
pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
pcl::PointCloud<PointT>::Ptr by_slicing_cloud(new pcl::PointCloud<PointT>);
// 读取点云数据
if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("cloud.pcd", *cloud) == -1) {
PCL_ERROR("Couldn't read file cloud.pcd\n");
return -1;
}
// 调用切片函数
qipian(cloud);
return 0;
}
在主函数中,首先创建了两个点云指针cloud和by_slicing_cloud,用于存储原始点云和切片点云。
然后读取点云数据到cloud中。
最后调用qipian函数进行切片,并将切片后的点云保存到by_slicing_cloud中
原文地址: https://www.cveoy.top/t/topic/h4kt 著作权归作者所有。请勿转载和采集!