Ⅰ. 状态向量与目标函数

状态向量共包括滑动窗口内的 $n+1$ 个所有 IMU 状态$\mathbf{x}_k,k\in[0,n]$(包括位置、朝向、速度、加速度计 $bias$ 和陀螺仪 $bias$)、 Camera 到 IMU 的外参 $\mathbf{x}^b_c$、 $m+1$ 个 3D 点的逆深度$\lambda_l,l\in[0,m]$:

$$ \begin{align*} \mathcal{X}&=[\mathbf{x}_0,\mathbf{x}_1,···,\mathbf{x}_n,\mathbf{x}^b_c,\lambda_0,\lambda_1,···,\lambda_m]\\ \mathbf{x}_k&=[\mathbf{p}^w_{b_k},\mathbf{v}^w_{b_k},\mathbf{q}^w_{b_k},\mathbf{b}_a,\mathbf{b}_g],k\in[0,n]\\ \mathbf{x}^b_c&=[\mathbf{p}^b_c,\mathbf{q}^b_c] \end{align*}\tag{1} $$

$n$ 是关键帧的总数,$m$ 是滑动窗口中的特征总数。 $λ_l$ 是第 $l$ 个特征与其第一次观察的逆深度。

使用 BA 公式,我们最小化所有测量残差的先验和马氏距离之和,以获得最大后验估计为:

$$ \underset{\mathcal{X} }{\min} \left\{ \underbrace {\parallel \mathbf{r}_p-\mathbf{H}_p\mathcal{X} \parallel^2}_{边缘化的先验信息} +\underbrace {\sum_{k\in\mathcal{B} }\parallel\mathbf{r}_{\mathcal{B} }(\hat{\mathbf{z} }^{b_k}_{b_{k+1} },\mathcal{X}) \parallel^2_{\mathbf{P}^{b_k}_{b_{k+1} } } }_{IMU 测量残差} +\underbrace {\sum_{(l,j)\in\mathcal{C} }\rho(\parallel\mathbf{r}_\mathcal{C}(\hat{\mathbf{z} }^{c_j}_{l},\mathcal{X}) \parallel^2_{\mathbf{P}^{c_j}_l })}_{视觉的重投影残 差} \right\}\tag{2} $$

其中 $\mathbf{r}_{\mathcal{B} }(\hat{\mathbf{z} }^{b_k}_{b_{k+1} },\mathcal{X})$ 和 $\mathbf{r}_\mathcal{C}(\hat{\mathbf{z} }^{c_j}_{l},\mathcal{X}) $ 分别是 IMU 和视觉测量的残差。 $\mathcal{B}$ 是所有 IMU 测量值的集合,$\mathcal{C}$ 是在当前滑动窗口中至少观察到两次的特征集合。 $\{\mathbf{r}_p, \mathbf{H}_p\}$ 是来自边缘化的先验信息。 三种残差都是用马氏距离表示。

Ⅱ. 视觉约束

引言

  • VINS中的重投影误差

image-20220726151556969

视觉的帧间约束为重投影误差,如上图所示,对于路标点 $l$ ,我们使用相机可以观测到它在第 $i$ 帧(关键帧)中的像素坐标,即在第 $i$ 帧中的 $\overline{\mathcal{P} }^{c_i}_l$(图中红色点),然后根据光流法可以求得其在第 $j$ 帧中的 $\hat{\overline{\mathcal{P} } }^{c_j}_l$(图中红色点);

而根据坐标变换,我们同样可以利用我们所求的外参计算第 $i$ 帧中的点,经过坐标变换,在第 $j$ 帧中的理论值 $\mathcal{P}^{c_j}_l$; 由于收到噪声的影响,两者是不可能重合的,两者之间的差值就是我们需要优化的残差,进而达到优化外参以及3D点等优化变量的目的。

  • 为什么使用逆深度?

① 逆深度可以用来降低 较远的深度点(视差应该越小)造成的误差,这样做更符合精度的表达,除此,逆深度表达相对较远的点时,其分布接近高斯分布

