【ORBSLAM2点线融合】点特征图模型构建

在SLAM中，通常用BA（Bundle Adjustment）来实现多个三维点和不同相机位姿的优化
本文描述如何建立基于点特征的图优化，并推导相应的雅克比矩阵，并用g2o实现相应的类

1. 点特征误差及观测模型

假设相机位姿为$T_{cw}$表示世界坐标系到相机坐标系的变换，或者表示成$R_{cw},t_{cw}$。三维空间点表示成
世界坐标下$P^w={\left[\begin{array}{c}{P}_x^w,P_y^w,P_z^w\end{array}\right]}^T$
相机坐标下$P^c={\left[\begin{array}{c}{P}_x^c,P_y^c,P_z^c\end{array}\right]}^T$
相机内参为$K$
重投影误差为$e_p=p-p^{{}^{\prime }}=\left[\begin{array}{c}u\\ v\end{array}\right]-\left[\begin{array}{c}{u}^{{}^{\prime }}\\ v^{{}^{\prime }}\end{array}\right]$
其中$p$为当前图像中提取的点，$p^{{}^{\prime }}$为与$p$匹配的三维空间点的投影点，计算方式为
$$\begin{array}{rl}{P}^c& =R_{cw}{P}^w+t_{cw}\\ u^{{}^{\prime }}& =\frac{f_x{P}_x^c}{P_z^c}+c_x\\ v^{{}^{\prime }}& =\frac{f_y{P}_y^c}{P_z^c}+c_y\end{array}$$

2. 点特征重投影误差关于位姿增量的雅克比矩阵

利用链式法则，可以求得
$$\frac{\partial e_p}{\partial \delta \xi }=\frac{\partial e_p}{\partial P^c}\frac{\partial P^c}{\partial \delta \xi }$$
下面分别计算等式右边两项

这里注意，这里需要涉及到矩阵求导，矩阵求导通常分为分子布局和分母布局，这两种情况只是表达形式的不同，本身并没有两样，下面统一都用分母布局，具体求导结果可以看这篇文章

