[论文]QuadricSLAM: 面向机器人的物体级语义SLAM系统

原野寻踪

Robotics, 3D Vision, AI

前言

上一篇文章梳理了物体级表示方法中的立方体表达：

这篇文章探讨了一种将物体表达为紧凑的数学模型的方式。目前大部分语义SLAM的研究，仍然是将检测或分割所得到的语义信息往点里面融合而已，并基于此构造一个新的约束项往已有的优化方程里加。这样的工作是否真的能满足语义SLAM的需要？

语义SLAM有一个很美好的愿景在于机器人在导航时能理解环境，而人类理解环境时很重要的单位是物体，我们能够理解物体是什么(桌子、书本、水杯...)，具有哪些属性(静态、易碎、竖直放置)，该摆在哪里(桌面上、地上、挂在墙上..)。

而实现这一切的基础，在于物体需要有其在三维地图中的表达方式。近段时间探索了四种表达方式如下：

CubeSLAM+Plane: Monocular Object and Plane SLAM in Structured Environments; QuadricSLAM+Plane: Structure Aware SLAM using Quadrics and Planes

大部分在专栏里已经出过论文分析。其中CAD是帝国理工的名作SLAM++，而稠密点云是稠密表达，比如我讲过的MaskFusion：

它们更倾向于三维重建而非辅助机器人来做导航。而立方体和椭圆体表达都是紧凑的数学表达，表现在它们用方程来描述这个物体，而不是一堆离散的点或面元或已知的CAD模型。

立方体表达开篇就说过了，它的观测需要两个输入：物体检测的bbox和物体内提取线条恢复消影点。而这篇文章描述了稀疏表达的另一种椭圆体表达，要求的观测输入只有物体检测的bbox，理论深度更高一层。它有两个特点：

有完备的射影几何数学原理支撑，见Richard Hartley等的《Multiple View Geometry in Computer Vision》
过去在三维重建领域有大量研究，但目前在SLAM特别是移动机器人上的研究甚少，最近的在SLAM上的应用是2018-2019年的文章；

好，那我们来瞅瞅这个QuadricSLAM.

文章背景

QuadricSLAM: Dual quadrics from object detections as landmarks in object-oriented SLAM，改文章发表在 IEEE Robotics and Automation Letters 2018, 7月，由澳大利亚Queensland University of Technology的实验室Niko S¨underhauf教授发表。在此之前，他已经在这个领域探索很深，具体如下：

见澳大利亚分支，基本都是教授Niko S¨underhauf所做的工作。他给椭圆体SLAM专门做了个主页：

这个RA-L很有意思，我发现最近不少文章在RA-L上，但他是一个很新的期刊。在投IROS的时候会有个选项是否投RA-L，所以跟IROS关系绑得很密吧。

此外意大利有做三维重建的一些工作。但核心理论都是多视图几何中的对偶椭圆体、对偶二次曲线以及其投影关系，有时间我将单独另开一篇文章详细讲椭圆体的几何数学。

Introduction

文章所采用的椭圆体表达建模了物体这几个信息：大小、位置和朝向，可以作为更细致的三维稠密重建的锚点。

而对于该模型的观测，本文提出可以直接从2d检测的矩形框中估计出来二次曲线，然后构造椭圆体的约束。

如左图蓝色框为物体检测如Yolo给出的结果，然后通过这四条边恢复出空间平面，构造与椭圆体相切的约束。

III. 对偶二次曲面及理论

椭圆体属于二次曲面，而二次曲面的投影数学表达比较复杂，对偶二次曲面则非常简洁。这里仅仅介绍基础理论，详细的可参考圣经《计算机视觉中的多视图几何》。

A. Dual Quadrics

一般二次曲面由在面上的所有点定义，用4x4对称矩阵Q表示。而对偶二次曲面即用切面定义的二次曲面，用Q*表示，即所有平面满足： \pi^{\top} \mathbf{Q}^{*} \pi=0 ，一共有十个自由度，包含一个尺度。

它在图像平面的投影可以用一个公式描述，相当简洁：

\mathbf{C}^{*}=\mathbf{P Q}^{*} \mathbf{P}^{\top}

其中 \mathbf{P}=\mathbf{K}[\mathbf{R} | \mathbf{t}] 是相机投影矩阵。

B. Constrained Dual Quadric Parametrization

通用形式既可以表达封闭曲面，如球体、椭球，还有非封闭曲面如抛物面和双曲面。由于仅有封闭曲面是有意义的物体表达方式，我们使用带约束的对偶二次曲面来确保表面是椭球和球体。

Q*是个对称矩阵，通过特征值分解我们可以得到： \mathbf{Q}^{*}=\mathbf{Z} \breve{\mathbf{Q}}^{*} \mathbf{Z}^{\top} ，其中 Q是一个在原点的椭球，而Z则是一个齐次变换。即：

