前言
由于道路的非平面性质，来自3D激光扫描仪的点云还包括被车辆视为非障碍物（即可导航）的地形信息。从可能构成障碍物的高点对可导航地形（此处称为地面）进行语义标记非常有用。地面提取是目标检测过程中一个重要的前期处理过程。由于我们将要处理大量原始激光雷达测量，在实施过程中必须考虑计算负载。
地面提取过程主要分为三个方向：基于网格/单元的方法、基于直线的方法和基于曲面的方法。Rieken等人将基于线和基于表面的方法视为一大类，称为基于扫描的方法。基于网格单元的方法将激光雷达数据划分为极坐标单元和通道，该方法利用高度和相邻网格之间的径向距离信息来推断当单元格之间的坡度超过一定阈值时是否存在障碍物，即基于坡度的方法。
基于扫描的方法从特定标准中提取平面地面（即平坦世界假设），其中一种方法是取最低z值，并应用随机样本一致性（RANSAC）拟合来确定可能的地面。基于网格单元的方法的优点是保留了地面轮廓，更好地表现了平坦的地形。然而，与基于扫描的方法相比，它没有考虑邻近通道的值，因此可能会由于对坡度变化的过度敏感而导致高程不一致。由于被大型物体遮挡，地面测量也可能不完整。因此Rieken等人提出了一种考虑遮挡的显著方法，将基于通道的地面分类与基于栅格的地面高度表示相结合，以应对虚假的空间测量，并使用通道间相关性来补偿丢失的数据。
一、栅格法的主要方法
基于斜率的通道分类(slope-based channel classification)，主要过划分激光雷达点云并比较连续隔间的高度差（即坡度）来确定地面高度，并辅以一致性检查和中值滤波，来完成分割。移除地面后的激光雷达测量称为非地面点(elevatedCloud)。
二、处理步骤
1.函数入口
main函数中的
groundRemove(z_filter_cloud, elevatedCloud, groundCloud);
其中其中三个点云类别均为PointCloud pcl::PointXYZ::Ptr类型，z_filter_cloud为已经进行感兴趣区域切除过的点云数据作为输入，原文中通过
filter_mid_area(z_filter_cloud); // ConditionalRemoval滤波 条件限定下的滤波器
设置条件滤波器以及xyz方向上的比较算子分割出感兴趣区域。elevatedCloud为函数输出非地面点云，groundCloud为函数输出地面点云。
2.groundRemvoe函数
1.进入groundRemvoe首先去除车身周围的异常反射点，欧氏距离3.4-120。
filterCloud(cloud, filteredCloud); // 判断欧氏距离，过滤去除车周围半径外异常点
2.原始激光雷达扫描被划分为一个极坐标网格，其中m个单元×n个通道——最小半径r min是距离车辆最近的半径，rmax是距离车辆最远的半径。最小半径是指无法再看到车辆反射的半径，而最大半径由传感器的有效范围确定。
栅格划分示意图
点云数据(pcl::PointXYZ)到栅格坐标(channel,bin )的映射函数入口为createAndMapPolarGrid中的getCellIndexFromPoints，通过该函数建立点云坐标与栅格坐标映射并在此过程中更新栅格内的最低点。
createAndMapPolarGrid(filteredCloud, polarData); // 极坐标网格映射
--------------------------------getCellIndexFromPoints(x, y, chI, binI);
每个点云数据p i={x i,y i,z i,i} 到每个通道channel(80)和bin(120)的映射如下：
void getCellIndexFromPoints(float x, float y, int& chI, int& binI){
float distance = sqrt(x * x + y * y);
float chP = (atan2(y, x) + M_PI) / (2 * M_PI);
float binP = (distance - rMin) / (rMax - rMin);
chI = floor(chP*numChannel);
binI = floor(binP*numBin);
}
其中将每个划分出的栅格规定为一个自定义对象Cell,对象中包括最低点，地面标志，栅格内高度差，平滑高度，平均高度等。
class Cell{
private:
float smoothed;float height;
float hDiff;float hGround;
float minZ;bool isGround;
};
最低点z i的点的高度信息存储为映射cell的属性。最低点z i点(原型点)用作“局部”接地点，以确定所有Cell的可能最低点。此外，可以通过计算同一单元格内最低z i和最高z i之间的差值来计算单元格中的绝对（局部）高度。
接下来，定义了基于阈值分类的可能地面高度hi的间隔[T hmin T hmax]。由于传感器的安装高度高于地面传感器是先验的，因此我们可以推断距离传感器最近的点必须位于地面以上。因此，传感器高度随后被用作将被视为地面的点的T hmax。Cell信息从最靠近车辆和最远车辆的bins中填充，如果cell的原型点位于间隔[T hmin T hmax]内，则将其设置为该cell的h i。如果原型点高于T hmax，则地面标高设置为等于传感器高度的地面标高。
更新后使用高斯平滑滤波器对cell中的高度进行平滑处理，获得cell中的smoothed高度(没太看懂这块)。
在列举了所有单元后，可以计算单元之间的斜率（简单欧氏梯度）m，以检查单元之间的高度是否突然增加。此外，还计算绝对高度(computeHDiffAdjacentCell)差作为二次检查，以识别可能存在于相邻单元中的非圆形物体。坡度的橙色线所示，正常地面由绿色圆圈表示，由于坡度变化和与前一个单元的高度差超过预定义阈值T斜率和T hdf，升高点被涂成黄色。
初步判断地面主要逻辑如下：
if(polarData[channel][bin].getSmoothed() < tHmax && polarData[channel][bin].getHDiff() < tHDiff){ // tHDiff = 0.4; 地面应该是相对平滑的
polarData[channel][bin].updateGround(); // void updateGround(){isGround = true; hGround = height;}
}
else if(polarData[channel][bin].getHeight() < tHmax &&polarData[channel][bin].getHDiff() < tHDiff){
polarData[channel][bin].updateGround();
}
尽管这种方法适用于平滑的地形，但小而突出的地形特征，如道路颠簸和草地，仍然可以归类为非地面点。为了解决这一问题，采用了一致性检查(outlierFilter)和中值滤波(applyMedianFilter)来进一步展平地平面，并产生更好的地面估计。
中值滤波处理丢失的地平面信息（由于遮挡而常见），顾名思义，丢失单元的高度值将替换为相邻单元的中值。
//中值滤波
void applyMedianFilter(array<array<Cell, numBin>, numChannel>& polarData){
// maybe later: consider edge case
for(int channel = 1; channel < polarData.size()-1; channel++){
for(int bin = 1; bin < polarData[0].size()-1; bin++){
if(!polarData[channel][bin].isThisGround()){ // 判断不是地面
// target cell is non-ground AND surrounded by ground cells
if(polarData[channel][bin+1].isThisGround()&& // 四周是地面
polarData[channel][bin-1].isThisGround()&&
polarData[channel+1][bin].isThisGround()&&
polarData[channel-1][bin].isThisGround())
{
vector<float> sur{
polarData[channel][bin+1].getHeight(), // target cell is non-ground AND surrounded by ground cells
polarData[channel][bin-1].getHeight(),
polarData[channel+1][bin].getHeight(),
polarData[channel-1][bin].getHeight()
};
sort(sur.begin(), sur.end()); // 从小到大排序
float m1 = sur[1]; float m2 = sur[2];// 取中间2个值
float median = (m1+m2)/2; // 取中值
polarData[channel][bin].updataHeight(median); //高度取中值 缺失单元的高度值将替换为相邻单元的中值
polarData[channel][bin].updateGround(); // 是地面
}
}
}
}
}
一致性检查是通过迭代非基本单元来完成的，这些单元的两侧是非模糊的基本单元，然后比较这些单元与相邻单元的高度一致性。将单元高度与预定义的绝对高度T f lat进行比较，并将其与相邻单元的高度差与阈值T一致性进行比较。低于阈值的值表示cell应该属于地面，因此它们将被相应地重新分类。
//一致性平滑
void outlierFilter(array<array<Cell, numBin>, numChannel>& polarData){
for(int channel = 1; channel < polarData.size() - 1; channel++) {
for (int bin = 1; bin < polarData[0].size() - 2; bin++) {
if(polarData[channel][bin].isThisGround()&& // 都是Ground
polarData[channel][bin+1].isThisGround()&&
polarData[channel][bin-1].isThisGround()&&
polarData[channel][bin+2].isThisGround())
{
float height1 = polarData[channel][bin-1].getHeight();
float height2 = polarData[channel][bin].getHeight(); // height2本尊
float height3 = polarData[channel][bin+1].getHeight();
float height4 = polarData[channel][bin+2].getHeight();
if(height1 != tHmin && height2 == tHmin && height3 != tHmin){ // float tHmin = -2.0; // 高度
float newH = (height1 + height3)/2; //取两边均值
polarData[channel][bin].updataHeight(newH); // 更新高度值
polarData[channel][bin].updateGround(); // void updateGround(){isGround = true; hGround = height;}
}
else if(height1 != tHmin && height2 == tHmin && height3 == tHmin && height4 != tHmin){
float newH = (height1 + height4)/2;
polarData[channel][bin].updataHeight(newH);
polarData[channel][bin].updateGround();
}
}
}
}
}
在最后一步，在最终分类过程中使用公差值h tol来进一步平滑噪声测量中地面和非地面点之间的过渡。
伪代码
文章出处登录后可见！
