PCL voxelgrid实现

体素滤波器可以达到向下采样同时不破坏点云本身几何结构的功能，但是会移动点的位置。

此外体素滤波器可以去除一定程度的噪音点及离群点。主要功能是用来进行降采样。

（1）它的原理是根据输入的点云，首先计算一个能够刚好包裹住该点云的立方体，然后根据设定的分辨率，将该立方体分割成不同的小立方体。类似于octree里的立方体划分，对于每一个小立方体内的点，计算他们的质心（重心），并用该质心的坐标来近似该立方体内的若干点。

（2）ApproximateVoxelGrid的不同在于这种方法是利用每一个小立方体的中心来近似该立方体内的若干点。相比于 VoxelGrid，计算速度稍快，但也损失了原始点云局部形态的精细度。

原理不多讲，直接上代码：

c++代码如下：
 

#include <iostream>
#include <pcl/io/pcd_io.h>//输入输出头文件
#include <pcl/point_types.h>//PCD读写类相关的头文件
#include <pcl/filters/voxel_grid.h>//点类型相关头文件
#include <pcl/common/time.h> // 计时,使用 pcl::StopWatch time;的支持头文件
#include <thread>//1 detach脱离当前主线程，自由执行，乱序;2 join()等待模式，执行完再执行下一个3 std::this_thread::get_id()获取当前线程编号4 std::thread::hardware_concurrency()检测CPU有多少个核心
#include <pcl/common/common_headers.h>//可视化相关头文件
#include <pcl/features/normal_3d.h>//创建3d模型所需头文件 属于可视化相关头文件之一
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>//pcl程序中经常用到程序后面带选项，选项解析使用pcl::console::parse_argument()来完成 执行getchar();  防止闪退所需头文件
#include<sstream>// C++标准库中的<sstream>提供了比ANSI C的<stdio.h>更高级的一些功能
using namespace std;
using namespace pcl::console;

int main(int argc, char** argv)
{
	pcl::StopWatch time; // 计时开始 有时需要计算代码运行的时间，使用PCL里的StopWatch类以及ScopeTime类可以实现这个功能
	// pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2());//Linux下的编写方式
	// pcl::PCLPointCloud2::Ptr cloud_filtered(new pcl::PCLPointCloud2());

	//输入点云
	pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
	pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZI>);

	// Fill in the cloud data
	pcl::PCDReader reader;//pcd文件读操作
	// Replace the path below with the path where you saved your file
	reader.read("D:\\pclcode\\filter\\voxel_grid\\source\\ChinaDragon.pcd", *cloud); // Remember to download the file first!
	std::stringstream str1;//* 使用stringstream对象简化类型转换 C++标准库中的<sstream>提供了比ANSI C的<stdio.h>更高级的一些功能   

	std::cerr << "PointCloud before filtering: " << cloud->width * cloud->height
		<< " data points (" << pcl::getFieldsList(*cloud) << ")." << endl;
		
	str1 << "PointCloud before filtering: " << cloud->width * cloud->height
		<< " data points (" << pcl::getFieldsList(*cloud) << ")." << endl;

	// Create the filtering object
	// 创建一个大小为2cm的pcl::VoxelGrid滤波器
	// pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
	pcl::VoxelGrid<pcl::PointXYZI> sor;//创建滤波器对象
	sor.setInputCloud(cloud); // 给滤波对象设置需要过滤的点云
	//sor.setLeafSize(0.02f, 0.02f, 0.02f); // 设置滤波时创建的体素大小为2cm立方体
	sor.setLeafSize(0.06f, 0.06f, 0.06f);
	sor.filter(*cloud_filtered); // 执行滤波处理，存储输出cloud_filtered
	std::stringstream str2;//类型转换实现  安全和自动的类型转换
	std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height
		<< " data points (" << pcl::getFieldsList(*cloud_filtered) << ")." << endl;
		
	str2 << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height
		<< " data points (" << pcl::getFieldsList(*cloud_filtered) << ")."<<endl;
	pcl::PCDWriter writer;//PCD文件写操作
	writer.write("ChinaDragon.pcd_downsampled.pcd", *cloud_filtered);
	// writer.write("table_scene_lms400_downsampled.pcd", *cloud_filtered, Eigen::Vector4f::Zero(), Eigen::Quaternionf::Identity(), false);
	//linux下操作写法
	std::cout << "运行时间:" << time.getTime() << "ms（毫秒）" << std::endl; //有时需要计算代码运行的时间，使用PCL里的StopWatch类以及ScopeTime类可以实现这个功能
	std::cout << "运行时间:" << time.getTimeSeconds() << "s（秒）" << std::endl;//有时需要计算代码运行的时间，使用PCL里的StopWatch类以及ScopeTime类可以实现这个功能

	// 可视化
    // 创建一个boost共享指针并进行实例化
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("voxel_grid 3D Viewer"));
	viewer->initCameraParameters(); // 通过设置照相机参数使得从默认的角度和方向观察点云

	int v1(0);  //创建新的视口
	viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1); // 4个参数分别是X轴的最小值，最大值，Y轴的最小值，最大值，取值0-1，v1是标识
	viewer->setBackgroundColor(0, 0, 0, v1); // 设置视窗的背景颜色 黑色
	// viewer->addText("cloud before voxelgrid filtering", 10, 10, "v1 text", v1); // 添加一个标签区别其他窗口
	viewer->addText(str1.str(), 10, 10, "v1 text", v1); // 添加一个标签区别其他窗口 str1.str()调用上面的str1
	pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI>cloud_color(cloud, "intensity");//创建颜色处理对象 PointCloudColorHandlerCustom ，PCL Visualizer 类利用这样的对象显示自定义颜色数据
	viewer->addPointCloud<pcl::PointXYZI>(cloud, cloud_color, "cloud", v1);

	// 对第二视口做同样的操作，使得做创建的点云分布于右半窗口，将该视口背景赋值于灰色，以便明显区别，虽然添加同样的点云，给点云自定义颜色着色
	int v2(0);
	viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);// 4个参数分别是X轴的最小值，最大值，Y轴的最小值，最大值，取值0-1，v2是标识
	viewer->setBackgroundColor(0.3, 0.3, 0.3, v2);// 设置视窗的背景颜色
	// viewer->addText("cloud after voxelgrid filtering", 10, 10, "v2 text", v2);
	viewer->addText(str2.str(), 10, 10, "v2 text", v2);// 添加一个标签区别其他窗口 str1.str()调用上面的str2  PointCloud after filtering:
	pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI>cloud_filtered_color(cloud_filtered, "intensity");//创建颜色处理对象 PointCloudColorHandlerCustom ，PCL Visualizer 类利用这样的对象显示自定义颜色数据
	viewer->addPointCloud<pcl::PointXYZI>(cloud_filtered, cloud_filtered_color, "cloud_filtered", v2);
	// 为所有视口设置属性
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud");//第一个窗口
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud_filtered");//第二个窗口
	//  改变显示点云的尺寸
	viewer->addCoordinateSystem(1.0); // 从默认的角度和方向观察点云  添加坐标系

	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(10000));
	} // 每次调用spinOnce都给视窗处理事件的时间，允许鼠标、键盘等交互操作

	getchar(); // 防止闪退
	return (0);
}

