系统流程

预处理

深度测量值为深度真值加上高斯误差。由于设备的深度传感器可靠程度较差，每个深度测量值代表一个圆锥体，而不是一个射线。

深度图融合

对于每个深度测量值，根据深度确定方差和融合权重。将射线上深度比测量值小得多的体素恢复为初始设置。

基于动态Hash的体素存储

采用“Real-time 3d reconstruction at scale using voxel hashing”中提到的方法，来进行地图存储和管理。

位姿估计

使用VIO粗略估计位姿，然后用ICP进行优化。