ROS-读取pcd点云数据进行滤波并在Rviz中显示

环境:
Ubuntu16.04
ROS Kinetic
C++
创建工作空间:

cd Downloads/ROS
mkdir -p PointCloudFilter_ws/src
cd PointCloudFilter_ws/src
catkin_init_workspace
cd ..
catkin_make

将该工作空间的setup文件路径放到Home路径下的.bashrc文件中:
创建功能包

 cd src
 catkin_create_pkg pointcloudfilter_pkg pcl_conversions pcl_ros roscpp sensor_msgs

在功能包路径下的src文件夹中新建pointcloudfilter_.cpp文件,并输入下面的代码:

#include <iostream>
#include<ros/ros.h>
#include<pcl_conversions/pcl_conversions.h>
#include<sensor_msgs/PointCloud2.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/crop_box.h>
#include <pcl/visualization/cloud_viewer.h>
 
int main(int argc,char **argv)
{
    ros::init(argc, argv, "pointfilter");
    ros::NodeHandle nh;
    ros::Publisher pcl_pub=nh.advertise<sensor_msgs::PointCloud2>("pointfilter_output",1);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PCDReader reader;
	reader.read("/home/wyh/Downloads/ROS/pcdreadshow_ws/src/read_pcd/src/data/pcd/data_1/0000000001.pcd", *cloud);
 
	pcl::VoxelGrid<pcl::PointXYZ> vg;   //滤波对象
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFiltered(new pcl::PointCloud<pcl::PointXYZ>);//创建保存体素滤波后的点对象cloudFiltered
	vg.setInputCloud(cloud);//输入cloud
	vg.setLeafSize(0.4, 0.4, 0.4);设置叶子大小（这么大个叶子节点内只提取一个点）
	vg.filter(*cloudFiltered);//滤波后的点云保存在cloudFiltered
							  
	//过滤掉在用户给定立方体内的点云数据
	//理解：将自身车辆作为坐标轴的中心点，然后在身边自身为中心 ，圈出范围，成为每一次运动时候的感兴趣区域，也就是只关心区域内点的聚类等后续操作
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudRegion(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::CropBox<pcl::PointXYZ> region(true);
	//Eigen::Vector4f minpoint(-10, -6.5, -2, 1);
	//Eigen::Vector4f maxpoint(30, 6.5, 1, 1);
	region.setMin(Eigen::Vector4f(-10, -6.5, -2, 1));
	region.setMax(Eigen::Vector4f(30, 6.5, 1, 1));
	region.setInputCloud(cloudFiltered);
	region.filter(*cloudRegion);
 
	//提取车身周围范围内的所有的点，并将提取到的所有点保存在indices中
	std::vector<int> indices;
	pcl::CropBox<pcl::PointXYZ> roof(true);
	roof.setMin(Eigen::Vector4f(-1.5, -1.7, -1, 1));//看数据像是车身大小
	roof.setMax(Eigen::Vector4f(2.6, 1.7, -0.4, 1));
	roof.setInputCloud(cloudRegion);//输入的是上部中的立方体
	roof.filter(indices);
	pcl::PointIndices::Ptr inliers{ new pcl::PointIndices }; //创建一个内点对象，将提取到车身周围点，放到内点对象中
	for (int point : indices) 
	{
		inliers->indices.push_back(point);
	}
 
	//创建点云提取函数
	pcl::ExtractIndices<pcl::PointXYZ> extract;
	extract.setInputCloud(cloudRegion);
	extract.setIndices(inliers);
	extract.setNegative(true);  //false 提取内点也就是提取车身周围的几个点，， true提取出了车身周围的点
	extract.filter(*cloudRegion);
 
	// /*pcl::PCDWriter writer;
	// writer.write("C:\\Users\\Administrator\\Downloads\\求助\\求助\\tree-2-Rend.pcd", *cloud_filtered1);*/
 
	// pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("显示窗口"));  //窗口显示点云
	// viewer->addPointCloud(cloudRegion, "*cloud");
	// viewer->resetCamera();		//相机点重置
	// viewer->spin();

    sensor_msgs::PointCloud2 output;
    pcl::toROSMsg(*cloudRegion,output);
    output.header.frame_id="odom";
    
    ros::Rate loop_rate(1);
    while(ros::ok()){
        pcl_pub.publish(output);
        ros::spinOnce();
        loop_rate.sleep();
    }

	//system("pause");
	return (0);
}

将下面的编译规则写到CMAKE文件中:

add_executable(pointcloudfilter_ src/pointcloudfilter_.cpp)
target_link_libraries(pointcloudfilter_ ${catkin_LIBRARIES})

回到工作空间路径下,在终端中输入catkin_make进行编译
打开多个终端依次输入下面的指令:

roscore
rosrun pointcloudfilter_pkg pointcloudfilter_
rviz

打开rviz后注意佐鸣参数的配置,根据途中相应配置来即可.


代码涉及到的相关知识:
FilterCloud()所包括的功能：
1.首先使用体素滤波（相当于做稀释减少点的数量）（体素网格过滤将创建一个立方体网格, 过滤点云的方法是每个体素立方体内只留下一个点, 因此立方体每一边的长度越大, 点云的分辨率就越低. 但是如果体素网格太大, 就会损失掉物体原本的特征。）

2.过滤掉给定立方体之外的点云数据。（定义感兴趣区域, 并删除感兴趣区域外的任何点. 感兴趣区域的选择两侧需要尽量覆盖车道的宽度, 而前后的区域要保证你可以及时检测到前后车辆的移动。 在最终结果中, 我们使用pcl CropBox 查找自身车辆车顶的点云数据索引, 然后将这些索引提供给 pcl ExtractIndices 对象删除, 因为这些对于我们分析点云数据没有用处。）（相当于将自身车辆作为坐标轴的中心点（原点），然后以自身为中心 ，圈出一个立方体的范围（该立方体只需要知道对角线上的两个点AB坐标即可确定），成为每一次运动时候的感兴趣区域，也就是只关心区域内点的聚类等后续操作。如下图：立方体之外的就不要了。）

3.提取车身周围小范围内的所有的点，并将提取到的所有点保存在indices中。相当于在上图立方体内再扣掉一个立方体（扣掉的这个立方体即为车身范围内的即把车这部分没采取到的盲区给去掉。）如上图右图。

1、Eigen是可以用来进行线性代数、矩阵、向量操作等运算的C++库，它里面包含了很多算法。
Eigen：矩阵(Matrix)类的介绍及使用（在Eigen中，所有矩阵和向量均为Matrix模板类的对象，向量是矩阵的行（或列）为1是的特殊情况。）
1.1、矩阵的三参数模板
Matrix类有六个模板参数，其中三个有默认值，因此只要学习三个参数就足够了。

/* 强制性的三参数模板的原型 (三个参数分别表示：标量的类型，编译时的行，编译时的列) */
Matrix<typename Scalar, int RowsAtCompileTime, int ColsAtCompileTime> 
 
/* 用typedef定义了很多模板，例如：Matrix4f 表示 4×4 的floats 矩阵 */ 
typedef Matrix<float, 4, 4> Matrix4f;

1.2、向量（Vectors）
向量是矩阵的特殊情况，也是用矩阵定义的。

typedef Matrix<float, 3, 1> Vector3f;  //"Vector3f"直接定义了一个维度为3的列向量，3行1列
typedef Matrix<int, 1, 2> RowVector2i;//行向量1行2列

1.3、特殊动态值（special value Dynamic）
Eigen的矩阵不仅能够在编译是确定大小（fixed size），也可以在运行时确定大小，就是所说的动态矩阵（dynamic size）。

typedef Matrix<double, Dynamic, Dynamic> MatrixXd; //行列都不固定 
typedef Matrix<int, Dynamic, 1> VectorXi;  //行不限制固定列
 
/* 也可使用‘行’固定‘列’动态的矩阵 */
Matrix<float, 3, Dynamic>

1.4、构造函数（Constructors）
可以使用默认的构造函数，不执行动态分配内存，也没有初始化矩阵参数：

Matrix3f a;   // a是3-by-3矩阵，包含未初始化的 float[9] 数组
MatrixXf b;   // b是动态矩阵，当前大小为 0-by-0, 没有为数组的系数分配内存
 
/* 矩阵的第一个参数表示“行”，数组只有一个参数。根据跟定的大小分配内存，但不初始化 */
MatrixXf a(10,15);    // a 是10-by-15阵，分配了内存，没有初始化
VectorXf b(30);       // b是动态矩阵，当前大小为 30, 分配了内存，没有初始化
 
/* 对于给定的矩阵，传递的参数无效 */
Matrix3f a(3,3); 
 
/* 对于维数最大为4的向量，可以直接初始化 */
Vector2d a(5.0, 6.0);  
Vector3d b(5.0, 6.0, 7.0);  
Vector4d c(5.0, 6.0, 7.0, 8.0);

1.5、系数访问
系数都是从0开始，矩阵默认按列存储

#include <iostream>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;
 
int main()
{
    MatrixXd m(2, 2);
    m(0, 0) = 3;
    m(1, 0) = 2.5;
    m(0, 1) = -1;
    m(1, 1) = m(1, 0) + m(0, 1);
    cout << "Here is the matrix m:" << endl;
    cout << m << endl;
 
    VectorXd v(2);
    v(0) = 4;
    v[1] = v[0] - 1;     //operator[] 在 vectors 中重载，意义和()相同
    cout << "Here is the vector v:" << endl;
    cout << v << endl;
 
    getchar();
    getchar();
}

1.6、逗号分隔的初始化

Matrix3f m;
m << 1, 2, 3,   4, 5, 6,   7, 8, 9;
cout << m;

1.7、Resizing
可以用rows(), cols() and size() 改变现有矩阵的大小。这些类方法返回行、列、系数的数值。也可以用resize()来改变动态矩阵的大小。
矩阵与向量间的点成叉乘转置以及矩阵求逆等见添加链接描述
2、CropBoxfilterr---------过滤指定立方体内的点
参照博客：添加链接描述

#include <pcl/filters/crop_box.h>//要包含的头文件。要包含哪些头文件，需要去查官方文档对该类的介绍。
pcl::PointCloud<pcl::PointXYZ>::Ptr body {new pcl::PointCloud<pcl::PointXYZ>};
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_body {new pcl::PointCloud<pcl::PointXYZ>};//指针还是对象，有时候只能指针，有时候都行。报错就换。
pcl::CropBox<pcl::PointXYZRGBA> box_filter;//滤波器对象
box_filter.setMin(Eigen::Vector4f(x_min, y_min, z_min, 1.0));//Min和Max是指立方体的两个对角点。每个点由一个四维向量表示，通常最后一个是1.（不知道为什么要有四个，大神知道的给解答一下下）
box_filter.setMax(Eigen::Vector4f(x_max, y_max, z_max, 1.0));

clipper.setNegative(false);//是保留立方体内的点而去除其他点，还是反之。false是将盒子内的点去除，默认为false
box_filter.setInputCloud(body);//输入源
box_filter.filter(*filtered_body);//滤它！