PCL knowledge points - voxelized grid method for down sampling of point clouds

Voxelized mesh method ：

“ The voxel mesh method is used to realize down sampling , That is, reduce the number of points , Reduce point cloud data , And maintain the shape characteristics of the point cloud at the same time , In improving registration 、 Surface reconstruction 、 Shape recognition and other algorithms are very practical in speed .
PCL Realized VoxelGrid Class creates a three-dimensional Voxel grid ( You can think of voxel grid as a collection of tiny spatial three-dimensional cubes ), And then in each Voxel ( Three dimensional cube ) Inside , The center of gravity of all points in the voxel is used to approximate the other points in the voxel , In this way, all points in the voxel are finally represented by a center of gravity , For all voxels, the filtered point cloud is obtained . This method is slower than the method of approaching with voxel Center , However, it is more accurate for the representation of the surface corresponding to the sampling point .”

2 Knowledge points supplement

2.1 Point cloud data structure

Point clouds from laser measurements , Contains three-dimensional coordinate information (xyz) And laser reflection intensity information （intensity）, Laser reflection intensity and laser emission energy of the instrument 、 wavelength , The surface material of the target 、 Roughness 、 Incident angle correlation . Point clouds from photogrammetry , Including three-dimensional coordinates （xyz） And color information （rgb）

PCL The basic data type of is PointCloud, One PointCloud It's a C++ Template class of , It contains the following fields ：

width(int)： Specifies the width of the point cloud dataset
For unorganized data sets ,width Represents the total number of all points
For data sets in an organized format ,width Represents the total number of points in a row
height(int)： Set the height of the point cloud dataset
For unorganized data sets , The value is 1
For data sets in an organized format , It means the total number of rows
points(std::vector)： Contains all the PointT Data list of points of type

PLC The derivation types of are as follows ：

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 Normal direction , The measured value of the corresponding curvature
PointNormal - float x, y, z, normal[3], curvature Sampling point , Normals and curvature
Histogram - float histogram[N] For general purpose storage n Dimensional histogram

2.2 Reading and writing point cloud data

#include <iostream>
#include <pcl/io/pcd_io.h> //PCD data , Read and write class related header files 
#include <pcl/point_types.h> //PCL Point type header files supported in , Include some PointT Structure declaration of type , Such as ：pcl::PointXYZ This type of 


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>);// Create a PointCloud<PointXYZ> Ptr Share pointers and instantiate them .
	//pcl::PointCloud<pcl::PointXYZ> By analogy vector<int> 
	if(pcl::io::loadPCDFile<pcl::PointXYZ>("test_pcd.pcd",*cloud)==-1)// Open the point cloud file 
	{
    
		PCL_ERROR("Couldn't read file test_pcd.pcd\n");
		return -1;
	}
	
	//  Very important ！！！！
	//sensor_msgs::PointCloud2 cloud_blob; //sensor_msgs::PointCloud2 It is a kind of point cloud structure （ROS Point cloud topic ）, You can talk to pcl::PointCloud<T> This transformation 
	//pcl::io::loadPCDFile("test_pcd.pcd", cloud_blob);
	//pcl::fromROSMsg(cloud_blob, *cloud); // take sensor_msgs::PointCloud2 Convert to 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;
	//  Create a point 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;
	// Save point cloud 
	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})

Output content ：

[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

Use cat Commands can be viewed pcd file （PCD The documents are as follows ）

[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  Appoint PCS Document version 
FIELDS x y z  Specify the name of each dimension and field of a point , for example 
    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  Specify the number and size of bytes for each dimension 
TYPE F F F  Specify the type of each dimension ,I Express int,U Express uint,F It means floating point 
COUNT 1 1 1  Specify the number of elements contained in each dimension , without COUNT, The default is 1
WIDTH 5  The width of the point cloud dataset 
HEIGHT 1  The height of the point cloud dataset 
VIEWPOINT 0 0 0 1 0 0 0  Specify the viewpoint and angle obtained by the point cloud , Use... When converting between different coordinate systems （ from 3 A translation +4 Quaternions form ）
POINTS 5  Total points （ It seems superfluous ）
DATA ascii  The data type for storing point cloud data ,ASCII and binary

Add ：PCL in PCD The official release of the file format is 0.7 edition , Before that 0.5,0.6 edition .

Sometimes to save space , Improve reading and writing efficiency , It will also binary To be serialized , the save Operation changed to

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

At this time, the output content is as follows ：

[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�

Note that the last line of output is saved in binary , Therefore, you cannot directly see the value .

2.3 ROS Real time acquisition and visualization of point cloud data

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";
  // Form 1 ：substr(pos,n) notes ：1.pos Indicates the subscript of the intercepted character .2.n Represents intercepting from pos After that n Characters .
  // Form 2 ：substr(n) notes ：1. if n>=0, Indicates that the intercept subscript is n All characters after .2. if n<0, Indicates the penultimate of the intercepted string n All characters after .
  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})