Direct Sparse Odometry (DSO) 初始化过程中的导数推导

---

---

前言

DSO初始化通过高斯-牛顿法优化初始多帧与起始帧之间的光度误差，从而估计起始帧中点的深度以及相机位姿，为后续的里程计提供初始信息。在进行优化之前需要求解光度误差的雅克比矩阵等信息，因此本文对此进行相关记录。如理解有误，欢迎指教。

---

一、光度误差建模

首先根据DSO作者关于光度校准的工作，可以将相机像素强度值与辐照度（相机感知到的强度值）建模为：
I i ( x ) = G ( t i V ( x ) B i ( x ) ) \begin{equation} I_{i}(\mathbf{x})=G\left(t_{i} V(\mathbf{x}) B_{i}(\mathbf{x})\right) \end{equation} Ii​(x)=G(ti​V(x)Bi​(x))​​
上述公式中 G G G为非线性响应函数， V V V描述由于光学透镜导致图像中存在的晕影， B i B_i Bi​表示场景中的辐照度，可以理解为相机感光元件从环境中接受到的真实信息， I i I_{i} Ii​为相机产生图像的像素强度值，可以理解为为了便于人眼观看，进行相应处理后的信息。相关细节可以参考DSO光度标定博文和作者光度校准文章。
由于DSO对光度校准比较依赖，而并非所有相机或者数据集都进行了细致的光度校准，因此作者使用仿射变换对光度校准参数进行了相对粗略的建模，用以在一定程度上去除响应函数 G G G的影响，而 V V V不易建模，故暂时未考虑。因此可以将辐照度和图像像素强度值表示为
I i ( x ) = t i exp ⁡ a i + b i \begin{equation} {I_i}\left( x \right) = t_i{\exp ^{{a_i}}} + {b_i} \end{equation} Ii​(x)=ti​expai​+bi​​​
其中 t i t_i ti​为曝光时间。
记参考帧 i i i（host帧）图像的辐照度为 B i {B_i} Bi​，当前帧 j j j（target帧）图像的辐照度为 B j {B_j} Bj​，两坐标系之间的相对位姿变换的李群表示为 T i ∈ S E ( 3 ) {T_i} \in SE\left( 3 \right) Ti​∈SE(3) ,相应的李代数为 ξ i ∈ s e ( 3 ) {\xi _i} \in se\left( 3 \right) ξi​∈se(3)，定义能量函数为参考帧 B i {B_i} Bi​上的像素点 p p p的辐照度 B i {B_i} Bi​和其在当前帧 j j j上观测到的辐照度 B j {B_j} Bj​之间的差值的鲁棒核函数形式，具体定义如下：
E p j : = ∑ p ∈ N p w p ∥ ( I j [ q ] − b j ) − t j e a j t i e a i ( I i [ p ] − b i ) ∥ γ \begin{equation} {\tiny E_{{p} j}:=\sum_{{p} \in \mathcal{N}_{{p}}} w_{{p}}\left\|\left(I_{j}\left[{q}\right]-b_{j}\right)-\frac{t_{j} e^{a_{j}}}{t_{i} e^{a_{i}}}\left(I_{i}[{p}]-b_{i}\right)\right\|_{\gamma}} \end{equation} Epj​:=p∈Np​∑​wp​∥ ∥​(Ij​[q]−bj​)−ti​eai​tj​eaj​​(Ii​[p]−bi​)∥ ∥​γ​​​
推导过程中涉及公式（2）以及移项等简单操作，不作赘述。其中 ∥ ⋅ ∥ γ {\left\| \cdot \right\|_\gamma } ∥⋅∥γ​ 为Huber鲁棒核函数， 形式为： H ( r ) = w h r 2 ( 1 − w h / 2 ) {H}({r})={w}_{{h}} {r}^{2}\left(1-{w}_{{h}} / 2\right) H(r)=wh​r2(1−wh​/2)
w h = { 1 , ∣ r ∣ < σ σ / ∣ r ∣ , ∣ r ∣ > = σ \begin{equation} {w}_{{h}}=\left\{\begin{array}{ll} 1 & ,|{r}|<\sigma \\ \sigma /|{r}| & ,|{r}|>=\sigma \end{array}\right. \end{equation} wh​={1σ/∣r∣​,∣r∣<σ,∣r∣>=σ​​​
定义相对光度变换 e a j i = t j e a j t i e a i e ^ {a_{ji}}=\frac{t_{j} e^{a_{j}}}{t_{i} e^{a_{i}}} eaji​=ti​eai​tj​eaj​​， b j i = b j − e a j i b i b_{ji}=b_j- e^{a_{ji}}b_i bji​=bj​−eaji​bi​，表示从像素 i i i到像素 j j j的光度变换，则公式（3）可以表示为：
E p j : = ∑ p ∈ N p w p ∥ I j [ q ] − b j i − e a j i I i [ p ] ∥ γ \begin{equation} {\tiny E_{{p} j}:=\sum_{{p} \in \mathcal{N}_{{p}}} w_{{p}}\left\|I_{j}\left[{q}\right]-b_{ji}-e^{a_{ji}}I_{i}[{p}]\right\|_{\gamma}} \end{equation} Epj​:=p∈Np​∑​wp​∥ ∥​Ij​[q]−bji​−eaji​Ii​[p]∥ ∥​γ​​​
p p p为 i i i帧中的像素位置， q q q为 j j j帧中的像素位置，二者具有如下转换关系：
q = Π c ( R Π c − 1 ( p , d p ) + t ) \begin{equation} q=\Pi_{{c}}\left({R} \Pi_{{c}}^{-1}\left({p}, d_{{p}}\right)+{t}\right) \end{equation} q=Πc​(RΠc−1​(p,dp​)+t)​​
其中
[ R t 0 1 ] : = T j T i − 1 \begin{equation} \left[\begin{array}{cc} \mathbf{R} & \mathbf{t} \\ 0 & 1 \end{array}\right]:=\mathbf{T}_{j} \mathbf{T}_{i}^{-1} \end{equation} [R0​t1​]:=Tj​Ti−1​​​
表示 i i i到 j j j的变换矩阵， Π c \Pi_{{c}} Πc​相机坐标系到像素坐标系的变换；d_{{p}}表示逆深度，即 Z Z Z坐标的倒数，因为在远距离情况下，逆深度更加符合高斯分布，因此选择逆深度作为优化变量。同时，由于单个像素辐照度的区分度太小，因此DSO选择8个点的块构建光度误差函数，其中红色为当前点，绿点为附近选择的7个点，8个点共享的深度信息。

公式（3）中出现的 w p w_p wp​为权重，表示为
w p = c 2 c 2 + ∥ ∇ I ( p ) ∥ 2 2 \begin{equation} {w}_{{p}}=\frac{{c}^{2}}{{c}^{2}+\|\nabla {I}({p})\|_{2}^{2}} \end{equation} wp​=c2+∥∇I(p)∥22​c2​​​
上式可以降低高梯度点的权重值，但是在DSO代码中并没有见到相应部分（后续再仔细研读一下）。

---

二、光度误差的雅克比矩阵

为了使用高斯-牛顿法优化求解目标函数，需要求解残差相对状态量的导数。待优化的状态变量 x x x包括：两帧之间的相对位姿 ξ i j {\xi _{ij}} ξij​、相对光度参数 a j i a_{ji} aji​和 b j i b_{ji} bji​、 N N N个像素点在参考帧中的逆深度 p p p，一共 8 + N 8+N 8+N个参数。因此需要求取误差函数相对于上述状态的导数。
由于使用Huber核函数，根据公式（3）中能量函数形式，误差函数表示为
f ( x ) = w h r = w h ( I j ( q ) − e a j i I i ( p ) − b j i ) \begin{equation} {f}({x})=\sqrt{{w}_{{h}}}{r}=\sqrt{{w}_{{h}}}\left({I}_{j}\left(q\right)- e^{a_{ji}} {I}_{i}\left({p}\right)-{b_{ji}}\right) \end{equation} f(x)=wh​ ​r=wh​ ​(Ij​(q)−eaji​Ii​(p)−bji​)​​

1.误差对 相对光度参数 a j i a_{ji} aji​ 和 b j i b_{ji} bji​ 的导数

根据公式（9），误差 f ( x ) {f}({x}) f(x)相对光度参数的导数可以记为（简单的求导）
∂ f ( x ) ∂ a j i = − w h e a j i I i ( p ) \begin{equation} \frac{\partial {f}({x})}{\partial {a_{ji}}}=-\sqrt{{w}_{{h}}}e^{a_{ji}} {I}_{i}\left({p}\right) \end{equation} ∂aji​∂f(x)​=−wh​ ​eaji​Ii​(p)​​
∂ f ( x ) ∂ b j i = − w h \begin{equation} \frac{\partial {f}({x})}{\partial {b_{ji}}}=-\sqrt{{w}_{{h}}} \end{equation} ∂bji​∂f(x)​=−wh​ ​​​

2、误差对 位姿 ξ j i {\xi_{ji}} ξji​ 的导数

随后求取误差相对于位姿的导数 ∂ f ( x ) ∂ ξ j i \frac{\partial {f}({x})}{\partial {\xi_{ji}}} ∂ξji​∂f(x)​
根据链式法则可以得到：
∂ f ( x ) ∂ ξ j i = ∂ f ( x ) ∂ q ∂ q ∂ ξ j i \begin{equation} \frac{\partial f(x)}{\partial \xi_{ji}}=\frac{\partial f(x)}{\partial q} \frac{\partial q}{\partial \xi_{ji}} \end{equation} ∂ξji​∂f(x)​=∂q∂f(x)​∂ξji​∂q​​​
由于 ∂ f ( x ) ∂ q \frac{\partial f(x)}{\partial q} ∂q∂f(x)​可以方便求得：
∂ f ( x ) ∂ q = w h ∂ I j [ q ] ∂ q = w h [ g x g y ] \begin{equation} \frac{\partial f(x)}{\partial q}=\sqrt{w_{h}} \frac{\partial I_{j}\left[ q\right]}{\partial q}=\sqrt{w_{h}}\left[\begin{array}{l} g_{x} & g_{y} \end{array}\right] \end{equation} ∂q∂f(x)​=wh​ ​∂q∂Ij​[q]​=wh​ ​[gx​​gy​​]​​
其中， g x g_x gx​和 g y g_y gy​表示图像 I j I_j Ij​在 q q q 处的梯度。
接下来需要求取 ∂ q ∂ ξ j i \frac{\partial q}{\partial \xi_{ji}} ∂ξji​∂q​。将公式（6）展开可以得到
q = K d q ( R j i d p − 1 K − 1 p + t j i ) \begin{equation} q= K d_{q}\left(R_{ji} d_{p}^{-1}K^{-1} p+t_{ji}\right) \end{equation} q=Kdq​(Rji​dp−1​K−1p+tji​)​​
其中 p p p为 i i i帧图像中的像素点， q q q为 j j j帧图像中的对应像素点。令 q ′ = d q ( R j i d p − 1 K − 1 p + t j i ) q^\prime = d_{q}\left(R_{ji} d_{p}^{-1}K^{-1} p+t_{ji}\right) q′=dq​(Rji​dp−1​K−1p+tji​)，公式（14）可以改写为
q = K q ′ \begin{equation} q= K q^\prime \end{equation} q=Kq′​​
则使用链式法则可以得到：
∂ q ∂ ξ j i = ∂ q ∂ q ′ ∂ q ′ ∂ ξ j i \begin{equation} \frac{\partial q}{\partial \xi_{ji}}=\frac{\partial q}{\partial q^{\prime}} \frac{\partial q^{\prime}}{\partial \xi_{ji}} \end{equation} ∂ξji​∂q​=∂q′∂q​∂ξji​∂q′​​​
首先计算 ∂ q ∂ q ′ \frac{\partial q}{\partial q^{\prime}} ∂q′∂q​，将 q q q和 q ′ q^\prime q′写成齐次坐标形式为 q = [ u q , v q , 1 ] T q=\left[u_q, v_q, 1\right]^T q=[uq​,vq​,1]T， q ′ = [ x q ′ , y q ′ , 1 ] T q^\prime=\left[x_{q^\prime}, y_{q^\prime}, 1\right]^T q′=[xq′​,yq′​,1]T，公式（15）可以展开写成：
[ u q v q 1 ] = [ f x 0 c x 0 f y c y 0 0 1 ] [ x q ′ y q ′ 1 ] \begin{equation} \left[\begin{array}{c} u_{q} \\ v_{q} \\ 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_{q^{\prime}} \\ y_{q^{\prime}} \\ 1 \end{array}\right] \end{equation} ⎣ ⎡​uq​vq​1​⎦ ⎤​=⎣ ⎡​fx​00​0fy​0​cx​cy​1​⎦ ⎤​⎣ ⎡​xq′​yq′​1​⎦ ⎤​​​
可以得到：
∂ q ∂ q ′ = [ ∂ u q ∂ x q ′ ∂ u q ∂ y q ′ ∂ u q ∂ 1 ∂ v q ∂ x q ′ ∂ v q ∂ y q ′ ∂ v q ∂ 1 ∂ 1 ∂ x q ′ ∂ 1 ∂ y q ′ ∂ 1 ∂ 1 ] = [ f x 0 0 0 f y 0 0 0 0 ] \begin{equation} \frac{\partial q}{\partial q^{\prime}}=\left[\begin{array}{lll} \frac{\partial u_{q}}{\partial x_{q^{\prime}}} & \frac{\partial u_{q}}{\partial y_{q^{\prime}}} & \frac{\partial u_{q}}{\partial 1} \\ \frac{\partial v_{q}}{\partial x_{q^{\prime}}} & \frac{\partial v_{q}}{\partial y_{q^{\prime}}} & \frac{\partial v_{q}}{\partial 1} \\ \frac{\partial 1}{\partial x_{q^{\prime}}} & \frac{\partial 1}{\partial y_{q^{\prime}}} & \frac{\partial 1}{\partial 1} \end{array}\right]=\left[\begin{array}{ccc} f_{x} & 0 & 0 \\ 0 & f_{y} & 0 \\ 0 & 0 & 0 \end{array}\right] \end{equation} ∂q′∂q​=⎣ ⎡​∂xq′​∂uq​​∂xq′​∂vq​​∂xq′​∂1​​∂yq′​∂uq​​∂yq′​∂vq​​∂yq′​∂1​​∂1∂uq​​∂1∂vq​​∂1∂1​​⎦ ⎤​=⎣ ⎡​fx​00​0fy​0​000​⎦ ⎤​​​
然后计算 ∂ q ′ ∂ ξ j i \frac{\partial q^{\prime}}{\partial \xi_{ji}} ∂ξji​∂q′​，由于 q ′ = [ x q ′ , y q ′ , 1 ] T q^\prime=\left[x_{q^\prime}, y_{q^\prime}, 1\right]^T q′=[xq′​,yq′​,1]T， q ′ = d q ( R j i d p − 1 K − 1 p + t j i ) q^\prime = d_{q}\left(R_{ji} d_{p}^{-1}K^{-1} p+t_{ji}\right) q′=dq​(Rji​dp−1​K−1p+tji​)，令 Q = R j i d p − 1 K − 1 p + t j i = [ x , y , z ] T Q=R_{ji} d_{p}^{-1}K^{-1} p+t_{ji}=\left[x, y, z\right]^T Q=Rji​dp−1​K−1p+tji​=[x,y,z]T，则可以得到
∂ q ′ ∂ ξ j i = [ ∂ d q ∂ ξ j i x ∂ d q ∂ ξ j i y ∂ d q ∂ ξ j i z ] + d q [ ∂ x ∂ ξ j i ∂ y ∂ ξ j i ∂ z ∂ ξ j i ] = [ ∂ 1 z ∂ ξ j i x ∂ 1 z ∂ ξ j i y ∂ 1 z ∂ ξ j i z ] + d q [ ∂ x ∂ ξ j i ∂ y ∂ ξ j i ∂ z ∂ ξ j i ] \begin{equation} \frac{\partial q^{\prime}}{\partial \xi_{ji}} =\left[\begin{array}{c} \frac{\partial d_{q}}{\partial \xi_{ji}} x \\ \frac{\partial d_{q}}{\partial \xi_{ji}} y \\ \frac{\partial d_{q}}{\partial \xi_{ji}} z \end{array}\right]+d_{q} \left[\begin{array}{c} \frac{\partial x}{\partial \xi_{ji}} \\ \frac{\partial y}{\partial \xi_{ji}} \\ \frac{\partial z}{\partial \xi_{ji}} \end{array}\right]=\left[\begin{array}{c} \frac{\partial \frac{1}{z}}{\partial \xi_{ji}} x \\ \frac{\partial \frac{1}{z}}{\partial \xi_{ji}} y \\ \frac{\partial \frac{1}{z}}{\partial \xi_{ji}} z \end{array}\right]+d_{q} \left[\begin{array}{c} \frac{\partial x}{\partial \xi_{ji}} \\ \frac{\partial y}{\partial \xi_{ji}} \\ \frac{\partial z}{\partial \xi_{ji}} \end{array}\right] \end{equation} ∂ξji​∂q′​=⎣ ⎡​∂ξji​∂dq​​x∂ξji​∂dq​​y∂ξji​∂dq​​z​⎦ ⎤​+dq​⎣ ⎡​∂ξji​∂x​∂ξji​∂y​∂ξji​∂z​​⎦ ⎤​=⎣ ⎡​∂ξji​∂z1​​x∂ξji​∂z1​​y∂ξji​∂z1​​z​⎦ ⎤​+dq​⎣ ⎡​∂ξji​∂x​∂ξji​∂y​∂ξji​∂z​​⎦ ⎤​​​
根据扰动公式（具体推导过程参考《视觉SLAM十四讲（第二版）P85 4.3.5》）可以得到
[ ∂ x ∂ ξ j i ∂ y ∂ ξ j i ∂ z ∂ ξ j i ] = [ 1 0 0 0 z − y 0 1 0 − z 0 x 0 0 1 y − x 0 ] \begin{equation} \left[\begin{array}{c} \frac{\partial x}{\partial \xi_{ji}} \\ \frac{\partial y}{\partial \xi_{ji}} \\ \frac{\partial z}{\partial \xi_{ji}} \end{array}\right]=\left[\begin{array}{cccccc} 1 & 0 & 0 & 0 & z & -y \\ 0 & 1 & 0 & -z & 0 & x \\ 0 & 0 & 1 & y & -x & 0 \end{array}\right] \end{equation} ⎣ ⎡​∂ξji​∂x​∂ξji​∂y​∂ξji​∂z​​⎦ ⎤​=⎣ ⎡​100​010​001​0−zy​z0−x​−yx0​⎦ ⎤​​​
根据上式可以得到（可以理解为复合函数求导过程）：
∂ 1 z ∂ ξ j i = − 1 z 2 [ 0 0 1 y 2 − x 2 0 ] \begin{equation} \frac{\partial \frac{1}{z}}{\partial \xi_{ji}}=\frac{-1}{z^{2}}\left[\begin{array}{llllll} 0 & 0 & 1 & y_{2} & -x_{2} & 0 \end{array}\right] \end{equation} ∂ξji​∂z1​​=z2−1​[0​0​1​y2​​−x2​​0​]​​
综合公式（19）（20）（21）可以得到
∂ q ′ ∂ ξ j i = [ 0 0 − x z 2 − x y z 2 x 2 z 2 0 0 0 − y z 2 − y 2 z 2 x y z 2 0 0 0 − z z 2 − y z z 2 x z z 2 0 ] + [ 1 z 0 0 0 1 − y z 0 1 z 0 − 1 0 x z 0 0 1 z y z − x z 0 ] \begin{equation} \frac{\partial q^{\prime}}{\partial \xi_{ji}}=\begin{aligned} & {\left[\begin{array}{cccccc} 0 & 0 & -\frac{x}{z^{2}} & -\frac{x y}{z^{2}} & \frac{x^{2}}{z^{2}} & 0 \\ 0 & 0 & -\frac{y}{z^{2}} & -\frac{y^{2}}{z^{2}} & \frac{xy}{z^{2}} & 0 \\ 0 & 0 & -\frac{z}{z^{2}} & -\frac{y z}{z^{2}} & \frac{x z}{z^{2}} & 0 \end{array}\right] } +\left[\begin{array}{cccccc} \frac{1}{z} & 0 & 0 & 0 & 1 & -\frac{y}{z} \\ 0 & \frac{1}{z} & 0 & -1 & 0 & \frac{x}{z} \\ 0 & 0 & \frac{1}{z} & \frac{y}{z} & -\frac{x}{z} & 0 \end{array}\right] \end{aligned} \end{equation} ∂ξji​∂q′​=​⎣ ⎡​000​000​−z2x​−z2y​−z2z​​−z2xy​−z2y2​−z2yz​​z2x2​z2xy​z2xz​​000​⎦ ⎤​+⎣ ⎡​z1​00​0z1​0​00z1​​0−1zy​​10−zx​​−zy​zx​0​⎦ ⎤​​​​
整理可以得到：
∂ q ′ ∂ ξ j i = [ 1 z 0 − x z 2 − x y z 2 1 + x 2 z 2 − y z 0 1 z − y z 2 − ( y 2 z 2 + 1 ) x y z 2 x z 0 0 0 0 0 0 ] \begin{equation} \frac{\partial q^{\prime}}{\partial \xi_{ji}}=\begin{aligned} & {\left[\begin{array}{cccccc} \frac{1}{z} & 0 & -\frac{x}{z^{2}} & -\frac{x y}{z^{2}} & 1+\frac{x^{2}}{z^{2}} & -\frac{y}{z} \\ 0 & \frac{1}{z} & -\frac{y}{z^{2}} & -(\frac{y^{2}}{z^{2}}+1) & \frac{xy}{z^{2}} & \frac{x}{z} \\ 0 & 0 & 0 & 0 & 0 & 0 \end{array}\right] } \end{aligned} \end{equation} ∂ξji​∂q′​=​⎣ ⎡​z1​00​0z1​0​−z2x​−z2y​0​−z2xy​−(z2y2​+1)0​1+z2x2​z2xy​0​−zy​zx​0​⎦ ⎤​​​​
根据 z = 1 d q z=\frac{1}{d_q} z=dq​1​， y = d q v q y=d_qv_q y=dq​vq​， x = d q u q x=d_qu_q x=dq​uq​，可以得到
∂ q ′ ∂ ξ j i = [ d q 0 − d q u q − u q v q 1 + u q 2 − v q 0 d q − d q v q − ( 1 + v q 2 ) u q v q u q 0 0 0 0 0 0 ] \begin{equation} \frac{\partial q^{\prime}}{\partial \xi_{ji}}=\begin{aligned} & {\left[\begin{array}{cccccc} d_q & 0 & -d_qu_q & -u_qv_q& 1+u_q^2 & -v_q \\ 0 & d_q & -d_qv_q & -(1+v_q^2) & u_qv_q & u_q \\ 0 & 0 & 0 & 0 & 0 & 0 \end{array}\right] } \end{aligned} \end{equation} ∂ξji​∂q′​=​⎣ ⎡​dq​00​0dq​0​−dq​uq​−dq​vq​0​−uq​vq​−(1+vq2​)0​1+uq2​uq​vq​0​−vq​uq​0​⎦ ⎤​​​​
结合公式（12）（16）可以得到：
∂ f ( x ) ∂ ξ j i = w h [ g x g y 0 ] [ f x 0 0 0 f y 0 0 0 0 ] [ d q 0 − d q u q − u q v q 1 + u q 2 − v q 0 d q − d q v q − ( 1 + v q 2 ) u q v q u q 0 0 0 0 0 0 ] \begin{equation} \frac{\partial f(x)}{\partial \xi_{ji}}=\sqrt{w_{h}}\left[\begin{array}{l} g_{x} & g_{y}&0 \end{array}\right]\left[\begin{array}{ccc} f_{x} & 0 & 0 \\ 0 & f_{y} & 0 \\ 0 & 0 & 0 \end{array}\right] \\ \begin{aligned} & {\left[\begin{array}{cccccc} d_q & 0 & -d_qu_q & -u_qv_q& 1+u_q^2 & -v_q \\ 0 & d_q & -d_qv_q & -(1+v_q^2) & u_qv_q & u_q \\ 0 & 0 & 0 & 0 & 0 & 0 \end{array}\right] } \end{aligned} \end{equation} ∂ξji​∂f(x)​=wh​ ​[gx​​gy​​0​]⎣ ⎡​fx​00​0fy​0​000​⎦ ⎤​​⎣ ⎡​dq​00​0dq​0​−dq​uq​−dq​vq​0​−uq​vq​−(1+vq2​)0​1+uq2​uq​vq​0​−vq​uq​0​⎦ ⎤​​​​
上式整理可得：
∂ f ( x ) ∂ ξ j i = w h [ g x f x d q g y f y d q − g x f x d q u q − g y f y d q v q − g x f x u q v q − g y f y ( 1 + v q 2 ) g x f x ( 1 + u q 2 ) + g y f y u q v q − g x f x v q + g y f y u q ] T \begin{equation} \frac{\partial f(x)}{\partial \xi_{ji}}=\sqrt{w_{h}}\left[\begin{array}{c} g_{x} f_{x}d_q \\ g_{y} f_{y} d_q \\ -g_{x} f_{x} d_q u_q-g_{y} f_{y} d_q v_q \\ -g_{x} f_{x} u_q v_q-g_{y} f_{y}\left(1+{v_q}^2\right) \\ g_{x} f_{x}\left(1+{u_q}^2\right)+g_{y} f_{y} u_q v_q \\ -g_{x} f_{x} v_q+g_{y} f_{y} u_q \end{array}\right]^{T} \end{equation} ∂ξji​∂f(x)​=wh​ ​⎣ ⎡​gx​fx​dq​gy​fy​dq​−gx​fx​dq​uq​−gy​fy​dq​vq​−gx​fx​uq​vq​−gy​fy​(1+vq​2)gx​fx​(1+uq​2)+gy​fy​uq​vq​−gx​fx​vq​+gy​fy​uq​​⎦ ⎤​T​​

3、误差相对于逆深度 d p d_p dp​的导数

根据链式法则有：
∂ f ( x ) ∂ d p = ∂ f ( x ) ∂ q ∂ q ∂ d p \begin{equation} \frac{\partial f(x)}{\partial d_p}=\frac{\partial f(x)}{\partial q} \frac{\partial q}{\partial d_p} \end{equation} ∂dp​∂f(x)​=∂q∂f(x)​∂dp​∂q​​​
∂ f ( x ) ∂ q \frac{\partial f(x)}{\partial q} ∂q∂f(x)​可以由公式（13）得到，针对 ∂ q ∂ d p \frac{\partial q}{\partial d_p} ∂dp​∂q​，再次使用链式法则有：
∂ q ∂ d p = ∂ q ∂ q ′ ∂ q ′ ∂ d p \begin{equation} \frac{\partial q}{\partial d_p}=\frac{\partial q}{\partial q^\prime} \frac{\partial q^\prime}{\partial d_p} \end{equation} ∂dp​∂q​=∂q′∂q​∂dp​∂q′​​​
∂ q ∂ q ′ \frac{\partial q}{\partial q^\prime} ∂q′∂q​可以由公式（18）得到，根据 q ′ = [ x q ′ , y q ′ , 1 ] T q^\prime=\left[x_{q^\prime}, y_{q^\prime}, 1\right]^T q′=[xq′​,yq′​,1]T可以得到
∂ q ′ ∂ d p = [ ∂ x q ′ ∂ d p ∂ y q ′ ∂ d p 0 ] \begin{equation} \frac{\partial q^\prime}{\partial d_p}=\left[\begin{array}{c} \frac{\partial x_q^{\prime}}{\partial d_p} \\ \frac{\partial y_q^{\prime}}{\partial d_p} \\ 0 \end{array}\right] \end{equation} ∂dp​∂q′​=⎣ ⎡​∂dp​∂xq′​​∂dp​∂yq′​​0​⎦ ⎤​​​
根据 q ′ = d q ( R j i d p − 1 K − 1 p + t j i ) q^\prime = d_{q}\left(R_{ji} d_{p}^{-1}K^{-1} p+t_{ji}\right) q′=dq​(Rji​dp−1​K−1p+tji​)，定义 A = R j i K − 1 p = [ α 1 , α 2 , α 3 ] T A=R_{ji} K^{-1} p=\left[\alpha_1, \alpha_2, \alpha_3\right]^T A=Rji​K−1p=[α1​,α2​,α3​]T，则 q ′ q^\prime q′可以表示成：
q ′ = [ x q ′ y q ′ 1 ] = d q [ d p − 1 α 1 p + t j i x d p − 1 α 2 p + t j i y d p − 1 α 3 p + t j i z ] \begin{equation} q^\prime=\left[\begin{array}{c} x_{q^\prime} \\ y_{q^\prime}\\ 1 \end{array}\right]=d_q\left[\begin{array}{c} d_{p}^{-1}\alpha_1p + t_{ji}^x \\ d_{p}^{-1}\alpha_2p+ t_{ji}^y\\ d_{p}^{-1}\alpha_3p+ t_{ji}^z \end{array}\right] \end{equation} q′=⎣ ⎡​xq′​yq′​1​⎦ ⎤​=dq​⎣ ⎡​dp−1​α1​p+tjix​dp−1​α2​p+tjiy​dp−1​α3​p+tjiz​​⎦ ⎤​​​
根据上式可以得到：
d q = ( d p − 1 α 3 p + t j i z ) − 1 \begin{equation} d_q={\left(d_{p}^{-1}\alpha_3p+ t_{ji}^z\right)}^{-1} \end{equation} dq​=(dp−1​α3​p+tjiz​)−1​​
则可以得到：
x q ′ = d p − 1 α 1 p + t j i x d p − 1 α 3 p + t j i z = α 1 p + d p t j i x α 3 p + d p t j i z \begin{equation} x_{q^\prime}=\frac{d_{p}^{-1}\alpha_1p + t_{ji}^x}{d_{p}^{-1}\alpha_3p+ t_{ji}^z}=\frac{\alpha_1p + d_{p}t_{ji}^x}{\alpha_3p+ d_{p}t_{ji}^z} \end{equation} xq′​=dp−1​α3​p+tjiz​dp−1​α1​p+tjix​​=α3​p+dp​tjiz​α1​p+dp​tjix​​​​
因此可以求得：
∂ x q ′ ∂ d p = t j i x 1 α 3 p + d p t j i z − ( α 1 p + d p t j i x ) t j i z 1 ( α 3 p + d p t j i z ) 2 \begin{equation} \frac{\partial x_q^{\prime}}{\partial d_p} = t_{ji}^x\frac{1}{\alpha_3p+ d_{p}t_{ji}^z}-\left(\alpha_1p + d_{p}t_{ji}^x\right)t_{ji}^z\frac{1}{\left(\alpha_3p+ d_{p}t_{ji}^z\right)^2} \end{equation} ∂dp​∂xq′​​=tjix​α3​p+dp​tjiz​1​−(α1​p+dp​tjix​)tjiz​(α3​p+dp​tjiz​)21​​​
将公式（31）和（32）代入可以得到：
∂ x q ′ ∂ d p = d p − 1 d q ( t j i x − x q ′ t j i z ) \begin{equation} \frac{\partial x_q^{\prime}}{\partial d_p} = d_{p}^{-1}d_q\left(t_{ji}^x- x_{q^\prime }t_{ji}^z\right) \end{equation} ∂dp​∂xq′​​=dp−1​dq​(tjix​−xq′​tjiz​)​​
同理可得：
∂ y q ′ ∂ d p = d p − 1 d q ( t j i y − y q ′ t j i z ) \begin{equation} \frac{\partial y_q^{\prime}}{\partial d_p} = d_{p}^{-1}d_q\left(t_{ji}^y- y_{q^\prime }t_{ji}^z\right) \end{equation} ∂dp​∂yq′​​=dp−1​dq​(tjiy​−yq′​tjiz​)​​
结合公式（29）（33）（35）可得：
∂ q ′ ∂ d p = [ d p − 1 d q ( t j i x − x q ′ t j i z ) d p − 1 d q ( t j i y − y q ′ t j i z ) 0 ] \begin{equation} \frac{\partial q^\prime}{\partial d_p}=\left[\begin{array}{c} d_{p}^{-1}d_q\left(t_{ji}^x- x_{q^\prime }t_{ji}^z\right) \\ d_{p}^{-1}d_q\left(t_{ji}^y- y_{q^\prime }t_{ji}^z\right) \\ 0 \end{array}\right] \end{equation} ∂dp​∂q′​=⎣ ⎡​dp−1​dq​(tjix​−xq′​tjiz​)dp−1​dq​(tjiy​−yq′​tjiz​)0​⎦ ⎤​​​
结合公式（27）（28）可得：
∂ f ( x ) ∂ d p = ∂ f ( x ) ∂ q ∂ q ∂ d p = w h [ g x g y 0 ] [ f x 0 0 0 f y 0 0 0 0 ] [ d p − 1 d q ( t j i x − x q ′ t j i z ) d p − 1 d q ( t j i y − y q ′ t j i z ) 0 ] \begin{equation} \frac{\partial f(x)}{\partial d_p}=\frac{\partial f(x)}{\partial q} \frac{\partial q}{\partial d_p}=\sqrt{w_{h}}\left[\begin{array}{l} g_{x} & g_{y}&0 \end{array}\right]\left[\begin{array}{ccc} f_{x} & 0 & 0 \\ 0 & f_{y} & 0 \\ 0 & 0 & 0 \end{array}\right]\left[\begin{array}{c} d_{p}^{-1}d_q\left(t_{ji}^x- x_{q^\prime }t_{ji}^z\right) \\ d_{p}^{-1}d_q\left(t_{ji}^y- y_{q^\prime }t_{ji}^z\right) \\ 0 \end{array}\right] \end{equation} ∂dp​∂f(x)​=∂q∂f(x)​∂dp​∂q​=wh​ ​[gx​​gy​​0​]⎣ ⎡​fx​00​0fy​0​000​⎦ ⎤​⎣ ⎡​dp−1​dq​(tjix​−xq′​tjiz​)dp−1​dq​(tjiy​−yq′​tjiz​)0​⎦ ⎤​​​
上式整理可得：
∂ f ( x ) ∂ d p = w h ( g x f x d p − 1 d q ( t j i x − x q ′ t j i z ) + g y f y d p − 1 d q ( t j i y − y q ′ t j i z ) ) \begin{equation} \frac{\partial f(x)}{\partial d_p}= \sqrt{w_{h}}\left(g_{x}f_xd_{p}^{-1}d_q\left(t_{ji}^x- x_{q^\prime }t_{ji}^z\right)+g_{y}f_{y}d_{p}^{-1}d_q\left(t_{ji}^y- y_{q^\prime }t_{ji}^z\right)\right) \end{equation} ∂dp​∂f(x)​=wh​ ​(gx​fx​dp−1​dq​(tjix​−xq′​tjiz​)+gy​fy​dp−1​dq​(tjiy​−yq′​tjiz​))​​

总结

根据上述公式可以求解光度误差对状态变量的导数，进而可以使用高斯-牛顿方法优化状态变量，从而实现DSO初始化过程，下一章会讲解高斯-牛顿优化过程中的Hessian矩阵的构建以及schur补等过程。

Direct Sparse Odometry （一）初始化过程中的光度误差优化相关推荐

1. DSO详解-Direct Sparse Odometry论文解读

   元学习论文总结||小样本学习论文总结 2017-2019年计算机视觉顶会文章收录 AAAI2017-2019 CVPR2017-2019 ECCV2018 ICCV2017-2019 ICLR2017 ...

2. 论文阅读《Omnidirectional DSO: Direct Sparse Odometry with Fisheye Cameras》

   目录 摘要 1 介绍 2 相关工作 3 记号 4 相机模型 A 针孔模型 B 统一的全向模型 5 系统概述 A 模型表述 B 沿极线的距离估计 C 帧的管理 D 窗口优化 6 评估 A TUM SLA ...

3. 论文阅读《Direct Sparse Odometry》1

   目录 摘要 1 介绍 1.1 动机 1.2 贡献 2 直接稀疏模型 2.1 标定 2.1.1 几何相机标定 2.1.2 光度相机标定 2.2 模型表述 2.3 窗口优化 摘要 直接稀疏里程计方法(Di ...

4. memblock初始化过程中fdt mem处理

   memblock初始化过程中fdt mem处理 导读 本部分将memblock 初始化时FDT中reserved memory的扫描处理细化整理,调用情况如下: code目录: 涉及到的目录 简单描述 ...

5. 论文阅读《Direct Sparse Odometry》2

   目录 3 视觉里程计前端 3.1 帧管理 3.2 点管理 4 结果 4.1 定量比较 4.2 参数研究 4.3 几何与光度噪声研究 4.4 定性结果 5 结论 致谢 参考文献 3 视觉里程计前端 前端 ...

6. CDN高级技术专家周哲：深度剖析短视频分发过程中的用户体验优化技术点

   摘要: 深圳云栖大会已经圆满落幕,在3月29日飞天技术汇-弹性计算.网络和CDN专场中,阿里云CDN高级技术专家周哲为我们带来了<海量短视频极速分发>的主题分享,带领我们从视频内容采集.上 ...

7. 深度剖析短视频分发过程中的用户体验优化技术点

   2018深圳云栖大会已经圆满落幕,在飞天技术汇-弹性计算.网络和CDN专场中,阿里云CDN高级技术专家周哲为我们带来了<海量短视频极速分发>的主题分享,带领我们从视频内容采集.上传.存储和 ...

8. 论文阅读《Direct Sparse Visual-Inertial Odometry Using Dynamic Marginalization》

   目录 摘要 1 介绍 2 相关工作 3 直接稀疏视惯里程计 3.1 记号 3.2 光度误差 3.3 惯性误差 3.4 IMU初始化与可观性问题 3.5 基于SIM(3)的世界表示 3.6 尺度感知的视 ...

9. ORB-SLAM3 细读单目初始化过程（终结篇）

   本文原创,转载请说明地址:https://blog.csdn.net/shanpenghui/article/details/110522368 一.前言 请阅读本文之前最好把ORB-SLAM3的单目 ...

最新文章

1. Git基本命令 -- 别名 + 忽略 + 推送
2. Spring MVC(三)--控制器接受普通请求参数
3. java.lang.UnsatisfiedLinkError:org.apache.hadoop.io.nativeio.NativeIO$Windows.access0
4. Android获取EditText，Spinner，CheckBox，RadioButton信息Toast显示
5. 说一说javascript跨域和jsonp
6. windows核心编程之DIPS工具学习总结
7. 论如何高速下载磁力链接、电驴链接、种子文件里的资源
8. BitBake用户手册-3.语法和操作
9. Pomodoro Technique番茄钟的理解
10. TensorFlow 2.9的零零碎碎（一）-tf.keras里的兜兜转转
11. C罗轮播图（HTML+CSS+JS）
12. ios Mac下的SVN工具：Cornerstone与Versions和使用subversion管理iOS源代码
13. 关于c语言中 scanf 对多行字符的输入问题
14. 【Vue生命周期详解】
15. Android 零基础学习之路
16. XtraBackup 物理备份工具
17. iOS暗黑(dark)模式适配
18. NAND FLash基础概念介绍
19. Thrift框架-Windows-C++ x64编译
20. python爬取javascript变量数据_javascript - [新手]python爬虫爬取中证指数官网数据

热门文章

1. 非线性规划在计算机中的应用研究,Hopfield网络在非线性规划中的应用研究.pdf
2. 微信小程序学习笔记(7.16)
3. 一个处于发展过程中的企业，该如何进行精准获客？且不话大价钱。
4. ORA-12012: error on auto execute of job SYS.BSLN_MAINTAIN_STATS_JOB
5. CF Educational Round 57(1096) 比赛记录
6. 《Real-Time Rendering 4th Edition》全文翻译 - 第4章 变换（下）4.5 ~ 4.7
7. [小红猪]算法：人工智能的新曙光
8. 欣弗（克林霉素磷酸酯葡萄糖注射液）----恐怖的抗生素
9. 中职计算机师德范文,计算机教师师德师风心得体会
10. 晕菜, silverlight !