使用椭球表示物体的语义SLAM

远方传来风笛_Jay

已于 2022-03-25 19:29:32 修改

阅读量3.9k

点赞数 8

分类专栏： SLAM 文章标签：算法

于 2022-03-25 17:45:39 首次发布

本文链接：https://blog.csdn.net/wmx1999/article/details/123737984

版权

SLAM 专栏收录该内容

3 篇文章 1 订阅

订阅专栏

基于二次曲面的物体级SLAM

1.SLAMer的入坑经历
2.相关论文
3.基础知识
4.总结

1.SLAMer的入坑经历

博主2020年9月份保研后开始在实验室接触视觉SLAM。一开始是从深蓝学院跟着高翔博士学习《视觉SLAM十四讲》，后来在计算机视觉life公众号学习ORBSLAM2源码讲解，把整个ORBSLAM2代码学习了一遍。本科毕业设计开始涉及语义SLAM，当初是在Voxblox++基础上完成了物体位置获取等功能。Voxblox++与其说是语义SLAM，不如说是三维重建，非常地吃计算资源，无法实时运行。研一入学后，开始学习物体级SLAM，使用包围盒或者是椭球来描述物体。对于椭球，使用9个参数就可以描述物体，减小了内存的消耗，并且可以有语义信息，使机器人能够充分地感知环境信息，完成复杂的人机交互，物体抓取等功能。That’s all,下面开始记录物体级语义SLAM的相关基础和论文阅读笔记。（第一次在CSDN上发博客，这markdown好难用呀）

2.相关论文

//这里先简单记录下使用椭球描述物体的相关论文，具体论文内容后面有时间再更新。

QuadricSLAM: Dual Quadrics From Object Detections as Landmarks in Object-Oriented SLAM
CubeSLAM: Monocular 3-D Object SLAM
EAO-SLAM: Monocular Semi-Dense Object SLAM Based on Ensemble Data Association
Accurate and Robust Object-oriented SLAM with 3D Quadric Landmark Construction in Outdoor Environment
An Orientation Factor for Object-Oriented SLAM
DSP-SLAM: Object Oriented SLAM with Deep Shape Priors
Dual Quadrics from Object Detection Bounding Boxes as Landmark Representations in SLAM
Object-oriented SLAM using Quadrics and Symmetry Properties for Indoor Environments
Real-Time Monocular Object-Model Aware Sparse SLAM
RGB-D Object SLAM Using Quadrics for Indoor Environments
Robust Improvement in 3D Object Landmark Inference for Semantic Mapping
Robust Object-based SLAM for High-speed Autonomous Navigation
Semantic SLAM with Autonomous Object-Level Data Association
Structure Aware SLAM using Quadrics and Planes
Unified Representation of Geometric Primitives for Graph-SLAM Optimization Using Decomposed Quadrics

3.基础知识

3.1视觉SLAM基础

大家既然看到这篇博客了，相信大家都已经或多或少学习了视觉SLAM，学习的资料可以有高翔博士的《视觉SLAM十四讲》，现在已经出到第二版了；小破站上有高翔博士的课程bilibili，其他的资料如ORBSLAM系列的论文，注释源码等。

《计算机视觉中的多视图几何》，这本书一定要看一下，其中的二次曲面等知识都是我们使用椭球表示SLAM的基础。如果觉得英文教材太复杂啰嗦，可以看一下吴福朝老师编写的《计算机视觉中的数学方法》。

3.2椭球表示

3.2.1 2D椭圆

在高中的时候我们学习二次曲线就知道，椭圆的标准公式为
$\frac{x^2}{a^2}+\frac{y^2}{b^2}=1\tag{3.1}$
移相后可以表示为
$\frac{x^2}{a^2}+\frac{y^2}{b^2}-1 =0 \tag{3.2}$
在本科的时候，我们学过线性代数，公式(3.2)可以使用二次型来表示
$\left[ \begin{matrix} x \quad y \quad 1 \end{matrix} \right] \left[\begin{matrix} \frac{1}{a^2} \quad 0 \quad 0 \\ 0 \quad \frac{1}{b^2} \quad 0 \\ 0 \quad 0 \quad -1 \end{matrix}\right] \left[ \begin{matrix} x \\ y \\ 1 \end{matrix}\right] = 0\tag{3.3}$
上述公式我们可以记作
$X^TCX=0\tag{3.4}$
这就是使用齐次坐标来表示2D椭球的公式。在我们SLAM中，上述公式可以表示椭球投影到图像中所形成的椭圆，只不过增加了额外的旋转和平移。

到这里，是不是感觉很通俗易懂，OK，下面我们开始介绍2D空间中椭圆的对偶（dual）表示。什么是对偶空间呢，看下面这张图。
在这里插入图片描述
在点空间中（即公式3.3所定义的），我们使用点来描述椭圆。在对偶空间中，我们使用椭圆的包络切线来描述椭圆。在2D空间中，点和线是对偶的，3D空间中，点和面是对偶的。在对偶空间中描述椭圆为
$X^TC^*X = 0\tag{3.5}$
其中 $C^*$ 表示矩阵 $C$ 的伴随矩阵，即 $C^*=C^{-1}*|C|$ ,由于使用的是齐次坐标， $∣ C ∣$ 只是一个尺度，可以忽略，因此 $C^*$ 可以表示为
$C^* = \left[\begin{matrix} a^2\quad 0 \quad 0 \\ 0 \quad b^2 \quad 0 \\ 0 \quad 0 \quad -1 \end{matrix}\right] \tag{3.6}$
这就是2D图像平面中表示椭圆的矩阵形式，下面我们开始介绍3D空间中的椭球表示，及其坐标变换。