② 采用逆深度的方式,可以减少优化变量,节省计算量;例如:表达一个路标点的坐标 $(x,y,z)$,可以写成 $(x,y,z)=\frac{1}{\lambda}\begin{bmatrix}u\\v\\1\end{bmatrix}$,将3个优化变量,变成了一个优化变量($u,v$ 为归一化相机坐标系下的三维点的坐标,通过观测数据是可知的),所以优化变量少了。

A. 残差

视觉残差是重投影误差, 对于第 $l$ 个路标点 $P$,将 $P$ 从第一次观看到它的第 $i$ 个相机坐标系,转换到当前的第 $j $个相机坐标系下的像素坐标,可定义视觉误差项为:

$$ \begin{align*} \mathbf{r}_\mathcal{C}(\hat{\mathbf{z} }^{c_j}_{l},\mathcal{X})&=[\mathbf{b}_1,\mathbf{b}_2]^T\cdot(\hat{\overline{\mathcal{P} } }^{c_j}_l-\frac{\mathcal{P}^{c_j}_l}{\parallel\mathcal{P}^{c_j}_l \parallel^2})\\ \end{align*}\tag{3} $$

$\hat{\overline{\mathcal{P} } }^{c_j}_l$ 为第 $l$ 个路标点在第 $j$ 个相机归一化相机坐标系中的观察到的坐标

$$ \hat{\overline{\mathcal{P} } }^{c_j}_l=\pi^{-1}_c \left(\begin{bmatrix}\hat{u}^{c_j}_l\\\hat{v}^{c_j}_l \end{bmatrix}\right)\tag{4} $$

$\mathcal{P}^{c_j}_l$ 是估计第 $i$ 个图像中发生的第 $l$个特征的第一次观察,转换到第 $j$ 个相机归一化相机坐标系中的理论的坐标:

$$ \begin{align*} \mathcal{P}^{c_j}_l &=\begin{bmatrix} x_{c_j}\\y_{c_j}\\z_{c_j} \end{bmatrix}\\ &=\mathbf{R}^c_b \left\{\mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c} \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right]-\mathbf{p}^{b}_{c} \right\}\\ &=\mathbf{R}^c_b\mathbf{R}^{w}_{b_i}\mathbf{R}^{w}_{b_i}\mathbf{R}^{b}_{c} [\frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l]+\mathbf{R}^c_b \left\{\mathbf{R}^{b_j}_w \left[ \left(\mathbf{R}^{w}_{b_i}\mathbf{p}^{b}_{c} \right) -\mathbf{p}^{w}_{b_j}\right] -\mathbf{p}^{b}_{c} \right\} \end{align*} $$

其中, $[\hat{u}^{c_i}_l,\hat{v}^{c_i}_l]$ 是第 $i$ 个图像中发生的第 $l$个特征的第一次观察。 $[\hat{u}^{c_j}_l,\hat{v}^{c_j}_l]$ 是对第 $j$ 个图像中相同特征的观察。 $\pi^{-1}_c$ 是反投影函数,它使用相机内在参数将像素位置转换为单位向量。

$$ \overline{\mathcal{P} }^{c_i}_l= \pi^{-1}_c \left(\begin{bmatrix}\hat{u}^{c_i}_l\\\hat{v}^{c_i}_l \end{bmatrix} \right)\tag{6} $$

由于视觉残差的自由度为 $2$,我们将残差向量投影到切平面上。 $\mathbf{b}_1$ 和 $\mathbf{b}_2$ 是两个任意选择的正交基,它们跨越 $\hat{\overline{\mathcal{P} } }^{c_j}_l$ 的切平面,如图所示。在 $(2)$ 中使用的方差 $\mathbf{P}^{c_j}_l$ 也从像素坐标传播到单位球体上.

image-20220520195143150

