SLAM中的Bundle Adjustment

Bundle Adjustment

反对称符号的定义

  两个向量的內积，內积可以描述向量间的投影关系：
$$a\cdot b=a^{T}b=\sum _{i=1}^3{a}_i{b}_i=\left|a\right|\left|b\right|\cos ⟨a,b⟩a,b\in {\mathbb{R}}^3$$
  两个向量的外积，外积的方向垂直于这两个向量，大小为$\left|a\right|\left|b\right|\sin ⟨a,b⟩$，是两个向量张成的四边形的有向面积。外积只对三维向量存在定义，还能用外积表示向量的旋转。
$$a\times b=\left[\begin{array}{ccc}i& j& k\\ a_1& a_2& a_3\\ b_1& b_2& b_3\end{array}\right]=\left[\begin{array}{c}{a}_2{b}_3-a_3{b}_2\\ a_3{b}_1-a_1{b}_3\\ a_1{b}_2-a_2{b}_1\end{array}\right]=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]b\triangleq a^{\wedge }b$$
   可以将${}^{\wedge }$符号记成反对称符号。这样就把外积写成了矩阵与向量的乘法，把外积运算变成了线性运算。还可以用外积表示向量的旋转。同理，对于任意反对称矩阵，亦能找到一个与之对应的向量。把这个运算用符号${}^{\vee }$表示。
$$\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]=A$$
$$A^{\vee }=a$$
   考虑两个不平行的向量$a,b$，要描述从$a$到$b$之间是如何旋转的？可以用一个向量描述三维空间中两个向量的旋转关系。在右手法则下，右手的4个手指从$a$转向$b$，大拇指朝向就是旋转向量的方向，也是$a\times b$的方向，大小则由$a$和$b$的夹角决定。通过这种方式，构造了从$a$到$b$的一个旋转向量，这个向量同样位于三维空间中，在此坐标系下可以用3个实数来描述。

李代数 $\mathfrak{s}\mathfrak{o}\left(3\right)$

  任意旋转矩阵$R$满足（旋转矩阵为正交矩阵，$R^T=R^{-1}$）：
$$R{R}^T=I$$
  $R$是某个相机的旋转，会随时间连续地变化，即为时间的函数：
$$R\left(t\right){R\left(t\right)}^T=I$$
  两边同时对时间求导：
$$\dot{R}\left(t\right){R\left(t\right)}^T+R\left(t\right){\dot{R}\left(t\right)}^T=0$$
  整理得：
$$\dot{R}\left(t\right){R\left(t\right)}^T=-{\left(\dot{R}\left(t\right){R\left(t\right)}^T\right)}^T$$
  根据上式可以看出$\dot{R}\left(t\right){R\left(t\right)}^T$是一个反对称矩阵。因此可以找到一个三维向量$\varphi \left(t\right)\in {\mathbb{R}}^3$与之对应。于是有：
$$\dot{R}\left(t\right){R\left(t\right)}^T=\varphi {\left(t\right)}^{\wedge }$$
  等式两边右乘$R\left(t\right)$，由于$R$为正交阵，有：
$$\dot{R}\left(t\right)=\varphi {\left(t\right)}^{\wedge }R\left(t\right)=\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]R\left(t\right)$$
  根据上式可以看出，每对旋转矩阵求一次导数，只需要左乘一个${\varphi }^{\wedge }\left(t\right)$矩阵即可。设$t_0=0$，同时设此时的旋转矩阵为$R\left(0\right)=I$，按照导数的定义，可以把$R\left(t\right)$在$0$附近进行一阶泰勒展开：
$$R\left(t\right)\approx R\left(t_0\right)+\dot{R}\left(t_0\right)\left(t-t_0\right)=I+\varphi {\left(t_0\right)}^{\wedge }\left(t\right)$$
  根据上式可以看到$\varphi$反映了$R$的导数性质，同时在$t_0$附近，设$\varphi$保持为常数$\varphi \left(t_0\right)={\varphi }_0$有：
$$\dot{R}\left(t\right)=\varphi {\left(t_0\right)}^{\wedge }R\left(t\right)={\varphi }_0^{\wedge }R\left(t\right)$$
  上式是一个关于$R$的微分方程，而且知道初始值$R\left(t_0\right)=I$，解之得：
$$\frac{dR\left(t\right)}{dt}={\varphi }_0^{\wedge }R\left(t\right)$$
$$\frac{dR\left(t\right)}{R\left(t\right)}={\varphi }_0^{\wedge }dt$$
  两边积分：
