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