SLAM--最小化重投影误差

一、相机模型

设相机坐标系下的左边点$[X,Y,Z{]}^T$,相机物理成像的其次坐标为$[X^{\prime },Y^{\prime },1]$，成像焦距为$f$，由相似三角形的关系可以得到：
$$\begin{gathered} X^{\prime }=f\frac{X}{Z} \\ {Y}^{\prime }=f\frac{Y}{Z}\text{\hspace{1em}(1)} \end{gathered}$$
设像素坐标为$[u,v{]}^T$，如果像素在u轴上缩放$\alpha$倍，在v轴上缩放$\beta$倍，则：
$$\{\begin{array}{r}u=\alpha X^{\prime }+c_x\\ v=\beta Y^{\prime }+c_y\end{array}\text{\hspace{1em}(2)}$$
将$\alpha f$合并成$f_x$，$\beta X^{\prime }$合并成$f_y$:
$$\{\begin{array}{r}u=f_x\frac{X}{Z}+c_x\\ v=f_y\frac{Y}{Z}+c_x\end{array}\text{\hspace{1em}(3)}$$
所以有:
$$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]\left[\begin{array}{c}X\\ \\ Y\\ \\ Z\end{array}\right]\text{\hspace{1em}(4)}$$
即：
$$Z{\mathbf{P}}_{uv}=Z\left[\begin{array}{c}u\\ \\ v\\ \\ 1\end{array}\right]=\mathbf{K}\mathbf{P}={\mathbf{K}\mathbf{T}\mathbf{P}}_w\text{\hspace{1em}(5)}$$
 
 

二、最小化重投影误差

2.1 优化位姿

若空间中的点为${\mathbf{P}}_i=[X_i,Y_i,Z_i{]}^T$，其投影的像素坐标为${\mathbf{u}}_i=[u_i,v_i{]}^T$，相机的位姿为$\mathbf{T}$，所以投影关系为：
$${\mathbf{u}}_i=\frac{1}{s_i}{\mathbf{K}\mathbf{T}\mathbf{P}}_i\text{\hspace{1em}(6)}$$
由于观测点的噪声，所以就存在误差，所以为了找到一个最优的位姿，我们根据误差构造一个最小二乘问题：
$$\underset{T}{\mathrm{argmin}}\frac{1}{2}\sum _{i=1}^n{\Vert {\mathbf{u}}_i-\frac{1}{s_i}{\mathbf{K}\mathbf{T}\mathbf{P}}_i\Vert }_2^2\text{\hspace{1em}(7)}$$
面对这个最小二乘法，我们一般利用高斯牛顿法或者列文伯格-马夸尔特方法进行求解，我们令:
$$\begin{gathered} \mathbf{e}={\mathbf{u}}_i-\frac{1}{s_i}{\mathbf{K}\mathbf{T}\mathbf{P}}_i \\ \mathbf{e}(\xi )={\mathbf{u}}_i-\frac{1}{s_i}\mathbf{K}\exp ({\xi }^{\wedge }){\mathbf{P}}_i\text{\hspace{1em}(8)} \end{gathered}$$
参考我的博文：SLAM–线性化求解估计位姿

$$\mathbf{e}(\Delta \xi \oplus \xi )={\mathbf{u}}_i-\frac{1}{s_i}\mathbf{K}\exp (\Delta {\xi }^{\wedge })\exp ({\xi }^{\wedge }){\mathbf{P}}_i\text{\hspace{1em}(9)}$$

所以对误差函数线性化可得：
$$\begin{gathered} \mathbf{e}(\Delta \xi \oplus \xi )=\mathbf{e}({\mathbf{J}}_l^{-1}\Delta \xi +\xi )\approx \mathbf{e}(\xi )+{\mathbf{J}}_{\delta \xi }\Delta {\xi }_{6\times 1} \\ \to \mathbf{e}(\Delta \xi \oplus \xi )\approx \mathbf{e}(\xi )+{\mathbf{J}}_{\delta \xi }\Delta {\xi }_{6\times 1}\text{\hspace{1em}(10)} \end{gathered}$$
其中:
$$\begin{array}{rl}{\mathbf{J}}_{\delta \xi }& =\frac{\partial \mathbf{e}(\xi )}{\partial \delta \xi }\\ & \\ & \\ & =-\frac{\partial (\frac{1}{s_i}\mathbf{K}\exp ({\xi }^{\wedge }){\mathbf{P}}_i)}{\partial \delta \xi }\\ & \\ & =-\frac{\partial \mathbf{u}}{\partial \delta \xi }\end{array}\text{\hspace{1em}(11)}$$
我们不使用像素坐标的其次坐标，所以：
$$\mathbf{u}=\left[\begin{array}{c}u\\ \\ v\end{array}\right]=\left[\begin{array}{c}{f}_x\frac{X}{Z}+c_x\\ \\ f_y\frac{Y}{Z}+c_x\end{array}\right],{\mathbf{P}}^{\prime }=\left[\begin{array}{c}{X}^{\prime }\\ \\ Y^{\prime }\\ \\ Z^{\prime }\end{array}\right]\text{\hspace{1em}(12)}$$
由链式求导法则可得：
$$J_{\delta \xi }=-\frac{\partial \mathbf{u}}{\partial \delta \xi }=-\frac{\partial \mathbf{u}}{\partial {\mathbf{P}}^{\prime }}\cdot \frac{\partial {\mathbf{P}}^{\prime }}{\partial \delta \xi }\text{\hspace{1em}(13)}$$
这样我们就可以求出第一个导数：
$$\frac{\partial \mathbf{u}}{\partial {\mathbf{P}}^{\prime }}=\left[\begin{array}{ccc}\frac{\partial \mathbf{u}}{\partial {\mathbf{X}}^{\prime }}& \frac{\partial \mathbf{u}}{\partial {\mathbf{Y}}^{\prime }}& \frac{\partial \mathbf{u}}{\partial {\mathbf{Z}}^{\prime }}\\ & & \\ \frac{\partial \mathbf{v}}{\partial {\mathbf{X}}^{\prime }}& \frac{\partial \mathbf{v}}{\partial {\mathbf{Y}}^{\prime }}& \frac{\partial \mathbf{v}}{\partial {\mathbf{Z}}^{\prime }}\end{array}\right]=\left[\begin{array}{ccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}\\ & & \\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}\end{array}\right]$$
求第二个导数：
$$\begin{gathered} \frac{\partial {\mathbf{P}}^{\prime }}{\partial \delta \xi }=\frac{\partial {(\mathbf{T}\bar{\mathbf{P}})}_{1:3}}{\partial \delta \xi }=[(\mathbf{T}\bar{\mathbf{P}}{)}^{\odot }{]}_{1:3} \\ ={\left[\begin{array}{cc}{\mathbf{I}}_{3\times 3}& -(\mathbf{T}\bar{\mathbf{P}}{)}_{1:3}^{\wedge }\end{array}\right]}_{3\times 6} \end{gathered}$$
所以有：
$$\begin{array}{rl}{J}_{\delta \xi }& =-\left[\begin{array}{ccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}\\ & & \\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}\end{array}\right]\cdot {\left[\begin{array}{cc}{\mathbf{I}}_{3\times 3}& -(\mathbf{T}\bar{P}{)}_{1:3}^{\wedge }\end{array}\right]}_{3\times 6}\\ & \\ & =-\left[\begin{array}{ccccccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}& -\frac{f_x{X}^{\prime }{Y}^{\prime }}{Z^{\prime 2}}& f_x+\frac{f_x{X}^{\prime 2}}{Z^{\prime 2}}& -\frac{f_x{Y}^{\prime }}{Z^{\prime }}& \\ & & & & & & \\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}& -f_y-\frac{f_y{Y}^{\prime 2}}{Z^{\prime 2}}& \frac{f_y{Y}^{\prime }{X}^{\prime }}{Z^{\prime 2}}& \frac{f_y{X}^{\prime }}{Z^{\prime }}& \end{array}\right]\end{array}$$
这样我们就可以求出$\Delta \xi$:
$$\Delta \xi =-(J_{\delta \xi }^T\cdot J_{\delta \xi }{)}^{-1}(J_{\delta \xi }^T\cdot \mathbf{e}(\xi ))$$
利用高斯迭代法，对于第K+1次更新位姿：
$$T({\xi }_{k+1})=\exp (\Delta x^{\wedge })\cdot T({\xi }_0)=\exp (\Delta {\xi }^{\wedge })\cdot T({\xi }_0)$$
 
 