$$ln\left(R\left(t\right)\right)={\varphi }_0^{\wedge }t$$
$$R\left(t\right)=exp\left({\varphi }_0^{\wedge }t\right)$$
  之前提到的$\varphi$，是$SO\left(3\right)$对应的李代数$\mathfrak{s}\mathfrak{o}\left(3\right)$，$\mathfrak{s}\mathfrak{o}\left(3\right)$定义在${\mathbb{R}}^3$上的向量，记作$\varphi$，每一个$\varphi$都可以生成一个反对称矩阵：
$$\Phi ={\varphi }^{\wedge }=\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]\in {\mathbb{R}}^{3\times 3}$$
  由于$\varphi$与反对称矩阵关系很紧密，在不引起歧义的情况系，就说$\mathfrak{s}\mathfrak{o}\left(3\right)$的元素是三维向量或者三维反对称矩阵：
$$\mathfrak{s}\mathfrak{o}\left(3\right)=\left\{\varphi \in {\mathbb{R}}^3,\Phi ={\varphi }^{\wedge }\in {\mathbb{R}}^{3\times 3}\right\}$$

$SO\left(3\right)$上的指数映射

  由于$\varphi$是三维向量，可以定义它的模长和它的方向，分别记作$\theta$和$a$，于是有$\varphi =\theta a$，这里的$a$是一个长度为$1$的向量，首先对于$a^{\wedge }$，有以下两条性质：
$$a^{\wedge }{a}^{\wedge }=a{a}^T-I$$
$$a^{\wedge }{a}^{\wedge }{a}^{\wedge }=-a^{\wedge }$$
  任意矩阵的指数映射可以写成一个泰勒展开，只有在收敛的情况下才会有结果，其结果仍是一个矩阵。
$$exp\left(A\right)=\sum _{n=0}^{\infty }\frac{1}{n!}{A}^n$$
  同样的，对$\mathfrak{s}\mathfrak{o}\left(3\right)$中的任意元素$\varphi$，也可以用此方式定义它的指数映射：
$$exp\left({\varphi }^{\wedge }\right)=\sum _{n=0}^{\infty }\frac{1}{n!}{\left({\varphi }^{\wedge }\right)}^n$$
  可以把指数映射写成：
$$exp\left({\varphi }^{\wedge }\right)=exp\left(\theta a^{\wedge }\right)=\sum _{n=0}^{\infty }\frac{1}{n!}{\left(\theta a^{\wedge }\right)}^n$$
$$=I+\theta a^{\wedge }+\frac{1}{2!}{\theta }^2{a}^{\wedge }{a}^{\wedge }+\frac{1}{3!}{\theta }^3{a}^{\wedge }{a}^{\wedge }{a}^{\wedge }+\frac{1}{4!}{\theta }^4{\left(a^{\wedge }\right)}^4+\cdots$$
$$=a{a}^T-a^{\wedge }{a}^{\wedge }+\theta a^{\wedge }+\frac{1}{2!}{\theta }^2{a}^{\wedge }{a}^{\wedge }-\frac{1}{3!}{\theta }^3{a}^{\wedge }-\frac{1}{4!}{\theta }^4{\left(a^{\wedge }\right)}^2+\cdots$$
$$=a{a}^T+\left(\theta -\frac{1}{3!}{\theta }^3+\frac{1}{5!}{\theta }^5-\cdots \right)a^{\wedge }-\left(1-\frac{1}{2!}{\theta }^2+\frac{1}{4!}{\theta }^4-\cdots \right)a^{\wedge }{a}^{\wedge }$$
$$=a^{\wedge }{a}^{\wedge }+I+\sin \theta a^{\wedge }-\cos \theta a^{\wedge }{a}^{\wedge }$$
$$=\left(1-\cos \theta \right)a^{\wedge }{a}^{\wedge }+I+\sin \theta a^{\wedge }$$
$$=\cos \theta I+\left(1-\cos \theta \right)a{a}^T+\sin \theta a^{\wedge }$$
  罗德里格斯公式：$R=\cos \theta I+\left(1-\cos \theta \right)n{n}^T+\sin \theta n^{\wedge }$

  表明 $\mathfrak{s}\mathfrak{o}\left(3\right)$ 实际上就是由旋转向量组成的空间，指数映射即罗德里格斯公式。通过它们我们把$\mathfrak{s}\mathfrak{o}\left(3\right)$中任意一个向量对应到一个位于$SO\left(3\right)$中的旋转矩阵。反之，如果定义对数映射，也能把$SO\left(3\right)$中的元素对应到$\mathfrak{s}\mathfrak{o}\left(3\right)$中：
$$\varphi =\ln {\left(R\right)}^{\vee }={\left(\sum _{n=0}^{\infty }\frac{{\left(-1\right)}^n}{n+1}{\left(R-I\right)}^{n+1}\right)}^{\vee }$$

  由以上可得，它与$SO\left(3\right)$（特殊正交群—旋转矩阵群）的关系由指数映射给定：
