[PCL study notes] Commonly used libraries and APIs for point cloud processing (PCL library Eigen)

I recently started learning about point cloud processing,found to usePCL库和Eigen库有很多APII don't understand,Now nibble and record.

一. PCL库

1. 首先是PointT的类型
   In many routines written by others,直接就用PointTto indicate the type of point cloud,但是实际上PointTJust a general name,It has many types：

• PointXYZ：三维XYZ坐标信息
• PointXYZI：除了上述的XYZ坐标信息,There is also an intensity message,intensity
• PointXYZRGB：除了上述的XYZ坐标信息,还有RGB信息
• PointXY：只有XY坐标信息,This type is mostly used in single-line radar
• …

2. Filters the specified range of points：pcl::PassThrough<pcl::PointXYZ> pass; 需要设置几个参数,The first is to choose what axis to filter along,Then there is the scope of setting the filter,Then set the point cloud within the filter range or the point cloud outside the range

        pcl::PassThrough<pcl::PointXYZ> pass;// 创建滤波器对象
        pass.setInputCloud (cloud_raw_tf);//设置输入点云
        pass.setFilterFieldName ("y");//滤波字段名被设置为Y轴方向
        pass.setFilterLimits (-0.2, 0.5);//The acceptable range is flakesmarkerthickness range
        pass.setFilterLimitsNegative (true);//设置保留范围内 还是 过滤掉范围内
        pass.filter (*cloud_no_ground); //执行滤波,保存过滤结果在cloud_final*/

3. 降采样：pcl::VoxelGrid<pcl::PointXYZ> filter;
   VoxelGrid Voxel downsampling is a common method for point cloud downsampling.Voxel The translation is the meaning of voxel,VoxelGrid 对点云进行体素化,Create a 3D voxel grid.inside each voxel,Find the centroid of all point clouds in the cube to represent the cube,So as to achieve the purpose of downsampling.而leafsize越大,means the larger the voxel,The final output point cloud has fewer points.

    pcl::VoxelGrid<pcl::PointXYZ> filter;
    filter.setInputCloud(cloud_no_ground_tf);
    filter.setLeafSize(0.03f, 0.03f, 0.03f);
    filter.filter(*cloud_downsampled);

4. 去除离群点（噪点）：pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;,
   The method of removing outliers here is based on the Gaussian distribution,对每个点,Circle a part of the point near it,How much is set according to need,Then calculate the distance of each point to their neighbors,再求均值.If the obtained result conforms to a Gaussian distribution,An average value can be obtained μ 和一个标准差 σ.Then the principle of elimination is,If some of the points just circled are in the interval(μ + n * σ)外的话,regarded as outliers.里面的这个nRepresents a multiple,用来倍乘标准差,设得越大,The more points are covered.
   所以,Several parameters need to be set when calling,首先是MeanK的值,That is, the number of neighboring points just mentioned,还有setStddevMulThreshSet the standard deviation multiple

    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor; //remove the outlier
    sor.setInputCloud(cloud_downsampled);
    sor.setMeanK(5); //KThe number of nearest neighbor search points
    sor.setStddevMulThresh(1.0); //Standard deviation multiple
    sor.setNegative(false); //Keep unfiltered points（内点）
    sor.filter(*cloud_filtered);  //Save the filter results to cloud_filter

There is another way to remove outliers：Eliminate outliers based on the number of adjacent points within the radius of the spatial point,对应的类名是 RadiusOutlinerRemoval,Set each point to have several neighbors within a certain radius,Satisfied reservation,不满足的剔除.The parameters for radius and number of neighbors are respectivelysetRadiusSearch和setMinNeighborsInRadius

pcl::RadiusOutlierRemoval<pcl::PointXYZ> pcFilter; //创建滤波器对象
pcFilter.setInputCloud(cloud); //设置待滤波的点云
pcFilter.setRadiusSearch( 0.8); // 设置搜索半径
pcFilter.setMinNeighborsInRadius( 2); // Sets the minimum number of neighbors for an interior point
pcFilter.filter(*cloud_filtered); //滤波结果存储到cloud_filtered

