查看原文
其他

「技术博客 004」TaichiSLAM 系列 2: 网格的建立,TSDF,ESDF & Marching Cube

徐浩 太极图形 2023-03-21
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,则就是这么简单:
def data_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.kernel
    def generate_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] = 0
        for 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.func
def vertexInterp_color(self, _p1, _p2, valp1, valp2, c1, c2, isolevel):
    p_ret = p1
    p_color = c1
    mu = 0.0
    if abs(isolevel - valp1) < EPS:
        pass
    elif 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

您可能也对以下帖子感兴趣

文章有问题?点此查看未经处理的缓存