TaichiSLAM: 一周构建稠密SLAM后端的尝试（一）介绍了 Taichi 用于 Mapping 的诸多优势，本文将更进一步地介绍使用 Taichi 建立 TSDF，ESDF，最后使用 Marching Cube 建立网格（Mesh）的过程。
SLAM 全称为 Simultaneous Localization and Mapping，即实时定位与地图构建，对移动机器人/无人机/自动驾驶/ eVTOL 等至关重要。
*相关术语注释：
TSDF (Truncated Signed Distance Functions)
ESDF (Euclidean Signed Distance Functions)
作者介绍
徐浩，香港科技大学电子计算机工程系博士在读。本科毕业于中国科学技术大学物理系，主要研究方向为无人机（UAV），无人机集群 （Aerial Swarm），无人机集群状态估计，协同定位与建图（CSLAM）。
作者在这里使用了 RealSense+DJI N3 采集了数据集，visual odometry 由作者维护的 VINS-Fusion 的鱼眼分支的非鱼眼版本建立。视频演示在作者的 B 站，阅读原文即可前往观看。
正文
渲染是在 GGUI 基础上开发的 TaichiSLAMRender，这里展示了上一章提到过的 Octomap 在此数据集上的显示。
TSDF & ESDF
言归正传，本文不会详细介绍算法，仅做成功展示。首先是 TSDF/ESDF 是很相似的两种东西，本质上都是使用一个数字表示是否在物体的内部外部，TSDF 比 ESDF 建立的过程要快很多，ESDF 适合无人机导航（例如 Fast-planner[1]），TSDF 则可以用于生成网格，关于这二者的具体讨论可以参见论文 「Voxblox: Incremental 3D Euclidean Signed Distance Fields for On-Board MAV Planning」[2]。
使用 Taichi 建立 TSDF/ESDF 的主要优点在于极容易并行化，可以无成本地使用 CUDA，并且可以直接使用稀疏数据结构，这对大场景的无人机应用是非常重要的。在我们先前的一些实验中，由于科研代码没有写复杂的数据结构，ESDF 和 TSDF 所占用的内存困扰了无人机实验很久，而如果使用 TaichiSLAM，则就是这么简单：
defdata_structures(self, block_num_xy, block_num_z, block_size):if
 block_size >
1
:
        Broot = ti.root.pointer(ti.ijk, (block_num_xy, block_num_xy, block_num_z))
        B = Broot.dense(ti.ijk, (block_size, block_size, block_size))
else
:
        B = ti.root.dense(ti.ijk, (block_num_xy, block_num_xy, block_num_z))
        Broot = B
return
 B, Broot
建立 TSDF 是一个高度需要并行化的过程。在 TaichiSLAM 中，我们读取 visual odometry 和 depth map，然后依据以下公式建立 TSDF，并进一步地迭代为 ESDF。
目前 ESDF 的功能还有点小问题，TSDF 倒是可以使用了，效果如下图所示。
图中显示了 TSDF 的切面和 TSDF 接近零的 voxel，也就是物体表面。
Marching Cube
仅仅使用粒子显示物体表面显然有问题，更常用的是使用网格。
原图请复制文末网址访问[3]
Marching Cube 则是根据 SDF 建立多面体的算法，本质上是查表并且根据查表的结果生成三角形。其算法极度简单而非常非常适合并行化，用 C++/CUDA 去写成本非常高（在并行化和内存管理），而用 Taichi 的实现大概就是👇
@ti.kerneldefgenerate_mesh_kernel(self, obs:ti.template(), tsdf: ti.template(), color: ti.template(), step:ti.i32, edgeTable: ti.ext_arr(), triTable: ti.ext_arr()):
        isolevel =
0.0
        self.num_triangles[
None
] =
0for
 i, j, k
in
 tsdf:
if
 obs[i, j, k] ==
1
:
#Marching...
            generate_marching_cube(i,j, k)
代码就自动并行了。得益于 Taichi 新的 GGUI，我们可以直接在显卡中把 Meshing 的计算用于渲染，整个过程全在显卡上运行，效率非常高！我们可以很简单的生成一个球体的 SDF，发现 Marching Cube 效果如下图：
不好意思放错图了，这图其实是 Bug 的产物（猜猜 Bug 出在了哪？）
@ti.funcdefvertexInterp_color(self, _p1, _p2, valp1, valp2, c1, c2, isolevel):
    p_ret = p1
    p_color = c1
    mu =
0.0if
 abs(isolevel - valp1) < EPS:
passelif
 abs(isolevel - valp2) < EPS:
        p_ret = p2
else
:
        mu = (isolevel - valp1) / (valp2 - valp1)
        p_ret = p1 + mu*(p2-p1)
在 0.9.2 版本的 Taichi 中，如果 p1 是 int 类型，则 p_ret 一直是 int 类型，重新赋值并不会改变其数据类型。
如果根据 TSDF 的 gradient 计算 normals，则显示效果如下图：
球体的显示如下图，接下来我们可以载入真正的地图显示网格了！
下面是基于 TSDF 的 Marching Cube 的效果演示，可以看到还是有一些问题，这需要对传感器进行降噪等等，就不在讨论范围内了。此外还有网格着色只有黑白，这是因为 RealSense 的彩色相机质量过差，导致彩图效果还不如黑白。等法向向量（目前根据 gradient 计算，还需要进一步优化），网格合并这些坑填完了，我会发布完整的演示视频。
计算速度有多快呢？在 5900X+3080 上是这么个速度：
Time: pcl2npy 0.6ms t_recast 9.8ms ms t_v2p 0.0ms t_export0.0ms t_mesh 4.9ms
小结
使用 Taichi 可以轻松完成一些 SLAM 的 Mapping 部分算法的加速，并且拥有极高的内存存储效率，在作者的下一篇主要论文中，会使用 TaichiSLAM 作为主要的 Mapping & Visualization 工具。
最近作者发现 Taichi 添加了 Sparse Matrix 有关的功能，结合 Difftaichi 已有的求导功能，我们甚至可以把 SLAM 中的 Localization 也交给 Taichi，这部分工作通常是通过Bundle Adjustment 或在 Pose Graph Optimization 这类优化算法实现的，而 Taichi 似乎可以让这个过程变得极度简单？
作者还在探索新的可能性，下一篇 TaichiSLAM 很快会和大家见面。
References
[1] https://github.com/HKUST-Aerial-Robotics/Fast-Planner
[2] Helen Oleynikova, Zachary Taylor, Marius Fehr, Juan Nieto, and Roland Siegwart, “Voxblox: Incremental 3D Euclidean Signed Distance Fields for On-Board MAV Planning”, in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2017.
[3] https://en.wikipedia.org/wiki/Marching_cubes
继续阅读
阅读原文