hdl_graph_slam的论文阅读

论文下载地址
A Portable 3D LIDAR-based System forLong-term and Wide-area PeopleBehavior Measurement
用于长期和广区域的人类行为检测的便携式3D Lidar系统

摘要：

在设计与人交互的系统时，度量和分析人的行为是很重要的。介绍了一种基于三维激光雷达的便携式人体行为测量系统。在这个系统中，一个观察者携带一个配备了3D激光雷达的系统，并跟踪被测者，同时保持他们在传感器视图中。该系统在三维环境地图中估计传感器的姿态并跟踪目标人物。它使长期和广域的人员行为测量成为可能，这对于现有的人员跟踪系统来说是困难的。作为现场试验，我们记录了在医院照顾老年痴呆患者的专业护理人员的行为。对行为的初步分析揭示了照顾者在检查周围人和环境时如何决定注意位置。根据分析结果，提出了服务机器人行为设计的经验规则。

Itroduction

现有的模型都是基于人们之间距离的简单分析，现有的机器人只是保持与目标任务的距离不变，这种幼稚的跟随策略会让人感到不舒服。本文的特点在于利用3DLidar实际的观测专业护工与患老年痴呆症的病人之间的交互，制作合适的数据集，以便后续为机器人的行为制定更加拟人的规则。

Related Work

人类行为观测系统可以被归为两类：（1）固定于场景中的静态传感器系统；（2）在目标人物上连接可穿戴传感器，连至系统。
RGB相机的人类识别和追踪已有成熟的发展。如何重新识别出一个曾经离开过一次视域的人是该领域的一个重要问题。目前已有一些基于人物特征、软生物特征的方法被提出。激光测距仪比RGB有更广的视域，并且也具有极高的定位精度。但是固定场景的传感器终究视域有限，为此必须配置多个传感器，这提升了安装和标定的时间和花费。
而另一个长期、宽范围的人类行为观测方式是通过可携带设备。如GPS和INS可以帮助在室外环境的定位。但GPS在靠近建筑物时和室内环境里是不可用的。Wifi信号定位是使用了三角测距法，精度大概在分米级甚至厘米级。但由于需要多重天线布置，因此不太适合用于大的环境。而Wifi指纹匹配方法不需要额外的天线，因此可以用于大环境中，但精度有限。
在室内环境中还可以使用行人行尾推算（基于INS）和地图定位，需要用到的设备是智能手机。该方法可以很好的跟踪到人的位置，但由于INS是内部传感器，无法检测周围环境和人与人之间的交互，因此该方法无法用于这个课题。

System Overview

本文系统包括两部分：（1）离线建图；（2）在线传感器定位和人物检测/追踪。系统结构图如下图所示。

在离线建图阶段，我们创建一个覆盖整个测量区域的3D环境地图。对于地图，我们采用了基于图优化的SLAM方法(即， Graph SLAM[31])。为了补偿扫描匹配的累积旋转误差，我们分别对室内和室外环境引入了地面和GPS位置约束。
在行为测量阶段，通过结合扫描匹配算法和基于角速度的姿态预测(使用无迹卡尔曼滤波器[32])，系统在离线创建的地图上估计其姿态。同时，系统对目标人员进行检测和跟踪。

离线建图

由于懒得插入公式，因此直接截图了。然而，如果参数跨越非欧几里德空间(如位姿参数)，这些算法可能会导致次优或无效的解决方案。
算法的理论部分，尤其是引入的那个算子，不太能理解。
算法的实现部分倒是很简单，参考知乎专栏文章即可。

在室外地面不平整的情况下，我们使用GPS约束来代替地面检测约束。
对于2400m的45min的室内slam，作者既做了有无地面约束的对比，又用了现存的开源SLAM框架BLAM和LeGO-LOAM。BLAM由于没有累积的扫描匹配旋转误差无法找到闭环，并且生成了弯曲、不精确的地图。LeGO-LOAM保持了连续帧之间的地面局部一致性，相比BLAM略平整，但由于没有全局地面约束，仍然受到累积误差的影响。本文算法始终可以生成正确的位姿图，这有赖于NDT，但是如果除去地面约束，地面将会有坡度，即高度信息均出错。
实时性方面，LeGO-LOAM为实时的，无法计算时间。BLAM用了153327s，本文算法用了5382s，其中扫描匹配，地图检测，闭环检测占用时间为：1542，231，3619s。
在室外的测试中，如果除去GPS，由于起伏，将会导致无法闭环（因为匹配得分太高）。这也就解释了为什么我自己的室内没有IMU，人拿着都可以建出不错的图，原因在于使用了地面检测。

在线人物行为检测

为了测量人们的行为，该系统同时估计三维环境地图上的传感器姿态，并跟踪观察者周围的人。通过集成激光雷达提供的角速度和距离数据，系统估计传感器的姿态。然后，它检测并跟踪人们，以了解人们在环境地图上的位置。注意，传感器的初始姿态是手动给出的，以避免全局定位问题。在线系统框图见下图。
在这种情况下，传感器在帧间的运动变得非常大，扫描匹配可能会由于较大的位移而错误地估计传感器的自运动。为了解决这一问题，我们将基于无迹卡尔曼滤波器把NDT扫描与三维激光雷达提供的角速度数据结合在一起。

