hdl-graph-slam论文阅读

A Portable 3D LIDAR-based System for Long-term and Wide-area People Behavior Measurement

一、摘要

本文主要介绍了一种基于三维激光雷达的便携式人体行为测量系统。采用人背着三维激光雷达设备的形式，跟踪被测者并同时保证被测者在传感器视野内。他实现了长期和广域的测量人员的行为，解决了现有的人员跟踪系统里的难题。作为现场实验，作者记录了医院照顾老年痴呆的专业护理人员的行为，并对行为进行初步分析得到护理人员是如何在检查周围人和周围环境的同时确定位置。根据这些结果，提出在设计此类服务机器人的时候需要遵循的一些经验规则。

二、Introduction

人们需要了解人们对周围人和环境的行为方式，以实现和人自然而丰富的互动的系统。有些模型只是简单的分析了模型和人们之间的距离，无法处理一些限制，如周围环境和其他人的影响，人们需要在不同环境测量并构建复杂的交互模型。本文利用3DLidar实际观测专业护工与患老年痴呆症之间的交互，并制作合适的数据集，以便后续为机器人的行为制定更加拟人的规则。

如图为提出的系统，该系统同时估计3D环境地图中的传感器姿态并跟踪目标人员。

贡献：1. 提出了一种便携式测量系统，可进行长期和广域的行为测量。同时保证该系统的跟踪精度和基于静态传感器的人员跟踪系统相差无几。

2.对在医院里进行的测试做了简单的分析，记录了专业的护理人员照顾老年痴呆患者的行为，验证了该系统的可应用性，并根据测试的数据提出设计该类服务机器人的经验规则。

3.把软件系统和制作的数据集公开。

三、Related Work

人类行为观测系统可分为两类：（1）固定于场景中的静态传感器系统；（2）使用附着在目标人身上的可穿戴传感器的系统。

尽管RGB相机的人类跟踪系统发展已经越来越成熟，但是如何重新识别出一个曾经离开过相机视野的人的轨迹还是一个重要问题。已经提出了许多基于人物特征和基于软生物特征的方法去解决重定位问题。

相较于现在发展比较成熟的RGB相机的人类跟踪系统，激光测距仪的测量范围更广。静态传感器可靠性和精度很好，但受限于传感器的视野范围，就需要安装多个传感器，提高了安装和标定的时间成本和花销。

另一个长期，宽范围的人类行为观测方式是用可携带传感器，如GPS和INS。但是他们只能用于室外环境的人类为跟踪，在靠近建筑和室内环境是不可用的。

还有基于WiFi信号的定位，他使用了三角测距法，但需要布置多重天线，不适用于大的环境。而WiFi指纹匹配方法不需要多重天线，可用于大的环境，但是精度不够。

INS是内部传感器，无法检测周围环境和人与人之间的交互，故也无法应用。

四、System Overview

3D Lidar提供10Hz的360度距离数据，系统根据距离数据估计其姿态，同时跟踪目标人物。系统包括两部分：（1）离线建图；（2）在线传感器定位和人物检测/追踪。

离线建图过程中，本文创建了一个覆盖整个测量区域的3D环境地图。本文采用基于图优化的SLAM方法。为了补偿扫描匹配累积的旋转误差，我们分别为室内和室外环境引入了地平面和 GPS 位置约束。

在行为检测过程中，通过结合扫描匹配算法和基于角速度的姿态预测（使用无迹卡尔曼滤波器），系统在离线创建的地图上估计其姿态。同时，系统对目标人员进行检测和跟踪。

1.离线建图

图优化SLAM

通过构建和优化一个图来解决SLAM问题。该图的节点表示要优化的参数，如传感器姿态和地标位置；边表示约束条件，如传感器姿态和地标之间的相对姿态。即构建图，并找到一个最优配置（各节点的位姿），让参数（预测）与约束（观测）的误差最小。

目标函数：

和分别为均值和信息矩阵，是参数（状态向量）和约束（观测值）之间的误差函数。我们用高斯牛顿和LM方法对目标函数进行线性化和最小化。

然而，如果参数跨越非欧式空间（如姿势参数），这些算法可能会导致次优或无效的解决方案。解决这个问题，要引入一个算子“田”，（没懂）

？？？

在所提出的系统中，我们首先通过在连续帧之间迭代应用 NDT（正态分布变换）扫描匹配来估计传感器轨迹，兼顾了可靠性和运行速度。设是t时刻处传感器的位姿，由平移向量t和四元数q组成，是通过扫描匹配估计的在t和t+1时刻之间的相对传感器位姿。本文将他们作为节点[,...]和边[......]添加到位姿图中，然后我们在轨迹中找到循环并把他们作为边（即回环）添加到图中去loop-detection算法校正扫描匹配的累积误差。

2-11行：首先我们根据平移距离和节点之间的轨迹长度去检测循环候选。

