Slam学习笔记——矩阵扰动求导

参考书籍：《视觉SLAM十四讲-从理论到实践》——高翔

1. 推导结论

1.1 旋转矩阵R

旋转矩阵的逆等于转置：
$R^T=R^{-1}$

旋转矩阵$R$的左扰动：$-(Rp{)}^{\wedge }$
旋转矩阵$R$的右扰动：$-R{p}^{\wedge }$
逆旋转矩阵$R^{-1}$的左扰动：$R^{-1}{p}^{\wedge }$
逆旋转矩阵$R^{-1}$的右扰动：$(R^{-1}p{)}^{\wedge }$

1.2 变换矩阵T

1.2.1 结构

变换矩阵T：
$$\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]$$

逆变换矩阵$T^{-1}$：
$$\left[\begin{array}{cc}{R}^T& -R^{T}t\\ 0^T& 1\end{array}\right]$$

1.2.2 左扰动

与《十四讲》不同！本文涉及的扰动是对矩阵求导，而不是对李代数求导！
因此，得到的答案会有所不同！

变换矩阵T的左扰动：
$$\left[\begin{array}{cc}I& -(Rp{)}^{\wedge }\\ 0^T& 0^T\end{array}\right]$$
逆变换矩阵$T^{-1}$的左扰动：
$$\left[\begin{array}{cc}-R^T& R^T{p}^{\wedge }-R^T{t}^{\wedge }\\ 0^T& 0^T\end{array}\right]$$

1.2.3 右扰动

变换矩阵T的右扰动：
$$\left[\begin{array}{cc}I& -R{p}^{\wedge }\\ 0^T& 0^T\end{array}\right]$$
逆变换矩阵$T^{-1}$的右扰动：
$$\left[\begin{array}{cc}-R^T& (R^{T}p{)}^{\wedge }-(R^{T}t{)}^{\wedge }\\ 0^T& 0^T\end{array}\right]$$

1.3 反对称矩阵a^

$a=[x,y,z{]}^T$
则反对称矩阵$a^{\wedge }$=
$$\left[\begin{array}{ccc}0& -z& y\\ z& 0& -x\\ -y& x& 0\end{array}\right]$$

1.4 内参矩阵K

使用内参矩阵K将相机平面上的2维坐标$(u,v)$与世界坐标系下的3维坐标$(X,Y,Z)$相互转换：
$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]=\frac{1}{Z}\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]$$
$Z$为深度，经常使用逆深度$\lambda$代替$Z$，即写为：
$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]=\lambda \left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]=\left[\begin{array}{c}\lambda X\\ \lambda Y\\ 1\end{array}\right]$$

$\lambda$的优点：
1、在表示较远处物体时，计算精度更好
2、假设2维点正确的情况下，只需要优化$\lambda$这1个参数即可以确定3维点坐标。

2. 推导过程

$T_b^a$表示将坐标从b坐标系转换到a坐标系。

2.1 推导中用到的公式

旋转矩阵的逆等于转置：$R^T=R^{-1}$

全微分公式：
当f(x) = f(a(x), b(x))时：
$$\frac{\partial f}{\partial x}=\frac{\partial f}{\partial a}\frac{\partial a}{\partial x}+\frac{\partial f}{\partial b}\frac{\partial b}{\partial x}$$

李代数的泰勒展开（近似）：
$exp(\Delta x)=(I+\Delta x^{\wedge })$
$exp(-\Delta x)=(I-\Delta x^{\wedge })$

交换反对称符号（交换位置且改变正负）：
$\Delta x^{\wedge }p=-p^{\wedge }\Delta x$
$\Delta x^{\wedge }Rp=-(Rp{)}^{\wedge }\Delta x$

向量叉乘交换律：
a X b = - b X a

基于反对称的定义可得：
$(a+b{)}^{\wedge }=a^{\wedge }+b^{\wedge }$

2.2 详细推导

2.2.1 旋转矩阵的右扰动

