【论文笔记】2019 基于激光点云pole检测的重定位方法 Long-Term Urban Vehicle Localization Using Pole Landmarks

https://github.com/acschaefer/polex

本文提出了一个基于激光三维点云的二维车辆定位系统，其根据道路场景中的 “pole landmarks” （极点/柱状目标，即交通灯杆，树，灯箱等）来进行定位。
这不是首个提出该类定位方法的工作，却是首个同时考虑了 “laser ray endpoints” 以及 “the free space in between the laser sensor and the endpoints” 的工作。
在进行重定位时作者使用粒子滤波的方式对齐检测到的pole进而预测车辆位姿。

相关工作

基于场景中的柱状目标进行定位的方法至少包括两个部分：柱状目标检测器和基于地标的位姿预测器。

[6] 提出使用voxel 的方法，检测在竖直方向上有连续的voxel占用的目标，对于激光反射超过阈值的就认为是一个pole.为了从这些pole中提取pole的参数，检测器通过RANSAC将圆柱体拟合到pole中的所有点[7]. 对于二维姿态估计，作者使用了具有最近邻数据关联的粒子滤波器。

[8]提出了一个pole检测器，该检测器从给定的点云中移除地面点，将剩余的点投影到水平规则网格上，根据占用率和高度将相邻的单元聚类，并为每个类拟合一个圆柱体，即pole。与[6]一样，[8]也从执行最近邻数据关联的粒子过滤器中获得二维位姿估计。

[5]使用了[8]的极点检测器，但为了进一步完善位姿估计，他们还将平面拟合到了建筑物立面的平面以及对立体摄像机图像中车道标记进行了线条拟合。

以上方法的姿势估计器依靠蒙特卡洛方法来解决数据关联问题（即在数据关联阶段通过积累基于里程计在过去时间步中检测到的地标来构建局部地图），但是使用优化方法来计算最优位姿。然后对当前GPS位置周围的多个姿态进行采样，使用这些姿态假设将局部地图投影到全局地图中，并通过手工制作的地标匹配度量确定最可能假设。在给定结果数据关联的情况下，通过非线性最小二乘优化，在过去的车辆姿态和地标图上对当前车辆姿态估计进行细化。

[9] 从双目相机图像中提取极点地标。为了估计车辆的姿态，他们将车轮里程计、GPS和online极点检测数据输入粒子过滤器。

这些方法都提供了一个完整的定位系统，包括极点提取器和一个基于地标的定位模块，但目前已有大量的研究论文只关注极点检测。从激光雷达数据中提取极点是道路基础设施维护和城市规划中的一个常见问题。在该领域中研究者们不仅尝试定位极点的坐标以及拟合其几何参数，还尝试对极点目标进行点云分割。

[10,11,12，13]提出了从点云中提取类极点对象的不同方法，即不考虑free space的信息
[14,15]提出仅仅针对路灯杆的检测方法，[16]提出了一种用于被绿化带（vegetation）局部遮挡的pole检测器。
[17]不仅提取电线杆，而且将其分类为灯柱，电线杆和路牌。
[18]基于[11]提出的极点检测器，并将结果分为6类，包括树木，路灯柱，交通标志和交通灯。
[19] 通过将多功能结构（例如承载交通标志的灯柱）分解为单独的元素，进一步进行分类

除了pole，[20] 研究了基于路缘和交叉口特征的蒙特卡罗车辆定位方法。正如[21]和[22]的工作所证明的，道路标记（ road markings ）作为地标也能产生很高的定位精度。
[23]将路缘特征（curb features）和道路标记输入其粒子滤波器。
[24]探索了使用交通标志作为landmark的想法。尽管与道路标线或路灯杆等其他类型的道路设施相比，交通标志在城市场景中出现的频率较低，但它们不仅提供了编码位置，而且提供了明确的非歧义的ID。
[25]基于单眼相机图像和激光雷达扫描数据，使用出现在建筑物拐角处的垂直拐角特征进行定位。
参考文献