CmakeLists文件：

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

project(voxel_grid)

find_package(PCL 1.2 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})

link_directories(${PCL_LIBRARY_DIRS})

add_definitions(${PCL_DEFINITIONS})

add_executable (voxel_grid voxel_grid.cpp)

target_link_libraries (voxel_grid ${PCL_LIBRARIES})

具体的执行效果不展示了，代码里的点云文件换成自己想要滤波的点云文件就可以了。

换了一个点云文件的执行效果，由于场景较大，需要将体素大小设置的大一点

#include <iostream>

#include <pcl/io/pcd_io.h>//输入输出头文件

#include <pcl/point_types.h>//PCD读写类相关的头文件

#include <pcl/filters/voxel_grid.h>//点类型相关头文件

#include <pcl/common/time.h> // 计时,使用 pcl::StopWatch time;的支持头文件

#include <thread>//1 detach脱离当前主线程，自由执行，乱序;2 join()等待模式，执行完再执行下一个3 std::this_thread::get_id()获取当前线程编4 std::thread::hardware_concurrency()检测CPU有多少个核心

#include <pcl/common/common_headers.h>//可视化相关头文件

#include <pcl/features/normal_3d.h>//创建3d模型所需头文件 属于可视化相关头文件之一

#include <pcl/visualization/pcl_visualizer.h>

#include <pcl/console/parse.h>//pcl程序中经常用到程序后面带选项，选项解析使用pcl::console::parse_argument()来完成 执行getchar();  防止闪退所需头文件

#include<sstream>// C++标准库中的<sstream>提供了比ANSI C的<stdio.h>更高级的一些功能

using namespace std;

using namespace pcl::console;



int main(int argc, char** argv)