12-17行：为了验证候选，在每个候选的节点之间应用扫描匹配算法（NDT）。如果适应度分数低于阈值（如0.2），我们将循环添加到图中作为节点之间的边。每次找到循环时，位姿图都会更新，使目标函数最小化。我们利用g2o，超图优化的通用框架，用于位姿图的优化。

如上图，由于当生成的地图越来越大，因为扫描匹配积累的旋转的误差导致地图产生弯曲。为了补偿这个误差，我们分别在室内和室外环境引入了地平面约束和GPS位置约束。

如下图，展示了提出的系统的图形结构说明。

地平面约束

为了可靠地生成大型室内环境的地图，我们假设环境只有一个平坦的地板，并引入地平面约束来优化位姿图，使得在每次观察中检测到的地平面成为同一平面。如：学校医院。

我们假设传感器的大致高度是已知的（例如，2m）并提取特定高度范围内的点，该范围应包含地平面点（例如，距地面 [-1.0, +1.0]m）。然后，我们将 RANSAC 应用于提取的点云并检测地平面。如果检测平面的法线几乎垂直（法线与单位垂直向量的夹角小于10度），我们认为地平面被正确检测到，并在图中添加地平面约束边。

通过高度阈值提取一定高度范围内的点（绿点），然后对其应用RANSAC检测地平面（红点）

（面试小问题：特征匹配要是遇到误匹配时，如何筛选处理？使用RANSAC算法进行过滤。）

（简单介绍一下RANSAC（14讲中出现过，第九章project中src中的visual_odometry.cpp,最核心的求解3d-2d变换中）：即RANdom SAmple Consensus（随机抽样一致）。基本假设：（1）数据由局内点组成；（2）局外点是不能适应该模型的数据；（3）除此之外的数据属于噪声。局外点产生的原因有：噪声的极值；错误的测量方法；对数据的错误假设。

RANSAC算法的输入是一组观测数据，一个可以解释或者适应于观测数据的参数化模型，一些可信的参数。RANSAC通过反复选择数据中的一组随机子集来达成目标。被选取的子集被假设为局内点，并用下述方法进行验证：
1.首先我们先随机假设一小组局内点为初始值。然后用此局内点拟合一个模型，此模型适应于假设的局内点，所有的未知参数都能从假设的局内点计算得出。
2.用1中得到的模型去测试所有的其它数据，如果某个点适用于估计的模型，认为它也是局内点，将局内点扩充。
3.如果有足够多的点被归类为假设的局内点，那么估计的模型就足够合理。
4.然后，用所有假设的局内点去重新估计模型，因为此模型仅仅是在初始的假设的局内点估计的，后续有扩充后，需要更新。
5.最后，通过估计局内点与模型的错误率来评估模型。
整个这个过程为迭代一次，此过程被重复执行固定的次数，每次产生的模型有两个结局：
1、要么因为局内点太少，还不如上一次的模型，而被舍弃，
2、要么因为比现有的模型更好而被选用。）

GPS约束

在室外环境中，地面不平整时，使用GPS 约束代替地面检测约束。

SLAM框架评估

BLAM 和 LeGO-LOAM 在序列中途因未能估计轨迹且未恢复而被中止。

BLAM因为扫描匹配的累积旋转误差，导致无法找到闭环并产生扭曲不准确的轨迹。

LeGO-LOAM估计的轨迹则更平滑（保持了连续帧之间地平面的局部一致性）。

但是由于没有全局地面约束，还是会受累积的旋转误差的影响。

实时性方面，LeGO-LOAM是实时的，无法计算时间，本文算法所用时间比BLAM所用时间少的多。

本文算法能够始终生成正确的位姿图，有赖于NDT（正态分布变换，计算正态分布是一个一次性的工作（初始化），不需要消耗大量代价计算最近邻搜索匹配点，概率密度函数在两幅图像采集之间的时间可以离线计算出来）。并且对有无地平面约束的结果进行了对比，得出在地平面约束下，可以修正累积的旋转误差，得到的地图完全平坦。同时若无GPS限制，会导致无法闭环（匹配得分过高）

2.在线人们行为测量

为了测量人的行为，系统同时估计 3D 环境地图上的传感器姿态并跟踪观察者周围的人。通过集成雷达提供的角速度和距离数据，估计传感器的位姿，然后检测并跟踪人们，以了解人们在环境地图上的位置。注意：手动给出传感器的初始位置，避免全局定位问题。

传感器定位

观察者在测量过程中必须跟随目标人，有时必须快速移动以将他们保持在传感器中。在这种情况下，帧之间的传感器运动变得非常大，扫描匹配可能会由于大位移而错误地估计传感器自我运动。本文采用用无迹卡尔曼滤波把NDT扫描匹配和3D LIDAR提供的角速度数据结合起来解决这个问题。系统利用无迹卡尔曼滤波对传感器的位姿进行预测，然后利用NDT对观察到的点云与全局地图进行匹配，用估计到的和作为传感器的初始猜测。然后系统用扫描匹配得到的传感器位姿校正其状态。

