hdl_graph_slam源码解析（一）

1. hdl_graph_slam简介

hdl_graph_slam是由日本风桥科技大学的Kenji Koide在github上开源的六自由度三维激光SLAM算法。主要由激光里程计、回环检测以及后端图优化构成，同时融合了IMU、GPS以及地面检测的信息作为图的额外约束。其算法流程图如下所示：

从图中可以看出，算法首先读入激光雷达的点云数据，然后将原始的点云数据进行预滤波，经过滤波后的数据分别给到点云匹配里程计以及地面检测节点，两个节点分别计算连续两帧的相对运动和检测到的地面的参数，并将这两种消息送到hdl_graph_slam节点进行位姿图(pose graph)的更新以及回环检测，并发布地图的点云数据。下图是利用该算法重建的3D环境点云效果图：

从上图可以看出该算法的建图效果很赞，且在github上开源了质量较高的代码，唯一美中不足的是对应的论文还处于review状态，且据笔者了解，目前网络上也还没有关于该算法的详细解读，因此只能通过阅读源码来学习理解该算法。接下来笔者将按照自己的理解分别对该算法的4个主要部分进行较为详细地介绍。

2. prefiltering

3D激光雷达每一帧都会产生大量的点云数据，以velodyne HDL-32E为例，大约每帧会返回包含7万个点的点云数据，同时激光雷达的测量原理又决定了这部分点云密度并不一致，近处密度过大有一些冗余的点云，而过远处的地方点云又很稀疏，无法用来进行有效地感知。另一方面，当激光雷达在较细小物体上扫描或产生错误测量时，会产生明显的稀疏离群点，这部分稀疏的离群点首先无法进行有效的感知，其次会对点云局部形态的估计例如法向量和曲率产生很大影响，因此这一部分主要是对激光雷达产生的原始点云数据进行下采样和野值点滤除。
从原作者的源程序中可以看出，其预滤波部分的源码主要集中在这三个函数里面：

pcl::PointCloud<PointT>::ConstPtr filtered = distance_filter(src_cloud);
filtered = downsample(filtered);	
filtered = outlier_removal(filtered);

其中，distance_filter函数用来滤除距离过远的稀疏点云，downsample函数用来对距离滤波后的点云进行下采样，outlier_removal函数用来滤除野值点。

2.1 distance_filter

这部分代码比较简单，即：遍历原始点云cloud，若某一点对应的欧式距离distance_near_thresh大于且小于distance_far_thresh，则压入点云filtered，时间复杂度为$O(N)$。

std::copy_if(cloud->begin(), cloud->end(), std::back_inserter(filtered->points),
[&](const PointT& p) {
	double d = p.getVector3fMap().norm();
	return d > distance_near_thresh && d < distance_far_thresh;
	}
);

2.2 downsample

这部分代码原作者采用了pcl中下采样的两种方法：VoxelGrid和ApproximateVoxelGrid，并提供了不同的接口，只需要在.launch文件中修改对应的选项，即可在两种方法中切换。
如下代码完成了是根据.launch文件的不同设置生成对应的下采样滤波器：

if(downsample_method == "VOXELGRID") {
	std::cout << "downsample: VOXELGRID " << downsample_resolution << std::endl;
	boost::shared_ptr<pcl::VoxelGrid<PointT>> voxelgrid(new pcl::VoxelGrid<PointT>());
	voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
	downsample_filter = voxelgrid;
} else if(downsample_method == "APPROX_VOXELGRID") {
	std::cout << "downsample: APPROX_VOXELGRID " << downsample_resolution << std::endl;
	boost::shared_ptr<pcl::ApproximateVoxelGrid<PointT>> approx_voxelgrid(new pcl::ApproximateVoxelGrid<PointT>());
	approx_voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
	downsample_filter = approx_voxelgrid;
} else {
	std::cerr << "warning: unknown downsampling type (" << downsample_method << ")" << std::endl;
}

如下是根据选择的下采样滤波器对点云进行下采样滤波：

pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
downsample_filter->setInputCloud(cloud);
downsample_filter->filter(*filtered);