$$\frac{f(x+\Delta x)-f(x)}{\Delta x}=\frac{[exp(x^{\wedge })exp(\Delta x^{\wedge }){]}^{-1}p-exp(x{)}^{-1}p}{\Delta x}\text{\hspace{1em}(代入)}$$

$$\frac{f(x+\Delta x)-f(x)}{\Delta x}=\frac{exp(\Delta x^{\wedge }{)}^{-1}exp(x^{\wedge }{)}^{-1}p-exp(x^{\wedge }{)}^{-1}p}{\Delta x}\text{\hspace{1em}(分解后交换)}$$

$$\frac{f(x+\Delta x)-f(x)}{\Delta x}=\frac{(I-\Delta x^{\wedge })exp(x^{\wedge }{)}^{-1}p-exp(x^{\wedge }{)}^{-1}p}{\Delta x}\text{\hspace{1em}(泰勒展开)}$$

$$\frac{f(x+\Delta x)-f(x)}{\Delta x}=\frac{-\Delta x^{\wedge }\ast exp(x^{\wedge }{)}^{-1}p}{\Delta x}\text{\hspace{1em}(相减)}$$

$$\frac{f(x+\Delta x)-f(x)}{\Delta x}=\frac{(exp(x{)}^{-1}p{)}^{\wedge }\Delta x}{\Delta x}\text{\hspace{1em}(反对称交换)}$$

$$\frac{f(x+\Delta x)-f(x)}{\Delta x}=(exp(x{)}^{-1}p{)}^{\wedge }=(R^{-1}p{)}^{\wedge }\text{\hspace{1em}(消去Δx)}$$

2.2.2 变换矩阵的右扰动

已知：
旋转矩阵$R$的右扰动：$-R{p}^{\wedge }$
逆旋转矩阵$R^{-1}$的右扰动：$(R^{-1}p{)}^{\wedge }$

对于变换矩阵T：
Tp = Rp + t
因此：
1、t的偏导为I
2、R的偏导为Rp的右扰动，即$-R{p}^{\wedge }$
得到变换矩阵T的右扰动：
$$\left[\begin{array}{cc}I& -R{p}^{\wedge }\\ 0^T& 0^T\end{array}\right]$$

对于逆变换矩阵$T^{-1}$：
$Tp=R^{-1}(p-t)=R^{-1}p-R^{-1}t$
因此：
1、t的偏导为$-R^{-1}$
2、R的偏导为 $R^{-1}p$的右扰动减$R^{-1}t$的右扰动，即 $(R^{T}p{)}^{\wedge }-(R^{T}t{)}^{\wedge }$
得到逆变换矩阵$T^{-1}$的右扰动：
$$\left[\begin{array}{cc}-R^T& (R^{T}p{)}^{\wedge }-(R^{T}t{)}^{\wedge }\\ 0^T& 0^T\end{array}\right]$$

2.2.3 Vins-Mono中重投影误差的雅可比矩阵求导

源代码位置：/VINS-Mono/vins_estimator/src/factor/projection_factor.cpp
函数：ProjectionFactor::Evaluate

原理：第i帧的图像通过位姿变换后得到第j帧对应点在相机坐标系下的坐标，然后与实际观测到的坐标相减，通过最小化重投影误差来优化位姿的参数。

使用右扰动的方式计算变换矩阵的偏导。

误差计算公式：
$e=T_c^{imu-1}{T}_{imuj}^{w-1}{T}_{imui}^w{T}_c^{imu}{P}_{ci}-P_j$

1、对$T_{imui}^w$求偏导
$$\frac{\partial e}{\partial P_w}\frac{\partial P_w}{\partial T_{imui}^w}$$

$$=(T_c^{imu-1}{T}_{imuj}^{w-1})\left[\begin{array}{cc}I& -R_{imui}^w{P}_{imui}\hat{}\\ 0^T& 0^T\end{array}\right]$$

