点击上方“3D视觉工坊”，选择“星标”
干货第一时间送达
本文由知乎作者原野寻踪授权转载，不得擅自二次转载。原文链接：https://zhuanlan.zhihu.com/p/73420573
前言
上一篇文章梳理了物体级表示方法中的立方体表达：
https://zhuanlan.zhihu.com/p/69683253
这篇文章探讨了一种将物体表达为紧凑的数学模型的方式。目前大部分语义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：
https://zhuanlan.zhihu.com/p/123142370
它们更倾向于三维重建而非辅助机器人来做导航。而立方体和椭圆体表达都是紧凑的数学表达，表现在它们用方程来描述这个物体，而不是一堆离散的点或面元或已知的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专门做了个主页：
https://nikosuenderhauf.github.io/semanticslam.ai/quadricslam.html
这个RA-L很有意思，我发现最近不少文章在RA-L上，但他是一个很新的期刊。在投IROS的时候会有个选项是否投RA-L，所以跟IROS关系绑得很密吧。
此外意大利有做三维重建的一些工作。但核心理论都是多视图几何中的对偶椭圆体、对偶二次曲线以及其投影关系，有时间我将单独另开一篇文章详细讲椭圆体的几何数学。
Introduction
文章所采用的椭圆体表达建模了物体这几个信息：大小、位置和朝向，可以作为更细致的三维稠密重建的锚点。
而对于该模型的观测，本文提出可以直接从2d检测的矩形框中估计出来二次曲线，然后构造椭圆体的约束。
如左图蓝色框为物体检测如Yolo给出的结果，然后通过这四条边恢复出空间平面，构造与椭圆体相切的约束。
对偶二次曲面及理论
椭圆体属于二次曲面，而二次曲面的投影数学表达比较复杂，对偶二次曲面则非常简洁。这里仅仅介绍基础理论，详细的可参考圣经《计算机视觉中的多视图几何》。
A. Dual Quadrics
一般二次曲面由在面上的所有点定义，用4x4对称矩阵Q表示。而对偶二次曲面即用切面定义的二次曲面，用Q*表示，即所有平面满足： ，一共有十个自由度，包含一个尺度。
它在图像平面的投影可以用一个公式描述，相当简洁：
其中 是相机投影矩阵。
B. Constrained Dual Quadric Parametrization
通用形式既可以表达封闭曲面，如球体、椭球，还有非封闭曲面如抛物面和双曲面。由于仅有封闭曲面是有意义的物体表达方式，我们使用带约束的对偶二次曲面来确保表面是椭球和球体。
Q*是个对称矩阵，通过特征值分解我们可以得到： ， 其中 Q是一个在原点的椭球，而Z则是一个齐次变换。即：
and
于是我们紧凑地将其表达为9自由度向量: ， 包含旋转角、平移参数和大小。
物体检测器模型
我们将物体检测器，如yolo，直接建模为SLAM中的一个感知模型. 即在思维上将物体检测当成同激光、超声波、深度相机一样的传感器。
于是要先定义传感器模型，要求能够在给定相机位姿和地图中双曲面参数时，给出预测观测结果。即：
我们要找一个这样的 。
对于点的路标、激光雷达传感器或占据栅格地图，这类传感器模型都很简单，但对于物体检测器来说比较复杂。
推导观测模型表达式。将前面所说的二次曲面的投影过程应用，可以得到成像平面内的二次曲线。而比较naive的方法是对双曲线取外包络矩形，然后把成像到图像边界外的截断，如左图：
这样会造成一段error，因为物体检测只会框出较小的那部分矩形。比较好的方法是右侧这种，先计算出双曲线和成像边界的交点，然后去算一个矩形。我们把这个过程先记录为 ， 之后详细介绍。所以总体的观测模型为：
其中adjugate()表示将投影得到的对偶二次曲线取伴随，得到正常点表达的二次曲线。即正常二次曲线的C和对偶二次曲线的C*表达之间是伴随关系。
提出的 计算过程如下：
1） 先找到四个双曲线的xy最大最小极值点，不考虑图像边界
2） 找到双曲线与图像边界的所有交点，最多8个
3） 去除不真实的点和所有在边界外的点
4） 在剩下的点中找最大最小xy坐标作为最终的bounding box
即有：
小结 这个投影模型其实并不复杂，即双曲面自身的投影性质加上一个筛选bbox的过程。
使用对偶双曲面路标的SLAM 过程
注意，本文不探究 data association 问题，而是假定数据关联是solved的. 而CubeSLAM中的data association是这么考虑的: 跟踪方框内的orb特征点。
SLAM问题的本质是一个最大后验估计，我们要求得使得后验最大的X(相机位姿)以及路标Q(我们的椭球体们):
可以将该公式如传统方法一样建模成因子图。
这里简单理解就是在SLAM通用的图优化框架中，针对椭球体的数学模型增加边和节点。其中前面的式子是里程计带来的约束项，后面一项则是椭球体作为地图landmark的投影和观测带来的约束项。前面已经介绍过观测模型了。
几何误差项
针对椭球体的观测提出的几何误差项 ，与前面的工作[14]-[16]提出的代数误差项比起来，当物体只有局部可见时的效果会更好(见前面的右图)。
使用带几何意义的误差项，使得我们能方便地通过协方差矩阵 ，传递物体检测器的空间不确定度到SLAM中。
变量初始化
包括相机位姿和椭球体在内的需要求解前先初始化。相机位姿的初始化可以通过里程计来算，而对偶双曲面路标的初始化则需要好好考虑一下。
首先我们可以用满足下列公式的最小二乘来初始化
因为根据对偶二次曲面的定义，它由与其表面相切的无限多个平面来定义。换句话说，任何与其表面相切的平面 ，都满足上述等于零的条件。
回忆一下这里 代表的是十自由度向量
。而且这里描述的是通常的对偶二次曲面，而非我们想要的对偶椭球体，因为没有加约束。
于是问题变成了，我们需要找到足够多的平面，每一个平面构成上述等式的一个约束，然后最小二乘求一个解作为我们初始化的椭球体参数。
由于我们的观测是在二维图像平面上，平面会投影为一条线，由摄影几何，根据平面上投影的线和相机矩阵可以恢复出空间平面：
那么这些线从哪来呢，我们的物体检测器给的bounding box不就提供了四条线吗?
于是每一帧观测便提供了四个平面的约束，多帧观测一起约束就可以求解出一个值了。
而这里的相机矩阵 则上一时刻位姿和里程计推算得到的当前位姿，也是一个粗略量，但我们求的只是椭球体的初始值，所以是没问题的。
我们知道平面由四个参数定义，而Q是对称的，于是
可以写为：
再简写为： ， 多个这样的方程联立，我们要找这样的
使得
最小。
而这个求解可以这样算：先完成SVD分解 ，取矩阵
的最后一列，即为我们要的最小二乘解
。
从通用二次曲面得到椭球体
那么注意了，这下解出来的$\hat{\mathbf{q}}_{j}$仍然是一个通用的二次曲面，并非约束到一个椭球体。接下来我们从中提取出双曲面的旋转、平移和形状参数。
按照引文[14]，双曲面的形状可以这样提取：
是左上角3x3的矩阵。而旋转矩阵等于
特征向量矩阵。而平移由
最后一列定义：
。于是我们可以重建约束后的椭球体了。
总结 作者初始化椭球体的方式，是先得到通用形式，再约束成一个椭球体形式。那么优化过程中，约束是否还存在呢？
个人对这个问题的解答是：优化过程使用的参数模型，已经是经过参数化的椭球体表达，即旋转、平移和大小，它们彼此之间已经没有了约束。所以只有初始化时比较难得到，一旦初始化，之后就与约束无关。
Future
文章QuadricSLAM核心验证了将物体表达为椭圆体的想法，在之后的Structure Aware SLAM using Quadrics and Planes这篇文章中，进一步探索了与面的约束，如我们的先验知识“物体摆在桌面上”，几何描述即为桌面平面与椭圆体相切，转化为数学模型即：
非常优雅。作者在这条路线上探索室内环境更多物体-物体，物体-结构的约束，似乎也是不错的方向。
右图增加接触约束后，椭圆体不再陷入到桌面里面。
上述内容，如有侵犯版权，请联系作者，会自行删文。
推荐阅读：
那些精贵的3D视觉系统学习资源总结（附书籍、网址与视频教程）
机器人抓取汇总|涉及目标检测、分割、姿态识别、抓取点检测、路径规划
重磅！3DCVer-知识星球和学术交流群已成立
3D视觉从入门到精通知识星球：针对3D视觉领域的知识点汇总、入门进阶学习路线、最新paper分享、疑问解答四个方面进行深耕，更有各类大厂的算法工程人员进行技术指导，700+的星球成员为创造更好的AI世界共同进步，知识星球入口：
学习3D视觉核心技术，扫描查看介绍，3天内无条件退款
圈里有高质量教程资料、可答疑解惑、助你高效解决问题
欢迎加入我们公众号读者群一起和同行交流，目前有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流等微信群，请扫描下面微信号加群，备注：”研究方向+学校/公司+昵称“，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进去相关微信群。原创投稿也请联系。
▲长按加群或投稿
226
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
