视觉残差函数及雅可比公式推导

最新推荐文章于 2024-02-22 11:03:08 发布

changshen_xu

最新推荐文章于 2024-02-22 11:03:08 发布

阅读量2.5k

点赞数 4

分类专栏： # ----------VIO原理系列文章标签： VIO

本文链接：https://blog.csdn.net/csdn_xcs/article/details/96288327

版权

----------VIO原理系列专栏收录该内容

14 篇文章 5 订阅

订阅专栏

【约定符号】：
特征点在相机坐标系下的坐标为 $x,y,z]^T$ ；
特征点在归一化相机坐标系下的坐标为 $[\mu,\nu,1]^T$ 或 $[\mu,\nu]^T$
特征点的这两种坐标之间的关系：
$\begin{bmatrix} x\\ y\\ z \end{bmatrix}= \frac{1}{\lambda} \begin{bmatrix} \mu\\ \nu\\ 1 \end{bmatrix}$
其中， $\lambda=1/z$ ，称为逆深度。

【定义概念】视觉重投影误差
假设预测的（估计的） 特征点的坐标为 $x,y,z]^T$ （相机坐标系），观测到的 特征点的坐标为 $[\mu,\nu]^T$ （归一化相机坐标系），则视觉重投影误差定义为：
$r_c=\begin{bmatrix} \frac{x}{z}-\mu\\ \frac{y}{z}-\nu \end{bmatrix}$
基于以上内容，开始推导。

已知第 $i$ 帧中某特征点的坐标 $[\mu_i,\nu_i]^T$ （归一化相机坐标系）及逆深度 $\lambda_i$ ，可以预测该特征点在第 $j$ 帧的相机坐标系下的坐标 $x_{c_j},y_{c_j},z_{c_j}]^T$ 为：
$\begin{bmatrix} x_{c_j}\\ y_{c_j}\\ z_{c_j}\\1 \end{bmatrix}= T^{-1}_{bc}T^{-1}_{wb_j} T_{wb_i}T_{bc} \begin{bmatrix} \frac{1}{\lambda_{c_i}}\mu\\ \frac{1}{\lambda_{c_i}}\nu\\ \frac{1}{\lambda_{c_i}} \\1 \end{bmatrix} \tag{1-1}$
【注】关于 $T_{wb_i}$ 和 $T_{wb_j}$ ，此时我们有一个粗略的值。
同时，该特征点在第 $j$ 帧确实被观测到了，坐标为 $[\mu_{c_j},\nu_{c_j}]^T$ ，则不难构建重投影误差（抄过来）如下：
$r_c=\begin{bmatrix} \frac{x_{c_j}}{z_{c_j}}-\mu_{c_j}\\ \frac{y_{c_j}}{z_{c_j}}-\nu_{c_j} \end{bmatrix}\triangleq \begin{bmatrix} r_{c1}\\ r_{c2} \end{bmatrix}$
这就是残差函数。
残差函数构成损失函数，在使用LM算法优化过程中，需要使用残差函数的Jacobian矩阵（一阶泰勒展开） $\frac{\partial r_c}{\partial state}=\frac{\partial r_c}{\partial f_{c_j}}\cdot \frac{\partial f_{c_j}}{\partial state}$ 。【具体详见LM算法】

求残差函数的Jacobian矩阵
首先，明确 $r_c$ 需要对哪些变量求偏导。
共四大部分：1. $i$ 时刻的位移和姿态，2. $j$ 时刻的位移和姿态，3. imu和相机的外参，4. 逆深度。

应用链式法则， $\frac{\partial r_c}{\partial state}=\frac{\partial r_c}{\partial f_{c_j}}\cdot \frac{\partial f_{c_j}}{\partial state}$