$$=(\left[\begin{array}{cc}{R}_c^{imu-1}& -R_c^{imu-1}{t}_c^{imu}\\ 0^T& 1\end{array}\right]\left[\begin{array}{cc}{R}_{imuj}^{w-1}& -R_{imuj}^{w-1}{t}_{imuj}^w\\ 0^T& 1\end{array}\right])\left[\begin{array}{cc}I& -R_{imui}^w{P}_{imui}^{\wedge }\\ 0^T& 0^T\end{array}\right]$$

$$=\left[\begin{array}{cc}{R}_c^{imu-1}{R}_{imuj}^{w-1}& R_c^{imu-1}(-R_{imuj}^{w-1}{t}_{imuj}^w)-R_c^{imu-1}{t}_c^{imu}\\ 0^T& 1\end{array}\right]\left[\begin{array}{cc}I& -R_{imui}^w{P}_{imui}^{\wedge }\\ 0^T& 0^T\end{array}\right]$$

$$=\left[\begin{array}{cc}{R}_c^{imu-1}{R}_{imuj}^{w-1}& R_c^{imu-1}{R}_{imuj}^{w-1}(-R_{imui}^w{P}_{imui}^{\wedge })\\ 0^T& 0^T\end{array}\right]$$

$$=\left[\begin{array}{cc}{R}_c^{imu-1}{R}_{imuj}^{w-1}& R_c^{imu-1}{R}_{imuj}^{w-1}{R}_{imui}^w(-P_{imui}^{\wedge })\\ 0^T& 0^T\end{array}\right]$$

2、对$T_{imuj}^w$求偏导

$$\frac{\partial e}{\partial P_{imuj}}\frac{\partial P_{imuj}}{\partial T_{imuj}^w}$$

$$=T_c^{imu-1}\left[\begin{array}{cc}-R_{imuj}^{w-1}& (R_{imuj}^{w-1}{P}_w{)}^{\wedge }-(R_{imuj}^{w-1}{t}_{imuj}^w{)}^{\wedge }\\ 0^T& 0^T\end{array}\right]$$

$$=\left[\begin{array}{cc}{R}_c^{imu-1}& -R_c^{imu-1}{t}_c^{imu}\\ 0^T& 1\end{array}\right]\left[\begin{array}{cc}-R_{imuj}^{w-1}& (R_{imuj}^{w-1}(P_w-t_{imuj}^w){)}^{\wedge }\\ 0^T& 0^T\end{array}\right]$$

$$=\left[\begin{array}{cc}{R}_c^{imu-1}& -R_c^{imu-1}{t}_c^{imu}\\ 0^T& 1\end{array}\right]\left[\begin{array}{cc}-R_{imuj}^{w-1}& P_{imuj}^{\wedge }\\ 0^T& 0^T\end{array}\right]$$

$$=\left[\begin{array}{cc}{R}_c^{imu-1}(-R_{imuj}^{w-1})& R_c^{imu-1}{P}_{imuj}^{\wedge }\\ 0^T& 0^T\end{array}\right]$$

3、对$T_c^{imu}$求偏导

已知：
$e=T_c^{imu-1}{T}_{imuj}^{w-1}{T}_{imui}^w{T}_c^{imu}{P}_{ci}-P_j$

$P_{cj}=T_c^{imu-1}{T}_{imuj}^{w-1}{T}_{imui}^w{T}_c^{imu}{P}_{ci}$

$P_{imuj}=T_{imuj}^{w-1}{T}_{imui}^w{T}_c^{imu}{P}_{ci}$

$P_{imui}=T_c^{imu}{P}_{ci}$

由全微分公式可得：
$$\frac{\partial e}{\partial T_c^{imu}}=\frac{\partial P_{cj}}{\partial T_c^{imu}}=\frac{\partial P_{cj}}{\partial T_c^{imu-1}}\frac{\partial T_c^{imu-1}}{\partial T_c^{imu}}+\frac{\partial P_{cj}}{\partial P_{imuj}}\frac{\partial P_{imuj}}{\partial T_c^{imu}}$$

