SLAM中线特征的参数化和求导

众所周知，线特征比点特征可以提供更多的约束条件，并且在某些场合下比点特征要鲁棒。但是如果我们想要把线特征加入到SLAM系统中面临的是参数化和优化求导。这篇文章介绍了贺博的PL-VIO中线特征的参数化和求雅克比。

下面我们就来详细介绍一下空间直线的两种参数化方法：Plücker参数化方法，直线正交表示方法。为什么需要两种参数化方法呢？因为空间中的直线有4个自由度，而Plücker参数化方法需要使用6个参数表示直线，这样就会导致过参数化，过参数化在优化的时候就需要采用带约束的优化，不太方便。于是引入了可以用4个参数更新直线的正交表示来方便优化。这两种参数化方法可以很方便的相互转换，所以我们可以在SLAM系统中同时使用这两种参数化形式，在初始化和进行空间变换的时候使用Plücker坐标，在优化的时候使用正交表示。下面我们就来详细了解这两种参数化方法和优化的雅克比求导。

1. Plücker参数化方法

在3D空间中，直线 $\mathcal{L}$ 的Plücker坐标表示为 $\mathcal{L} = (\bold{n}^T, \bold{d}^T)^T \in R^6$ ，其中 $\bold{d} \in R^3$ 是直线的方向向量， $\bold{n} \in R^3$ 是由直线和光心构成的平面 $\pi$ 的法向量。Plücker坐标是过参数化的，比如 $\bold{n}$ 和 $\bold{d}$ 之间存在约束关系（ $\bold{n}^T \bold{d} = 0$ ）。于是Plücker坐标不能直接使用无约束的优化。不过这种使用法向量和方向向量的表现形式在初始化直线和进行空间变换的时候很方便。所以在SLAM系统中我们可以使用Plücker坐标来初始化和变换，具体的初始化和变换方法如下。

图1 线特征的Plücker坐标（a）Plücker线坐标系（b）新观测到的线的初始化

当给定从世界坐标系w到相机c的变换矩阵 $T_{cw}=\left[\begin{matrix}{R_{cw}}&{{[p_{cw}]_{\times}R_{cw}}}\\{0}&{R_{cw}}\\\end{matrix}\right]$ ，可以通过下面的形式将世界坐标系和相机坐标系下的Plücker线坐标进行变换：

$\mathcal{L}^c = \begin{bmatrix} \bold{n}^c \\ \bold{d}^c \end{bmatrix} = T_{cw} \mathcal{L}^w = \begin{bmatrix} R_{cw} & [p_{cw}]_{\times} R_{cw} \\ 0 & R_{cw} \end{bmatrix} \mathcal{L}^w$

当我们从不同的两帧相机观测到一个新的线路标的时候，Plücker坐标初始化方式也很简单。如图1b中所示，直线 $\mathcal{L}$ 在两帧图像 c_1 和 c_2 中表示为两个线段 $z^{c_1}_{\mathcal{L}}$ 和 $z^{c_2}_{\mathcal{L}}$ 。线段 $z^{c_1}_{\mathcal{L}}$ 在归一化平面可以被两个端点表示， $s^{c_1} = \begin{bmatrix} u_s & v_s & 1 \end{bmatrix}^T$ 和 $s^{c_2} = \begin{bmatrix} u_e & v_e & 1 \end{bmatrix}^T$ 。再加上坐标原点 $C = \begin{bmatrix} x_0 & y_0 & z_0 \end{bmatrix}^T$ ，这三个点可以确定一个平面： $\Pi = \begin{bmatrix} \Pi_x & \Pi_y & \Pi_z & \Pi_w \end{bmatrix}^T$ ，使其满足

$\Pi_x (x - x_0) + \Pi_y (y - y_0) + \Pi_z (z - z_0) = 0$

其中：

$\begin{bmatrix} \Pi_x \\ \Pi_y \\ \Pi_z \end{bmatrix} = [s^{c_1}]_{\times} e^{c_1}$

$\Pi_w = \Pi_x x_0 + \Pi_y y_0 + \Pi_z z_0$

然后对同一个空间直线在两帧相机平面的投影和相机光心可以确定两个平面 $\pi_1$ 和 $\pi_2$ ，通过两个平面可以确定唯一的空间直线。有了这两个平面我们可以得到对偶Plücker矩阵 L^*

$L^* = \begin{bmatrix} [\bold{d}]_{\times} & \bold{n} \\ -\bold{n}^T & 0 \end{bmatrix} = \pi_1 \pi_2^T - \pi_2 \pi_1^T \in R^{4 \times 4}$