单位球面上的视觉残差图。 $\hat{\overline{\mathcal{P} } }^{c_j}_l$ 是在第 $j$ 帧中观察第 $l$ 个特征的单位向量。$\mathcal{P}^{c_j}_l$ 是通过将其在第 $i$ 帧中的第一个观察值转换为第 $j$ 帧来预测的单位球体上的特征测量。残差定义在 $\hat{\overline{\mathcal{P} } }^{c_j}_l$ 的切平面上

这里给出公式 $(5)$ 的推导:

设特征 $l$ 点 在第 $i$ 帧被观测到的像素坐标系下的坐标为: $\mathcal{P}_{uv_i}$ ,在世界坐标系下的坐标为:$\mathcal{P}^w_l$,则根据变换矩阵可得:

$$ \begin{align*} \mathcal{P}_{uv_i}&=\lambda_l\pi_c\left({\mathbf{T}^b_c}^{-1}{\mathbf{T}^w_{b_i} }^{-1}\mathcal{P}^w_l \right)\\ \Leftrightarrow \mathcal{P}^w_l&=\mathbf{T}^w_{b_i}\mathbf{T}^b_c\frac{1}{\lambda_l}{\pi_c}^{-1}(\mathcal{P}_{uv_i})\\ &=\mathbf{T}^w_{b_i}\mathbf{T}^b_c\frac{1}{\lambda_l}\overline{\mathcal{P} }^{c_i}_l\\ \mathcal{P}^w_l&= \begin{bmatrix}\mathbf{R}^w_{b_i}&\mathbf{p}^w_{b_i}\\0&1\end{bmatrix} \begin{bmatrix}\mathbf{R}^b_c&\mathbf{p}^b_c\\0&1\end{bmatrix}\frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l\\ &=\begin{bmatrix} \mathbf{R}^w_{b_i}\mathbf{R}^b_c&\mathbf{R}^w_{b_i}\mathbf{p}^b_c+\mathbf{p}^w_{b_i}\\ 0&1\end{bmatrix}\frac{1}{\lambda_l}\overline{\mathcal{P} }^{c_i}_l\\ &=\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c} \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i} \end{align*}\tag{IIA-1} $$

设特征 $l$ 点 在第 $j$ 帧被观测到的相机坐标系下的坐标为: $\mathcal{P}^{c_j}_{l}$

$$ \begin{align*} \mathcal{P}^{c_j}_l&={\mathbf{T}^b_c}^{-1}{\mathbf{T}^w_{b_j} }^{-1}\mathcal{P}^w_l \\ \Leftrightarrow \mathcal{P}^w_l&=\mathbf{T}^w_{b_j}\mathbf{T}^b_c\mathcal{P}^{c_j}_l\\ \mathcal{P}^w_l&=\begin{bmatrix}\mathbf{R}^w_{b_j}&\mathbf{p}^w_{b_j}\\0&1\end{bmatrix} \begin{bmatrix}\mathbf{R}^b_c&\mathbf{p}^b_c\\0&1\end{bmatrix}\mathcal{P}^{c_j}_l\\ &=\begin{bmatrix} \mathbf{R}^w_{b_j}\mathbf{R}^b_c&\mathbf{R}^w_{b_j}\mathbf{p}^b_c+\mathbf{p}^w_{b_j}\\ 0&1\end{bmatrix}\mathcal{P}^{c_j}_l\\ &=\mathbf{R}^{w}_{b_j} \left(\mathbf{R}^{b}_{c} \mathcal{P}^{c_j}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_j} \end{align*}\tag{IIA-2} $$

将 $\mathrm{(IIA-1)}$ 带入 $\mathrm{(IIA-2)}$ ,可以得到:

$$ \begin{align*} &\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c} \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}=\mathbf{R}^{w}_{b_j} \left(\mathbf{R}^{b}_{c} \mathcal{P}^{c_j}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_j}\\ &\Rightarrow \mathcal{P}^{c_j}_l =\mathbf{R}^c_b \left\{\mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c} \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right]-\mathbf{p}^{b}_{c} \right\} \end{align*}\tag{IIA-3} $$

B. 待优化变量

