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

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

一. PCL库

  1. 首先是PointT的类型
    很多别人写的例程里,直接就用PointT来表示点云的类型,但是实际上PointT只是一个总的名称,它有很多种类型:
  • PointXYZ:三维XYZ坐标信息
  • PointXYZI:除了上述的XYZ坐标信息,还有一个强度信息,intensity
  • PointXYZRGB:除了上述的XYZ坐标信息,还有RGB信息
  • PointXY:只有XY坐标信息,这种类型用得比较多是在单线雷达里头
  1. 过滤指定范围的点: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*/
  1. 降采样: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);
  1. 去除离群点(噪点):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,设定每个点一定半径范围内要有几个邻点,满足的保留,不满足的剔除。半径和邻点数目的参数分别是setRadiusSearchsetMinNeighborsInRadius

pcl::RadiusOutlierRemoval<pcl::PointXYZ> pcFilter; //创建滤波器对象
pcFilter.setInputCloud(cloud); //设置待滤波的点云
pcFilter.setRadiusSearch( 0.8); // 设置搜索半径
pcFilter.setMinNeighborsInRadius( 2); // 设置一个内点最少的邻点数目
pcFilter.filter(*cloud_filtered); //滤波结果存储到cloud_filtered
  1. 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)
  1. 根据视锥对点云进行裁切
    视锥的获取方法,有很多,可以通过相机的真实FOV来定,也可以通过BBox换算得到,都是可以的。因为从二维到三维,缺一维的数据,所以需要设近平面距离(setNearPlaneDistance)和远平面距离(setFarPlaneDistance),也是下面图中的前裁剪平面和后裁剪平面,网上随便找的图,但是意思都是一样的。
    有了FOV和近跟远平面的距离后,还需要一个相机做的变换矩阵才行,这个矩阵的获取方法就很多了,具体情况具体分析就行。有了上面这些,就可以实现这个根据视锥裁切点云了
    【PCL学习笔记】点云处理常用的库和API(PCL库+Eigen)_第1张图片
	  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);
  1. 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); // 计算特征值
  1. 读取点云文件中,每个点的数据
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 );
  1. 手动输入点云信息
    跟通过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的变换矩阵,还是老结构 [ R T 0 1 ] \begin{bmatrix} R & T \\ 0 & 1 \end{bmatrix} [R0T1]

例程如下:

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

你可能感兴趣的:(PCL,Learning,PCL)