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椭圆

在高中的时候我们学习二次曲线就知道，椭圆的标准公式为
x2a2+y2b2=1(3.1)\frac{x^2}{a^2}+\frac{y^2}{b^2}=1\tag{3.1} a2x2+b2y2=1(3.1)
移相后可以表示为
x2a2+y2b2−1=0(3.2)\frac{x^2}{a^2}+\frac{y^2}{b^2}-1 =0 \tag{3.2} a2x2+b2y2−1=0(3.2)
在本科的时候，我们学过线性代数，公式(3.2)可以使用二次型来表示
[xy1][1a20001b2000−1][xy1]=0(3.3)\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} [xy1]⎣⎡a21000b21000−1⎦⎤⎣⎡xy1⎦⎤=0(3.3)
上述公式我们可以记作
XTCX=0(3.4)X^TCX=0\tag{3.4} XTCX=0(3.4)
这就是使用齐次坐标来表示2D椭球的公式。在我们SLAM中，上述公式可以表示椭球投影到图像中所形成的椭圆，只不过增加了额外的旋转和平移。

到这里，是不是感觉很通俗易懂，OK，下面我们开始介绍2D空间中椭圆的对偶（dual）表示。什么是对偶空间呢，看下面这张图。

在点空间中（即公式3.3所定义的），我们使用点来描述椭圆。在对偶空间中，我们使用椭圆的包络切线来描述椭圆。在2D空间中，点和线是对偶的，3D空间中，点和面是对偶的。在对偶空间中描述椭圆为
XTC∗X=0(3.5)X^TC^*X = 0\tag{3.5} XTC∗X=0(3.5)
其中C∗C^*C∗表示矩阵CCC的伴随矩阵，即C∗=C−1∗∣C∣C^*=C^{-1}*|C|C∗=C−1∗∣C∣,由于使用的是齐次坐标，∣C∣|C|∣C∣只是一个尺度，可以忽略，因此C∗C^*C∗可以表示为
C∗=[a2000b2000−1](3.6)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} C∗=⎣⎡a2000b2000−1⎦⎤(3.6)
这就是2D图像平面中表示椭圆的矩阵形式，下面我们开始介绍3D空间中的椭球表示，及其坐标变换。

3.2.2椭球表示

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

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

在三维空间中，椭球的点表示为
[xyz1][1a200001b200001c20000−1][xyz1]=0(3.7)\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} [xyz1]⎣⎢⎢⎡a210000b210000c210000−1⎦⎥⎥⎤⎣⎢⎢⎡xyz1⎦⎥⎥⎤=0(3.7)
类似于2D空间，其对偶空间为
[n1n2n3d][a20000b20000c20000−1][n1n2n3d]=0(3.8)\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} [n1n2n3d]⎣⎢⎢⎡a20000b20000c20000−1⎦⎥⎥⎤⎣⎢⎢⎡n1n2n3d⎦⎥⎥⎤=0(3.8)
公式(3.7)和公式(3.8)描述的是在物体坐标系中的表示，即物体的中心为坐标系的原点，物体的长宽高分别为三个坐标轴。我们描述物体，都在放在世界坐标系（全局坐标系）中去描述。为了方便区分，上述在物体坐标系中的公式分别记作
XoTQoXo=0(3.9)X_o ^T Q_oX_o=0\tag{3.9} XoTQoXo=0(3.9)πoTQo∗πo=0(3.10)\pi_o^TQ_o^*\pi_o=0 \tag{3.10}πoTQo∗πo=0(3.10)

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

3.2.3 坐标转换

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

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

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

3.3 椭球投影

由公式(3.14)我们得到了物体在世界坐标系中的椭球矩阵，对于椭球的投影主要基于对偶空间。首先我们定义相机的投影矩阵PPP，即P=K[Rcw∣tcw]P=K[R_{cw}|t_{cw}]P=K[Rcw∣tcw]，是一个3x4的矩阵。可以想象到，一个三维空间中的平面，沿着垂直于法向量的方向投影到一个2D平面中，得到的是一条线段。

即πw=PTl\pi_w=P^Tlπw=PTl,代入公式(3.14)得，
lTPQw∗PTl=0(3.15)l^TPQ_w^*P^Tl=0 \tag{3.15} lTPQw∗PTl=0(3.15)
令C∗=PQw∗PTC^* =PQ_w^*P^TC∗=PQw∗PT，则C∗C^*C∗表示在图像坐标系中对偶空间中椭圆，然后求逆就可以得到点空间中的椭圆表示。下面是在Matlab中椭球变换。

4.总结

到这儿基于椭球的语义SLAM所涉及的基础知识就结束了，后面的内容将根据论文内容再更新。欢迎各位SLAMer批评指正。

使用椭球表示物体的语义SLAM相关推荐

QuadricSLAM: 面向机器人的物体级语义SLAM系统
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达本文由知乎作者原野寻踪授权转载,不得擅自二次转载.原文链接:https://zhuanlan.zhih ...
伦敦大学学院开源物体级语义SLAM！支持单/双目+LiDAR！
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达来源丨计算机视觉life 作者丨王敬文 1. 摘要最近,伦敦大学学院开源了面向对象的SLAM系统D ...
Co-Fusion：物体级别的语义SLAM
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者:robot L https://zhuanlan.zhihu.com/p/137100522 本 ...
论文精读 | 语义SLAM综述：现状回顾问题分析未来趋势！
作者 | 鱼肖农编辑 | 汽车人原文链接:zhuanlan.zhihu.com/p/590974335 点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨卷干货,即可获取 ...
Towards Real-time Semantic RGB-D SLAM in Dynamic Environments（动态语义SLAM）
动态环境下的实时语义SLAM 简介摘要系统流程实验结果总结简介在ICRA 2021上看到这样一篇论文:Towards Real-time Semantic RGB-D SLAM in Dy ...
用于自动泊车的鸟瞰图的边缘线的语义SLAM系统
文章:Hybrid Bird's-Eye Edge Based Semantic Visual SLAM for Automated Valet Parking 作者:Zhenzhen Xiang, ...
在结构化场景中基于单目的物体与平面SLAM方案
点云PCL免费知识星球,点云论文速读. 文章:Monocular Object and Plane SLAM in Structured Environments 作者:Shichao Yang, S ...
从五个经典工作看语义SLAM
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达本文试图概括语义(Semantic) SLAM的主要思路和近年工作 ...
语义SLAM近5年主要技术进展及研究趋势
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达本文试图概括语义(Semantic) SLAM的主要思路和近年工作 ...

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

基于二次曲面的物体级SLAM