\mathbf{Z}=\left(\begin{array}{cc}{\mathbf{R}(\boldsymbol{\theta})} & {\mathbf{t}} \\ {\mathbf{0}_{3}^{\top}} & {1}\end{array}\right) and \mathbf{Q}^{*}=\left(\begin{array}{cccc}{s_{1}^{2}} & {0} & {0} & {0} \\ {0} & {s_{2}^{2}} & {0} & {0} \\ {0} & {0} & {s_{3}^{2}} & {0} \\ {0} & {0} & {0} & {-1}\end{array}\right)

于是我们紧凑地将其表达为9自由度向量: \mathbf{q}=\left(\theta_{1}, \theta_{2}, \theta_{3}, t_{1}, t_{2}, t_{3}, s_{1}, s_{2}, s_{3}\right)^{\top} ，包含旋转角、平移参数和大小。

IV. 物体检测器模型

我们将物体检测器，如yolo，直接建模为SLAM中的一个感知模型. 即在思维上将物体检测当成同激光、超声波、深度相机一样的传感器。

于是要先定义传感器模型，要求能够在给定相机位姿和地图中双曲面参数时，给出预测观测结果。即：

\boldsymbol{\beta}\left(\mathbf{x}_{i}, \mathbf{q}_{j}\right)=\hat{\mathbf{b}}_{i j}

我们要找一个这样的 \boldsymbol{\beta} 。

对于点的路标、激光雷达传感器或占据栅格地图，这类传感器模型都很简单，但对于物体检测器来说比较复杂。

推导观测模型表达式。将前面所说的二次曲面的投影过程应用，可以得到成像平面内的二次曲线。而比较naive的方法是对双曲线取外包络矩形，然后把成像到图像边界外的截断，如左图：

这样会造成一段error，因为物体检测只会框出较小的那部分矩形。比较好的方法是右侧这种，先计算出双曲线和成像边界的交点，然后去算一个矩形。我们把这个过程先记录为 BBOX (C) ，之后详细介绍。所以总体的观测模型为：

\boldsymbol{\beta}\left(\mathbf{x}_{i}, \mathbf{q}_{j}\right)=\operatorname{BBox}\left(\operatorname{adjugate}\left(\mathbf{P} \mathbf{Q}_{\left(\mathbf{q}_{j}\right)}^{*} \mathbf{P}^{\top}\right)\right)=\hat{\mathbf{b}}_{i j}

其中adjugate()表示将投影得到的对偶二次曲线取伴随，得到正常点表达的二次曲线。即正常二次曲线的C和对偶二次曲线的C*表达之间是伴随关系。

提出的 BBOX (C) 计算过程如下：

1）先找到四个双曲线的xy最大最小极值点，不考虑图像边界

2）找到双曲线与图像边界的所有交点，最多8个

3）去除不真实的点和所有在边界外的点

4）在剩下的点中找最大最小xy坐标作为最终的bounding box

即有： \hat{\mathbf{b}}=\left(\min _{x} \hat{\mathcal{P}}, \min _{y} \hat{\mathcal{P}}, \max _{x} \hat{\mathcal{P}}, \max _{y} \hat{\mathcal{P}}\right)

小结这个投影模型其实并不复杂，即双曲面自身的投影性质加上一个筛选bbox的过程。

V. 使用对偶双曲面路标的SLAM 过程

注意，本文不探究 data association 问题，而是假定数据关联是solved的. 而CubeSLAM中的data association是这么考虑的: 跟踪方框内的orb特征点。

SLAM问题的本质是一个最大后验估计，我们要求得使得后验最大的X(相机位姿)以及路标Q(我们的椭球体们):

P(X, Q | U, B) \propto \underbrace{\prod_{i} P\left(\mathbf{x}_{i+1} | \mathbf{x}_{i}, \mathbf{u}_{i}\right)}_{\text { Odometry Factors }} \underbrace{\prod_{i j} P\left(\mathbf{q}_{j} | \mathbf{x}_{i}, \mathbf{b}_{i j}\right)}_{\text { Landmark Factors }}

可以将该公式如传统方法一样建模成因子图。

\begin{aligned} X^{*}, Q^{*}=\underset{X, Q}{\operatorname{argmin}} &-\log P(X, Q | U, B) \\=& \underset{X, Q}{\operatorname{argmin}} \sum_{i}\left\|f\left(\mathbf{x}_{i}, \mathbf{u}_{i}\right) \ominus \mathbf{x}_{i+1}\right\|_{\Sigma_{i}}^{2} \\ &+\underbrace{\sum_{i j}\left\|\mathbf{b}_{i j}-\boldsymbol{\beta}_{\left(\mathbf{x}_{i}, \mathbf{q}_{j}\right)}\right\|_{\Lambda_{i j}}^{2}}_{\text { Quadric Landmark Factors }} \end{aligned}

这里简单理解就是在SLAM通用的图优化框架中，针对椭球体的数学模型增加边和节点。其中前面的式子是里程计带来的约束项，后面一项则是椭球体作为地图landmark的投影和观测带来的约束项。前面已经介绍过观测模型了。

几何误差项

针对椭球体的观测提出的几何误差项 \left\|\mathbf{b}_{i j}-\boldsymbol{\beta}_{\left(\mathbf{x}_{i}, \mathbf{q}_{j}\right)}\right\|_{\Lambda_{i j}}^{2} ，与前面的工作[14]-[16]提出的代数误差项比起来，当物体只有局部可见时的效果会更好(见前面的右图)。