$$R=exp\left({\varphi }^{\wedge }\right)$$

李代数$\mathfrak{s}\mathfrak{e}\left(3\right)$

  与$\mathfrak{s}\mathfrak{o}\left(3\right)$相似，$\mathfrak{s}\mathfrak{e}\left(3\right)$位于${\mathbb{R}}^6$空间中：
$$\mathfrak{s}\mathfrak{e}\left(3\right)=\left\{\xi =\left[\begin{array}{c}\rho \\ \varphi \end{array}\right]\in {\mathbb{R}}^6,\rho \in {\mathbb{R}}^3,\varphi \in \mathfrak{s}\mathfrak{o}\left(3\right),{\xi }^{\wedge }=\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}\right\}$$
  在$\mathfrak{s}\mathfrak{e}\left(3\right)$中扩展了${}^{\wedge }$符号的含义，将一个6维向量转换成四维矩阵，但这里不再表示反对称：
$${\xi }^{\wedge }=\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}$$

$SE\left(3\right)$上的指数映射

$$\exp \left({\xi }^{\wedge }\right)=\left[\begin{array}{cc}{\sum }_{n=0}^{\infty }\frac{1}{n!}{\left({\varphi }^{\wedge }\right)}^n& {\sum }_{n=0}^{\infty }\frac{1}{\left(n+1\right)!}{\left({\varphi }^{\wedge }\right)}^n\rho \\ 0^T& 1\end{array}\right]\triangleq \left[\begin{array}{cc}R& J\rho \\ 0^T& 1\end{array}\right]=T$$
  右上角的$J$可以整理为（设$\varphi =\theta a$）
$$J=\frac{\sin \theta }{\theta }I+\left(1-\frac{\sin \theta }{\theta }\right)a{a}^T+\frac{1-\cos \theta }{\theta }{a}^{\wedge }$$

$SE\left(3\right)$上的李代数求导

  假设某空间点$p^W$经过一次变换$T$（对应李代数为$\xi$），得到$T{P}^W$，现在给$T$左乘一个扰动$\Delta T=\exp \left(\delta {\xi }^{\wedge }\right)$，设扰动项的李代数为$\delta \xi ={\left[\delta \rho ,\delta \varphi \right]}^T$，有：
$$\frac{\partial \left(T{P}^W\right)}{\partial \delta \xi }=\underset{\partial \xi \to 0}{\lim }\frac{\exp \left(\delta {\xi }^{\wedge }\right)\exp \left({\xi }^{\wedge }\right)P^W-\exp \left({\xi }^{\wedge }\right)P^W}{\delta \xi }$$
$$\approx \underset{\partial \xi \to 0}{\lim }\frac{\left(I+\delta {\xi }^{\wedge }\right)\exp \left({\xi }^{\wedge }\right)P^W-\exp \left({\xi }^{\wedge }\right)P^W}{\delta \xi }$$
$$=\underset{\partial \xi \to 0}{\lim }\frac{\delta {\xi }^{\wedge }\exp \left({\xi }^{\wedge }\right)P^W}{\delta \xi }$$
$$=\underset{\partial \xi \to 0}{\lim }\frac{\left[\begin{array}{cc}\delta {\varphi }^{\wedge }& \delta \rho \\ 0^T& 0\end{array}\right]\left[\begin{array}{c}R{P}^W+t\\ 1\end{array}\right]}{\delta \xi }$$
$$=\underset{\partial \xi \to 0}{\lim }\frac{\left[\begin{array}{c}\delta {\varphi }^{\wedge }\left(R{P}^W+t\right)+\delta \rho \\ 0\end{array}\right]}{\delta \xi }=\left[\begin{array}{cc}I& -{\left(R{P}^W+t\right)}^{\wedge }\\ 0^T& 0^T\end{array}\right]\triangleq \left(T{P}^W\right)$$
  观察上式其中$P^C=R{P}^W+t$

BA

  有$n$个三维空间点$P^W$及其投影 $p$（像素坐标），和对应通过匹配得到像素坐标$Pixel=\left[U,V\right]$, 希望计算相机的位姿$R,t$，它的李代数表示为$\xi$。
  相机内参：
$$K=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]$$
  由以上信息可得，世界坐标系下三维点转到相机坐标系下$P^C$：
$$\left[\begin{array}{c}{P}^C\\ 1\end{array}\right]=exp\left({\xi }^{\wedge }\right)\left[\begin{array}{c}{P}^W\\ 1\end{array}\right]$$
  相机坐标系$P^C$到相机坐标系归一化平面$P_{normal}^C$：