待优化变量主要是 $i$ 和 $j$ 两个时刻相机的位姿以及逆深度:

$$ [\mathbf{p}^w_{b_i},\mathbf{q}^w_{b_i}],\ \ \ \ \ [\mathbf{p}^w_{b_j},\mathbf{q}^w_{b_j}],\ \ \ \ \ [\mathbf{p}^b_{c},\mathbf{q}^b_{c}],\ \ \ \ \ \lambda_l\tag{7} $$

C. Jacobian

综上所述,我们所得到的残差指的是,将第 $i$ 帧图像中发生的第 $l$个特征的第一次观察的像素坐标系(观测值:$\overline{\mathcal{P} }^{c_i}_l$),经过坐标变换,转移到第 $j$ 帧相机归一化相机坐标系中,得到 $\mathcal{P}^{c_j}_l$,与第 $l$ 个路标点在第 $j$ 帧相机归一化相机坐标系中的观察到的坐标$\hat{\overline{\mathcal{P} } }^{c_j}_l$ 做比较,理论上,它们是相等的,但是由于存在噪声,故而存在偏差。因此,我们的目的就是优化位姿来最小化残差。

上述残差使用的是球面模型,实际代码中使用的仍是归一化相机坐标的模型,

由于残差:

$$ \mathbf{r}_{\mathcal{C} }= \begin{bmatrix} \frac{x_{c_j} }{z_{c_j} }-u_{c_j}\\ \frac{y_{c_j} }{z_{c_j} }-v_{c_j} \end{bmatrix} $$

其中,$\mathcal{P}^{c_j}_l =\begin{bmatrix}x_{c_j}\\y_{c_j}\\z_{c_j}\end{bmatrix}$表示路标在 $j$ 帧下的坐标,$\begin{bmatrix}\frac{x_{c_j} }{z_{c_j} }&\frac{y_{c_j} }{z_{c_j} }\end{bmatrix}^T$ 表示对应的归一化坐标,$\begin{bmatrix}u_{c_j}&v_{c_j}\end{bmatrix}^T$表示光流得到的在第 $j$ 帧下的观测。

则利用链式求导法则:

$$ \frac{\partial \mathbf{r}_{\mathcal{C} } }{\partial \mathcal{X} } =\frac{\partial \mathbf{r}_{\mathcal{C} } }{\partial \mathcal{P}^{c_j}_l }* \frac{\partial \mathcal{P}^{c_j}_l }{\partial \mathcal{X} } $$

a.求取 $\frac{\partial \mathbf{r}_{\mathcal{C} } }{\partial \mathcal{P}^{c_j}_l }$

$$ \begin{align*} \frac{\partial \mathbf{r}_{\mathcal{C} } }{\partial \mathcal{P}^{c_j}_l } =\begin{bmatrix} \frac{1}{z_{c_j} }&0&-\frac{x}{z_{c_j}^2 }\\ 0&\frac{1}{z_{c_j} }&-\frac{y}{z_{c_j}^2 } \end{bmatrix} \end{align*} $$

b.求取 $\frac{\partial \mathcal{P}^{c_j}_l }{\partial \mathcal{X} }$

根据视觉残差公式,我们可以得到相对于各优化变量的 Jacobian:

$$ \begin{align*} \frac{\partial \mathcal{P}^{c_j}_l }{\partial \mathcal{X} } &= \begin{bmatrix} \mathbf{J}[0]&\mathbf{J}[1]\\ \mathbf{J}[2]&\mathbf{J}[3] \end{bmatrix}\\ &=\begin{bmatrix} \left[ \frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{p}^w_{b_i} }, \frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{q}^w_{b_i} } \right]& \left[ \frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{p}^w_{b_j} }, \frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{q}^w_{b_j} } \right]\\ \left[ \frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{p}^b_c }, \frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{q}^b_c } \right]& \frac{\partial \mathcal{P}^{c_j}_l }{\partial\lambda_l } \end{bmatrix} \end{align*} $$