[5] J. K ̈ummerle, M. Sons, F. Poggenhans, T. Kuehner, M. Lauer, andC. Stiller, “Accurate and efficient self-localization on roads usingbasic geometric primitives,” in2019 IEEE International Conferenceon Robotics and Automation, May 2019.
[6] L. Weng, M. Yang, L. Guo, B. Wang, and C. Wang, “Pole-based real-time localization for autonomous driving in congested urban scenar-ios,” in2018 IEEE International Conference on Real-time Computingand Robotics, August 2018, pp. 96–101.
[7] M. A. Fischler and R. C. Bolles, “Random sample consensus: aparadigm for model fitting with applications to image analysis andautomated cartography,”Communications of the ACM, vol. 24, no. 6,pp. 381–395, 1981.
[8] M. Sefati, M. Daum, B. Sondermann, K. D. Kreisk ̈other, and A. Kamp-ker, “Improving vehicle localization using semantic and pole-likelandmarks,” in2017 IEEE Intelligent Vehicles Symposium, June 2017,pp. 13–19.
[9] R. Spangenberg, D. Goehring, and R. Rojas, “Pole-based localizationfor autonomous vehicles in urban scenarios,” in2016 IEEE Interna-tional Conference on Intelligent Robots and Systems, October 2016,pp. 2161–2166.
[10] C. Brenner, “Global localization of vehicles using local pole patterns,”inPattern Recognition, J. Denzler, G. Notni, and H. S ̈uße, Eds.Springer Berlin Heidelberg, 2009, pp. 61–70.
[11] C. Cabo, C. Ord ́o ̃nez, S. Garcia-Cortes, and J. Mart ́ınez-S ́anchez,“An algorithm for automatic detection of pole-like street furnitureobjects from mobile laser scanner point clouds,”ISPRS Journal ofPhotogrammetry and Remote Sensing, vol. 87, pp. 47–56, 01 2014.
[12] F. Tombari, N. Fioraio, T. Cavallari, S. Salti, A. Petrelli, and L. D.Stefano, “Automatic detection of pole-like structures in 3D urban envi-ronments,” in2014 IEEE/RSJ International Conference on IntelligentRobots and Systems, September 2014, pp. 4922–4929.
[13] B. Rodr ́ıguez-Cuenca, S. Garc ́ıa-Cort ́es, C. Ord ́o ̃nez, and M. C.Alonso, “Automatic detection and classification of pole-like objects inurban point cloud data using an anomaly detection algorithm,”RemoteSensing, vol. 7, no. 10, pp. 12 680–12 703, 2015.[14] Y. Yu, J. Li, H. Guan, C. Wang, and J. Yu, “Semiautomated extrac-tion of street light poles from mobile LiDAR point-clouds,”IEEETransactions on Geoscience and Remote Sensing, vol. 53, no. 3, pp.1374–1386, 2015
[14] Y. Yu, J. Li, H. Guan, C. Wang, and J. Yu, “Semiautomated extrac-tion of street light poles from mobile LiDAR point-clouds,”IEEETransactions on Geoscience and Remote Sensing, vol. 53, no. 3, pp.1374–1386, 2015.
[15] F. Wu, C. Wen, Y. Guo, J. Wang, Y. Yu, C. Wang, and J. Li,“Rapid localization and extraction of street light poles in mobilelidar point clouds: a supervoxel-based approach,”IEEE Transactionson Intelligent Transportation Systems, vol. 18, no. 2, pp. 292–305,February 2017.
[16] H. Zheng, F. Tan, and R. Wang, “pole-like object extraction frommobile lidar data,”International Archives of the Photogrammetry,Remote Sensing & Spatial Information Sciences, vol. 41, 2016.
[17] H. Yokoyama, H. Date, S. Kanai, and H. Takeda, “Detection andclassification of pole-like objects from mobile laser scanning dataof urban environments,”International Journal of CAD/CAM, vol. 13,no. 2, pp. 31–40, 2013.
[18] C. Ord ́o ̃nez, C. Cabo, and E. Sanz-Ablanedo, “Automatic detection andclassification of pole-like objects for urban cartography using mobilelaser scanning data,”Sensors, vol. 17, no. 7, p. 1465, 2017.
[19] F. Li, S. Oude Elberink, and G. Vosselman, “Pole-like road furnituredetection and decomposition in mobile laser scanning data based onspatial relations,”Remote Sensing, vol. 10, no. 4, 2018.
[20] B. Qin, Z. Chong, T. Bandyopadhyay, M. H. Ang, E. Frazzoli, andD. Rus, “Curb-intersection feature based Monte Carlo localization onurban roads,” in2012 IEEE International Conference on Robotics andAutomation. IEEE, 2012, pp. 2640–2646.
[21] A. Schindler, “Vehicle self-localization with high-precision digitalmaps,” in2013 IEEE Intelligent Vehicles Symposium, June 2013, pp.141–146.
[22] M. Schreiber, C. Kn ̈oppel, and U. Franke, “LaneLoc: lane markingbased localization using highly accurate maps,” in2013 IEEE Intelli-gent Vehicles Symposium, June 2013, pp. 449–454.
[23] A. Y. Hata and D. F. Wolf, “Feature detection for vehicle localizationin urban environments using a multilayer lidar,”IEEE Transactionson Intelligent Transportation Systems, vol. 17, no. 2, pp. 420–429,February 2016.
[24] A. Welzel, P. Reisdorf, and G. Wanielik, “Improving urban vehiclelocalization with traffic sign recognition,” in2015 IEEE InternationalConference on Intelligent Transportation Systems, September 2015,pp. 2728–2732.
[25] J.-H. Im, S.-H. Im, and G.-I. Jee, “Vertical corner feature based precisevehicle localization using 3D lidar in urban area,”Sensors, vol. 16,no. 8, p. 1268, 2016.

论文方法：二维车辆定位系统

包括三个部分： pole检测器，建图模块，定位模块。
本博客只介绍第一部分

Pole检测器