3.2.2椭球表示

为了方便描述，我们预先定义一些符号

$Q$ ：3D空间中的椭球矩阵，是一个对称矩阵
$Q^*$ ：椭球的对偶表示形式
$Q_o$ ：在物体坐标系中的椭球矩阵
$Q_w$ ：世界坐标系中的椭球矩阵
$Q_c$ ：相机坐标系中的椭球矩阵， $Q^*$ 也有上述的类似表示
$X$ ：表示3D空间点，使用齐次坐标表示，即 $x,y,z,1)^T$
$\pi$ ：表示3D平面， $n_1,n_2,n_3,d)^T$ ，前三维表示平面的法向量，最后一维表示距离
$T_{wo}$ ：表示物体坐标系到世界坐标系的转换， $T_{wc}$ 表示相机到世界坐标系转换
$K$ ：表示相机的内参矩阵
物体的长宽高分别为 $2 a, 2 b, 2 c$ 。
物体在世界坐标系中的位置为 $t_x,t_y,t_z)$
OK~ ~ ~ ，下面我们开始进入正题。

在三维空间中，椭球的点表示为
$\left[ \begin{matrix} x & y & z & 1\end{matrix}\right] \left[ \begin{matrix} \frac{1}{a^2} & 0 & 0 &0 \\ 0 & \frac{1}{b^2} & 0 &0 \\ 0 & 0 & \frac{1}{c^2} & 0 \\ 0 & 0 & 0 & -1\end{matrix}\right] \left[ \begin{matrix} x \\ y \\ z \\ 1\end{matrix}\right]=0\tag{3.7}$
类似于2D空间，其对偶空间为
$\left[ \begin{matrix} n_1 & n_2 & n_3 & d\end{matrix}\right] \left[ \begin{matrix} a^2 & 0 & 0 &0 \\ 0 & b^2 & 0 &0 \\ 0 & 0 & c^2 & 0 \\ 0 & 0 & 0 & -1\end{matrix}\right] \left[ \begin{matrix} n_1 \\ n_2 \\ n_3 \\ d\end{matrix}\right]=0\tag{3.8}$
公式(3.7)和公式(3.8)描述的是在物体坐标系中的表示，即物体的中心为坐标系的原点，物体的长宽高分别为三个坐标轴。我们描述物体，都在放在世界坐标系（全局坐标系）中去描述。为了方便区分，上述在物体坐标系中的公式分别记作
$X_o ^T Q_oX_o=0\tag{3.9}$ $\pi_o^TQ_o^*\pi_o=0 \tag{3.10}$

有了上述基础，我们开始进行坐标变换，如何一步步从3D椭球投影到2D图像中

3.2.3 坐标转换

在SLAM十四讲中，我们知道可以使用变换矩阵来将齐次坐标点从一个坐标系转换到另一个坐标系。对于物体坐标系中的点 $X_o$ ，到世界坐标系的转换为
$X_w = T_{wo}X_o\tag{3.11}$
上述公式可以表示为 $X_o = T_{wo}^{-1}X_w=T_{ow}X_w$ ，带入到公式(3.9)中则有
$X_w^TT_{ow}^TQ_oT_{ow}X_w=X_w^TQ_wX_w = 0\tag{3.12}$
上述公式中的 $Q_w$ 即椭球在世界坐标系中的转换。同时，可以得到 $Q_w$ 是一个对称矩阵，并且其中的元素包含物体的旋转、平移和尺寸。在QuadricSLAM中，就是从拟合出来的 $Q_w$ 来得到物体的九维数据 $roll,pitch,yaw,t_x,t_y,t_z,a,b,c)$ 。

上面我们推导的是点空间中的坐标变换，下面我们来推导对偶空间中的坐标变换，这个比点空间更加常用

在多视图几何中（这部分可以参考《计算机视觉中的多视图几何》，这本书简直是SLAM领域中的圣经呀）物体坐标系中的平面 $\pi_o$ 到世界坐标系中的 $\pi_w$ 转化为
$\pi_w = T_{wo}^{-T}\pi_o\tag{3.13}$
可以表示为 $\pi_o=T_{wo}^T\pi_w$ ，代入到公式（3.10）中，则有
$\pi_w^TT_{wo}Q_o^*T_{wo}^T\pi_w=\pi_w^TQ_w^*\pi_w=0\tag{3.14}$
上述 $Q_w^*$ 为世界坐标系中椭球的对偶表示。

3.3 椭球投影

由公式(3.14)我们得到了物体在世界坐标系中的椭球矩阵，对于椭球的投影主要基于对偶空间。首先我们定义相机的投影矩阵 $P$ ，即 $P=K[R_{cw}|t_{cw}]$ ，是一个3x4的矩阵。可以想象到，一个三维空间中的平面，沿着垂直于法向量的方向投影到一个2D平面中，得到的是一条线段。
在这里插入图片描述
即 $\pi_w=P^Tl$ ,代入公式(3.14)得，
$l^TPQ_w^*P^Tl=0 \tag{3.15}$
令 $C^* =PQ_w^*P^T$ ，则 $C^*$ 表示在图像坐标系中对偶空间中椭圆，然后求逆就可以得到点空间中的椭圆表示。下面是在Matlab中椭球变换。