【PCL学习笔记】点云处理常用的库和API（PCL库+Eigen）

最近开始学习点云处理，发现要使用的PCL库和Eigen库有很多API都没不懂，现在边啃边记录一下。

一. PCL库

1. 首先是PointT的类型
   很多别人写的例程里，直接就用PointT来表示点云的类型，但是实际上PointT只是一个总的名称，它有很多种类型：

• PointXYZ：三维XYZ坐标信息
• PointXYZI：除了上述的XYZ坐标信息，还有一个强度信息，intensity
• PointXYZRGB：除了上述的XYZ坐标信息，还有RGB信息
• PointXY：只有XY坐标信息，这种类型用得比较多是在单线雷达里头
• …

2. 过滤指定范围的点：pcl::PassThrough pass; 需要设置几个参数，首先是选择沿什么坐标轴进行过滤，然后是设置过滤的范围，然后设置过滤范围内的点云还是范围外的点云

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

3. 降采样：pcl::VoxelGrid filter;
   VoxelGrid 体素降采样是点云降采样常用的手段。Voxel 翻译过来就是体素的意思，VoxelGrid 对点云进行体素化，创建一个三维体素栅格。在每个体素里面，求取该立方体内的所有点云重心点来代表这个立方体，从而达到降采样的目的。而leafsize越大，则表示体素越大，最后输出的点云点数越少。

    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 sor;，
   这里剔除离群点的方法是根据高斯分布来做的，对每个点，都圈起它附近的一部分点，多少根据需要来设，然后计算每个点到它们相邻点的距离，再求均值。如果得到的结果符合高斯分布，可以得到一个均值 μ 和一个标准差 σ。然后剔除原则就是，如果刚刚圈的这些点里面某些点在区间(μ + n * σ)外的话，就视作离群点。里面的这个n表示的是倍数，用来倍乘标准差，设得越大，覆盖的点就越多。
   所以，在调用时需要设置几个参数，首先是MeanK的值，也就是刚刚说的邻近点的数量，还有setStddevMulThresh设置标准差倍数

    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor; //remove the outlier
    sor.setInputCloud(cloud_downsampled);
    sor.setMeanK(5); //K近邻搜索点个数
    sor.setStddevMulThresh(1.0); //标准差倍数
    sor.setNegative(false); //保留未滤波点（内点）
    sor.filter(*cloud_filtered);  //保存滤波结果到cloud_filter

还有另一种去离群点的方法：根据空间点半径范围内临近点数量来剔除离群点，对应的类名是 RadiusOutlinerRemoval，设定每个点一定半径范围内要有几个邻点，满足的保留，不满足的剔除。半径和邻点数目的参数分别是setRadiusSearch和setMinNeighborsInRadius

pcl::RadiusOutlierRemoval<pcl::PointXYZ> pcFilter; //创建滤波器对象
pcFilter.setInputCloud(cloud); //设置待滤波的点云
pcFilter.setRadiusSearch( 0.8); // 设置搜索半径
pcFilter.setMinNeighborsInRadius( 2); // 设置一个内点最少的邻点数目
pcFilter.filter(*cloud_filtered); //滤波结果存储到cloud_filtered

5. pcl::transformPointCloud(*source_cloud, *target_cloud, transform)
   这个API可以将原始点云经过变换矩阵得到目标点云，其中的变换矩阵可以自己定义一个，通过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. 根据视锥对点云进行裁切
   视锥的获取方法，有很多，可以通过相机的真实FOV来定，也可以通过BBox换算得到，都是可以的。因为从二维到三维，缺一维的数据，所以需要设近平面距离（setNearPlaneDistance）和远平面距离（setFarPlaneDistance），也是下面图中的前裁剪平面和后裁剪平面，网上随便找的图，但是意思都是一样的。
   有了FOV和近跟远平面的距离后，还需要一个相机做的变换矩阵才行，这个矩阵的获取方法就很多了，具体情况具体分析就行。有了上面这些，就可以实现这个根据视锥裁切点云了
   

	  pcl::FrustumCulling<pcl::PointXYZ> fc_now_cloud;
		fc_now_cloud.setInputCloud(cloud_filtered);  //设置需要过滤的点云给滤波对象
    //std::cout<<"use second ToF?  "<
    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
   计算法向量：点云的法向量主要是通过点所在区域的局部拟合的表面进行计算。平面通过一个点和法向量进行表示。

   //计算表面法线
   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对象，并把它传递给法线估计对象
   ne_src.setSearchMethod(tree_src);
   pcl::PointCloud<pcl::Normal>::Ptr cloud_src_normals(new pcl::PointCloud< pcl::Normal>);
   ne_src.setRadiusSearch(0.02); //使用半径在查询点周围2厘米范围内的所有邻元素
   ne_src.compute(*cloud_src_normals); // 计算特征值

8. 读取点云文件中，每个点的数据

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 );

9. 手动输入点云信息
   跟通过pcd读入的点云数据不同，有时也需要把一些人为设定的点加入到点云的数组中，这里的数组是按照点云格式的数组，不是简单的数组信息。

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

  cloud_output->points.resize(100); //里面定义的100是这个点云的size，根据实际情况设定，这点很关键，不要设置多余的，因为多余的点默认是0 0 0，就会在原点有一堆重叠的0 0 0点
  cloud_output->points[0].x = 1.0;
  cloud_output->points[0].y = 1.1;
  cloud_output->points[0].z = 1.2;
  ...
  //自定义里面的xyz值就可以了，然后就可以常规操作，把这些手动输入的点云信息，在rviz可视化出来看看，有没有问题

  ros::Publisher cloud_output_Pub = node.advertise<sensor_msgs::PointCloud2>("/cloud_output", 1);

  sensor_msgs::PointCloud2 cloud_output_msg;

  pcl::toROSMsg(*cloud_output, cloud_output_msg);//把这个点云的msg发出去就可以了

  cloud_output_msg.header.frame_id = "odom";//这里也很关键，这些点基于什么坐标系下的，根据实际情况设定
  cloud_output_msg.header.stamp = ros::Time::now();
  cloud_output_Pub.publish(cloud_output_msg);

二. Eigen

变换矩阵有两种使用方式：
（1）普通矩阵 Matrix4f
这个方法不仅能建44的，33也可以，直接改成Matrix3f就行
说回4*4的变换矩阵，还是老结构 $\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]$

例程如下：

Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity(); // 定义一个单位4*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 轴上定义一个 2.5 米的平移.
transform_1 (0,3) = 2.5;
// 打印变换矩阵
std::cout << transform_1 << std::endl;

（2）使用 Affine3f，这个是专门用于矩阵的平移+旋转，这种类型有两个成员，分别对应平移和旋转

例程如下：

Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
// 在 X 轴上定义一个 2.5 米的平移.
transform_2.translation() << 2.5, 0.0, 0.0; // X Y Z
// Eigen::AngleAxisf 有两个参数，第一个是要旋转的角度，第二个设置绕什么轴转动
// 和前面一样的旋转; Z 轴上旋转 theta 弧度，AngelAxisf(theta, axis)得到一个3*3的旋转矩阵
transform_2.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitZ()));

（3）在不知道矩阵大小的情况下建立一个动态矩阵，使用 Eigen::Matrix src(M, N);
有时矩阵的大小是在计算过程中才能确定的，所以就需要用到上面这个动态参数来表示行和列。
注意，Eigen::Matrix定义的矩阵最大是，50*50的，所以如果行或列超过50，只能用上面的动态参数表示。

Reference

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