[paper阅读笔记]：先验3D surfel map下的单目相机定位

Title：Monocular Direct Sparse Localization in a Prior 3D Surfel Map

Haoyang ye, 港科大的工作
https://ieeexplore.ieee.org/xpl/conhome/9187508/proceeding可以用这个链接检索一个会议的所有论文。

TODO： 这篇文章是2020年的icra上的，那感觉可以追踪一下这个组近两年有什么更新

知识扫盲

%surfel map全称其实是surf element-based map，应该是根据loam提出来的，loam里把lidar建图里的点提取为edge点和surf点，那么surfel map应该具有包含平面点信息的一张地图。

1 Summary 写完笔记之后最后填，概述文章的内容，以后查阅笔记的时候先看这一段。注：写文章summary切记需要通过自己的思考，用自己的语言描述。忌讳直接Ctrl + c原文。

本文作者提出了一个VO，这个VO需要用到lidar或者其他什么传感器建立的先验3D地图，并需要把它处理成surfel map，因为后续构建损失函数的时候涉及到平面参数的使用。然后因为约束中涉及到的平面是global坐标系下的，所以估计的当前帧位姿可以直接得到当前帧在global下的pose，因此可以避免单目相机的尺度漂移。此外，因为使用的是global下的平面参数，而不是上一参考帧下的平面参数，所以可以减少位姿漂移。
但是也存在不使用的情况，在后文中也有提到，或者直接参考原文。
我觉得本文最大的贡献就是巧用了global地图的平面参数添加到损失函数中用于后续优化，然后我只需要一个大概的初始位姿，因为在优化的时候，要同时优化参考帧在global的位姿和当前帧在global的位姿，相当于是普通的3D-2D优化（单目的，需要对深度比例因子也进行优化，如果是rgbd就不需要优化）加上了一个固定深度的点到平面优化(相当于深度已知)。

2 Research Objective(s) 作者的研究目标是什么？

在一个先验的lidar点云地图中，用相机去定位。

3 Problem Statement 问题陈述：作者需要解决的问题是什么？

同步建图与定位通常是lidar建图lidar定位或者相机建图相机定位，但是lidar无法部署在小型和low-cost的平台上，而相机又没有足够的准确率和鲁棒性（相比于lidar，尤其单目相机有尺度漂移问题，天气和光照变换，appearance的变化等等都会影响相机定位与建图的稳定性，虽然这些变化导致的问题可以用同一个地方存储多种不同的地图进行一定程度的解决，但是太costly要存好多的地图）。
所以干脆不要同步建图与定位了，先用lidar把图建了，然后再用轻量化的，低成本的相机在地图里进行定位。于是作者希望
1. 实现单目相机在已经构建好的surfel map（有lidar建立的）中进行相机的定位。
2. 利用先验地图中的全局约束构建单目相机的尺度，和相机与全局坐标系之间的转换。
3. 对所提出的方法进行退化分析。
4. 在仿真和真实世界的实验中outperform先进的定位或者SLAM算法。

总结一下，上面说的可能不够清晰，这里直接回答“作者要解决什么问题？”，我觉得是：

1. 解决单目相机尺度不确定性问题。
2. 解决相机建图再定位过程中受光照变化影响的问题。

4 Method(s) 作者解决问题的方法/算法是什么？是否基于前人的方法？基于了哪些？

1. 首先需要有一个surfel map，这个surfel map上的每个点由position，点所在的local 平面的法向量和一个半径组成。
2. 因为定位的问题其实是在给定initial pose的基础上要返回后续相机在全局的位姿（基于绝对尺度的六自由度的位姿），所以需要通过这个位姿构建一个目标函数，然后通过牛顿法或者LM算法最小化这个目标函数以得到最优的pose。然后这里涉及到surfel的点和非surfel的点，非surfel的点tracking之后可以为帧间的相对位姿提供约束，而surfel的点可以为全局位姿（包括尺度）提供约束，而目标函数是通过构建光度误差得到的。这一部分是参考DSO构建的
3. 有一个map rendering模块，接受global frame到camera frame的转换矩阵，然后把surfels proejct 成一个depth map或者vertex&normal map（用opengl shading language），这样的话，对于当前帧像素坐标系上的点，我们就可以知道它的深度值或者它所在空间平面的position和normal (法向量和偏移可以确定一个三维平面)。
4. 构建tracked点在参考帧和当前帧之间的光度误差，如下图所示。
   
   
   然后引入了一个平面约束，基于下面这个假设，把目标函数里的Pt用平面参数，Ph，待估计参考帧和当前帧之间的pose表示出来
   （作者有一个轻微假设，就是认为参考帧和当前帧上的匹配的时候，一对匹配里的两个点即便不是准确的匹配但是也会是附近的点，所以他们极有可能是同一local平面上的点，所以参考帧上的点如果在depth和vertex&nromal map中都是有效点的话，那么会有一个local平面方程，那么这个方程应该对当前帧投影到参考帧的点也成立）。
   
