BA问题理论及仿真验证

BA问题：
已知n个相机位姿（估计值），m个特征点世界坐标系坐标（估计值）和第j个特征点在第i个相机中像的像素坐标（观测值）；
目标是求解使重投影误差最小化的相机位姿和特征点坐标。

【符号约定】
待估计状态：$\mathrm{X}=[{\xi }_1,{\xi }_2,\cdots ,{\xi }_n,P_1^w,P_2^w,\cdots ,P_m^w{]}^T$；
${\xi }_i$：第i个相机的位姿，$6\times 1$的李代数；
$P_j^w,P_j^{c_i}$：分别表示世界坐标系、第i个相机坐标系下，第j个特征点的坐标，$3\times 1$；
${\hat{u}}_{ij},u_{ij}$：分别表示第j个特征点在第i个相机中像的像素坐标（的估计值、观测值），$2\times 1$；
$e_{ij}={\hat{u}}_{ij}-u_{ij}$：表示像素坐标重投影误差，$2\times 1$。

---

符号化BA问题：
$$({\xi }^{\ast },P_j^{w\ast })=\arg \underset{(\xi ,P_j^w)}{\min }\frac{1}{2}\sum _{(i,j)=(1,1)}^{(n,m)}\Vert {\hat{u}}_{ij}-u_{ij}{\Vert }_{\Sigma }^2\text{\hspace{1em}(1)}$$

式中，${\hat{u}}_{ij}$为依据估计的特征点坐标和估计的相机位姿计算得到的估计的像素坐标，详见公式（2）；$e_{ij}={\hat{u}}_{ij}-u_{ij}$为像素坐标重投影误差【表示，估计的的像素坐标与观测到的像素坐标（特征点在相机上的投影）相比较得到的误差】；$\Sigma$为观测噪声；
$$\begin{array}{rl}{s}_{ij}{\hat{u}}_{ij}& =K[exp({\xi }_i^{\wedge })P_j^w{]}_{1:3}\\ s_{ij}{\hat{u}}_{ij}& =K{P}_j^{c_i}\\ \left[\begin{array}{c}{s}_{ij}{\hat{\mu }}_{ij}\\ s_{ij}{\hat{\nu }}_{ij}\\ s_{ij}\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}_j^{c_i}\\ y_j^{c_i}\\ z_j^{c_i}\end{array}\right]\end{array}\text{\hspace{1em}(2)}$$

式中，定义$P_j^{c_i}\triangleq [exp({\xi }_i^{\wedge })P_j^w{]}_{1:3}$。
【注】公式中有一些非齐次、齐次坐标之间的转换。