$$ \mathbf{J}[0]^{3\times7}= \left[ \frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{p}^w_{b_i} }, \frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{q}^w_{b_i} } \right] = \begin{bmatrix} \mathbf{R}^c_b\mathbf{R}^{b_j}_w & -\mathbf{R}^c_b\mathbf{R}^{b_j}_w\mathbf{R}^w_{b_i}\ \left(\mathbf{R}^{b}_{c}\frac{\overline{\mathcal{P} }^{c_i}_l}{\lambda_l}+\mathbf{p}^{b}_{c} \right)^{\land} \end{bmatrix}\tag{J0} $$


  • 推导 $\frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{p}^w_{b_i} }$:

$$ \begin{align*} \frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{p}^w_{b_i} } &=\frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{p}^w_{b_i} }\\ &=\frac{\partial \mathbf{R}^c_b \left\{\mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c} \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right]-\mathbf{p}^{b}_{c} \right\} }{\partial\mathbf{p}^w_{b_i} }\\ &=\mathbf{R}^c_b\mathbf{R}^{b_j}_w \end{align*}\tag{J0-1} $$

  • 推导 $\frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{q}^w_{b_i} }$:

$$ \begin{align*} \frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{q}^w_{b_i} } &=\frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{q}^w_{b_i} }\\ &=\frac{\partial \mathbf{R}^c_b \left\{\mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c} \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right]-\mathbf{p}^{b}_{c} \right\} }{\partial\mathbf{q}^w_{b_i} }\\ &=\underset{\delta\theta^w_{b_i}\rightarrow0 }{\lim} \frac{\mathbf{R}^c_b \left\{\mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c} \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right]-\mathbf{p}^{b}_{c} \right\} }{\delta\theta^w_{b_i} }\\ &=\underset{\delta\theta^w_{b_i}\rightarrow0 }{\lim} \frac{ \mathbf{R}^c_b \mathbf{R}^{b_j}_w \mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c} \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right) }{\delta\theta^w_{b_i} }\\ &=\underset{\delta\theta^w_{b_i}\rightarrow0 }{\lim} \frac{ \mathbf{R}^c_b \mathbf{R}^{b_j}_w \mathbf{R}^{w}_{b_i}\exp({\delta\theta^w_{b_i} }^{\land}) \left(\mathbf{R}^{b}_{c} \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)-(···) }{\delta\theta^w_{b_i} }\\ &=\underset{\delta\theta^w_{b_i}\rightarrow0 }{\lim} \frac{ \mathbf{R}^c_b \mathbf{R}^{b_j}_w \mathbf{R}^{w}_{b_i}(I+{\delta\theta^w_{b_i} }^{\land}) \left(\mathbf{R}^{b}_{c} \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)-(···) }{\delta\theta^w_{b_i} }\\ &=\underset{\delta\theta^w_{b_i}\rightarrow0 }{\lim} \frac{ \mathbf{R}^c_b \mathbf{R}^{b_j}_w \mathbf{R}^{w}_{b_i}{\delta\theta^w_{b_i} }^{\land} \left[\left(\mathbf{R}^{b}_{c} \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)\right] }{\delta\theta^w_{b_i} }\\ &=-\underset{\delta\theta^w_{b_i}\rightarrow0 }{\lim} \frac{ \mathbf{R}^c_b \mathbf{R}^{b_j}_w \mathbf{R}^{w}_{b_i} \left[\left(\mathbf{R}^{b}_{c} \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)\right]^{\land}\delta\theta^w_{b_i} }{\delta\theta^w_{b_i} }\\ &=-\mathbf{R}^c_b \mathbf{R}^{b_j}_w \mathbf{R}^{w}_{b_i} \left[\left(\mathbf{R}^{b}_{c} \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)\right]^{\land} \end{align*}\tag{J0-2} $$


