PCL知识点——体素化网格方法对点云进行下采样

体素化网格方法：

“使用体素化网格方法实现下采样,即减少点的数量,减少点云数据,并同时保持点云的形状特征,在提高配准、曲面重建、形状识别等算法速度中非常实用。
PCL实现的VoxelGrid类通过输入的点云数据创建一个三维体素栅格(可把体素栅格想象为微小的空间三维立方体的集合),然后在每个体素(即三维立方体)内,用体素中所有点的重心来近似显示体素中其他点,这样该体素内所有点就用一个重心点最终表示,对于所有体素处理后得到过滤后的点云。这种方法比用体素中心来逼近的方法更慢,但它对于采样点对应曲面的表示更为准确。”

2 知识点补充

2.1 点云数据结构

根据激光测量原理得到的点云，包含三维坐标信息(xyz)和激光反射强度信息（intensity），激光反射强度与仪器的激光发射能量、波长，目标的表面材质、粗糙程度、入射角相关。根据摄影测量原理得到的点云，包括三维坐标（xyz）和颜色信息（rgb）

PCL的基本数据类型是PointCloud，一个PointCloud是一个C++的模板类，它包含了以下字段：

width(int)：指定点云数据集的宽度
对于无组织格式的数据集，width代表了所有点的总数
对于有组织格式的数据集，width代表了一行中的总点数
height(int)：制定点云数据集的高度
对于无组织格式的数据集，值为1
对于有组织格式的数据集，表示总行数
points(std::vector)：包含所有PointT类型的点的数据列表

PLC的衍生类型如下：

PointXYZ - float x, y, z
PointXYZI - float x, y, z, intensity
PointXYZRGB - float x, y, z, rgb
PointXYZRGBA - float x, y, z, uint32_t rgba
Normal - float normal[3], curvature 法线方向，对应的曲率的测量值
PointNormal - float x, y, z, normal[3], curvature 采样点，法线和曲率
Histogram - float histogram[N] 用于存储一般用途的n维直方图

2.2 点云数据的读写

#include <iostream>
#include <pcl/io/pcd_io.h> //PCD数据，读写类相关的头文件
#include <pcl/point_types.h> //PCL中支持的点类型头文件，包含一些PointT类型的结构体声明，如：pcl::PointXYZ该类型


int read();
void write();

int main(int argc,char** argv)
{
    
	int ret_r = read();
	write();


	return 0;
}


int read()
{
    
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);//创建一个PointCloud<PointXYZ> Ptr共享指针并进行实例化。
	//pcl::PointCloud<pcl::PointXYZ>可以类比vector<int> 
	if(pcl::io::loadPCDFile<pcl::PointXYZ>("test_pcd.pcd",*cloud)==-1)//打开点云文件
	{
    
		PCL_ERROR("Couldn't read file test_pcd.pcd\n");
		return -1;
	}
	
	// 很重要！！！！
	//sensor_msgs::PointCloud2 cloud_blob; //sensor_msgs::PointCloud2是一类点云结构（ROS点云话题），可以和pcl::PointCloud<T>这个转换
	//pcl::io::loadPCDFile("test_pcd.pcd", cloud_blob);
	//pcl::fromROSMsg(cloud_blob, *cloud); //将sensor_msgs::PointCloud2转换为pcl::PointCloud<T>

	std::cout<<"Loaded "<<cloud->width*cloud->height<< " data points from test_pcd.pcd with the following fields: "<<std::endl;
	for(size_t i=0;i<cloud->points.size();++i)
		std::cout<<" "<<cloud->points[i].x<<" "<<cloud->points[i].y<<" "<<cloud->points[i].z<<std::endl;
	return 0;
}

void write()
{
    
	pcl::PointCloud<pcl::PointXYZ> cloud;
	// 创建点云
	cloud.width=5;
	cloud.height=1;
	cloud.is_dense=false;
	cloud.points.resize(cloud.width*cloud.height);
	for(size_t i=0;i<cloud.points.size();++i)
	{
    
		cloud.points[i].x=1024*rand()/(RAND_MAX+1.0f);
		cloud.points[i].y=1024*rand()/(RAND_MAX+1.0f);
		cloud.points[i].z=1024*rand()/(RAND_MAX+1.0f);
	}
	std::cout<<"rand: "<<rand()<<"RAND_MAX: "<<RAND_MAX;
	//保存点云
	pcl::io::savePCDFileASCII("test_write_pcd.pcd",cloud);
	std::cerr<<"Saved "<<cloud.points.size()<<" data points to test_pcd.pcd."<<std::endl;
	for(size_t i=0;i<cloud.points.size();++i)
	std::cerr<<" "<<cloud.points[i].x<<" "<<cloud.points[i].y<<" "<<cloud.points[i].z<<std::endl;
}

CMakeLists.txt:

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
project(pcd_read)