2.2 优化路标

同样写出相关函数：
$$\begin{gathered} \mathbf{e}={\mathbf{u}}_i-\frac{1}{s_i}{\mathbf{K}\mathbf{T}\mathbf{P}}_i \\ \mathbf{e}({\mathbf{P}}_i)={\mathbf{u}}_i-\frac{1}{s_i}\mathbf{K}\exp ({\xi }^{\wedge }){\mathbf{P}}_i\text{\hspace{1em}()} \end{gathered}$$
利用链式求导法则：
$$\frac{\partial \mathbf{e}(\mathbf{P})}{\partial \mathbf{P}}=\frac{\partial \mathbf{e}(\mathbf{P})}{\partial {\mathbf{P}}^{\prime }}\frac{\partial {\mathbf{P}}^{\prime }}{\partial \mathbf{P}}$$
第一个导数我们已经求出：
$$\frac{\partial \mathbf{e}(\mathbf{P})}{\partial {\mathbf{P}}^{\prime }}=-\frac{\partial \mathbf{u}}{\partial {\mathbf{P}}^{\prime }}=-\left[\begin{array}{ccc}\frac{\partial \mathbf{u}}{\partial {\mathbf{X}}^{\prime }}& \frac{\partial \mathbf{u}}{\partial {\mathbf{Y}}^{\prime }}& \frac{\partial \mathbf{u}}{\partial {\mathbf{Z}}^{\prime }}\\ & & \\ \frac{\partial \mathbf{v}}{\partial {\mathbf{X}}^{\prime }}& \frac{\partial \mathbf{v}}{\partial {\mathbf{Y}}^{\prime }}& \frac{\partial \mathbf{v}}{\partial {\mathbf{Z}}^{\prime }}\end{array}\right]$$
对于第二个导数有${\mathbf{P}}^{\prime }={(\mathbf{T}\bar{\mathbf{P}})}_{1:3}=\mathbf{R}\mathbf{P}+\mathbf{t}$，所以：
$$\frac{\partial {\mathbf{P}}^{\prime }}{\partial \mathbf{P}}=\mathbf{R}$$
于是有：
$$\frac{\partial \mathbf{e}(\mathbf{P})}{\partial \mathbf{P}}=-\left[\begin{array}{ccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}\\ & & \\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}\end{array}\right]\cdot \mathbf{R}$$
由泰勒展开式：
$$\mathbf{e}(\Delta \mathbf{P}+\mathbf{P})\approx \mathbf{e}(\mathbf{P})+{\mathbf{J}}_e\cdot \Delta \mathbf{P}$$

$$(\Delta \mathbf{P})=-({\mathbf{J}}_e^T\cdot {\mathbf{J}}_e{)}^{-1}({\mathbf{J}}_e^T\cdot \mathbf{e}(\mathbf{P}))$$

对于第K+1次更新位姿：
$${\mathbf{P}}_{k+1}=\Delta {\mathbf{P}}_k+{\mathbf{P}}_k$$
至此重投影误差求解推导完成。