然后从对偶矩阵中可以提取出Plücker坐标 $\mathcal{L} = (\bold{n}^T, \bold{d}^T)^T$ 。法向量 $\bold{n}$ 和方向向量 $\bold{d}$ 都不要求是单位向量。

2. 正交表示法

由于3D空间的直线只有4个自由度，使用Plücker坐标这种过参数化的表示形式在优化的时候不方便，所以我们引入四个参数的正交表示 $\left ( \bold{U}, \bold{W} \right ) \in SO(3) \times SO(2)$ 。Plücker坐标和正交表示之间可以很方便的互相转换，我们之后会分别介绍如何从Plücker坐标到正交表示和正交表示怎么变换成Plücker坐标。首先我们需要知道了直线的Plücker坐标 $\mathcal{L} = (\bold{n}^T, \bold{d}^T)^T$ ，然后对 $\mathcal{L}$ 进行QR分解，得到：

$\begin{bmatrix} \bold{n} & \bold{d} \end{bmatrix} = \begin{bmatrix} \frac{\bold{n}}{\left \| \bold{n} \right \|} & \frac{\bold{d}}{\left \| \bold{d} \right \|} & \frac{\bold{n} \times \bold{d}}{\left \| \bold{n} \times \bold{d} \right \|} \end{bmatrix} \begin{bmatrix} \left \| \bold{n} \right \| & 0 \\ 0 & \left \| \bold{d} \right \| \\ 0 & 0 \end{bmatrix}$

分解得到的第一项是正交矩阵 $\bold{U}$ ，是一个旋转矩阵。所表示的是相机坐标系到直线坐标系的旋转。其中直线坐标系的定义如下：用直线的方向向量以及直线和光心组成平面的法向量作为坐标的两个轴，再用他们叉乘得到的向量作为第三个轴，所以

$\bold{U} = R(\psi) = \begin{bmatrix} \frac{\bold{n}}{\left \| \bold{n} \right \|} & \frac{\bold{d}}{\left \| \bold{d} \right \|} & \frac{\bold{n} \times \bold{d}}{\left \| \bold{n} \times \bold{d} \right \|} \end{bmatrix}$

其中 $\bold{\psi} = [\psi_1, \psi_2, \psi_3]^T$ $\bold{\psi} = [\psi_1, \psi_2, \psi_3]^T$ 代表的是相机坐标系到直线坐标系在x，y和z轴的旋转角。

到此我们得到了正交表示的第一项，第二项需要做一些小变换。由于将 $(\left \| \bold{n} \right \|, \left \| \bold{d} \right \|)$ ，结合之后只有一个自由度，所以我们可以用三角函数矩阵参数化：

上式中的 $\phi$ 是旋转角。由于坐标原点（相机光心）到3D直线的距离是 $d = \frac{\left \| \bold{n} \right \|}{\left \| \bold{d} \right \|}$ ，所以包含了距离信息d的。根据 $\bold{U}$ 和 $\bold{W}$ 的定义可以看出，4个自由度包括旋转的3个自由度和距离的一个自由度。在优化的时候，我们使用 $\mathcal{O} = [\bold{\psi}, \bold{\phi}]^T$ 作为空间直线更新的最小表示。

正交表示到Plücker坐标之间的变换可以通过下面的方式计算出来：

其中 u_i 代表的是 $\bold{U}$ 矩阵的第i列。虽然 $\mathcal{L}$ 和 $\mathcal{L}$ 有一个尺度的差，但是它们表示的是同一条空间直线。

3. 直线的观测模型和误差

图2 空间直线投影到像素平面

要想知道线特征的观测模型，我们需要知道线特征从归一化平面到像素平面的投影内参矩阵 $\mathcal{K}$ 。如图2，点C和D是直线 $\mathcal{L} = (\bold{n}^T, \bold{d}^T)^T$ 上两点，点c和d是它们在像素平面上的投影。 c = KC ， d = KD ，K是相机的内参矩阵。 $\bold{n} = [C]_{\times} D$ ， $l = \begin{bmatrix} l_1 & l_2 & l_3 \end{bmatrix} = [c]_{\times} d$ 。那么有

上式表明，直线的线投影只和法向量有关和方向向量无关。

