基于体素化方法的点云降采样

前两天做了一个点云降采样的项目，用pcl自带的降采样方法出来的结果不是很理想，于是就自己写了一个。为了使代码执行效率高点就采用了基于点云索引的方式。
本文使用的方法为：首先，计算点云群的Bounding Box;然后，根据一定的分辨率将空间点云体素化，并记录下每个体素所包含点云的索引（体素化的实质就是给点云赋予体素标签）；最后，遍历体素，根据每个体素内点云的索引取点云坐标，计算每个体素重心的坐标，保留体素内距离重心最近的点。从而实现降采样。
经过以上操作获取的点云密度相对较为均匀。

1、点云索引计算公式（即计算每个点云属于哪个体素）

2、重心坐标计算公式

#include <liblas\liblas.hpp>
#include <fstream>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/keypoints/uniform_sampling.h>
#include <pcl/common/common.h>
#include <algorithm>using namespace pcl;void Subsample(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &in_cloud , pcl::PointCloud<pcl::PointXYZRGB>::Ptr &filtered_cloud)
{//一、体素化（实质为给每个点云赋予体素标签）pcl::PointXYZRGB min_pt, max_pt;pcl::getMinMax3D(*in_cloud,min_pt,max_pt);double resolution = 0.1;int x_size = ceil((max_pt.x - min_pt.x)/resolution);int y_size = ceil((max_pt.y - min_pt.y)/resolution);int z_size = ceil((max_pt.z - min_pt.z)/resolution);int voxel_number = x_size*y_size*z_size;//为每个点云附体素编号，并在相应体素中记录其点云索引std::vector<std::vector<size_t>> all_label;all_label.resize(voxel_number);for (size_t i = 0; i < in_cloud->size(); ++i){int x_index = (in_cloud->points[i].x - min_pt.x) / resolution;int y_index = (in_cloud->points[i].y - min_pt.y) / resolution;int z_index = (in_cloud->points[i].z - min_pt.z) / resolution;int voxel_index = z_index * (x_size*y_size) + y_index * x_size + x_index;all_label[voxel_index].push_back(i);}//二、计算相同标签点云的重心坐标，并求出距离重心最近的点云，保存该点云至filter_cloud//每个体素中点云的个数有三种：0,1，>1for (size_t i = 0; i < voxel_number; ++i){if (all_label[i].size() == 0)   //体素中无点云{continue;}else{if (all_label[i].size() == 1)   //体素中只有一个点云{int pt_index_0 = all_label[i][0];pcl::PointXYZRGB pt1;pt1.x = in_cloud->points[pt_index_0].x;pt1.y = in_cloud->points[pt_index_0].y;pt1.z = in_cloud->points[pt_index_0].z;pt1.r = in_cloud->points[pt_index_0].r;pt1.g = in_cloud->points[pt_index_0].g;pt1.b = in_cloud->points[pt_index_0].b;filtered_cloud->points.push_back(pt1);}else     //体素中点云个数大于1{//计算重心坐标double sum_x = 0;double sum_y = 0;double sum_z = 0;for (size_t j = 0; j < all_label[i].size(); ++j)      {int pt_index_1 = all_label[i][j];double sum_x0 = in_cloud->points[pt_index_1].x;       double sum_y0 = in_cloud->points[pt_index_1].y;       double sum_z0 = in_cloud->points[pt_index_1].z;       sum_x = sum_x0++;sum_y = sum_y0++;sum_z = sum_z0++;}int pt_number = all_label[i].size();double center_x = sum_x / pt_number;double center_y = sum_y / pt_number;double center_z = sum_z / pt_number;//寻找最近点//计算体素中每个点与重心的距离std::vector<size_t> distance;for (size_t k = 0; k < all_label[i].size(); ++k){int pt_index2 = all_label[i][k];double pt_x = in_cloud->points[pt_index2].x;double pt_y = in_cloud->points[pt_index2].y;double pt_z = in_cloud->points[pt_index2].z;double dx = pt_x - center_x;double dy = pt_y - center_y;double dz = pt_z - center_z;double dis = sqrt(dx*dx+dy*dy+dz*dz);distance.push_back(dis);}//记录最近点的索引auto min_dis = std::min_element(std::begin(distance), std::end(distance));int min_dis_index = std::distance(std::begin(distance),min_dis);int pt_index = all_label[i][min_dis_index];//保存最近点pcl::PointXYZRGB pt;pt.x = in_cloud->points[pt_index].x;pt.y = in_cloud->points[pt_index].y;pt.z = in_cloud->points[pt_index].z;pt.r = in_cloud->points[pt_index].r;pt.g = in_cloud->points[pt_index].g;pt.b = in_cloud->points[pt_index].b;filtered_cloud->points.push_back(pt);}}}all_label.clear();}void las2las(std::string fname)
{//打开las文件std::ifstream ifs(fname, std::ios::in | std::ios::binary);liblas::ReaderFactory f;liblas::Reader reader = f.CreateWithStream(ifs);//读取las文件信息头liblas::Header const& header = reader.GetHeader();int nbPoints = header.GetPointRecordsCount();//设置初始偏移量（这一步没有的话会报错，偏移量写入输出部分的文件头。保证两份数据在相同坐标体系下）double x_setoff = header.GetOffsetX();double y_setoff = header.GetOffsetY();double z_setoff = header.GetOffsetZ();//转换为pcl格式pcl::PointCloud<pcl::PointXYZRGB>::Ptr in_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);in_cloud->resize(nbPoints);int i = 0;while (reader.ReadNextPoint()){//坐标信息in_cloud->points[i].x = reader.GetPoint().GetX();in_cloud->points[i].y = reader.GetPoint().GetY();in_cloud->points[i].z = reader.GetPoint().GetZ();//颜色信息uint16_t r1 = reader.GetPoint().GetColor().GetRed();uint16_t g1 = reader.GetPoint().GetColor().GetGreen();uint16_t b1 = reader.GetPoint().GetColor().GetBlue();uint32_t r2 = ceil(((float)r1 / 65536)*(float)256);uint32_t g2 = ceil(((float)g1 / 65536)*(float)256);uint32_t b2 = ceil(((float)b1 / 65536)*(float)256);uint32_t rgb = ((int)r2) << 16 | ((int)g2) << 8 | ((int)b2);in_cloud->points[i].rgb = *reinterpret_cast<float*>(&rgb);i++;}//自制采样pcl::PointCloud<pcl::PointXYZRGB>::Ptr filteredCloud(new pcl::PointCloud<pcl::PointXYZRGB>);//以0.1m为分辨率进行体素化->寻找每个体素中，距离体素重心最近的点云保留下来Subsample(in_cloud,filteredCloud);//写入las文件fname.replace(fname.find("in"), 2, "out");std::ofstream ofs = std::ofstream(fname, std::ios::out | std::ios::binary);//设置文件头、点数、格式、缩放因子、偏移量liblas::Header f_header;f_header.SetVersionMajor(1);f_header.SetVersionMinor(2);f_header.SetDataFormatId(liblas::PointFormatName::ePointFormat3);f_header.SetOffset(x_setoff,y_setoff,z_setoff);   //这里注意偏移量要从读取部分的头文件获取f_header.SetScale(0.001, 0.001, 0.001);int out_p_n = filteredCloud->size();f_header.SetPointRecordsCount(out_p_n);liblas::Writer writer(ofs, f_header);liblas::Point point(&f_header);//转换为lasfor (size_t i = 0; i < filteredCloud->size(); ++i){double x = filteredCloud->points[i].x;double y = filteredCloud->points[i].y;double z = filteredCloud->points[i].z;point.SetX(x);point.SetY(y);point.SetZ(z);uint32_t r = (uint32_t)filteredCloud->points[i].r;uint32_t g = (uint32_t)filteredCloud->points[i].g;uint32_t b = (uint32_t)filteredCloud->points[i].b;liblas::Color pointColor(r, g, b);point.SetColor(pointColor);writer.WritePoint(point);}writer.SetHeader(f_header);ofs.flush();ofs.close();std::cout << fname << "     " <<"finished!"<<std::endl;in_cloud->clear();filteredCloud->clear();}int main()
{测试//las2las("E:/temp/in/Tile_1321222320002302112.las");std::ifstream ifs;ifs.open("G:/Production_3 (2)_in/dir.txt");std::vector<std::string> dir;while (!ifs.eof()){std::string str;std::getline(ifs, str);dir.push_back(str);}ifs.close();//std::cout << dir.size() << std::endl;for (size_t i = 0; i < dir.size()-1; ++i){las2las(dir[i]);}return EXIT_SUCCESS;}

基于体素化方法的点云降采样相关推荐

NormalSpaceSampling 基于法向空间的点云降采样
随着工业自动化.智能化的不断推进,机器视觉(2D/3D)在工业领域的应用和重要程度也同步激增(识别.定位.抓取.测量,缺陷检测等),而针对不同作业场景进行解决方案设计时,通常会借助PCL.OpenCV ...
我们成功给OpenCV添加了三维点云降采样算法！
作者:阮业淳,钟万里,张昌圳(本科生) 指导教师:于仕琪南方科技大学计算机科学与工程系知名开源计算机视觉库OpenCV(Open Source Computer Vision Library)在其 ...
点云降采样（DownSampling）
点云降采样 1 概述三维点云往往包含大量冗余数据,直接处理计算量大,消耗时间长,因此对其进行降采样是十分必要的.降采样同时也是点云预处理过程中的关键环节. 2 常用方法 2.1 体素网格下采样 2. ...
点云降采样--ApproximateVoxelGrid点云降采样
1.版本要求版本: >PCL1.0 2.简介 ApproximateVoxelGrid体素降采样是PCL开源库中非常有效的点云降采样手段.ApproximateVoxelGrid对点云进行体素 ...
3D目标检测常见体素化方法总结（原理及代码）（持续更新）
体素化就是将点云分成在空间上均匀的体素格,然后生成3D点和它们对应的体素之间多对一的映射. 一.硬体素化在VoxelNet中,体素化分为两个阶段,第一个阶段叫grouping--分组,第二个阶段叫s ...
详解基于格网法统计平面点云面积
主函数展示: void main() {char *inputpath = "D:\\testdata.xyz";vector<pcl::PointXYZ> point ...
matlab 降采样代码,matlab 点云降采样 pcdownsample()
** 点云数据降采样 pcdownsample() ** pcdownsample降采样减少点云数据量: 一.语法: ptCloudOut = pcdownsample(ptCloudIn, 'ran ...
基于数值化方法的污水管网中地下水入渗控制研究与展望
封面 | 城市智慧水务原创全文共计1937字, 阅读大约需要10分钟 1.导读世界范围内污水管网破损导致的地下水入渗是制约管网系统正常运行的瓶颈问题. 已有研究表明, 地下水入渗量可达污水处理厂 ...
GICP：基于体素泛化ICP方式的准确快速点云配准方法
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达标题: Voxelized GICP for Fast and Accurate 3D Point C ...

基于体素化方法的点云降采样

基于体素化方法的点云降采样相关推荐

最新文章

热门文章