$$ \mathbf{J}[1]^{3\times7}= \left[ \frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{p}^w_{b_j} }, \frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{q}^w_{b_j} } \right] = \begin{bmatrix} -\mathbf{R}^c_b\mathbf{R}^{b_j}_w & \mathbf{R}^c_b \left\{ \mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c} \frac{\overline{\mathcal{P} }^{c_j}_l}{\lambda_l}+\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right] \right\}^{\land} \end{bmatrix}\tag{J1} $$


  • 推导 $\frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{p}^w_{b_j} }$:

$$ \begin{align*} \frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{p}^w_{b_j} } &=\frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{p}^w_{b_j} }\\ &=\frac{\partial \mathbf{R}^c_b \left\{\mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c} \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right]-\mathbf{p}^{b}_{c} \right\} }{\partial\mathbf{p}^w_{b_j} }\\ &=-\mathbf{R}^c_b\mathbf{R}^{b_j}_w \end{align*}\tag{J1-1} $$

  • 推导 $\frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{q}^w_{b_j} }$:

$$ \begin{align*} \frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{q}^w_{b_j} } &=\frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{q}^w_{b_j} }\\ &=\frac{\partial \mathbf{R}^c_b \left\{\mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c} \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right]-\mathbf{p}^{b}_{c} \right\} }{\partial\mathbf{q}^w_{b_j} }\\ &=\frac{\partial \mathbf{R}^c_b \mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c} \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right] }{\partial\mathbf{q}^w_{b_j} }\\ &=\underset{\delta\theta^w_{b_j}\rightarrow0 }{\lim} \frac{\ \mathbf{R}^c_b \left[\mathbf{R}^{b_j}_w\exp({\delta\theta^w_{b_j} }^{\land}) \right]^{-1} \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c} \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right] }{\delta\theta^w_{b_j} }\\ &=\underset{\delta\theta^w_{b_j}\rightarrow0 }{\lim} \frac{ \mathbf{R}^c_b (-{\delta\theta^w_{b_j} }^{\land}){\mathbf{R}^{b_j}_w }^{-1} \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c} \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right] }{\delta\theta^w_{b_j} }\\ &=-\underset{\delta\theta^w_{b_j}\rightarrow0 }{\lim} \frac{ \mathbf{R}^c_b ({\delta\theta^w_{b_j} })^{\land}\mathbf{R}^w_{b_j} \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c} \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right] }{\delta\theta^w_{b_j} }\\ &=\underset{\delta\theta^w_{b_j}\rightarrow0 }{\lim} \frac{ \mathbf{R}^c_b \left\{\mathbf{R}^w_{b_j} \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c} \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right]\right\}^{\land} ({\delta\theta^w_{b_j} }) }{\delta\theta^w_{b_j} }\\ &= \mathbf{R}^c_b \left\{\mathbf{R}^w_{b_j} \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c} \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right]\right\}^{\land} \end{align*}\tag{J1-2} $$


$$ \begin{align*} \mathbf{J}[2]^{3\times7}&= \left[ \frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{p}^b_c }, \frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{q}^b_c } \right]\\ &=\begin{bmatrix} \mathbf{R}^c_b(\mathbf{R}^{b_j}_w\mathbf{R}^w_{b_i}-I_{3\times3}) \\ -\mathbf{R}^c_b\mathbf{R}^{b_j}_w\mathbf{R}^w_{b_i}\ \mathbf{R}^{b}_{c}\left(\frac{\overline{\mathcal{P} }^{c_i}_l}{\lambda_l} \right)^{\land} +\left(\mathbf{R}^c_b\mathbf{R}^{b_j}_w\mathbf{R}^w_{b_i}\ \mathbf{R}^{b}_{c}\frac{\overline{\mathcal{P} }^{c_i}_l}{\lambda_l} \right)^{\land} +\left\{ \mathbf{R}^c_b \left[ \mathbf{R}^{b_j}_w \left( \mathbf{R}^w_{b_i}\mathbf{p}^{b}_{c}+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right)-\mathbf{p}^{b}_{c} \right] \right\}^{\land} \end{bmatrix}^T \end{align*}\tag{J2} $$


  • 推导 $\frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{p}^b_c }$:

