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(tiV(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)=tiexpai+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)−tieaitjeaj(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)=whr2(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=tieaitjeaj，
b
j
i
=
b
j
−
e
a
j
i
b
i
b_{ji}=b_j- e^{a_{ji}}b_i
bji=bj−eajibi，表示从像素
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−eajiIi[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}
[R0t1]:=TjTi−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)∥22c2
上式可以降低高梯度点的权重值，但是在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)=whr=wh(Ij(q)−eajiIi(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)=−wheajiIi(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[gxgy]
其中，
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(Rjidp−1K−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(Rjidp−1K−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}
⎣
⎡uqvq1⎦
⎤=⎣
⎡fx000fy0cxcy1⎦
⎤⎣
⎡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⎦
⎤=⎣
⎡fx000fy0000⎦
⎤
然后计算
∂
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(Rjidp−1K−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=Rjidp−1K−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∂dqx∂ξji∂dqy∂ξji∂dqz⎦
⎤+dq⎣
⎡∂ξji∂x∂ξji∂y∂ξji∂z⎦
⎤=⎣
⎡∂ξji∂z1x∂ξji∂z1y∂ξji∂z1z⎦
⎤+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⎦
⎤=⎣
⎡1000100010−zyz0−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[001y2−x20]
综合公式（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′=⎣
⎡000000−z2x−z2y−z2z−z2xy−z2y2−z2yzz2x2z2xyz2xz000⎦
⎤+⎣
⎡z1000z1000z10−1zy10−zx−zyzx0⎦
⎤
整理可以得到：
∂
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′=⎣
⎡z1000z10−z2x−z2y0−z2xy−(z2y2+1)01+z2x2z2xy0−zyzx0⎦
⎤
根据
z
=
1
d
q
z=\frac{1}{d_q}
z=dq1，
y
=
d
q
v
q
y=d_qv_q
y=dqvq，
x
=
d
q
u
q
x=d_qu_q
x=dquq，可以得到
∂
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′=⎣
⎡dq000dq0−dquq−dqvq0−uqvq−(1+vq2)01+uq2uqvq0−vquq0⎦
⎤
结合公式（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[gxgy0]⎣
⎡fx000fy0000⎦
⎤⎣
⎡dq000dq0−dquq−dqvq0−uqvq−(1+vq2)01+uq2uqvq0−vquq0⎦
⎤
上式整理可得：
∂
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⎣
⎡gxfxdqgyfydq−gxfxdquq−gyfydqvq−gxfxuqvq−gyfy(1+vq2)gxfx(1+uq2)+gyfyuqvq−gxfxvq+gyfyuq⎦
⎤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(Rjidp−1K−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=RjiK−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α1p+tjixdp−1α2p+tjiydp−1α3p+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α3p+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α3p+tjizdp−1α1p+tjix=α3p+dptjizα1p+dptjix
因此可以求得：
∂
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α3p+dptjiz1−(α1p+dptjix)tjiz(α3p+dptjiz)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−1dq(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−1dq(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−1dq(tjix−xq′tjiz)dp−1dq(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[gxgy0]⎣
⎡fx000fy0000⎦
⎤⎣
⎡dp−1dq(tjix−xq′tjiz)dp−1dq(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(gxfxdp−1dq(tjix−xq′tjiz)+gyfydp−1dq(tjiy−yq′tjiz))
总结
根据上述公式可以求解光度误差对状态变量的导数，进而可以使用高斯-牛顿方法优化状态变量，从而实现DSO初始化过程，下一章会讲解高斯-牛顿优化过程中的Hessian矩阵的构建以及schur补等过程。
2万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