5. 最终的目标函数是多个光度误差的加权的和，如下图所示，最外一层累加代表7个关键帧，第二层累加代表当前关键帧上被后续几帧tracked的点，第三层累加一些“当前帧”，最里面一层累加是对于当前关键帧当前tracked到的点以及当前这个“当前帧”对于参考帧tracked这个点周围的一些点。然后目标函数会自适应采用含有平面约束或者不含有平面约束的残差项（上面的公式1或者公式5）。

5 Evaluation 作者如何评估自己的方法，实验的setup是什么样的，有没有问题或者可以借鉴的地方

在Euroc和仿真软件中进行了实验，并且统计了不同surfel约束的比例对误差的影响，和noramls方向分布对误差的影响，以及对surfel map添加噪声后的影响。

6 Conclusion 作者给出了哪些结论，哪些是strong conclusions, 哪些又是weak的conclusions（即作者并不确定，只在discussion中提到，或没有给出足够的evidenc

surfel约束对于提高定位精度有影响，在损失函数中surfel约束的比例越多，位姿估计的越准确。
然后normals趋向于同一个方向分布的时候误差会大。

7 Notes(optional) 不在以上列表中，但需要特别记录的笔记

其实看这篇文章的时候，作为一个localization方面的小白，我其实非常疑惑这个问题构建的合理性，主要有两个问题：
第一，初始位姿是怎么来的？
第二，因为他已经假设初始位姿是已知的了，那么后续的重点其实是在于获得后续帧相对于第一帧的位姿，那么这不是其实还是基本的vo问题吗？只不过相当于我拥有先验的lidar建图之后，可以tracking的时候直到last frame的深度图（作者是一起把normal图也渲染出来了），那么以我的理解，直接用KLT做tracking，然后再用3D-2D的方法不就可以获得relative pose了吗？

针对第一个问题，我找到了作者的lab主页，然后查看了他们实验室最近几年跟localization有关的publications，然后发现本篇文章的同一一作在2021年的ICRA上发表了一个有关relocalization的方法（3D Surfel Map-Aided Visual Relocalization with Learned Descriptors），基于一个先验地图，一个视觉数据库和一个learned描述符。然后我大概搞懂了这里面的关系，其实localization就是指对相对序列初始位姿的估计过程（mapping是在localization的时候顺带把所用到的特征点或者图像里的像素点反投影成为三维点，但是这个地图还是相对于第一帧相机坐标系的）。relocalization并不是为了解决相机序列的tracking问题，他只需要解决一帧图片所代表的相机位置相对于global地图的位姿即可，就是给定地图找那一篇我去过就可以。那这样子看的话，该作者2020ICRA是探讨了利用先验地图进行VO的问题，然后2021年拓展这个工作在relocalization上。
至于怎么做到有尺度的，应该是残差项里面包含了尺度信息，比如参考帧tracked点的深度值，或者参考帧tracked点的平面方程。

针对第二个问题，我目前的理解是，如果按照我所说的思路，其实仍然是camera-based的方法，所以仍然存在camera-based方法所具备的所有的问题，所以作者引入了平面约束针对位姿估计。但是我仍然有问题，依我的理解这篇文章仍然是用特征点匹配的方法，只不过引入了global坐标系下的平面参数和深度信息可以让这个单目的尺度不确定性得到解决，但是怎么解决相机建图的鲁棒性不好的问题的呢？（光照变化啥的？）
目前，看到那个损失函数的话，我大概有点想法，因为优化问题的优化变量包括了两个表示光度变化的系数。

具体的实施细节中，提到了优化过程中需要伴随着点的更新，更新包括两种动作，一个是去除，一个是与平面绑定。
在优化的每次迭代之后，surfel map会随之更新，那么tracked点与对应的平面元素会有所变化，这个时候通过检验两种方法算出来的深度和投影到当前帧的点的相似性来决定是否滤出或者与surfel绑

TODO：还有一些点管理和marginalization的操作参考DSO。
还有几种退化情况，分别是只有非平面约束构建的损失函数时，对估计的位姿施加任意T和比例因子都可以满足同一个损失值，即不存在唯一最小值。
再有就是只有一个平面上的约束，T和比例因子都求不出来。
再有就是只有两个平行平面的约束，T无法求出来，比例因子可以确定。
再有一种情况还没有看懂。
这里主要是想说一下怎么分析失效情况。首先你不是有了个损失函数吗，然后你的损失函数里有一些值是待估计的，比如这里的参考帧和当前帧的相机位姿，还有一个比例因子。那么你就看是否这些代估计的量在变化的时候损失函数是不变的，如果是的话，那你这个就估计不出来一个唯一的值。具体的可以参考原文。

这个估计法向量的过程还可以啊，整个大地图才10ms左右

8 References(optional) 列出相关性高的文献，以便之后可以继续track下去

Direct Sparse Odometry：本文参考了这篇文章的点管理和边缘化操作，就是tracking部分和优化部分基本都是借鉴这篇文章。
3D Surfel Map-Aided Visual Relocalization with Learned Descriptors：作者基于本文的延伸工作，涉及重定位，或许可以给KCC如何做基于先验地图的定位带来一些启发。