$$=(\frac{\partial P_{cj}}{\partial T_c^{imu-1}}\frac{\partial T_c^{imu-1}}{\partial T_c^{imu}})+\frac{\partial P_{cj}}{\partial P_{imuj}}\frac{\partial P_{imuj}}{\partial P_{imui}}(\frac{\partial P_{imui}}{\partial T_c^{imu}})$$

(对括号内的部分进行右扰动)
$$=\left[\begin{array}{cc}-R_c^{imu-1}& (R_c^{imu-1}{P}_{imuj}{)}^{\wedge }-(R_c^{imu-1}{t}_c^{imu}{)}^{\wedge }\\ 0^T& 0^T\end{array}\right]+T_c^{imu-1}(T_{imuj}^{w-1}{T}_{imui}^w)\left[\begin{array}{cc}I& -R_c^{imu}{P}_{ci}^{\wedge }\\ 0^T& 0^T\end{array}\right]$$

$$=\left[\begin{array}{cc}-R_c^{imu-1}& P_{cj}^{\wedge }\\ 0^T& 0^T\end{array}\right]+\left[\begin{array}{cc}{R}_c^{imu-1}& -R_c^{imu-1}{t}_c^{imu}\\ 0^T& 1\end{array}\right](\left[\begin{array}{cc}{R}_{imuj}^{w-1}& -R_{imuj}^{w-1}{t}_{imuj}^w\\ 0^T& 1\end{array}\right]\left[\begin{array}{cc}{R}_{imui}^w& t_{imui}^w\\ 0^T& 1\end{array}\right])\left[\begin{array}{cc}I& -R_c^{imu}{P}_{ci}^{\wedge }\\ 0^T& 0^T\end{array}\right]$$

$$=\left[\begin{array}{cc}-R_c^{imu-1}& P_{cj}^{\wedge }\\ 0^T& 0^T\end{array}\right]+\left[\begin{array}{cc}{R}_c^{imu-1}& -R_c^{imu-1}{t}_c^{imu}\\ 0^T& 1\end{array}\right]\left[\begin{array}{cc}{R}_{imuj}^{w-1}{R}_{imui}^w& R_{imuj}^{w-1}{t}_{imui}^w-R_{imuj}^{w-1}{t}_{imuj}^w\\ 0^T& 1\end{array}\right]\left[\begin{array}{cc}I& -R_c^{imu}{P}_{ci}^{\wedge }\\ 0^T& 0^T\end{array}\right]$$

$$=\left[\begin{array}{cc}-R_c^{imu-1}& P_{cj}^{\wedge }\\ 0^T& 0^T\end{array}\right]+\left[\begin{array}{cc}{R}_c^{imu-1}& -R_c^{imu-1}{t}_c^{imu}\\ 0^T& 1\end{array}\right]\left[\begin{array}{cc}{R}_{imuj}^{w-1}{R}_{imui}^w& R_{imuj}^{w-1}{R}_{imui}^w(-R_c^{imu}{P}_{ci}^{\wedge })\\ 0^T& 0^T\end{array}\right]$$

$$=\left[\begin{array}{cc}-R_c^{imu-1}& P_{cj}^{\wedge }\\ 0^T& 0^T\end{array}\right]+\left[\begin{array}{cc}{R}_c^{imu-1}{R}_{imuj}^{w-1}{R}_{imui}^w& R_c^{imu-1}{R}_{imuj}^{w-1}{R}_{imui}^w(-R_c^{imu}{P}_{ci}^{\wedge })\\ 0^T& 0^T\end{array}\right]$$
$$=\left[\begin{array}{cc}{R}_c^{imu-1}{R}_{imuj}^{w-1}{R}_{imui}^w-R_c^{imu-1}& P_{cj}^{\wedge }-R_c^{imu-1}{R}_{imuj}^{w-1}{R}_{imui}^w{R}_c^{imu}{P}_{ci}^{\wedge }\\ 0^T& 0^T\end{array}\right]$$