第一步，先求 $\frac{\partial r_c}{\partial f_{c_j}}$ 得：
$\begin{aligned} \frac{\partial r_c}{\partial f_{c_j}} &= \begin{bmatrix} \frac{\partial r_{c1}}{\partial x_{c_j}} & \frac{\partial r_{c1}}{\partial y_{c_j}} & \frac{\partial r_{c1}}{\partial z_{c_j}} \\ \frac{\partial r_{c2}}{\partial x_{c_j}} & \frac{\partial r_{c2}}{\partial y_{c_j}} & \frac{\partial r_{c2}}{\partial z_{c_j}} \end{bmatrix} \\ &= \begin{bmatrix} \frac{1}{z_{c_j}} & 0 & -\frac{x_{c_j}}{ z^2_{c_j}} \\ 0 & \frac{1}{z_{c_j}} & -\frac{y_{c_j}}{ z^2_{c_j}} \end{bmatrix} \\ \end{aligned}$

第二步：求 $\frac{\partial f_{c_j}}{\partial state}$ 。

在开始第二部分的求导之前，对 $f_{c_j}$ 做一些等价变形。
公式（1-1）的等价形式：公式（1-2） 将四维齐次形式改写，拆成三维形式，并做一些符号简化：
$\begin{aligned} f_{c_j} &\triangleq \begin{bmatrix} x_{c_j}\\ y_{c_j}\\ z_{c_j} \end{bmatrix} \\ & = R^{T}_{bc}R^{T}_{wb_j} R_{wb_i}R_{bc}\frac{1}{\lambda_{c_i}} \begin{bmatrix} \mu_{c_j}\\ \nu_{c_i}\\ 1 \end{bmatrix}\\ &+R^{T}_{bc}(R^{T}_{wb_j}(( R_{wb_i}p_{bc}+p_{wb_i})-p_{wb_j})-p_{bc}) \end{aligned} \tag{1-2}$

$\begin{aligned} f_{b_i} &\triangleq R_{bc}f_{c_i}+p_{bc}\\ f_{w} &\triangleq R_{wb_i}f_{b_i}+p_{wb_i}\\ f_{b_j} &\triangleq R^T_{wb_j}(f_{w}-p_{wb_j})\\ f_{c_j} &\triangleq R^T_{bc}(f_{b_j}-p_{bc}) \end{aligned}$
不难看出，上面四个式子依次给出了特征点在 $c_i,b_i,w,b_j,c_j$ 坐标系下的坐标。将四个式子依次从上到下代入，展开即可得到公式（1-2）的结果。

问： $p_{wc_j}$ 与 $f_{c_j}$ 含义相同吗？
答：不相同， $f_{c_j}$ 表示特征点在 $c_j$ 相机坐标系下的坐标；
$p_{wc_j}$ 表示相机 $c_j$ 在世界坐标系下的坐标！