该系统利用无迹卡尔曼滤波对传感器姿态进行预测，然后利用NDT对观测到的点云与全局地图进行匹配，以估计的xt和qt作为传感器姿态的初始猜测。然后利用扫描匹配得到的传感器姿态对传感器状态进行校正。
在每个预测和修正步骤之后，我们对状态向量中的四元数进行归一化处理，以防止其范数由于无迹变换(在每个预测和修正步骤之后，我们对状态向量中的四元数进行归一化处理，以防止其范数由于无迹变换和累积的计算误差而变化。值得一提的是，我们还实现了考虑加速度的位姿预测。然而，由于加速度观测的强噪声，使得估计结果变差)和累积的计算误差而变化。值得一提的是，我们还实现了考虑加速度的位姿预测。然而，由于加速度观测的强噪声，使得估计结果变差。(我记得之前看Tracking的一篇论文的时候也提到这个问题了，引入加速度观测反而结果更差，共同点都是使用了卡尔曼滤波，也有可能是数据集的问题，比如实际中我们的加速度都是没什么规律的，完全取决于自己用力的大小，可能比较难预测。)

人物检测与跟踪

我们首先从观察到的点云中移除背景点来提取前景点。然后，我们从环境地图创建一个具有特定体素大小(例如0.5m)的占用网格地图。根据UKF估计的传感器姿态，将输入点云转换为地图坐标，然后将包含环境地图点的体素上的每个点作为背景去除。然后将欧氏聚类应用于前景点来检测人类候选聚类。但是，如果人员非常接近，他们的集群可能会被错误地合并，并被检测为单个集群。
为了解决这个问题，我们采用了Haselich的分块-归并聚类算法。该算法首先通过使用dp-means[42]将一个集群划分为子集群，直到每个集群小于一个阈值(例如0.45m)，这样每个集群就不会有不同人的点。然后，如果这些子集群之间没有差距，则认为这些集群属于单个人，并重新合并到一个集群中。
检测到的集群可能包含非人类集群(即假阳性)。为了在检测到的聚类中消除非人类聚类，我们使用Kidono等人和Adaboost训练的切片特征的人类分类器来判断聚类是否是人类。假设人在地平面上行走，我们跟踪XY平面上的人，没有高度。将卡尔曼滤波与恒速模型和全局最近邻数据关联[45]相结合进行人员跟踪。只要被跟踪的人从传感器可见并被正确地检测到，跟踪方案就能很好地工作。

传感器定位评价

通过对比实验说明，基于角速度的姿态预测使得姿态估计对快速运动具有较强的鲁棒性，且具有较快的收敛速度。因为这个预测信息给了NDT更好的初始条件，这点在快速运动时尤为重要。预测信息除了基于角速度，还与距离信息有关。
但我不太理解，这个角速度是什么意思，是雷达旋转的角速度？

人物检测评价

通过四个对比试验，证明了the split-merge clustering和 the human classifier的effective。结果如下表所示。

表中precision和recall是相互影响的。precision高意味着检测出来的都是对的，recall高意味着检测出了全部对的。而F-measure是综合评估模型的一个指标。

与静态传感器系统的比较

为了揭示提议的系统的优点和缺点，我们将提议的系统与公共可用的基于静态传感器的人员跟踪框架OpenPTrack（基于RGB-D相机的人物识别跟踪）进行了比较。该框架使用低成本硬件，易于安装，可以扩展至大型相机网络，被许多非机器视觉专家的人使用，如艺术家、心理学家。
比较是通过在一个长走廊里，布置九个kinetic v2相机，然后走过一个实验对象和一个带着本文设备的观察者，比较OpenPTrack和本文方法得到结果的误差。最终平均值不到0.1，标准差约0.04。以此证明该方法的精度不错，且拥有更好的视域（因为可以移动）。比如一个相机的视域2mX20m，本文使用的雷达有200mX50m。

On the other hand, static sensor-based systems can measure behavior of all people in the covered area simultaneously while the proposed system covers only the surrounding area.
这里是原文，感觉是表述错误。他的意思应该是：所提出的系统适用于测量大范围内特定人群的行为，而基于静态传感器的系统适用于在相对较小的环境中测量所有人的行为。

医院现场测试

在测量过程中，会有其他的病人和物体，比如轮椅和药品架，观察者有时必须快速移动以保持被测者在传感器视野中。但是，由于三维激光雷达测量范围宽，扫描匹配和基于角速度的姿态预测相结合，该系统能够在所有序列中正确定位。
在人员跟踪方面，当患者在观察者和被观察对象之间时，系统无法跟踪被观察对象，当被观察对象再次出现时，系统会分配新的id给被观察对象。在这种情况下，系统会通知它丢失了主题的踪迹，然后我们手动为它们重新分配正确的id。因为我们只看到过几次这种情况，所以系统可以跟踪大部分序列的主体，并且我们可以用最小的努力重新分配所有的id。
（这里可以看到，估计他们下一步应该是怎么解决重识别问题。这个问题在第一部分里有讲过了，不过估计是还没有融入到这个系统里。）

初步分析陪伴行为

为了照顾一个可以行走的人，机器人必须能够以1.4m/s行动。其他结论都是参照实际的护工的统计行为，但一共也才统计了17个人，我认为没有啥意义。如果真的要制作这类机器人，还不如专门去做特化的调研，而非借鉴他的两小时不到的统计。

Conclusion

本文介绍了一种基于三维激光雷达的便携式人体行为测量系统。该系统支持长期和广域的行为测量。系统首先使用graph slam方法创建环境的3D地图，然后进行测量。然后，它估计自己的姿势，同时检测和跟踪人们。该系统的跟踪精度可与基于静态传感器的人员跟踪系统相媲美。作为一个现场测试，我们证明了该系统的有效性，以衡量专业护理人员照顾老年人的行为。在对所测行为进行分析的基础上，提出了辅助机器人行为设计的经验规则。测量系统和专业护理人员行为数据集已经公开，因此可以用于测量和分析人们的护理行为。目前的系统需要一个带着3D激光雷达背包的人类观察者，因此需要人工观察。将人的观察者替换为移动机器人，从而自动创建一个大型的服务员行为数据集，以改进机器人服务员的行为。