find_package(PCL 1.2 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable(pcd_read pcd_rw.cpp)
target_link_libraries(pcd_read ${PCL_LIBRARIES})

输出内容：

[email protected]:~/wuhd/code/pcl/3/1/build$ ./pcd_read
Loaded 5 data points from test_pcd.pcd with the following fields: 
    1.28125 577.094 197.938
    828.125 599.031 491.375
    358.688 917.438 842.562
    764.5 178.281 879.531
    727.531 525.844 311.281
rand: 1967513926RAND_MAX: 2147483647Saved 5 data points to test_pcd.pcd.
    0.352222 -0.151883 -0.106395
    -0.397406 -0.473106 0.292602
    -0.731898 0.667105 0.441304
    -0.734766 0.854581 -0.0361733
    -0.4607 -0.277468 -0.916762

使用cat指令可以查看pcd文件（PCD文件如下）

[email protected]:~/wuhd/code/pcl/3/1/build$ cat test_write_pcd.pcd 
# .PCD v0.7 - Point Cloud Data file format
VERSION 0.7
FIELDS x y z
SIZE 4 4 4
TYPE F F F
COUNT 1 1 1
WIDTH 5
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS 5
DATA ascii
0.35222197 -0.15188313 -0.10639524
-0.3974061 -0.47310591 0.29260206
-0.73189831 0.66710472 0.44130373
-0.73476553 0.85458088 -0.036173344
-0.46070004 -0.2774682 -0.91676188

VERSION 0.5 指定PCS文件版本
FIELDS x y z 指定一个点的每一个维度和字段名字，例如
    FIELDS x y z # XYZ data
    FIELDS x y z rgb # XYZ + colors
    FIELDS x y z normal_x normal_y normal_z # XYZ + surface normals
SIZE 4 4 4 指定每一个维度的字节数大小
TYPE F F F 指定每一个维度的类型，I表示int，U表示uint，F表示浮点
COUNT 1 1 1 指定每一个维度包含的元素数，如果没有COUNT，默认都为1
WIDTH 5 点云数据集的宽度
HEIGHT 1 点云数据集的高度
VIEWPOINT 0 0 0 1 0 0 0 指定点云获取的视点和角度，在不同坐标系之间转换时使用（由3个平移+4个四元数构成）
POINTS 5 总共的点数（显得多余）
DATA ascii 存储点云数据的数据类型，ASCII和binary

补充：PCL 中 PCD 文件格式的正式发布是 0.7版本，之前也有0.5，0.6版本。

有时为了节省空间，提高读写效率，还会以binary的格式进行序列化，即将save操作改为

pcl::io::savePCDFileBinary("test_pcd_binary.pcd", cloud);
// 或
pcl::io::savePCDFile("test_pcd_binary.pcd", cloud, true);

此时输出的内容如下：

[email protected]:~/wuhd/code/pcl/3/1/build$ cat test_write_pcd_bin.pcd 
# .PCD v0.7 - Point Cloud Data file format
VERSION 0.7
FIELDS x y z
SIZE 4 4 4
TYPE F F F
COUNT 1 1 1
WIDTH 5
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS 5
DATA binary
pV�>@����ٽ�x˾�:���ϕ>�];�`�*?���>�<���Z?�*�����P����j�

注意最后一行输出以二进制形式保存，故而不可以直接看到值。

2.3 ROS下进行点云数据实时获取与可视化

cpp

#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <sstream>
#include <string.h>

typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

PointCloudT::Ptr cloud(new PointCloudT);


//std::string world_frame_id;
std::string frame_id;

std::string path;

void cloud_cb (const PointCloudT::ConstPtr& callback_cloud)
{
    
  *cloud = *callback_cloud;
  frame_id = cloud->header.frame_id;
  std::stringstream ss;
  std::string s1;
  ss<<ros::Time::now();
  s1=ss.str();

  // cloud saving:
  std::string file_name=path+"cloud_"+frame_id.substr(0, frame_id.length())+"_"+s1+".pcd";
  //形式一：substr(pos,n)注释：1.pos表示截取字符的下标。2.n代表从截取pos之后的n个字符。
  //形式二：substr(n)注释：1.若n>=0,表示截取从下标为n后的所有字符。2.若n<0,表示截取字符串倒数第n个后的所有字符。
  std::cout<<file_name<<std::endl;
  pcl::io::savePCDFileBinary(file_name,*cloud); 	
}

int
main (int argc, char** argv)
{
    
  ros::init(argc, argv, "save_cloud");
  ros::NodeHandle nh("~");

  //Read some parameters from launch file:
  std::string pointcloud_topic = "/camera/depth_registered/points";
  path = "./pointtest/";
	
  // Subscribers:
  ros::Subscriber sub = nh.subscribe(pointcloud_topic, 1, cloud_cb);
  std::cout<<"receive messages successfully!"<<std::endl;
	
  ros::spin();

  return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(my_savecloud_tutorials)

find_package(catkin REQUIRED COMPONENTS
  pcl_conversions
  pcl_ros
  roscpp
  sensor_msgs
  std_msgs
)


catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES my_savecloud_tutorials
#  CATKIN_DEPENDS pcl_conversions pcl_ros roscpp sensor_msgs std_msgs
#  DEPENDS system_lib
)

include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)


add_executable(save_cloud src/save_cloud.cpp)
target_link_libraries(save_cloud ${catkin_LIBRARIES})