欲根据${\xi }_i$，$P_j^w$求${\hat{u}}_{ij}$，而公式（2）中包含额外变量$s_{ij}$，故消去如下：
$$\{\begin{array}{l}{\hat{\mu }}_{ij}=f_x\frac{x_j^{c_i}}{z_j^{c_i}}+c_x\\ {\hat{\nu }}_{ij}=f_y\frac{y_j^{c_i}}{z_j^{c_i}}+c_y\end{array}$$

则残差函数：
$$\{\begin{array}{l}{e}_{{\mu }_{ij}}={\hat{\mu }}_{ij}-{\mu }_{ij}=f_x\frac{x_j^{c_i}}{z_j^{c_i}}+c_x-{\mu }_{ij}\\ e_{{\nu }_{ij}}={\hat{\nu }}_{ij}-{\nu }_{ij}=f_y\frac{y_j^{c_i}}{z_j^{c_i}}+c_y-{\nu }_{ij}\end{array}$$

使用LM算法求解该问题，需要计算$e_{ij}=[e_{{\mu }_{ij}},e_{{\nu }_{ij}}{]}^T$关于状态$\mathrm{X}$的雅可比矩阵。

---

在雅可比中，仅状态${\xi }_i$和$P_j^w$对应的值非零，其余状态与$e_{ij}$的无关故为0，即
$$\begin{array}{rl}\frac{\partial e_{ij}}{\partial \mathrm{X}}& ={\left[\begin{array}{cccccccccc}\frac{\partial e_{ij}}{\partial \delta {\xi }_1}& \cdots & \frac{\partial e_{ij}}{\partial \delta {\xi }_i}& \cdots & \frac{\partial e_{ij}}{\partial \delta {\xi }_n}& \frac{\partial e_{ij}}{\partial P_1^w}& \cdots & \frac{\partial e_{ij}}{\partial P_j^w}& \cdots & \frac{\partial e_{ij}}{\partial P_m^w}\end{array}\right]}_{2\times (6n+3m)}\\ & =\left[\begin{array}{cccccccccc}0& \cdots & \frac{\partial e_{ij}}{\partial \delta {\xi }_i}& \cdots & 0& 0& \cdots & \frac{\partial e_{ij}}{\partial P_j^w}& \cdots & 0\end{array}\right]\end{array}$$

【注】对李代数的求导采用扰动形式。
根据信息矩阵的定义：
$$\begin{array}{rl}{\Lambda }_{ij}& =(\frac{\partial e_{ij}}{\partial \mathrm{X}}{)}^T{\Sigma }_{ij}^{-1}\frac{\partial e_{ij}}{\partial \mathrm{X}}\\ & ={\left[\begin{array}{c}0\\ ⋮\\ \frac{\partial e_{ij}}{\partial \delta {\xi }_i}\\ ⋮\\ 0\\ 0\\ ⋮\\ \frac{\partial e_{ij}}{\partial P_j^w}\\ ⋮\\ 0\end{array}\right]}_{(6n+3m)\times 2}[{\Sigma }_{ij}^{-1}{]}_{2\times 2}{\left[\begin{array}{cccccccccc}0& \cdots & \frac{\partial e_{ij}}{\partial \delta {\xi }_i}& \cdots & 0& 0& \cdots & \frac{\partial e_{ij}}{\partial P_j^w}& \cdots & 0\end{array}\right]}_{2\times (6n+3m)}\\ & ={\left[\begin{array}{cccccccccc}0& \cdots & 0& \cdots & 0& 0& \cdots & 0& \cdots & 0\\ ⋮& & ⋮& & ⋮& ⋮& & ⋮& & ⋮\\ 0& \cdots & (\frac{\partial e_{ij}}{\partial \delta {\xi }_i}{)}^T{\Sigma }_{ij}^{-1}\frac{\partial e_{ij}}{\partial \delta {\xi }_i}& \cdots & 0& 0& \cdots & (\frac{\partial e_{ij}}{\partial \delta {\xi }_i}{)}^T{\Sigma }_{ij}^{-1}\frac{\partial e_{ij}}{\partial P_j^w}& \cdots & 0\\ ⋮& & ⋮& & ⋮& ⋮& & ⋮& & ⋮\\ 0& \cdots & 0& \cdots & 0& 0& \cdots & 0& \cdots & 0\\ 0& \cdots & 0& \cdots & 0& 0& \cdots & 0& \cdots & 0\\ ⋮& & ⋮& & ⋮& ⋮& & ⋮& & ⋮\\ 0& \cdots & (\frac{\partial e_{ij}}{\partial P_j^w}{)}^T{\Sigma }_{ij}^{-1}\frac{\partial e_{ij}}{\partial \delta {\xi }_i}& \cdots & 0& 0& \cdots & (\frac{\partial e_{ij}}{\partial P_j^w}{)}^T{\Sigma }_{ij}^{-1}\frac{\partial e_{ij}}{\partial P_j^w}& \cdots & 0\\ ⋮& & ⋮& & ⋮& ⋮& & ⋮& & ⋮\\ 0& \cdots & 0& \cdots & 0& 0& \cdots & 0& \cdots & 0\end{array}\right]}_{(6n+3m)\times (6n+3m)}\end{array}$$

故仅需要计算$e_{ij}=[e_{{\mu }_{ij}},e_{{\nu }_{ij}}{]}^T$关于状态${\xi }_i$和$P_j^w$的雅可比矩阵。

---

已定义$P_j^{c_i}\triangleq [exp({\xi }_i^{\wedge })P_j^w{]}_{1:3}$，故利用链式法则可先对$e_{ij}=[e_{{\mu }_{ij}},e_{{\nu }_{ij}}{]}^T$关于$P_j^{c_i}$求导，再计算$P_j^{c_i}$关于状态${\xi }_i$和$P_j^w$的雅可比矩阵，即：
$$\begin{array}{rl}\frac{\partial e_{ij}}{\partial \delta {\xi }_i}& =\frac{\partial e_{ij}}{\partial P_j^{c_i}}\frac{\partial P_j^{c_i}}{\partial \delta {\xi }_i}\\ \frac{\partial e_{ij}}{\partial P_j^w}& =\frac{\partial e_{ij}}{\partial P_j^{c_i}}\frac{\partial P_j^{c_i}}{\partial P_j^w}\end{array}$$

式中，三个偏导分别为：
（1）
$$\begin{array}{rl}\frac{\partial e_{ij}}{\partial P_j^{c_i}}& =\left[\begin{array}{ccc}\frac{\partial e_{{\mu }_{ij}}}{\partial x_j^{c_i}}& \frac{\partial e_{{\mu }_{ij}}}{\partial y_j^{c_i}}& \frac{\partial e_{{\mu }_{ij}}}{\partial z_j^{c_i}}\\ \frac{\partial e_{{\nu }_{ij}}}{\partial x_j^{c_i}}& \frac{\partial e_{{\nu }_{ij}}}{\partial y_j^{c_i}}& \frac{\partial e_{{\nu }_{ij}}}{\partial z_j^{c_i}}\end{array}\right]\\ & =\left[\begin{array}{ccc}\frac{f_x}{z_j^{c_i}}& 0& -\frac{f_x{x}_j^{c_i}}{(z_j^{c_i}{)}^2}\\ 0& \frac{f_y}{z_j^{c_i}}& -\frac{f_y{y}_j^{c_i}}{(z_j^{c_i}{)}^2}\end{array}\right]\end{array}$$

（2）
$$\begin{array}{rl}\frac{\partial P_j^{c_i}}{\partial P_j^w}& =\frac{R_{c_{i}w}{P}_j^w+t}{\partial P_j^w}=R_{c_{i}w}\end{array}$$

（3）
$$\begin{array}{rl}\frac{\partial P_j^{c_i}}{\partial \delta {\xi }_i}& =\frac{\partial exp({\xi }_i^{\wedge })P_j^w}{\partial \delta {\xi }_i}\\ & =\underset{\delta {\xi }_i\to 0}{\lim }\frac{exp(\delta {\xi }_i^{\wedge })exp({\xi }_i^{\wedge })P_j^w-exp({\xi }_i^{\wedge })P_j^w}{\partial \delta {\xi }_i}\\ & =\underset{\delta {\xi }_i\to 0}{\lim }\frac{(I+\delta {\xi }_i^{\wedge })exp({\xi }_i^{\wedge })P_j^w-exp({\xi }_i^{\wedge })P_j^w}{\partial \delta {\xi }_i}\\ & =\underset{\delta {\xi }_i\to 0}{\lim }\frac{\delta {\xi }_i^{\wedge }exp({\xi }_i^{\wedge })P_j^w}{\partial \delta {\xi }_i}\\ & =\underset{\delta {\xi }_i\to 0}{\lim }\frac{\left[\begin{array}{cc}\delta {\varphi }^{\wedge }& \delta \rho \\ 0^T& 0\end{array}\right]\left[\begin{array}{c}{P}_j^{c_i}\\ 1\end{array}\right]}{\partial \left[\begin{array}{c}\delta \rho \\ \delta \varphi \end{array}\right]}\\ & =\underset{\delta {\xi }_i\to 0}{\lim }\frac{\left[\begin{array}{c}\delta {\varphi }^{\wedge }{P}_j^{c_i}+\delta \rho \\ 0\end{array}\right]}{\partial \left[\begin{array}{c}\delta \rho \\ \delta \varphi \end{array}\right]}\\ & =\left[\begin{array}{cc}I& -(P_j^{c_i}{)}^{\wedge }\\ 0^T& 0^T\end{array}\right]\end{array}$$

取前3行。
【注】如果$\mathfrak{s}\mathfrak{e}(3)$的定义方式是旋转在前、平移在后，只需将这个矩阵的前3列与后3列对调即可。

结论：
$$\begin{array}{rl}\frac{\partial e_{ij}}{\partial \delta {\xi }_i}& =\frac{\partial e_{ij}}{\partial P_j^{c_i}}\frac{\partial P_j^{c_i}}{\partial \delta {\xi }_i}\\ & =\left[\begin{array}{ccc}\frac{f_x}{z_j^{c_i}}& 0& -\frac{f_x{x}_j^{c_i}}{(z_j^{c_i}{)}^2}\\ 0& \frac{f_y}{z_j^{c_i}}& -\frac{f_y{y}_j^{c_i}}{(z_j^{c_i}{)}^2}\end{array}\right]R_{c_{i}w}\\ \frac{\partial e_{ij}}{\partial P_j^w}& =\frac{\partial e_{ij}}{\partial P_j^{c_i}}\frac{\partial P_j^{c_i}}{\partial P_j^w}\\ & =\left[\begin{array}{ccc}\frac{f_x}{z_j^{c_i}}& 0& -\frac{f_x{x}_j^{c_i}}{(z_j^{c_i}{)}^2}\\ 0& \frac{f_y}{z_j^{c_i}}& -\frac{f_y{y}_j^{c_i}}{(z_j^{c_i}{)}^2}\end{array}\right]\left[\begin{array}{cc}I& -(P_j^{c_i}{)}^{\wedge }\end{array}\right]\\ & =\left[\begin{array}{ccc}\frac{f_x}{z_j^{c_i}}& 0& -\frac{f_x{x}_j^{c_i}}{(z_j^{c_i}{)}^2}\\ 0& \frac{f_y}{z_j^{c_i}}& -\frac{f_y{y}_j^{c_i}}{(z_j^{c_i}{)}^2}\end{array}\right]\left[\begin{array}{cccccc}1& 0& 0& 0& z_j^{c_i}& -y_j^{c_i}\\ 0& 1& 0& -z_j^{c_i}& 0& x_j^{c_i}\\ 0& 0& 1& y_j^{c_i}& -x_j^{c_i}& 0\end{array}\right]\\ & =\left[\begin{array}{cccccc}\frac{f_x}{z_j^{c_i}}& 0& -\frac{f_x{x}_j^{c_i}}{(z_j^{c_i}{)}^2}& -\frac{f_x{x}_j^{c_i}{y}_j^{c_i}}{(z_j^{c_i}{)}^2}& f_x+\frac{f_x(x_j^{c_i}{)}^2}{(z_j^{c_i}{)}^2}& -\frac{f_x{y}_j^{c_j}}{z_j^{c_i}}\\ 0& \frac{f_y}{z_j^{c_i}}& -\frac{f_y{y}_j^{c_i}}{(z_j^{c_i}{)}^2}& -f_y-\frac{f_y(y_j^{c_i}{)}^2}{(z_j^{c_i}{)}^2}& \frac{f_y{x}_j^{c_i}{y}_j^{c_i}}{(z_j^{c_i}{)}^2}& \frac{f_y{x}_j^{c_j}}{z_j^{c_i}}\end{array}\right]\end{array}$$

---

对应的代码：

Eigen::Matrix<double,2,3> jacobian_uv_Pc;
jacobian_uv_Pc<< fx/z, 0 , -x * fx/z_2,
        0, fy/z, -y * fy/z_2;
Eigen::Matrix<double,2,3> jacobian_Pj = jacobian_uv_Pc * Rcw; // 2*3
Eigen::Matrix<double,2,6> jacobian_Ti;
jacobian_Ti << -x* y * fx/z_2, (1+ x*x/z_2)*fx, -y/z*fx, fx/z, 0 , -x * fx/z_2,
                -(1+y*y/z_2)*fy, x*y/z_2 * fy, x/z * fy, 0,fy/z, -y * fy/z_2;