$$ \begin{align*} \frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{p}^b_c} &=\frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{p}^b_c}\\ &=\frac{\partial \mathbf{R}^c_b \left\{\mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c} \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right]-\mathbf{p}^{b}_{c} \right\} }{\partial\mathbf{p}^b_c }\\ &=\frac{\partial\ \mathbf{R}^c_b \mathbf{R}^{b_j}_w \mathbf{R}^{w}_{b_i} \mathbf{p}^{b}_{c} -\mathbf{R}^c_b \mathbf{p}^{b}_{c} }{\partial\mathbf{p}^b_c }\\ &=\mathbf{R}^c_b(\mathbf{R}^{b_j}_w\mathbf{R}^w_{b_i}-I_{3\times3}) \end{align*}\tag{J2-1} $$

  • 推导 $\frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{q}^b_c }$:

$$ \begin{align*} \frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{q}^b_c } =&\frac{\partial \mathcal{P}^{c_j}_l }{\partial\mathbf{q}^b_c }\\ =&\frac{\partial \mathbf{R}^c_b \left\{\mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c} \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right]-\mathbf{p}^{b}_{c} \right\} }{\partial\mathbf{q}^b_c }\\ =&\underset{\delta\theta^b_c\rightarrow0 }{\lim} \frac{ \left[\mathbf{R}^c_b\exp({\delta\theta^b_c}^{\land})\right]^{-1} \left\{\mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c}\exp({\delta\theta^b_c}^{\land}) \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right]-\mathbf{p}^{b}_{c} \right\} }{\partial\mathbf{q}^b_c }\\ =&\underset{\delta\theta^b_c\rightarrow0 }{\lim} \frac{ (I-{\delta\theta^b_c}^{\land})\mathbf{R}^b_c \left\{\mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c}(I+{\delta\theta^b_c}^{\land}) \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right]-\mathbf{p}^{b}_{c} \right\} }{\partial\mathbf{q}^b_c }\\ =&\underset{\delta\theta^b_c\rightarrow0 }{\lim} \frac{\mathbf{R}^b_c \left\{\mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c}({\delta\theta^b_c}^{\land}) \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right]-\mathbf{p}^{b}_{c} \right\} }{\partial\mathbf{q}^b_c }\\ &-\underset{\delta\theta^b_c\rightarrow0 }{\lim} \frac{-{\delta\theta^b_c}^{\land}\mathbf{R}^b_c \left\{\mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c}(I+{\delta\theta^b_c}^{\land}) \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right]-\mathbf{p}^{b}_{c} \right\} }{\partial\mathbf{q}^b_c }\\ \approx&\underset{\delta\theta^b_c\rightarrow0 }{\lim} \frac{\mathbf{R}^b_c \left\{\mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c}({\delta\theta^b_c}^{\land}) \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right]-\mathbf{p}^{b}_{c} \right\} }{\partial\mathbf{q}^b_c }\\ &-\underset{\delta\theta^b_c\rightarrow0 }{\lim} \frac{-{\delta\theta^b_c}^{\land}\mathbf{R}^b_c \left\{\mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c} \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right]-\mathbf{p}^{b}_{c} \right\} }{\partial\mathbf{q}^b_c }\\ =&\underset{\delta\theta^b_c\rightarrow0 }{\lim} \frac{\mathbf{R}^b_c \mathbf{R}^{b_j}_w \mathbf{R}^{w}_{b_i} \mathbf{R}^{b}_{c}({\delta\theta^b_c}^{\land}) \frac{\overline{\mathcal{P} }^{c_i}_l}{\lambda_l} }{\partial\mathbf{q}^b_c } -\underset{\delta\theta^b_c\rightarrow0 }{\lim} \frac{ {\delta\theta^b_c}^{\land} \left\{\mathbf{R}^b_c \left\{\mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c} \frac{\overline{\mathcal{P} }^{c_i}_l}{\lambda_l} +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right]-\mathbf{p}^{b}_{c} \right\} \right\} }{\partial\mathbf{q}^b_c }\\ =&\underset{\delta\theta^b_c\rightarrow0 }{\lim} \frac{\mathbf{R}^b_c \mathbf{R}^{b_j}_w \mathbf{R}^{w}_{b_i} \mathbf{R}^{b}_{c}({\delta\theta^b_c}^{\land}) \frac{\overline{\mathcal{P} }^{c_i}_l}{\lambda_l} -{\delta\theta^b_c}^{\land} \left\{\mathbf{R}^b_c \left\{\mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c} \frac{\overline{\mathcal{P} }^{c_i}_l}{\lambda_l} +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right]-\mathbf{p}^{b}_{c} \right\} \right\} }{\partial\mathbf{q}^b_c }\\ =&-\mathbf{R}^b_c \mathbf{R}^{b_j}_w \mathbf{R}^{w}_{b_i} \mathbf{R}^{b}_{c} \left[\frac{\overline{\mathcal{P} }^{c_i}_l}{\lambda_l}\right]^{\land} +\left\{\mathbf{R}^b_c \left\{\mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c} \frac{\overline{\mathcal{P} }^{c_i}_l}{\lambda_l} +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right]-\mathbf{p}^{b}_{c} \right\} \right\}^{\land}\\ =&-\mathbf{R}^c_b\mathbf{R}^{b_j}_w\mathbf{R}^w_{b_i}\ \mathbf{R}^{b}_{c}\left(\frac{\overline{\mathcal{P} }^{c_i}_l}{\lambda_l} \right)^{\land} +\left(\mathbf{R}^c_b\mathbf{R}^{b_j}_w\mathbf{R}^w_{b_i}\ \mathbf{R}^{b}_{c}\frac{\overline{\mathcal{P} }^{c_i}_l}{\lambda_l} \right)^{\land} +\left\{ \mathbf{R}^c_b \left[ \mathbf{R}^{b_j}_w \left( \mathbf{R}^w_{b_i}\mathbf{p}^{b}_{c}+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right)-\mathbf{p}^{b}_{c} \right] \right\}^{\land} \end{align*}\tag{J2-2} $$