关于投影的误差，我们不可以直接从两幅图像的线段中得到，因为同一条直线在不同图像线段的长度和大小都是不一样的。衡量线的投影误差必须从空间中重投影回当前的图像中才能定义误差。在给定世界坐标系下的空间直线 $\mathcal{L}^w$ 和正交表示 $\mathcal{O}^w$ ，我们首先使用 $T_{cw} = \begin{bmatrix} R_{cw} & [p_{cw}]_{\times} R_{cw} \\ 0 & R_{cw} \end{bmatrix}$ 将直线变换到相机 c_i 坐标下。然后再将直线投影到成像平面上得到投影线段 $l^{c_i}$ ，然后我们就得到了线的投影误差。我们将线的投影误差定义为图像中观测线段的端点到从空间重投影回像素平面的预测直线的距离。

其中 $s^{c_i}$ 和 $e^{c_i}$ 是图像中观测到的线段端点， $l^{c_i}$ 是重投影的预测的直线。

4. 误差雅克比推导

如果要优化的话，需要知道误差的雅克比矩阵：

线特征在VIO下根据链式求导法则：

其中第一项 $\frac{\partial r}{ \partial l^{c_i} }$ ，因为

所以：

第二项 $\frac{ \partial l^{c_i} }{ \partial \mathcal{L}^{c_i} }$ ，因为

所以：

最后一项矩阵包含两个部分，一个是相机坐标系下线特征对的旋转和平移的误差导数，第二个是直线对正交表示的四个参数增量的导数

第一部分中，

在VIO中，如果要计算线特征的重投影误差，需要将在世界坐标系w下的线特征变换到IMU坐标系 b下，再用外参数 $T_{bc}$ 变换到相机坐标系c下。所以

其中

线特征 $\mathcal{L}$ 只优化状态变量中的位移和旋转，所以只需要对位移和旋转求导，其他都是零。下面我们来具体分析旋转和位移的求导。首先是线特征对旋转的求导：

然后是线特征对位移的求导：

第二部分中 $\frac{ \partial \mathcal{L}^{c_i} }{ \partial \mathcal{L}^{w} } \frac{ \partial \mathcal{L}^{w} }{ \partial \mathcal{O}^{w} }$ ，先解释第一个

所以

然后后面的 $\frac{ \partial \mathcal{L}^{w} }{ \partial \mathcal{O}^{w} }$ 有两种思路，先介绍第一种：

其中 $\mathcal{L}$ 对 $\bold{U}$ 和 $\bold{w} = [w_1, w_2]$ 求导，因为 $\mathcal{L}^w = \begin{bmatrix} w_1 \bold{u}_1^T & w_2 \bold{u}_2^T \end{bmatrix}^T$ ，所以

然后是 $\bold{U}$ 对 $\phi$ 和 $\bold{W}$ 对 $\phi$ 的求导，

附录

Plücker坐标变换矩阵自洽性证明

此处把位姿矩阵写作T，其对应的Plücker坐标变换矩阵写作 $\mathcal{T}$

$\mathcal{T}= \begin{bmatrix} R & [p]_{\times} R \\ 0 & R \end{bmatrix}$

如果 T = T_1 T_2 ，那么 $\mathcal{T} = \mathcal{T}_1 \mathcal{T}_2$ 是否成立呢？

证明如下

$\mathcal{T}_1 \mathcal{T}_2 = \begin{bmatrix} R_1 & [p_1]_{\times} R_1 \\ 0 & R_1 \end{bmatrix} \begin{bmatrix} R_2 & [p_2]_{\times} R_2\\ 0 & R_1 \end{bmatrix} \\ = \begin{bmatrix} R_ 1 R_2 & R_ 1 [p_2]_{\times} R_2 + [p_1]_{\times} R_1 \\ 0 & R_1 R_2 \end{bmatrix} \\$

而

$\mathcal{T}= \begin{bmatrix} R & [p]_{\times} R \\ 0 & 1 \end{bmatrix} \\ = \begin{bmatrix} R_1 R_2 & [R_1 p_2 + p_1]_{\times} R_1 R_2 \\ 0 & R_1 R_2 \end{bmatrix}$

有定理 $R p_{\times} R^T = (Rp)_{\times}$ ，所以

$[R_1 p_2 + p_1]_{\times} R_1 R_2 \\ = [R_1 p_2 + p_1]_{\times} R_1 R_2 \\ = [R_1 p_2]_{\times}R_1 R_2 + [p_1]_{\times} R_1 R_2 \\ = R_1 [p_2]_{\times} R_1^T R_1 R_2 + [p_1]_{\times} R_1 R_2 \\ = R_1 [p_2]_{\times} R_2 + [p_1]_{\times} R_1 R_2$

证毕

参考文献

PL-VIO: Tightly-Coupled Monocular Visual–Inertial Odometry Using Point and Line Features

SLAM中线特征的参数化和求导 - 知乎

分类:热门推荐日期:2024-06-26浏览:1评论:0