$$P_{normal}^C=\left[\begin{array}{c}\frac{P_x^C}{P_z^C}\\ \frac{P_y^C}{P_z^C}\\ 1\end{array}\right]$$
  相机坐标系归一化平面到图像$p$
$$\left[\begin{array}{c}p\\ 1\end{array}\right]=K{P}_{normal}^C=\left[\begin{array}{c}{f}_x{P_{normal}^C}_x+c_x\\ f_y{P_{normal}^C}_y+c_y\\ 1\end{array}\right]$$
  由于相机位姿未知和观测点的噪声，$Pixel-p$存在一个误差。为了使的误差最小化，我们把每个点的误差求和，构建最小二乘问题（列文伯格—马夸尔特方法），然后调整相机位姿和世界点的坐标使得误差尽量的小：
  $h\left(\xi ,P^W\right)$为世界点到投影点$p$的过程，需要解决的问题，使得下式最小 （误差为$2\times 1$矩阵）：
$$f\left(\xi ,P^W\right)=\sum _{i=0}^m\sum _{j=0}^{n}h\left({\xi }_i,P_j^W\right)-Pixe{l}_{i,j}$$
  列文伯格—马夸尔特方法公式，令$D=I$：
$$\left(J^{T}J\left(x\right)+\lambda D^{T}D\right)\Delta x=-{J\left(x\right)}^{T}f\left(x\right)$$
$$\left(J^{T}J\left(x\right)+\lambda diag\left(J^{T}J\right)\right)\Delta x=-{J\left(x\right)}^{T}f\left(x\right)$$
  像素误差值$e=f\left(\xi ,P^W\right)$ 对相机姿态的偏导矩阵$F_{ij}$
$$\frac{\partial e}{\partial \delta \xi }=\frac{\partial e}{\partial p}\frac{\partial p}{\partial P^C}\frac{\partial P^C}{\partial \delta \xi }$$
$$=\left[\begin{array}{cc}{f}_x& 0\\ 0& f_y\end{array}\right]\left[\begin{array}{ccc}\frac{1}{P_z^C}& 0& -\frac{P_x^C}{{P_z^C}^2}\\ 0& \frac{1}{P_z^C}& -\frac{P_y^C}{{P_z^C}^2}\end{array}\right]\left[\begin{array}{cccccc}1& 0& 0& 0& P_z^C& -P_y^C\\ 0& 1& 0& -P_z^C& 0& P_x^C\\ 0& 0& 1& P_y^C& -P_x^C& 0\end{array}\right]$$
  像素误差值$e=f\left(\xi ,P^W\right)$对世界点的偏导矩阵$E_{ij}$
$$\frac{\partial e}{\partial P^W}=\frac{\partial e}{\partial p}\frac{\partial p}{\partial P^C}\frac{\partial P^C}{\partial P^W}$$
$$=\left[\begin{array}{cc}{f}_x& 0\\ 0& f_y\end{array}\right]\left[\begin{array}{ccc}\frac{1}{P_z^C}& 0& -\frac{P_x^C}{{P_z^C}^2}\\ 0& \frac{1}{P_z^C}& -\frac{P_y^C}{{P_z^C}^2}\end{array}\right]R$$
  待优化的相机位姿集合：
$$\Xi =\left[{\xi }_1,{\xi }_2,{\xi }_3,\cdots ,{\xi }_m\right]$$
  待优化的世界点集合：
$$P^W=\left(P_1^W,P_2^W,P_3^W,\cdots ,P_n^W\right)$$
  带入列文伯格—马夸尔特方法公式：
$$\left(J^{T}J+\lambda \ast diag\left(J^{T}J\right)\right)\left[\begin{array}{c}\Delta \Xi \\ \Delta P^W\end{array}\right]=-J^{T}e$$
  令 $G=\left(J^{T}J+\lambda \ast diag\left(J^{T}J\right)\right)$，$G$为：
$$G=\left[\begin{array}{cc}{F}^{T}F& F^{T}E\\ E^{T}F& E^{T}E\end{array}\right]$$
$$-J^{T}e=\left[\begin{array}{c}-F^{T}e\\ -E^{T}e\end{array}\right]=\left[\begin{array}{c}{e}_c\\ e_p\end{array}\right]$$
$$\Delta \Xi ={\left(F^{T}F-F^{T}E{\left(E^{T}E\right)}^{-1}{E}^{T}F\right)}^{-1}\left(e_c-F^{T}E{\left(E^{T}E\right)}^{-1}{e}_p\right)$$
$$\Delta P^W={\left(E^{T}E\right)}^{-1}\left(e_p-E^{T}F\Delta \Xi \right)$$
  注意：此时$\Delta \Xi$为李代数形式，需要使用指数映射转换到$SE\left(3\right)$