在每个预测和修正步骤后，对状态向量里的四元数进行归一化，防止其范数由于无迹变换和累积计算误差而变化。我们还把加速度考虑进位姿预测中，但估计结果更差了（由于加速度观测的强噪声）。

人们的检测与跟踪

本文先从观察到的点云中去除背景点以提取前景点。然后从环境地图中创建一个有特定体素大小（如，0.5m）的栅格地图。根据UKF（无迹卡尔曼滤波）估计的传感器位姿，输入的点云转换为地图坐标，然后将包含环境地图点的体素上的每个点作为背景去除。然后将欧氏聚类应用于前景点来检测人类候选聚类。但是，当人们离的非常近的时候，他们的聚类会被错误的合并，被检测为单个类。我们用Haselich的分块-归并聚类算法。该算法首先通过使用 dp-means？将一个集群划分为子集群，直到每个集群变得小于阈值（例如，0.45m），以至于每个集群没有不同人的点。然后，如果这些子集群之间没有间隙，则认为这些集群属于一个人并重新合并为一个集群。由于拆分和重新合并过程，即使人员集群非常靠近，它们也能正确分离。

检测到的集群可能包含非人的集群。为了消除检测到的簇中的非人类簇，我们通过使用 Kidono 等人用切片特征训练的人类分类器来判断一个簇是否是人类。假设人在地平面上走，我们在没有高度的情况下跟踪 XY 平面上的人。采用卡尔曼滤波器，与恒速模型和全局最近邻数据关联组合起来跟踪人。只要被跟踪的人在传感器的视野内并且能被正确检测到，跟踪方案就能号很好的工作。

传感器定位评价

本文做了个对比实验，结果表明，基于角速度的位姿预测使得位姿估计对快速运动具有鲁棒性并快速收敛（预测信息给了NDT更好的初始条件）

人员检测评价

本文通过四个实验的对比，验证了the split-merge clustering和the human classifier的效率和可靠性。

问题：论文table3中的那三个指标分别代表什么意思？

precision:精确度 recall:召回率 F-measure:综合评价指标?

精度和召回率是广泛用于信息检索和统计学分类领域的两个度量值，用来评价结果的质量。其中精度是检索出相关文档数与检索出的文档总数的比率，衡量的是检索系统的查准率；召回率是指检索出的相关文档数和文档库中所有的相关文档数的比率，衡量的是检索系统的查全率。

一般来说，Precision就是检索出来的条目（比如：文档、网页等）有多少是准确的，Recall就是所有准确的条目有多少被检索出来了。

P和R指标有时候会出现的矛盾的情况，这样就需要综合考虑他们，最常见的方法就是F-Measure（又称为F-Score）。是P和R 的加权调和平均。

与静态传感器系统比较

通过对本文提出的系统和公共可用的基于静态传感器的人员跟踪框架OpenTrack，该系统使用低成本硬件，易于安装，可以扩展到大型相机网络。

通过比较本文提出的系统精确度和OpenTrack相差不大，精度方面相差无几，同时由于可以携带移动，故拥有更好的视野。本文提出的系统适用于测量大范围内特定人群的行为，而基于静态传感器的系统适用于在相对较小的环境中测量所有人的行为。

五、医院测试

测量护理人员照顾老人的行为

在测量过程中会出现其他病人或者物体（如轮椅，药品架等）的情况，观测者就不得不很快的移动，这样才能把目标人物放入传感器的视野里。然而，由于 3D LIDAR 的测量区域广泛以及扫描匹配和基于角速度的姿态预测的集成，所提出的系统可以通过所有序列正确定位自身。

关于ID的重新分配问题：在人员追踪时，会出现其他患者出现在观察者和被观察对象之间的问题，这种情况下系统无法跟踪对象，重新出现后，系统会为这些对象分配新的ID，系统会通知它丢失了对象的跟踪，文中手动为他们重新分配了正确的ID。

后续考虑如何解决重识别问题。

照顾行为的初步分析

得到五个准则：

（1）机器人在尽可能长时间保持并排定位的同时照顾人。尤其要保持在离人0.6m的位置。

（2）根据步行速度，相对位置会沿前后方向偏离。但是，即使在这种情况下，机器人也应该与人保持一定的距离。

（3）在拐角处，机器人应该走到拐角的外侧，这样它可以检查走廊的安全性，同时避免打扰人。

（4）如果机器人由于定位和障碍物不能从外侧走，则应在人进入角落之前从内侧走，并检查是否安全。它会稍微打扰人走路。但是，安全性比舒适性更重要。

（5）为了照顾一个可以走路的人，机器人必须能够以大约 1.4 m/s 的速度运行。