虽然现在的轮子很多，但我们在使用过程中会碰到很多问题，而我们经常不知道从哪里下手，说明轮子不是你造的你不熟悉。因此我们不仅要重复造轮子，还要好好造，深入造，才能用好轮子，把轮子转化成自身的力量。同样的道理适用于这篇文章。虽然网上BA的资料无穷无尽，但我们还是要好好深入理解其原理，并且一定要通过实践才能懂得其中原理。在"第一届SLAM论坛"中沈劭劼老师的发言中，他提到团队的成员都要手写BA，既然大佬都这么做，我们就照做吧。这篇文章是我手写BA的笔记，主要从原理推导入手，把公式都写一遍，然后通过g2o、ceres和eigen三种方式来编程实现，以便加深对BA的理解。本文所有例子代码参见https://github.com/shanpenghui/BA_exercise.git
一、前言
Bundle Adjustment中文译作光束平差法、捆集调整等，是指从视觉重建中提炼出最优的3D模型和相机参数（内参和外参）。从每个特征点反射出来的几束光线（bundles of light rays），在我们把相机姿态和特征点的位置做出最优的调整（adjustment）之后，最后收束到光心的这个过程，简称BA。
二、原理
现假设空间位置的3D点为
X = { x 1 , x 2 , … , x n } (1) \begin{matrix} X = \left\{ x_{1},x_{2},\ldots,x_{n} \right\} \end{matrix} \tag{1} X={x1,x2,…,xn}(1)
相机中心位姿为
P = { p 1 , p 2 , … , p n } (2) \begin{matrix} P = \left\{ p_{1},p_{2},\ldots,p_{n} \right\} \end{matrix} \tag{2} P={p1,p2,…,pn}(2)
u i u_{i} ui为 x i x_{i} xi对应的像素位置， K K K为相机内参矩阵， s i s_{i} si为 u i u_{i} ui对应的深度值。如图，BA要计算的误差是观测值和估计值之间的误差。
对于bundle adjustment可以构建重投影误差最小二乘如下：
ε ( X , T ) = ∑ i = 0 n ∣ ∣ u i − 1 s i K ⋅ T ⋅ X i ∣ ∣ 2 (3) \begin{matrix} \varepsilon\left( X,T \right) = \sum_{i = 0}^{n}\left| \left| u_{i} - \frac{1}{s_{i}}K \cdot T{\cdot X}_{i} \right| \right|^{2} \end{matrix} \tag{3} ε(X,T)=∑i=0n∣∣∣∣∣∣ui−si1K⋅T⋅Xi∣∣∣∣∣∣2(3)
对于变换矩阵T，满足如下约束：
T = [ R t 0 T 1 ] , R T ⋅ R = I , det ( R ) = 1 , t ∈ R 3 (4) \begin{matrix} T = \begin{bmatrix} \text{R\ t} \\ 0^{T}\ 1 \\ \end{bmatrix},R^{T} \cdot R = I,\det\left( R \right) = 1,t \in \mathbb{R}^{3} \end{matrix} \tag{4} T=[R t0T 1],RT⋅R=I,det(R)=1,t∈R3(4)
对于有约束的变换矩阵在最小二乘中不好求解，转换为无约束的李群求解：
ε ( X , ξ ) = ∑ i = 0 n ∣ ∣ u i − 1 s i K ⋅ e x p ( ξ ∧ ) ⋅ X i ∣ ∣ 2 (5) \begin{matrix} \varepsilon\left( X,\xi \right) = \sum_{i = 0}^{n}{||u_{i} - \frac{1}{s_{i}}K \cdot exp(\xi^{\land}){\cdot X}_{i}||}^{2} \end{matrix} \tag{5} ε(X,ξ)=∑i=0n∣∣ui−si1K⋅exp(ξ∧)⋅Xi∣∣2(5)
其中， ξ \xi ξ是旋转向量(李代数)， ξ ^ \xi^{\hat{}} ξ^是旋转向量的矩阵表达， e x p ( ξ ^ ) exp(\xi^{\hat{}}) exp(ξ^)表达从向量(李代数)到矩阵(李群)的对数映射。
定义误差函数为：
f ( X i , ξ ) = u i − K ⋅ exp ( ξ ^ ) ⋅ X i (6) \begin{matrix} f\left( X_{i},\xi \right) = u_{i} - K \cdot \exp\left( \xi^{\hat{}} \right){\cdot X}_{i} \end{matrix} \tag{6} f(Xi,ξ)=ui−K⋅exp(ξ^)⋅Xi(6)
对于高斯牛顿，函数 f ( x ) f\left( x \right) f(x)的 x x x变量指的就是函数 f ( X i , ξ ) f\left( X_{i},\xi \right) f(Xi,ξ)里面的 ( X i , ξ ) \left( X_{i},\xi \right) (Xi,ξ)。根据该方法，有增量方程：
J ( x ) T ⋅ J ( x ) ⋅ Δ x = − J ( x ) T ⋅ f ( x ) (7) \begin{matrix} {J\left( x \right)}^{T} \cdot J\left( x \right) \cdot \mathrm{\Delta}x = - {J\left( x \right)}^{T} \cdot f\left( x \right) \end{matrix} \tag{7} J(x)T⋅J(x)⋅Δx=−J(x)T⋅f(x)(7)
从增量方程可以看到，我们需要计算函数 f ( x ) f(x) f(x)的Jacobian矩阵，根据求导链式法则，要对多变量进行求导，最终表示为对 ( X i , ξ ) \left( X_{i},\xi \right) (Xi,ξ)的 ( X i ) \left( X_{i} \right) (Xi)求导，以及对 ( X i , ξ ) \left( X_{i},\xi \right) (Xi,ξ)的 ( ξ ) \left( \xi \right) (ξ)求导。
函数对变量 ( ξ ) \left( \xi \right) (ξ)进行求导，即重投影误差函数 f ( X i , ξ ) f\left( X_{i},\xi \right) f(Xi,ξ)对相机位姿 ( ξ ) \left( \xi \right) (ξ)求导：
∂ f ( X i , ξ ) ∂ ξ = [ f x z 0 − f x x z 2 0 f y z − f y y z 2 ] ⋅ R (8) \begin{matrix} \frac{\partial f\left( X_{i},\xi \right)}{\partial\xi} = \begin{bmatrix} \frac{f_{x}}{z}\ 0 - \frac{f_{x}x}{z^{2}} \\ 0\ \frac{f_{y}}{z}\ - \frac{f_{y}y}{z^{2}} \\ \end{bmatrix} \cdot R \end{matrix} \tag{8} ∂ξ∂f(Xi,ξ)=[zfx 0−z2fxx0 zfy −z2fyy]⋅R(8)
函数对变量 ( X i ) \left( X_{i} \right) (Xi)进行求导，即重投影误差函数 f ( X i , ξ ) f\left( X_{i},\xi \right) f(Xi,ξ)对坐标点 ( X i ) \left( X_{i} \right) (Xi)求导：
∂ f ( X i , ξ ) ∂ X i = [ f x z 0 − f x x z 2 − f x x y z 2 f x ( 1 + x 2 z 2 ) − f x y z 0 f y z − f y y z 2 − f y ( 1 + y 2 z 2 ) f y x y z 2 f y x z ] (9) \begin{matrix} \frac{\partial f\left( X_{i},\xi \right)}{\partial X_{i}} = \begin{bmatrix} \frac{f_{x}}{z}\ 0 - \frac{f_{x}x}{z^{2}}\ - \frac{f_{x}xy}{z^{2}}\ f_{x}\left( 1 + \frac{x^{2}}{z^{2}} \right)\ - \frac{f_{x}y}{z} \\ 0\ \frac{f_{y}}{z}\ - \frac{f_{y}y}{z^{2}}\ {- f}_{y}\left( 1 + \frac{y^{2}}{z^{2}} \right)\ \frac{f_{y}xy}{z^{2}}\ \frac{f_{y}x}{z} \\ \end{bmatrix} \end{matrix} \tag{9} ∂Xi∂f(Xi,ξ)=⎣⎡zfx 0−z2fxx −z2fxxy fx(1+z2x2) −zfxy0 zfy −z2fyy −fy(1+z2y2) z2fyxy zfyx⎦⎤(9)
三、必备知识
1.SO(3)的对数映射 Exponential Map
李群和李代数的映射关系为：
e x p : so ( 3 ) ↦ S O ( 3 ) (10) \begin{matrix} exp:\mathfrak{\text{so}}\left( 3 \right)\ \mapsto SO\left( 3 \right) \end{matrix} \tag{10} exp:so(3) ↦SO(3)(10)
ω ↦ R 3 × 3 (11) \begin{matrix} \omega\ \ \ \ \ \ \ \mapsto \ R_{3 \times 3} \end{matrix} \tag{11} ω ↦ R3×3(11)
转换成罗格里格式公式表示为：
e ω ≡ m a t e x p ( ω ∧ ) = I 3 + sinθ θ ⋅ ω ∧ + 1 − cosθ θ 2 ⋅ ( ω ∧ ) 2 (12) \begin{matrix} e^{\omega} \equiv matexp\left( \omega^{\land} \right) = I_{3} + \frac{\text{sinθ}}{\theta} \cdot \omega^{\land} + 1 - \frac{\text{cosθ}}{\theta^{2}} \cdot \left( \omega^{\land} \right)^{2} \end{matrix} \tag{12} eω≡matexp(ω∧)=I3+θsinθ⋅ω∧+1−θ2cosθ⋅(ω∧)2(12)
其中，
ω
\omega
ω是3维的旋转向量，表示为
ω
=
[
x
y
z
]
\omega = \begin{bmatrix} x \\ y \\z \end{bmatrix}
ω=⎣⎡xyz⎦⎤，
θ
\theta
θ是旋转角度，表示为
θ
=
∣
ω
∣
\theta = |\omega|
θ=∣ω∣，
ω
∧
\omega^{\land}
ω∧是旋转向量
ω
\omega
ω的反对称矩阵，表示为
ω
∧
=
[
0
−
z
y
z
0
−
x
−
y
x
0
]
\omega^{\land} = \begin{bmatrix} 0 & - z & y \\ z & 0 & - x \\- y & x & 0 \\ \end{bmatrix}
ω∧=⎣⎡0z−y−z0xy−x0⎦⎤。
2.向量外积
两个向量的叉积其实就是这个反对称矩阵的来源，比如：
a → × b → = [ i j k a 1 a 2 a 3 b 1 b 2 b 3 ] = [ a 2 b 3 − a 3 b 2 a 3 b 1 − a 1 b 3 a 1 b 2 − a 2 b 1 ] = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] b → ≡ a ∧ b (13) \begin{matrix} \overrightarrow{a} \times \overrightarrow{b} = \begin{bmatrix} i & j & k \\ a_{1} & a_{2} & a_{3} \\ b_{1} & b_{2} & b_{3} \\ \end{bmatrix} = \begin{bmatrix} 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{bmatrix} = \begin{bmatrix} 0 & - a_{3} & a_{2} \\ a_{3} & 0 & - a_{1} \\ -a_{2} & a_{1} & 0 \\ \end{bmatrix}\overrightarrow{b} \equiv a^{\land}b \end{matrix} \tag{13} a×b=⎣⎡ia1b1ja2b2ka3b3⎦⎤=⎣⎡a2b3−a3b2a3b1−a1b3a1b2−a2b1⎦⎤=⎣⎡0a3−a2−a30a1a2−a10⎦⎤b≡a∧b(13)
所以，上述旋转向量 ω \omega ω的反对称矩阵 ω ∧ \omega^{\land} ω∧表达的是其和单位向量之间的叉积，也叫外积，一般来说，向量的外积表示向量的旋转。
3.SO(3)的Jacobian
根据公式(12)，SO3的对数映射的导数是：
∂ e ω ∂ ω ∣ ω = 0 ≡ ∂ v e c ( e ω ) ∂ ω ∣ ω = 0 = [ − e 1 ∧ − e 2 ∧ − e 3 ∧ ] ( A 9 × 3 J a c o b i a n ) (14) \begin{matrix} \left. \ \frac{\partial e^{\omega}}{\partial\omega} \right|_{\omega = 0} \equiv \left. \ \frac{\partial{vec(e}^{\omega})}{\partial\omega} \right|_{\omega = 0} = \begin{bmatrix} -e_{1}^{\land} \\ -e_{2}^{\land} \\ -e_{3}^{\land} \\ \end{bmatrix}\ \ \ \ (A\ 9 \times 3\ Jacobian) \end{matrix} \tag{14} ∂ω∂eω∣∣ω=0≡ ∂ω∂vec(eω)∣∣∣ω=0=⎣⎡−e1∧−e2∧−e3∧⎦⎤ (A 9×3 Jacobian)(14)
其中， e 1 ∧ = [ 1 0 0 ] T e_{1}^{\land} = \begin{bmatrix} 1 & 0 & 0 \\ \end{bmatrix}^{T} e1∧=[100]T， e 2 ∧ = [ 0 1 0 ] T e_{2}^{\land} = \begin{bmatrix} 0 & 1 & 0 \\ \end{bmatrix}^{T} e2∧=[010]T， e 3 ∧ = [ 0 0 1 ] T e_{3}^{\land} = \begin{bmatrix} 0 & 0 & 1 \\ \end{bmatrix}^{T} e3∧=[001]T。
4.SE(3)的对数映射 Exponential Map
设有6维的李代数 se ( 3 ) \mathfrak{\text{se}}(3) se(3)为 ν \nu ν，表示为
ν = [ t ω ] (15) \begin{matrix} \nu = \begin{bmatrix} t \\ \omega \\ \end{bmatrix} \end{matrix} \tag{15} ν=[tω](15)
ν \nu ν分为2个3维向量，其中 ω \omega ω表示旋转向量， t t t表示平移向量，我们进一步假设由李代数 se ( 3 ) \mathfrak{\text{se}}(3) se(3)为 ν \nu ν映射出来的李群，即变换矩阵为 A A A，则 A A A表示为：
A ( ν ) = [ ω ∧ t 0 0 ] (16) \begin{matrix} A\left( \nu \right) = \begin{bmatrix} \omega^{\land}\text{\ t} \\ 0\ \ 0 \\ \end{bmatrix} \end{matrix} \tag{16} A(ν)=[ω∧ t0 0](16)
通过李代数到李群的对数映射有：
e x p : se ( 3 ) ↦ S E ( 3 ) (17) \begin{matrix} exp:\mathfrak{\text{se}}\left( 3 \right)\ \mapsto SE\left( 3 \right) \end{matrix} \tag{17} exp:se(3) ↦SE(3)(17)
那么，李代数 se ( 3 ) \mathfrak{\text{se}}(3) se(3)为 ν \nu ν的对数表示为：
e ν ≡ e A ( ν ) = [ e ω ∧ Vt 0 1 ] (18) \begin{matrix} e^{\nu} \equiv e^{A\left( \nu \right)} = \begin{bmatrix} e^{\omega^{\land}}\text{\ Vt} \\ 0\ \ \ 1 \\ \end{bmatrix} \end{matrix} \tag{18} eν≡eA(ν)=[eω∧ Vt0 1](18)
其中 V V V表示为：
V = I 3 + 1 − c o s θ θ 2 ⋅ ω ∧ + θ − s i n θ θ 3 ⋅ ( ω ∧ ) 2 (19) \begin{matrix} V = I_{3} + \frac{1 - cos\theta}{\theta^{2}} \cdot \omega^{\land} + \frac{\theta - sin\theta}{\theta^{3}} \cdot \left( \omega^{\land} \right)^{2} \end{matrix} \tag{19} V=I3+θ21−cosθ⋅ω∧+θ3θ−sinθ⋅(ω∧)2(19)
其中， ω \omega ω是3维的旋转向量，表示为 ω = [ x y z ] \omega = \begin{bmatrix}x \\y \\z \\ \end{bmatrix} ω=⎣⎡xyz⎦⎤， θ \theta θ是旋转角度，表示为 θ = ∣ ω ∣ \theta = |\omega| θ=∣ω∣， ω ∧ \omega^{\land} ω∧是旋转向量 ω \omega ω的反对称矩阵，表示为 ω ∧ = [ 0 − z y z 0 − x − y x 0 ] \omega^{\land} = \begin{bmatrix} 0 & - z & y \\ z & 0 & - x \\ -y & x & 0 \\ \end{bmatrix} ω∧=⎣⎡0z−y−z0xy−x0⎦⎤。
5.SE(3)的Jacobian
根据公式(18)，当李代数坐标 ε = 0 \varepsilon = 0 ε=0的时候，SE3的对数映射的导数是：
∂ e ε ∂ ε ∣ ε = 0 ≡ ∂ v e c ( e ε ) ∂ ε ∣ ε = 0 = [ 0 3 × 3 − e 1 ∧ 0 3 × 3 − e 2 ∧ 0 3 × 3 − e 3 ∧ I 3 × 3 0 3 × 3 ] ( A 12 × 6 J a c o b i a n ) (20) \begin{matrix} \left. \ \frac{\partial e^{\varepsilon}}{\partial\varepsilon} \right|_{\varepsilon = 0} \equiv \left. \ \frac{\partial{vec(e}^{\varepsilon})}{\partial\varepsilon} \right|_{\varepsilon = 0} = \begin{bmatrix} 0_{3 \times 3} & - e_{1}^{\land} \\ 0_{3 \times 3} & - e_{2}^{\land} \\ 0_{3 \times 3} & - e_{3}^{\land} \\ I_{3 \times 3} & 0_{3 \times 3} \\ \end{bmatrix}\ \ \ \ (A\ 12 \times 6\ Jacobian) \end{matrix} \tag{20} ∂ε∂eε∣∣ε=0≡ ∂ε∂vec(eε)∣∣∣ε=0=⎣⎢⎢⎡03×303×303×3I3×3−e1∧−e2∧−e3∧03×3⎦⎥⎥⎤ (A 12×6 Jacobian)(20)
其中， e 1 ∧ = [ 1 0 0 ] T e_{1}^{\land} = \begin{bmatrix} 1 & 0 & 0 \\ \end{bmatrix}^{T} e1∧=[100]T， e 2 ∧ = [ 0 1 0 ] T e_{2}^{\land} = \begin{bmatrix} 0 & 1 & 0 \\ \end{bmatrix}^{T} e2∧=[010]T， e 3 ∧ = [ 0 0 1 ] T e_{3}^{\land} = \begin{bmatrix} 0 & 0 & 1 \\ \end{bmatrix}^{T} e3∧=[001]T。而 ε \varepsilon ε可以为很小的值，比如 ε = [ dx dy dz ω T ] T \varepsilon = \begin{bmatrix} \text{dx} & \text{dy} & \text{dz} & \omega^{T} \\ \end{bmatrix}^{T} ε=[dxdydzωT]T。
6. ⨂ \mathbf{\bigotimes} ⨂(Kronecker operator)的含义
设现有矩阵A和B，其纬度分别是 M A × N A M_{A} \times N_{A} MA×NA和 M B × N B M_{B} \times N_{B} MB×NB，那么其进行 ⨂ \bigotimes ⨂运算的结果是一个 M A M B × N B N A {M_{A}M}_{B} \times N_{B}N_{A} MAMB×NBNA的矩阵，表示为：
A ⨂ B = [ a 11 B a 12 B a 13 B ⋯ a 21 B a 22 B a 23 B ⋯ ⋯ ] (21) \begin{matrix} A\bigotimes B = \begin{bmatrix} a_{11}\text{B\ }a_{12}\text{B\ }a_{13}B\ \cdots \\ a_{21}\text{B\ }a_{22}\text{B\ }a_{23}B\ \cdots \\ \cdots \\ \end{bmatrix} \end{matrix} \tag{21} A⨂B=⎣⎡a11B a12B a13B ⋯a21B a22B a23B ⋯⋯⎦⎤(21)
其中， A = [ a 11 a 12 a 13 ⋯ a 21 a 22 a 23 ⋯ ⋯ ] A = \begin{bmatrix} a_{11}\ a_{12}\ a_{13}\ \cdots \\ a_{21}\ a_{22}\ a_{23}\ \cdots \\ \cdots \\ \end{bmatrix} A=⎣⎡a11 a12 a13 ⋯a21 a22 a23 ⋯⋯⎦⎤
7.Pose-Pose的Jacobian
现有 p 1 p_{1} p1和 p 2 p_{2} p2两个位姿，那么位姿的叠加是 p 1 ⨁ p 2 p_{1}\bigoplus p_{2} p1⨁p2，也可以表示成 f ⨁ ( p 1 , p 2 ) ∈ S E ( 3 ) × S E ( 3 ) ↦ S E ( 3 ) f_{\bigoplus}(p_{1},p_{2}) \in SE(3) \times SE(3) \mapsto SE(3) f⨁(p1,p2)∈SE(3)×SE(3)↦SE(3)，即两个 S E ( 3 ) SE(3) SE(3)叠加之后成为新的 S E ( 3 ) SE(3) SE(3)，现在要利用 f ⨁ ( p 1 , p 2 ) f_{\bigoplus}(p_{1},p_{2}) f⨁(p1,p2)对 p 1 p_{1} p1求导，则表示为：
∂ f ⨁ ( p 1 , p 2 ) ∂ p 1 (22) \begin{matrix} \frac{\partial f_{\bigoplus}\left( p_{1},p_{2} \right)}{\partial p_{1}} \end{matrix} \tag{22} ∂p1∂f⨁(p1,p2)(22)
该表达式想表达什么呢？假如 p i p_{i} pi是标量，那该表达式就变成简单的一维求导公式。但现在 p i p_{i} pi是向量，那么该表达式变成Jacobian矩阵。实际上，任何不可逆的 4 × 4 4 \times 4 4×4矩阵 T T T都属于流形 T ∈ G L ( 4 , R ) T \in GL(4,\mathbb{R)} T∈GL(4,R)，可理解为任何不可逆的变换矩阵 T = [ ] ∈ G L ( 4 , R ) T = \left\lbrack \right\rbrack \in GL(4,\mathbb{R)} T=[]∈GL(4,R)。既然公式(23)也是位姿，那我们需要用参数来展开它以便理解它。一般来说，我们很容易理解，位姿可以用矩阵的形式来表达，那么在处理矩阵导数的时候，我们都会隐含地假定一个事实，就是所有涉及的矩阵实际上都是由向量展开的，这也表明矩阵的导数都是标准的Jacobian矩阵。注意到一点，在我们用矩阵描述刚体运动的时候，我们只用到 3 × 4 3 \times 4 3×4的部分（即R和t，下面的 0 3 × 3 0_{3 \times 3} 03×3和1都不用）。
那么，在位姿求导的公式(23)中，位姿用
4
×
4
4 \times 4
4×4矩阵来表达。
但当它用向量展开之后，最后一行是被忽略的。位姿变成12个向量。虽然这表明一个6自由度的实体过度参数化了，但通过这样的表达，很多重要的步骤变成线性的了，使得我们可以更加有效地获取精确的导数。我们用T代替p，则公式(23)表示成：
∂ f ⨁ ( p 1 , p 2 ) ∂ p 1 = ∂ f ⨁ ( T 1 , T 2 ) ∂ T 1 (23) \begin{matrix} \frac{\partial f_{\bigoplus}\left( p_{1},p_{2} \right)}{\partial p_{1}} = \frac{\partial f_{\bigoplus}\left( T_{1},T_{2} \right)}{\partial T_{1}} \end{matrix} \tag{23} ∂p1∂f⨁(p1,p2)=∂T1∂f⨁(T1,T2)(23)
其结果可表示成：
M = (24) \begin{matrix} M = \end{matrix} \tag{24} M=(24)
把f设为F，根据上式则有：
∂ f ⨁ ( p 1 , p 2 ) ∂ p 1 = ∂ F ( P , Q ) ∂ P = ∂ v e c ( F ( P , Q ) ) ∂ v e c ( P ) = ∂ [ f 11 f 21 f 31 f 12 f 22 ⋯ f 33 f 14 f 24 f 34 ] ∂ [ p 11 p 21 p 31 p 12 p 22 ⋯ p 33 p 14 p 24 p 34 ] = [ ∂ f 11 ∂ p 11 ⋯ ∂ f 11 ∂ p 34 ⋮ ⋱ ⋮ ∂ f 34 ∂ p 11 ⋯ ∂ f 34 ∂ p 34 ] 12 × 12 (25) \begin{matrix} \frac{\partial f_{\bigoplus}\left( p_{1},p_{2} \right)}{\partial p_{1}} = \frac{\partial F\left( P,Q \right)}{\partial P} = \frac{\partial vec\left( F\left( P,Q \right) \right)}{\partial vec\left( P \right)} \\ = \frac{\partial\left\lbrack f_{11}f_{21}f_{31}f_{12}f_{22}\cdots f_{33}f_{14}f_{24}f_{34} \right\rbrack}{\partial\left\lbrack p_{11}p_{21}p_{31}p_{12}p_{22}\cdots p_{33}p_{14}p_{24}p_{34} \right\rbrack} \\ = \begin{bmatrix} \frac{\partial f_{11}}{\partial p_{11}} & \cdots & \frac{\partial f_{11}}{\partial p_{34}} \\ \vdots & \ddots & \vdots \\ \frac{\partial f_{34}}{\partial p_{11}} & \cdots & \frac{\partial f_{34}}{\partial p_{34}} \\ \end{bmatrix}_{12 \times 12} \end{matrix} \tag{25} ∂p1∂f⨁(p1,p2)=∂P∂F(P,Q)=∂vec(P)∂vec(F(P,Q))=∂[p11p21p31p12p22⋯p33p14p24p34]∂[f11f21f31f12f22⋯f33f14f24f34]=⎣⎢⎢⎡∂p11∂f11⋮∂p11∂f34⋯⋱⋯∂p34∂f11⋮∂p34∂f34⎦⎥⎥⎤12×12(25)
根据以上理解，我们计算 ∂ f ⨁ ( p 1 , p 2 ) \partial f_{\bigoplus}\left( p_{1},p_{2} \right) ∂f⨁(p1,p2)的偏导：
∂ f ⨁ ( p 1 , p 2 ) ∂ p 1 = ∂ f ⨁ ( T 1 , T 2 ) ∂ T 1 = T 2 T ⨂ I 3 (26) \begin{matrix} \frac{\partial f_{\bigoplus}\left( p_{1},p_{2} \right)}{\partial p_{1}} = \frac{\partial f_{\bigoplus}\left( T_{1},T_{2} \right)}{\partial T_{1}} = {T_{2}}^{T}\bigotimes I_{3} \end{matrix}\tag{26} ∂p1∂f⨁(p1,p2)=∂T1∂f⨁(T1,T2)=T2T⨂I3(26)
∂ f ⨁ ( p 1 , p 2 ) ∂ p 2 = ∂ f ⨁ ( T 1 , T 2 ) ∂ T 2 = I 4 ⨂ R A (27) \begin{matrix} \frac{\partial f_{\bigoplus}\left( p_{1},p_{2} \right)}{\partial p_{2}} = \frac{\partial f_{\bigoplus}\left( T_{1},T_{2} \right)}{\partial T_{2}} = {I_{4}\bigotimes R}_{A} \end{matrix}\tag{27} ∂p2∂f⨁(p1,p2)=∂T2∂f⨁(T1,T2)=I4⨂RA(27)
记住公式(26)，待会要用来计算左乘模型的Jacobian矩阵。
8. D ⊞ ε = e ε ⨁ D \mathbf{D \boxplus \varepsilon =}\mathbf{e}^{\mathbf{\varepsilon}}\mathbf{\bigoplus D} D⊞ε=eε⨁D的Jacobian
假设现在有位姿 D ∈ S E ( 3 ) D \in SE(3) D∈SE(3)，那么其变换矩阵T为：
T ( D ) = [ ] (28) \begin{matrix} T\left( D \right) = \left\lbrack \right\rbrack \end{matrix}\tag{28} T(D)=[](28)
我们利用左乘的扰动模型，可知我们的问题描述为对 e ε ⨁ D \mathbf{e}^{\mathbf{\varepsilon}}\mathbf{\bigoplus D} eε⨁D的结果进行求导，则位姿对扰动的偏导数表示为：
∂ e ε D ∂ ε ∣ ε = 0 = ∂ A D ∂ A ∣ A = I 4 = e ε ⋅ ∂ e ε ∂ ε ∣ ε = 0 (29) \begin{matrix} \left. \ \frac{\partial e^{\varepsilon}D}{\partial\varepsilon} \right|_{\varepsilon = 0} = \left. \ \frac{\partial AD}{\partial A} \right|_{A = I_{4} = e^{\varepsilon}} \cdot \left. \ \frac{\partial e^{\varepsilon}}{\partial\varepsilon} \right|_{\varepsilon = 0} \end{matrix}\tag{29} ∂ε∂eεD∣∣ε=0= ∂A∂AD∣∣A=I4=eε⋅ ∂ε∂eε∣∣ε=0(29)
其中，根据公式(26)可得：
∂ A D ∂ A ∣ A = I 4 = e ε = T ( D ) T ⨂ I 3 (30) \begin{matrix} \left. \ \frac{\partial AD}{\partial A} \right|_{A = I_{4} = e^{\varepsilon}} = {T\left( D \right)}^{T}\bigotimes I_{3} \end{matrix}\tag{30} ∂A∂AD∣∣A=I4=eε=T(D)T⨂I3(30)
根据公式(20)可得：
∂ e ε ∂ ε ∣ ε = 0 ≡ ∂ v e c ( e ε ) ∂ ε ∣ ε = 0 = [ 0 3 × 3 − e 1 ∧ 0 3 × 3 − e 2 ∧ 0 3 × 3 − e 3 ∧ I 3 × 3 0 3 × 3 ] \left. \ \frac{\partial e^{\varepsilon}}{\partial\varepsilon} \right|_{\varepsilon = 0} \equiv \left. \ \frac{\partial{vec(e}^{\varepsilon})}{\partial\varepsilon} \right|_{\varepsilon = 0} = \begin{bmatrix} 0_{3 \times 3} & - e_{1}^{\land} \\ 0_{3 \times 3} & - e_{2}^{\land} \\ 0_{3 \times 3} & - e_{3}^{\land} \\ I_{3 \times 3} & 0_{3 \times 3} \\ \end{bmatrix} ∂ε∂eε∣∣∣∣ε=0≡ ∂ε∂vec(eε)∣∣∣∣ε=0=⎣⎢⎢⎡03×303×303×3I3×3−e1∧−e2∧−e3∧03×3⎦⎥⎥⎤
所以，最终公式(29)结果是：
∂ e ε D ∂ ε ∣ ε = 0 = [ T ( D ) T ⨂ I 3 ] ⋅ [ 0 3 × 3 − e 1 ∧ 0 3 × 3 − e 2 ∧ 0 3 × 3 − e 3 ∧ I 3 × 3 0 3 × 3 ] = [ 0 3 × 3 − d c 1 ∧ 0 3 × 3 − d c 2 ∧ 0 3 × 3 − d c 3 ∧ I 3 − d t ∧ ] (31) \begin{matrix} \left. \ \frac{\partial e^{\varepsilon}D}{\partial\varepsilon} \right|_{\varepsilon = 0} = \left\lbrack {T\left( D \right)}^{T}\bigotimes I_{3} \right\rbrack \cdot \begin{bmatrix} 0_{3 \times 3} & - e_{1}^{\land} \\ 0_{3 \times 3} & - e_{2}^{\land} \\ 0_{3 \times 3} & - e_{3}^{\land} \\ I_{3 \times 3} & 0_{3 \times 3} \\ \end{bmatrix} = \begin{bmatrix} 0_{3 \times 3} & - d_{c1}^{\land} \\ 0_{3 \times 3} & - d_{c2}^{\land} \\ 0_{3 \times 3} & - d_{c3}^{\land} \\ I_{3} & - d_{t}^{\land} \\ \end{bmatrix} \end{matrix}\tag{31} ∂ε∂eεD∣∣ε=0=[T(D)T⨂I3]⋅⎣⎢⎢⎡03×303×303×3I3×3−e1∧−e2∧−e3∧03×3⎦⎥⎥⎤=⎣⎢⎢⎡03×303×303×3I3−dc1∧−dc2∧−dc3∧−dt∧⎦⎥⎥⎤(31)
该公式接下用来计算重投影误差函数 f ( X i , ξ ) f\left( X_{i},\xi \right) f(Xi,ξ)对相机位姿 ( ξ ) \left( \xi \right) (ξ)里面的式子。
9.Pose-Point的Jacobian
假设现有坐标点 p ∈ R 3 p \in \mathbb{R}^{3} p∈R3，其位姿是 D ( R , t ) ∈ S E ( 3 ) D(R,t) \in SE(3) D(R,t)∈SE(3)，则重投影 g ⨁ ( D , p ) g_{\bigoplus}\left( D,p \right) g⨁(D,p)之后，其对位姿和点的偏导为：
∂ g ⨁ ( D , p ) ∂ p = ∂ D ⨁ p ∂ p = ∂ ( R p + t ) ∂ p = R (32) \begin{matrix} \frac{\partial g_{\bigoplus}\left( D,p \right)}{\partial p} = \frac{\partial D\bigoplus p}{\partial p} = \frac{\partial(Rp + t)}{\partial p} = R \end{matrix}\tag{32} ∂p∂g⨁(D,p)=∂p∂D⨁p=∂p∂(Rp+t)=R(32)
∂ g ⨁ ( D , p ) ∂ D = ∂ D ⨁ p ∂ D = ( p T 1 ) ⨂ I 3 (33) \begin{matrix} \frac{\partial g_{\bigoplus}\left( D,p \right)}{\partial D} = \frac{\partial D\bigoplus p}{\partial D} = {(p^{T}\ 1)\bigotimes I}_{3} \end{matrix}\tag{33} ∂D∂g⨁(D,p)=∂D∂D⨁p=(pT 1)⨂I3(33)
10. e ε ⨁ D ⨁ p \mathbf{e}^{\mathbf{\varepsilon}}\mathbf{\bigoplus D\bigoplus}\mathbf{p} eε⨁D⨁p的Jacobian
假设现有坐标点 p ∈ R 3 p \in \mathbb{R}^{3} p∈R3，其位姿是 D ∈ S E ( 3 ) D \in SE(3) D∈SE(3)，那么其变换矩阵是：
T ( D ) = [ ] (34) \begin{matrix} T\left( D \right) = \left\lbrack \right\rbrack \end{matrix}\tag{34} T(D)=[](34)
该问题则转换成重投影误差函数对位姿的求导，利用公式(33)和(31)，结果为：
∂ ( e ε D ) ⨁ p ∂ ε ∣ ε = 0 = ∂ A ⨁ p ∂ A ∣ A = e ε D = D ⋅ ∂ e ε D ∂ ε ∣ ε = 0 = ∂ D ⨁ p ∂ D ⋅ ∂ e ε D ∂ ε ∣ ε = 0 \left. \ \frac{\partial\left( e^{\varepsilon}D \right)\bigoplus p}{\partial\varepsilon} \right|_{\varepsilon = 0} = \left. \ \frac{\partial A\bigoplus p}{\partial A} \right|_{A = e^{\varepsilon}D = D}{\cdot \left. \ \frac{\partial e^{\varepsilon}D}{\partial\varepsilon} \right|}_{\varepsilon = 0} = \frac{\partial D\bigoplus p}{\partial D}{\cdot \left. \ \frac{\partial e^{\varepsilon}D}{\partial\varepsilon} \right|}_{\varepsilon = 0} ∂ε∂(eεD)⨁p∣∣∣∣ε=0= ∂A∂A⨁p∣∣∣∣A=eεD=D⋅ ∂ε∂eεD∣∣∣∣ε=0=∂D∂D⨁p⋅ ∂ε∂eεD∣∣∣∣ε=0
= ( ( p T 1 ) ⨂ I 3 ) ⋅ [ 0 3 × 3 − d c 1 ∧ 0 3 × 3 − d c 2 ∧ 0 3 × 3 − d c 3 ∧ I 3 − d t ∧ ] = \left( {\left( p^{T}\ 1 \right)\bigotimes I}_{3} \right) \cdot \begin{bmatrix} 0_{3 \times 3} & - d_{c1}^{\land} \\ 0_{3 \times 3} & - d_{c2}^{\land} \\ 0_{3 \times 3} & - d_{c3}^{\land} \\ I_{3} & - d_{t}^{\land} \\ \end{bmatrix} =((pT 1)⨂I3)⋅⎣⎢⎢⎡03×303×303×3I3−dc1∧−dc2∧−dc3∧−dt∧⎦⎥⎥⎤
= ( I 3 − [ D ⨁ p ] ∧ ) ( A 3 × 6 J a c o b i a n ) (35) \begin{matrix} = \left( I_{3}\ - \ \left\lbrack D\bigoplus p \right\rbrack^{\land} \right)\ \ (A\ 3 \times 6\ Jacobian) \end{matrix}\tag{35} =(I3 − [D⨁p]∧) (A 3×6 Jacobian)(35)
四、推导
1.针孔相机的投影函数
假设针孔相机的内参矩阵是K，则有：
K = [ f x 0 c x 0 f y c y 0 0 1 ] (36) \begin{matrix} K = \begin{bmatrix} f_{x} & 0 & c_{x} \\ 0 & f_{y} & c_{y} \\ 0 & 0 & 1 \\ \end{bmatrix} \end{matrix} \tag{36} K=⎣⎡fx000fy0cxcy1⎦⎤(36)
有投影函数 h : R 3 ↦ R 2 h:\mathbb{R}^{3} \mapsto \mathbb{R}^{2} h:R3↦R2，点 p = [ p x p y p z ] T p = {\lbrack p_{x}\ p_{y}\ p_{z}\rbrack}^{T} p=[px py pz]T在相机上的像素坐标 ( u , v ) (u,v) (u,v)是：
h ( p ) = h ( p x p y p z ) = ( c x + f x p x p z c y + f y p y p z ) (37) \begin{matrix} h\left( p \right) = h\begin{pmatrix} p_{x} \\ p_{y} \\ p_{z} \\ \end{pmatrix} = \begin{pmatrix} c_{x} + \frac{f_{x}p_{x}}{p_{z}} \\ c_{y} + \frac{f_{y}p_{y}}{p_{z}} \\ \end{pmatrix} \end{matrix}\tag{37} h(p)=h⎝⎛pxpypz⎠⎞=(cx+pzfxpxcy+pzfypy)(37)
投影函数h对点p求导：
∂ h ( p ) ∂ p = [ f x p z 0 − f x p x p z 2 0 f y p z − f y p y p z 2 ] (38) \begin{matrix} \frac{\partial h\left( p \right)}{\partial p} = \begin{bmatrix} \frac{f_{x}}{p_{z}} & 0 & - \frac{f_{x}p_{x}}{{p_{z}}^{2}} \\ 0 & \frac{f_{y}}{p_{z}} & - \frac{f_{y}p_{y}}{{p_{z}}^{2}} \\ \end{bmatrix} \end{matrix}\tag{38} ∂p∂h(p)=[pzfx00pzfy−pz2fxpx−pz2fypy](38)
2.三维坐标点p重投影函数的偏导数
上述的点p重投影问题可描述为：已知位姿 A ∈ S E ( 3 ) A \in SE(3) A∈SE(3)，和一些与该位姿相关的坐标点 p ∈ R 3 p \in \mathbb{R}^{3} p∈R3（即可通过该位姿投影之后得到对应的像素点 p ′ = A ⨁ p p^{'} = A\bigoplus p p′=A⨁p，称之为我们估计值，实际值则是相机图片上我们提取出来的像素点 p o p_{o} po），那么该问题变成函数 h ( e ε ⨁ A ⨁ p ) h(e^{\varepsilon}\bigoplus A\bigoplus p) h(eε⨁A⨁p)，要解非线性解，就要对该函数进行求导，由函数变量可知我们需要对A和p进行求导，首先对p求导，则：
∂ h ( e ε ⨁ A ⨁ p ) ∂ p = ∂ h ( p ′ ) ∂ p ′ ∣ p ′ = A ⨁ p = g ⋅ ∂ e ε ⨁ A ⨁ p ∂ p \frac{\partial h\left( e^{\varepsilon}\bigoplus A\bigoplus p \right)}{\partial p} = \left. \ \frac{\partial h\left( p^{'} \right)}{\partial p^{'}} \right|_{p^{'} = A\bigoplus p = g} \cdot \frac{\partial e^{\varepsilon}\bigoplus A\bigoplus p}{\partial p} ∂p∂h(eε⨁A⨁p)= ∂p′∂h(p′)∣∣∣∣∣∣p′=A⨁p=g⋅∂p∂eε⨁A⨁p
= ∂ h ( p ′ ) ∂ p ′ ∣ p ′ = A ⨁ p = g ⋅ ∂ A ⨁ p ∂ p (39) \begin{matrix} = \left. \ \frac{\partial h\left( p^{'} \right)}{\partial p^{'}} \right|_{p^{'} = A\bigoplus p = g} \cdot \frac{\partial A\bigoplus p}{\partial p} \end{matrix}\tag{39} = ∂p′∂h(p′)∣∣∣∣p′=A⨁p=g⋅∂p∂A⨁p(39)
根据公式(38)和(32)，由(39)可得：
∂ h ( e ε ⨁ A ⨁ p ) ∂ p = [ f x g z 0 − f x g x g z 2 0 f y g z − f y g y g z 2 ] ⋅ R A ( A 2 × 3 J a c o b i a n ) (40) \begin{matrix} \frac{\partial h\left( e^{\varepsilon}\bigoplus A\bigoplus p \right)}{\partial p} = \begin{bmatrix} \frac{f_{x}}{g_{z}} & 0 & - \frac{f_{x}g_{x}}{{g_{z}}^{2}} \\ 0 & \frac{f_{y}}{g_{z}} & - \frac{f_{y}g_{y}}{{g_{z}}^{2}} \\ \end{bmatrix} \cdot R_{A}\ \ (A\ 2 \times 3\ Jacobian) \end{matrix}\tag{40} ∂p∂h(eε⨁A⨁p)=[gzfx00gzfy−gz2fxgx−gz2fygy]⋅RA (A 2×3 Jacobian)(40)
重投影误差函数对坐标点的偏导数最终表达式已求得。再对位姿A求导，则：
∂ h ( e ε ⨁ A ⨁ p ) ∂ ε = ∂ h ( p ′ ) ∂ p ′ ∣ p ′ = A ⨁ p = g ⋅ ∂ e ε ⨁ A ⨁ p ∂ ε (41) \begin{matrix} \frac{\partial h\left( e^{\varepsilon}\bigoplus A\bigoplus p \right)}{\partial\varepsilon} = \left. \ \frac{\partial h\left( p^{'} \right)}{\partial p^{'}} \right|_{p^{'} = A\bigoplus p = g} \cdot \frac{\partial e^{\varepsilon}\bigoplus A\bigoplus p}{\partial\varepsilon} \end{matrix}\tag{41} ∂ε∂h(eε⨁A⨁p)= ∂p′∂h(p′)∣∣∣∣p′=A⨁p=g⋅∂ε∂eε⨁A⨁p(41)
根据公式(35)，由(41)可得：
∂ h ( e ε ⨁ A ⨁ p ) ∂ ε = [ f x g z 0 − f x g x g z 2 0 f y g z − f y g y g z 2 ] ⋅ ( I 3 − [ A ⨁ p ] ∧ ) \frac{\partial h\left( e^{\varepsilon}\bigoplus A\bigoplus p \right)}{\partial\varepsilon} = \begin{bmatrix} \frac{f_{x}}{g_{z}} & 0 & - \frac{f_{x}g_{x}}{{g_{z}}^{2}} \\ 0 & \frac{f_{y}}{g_{z}} & - \frac{f_{y}g_{y}}{{g_{z}}^{2}} \\ \end{bmatrix} \cdot \left( I_{3}\ - \ \left\lbrack A\bigoplus p \right\rbrack^{\land} \right) ∂ε∂h(eε⨁A⨁p)=[gzfx00gzfy−gz2fxgx−gz2fygy]⋅(I3 − [A⨁p]∧)
= [ f x g z 0 − f x g x g z 2 0 f y g z − f y g y g z 2 ] ⋅ ( I 3 − [ g ] ∧ ) = \begin{bmatrix} \frac{f_{x}}{g_{z}} & 0 & - \frac{f_{x}g_{x}}{{g_{z}}^{2}} \\ 0 & \frac{f_{y}}{g_{z}} & - \frac{f_{y}g_{y}}{{g_{z}}^{2}} \\ \end{bmatrix} \cdot \left( I_{3}\ - \ \left\lbrack g \right\rbrack^{\land} \right) =[gzfx00gzfy−gz2fxgx−gz2fygy]⋅(I3 − [g]∧)
∂ h ( e ε ⨁ A ⨁ p ) ∂ ε = [ f x g z 0 − f x g x g z 2 − f x g x g y g z 2 f x ( 1 + g x 2 g z 2 ) − f x g y g z 0 f y g z − f y g y g z 2 − f y ( 1 + g y 2 g z 2 ) f y g x g y g z 2 f y g x g z ] ( A 2 × 6 J a c o b i a n ) (42) \begin{matrix} \frac{\partial h\left( e^{\varepsilon}\bigoplus A\bigoplus p \right)}{\partial\varepsilon} = \begin{bmatrix} \frac{f_{x}}{g_{z}} & 0 & - \frac{f_{x}g_{x}}{{g_{z}}^{2}} & - \frac{f_{x}g_{x}g_{y}}{{g_{z}}^{2}} & f_{x}\left( 1 + \frac{{g_{x}}^{2}}{{g_{z}}^{2}} \right) & - \frac{f_{x}g_{y}}{g_{z}} \\ 0 & \frac{f_{y}}{g_{z}} & - \frac{f_{y}g_{y}}{{g_{z}}^{2}} & - f_{y}\left( 1 + \frac{{g_{y}}^{2}}{{g_{z}}^{2}} \right) & \frac{f_{y}g_{x}g_{y}}{{g_{z}}^{2}} & \frac{f_{y}g_{x}}{g_{z}} \\ \end{bmatrix}\ (A\ 2 \times 6\ Jacobian) \end{matrix}\tag{42} ∂ε∂h(eε⨁A⨁p)=⎣⎡gzfx00gzfy−gz2fxgx−gz2fygy−gz2fxgxgy−fy(1+gz2gy2)fx(1+gz2gx2)gz2fygxgy−gzfxgygzfygx⎦⎤ (A 2×6 Jacobian)(42)
函数对位姿的偏导数最终表达式已求得。
至此，函数的偏导数已求取完毕，分别是公式(40)和(42)。
五、g2o应用
这部分在g2o文件夹里面。现有外参 R , t R,t R,t，用李代数 ξ \xi ξ表示，像素坐标 ( u i , v i ) \left( u_{i},v_{i} \right) (ui,vi)用 z z z表示，三维世界的路标点 p = [ x , y , z ] T p = {\lbrack x,y,z\rbrack}^{T} p=[x,y,z]T，用 p p p表示，那么误差函数为：
e = z − h ( ξ , p ) (43) \begin{matrix} e = z - h\left( \xi,p \right) \end{matrix}\tag{43} e=z−h(ξ,p)(43)
其中， h ( ) h() h()是上面讲的相机投影函数。
那么，利用公式(40)和(42)，误差对 ξ \xi ξ的偏导数是(注意前面有个负号)：
∂ e ∂ ξ = ∂ ( z − h ( ξ , p ) ) ∂ ξ = − ∂ h ( e ε ⨁ A ⨁ p ) ∂ ε = − [ f x g z 0 − f x g x g z 2 − f x g x g y g z 2 f x ( 1 + g x 2 g z 2 ) − f x g y g z 0 f y g z − f y g y g z 2 − f y ( 1 + g y 2 g z 2 ) f y g x g y g z 2 f y g x g z ] (44) \begin{matrix} \frac{\partial e}{\partial\xi} = \frac{\partial\left( z - h\left( \xi,p \right) \right)}{\partial\xi} = - \frac{\partial h\left( e^{\varepsilon}\bigoplus A\bigoplus p \right)}{\partial\varepsilon} = - \begin{bmatrix} \frac{f_{x}}{g_{z}} & 0 & - \frac{f_{x}g_{x}}{{g_{z}}^{2}} & - \frac{f_{x}g_{x}g_{y}}{{g_{z}}^{2}} & f_{x}\left( 1 + \frac{{g_{x}}^{2}}{{g_{z}}^{2}} \right) & - \frac{f_{x}g_{y}}{g_{z}} \\ 0 & \frac{f_{y}}{g_{z}} & - \frac{f_{y}g_{y}}{{g_{z}}^{2}} & - f_{y}\left( 1 + \frac{{g_{y}}^{2}}{{g_{z}}^{2}} \right) & \frac{f_{y}g_{x}g_{y}}{{g_{z}}^{2}} & \frac{f_{y}g_{x}}{g_{z}} \\ \end{bmatrix} \end{matrix}\tag{44} ∂ξ∂e=∂ξ∂(z−h(ξ,p))=−∂ε∂h(eε⨁A⨁p)=−⎣⎡gzfx00gzfy−gz2fxgx−gz2fygy−gz2fxgxgy−fy(1+gz2gy2)fx(1+gz2gx2)gz2fygxgy−gzfxgygzfygx⎦⎤(44)
误差对 p p p的偏导数是(注意前面有个负号)：
∂ e ∂ p = ∂ ( z − h ( ξ , p ) ) ∂ p = − ∂ h ( e ε ⨁ A ⨁ p ) ∂ p = − [ f x g z 0 − f x g x g z 2 0 f y g z − f y g y g z 2 ] ⋅ R A (45) \begin{matrix} \frac{\partial e}{\partial p} = \frac{\partial\left( z - h\left( \xi,p \right) \right)}{\partial p} = - \frac{\partial h\left( e^{\varepsilon}\bigoplus A\bigoplus p \right)}{\partial p} = - \begin{bmatrix} \frac{f_{x}}{g_{z}} & 0 & - \frac{f_{x}g_{x}}{{g_{z}}^{2}} \\ 0 & \frac{f_{y}}{g_{z}} & - \frac{f_{y}g_{y}}{{g_{z}}^{2}} \\ \end{bmatrix} \cdot R_{A} \end{matrix}\tag{45} ∂p∂e=∂p∂(z−h(ξ,p))=−∂p∂h(eε⨁A⨁p)=−[gzfx00gzfy−gz2fxgx−gz2fygy]⋅RA(45)
特别注意的是，g2o里面和上面推导公式的区别有两个地方：
-
统一用 f f f来表示 f x f_{x} fx和 f y f_{y} fy；
-
g2o旋转在前，平移在后，和公式(15)是反的。
因此，最终g2o应用的公式是：
∂ e ∂ ξ = − [ − f g x g y g z 2 f ( 1 + g x 2 g z 2 ) − f g y g z f g z 0 − f g x g z 2 − f ( 1 + g y 2 g z 2 ) f g x g y g z 2 f g x g z 0 f g z − f g y g z 2 ] (46) \begin{matrix} \frac{\partial e}{\partial\xi} = - \begin{bmatrix} -\frac{fg_{x}g_{y}}{{g_{z}}^{2}} & f\left( 1 + \frac{{g_{x}}^{2}}{{g_{z}}^{2}} \right) & - \frac{fg_{y}}{g_{z}} & \frac{f}{g_{z}} & 0 & - \frac{fg_{x}}{{g_{z}}^{2}} \\ -f\left( 1 + \frac{{g_{y}}^{2}}{{g_{z}}^{2}} \right) & \frac{fg_{x}g_{y}}{{g_{z}}^{2}} & \frac{fg_{x}}{g_{z}} & 0 & \frac{f}{g_{z}} & - \frac{fg_{y}}{{g_{z}}^{2}} \\ \end{bmatrix} \end{matrix}\tag{46} ∂ξ∂e=−⎣⎡−gz2fgxgy−f(1+gz2gy2)f(1+gz2gx2)gz2fgxgy−gzfgygzfgxgzf00gzf−gz2fgx−gz2fgy⎦⎤(46)
∂ e ∂ p = − [ f g z 0 − f g x g z 2 0 f g z − f g y g z 2 ] ⋅ R A (47) \begin{matrix} \frac{\partial e}{\partial p} = - \begin{bmatrix} \frac{f}{g_{z}} & 0 & - \frac{fg_{x}}{{g_{z}}^{2}} \\ 0 & \frac{f}{g_{z}} & - \frac{fg_{y}}{{g_{z}}^{2}} \\ \end{bmatrix} \cdot R_{A} \end{matrix}\tag{47} ∂p∂e=−[gzf00gzf−gz2fgx−gz2fgy]⋅RA(47)
六、ceres应用
这部分在ceres文件夹里面。之前ceres用的不多，总结一下其使用步骤：
-
构建cost fuction，即代价函数。
-
通过代价函数构建待求解的优化问题。
-
配置求解器参数并求解问题。
针对公式(43)提出的问题，定义一个类，里面包含有观测值和估计值，最重要的是误差计算（ceres里面常用重载运算符来实现），然后利用该类生成代价函数，最终求解问题。
这里要说一下，ceres例子用的是AutoDiffCostFunction，即自动计算导数，所以不用自己去求导。其他的看代码就清楚了。
七、Eigen应用
这部分在eigen文件夹里面。只用Eigen就比较痛苦了，数据结构、Jacobian、高斯牛顿等都要自己一步步理解和实现，也踩了一些坑，最后还是完成了，不过过多展开，具体看代码好了，有几点强调：
-
所有的实现都只优化了pose，没有优化point；
-
高斯牛顿的步骤要记牢，根据理论的步骤一步步来，不要贪心；
-
Jacobian和deltase3的纬度和对应关系要搞清，否则会晕，导致中途放弃；
-
求解出来的deltase3的旋转和平移顺序要注意，参考公式(15)，否则会导致错误的结果。
八、总结
总体来说，g2o比较经典，容易让人理解，但有一定工程量，性能也不如ceres。Ceres实现起来最方便，不用过多关注细节，可快速开发。手写工程量就很大，性能最差，但可以让人上手，加深对BA的理解。本文所有例子代码参见https://github.com/shanpenghui/BA_exercise.git，代码中的refs文件夹有关于非线性优化库的性能指标的一些论文，感兴趣的可自行查阅。
1000
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