首先构建扫描区域的3D占用地图，将pole特征检测器应用于每个voxel，根据得到的pole map回归pole的位置和宽度。
输入：3D点云
输出：相对于地平面的极点的中心二维坐标以及估计的极点宽度
定义激光点云的一次scan 为 Z:={zi}，其中zi:={u,v}, u，v分别表示单个激光束的始端、末端的笛卡尔坐标。
点云scan被配准到地图坐标系中，且x-y平面与地平面对齐。
然后，对第 j 个体素的反射入射激光光线的后验概率进行建模：
p(ui∣Z)=Beta(hj+α,mj+β)p(u_i | Z) = Beta(h_j + \alpha, m_j + \beta) p(ui∣Z)=Beta(hj+α,mj+β)
先验反射概率的表达式:
p(uj)=Beta(α,β)p(u_j) = Beta(\alpha, \beta)p(uj)=Beta(α,β)
其中 hj,mjh_j, m_jhj,mj 表示第j个cell中的激光反射数目和透射数目。α,β\alpha, \betaα,β表示先验反射概率p中的两个参数，
α=−γ(γ2−γ+δ)δ,β=γ−δ+γδ−2δ2+γ3δ\alpha = -\frac{\gamma(\gamma^2 - \gamma + \delta)}{\delta}, \\ \beta = \frac{\gamma - \delta + \gamma \delta - 2\delta^2 + \gamma^3}{\delta} α=−δγ(γ2−γ+δ),β=δγ−δ+γδ−2δ2+γ3
最大似然反射地图M：=hj(hj+mj)−1M：={h_j(h_j + m_j)}^{-1}M：=hj(hj+mj)−1,
γ:=E[M],δ:=var[M]\gamma:=E[M], \ \delta:=var[M]γ:=E[M], δ:=var[M]分别表示均值和方差
注意{p(ui∣Z)}\{ p(u_i | Z) \}{p(ui∣Z)}与M不同，M赋予每个体素最可能的反射率，它在每个可能的反射率上产生后验分布.
本文尝试利用占用图的被占用概率提取pole，而不是反射率p,
因此需要将其转化为被占用概率，得到occupancy map O:={o_j} :
oj:=∫uo1p(uj∣Z)dujo_j := \int_{u_o}^1 p(u_j | Z) du_j oj:=∫uo1p(uj∣Z)duj
然后将占用图转化为2D 的 pole score map S
首先将其转化为与占用图等大小的3D中间 score map Qa:={qa,j}Qa:=\{q_{a,j}\}Qa:={qa,j}
{qa,j}\{q_{a,j}\}{qa,j}表示这部分空间是边缘长度为a的pole的的概率：
qa,j:=MAXk∈inside(j,a)(∑l∈inside(k,a)ola2−maxl∈outside(k,a,f)ol)q_{a,j}:=MAX_{k\in inside(j,a)}(\frac{\sum_{l\in inside(k,a)}o_l}{a^2} - max_{l\in outside(k,a,f)}o_l) qa,j:=MAXk∈inside(j,a)(a2∑l∈inside(k,a)ol−maxl∈outside(k,a,f)ol)
inside(j,a)和outside(j,a,f)能够根据给定索引j和极宽a，返回与j相同的水平索引中的体素中的一组索引。前者输出的是所有在pole内的索引，后者输出的是厚度f∈N+的pole周围的自由区域的索引。
得到的分数位于区间[-1,1]：分数越高，相应的空间分区成为极点部分的概率越大。
然后将得到的三维映射Qa{Q_a}Qa回归到二维，方法是将它们合并成一个单独的映射Q:=qj：=maxaqa，j，并为Q中的每个水平位置确定所有竖直方向上的超过给定阈值Q:={q_j}：={max_{a}q_{a，j}}，并为Q中的每个水平位置确定所有竖直方向上的超过给定阈值Q:=qj：=maxaqa，j，并为Q中的每个水平位置确定所有竖直方向上的超过给定阈值q_{min}$的连续体素。
最后去除掉高度低于某个阈值的体素，计算剩余体素的平均得分，得到2D score map S.
然后将离散的score map 转化为连续的pole 的位置和宽度的预测。
“ We identifythe pole positions as the modes of S, which we determine via mean shift [27] with a Gaussian kernel and with the local maxima of S as seed points. The width estimate of each pole is computed as the weighted average over all pole width s_a, where for every a, the weight is the mean of all cells in Q_a that touch the pole.”
该算法与其他极点提取算法不同之处在于它是基于光线跟踪的。它不仅考虑了扫描的端点，而且还考虑了扫描的起点，从而对占用和空闲空间进行了明确的建模。相比之下，大多数方法假设传感器周围没有任何反射记录的空间是不被占用的。然而，没有反射可能有两个原因：一是相关区域的确没有被占用，二是激光雷达传感器由于视线遮挡或者感知距离限制导致没有覆盖该区域。

实验

NCLT lidar data 检测效果

Kitti dataset 检测效果

利用检测的pole地标，使用粒子滤波进行重定位