因为：
$$P_{cj}^{\wedge }=(R_c^{imu-1}(R_{imuj}^{w-1}(R_{imui}^w(R_c^{imu}{P}_{ci}+t_c^{imu})+t_{imui}^w-t_{imuj}^w)-t_c^{imu-1}){)}^{\wedge }$$

$$=(R_c^{imu-1}(R_{imuj}^{w-1}(R_{imui}^w{R}_c^{imu}{P}_{ci}+R_{imui}^w{t}_c^{imu}+t_{imui}^w-t_{imuj}^w)-t_c^{imu-1}){)}^{\wedge }$$

$$=(R_c^{imu-1}(R_{imuj}^{w-1}{R}_{imui}^w{R}_c^{imu}{P}_{ci}+R_{imuj}^{w-1}{R}_{imui}^w{t}_c^{imu}+R_{imuj}^{w-1}{t}_{imui}^w-R_{imuj}^{w-1}{t}_{imuj}^w)-t_c^{imu-1}{)}^{\wedge }$$

$$=(R_c^{imu-1}{R}_{imuj}^{w-1}{R}_{imui}^w{R}_c^{imu}{P}_{ci}+R_c^{imu-1}{R}_{imuj}^{w-1}{R}_{imui}^w{t}_c^{imu}+R_c^{imu-1}{R}_{imuj}^{w-1}{t}_{imui}^w-R_c^{imu-1}{R}_{imuj}^{w-1}{t}_{imuj}^w-R_c^{imu-1}{t}_c^{imu-1}{)}^{\wedge }$$

$$=(R_c^{imu-1}{R}_{imuj}^{w-1}{R}_{imui}^w{R}_c^{imu}{P}_{ci}{)}^{\wedge }+(R_c^{imu-1}{R}_{imuj}^{w-1}{R}_{imui}^w{t}_c^{imu}+R_c^{imu-1}{R}_{imuj}^{w-1}{t}_{imui}^w-R_c^{imu-1}{R}_{imuj}^{w-1}{t}_{imuj}^w-R_c^{imu-1}{t}_c^{imu-1}{)}^{\wedge }$$

$$=(R_c^{imu-1}{R}_{imuj}^{w-1}{R}_{imui}^w{R}_c^{imu}{P}_{ci}{)}^{\wedge }+(R_c^{imu-1}(R_{imuj}^{w-1}(R_{imui}^w{t}_c^{imu}+t_{imui}^w-t_{imuj}^w)-t_c^{imu-1}){)}^{\wedge }$$

因此，答案为：
$$\left[\begin{array}{cc}{R}_c^{imu-1}{R}_{imuj}^{w-1}{R}_{imui}^w-R_c^{imu-1}& (R_c^{imu-1}{R}_{imuj}^{w-1}{R}_{imui}^w{R}_c^{imu}{P}_{ci}{)}^{\wedge }+(R_c^{imu-1}(R_{imuj}^{w-1}(R_{imui}^w{t}_c^{imu}+t_{imui}^w-t_{imuj}^w)-t_c^{imu-1}){)}^{\wedge }-R_c^{imu-1}{R}_{imuj}^{w-1}{R}_{imui}^w{R}_c^{imu}{P}_{ci}^{\wedge }\\ 0^T& 0^T\end{array}\right]$$
$$=\left[\begin{array}{cc}{R}_c^{imu-1}{R}_{imuj}^{w-1}{R}_{imui}^w-R_c^{imu-1}& -R_c^{imu-1}{R}_{imuj}^{w-1}{R}_{imui}^w{R}_c^{imu}{P}_{ci}^{\wedge }+(R_c^{imu-1}{R}_{imuj}^{w-1}{R}_{imui}^w{R}_c^{imu}{P}_{ci}{)}^{\wedge }+(R_c^{imu-1}(R_{imuj}^{w-1}(R_{imui}^w{t}_c^{imu}+t_{imui}^w-t_{imuj}^w)-t_c^{imu-1}){)}^{\wedge }\\ 0^T& 0^T\end{array}\right]$$

