【自动驾驶】Submap总结

大规模建图优化问题

使用图像或雷达扫描点云帧序列完成大规模三维重建，如城市规模三维重建，将全局环境地图分割为submap（子地图），“各个击破”——逐一优化每个子地图，并优化子地图间关联关系，如相对位姿优化调整。
Ni Kai等人提出的主要流程：a. 消除每个submap内部的优化变量，只保留submap之间的变量 b. 优化submap之间的变量 c. 优化每个submap内部的变量。问题中变量是指传感器位姿和地图点坐标等。引入”局部坐标系“，submap内部变量用基础节点（base node，笔者理解为根据submap设定的局部坐标系）表达相对坐标，submap之间优化通过调整基础节点实现，使得submap整体发生很大变化，但内部变量无需改变。

1. 分割submap在工程上具体操作是什么？
   形如切豆腐，分割后一个几何空间中的路标点和相机位姿组成一个submap。
2. submap间的约束如何构建？
   如使用重投影约束，子地图 A中某陆标点${\mathbf{X}}_A$（坐标为A对应的局部坐标系）被B中的相机观测到，可据此构建submap间的重投影约束。Ni Kai的方法以基础节点的姿态与位置参数为优化参数，间接改变陆标点坐标和相机的位姿参数，以优化重投影误差：该陆标点在B相应局部坐标系下的表达，

$${\mathbf{X}}_B=R_{WB}^{-1}[(R_{WA}{\mathbf{X}}_A+t_{WA})-t_{WB}]$$

则在相机坐标系下的表达，
$${\mathbf{X}}_C=R_{CB}{\mathbf{X}}_B+t_{CB}$$
其中，$t_{CB}$ 为相机坐标系下B相应局部坐标系的原点坐标，$R_{CB}$为相机坐标系下B相应局部坐标系的姿态变换矩阵；陆标点在待投影相机的坐标系下表示为${\mathbf{X}}_C$，投影方程表达如下，
π ( X C ) = [ f u X C Z C + c u f v Y C Z C + c v ] \pi(\bold X_C)=\left[

$$\begin{array}{ccc}f_u\frac{X_C}{Z_C}+c_u\\ f_v\frac{Y_C}{Z_C}+c_v\end{array}$$ \right] π(XC​)=[fu​ZC​XC​​+cu​fv​ZC​YC​​+cv​​]

设图像检测得同名点为$x$，则重投影误差为，
$$e=||x-\pi ({\mathbf{X}}_C)||$$

大规模点云地图可视化问题

大规模点云地图的点云数量一般为10的6+次方量级，直接而全部加载对内存大小要求高， 空间尺度大，不便于快速变换视角，为便于可视化，提高查看效率，可将原始点云按网格分组，如以20m见方将点云分划为不同的submap，可视化“缩略地图”，每个submap以单个点显示，一瞥全貌，同时可选中感兴趣的区域，“放大”，可显示选中区域的原始点云细节。