The brother asked a small question: understand PCL's voxelgrid filter, and verify how the intensity of the point cloud changes. This article investigates this issue.
Article directory
principle:
introduction
SLAM functions are generally divided into two types: positioning and mapping. There are many types of maps, sparse, dense, and semi-sparse. The point cloud map is a sparse map. When zoomed in, it is a discrete space point, but we can turn it into a continuous dense grid. This process Also called point cloud meshing. The meshing of the point cloud requires a series of processing on the point cloud, and the first step is to filter the point cloud.
Why filter the point cloud?
Filtering originated from the concept of digital signal processing, which can be understood as a filter, which is a preprocessing method for point clouds. You can observe the following figure, the left side is the original point cloud, and the right side is the "impurity" filtered out after filtering;
however, not all point clouds need to be filtered. In general, the following situations require point cloud filtering:
(1) Point cloud data density is irregular and needs to be smoothed (2) Outliers need to be removed
due to problems such as occlusion (3) A large amount of data needs to be down-sampled (4) ) Noise data needs to be removed Among them, there are several sources of noise in the point cloud. On the one hand, it comes from the equipment. For example, when we use laser scanners, RGB-D cameras and other equipment to obtain point cloud data, noise will be introduced due to equipment accuracy and diffraction characteristics of electromagnetic waves. On the other hand, it comes from the impact of environmental factors, such as changes in the surface properties of the measured object. Another important aspect is the influence of the operator's experience, such as some noise introduced in the process of processing point cloud data splicing and registration. Noise points in the point cloud have a greater impact on subsequent operations. If even the experimental data has large errors, then there is not much scientific research value. There is a special point cloud filtering module in PCL, which can remove noise points and perform operations such as point cloud compression, which is very flexible and practical, such as: bilateral filtering, statistical filtering, conditional filtering, random sampling consistency filtering, etc. In this way, subsequent application processing such as registration, feature extraction, surface reconstruction, and visualization can be better performed . The filtering module mainly calls some packaged filtering functions, and then sets the parameters according to the needs, which is still very intuitive. In general, there are several schemes for filtering:
(1) Filter and remove points according to the given rules
(2) Modify some attributes of points through common filtering algorithms
(3) Downsample the data
Point Cloud Downsampling
Why downsampling? Once you know why, you will know when to use it! Let me give you an example, point cloud fusion, a 640x480 Depth image, if each place has a depth value, it can be converted into a point cloud composed of 300,000 points, if there are dozens or hundreds of images such violent fusion, Then the fused point cloud will become bigger and bigger, and storage and operation will be a big problem! It is equivalent to having a large-capacity sample, and I draw a representative sample from it according to certain rules, which can replace the original sample. The voxel filter can achieve the function of downsampling without destroying the geometric structure of the point cloud itself, but it will move the position of the point .
There is a dedicated class in this downsampling PCL called:
class pcl::ApproximateVoxelGrid< PointT >
The realization principle of downsampling
It is more suitable for data compression of massive point clouds before processing, and can select appropriate voxel (voxel) size and other parameters in feature extraction and other processing to improve algorithm efficiency. This function creates a three-dimensional voxel grid for the input point cloud data. Each voxel uses the center of gravity of all points in the voxel to approximate other points in the voxel, so that all points in the voxel use a center of gravity for the final express. Its advantage is that it can preserve the shape features of the point cloud when downsampling. (Or calculate a cube that can just wrap the point cloud, and then divide the large cube into different small cubes according to the set resolution. For each point in the small cube, calculate their centroid, and use the The coordinates of the centroid are used to approximate several points in the cube.)
Look at the result below, the left is to be processed, the right is before processing, and the right is after processing. The
key code is only four lines:
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;// 创建滤波对象
sor.setInputCloud (cloud); // 设置需要过滤的点云给滤波对象
sor.setLeafSize (0.01f, 0.01f, 0.01f); // 设置滤波时创建的体素体积为边长1cm的立方体
sor.filter (*cloud_filtered); // 执行滤波处理，存储输出
Two commonly used functions:
setLeafSize( float lx, float ly, float lz);
The three parameters after setLeafSize indicate the size of the voxel grid leaf, which respectively indicate the size of the voxel in the XYZ direction, in meters. The above is set to be 1cm in length, width and height.
setDownsampleAllData(bool downsample);
Set whether to downsample all fields. There are different types of point clouds, for example, some are PointXYZ, some are PointXYZRGB, and there are other types, that is, a point contains a variety of different information, such as spatial position XYZ, color information RGB, or intensity information, etc. If you want to compare all information ( field) downsampling is set to true, if only XYZ downsampling is set to false
How to fully understand the function of each class?
The first step is to log in to the PCL API documentation.
The second step: enter the name of the template class we want to query in the search box in the upper right corner. Here we enter ApproximateVoxelGrid, as shown below, and the matching results will automatically pop up
Step 3: Select the class name you are looking for, click to enter, and you will arrive at the following interface, which lists all member variables and member functions. Click on each member function, and you will jump to the corresponding explanation interface
Remove outliers from the point cloud
Use it again.
Code implementation part:
CMakeLists.txt
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
project(voxel_grid)
find_package(PCL 1.8 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})
Observation position (x, y, z) information after filtering
voxel_grid.cpp
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/console/parse.h> //console中的字段的头文件
#include <pcl/visualization/cloud_viewer.h>
int main(int argc, char** argv)
{
std::vector<int> filenames; //创建一个动态数组用于保存文件名
filenames = pcl::console::parse_file_extension_argument(argc, argv, ".pcd"); //文件名通过输入来定义
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>); //定义共享指针
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filter(new pcl::PointCloud<pcl::PointXYZI>);
pcl::io::loadPCDFile("table_scene_lms400.pcd", *cloud);
pcl::visualization::CloudViewer viewer1("Cloud Viewer1");
viewer1.showCloud(cloud); //showcloud函数是同步的，会在此处等到渲染显示
while (!viewer1.wasStopped())
{
}
std::cerr << "PointCloud before filtering:" << cloud->width * cloud->height << "data points(" << pcl::getFieldsList(*cloud) << ")." << std::endl;
pcl::VoxelGrid<pcl::PointXYZI> sor; //实例化一个VoxelGrid滤波器
sor.setInputCloud(cloud);
sor.setLeafSize(0.01f, 0.01f, 0.01f); //设置处理时所使用的体素的大小
sor.filter(*cloud_filter);
std::cerr << "PointCloud after filtering:" << cloud_filter->width * cloud_filter->height << "data points(" << pcl::getFieldsList(*cloud_filter) << ")." << endl;
pcl::io::savePCDFileASCII("output_downsampled", *cloud_filter);
pcl::visualization::CloudViewer viewer2("Cloud Viewer2");
viewer2.showCloud(cloud_filter); //showcloud函数是同步的，会在此处等到渲染显示
while (!viewer2.wasStopped())
{
}
return(0);
}
operation result:
angle1:
angle2:
It can be seen that the number of point clouds is approximately 0.1 times the original when the side length of the cube is 1cm, and the downsampling effect is obvious. The larger the LeafSize is set, the larger the voxel grid will be, and the filtering effect will be more obvious:
sor.setLeafSize(0.02f, 0.02f, 0.02f);
When the LeafSize is 0.02, the result:
it can be clearly observed that the density of the filtered points is significantly smaller than before, and the actual output data also shows that only the original 1 40 \frac{1}{40}401。
After filtering, observe the intensity (intensity) information
It is reasonable to say that the intensity should be the center of mass like the position, but the position can be clearly observed from the figure, but the value of the intensity is not obvious in the figure. So I went ahead and tested with points from a custom point cloud.
Save test data in PCD format
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
int main(int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZI> cloud;
// Fill in the cloud data
cloud.width = 3; //点云个数
cloud.height = 1; //无序点云
cloud.is_dense = false; //不是密集点云
cloud.points.resize(cloud.width * cloud.height); //确定点云的大小
//定义3个点，并存入cloud中
cloud.points[0].x = 0.1;
cloud.points[0].y = 0.1;
cloud.points[0].z = 0.1;
cloud.points[0].intensity = 0.1;
cloud.points[1].x = 0.2;
cloud.points[1].y = 0.2;
cloud.points[1].z = 0.2;
cloud.points[1].intensity = 0.2;
cloud.points[2].x = 0.3;
cloud.points[2].y = 0.3;
cloud.points[2].z = 0.3;
cloud.points[2].intensity = 0.3;
//将cloud中的点云数据保存至test_pcd.pcd文件中
pcl::io::savePCDFileASCII("test_pcd.pcd", cloud);
std::cerr << "Saved " << cloud.points.size() << " data points to test_pcd.pcd." << std::endl;
return (0);
}
Read PCD format data and output
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
int main(int argc, char** argv)
{
//创建一个保存点云的对象
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
//从文件中加载点云
if (pcl::io::loadPCDFile<pcl::PointXYZI>("test_pcd.pcd", *cloud) == -1) //* load the file
{
PCL_ERROR("Couldn't read file test_pcd.pcd \n");
return (-1);
}
std::cout << "Loaded "
<< cloud->width * cloud->height
<< " data points from test_pcd.pcd with the following fields: "
<< std::endl;
//打印点云
for (std::size_t i = 0; i < cloud->points.size(); ++i)
std::cout << " " << cloud->points[i].x
<< " " << cloud->points[i].y
<< " " << cloud->points[i].z
<< " " << cloud->points[i].intensity << std::endl;
return (0);
}
Small amount of test data code
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/console/parse.h> //console中的字段的头文件
#include <pcl/visualization/cloud_viewer.h>
int main(int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>); //定义共享指针
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filter(new pcl::PointCloud<pcl::PointXYZI>);
pcl::io::loadPCDFile("test_pcd.pcd", *cloud);
std::cerr << "PointCloud before filtering:" << cloud->width * cloud->height << "data points(" << pcl::getFieldsList(*cloud) << ")." << std::endl;
pcl::VoxelGrid<pcl::PointXYZI> sor; //实例化一个VoxelGrid滤波器
sor.setInputCloud(cloud);
sor.setLeafSize(0.4, 0.4, 0.4); //设置处理时所使用的体素的大小
sor.filter(*cloud_filter);
std::cerr << "PointCloud after filtering:" << cloud_filter->width * cloud_filter->height << "data points(" << pcl::getFieldsList(*cloud_filter) << ")." << endl;
pcl::io::savePCDFileASCII("test_after.pcd", *cloud_filter);
return(0);
}
The preliminary conclusions are as follows:
After modifying the above few data for multiple tests, I found that the cube selected by the voxel filter should start from (0, 0, 0) coordinates. In this example, I used the leaf size as (0.4, 0.4, 0.4 ) (unit is M), then the point cloud coordinates after voxel filtering are actually the centroids of all points in x:[0,0.4), y:[0,0.4), z:[0,0.4) ( Note: It contains 0 but does not contain 0.4, the value of negative numbers and subsequent points should obey this rule , such as [-0.4,0)). And the coordinate calculation of the center of mass is very simple, namely:
( x ′ , y ′ , z ′ , I ′ ) = ∑ 0 ⩽ i < n ( xi , yi , zi , I i ) n , where n is the Number(x^{'},y^{'},z^{'},I^{'}) = \frac{\sum\limits_{0\leqslant i＜n}{(x_i,y_i,z_i ,I_i)}}{n}, where n is the number of points in the range(x′,y′,z′,I′)=n0⩽i＜n∑(xi,yi,zi,Ii), where n is the number of points within the range
, and of course the intensity follows the points.
Medium data volume test the above conclusions:
The test code is as follows
input.cpp
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
int main(int argc, char** argv)
{
srand(time(NULL)); //随机数种子
pcl::PointCloud<pcl::PointXYZI> cloud;
// Fill in the cloud data
cloud.width = 100; //点云个数
cloud.height = 1; //无序点云
cloud.is_dense = false; //不是密集点云
cloud.points.resize(cloud.width * cloud.height); //确定点云的大小
double c[8][4] = {
{
0,0,0,0}, {
0,0,0,0},{
0,0,0,0} ,{
0,0,0,0},{
0,0,0,0}, {
0,0,0,0},{
0,0,0,0} ,{
0,0,0,0} };
//打算取leafsize为0.5，即将大小为1的立方体分割为8块(即第一个纬度为8)，所以应当有8个质心（点），第二个纬度分别代表(x,y,z,I)
int n[8] = {
0,0,0,0,0,0,0,0}; //记录方块内的数据个数
//生成100个0~1的随机数，并存入cloud中
for (std::size_t i = 0; i < cloud.points.size(); ++i)
{
cloud.points[i].x = rand() / (RAND_MAX + 1.0f); //随即生成[0,1)
cloud.points[i].y = rand() / (RAND_MAX + 1.0f);
cloud.points[i].z = rand() / (RAND_MAX + 1.0f);
cloud.points[i].intensity = rand() / (RAND_MAX + 1.0f);
if(cloud.points[i].x >= 0 && cloud.points[i].x < 0.5 && cloud.points[i].y >=0 && cloud.points[i].y < 0.5 && cloud.points[i].z>=0 && cloud.points[i].z < 0.5)
{
c[0][0] += cloud.points[i].x;
c[0][1] += cloud.points[i].y;
c[0][2] += cloud.points[i].z;
c[0][3] += cloud.points[i].intensity;
n[0]++;
}
else if(cloud.points[i].x >= 0 && cloud.points[i].x < 0.5 && cloud.points[i].y >=0 && cloud.points[i].y < 0.5 && cloud.points[i].z>=0.5 && cloud.points[i].z < 1)
{
c[1][0] += cloud.points[i].x;
c[1][1] += cloud.points[i].y;
c[1][2] += cloud.points[i].z;
c[1][3] += cloud.points[i].intensity;
n[1]++;
}
else if(cloud.points[i].x >= 0 && cloud.points[i].x < 0.5 && cloud.points[i].y >=0.5 && cloud.points[i].y < 1 && cloud.points[i].z>=0 && cloud.points[i].z < 0.5)
{
c[2][0] += cloud.points[i].x;
c[2][1] += cloud.points[i].y;
c[2][2] += cloud.points[i].z;
c[2][3] += cloud.points[i].intensity;
n[2]++;
}
else if(cloud.points[i].x >= 0 && cloud.points[i].x < 0.5 && cloud.points[i].y >=0.5 && cloud.points[i].y < 1 && cloud.points[i].z>=0.5 && cloud.points[i].z < 1)
{
c[3][0] += cloud.points[i].x;
c[3][1] += cloud.points[i].y;
c[3][2] += cloud.points[i].z;
c[3][3] += cloud.points[i].intensity;
n[3]++;
}
else if(cloud.points[i].x >= 0.5 && cloud.points[i].x < 1 && cloud.points[i].y >=0 && cloud.points[i].y < 0.5 && cloud.points[i].z>=0 && cloud.points[i].z < 0.5)
{
c[4][0] += cloud.points[i].x;
c[4][1] += cloud.points[i].y;
c[4][2] += cloud.points[i].z;
c[4][3] += cloud.points[i].intensity;
n[4]++;
}
else if(cloud.points[i].x >= 0.5 && cloud.points[i].x < 1 && cloud.points[i].y >=0 && cloud.points[i].y < 0.5 && cloud.points[i].z>=0.5 && cloud.points[i].z < 1)
{
c[5][0] += cloud.points[i].x;
c[5][1] += cloud.points[i].y;
c[5][2] += cloud.points[i].z;
c[5][3] += cloud.points[i].intensity;
n[5]++;
}
else if(cloud.points[i].x >= 0.5 && cloud.points[i].x < 1 && cloud.points[i].y >=0.5 && cloud.points[i].y < 1 && cloud.points[i].z>=0 && cloud.points[i].z < 0.5)
{
c[6][0] += cloud.points[i].x;
c[6][1] += cloud.points[i].y;
c[6][2] += cloud.points[i].z;
c[6][3] += cloud.points[i].intensity;
n[6]++;
}
else if(cloud.points[i].x >= 0.5 && cloud.points[i].x < 1 && cloud.points[i].y >=0.5 && cloud.points[i].y < 1 && cloud.points[i].z>=0.5 && cloud.points[i].z < 1)
{
c[7][0] += cloud.points[i].x;
c[7][1] += cloud.points[i].y;
c[7][2] += cloud.points[i].z;
c[7][3] += cloud.points[i].intensity;
n[7]++;
}
}
for(int i = 0;i < 8; i++)
for(int j = 0;j < 4; j++)
c[i][j] = c[i][j] / n[i]; //将前面的范围内的点求质心
for(int i = 0;i < 8; i++)
{
std::cout << "第"<< i + 1 << "个块有" << n[i] << "个数"<< std::endl;
std::cout << "第"<< i + 1 << "个点为" <<"("<< c[i][0]<<","<< c[i][1] << "," << c[i][2] << "," << c[i][3] << ")" << std::endl;
}
//将cloud中的点云数据保存至test_pcd.pcd文件中
pcl::io::savePCDFileASCII("test_pcd.pcd", cloud);
std::cerr << "Saved " << cloud.points.size() << " data points to test_pcd.pcd." << std::endl;
return (0);
}
test.cpp
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/console/parse.h> //console中的字段的头文件
#include <pcl/visualization/cloud_viewer.h>
int main(int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>); //定义共享指针
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filter(new pcl::PointCloud<pcl::PointXYZI>);
pcl::io::loadPCDFile("test_pcd.pcd", *cloud);
std::cerr << "PointCloud before filtering:" << cloud->width * cloud->height << "data points(" << pcl::getFieldsList(*cloud) << ")." << std::endl;
pcl::VoxelGrid<pcl::PointXYZI> sor; //实例化一个VoxelGrid滤波器
sor.setInputCloud(cloud);
sor.setLeafSize(0.5, 0.5, 0.5); //设置处理时所使用的体素的大小
sor.filter(*cloud_filter);
std::cerr << "PointCloud after filtering:" << cloud_filter->width * cloud_filter->height << "data points(" << pcl::getFieldsList(*cloud_filter) << ")." << endl;
pcl::io::savePCDFileASCII("test_after.pcd", *cloud_filter);
return(0);
}
test_after.pcd
# .PCD v0.7 - Point Cloud Data file format
VERSION 0.7
FIELDS x y z intensity
SIZE 4 4 4 4
TYPE F F F F
COUNT 1 1 1 1
WIDTH 8
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS 8
DATA ascii
0.13851911 0.24136128 0.33553141 0.40700749
0.64431816 0.24781057 0.24998038 0.53771544
0.29668111 0.72371972 0.30309331 0.58366519
0.76265746 0.68379813 0.26003802 0.55480695
0.30543846 0.25518233 0.69257158 0.61186677
0.75259429 0.23724338 0.78009278 0.4655337
0.1827049 0.72605532 0.72325444 0.63117778
0.76790327 0.79831183 0.74001992 0.52766722
The above results can be verified that indeed meet the preliminary conclusions. And it can also be observed that the data arrangement in the point cloud is according to x , y , zx,y,zx,y,The filtered points are output in the order that z increases successively, and the author usesz , y , xz,y,xz,y,The order in which x increases sequentially is output, so there is a slight difference. The boundary points are relatively simple, so the author will not show them here. Readers only need to set up one or two points for verification.
References:
Reference to learn SLAM together from scratch
| Add a filter to the
point
cloud
pcl library and configure vscode to use cmake to generate executable files