第一部分
$$\begin{array}{rl}\frac{\partial e_p}{\partial P^c}& =\frac{\partial \left[\begin{array}{c}u-u^{{}^{\prime }}\\ v-v^{{}^{\prime }}\end{array}\right]}{\partial P^c}\\ & =\left[\begin{array}{ccc}\frac{\partial (u-u^{{}^{\prime }})}{\partial P_x^c}& \frac{\partial (u-u^{{}^{\prime }})}{\partial P_y^c}& \frac{\partial (u-u^{{}^{\prime }})}{\partial P_z^c}\\ \frac{\partial (v-v^{{}^{\prime }})}{\partial P_x^c}& \frac{\partial (v-v^{{}^{\prime }})}{\partial P_y^c}& \frac{\partial (v-v^{{}^{\prime }})}{\partial P_z^c}\end{array}\right]\\ & =-{\left[\begin{array}{ccc}\frac{f_x}{P_z^c}& 0& \frac{f_x{P}_x^c}{P_z^{c2}}\\ 0& \frac{f_y}{P_z^c}& \frac{f_y{P}_y^c}{P_z^{c2}}\end{array}\right]}_{2\times 3}\end{array}$$
第二部分
$$\begin{array}{rl}\frac{\partial P^c}{\partial \delta \xi }& =\frac{\partial T_{cw}{P}^w}{\partial \delta \xi }\\ & =\underset{\delta \xi \to 0}{\lim }\frac{exp(\delta {\xi }^{\wedge })T_{cw}{P}^w-T_{cw}{P}^w}{\delta \xi }\end{array}$$对$exp(\delta {\xi }^{\wedge })$在$\delta \xi =0$处一阶泰勒展开，则上式变成
$$\begin{array}{rl}\frac{\partial P^c}{\partial \delta \xi }& =\underset{\delta \xi \to 0}{\lim }\frac{exp(\delta {\xi }^{\wedge })T_{cw}{P}^w-T_{cw}{P}^w}{\delta \xi }\\ & =\underset{\delta \xi \to 0}{\lim }\frac{(I+\delta {\xi }^{\wedge })T_{cw}{P}^w-T_{cw}{P}^w}{\delta \xi }\\ & =\underset{\delta \xi \to 0}{\lim }\frac{\delta {\xi }^{\wedge }{T}_{cw}{P}^w}{\delta \xi }\end{array}$$已知$\delta {\xi }^{\wedge }=\left[\begin{array}{cc}\delta {\varphi }^{\wedge }& \delta \rho \\ 0^T& 0\end{array}\right]$，则上式变成
$$\begin{array}{rl}\frac{\partial P^c}{\partial \delta \xi }& =\underset{\delta \xi \to 0}{\lim }\frac{\delta {\xi }^{\wedge }{T}_{cw}{P}^w}{\delta \xi }\\ & =\underset{\delta \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}_{cw}{P}^w+t_{cw}\\ 1\end{array}\right]}{\delta \xi }\end{array}$$这个地方$P^w$由齐次坐标转换成了非齐次坐标，把最后一行删去，就变成
$$\begin{array}{rl}\frac{\partial P^c}{\partial \delta \xi }& =\underset{\delta \xi \to 0}{\lim }\frac{\left[\begin{array}{cc}\delta {\varphi }^{\wedge }& \delta \rho \end{array}\right](R_{cw}{P}^w+t_{cw})}{\delta \xi }\\ & =\underset{\left[\begin{array}{c}\delta \varphi \\ \delta \rho \end{array}\right]\to 0}{\lim }\frac{\delta {\varphi }^{\wedge }(R_{cw}{P}^w+t_{cw})+\delta \rho }{\left[\begin{array}{c}\delta \varphi \\ \delta \rho \end{array}\right]}\\ & =\left[\begin{array}{cc}-(R_{cw}{P}^w+t_{cw}{)}^{\wedge }& I\end{array}\right]\\ & ={\left[\begin{array}{cc}-P^{c\wedge }& I\end{array}\right]}_{3\times 6}\end{array}$$其中，$\delta \xi =\left[\begin{array}{c}\delta \varphi \\ \delta \rho \end{array}\right]$，$\delta \varphi$表示旋转矩阵的增量，$\delta \rho$表示平移向量的增量，当然这里哪个在前哪个在后不会产生很大的影响，只是最终的雅克比矩阵前三列和后三列的位置不同而已。
点特征重投影误差关于位姿增量的雅克比矩阵为
$$\begin{array}{rl}\frac{\partial e_p}{\partial \delta \xi }& =\frac{\partial e_p}{\partial P^c}\frac{\partial P^c}{\partial \delta \xi }\\ & =-\left[\begin{array}{ccc}\frac{f_x}{P_z^c}& 0& \frac{f_x{P}_x^c}{P_z^{c2}}\\ 0& \frac{f_y}{P_z^c}& \frac{f_y{P}_y^c}{P_z^{c2}}\end{array}\right]\left[\begin{array}{cc}-P^{c\wedge }& I\end{array}\right]\\ & =-{\left[\begin{array}{cccccc}-\frac{f_x{P}_x^c{P}_y^c}{P_z^{c2}}& f_x+\frac{f_x{P}_x^{c2}}{P_z^{c2}}& -\frac{f_x{P}_y^c}{P_z^c}& \frac{f_x}{P_z^c}& 0& -\frac{f_x{P}_x^c}{P_z^{c2}}\\ -f_y-\frac{f_y{P}_y^{c2}}{P_z^{c2}}& \frac{f_y{P}_x^c{P}_y^c}{P_z^{c2}}& \frac{f_y{P}_x^c}{P_z^c}& 0& \frac{f_y}{P_z^c}& -\frac{f_y{P}_y^c}{P_z^{c2}}\end{array}\right]}_{2\times 6}\end{array}$$前面的负号主要和误差定义方式有关，这里我们是定义成匹配点减去空间投影点，如果误差是反过来定义的，则前面负号去掉就行。

3. 点特征重投影误差关于三维点坐标的雅克比矩阵

相比之下，误差关于三维点坐标的雅克比矩阵推导就容易多了。同样根据链式法则得
$$\frac{\partial e_p}{\partial P^w}=\frac{\partial e_p}{\partial P^c}\frac{\partial P^c}{\partial P^w}$$
第一项上一节已经算过了，第二项也容易
$$\begin{array}{rl}\frac{\partial P^c}{\partial P^w}& =\frac{\partial (R_{cw}{P}^w+t_{cw})}{\partial P^w}\\ & =R_{cw}\end{array}$$所以
$$\begin{array}{rl}{\frac{\partial e_p}{\partial P^w}}_{2\times 3}& =\frac{\partial e_p}{\partial P^c}\frac{\partial P^c}{\partial P^w}\\ & =-\left[\begin{array}{ccc}\frac{f_x}{P_z^c}& 0& \frac{f_x{P}_x^c}{P_z^{c2}}\\ 0& \frac{f_y}{P_z^c}& \frac{f_y{P}_y^c}{P_z^{c2}}\end{array}\right]R_{cw}\end{array}$$

4. g2o构建相应的顶点边

更新中

参考文献

[1] 高翔. 视觉SLAM十四讲[M].