QuadricSLAM

QuadricSLAM是object SLAM的一种解决方案，可以提升定位的鲁棒性。物体的三维表示可以是CAD模型、点云模型、矩形包围盒、二次曲面等。QuadricSLAM使用的是二次曲面中的椭球面。

椭球面的定义是：
$X^TQX=0$
其中$X$为面上的点，$\pi = QX$为切平面。$Q$为4x4的对称矩阵，共10个自由度，包括一个尺度。
这里使用的是它的对偶:
$\pi^TQ^*\pi = 0$
非退化情况下，$Q^*=Q^{-1}$
$Q^*$可以表示为：
$Q^*=Z \bar{Q}^*Z^T$
其中：$Z=\begin{bmatrix}R(\theta)&t\\0&1\end{bmatrix}$,$\bar{Q}^*=\begin{bmatrix}s_1^2&0&0&0\\0&s_2^2&0&0\\ 0&0&s_3^2&0\\0&0&0&-1\end{bmatrix}$
椭球面在像素平面上的投影为一个二次曲线，但常常只部分可见：
$C^* = PQ^*P^T \\ P=K[R|t]$
这里通过计算二次曲线与图像边界的角点确定物体的包围框，这样就可以与目标检测结果bbox计算几何误差。

注意到，通过目标检测只能获得bbox，下面将它们转化为切平面：
$\pi = P^Tl$
其中$l$为box的一条边的Plucker向量表示。

每个bbox可以提供4个约束，而$Q^*$共有10个自由度，所以，3个以上的bbox就可以求解出$Q^*$。

在求解出$Q^*$后，可通过下面的公式恢复出：

个人感觉，QuadricSLAM虽然提供了将目标检测结果融入SLAM的方式，但以二次曲面做约束太弱了，对定位精度不会有提升。
参考：
https://zhuanlan.zhihu.com/p/73420573
https://arxiv.org/pdf/1804.04011
http://cvrs.whu.edu.cn/downloads/ebooks/%E8%AE%A1%E7%AE%97%E6%9C%BA%E8%A7%86%E8%A7%89%E4%B8%AD%E7%9A%84%E6%95%B0%E5%AD%A6%E6%96%B9%E6%B3%95.pdf