4、对逆深度${\lambda }_i$求偏导

误差计算公式：
$e=T_c^{imu-1}{T}_{imuj}^{w-1}{T}_{imui}^w{T}_c^{imu}{P}_{ci}-P_j$
即：
$$e=T_c^{imu-1}{T}_{imuj}^{w-1}{T}_{imui}^w{T}_c^{imu}\frac{1}{{\lambda }_i}(ui,vi)-P_j$$

$$=T_c^{imu-1}{T}_{imuj}^{w-1}{T}_{imui}^w(R_c^{imu}(-\frac{1}{{\lambda }^2}(ui,vi))+t_c^{imu})$$

$$=T_c^{imu-1}{T}_{imuj}^{w-1}(R_{imui}^w(R_c^{imu}(-\frac{1}{{\lambda }^2}(ui,vi))+t_c^{imu})+t_{imui}^w)$$

$$=T_c^{imu-1}{R}_{imuj}^{w-1}(R_{imui}^w(R_c^{imu}(-\frac{1}{{\lambda }^2}(ui,vi))+t_c^{imu})+t_{imui}^w-t_{imuj}^w)$$

$$=R_c^{imu-1}(R_{imuj}^{w-1}(R_{imui}^w(R_c^{imu}(-\frac{1}{{\lambda }^2}(ui,vi))+t_c^{imu})+t_{imui}^w-t_{imuj}^w)-t_c^{imut-1})$$

可得：
$$\frac{\partial e}{\partial P_{ci}}\frac{\partial P_{ci}}{\partial {\lambda }_i}$$

$$=R_c^{imu-1}{R}_{imuj}^{w-1}{R}_{imui}^w{R}_c^{imu}(-\frac{1}{{\lambda }^2}(ui,vi))$$

2.2.4 Vins-Mono中回环检测的雅可比矩阵求导

源代码位置：/VINS-Mono/pose_graph/src/pose_graph.h
函数：struct FourDOFError

原理：计算回环两侧在世界坐标系下的平移向量与偏航角$\psi$(yaw)，然后与实际观测到的从i到j的坐标差相减，优化位姿的参数。

输入参数$(t_i^w,{\psi }_i,t_j^w,{\psi }_j)$，即i帧与j帧对应的在世界坐标系下的平移向量与偏航角$\psi$(yaw)

俯仰角$\theta$(pitch)与滚转角$\Phi$(roll)为固定观测值不进行优化。

残差计算公式为：
$$\left[\begin{array}{c}{R}_i^{w-1}(t_j^w-t_i^w)-t_{ij}^i\\ {\psi }_j-{\psi }_i-{\psi }_{ij}\end{array}\right]=\left[\begin{array}{c}{R}_i^{w-1}{t}_j^w-R_i^{w-1}{t}_i^w-t_{ij}^i\\ {\psi }_j-{\psi }_i-{\psi }_{ij}\end{array}\right]$$
$t_{ij}^i$为$t_j^i-t_i^i$
${\psi }_{ij}$为${\psi }_j-{\psi }_i$
这两个值和左侧的数据来源不太一样，由外部计算完成后直接导入。

对$t_i^w$求偏导，雅可比矩阵为：
$$\left[\begin{array}{c}-(R_i^{w-1}{t}_i^w{)}^{\wedge }\\ 0\end{array}\right]$$

对${\psi }_i$求偏导，雅可比矩阵为：
$$\left[\begin{array}{c}0\\ -1\end{array}\right]$$

对$t_j^w$求偏导，雅可比矩阵为：
$$\left[\begin{array}{c}(R_i^{w-1}{t}_j^w{)}^{\wedge }\\ 0\end{array}\right]$$
对${\psi }_j$求偏导，雅可比矩阵为：
$$\left[\begin{array}{c}0\\ 1\end{array}\right]$$