使用带几何意义的误差项，使得我们能方便地通过协方差矩阵 \Lambda_{i j} ，传递物体检测器的空间不确定度到SLAM中。

变量初始化

包括相机位姿和椭球体在内的需要求解前先初始化。相机位姿的初始化可以通过里程计来算，而对偶双曲面路标的初始化则需要好好考虑一下。

首先我们可以用满足下列公式的最小二乘来初始化 \mathbf{q}_{j}： \boldsymbol{\pi}_{i j k}^{\top} \mathbf{Q}_{\left(\hat{\mathbf{q}}_{j}\right.}^{*} ) \boldsymbol{\pi}_{i j k}=0

因为根据对偶二次曲面的定义，它由与其表面相切的无限多个平面来定义。换句话说，任何与其表面相切的平面 \boldsymbol{\pi}_{i j k} ，都满足上述等于零的条件。

回忆一下这里 \mathbf{q}_{j} 代表的是十自由度向量 \hat{\mathbf{q}}=\left(\hat{q}_{1}, \dots, \hat{q}_{10}\right) 。而且这里描述的是通常的对偶二次曲面，而非我们想要的对偶椭球体，因为没有加约束。

于是问题变成了，我们需要找到足够多的平面，每一个平面构成上述等式的一个约束，然后最小二乘求一个解作为我们初始化的椭球体参数。

由于我们的观测是在二维图像平面上，平面会投影为一条线，由摄影几何，根据平面上投影的线和相机矩阵可以恢复出空间平面：

\boldsymbol{\pi}_{i j k}=\mathbf{P}_{i}^{\top} \mathbf{I}_{i j k}

那么这些线从哪来呢，我们的物体检测器给的bounding box不就提供了四条线吗?

于是每一帧观测便提供了四个平面的约束，多帧观测一起约束就可以求解出一个值了。

而这里的相机矩阵 \mathbf{P}_{i} 则上一时刻位姿和里程计推算得到的当前位姿，也是一个粗略量，但我们求的只是椭球体的初始值，所以是没问题的。

我们知道平面由四个参数定义，而Q是对称的，于是

\boldsymbol{\pi}_{i j k}^{\top} \mathbf{Q}_{\left(\hat{\mathbf{q}}_{j}\right.}^{*} ) \boldsymbol{\pi}_{i j k}=0

可以写为：

\left(\pi_{1}^{2}, 2 \pi_{1} \pi_{2}, 2 \pi_{1} \pi_{3}, 2 \pi_{1} \pi_{4}, \pi_{2}^{2}, 2 \pi_{2} \pi_{3}, \ldots\right. 2 \pi_{2} \pi_{4}, \pi_{3}^{2}, 2 \pi_{3}, \pi_{4}^{2} ) \cdot\left(\hat{q}_{1}, \hat{q}_{2}, \ldots, \hat{q}_{10}\right)^{\top}=0

再简写为： \mathbf{A}_{j} \hat{\mathbf{q}}_{j}=0 ，多个这样的方程联立，我们要找这样的 \hat{\mathbf{q}}_{j} 使得 \left\|\mathbf{A}_{j} \hat{\mathbf{q}}_{j}\right\| 最小。

而这个求解可以这样算：先完成SVD分解 \mathbf{A}_{j} \hat{\mathbf{q}}_{i}=\mathbf{U D V}^{\top} ，取矩阵 \mathbf{V} 的最后一列，即为我们要的最小二乘解 \hat{\mathbf{q}}_{j} 。

从通用二次曲面得到椭球体

那么注意了，这下解出来的$\hat{\mathbf{q}}_{j}$仍然是一个通用的二次曲面，并非约束到一个椭球体。接下来我们从中提取出双曲面的旋转、平移和形状参数。

按照引文[14]，双曲面的形状可以这样提取：

\left(\begin{array}{c}{s_{1}} \\ {s_{2}} \\ {s_{3}}\end{array}\right)=| \sqrt{-\frac{\operatorname{det} \mathbf{Q}}{\operatorname{det} \mathbf{Q}_{33}}\left(\begin{array}{c}{\lambda_{1}^{-1}} \\ {\lambda_{2}^{-1}} \\ {\lambda_{3}^{-1}}\end{array}\right)}

\mathbf{Q}_{33} 是左上角3x3的矩阵。而旋转矩阵等于 \mathbf{Q}_{33} 特征向量矩阵。而平移由 Q^{*} 最后一列定义： \mathbf{t}=\left(\hat{q}_{4}, \hat{q}_{7}, \hat{q}_{9}\right) / \hat{q}_{10} 。于是我们可以重建约束后的椭球体了。

总结作者初始化椭球体的方式，是先得到通用形式，再约束成一个椭球体形式。那么优化过程中，约束是否还存在呢？

个人对这个问题的解答是：优化过程使用的参数模型，已经是经过参数化的椭球体表达，即旋转、平移和大小，它们彼此之间已经没有了约束。所以只有初始化时比较难得到，一旦初始化，之后就与约束无关。