$$ \mathbf{J}[3]^{3\times1}= \frac{\partial \mathcal{P}^{c_j}_l }{\partial\lambda_l } =-\mathbf{R}^c_b\mathbf{R}^{b_j}_w\mathbf{R}^w_{b_i}\ \mathbf{R}^{b}_{c}\frac{\overline{\mathcal{P} }^{c_i}_l}{\lambda_l^2}\tag{J3} $$


  • 推导 $\frac{\partial \mathcal{P}^{c_j}_l }{\partial\lambda_l }$:

$$ \begin{align*} \frac{\partial \mathcal{P}^{c_j}_l }{\partial\lambda_l} &=\frac{\partial \mathcal{P}^{c_j}_l }{\partial\lambda_l}\\ &=\frac{\partial \mathbf{R}^c_b \left\{\mathbf{R}^{b_j}_w \left[\mathbf{R}^{w}_{b_i} \left(\mathbf{R}^{b}_{c} \frac{1}{\lambda_l} \overline{\mathcal{P} }^{c_i}_l +\mathbf{p}^{b}_{c} \right)+\mathbf{p}^{w}_{b_i}-\mathbf{p}^{w}_{b_j} \right]-\mathbf{p}^{b}_{c} \right\} }{\partial\lambda_l}\\ &=-\mathbf{R}^c_b\mathbf{R}^{b_j}_w\mathbf{R}^w_{b_i}\ \mathbf{R}^{b}_{c}\frac{\overline{\mathcal{P} }^{c_i}_l}{\lambda_l^2} \end{align*}\tag{J3-1} $$

🧐 本文作者:
😉 本文链接:https://lukeyalvin.site/archives/63.html
😊 版权说明:本博客所有文章除特别声明外,均默认采用 CC BY-NC-SA 4.0 许可协议。
最后修改:2022 年 07 月 27 日
赏杯咖啡