5. pcl::transformPointCloud(*source_cloud, *target_cloud, transform)
   这个API可以将原始点云经过变换矩阵得到目标点云,One of the transformation matrices can be defined by yourself,通过Eigen,如：

   Eigen::Matrix4f T_trans;
   T_trans(0,0) = cos(M_PI/2);
   T_trans(0,1) = -sin(M_PI/2);
   T_trans(0,2) = 0;
   T_trans(1,0) = sin(M_PI/2);
   T_trans(1,1) = cos(M_PI/2);
   T_trans(1,2) = 0;
   T_trans(2,0) = 0;
   T_trans(2,1) = 0;
   T_trans(2,2) = 1;

   T_trans(0,3) = 0; //x
   T_trans(1,3) = 0; //y
   T_trans(2,3) = 0; //z
   
	pcl::transformPointCloud(*source_cloud, *target_cloud, T_trans)

6. Crop the point cloud according to the view frustum
   How to get the view cone,有很多,Reality that can pass the cameraFOV来定,也可以通过BBox换算得到,都是可以的.Because from 2D to 3D,One-dimensional data is missing,Therefore, it is necessary to set the near-plane distance（setNearPlaneDistance）and the far plane distance（setFarPlaneDistance）,Also the front and back clipping planes in the image below,Pictures you can find online,但是意思都是一样的.
   有了FOVand the distance between the near and far planes,You also need a transformation matrix made by the camera,There are many ways to obtain this matrix,Specific analysis on the line.有了上面这些,This can be achieved by clipping the point cloud according to the view frustum
   

	  pcl::FrustumCulling<pcl::PointXYZ> fc_now_cloud;
		fc_now_cloud.setInputCloud(cloud_filtered);  //设置需要过滤的点云给滤波对象
    //std::cout<<"use second ToF? "<<use_second_LiDAR <<std::endl;
    fc_now_cloud.setVerticalFOV(180*yaw_range/M_PI); // BBox V FoV
    fc_now_cloud.setHorizontalFOV(180*pitch_range/M_PI);  BBox H FoV
    //fc_now_cloud.setVerticalFOV(30); // BBox V FoV
    //fc_now_cloud.setHorizontalFOV(15); BBox H FoV
    fc_now_cloud.setNearPlaneDistance(0.0); //min disinfection distance
    fc_now_cloud.setFarPlaneDistance(2.0); //max disinfection distance
    //ROS_INFO("pcl::FrustumCulling set param. done");


    //transform "frustum" from /usb_cam to /pico_flexx_optical_frame (main) pico_flexx_optical_frame ---> /UV; /usb_cam; /pico_flexx_second_optical_frame 
    transform_bboxcenter = Eigen::Affine3f::Identity();
    transform_bboxcenter.rotate (Eigen::AngleAxisf (-M_PI/2, Eigen::Vector3f::UnitY()));
    transform_bboxcenter.rotate (Eigen::AngleAxisf (M_PI/2, Eigen::Vector3f::UnitX()));
    transform_bboxcenter.rotate (Eigen::AngleAxisf (-yaw, Eigen::Vector3f::UnitZ()));
    if (use_second_LiDAR == true)
    {
    
      std::cout<<"camera original pitch: "<< pitch << " extra part: "<< M_PI * 22.5/180 << std::endl << std::endl << std::endl << std::endl;
      transform_bboxcenter.rotate (Eigen::AngleAxisf (pitch + M_PI * 22.5/180 , Eigen::Vector3f::UnitY()));
    }
    else
    {
    
      transform_bboxcenter.rotate (Eigen::AngleAxisf (pitch, Eigen::Vector3f::UnitY()));
    }
    transform_bboxcenter.rotate (Eigen::AngleAxisf ( - yaw, Eigen::Vector3f::UnitZ()));
    camera_now_pose = Eigen::Matrix4f::Identity();
    camera_now_pose(0, 0) = transform_bboxcenter(0, 0);
    camera_now_pose(1, 0) = transform_bboxcenter(1, 0);
    camera_now_pose(2, 0) = transform_bboxcenter(2, 0);
    camera_now_pose(0, 1) = transform_bboxcenter(0, 1);
    camera_now_pose(1, 1) = transform_bboxcenter(1, 1);
    camera_now_pose(2, 1) = transform_bboxcenter(2, 1);
    camera_now_pose(0, 2) = transform_bboxcenter(0, 2);
    camera_now_pose(1, 2) = transform_bboxcenter(1, 2);
    camera_now_pose(2, 2) = transform_bboxcenter(2, 2);
    camera_now_pose(0, 3) = 0.065; //x
    camera_now_pose(1, 3) = 0.015; //y
    camera_now_pose(2, 3) = 0.0;   //z
    fc_now_cloud.setCameraPose(camera_now_pose);
    fc_now_cloud.filter(*cloud_frustum);