由于上面的代码比较简单，因此接下来着重介绍一下pcl中两种下采样方法的基本原理，具体源码可参见此链接。

• VoxelGrid： 根据输入的点云，该滤波方法首先计算一个能够刚好包裹住该点云的立方体，然后根据设定的分辨率，将该大立方体分割成不同的小立方体。对于每一个小立方体内的点，计算他们的质心，并用该质心的坐标来近似该立方体内的若干点，从而达到对点云下采样的目的。
• ApproximateVoxelGrid：从名字也可以看出，该方法与上一个滤波方法类似。唯一的不同在于这种方法是利用每一个小立方体的中心来近似该立方体内的若干点。相比于VoxelGrid，计算速度稍快，但也损失了原始点云局部形态的精细度。

2.3 outlier_removal

这一部分主要负责将野值点滤除，原作者同样采用了pcl中的两种野值点滤出方法：StatisticalOutlierRemoval和RadiusOutlierRemoval，并提供了相应的接口，同样在.launch文件中修改对应的属性即可选择不同的滤波方法。
如下代码是根据.launch文件中不同的属性修改滤波器选择：

if(outlier_removal_method == "STATISTICAL") {
	int mean_k = private_nh.param<int>("statistical_mean_k", 20);
	double stddev_mul_thresh = private_nh.param<double>("statistical_stddev", 1.0);
	std::cout << "outlier_removal: STATISTICAL " << mean_k << " - " << stddev_mul_thresh << std::endl;
	pcl::StatisticalOutlierRemoval<PointT>::Ptr sor(new pcl::StatisticalOutlierRemoval<PointT>());
	sor->setMeanK(mean_k);
	sor->setStddevMulThresh(stddev_mul_thresh);
	outlier_removal_filter = sor;
} else if(outlier_removal_method == "RADIUS") {
	double radius = private_nh.param<double>("radius_radius", 0.8);
	int min_neighbors = private_nh.param<int>("radus_min_neighbors", 2);
	std::cout << "outlier_removal: RADIUS " << radius << " - " << min_neighbors << std::endl;
	pcl::RadiusOutlierRemoval<PointT>::Ptr rad(new pcl::RadiusOutlierRemoval<PointT>());
	rad->setRadiusSearch(radius);
	rad->setMinNeighborsInRadius(min_neighbors);
} else {
	std::cout << "outlier_removal: NONE" << std::endl;
}

如下是根据选择的野值点滤出滤波器对点云进行处理：

pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
outlier_removal_filter->setInputCloud(cloud);
outlier_removal_filter->filter(*filtered);

同样，接下来简单介绍一下pcl中两种野值点滤波器的工作原理：

• StatisticalOutlierRemoval：这种方法首先计算遍历该组点云，对于每一个点，进行K近邻搜索，并计算该点与其K近邻的平均距离，然后计算这些平均距离的均值mean以及标准差stddev。则若其中一个点的平均距离大于如下阈值：
  $$mean+stddev\ast \alpha$$
  则认为该点为野值点，其中$\alpha$通常取1.0。个人认为这种方法时间复杂度过高，作为一种点云的预处理方法，占据过了过多的cpu计算时间，显然对于实时3DSLAM来讲并不是很好的选择。
• RadiusOutlierRemoval：顾名思义，首先应该为该算法设定一个半径，然后遍历整个点云，对于每一个点，计算与其欧式距离dist小于给定半径的点的数量，若超过一定数量，则认为不是野值点。反之，可以认为是野值点。下图所示情况可以很好的解释这种算法：
  
  绿色的点其规定半径内并没有其他点，而其他的红色点规定半径内则至少有三个其他点，因此可以可以根据这一规律分别出来野值点并将其滤出，可以发现该方法的时间复杂度为$O(N^2)$。

2.4 测试效果

如下图是对原始的输入点云进行prefilter的结果，其中左侧是滤波后的结果，右侧是原始点云，可以发现距离过远的稀疏点云、野值点都别滤出。同时相比于原始点云，滤波后的点云变得更为稀疏。

本节就先更到这里，下节会主要对scan_matching_odometry部分进行解析。