已知公式（1-2）：
$\begin{aligned} f_{c_j} & = R^{T}_{bc}R^{T}_{wb_j} R_{wb_i}R_{bc} f_{c_i} \\ &+R^{T}_{bc}(R^{T}_{wb_j}(( R_{wb_i}p_{bc}+p_{wb_i})-p_{wb_j})-p_{bc}) \end{aligned}$
1.1 $i$ 时刻的位移：
即 $p_{wb_i}:=p_{wb_i}+\delta p_{b_ib'_i}$ ，不难写出：
$\frac{\partial f_{c_j}}{\partial \delta p_{b_ib'_i}}=R^{T}_{bc}R^{T}_{wb_j}$

1.2 $i$ 时刻的姿态：
即 $R_{wb_i}:=R_{wb_i}(I+[\delta \theta_{b_ib'_i}]_\times)$
$f_{c_j}$ 中与 $R_{wb_i}$ 有关的项有两部分，可合成简化为：
$\begin{aligned} f_{c_j} & = R^{T}_{bc}R^{T}_{wb_j} R_{wb_i}R_{bc} f_{c_i} +R^{T}_{bc}R^{T}_{wb_j} R_{wb_i}p_{bc}+(...)\\ &= R^{T}_{bc}R^{T}_{wb_j} R_{wb_i}f_{b_i} +(...) \end{aligned}$
则：
$\begin{aligned} \frac{\partial f_{c_j}}{\partial \delta \theta_{b_ib'_i}} &=\frac{R^{T}_{bc}R^{T}_{wb_j} R_{wb_i}(I+[\delta \theta_{b_ib'_i}]_\times)f_{b_i} }{\delta \theta_{b_ib'_i}} \\ &=-R^{T}_{bc}R^{T}_{wb_j} R_{wb_i}[f_{b_i}]_\times \end{aligned}$
【注】这里有一个写法上的简化。

2.1 $j$ 时刻的位移：
即 $p_{wb_j}:=p_{wb_j}+\delta p_{b_jb'_j}$ ，不难写出：
$\frac{\partial f_{c_j}}{\partial \delta p_{b_jb'_j}}=-R^{T}_{bc}R^{T}_{wb_j}$

2.2 $j$ 时刻的姿态：
即 $R_{wb_j}:=R_{wb_j}(I+[\delta \theta_{b_jb'_j}]_\times)$
$f_{c_j}$ 中与 $R_{wb_j}$ 有关的项有两部分，可合成简化为：
$\begin{aligned} f_{c_j} & = R^{T}_{bc}R^{T}_{wb_j} R_{wb_i}R_{bc} f_{c_i} \\ &+R^{T}_{bc}(R^{T}_{wb_j}(( R_{wb_i}p_{bc}+p_{wb_i})-p_{wb_j})-p_{bc}) \\ &=R^{T}_{bc}R^{T}_{wb_j}(R_{wb_i}(R_{bc} f_{c_i}+p_{bc})+p_{wb_i}-p_{wb_j})+(...) \\ &=R^{T}_{bc}R^{T}_{wb_j}(f_w-p_{wb_j})+(...) \end{aligned}$
则：
$\begin{aligned} \frac{\partial f_{c_j}}{\partial \delta \theta_{b_jb'_j}} &=\frac{ R^{T}_{bc}[R_{wb_j}(I+[\delta \theta_{b_jb'_j}]_\times)]^T(f_w-p_{wb_j}) }{\delta \theta_{b_jb'_j}} \\ &=\frac{ R^{T}_{bc}(I-[\delta \theta_{b_jb'_j}]_\times)R_{wb_j}^T(f_w-p_{wb_j}) }{\delta \theta_{b_jb'_j}} \\ &=\frac{ R^{T}_{bc}(I-[\delta \theta_{b_jb'_j}]_\times)f_{b_j} }{\delta \theta_{b_jb'_j}} \\ &=R^{T}_{bc}[f_{b_j}]_\times \end{aligned}$

3.1 imu和相机之间外参中的位移：
即 $p_{bc}:=p_{bc}+\delta p_{cc'}$ ，不难写出：
$\frac{\partial f_{c_j}}{\partial \delta p_{cc'} } =R^{T}_{bc} (R^{T}_{wb_j} R^{T}_{wb_j}-I_{3\times 3})$
3.2 imu和相机之间外参中的姿态：
即 $R_{bc}:=R_{bc}(I+[\delta \theta_{cc'}]_\times)$
$f_{c_j}$ 中与 $R_{cc'}$ 有关的项有两部分，且不容易简化，故分为两部分求解：
第一部分：
$f^{[1]}_{c_j} \triangleq R^{T}_{bc}R^{T}_{wb_j} R_{wb_i}R_{bc} f_{c_i}$
则：
$\begin{aligned} \frac{\partial f^{[1]}_{c_j}}{\partial \delta \theta_{cc'}} &=\frac{ (I-[\delta \theta_{cc'}]_\times)R^{T}_{bc}R^{T}_{wb_j} R_{wb_i}R_{bc}(I+[\delta \theta_{cc'}]_\times) f_{c_i} }{\delta \theta_{cc'}} \\ &\approx \frac{ -[\delta \theta_{cc'}]_\times R^{T}_{bc}R^{T}_{wb_j} R_{wb_i}R_{bc} f_{c_i} + R^{T}_{bc}R^{T}_{wb_j} R_{wb_i}R_{bc} [\delta \theta_{cc'}]_\times f_{c_i}}{\delta \theta_{cc'}} \\ &=[R^{T}_{bc}R^{T}_{wb_j} R_{wb_i}R_{bc} f_{c_i}]_{\times}-R^{T}_{bc}R^{T}_{wb_j} R_{wb_i}R_{bc} [f_{c_i}]_{\times} \end{aligned}$
第二部分：
$f^{[2]}_{c_j} = R^{T}_{bc}(R^{T}_{wb_j}(( R_{wb_i}p_{bc}+p_{wb_i})-p_{wb_j})-p_{bc})$
则：
$\begin{aligned} \frac{\partial f^{[2]}_{c_j}}{\partial \delta \theta_{cc'}} &=\frac{ (I-[\delta \theta_{cc'}]_\times)R^{T}_{bc}(R^{T}_{wb_j}(( R_{wb_i}p_{bc}+p_{wb_i})-p_{wb_j})-p_{bc})}{\delta \theta_{cc'}} \\ & = [R^{T}_{bc}(R^{T}_{wb_j}(( R_{wb_i}p_{bc}+p_{wb_i})-p_{wb_j})-p_{bc})]_{\times} \end{aligned}$
两部分相加，即 $\frac{\partial f_{c_j}}{\partial \delta \theta_{cc'}}=\frac{\partial f^{[1]}_{c_j}}{\partial \delta \theta_{cc'}}+\frac{\partial f^{[2]}_{c_j}}{\partial \delta \theta_{cc'}}$

4.逆深度：
即 $\lambda_{c_i}:=\lambda_{c_i}+\delta \lambda_{c_i}$ ， $f_{c_j}$ 中仅 $f_{c_i}$ 与 $\lambda_{c_i}$ 有关，链式法则 $\frac{\partial f_{c_j}}{\partial \delta \lambda_{c_i}}=\frac{\partial f_{c_j}}{\partial \delta f_{c_i}}\cdot \frac{\partial f_{c_i}}{\partial \delta \lambda_{c_i}}$ ：
其中，
$f_{c_j} = R^{T}_{bc}R^{T}_{wb_j} R_{wb_i}R_{bc} f_{c_i}$
则：
$\frac{\partial f_{c_j}}{\partial \delta f_{c_i}} =R^{T}_{bc}R^{T}_{wb_j} R_{wb_i}R_{bc}$
又有，
$f_{c_i}=\frac{1}{\lambda_{c_i}} \begin{bmatrix} \mu_{c_j}\\ \nu_{c_i}\\ 1 \end{bmatrix}\\$
则：
$\frac{\partial f_{c_i}}{\partial \delta \lambda_{c_i}} =-\frac{1}{\lambda^2_{c_i}} \begin{bmatrix} \mu_{c_j}\\ \nu_{c_i}\\ 1 \end{bmatrix}= -\frac{1}{\lambda_{c_i}} f_{c_i}$
至此，推导完成！

changshen_xu

关注

4
点赞
踩
10

收藏

觉得还不错? 一键收藏
1
评论
视觉残差函数及雅可比公式推导

约定：特征点在相机坐标系下的坐标为[x,y,z]T[x,y,z]^T[x,y,z]T；特征点在归一化相机坐标系下的坐标为[μ,ν,1]T[\mu,\nu,1]^T[μ,ν,1]T或[μ,ν]T[\mu,\nu]^T[μ,ν]T特征点的这两种坐标之间的关系：[xyz]=1λ[μν1]\begin{bmatrix} x\\ y\\ z \end{bmatrix}=\frac{1}{\lamb...
复制链接

扫一扫

专栏目录