7. pcl::NormalEstimation
   计算法向量：The normal vector of the point cloud is mainly calculated by the locally fitted surface of the area where the point is located.A plane is represented by a point and normal vector.

   //Calculate surface normals
   pcl::NormalEstimation<pcl::PointXYZ,pcl::Normal> ne_src;
   ne_src.setInputCloud(cloud_src);
   pcl::search::KdTree< pcl::PointXYZ>::Ptr tree_src(new pcl::search::KdTree< pcl::PointXYZ>()); //创建一个空的kdtree对象,and pass it to the normal estimation object
   ne_src.setSearchMethod(tree_src);
   pcl::PointCloud<pcl::Normal>::Ptr cloud_src_normals(new pcl::PointCloud< pcl::Normal>);
   ne_src.setRadiusSearch(0.02); //Use a radius around the query point2厘米范围内的所有邻元素
   ne_src.compute(*cloud_src_normals); // 计算特征值

8. Read the point cloud file,data for each point

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

if(pcl::io::loadPCDFile<pcl::PointXYZ>("test_file.pcd",*cloud)==-1)
{
    
    PCL_ERROR("Couldn't read file test_pcd.pcd\n");
    return(-1);
}

for(size_t i=0;i<cloud->points.size();++i)
    printf("%lf %lf %lf", cloud->points[i].x, cloud->points[i].y, cloud->points[i].z );

二. Eigen

Transformation matrices can be used in two ways：
（1）普通矩阵 Matrix4f
This method can not only build44的,33也可以,直接改成Matrix3f就行
说回4*4的变换矩阵,Still the old structure $\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]$

例程如下：

Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity(); // Define a unit4*4矩阵
float theta = M_PI/4; // 弧度角
transform_1 (0,0) = cos (theta);
transform_1 (0,1) = -sin(theta);
transform_1 (1,0) = sin (theta);
transform_1 (1,1) = cos (theta);
// 在 X Define one on the axis 2.5 translation in meters.
transform_1 (0,3) = 2.5;
// Print the transformation matrix
std::cout << transform_1 << std::endl;

（2）使用 Affine3f,This is dedicated to translation of matrices+旋转,This type has two members,corresponding to translation and rotation, respectively

例程如下：

Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
// 在 X Define one on the axis 2.5 translation in meters.
transform_2.translation() << 2.5, 0.0, 0.0; // X Y Z
// Eigen::AngleAxisf 有两个参数,The first is the angle to rotate,The second sets what axis to rotate about
// Same rotation as before; Z 轴上旋转 theta 弧度,AngelAxisf(theta, axis)得到一个3*3的旋转矩阵
transform_2.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitZ()));

（3）Build a dynamic matrix without knowing the size of the matrix,使用 Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> src(M, N);
Sometimes the size of the matrix is ​​only determined during the calculation process,So you need to use the above dynamic parameters to represent rows and columns.
注意,Eigen::MatrixThe defined matrix is ​​at most ,50*50的,So if the row or column exceeds50,Can only be represented by the dynamic parameters above.

Reference

1. pcl_filters模块api代码解析
2. PCL学习笔记(30)——法线估计normal_estimation