{

    pcl::StopWatch time; // 计时开始 有时需要计算代码运行的时间，使用PCL里的StopWatch类以及ScopeTime类可以实现这个功能

    // pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2());//Linux下的编写方式

    // pcl::PCLPointCloud2::Ptr cloud_filtered(new pcl::PCLPointCloud2());



    //输入点云

    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);

    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZI>);



    // Fill in the cloud data

    pcl::PCDReader reader;//pcd文件读操作

    // Replace the path below with the path where you saved your file

    reader.read("D:\\pclcode\\filter\\voxel_grid\\source\\scan_003.pcd", *cloud); // Remember to download the file first!

    std::stringstream str1;//* 使用stringstream对象简化类型转换 C++标准库中的<sstream>提供了比ANSI C的<stdio.h>更高级的一些功能  



    std::cerr << "PointCloud before filtering: " << cloud->width * cloud->height

         << " data points (" << pcl::getFieldsList(*cloud) << ")." << endl;

        

    str1 << "PointCloud before filtering: " << cloud->width * cloud->height

         << " data points (" << pcl::getFieldsList(*cloud) << ")." << endl;



    // Create the filtering object

    // 创建一个大小为9cm的pcl::VoxelGrid滤波器

    // pcl::VoxelGrid<pcl::PCLPointCloud2> sor;

    pcl::VoxelGrid<pcl::PointXYZI> sor;//创建滤波器对象

    sor.setInputCloud(cloud); // 给滤波对象设置需要过滤的点云

    //sor.setLeafSize(0.01f, 0.01f, 0.01f); // 设置滤波时创建的体素大小为1cm立方体

    sor.setLeafSize(0.9f,0.9f,0.9f);// 设置滤波时创建的体素大小为9cm立方体

    sor.filter(*cloud_filtered); // 执行滤波处理，存储输出cloud_filtered

    std::stringstream str2;//类型转换实现  安全和自动的类型转换

    std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height

         << " data points (" << pcl::getFieldsList(*cloud_filtered) << ")." << endl;

        

    str2 << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height

         << " data points (" << pcl::getFieldsList(*cloud_filtered) << ")."<<endl;

    pcl::PCDWriter writer;//PCD文件写操作

    writer.write("scan_003_downsampled.pcd", *cloud_filtered);

    // writer.write("table_scene_lms400_downsampled.pcd", *cloud_filtered, Eigen::Vector4f::Zero(), Eigen::Quaternionf::Identity(), false);

    //linux下操作写法

    std::cout << "运行时间:" << time.getTime() << "ms（毫秒）" << std::endl; //有时需要计算代码运行的时间，使用PCL里的StopWatch类以及ScopeTime类可以实现这个功能

    std::cout << "运行时间:" << time.getTimeSeconds() << "s（秒）" << std::endl;//有时需要计算代码运行的时间，使用PCL里的StopWatch类以及ScopeTime类可以实现这个功能



    // 可视化

    // 创建一个boost共享指针并进行实例化

    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("voxel_grid 3D Viewer"));

    viewer->initCameraParameters(); // 通过设置照相机参数使得从默认的角度和方向观察点云



    int v1(0);  //创建新的视口

    viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1); // 4个参数分别是X轴的最小值，最大值，Y轴的最小值，最大值，取值0-1，v1是标识

    viewer->setBackgroundColor(0, 0, 0, v1); // 设置视窗的背景颜色 黑色

    // viewer->addText("cloud before voxelgrid filtering", 10, 10, "v1 text", v1); // 添加一个标签区别其他窗口

    viewer->addText(str1.str(), 10, 10, "v1 text", v1); // 添加一个标签区别其他窗口 str1.str()调用上面的str1

pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI>cloud_color(cloud, "intensity");//创建颜色处理对象 PointCloudColorHandlerCustom ，PCL Visualizer 类利用这样的对象显示自定义颜色数据

    viewer->addPointCloud<pcl::PointXYZI>(cloud, cloud_color, "cloud", v1);



    // 对第二视口做同样的操作，使得做创建的点云分布于右半窗口，将该视口背景赋值于灰色，以便明显区别，虽然添加同样的点云，给点云自定义颜色着色

    int v2(0);

    viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);// 4个参数分别是X轴的最小值，最大值，Y轴的最小值，最大值，取值0-1，v1是标识

    viewer->setBackgroundColor(0.3, 0.3, 0.3, v2);// 设置视窗的背景颜色

    // viewer->addText("cloud after voxelgrid filtering", 10, 10, "v2 text", v2);

    viewer->addText(str2.str(), 10, 10, "v2 text", v2);// 添加一个标签区别其他窗口 str1.str()调用上面的str2  PointCloud after filtering:

pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI>cloud_filtered_color(cloud_filtered, "intensity");//创建颜色处理对象 PointCloudColorHandlerCustom ，PCL Visualizer 类利用这样的对象显示自定义颜色数据

    viewer->addPointCloud<pcl::PointXYZI>(cloud_filtered, cloud_filtered_color, "cloud_filtered", v2);

    // 为所有视口设置属性

viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud");//第一个窗口

viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud_filtered");//第二个窗口

    //  改变显示点云的尺寸

    viewer->addCoordinateSystem(1.0); // 从默认的角度和方向观察点云  添加坐标系



    while (!viewer->wasStopped())

    {

         viewer->spinOnce(100);

         boost::this_thread::sleep(boost::posix_time::microseconds(10000));

    } // 每次调用spinOnce都给视窗处理事件的时间，允许鼠标、键盘等交互操作



    getchar(); // 防止闪退

    return (0);

}

//PointCloud before filtering : 2057209 data points(x y z intensity).

//PointCloud after filtering : 63939 data points(x y z intensity).

//运行时间 : 378ms（毫秒）

//运行时间 : 0.379s（秒）

CmakeLists文件：和上面使用的是一样的