视觉SLAM总结——LSD SLAM中关键知识点总结

视觉SLAM总结——LSD SLAM中关键知识点总结

视觉SLAM总结——LSD SLAM中关键知识点总结1. LSD SLAM的创新点/关键点是什么？2. LSD SLAM的整体框架是怎样的？3. LSD SLAM是怎样完成初始化的？4. LSD SLAM中的跟踪算法有什么特别的地方？5. LSD SLAM是如何进行关键帧选取的？6. LSD SLAM是如何进行深度估计和地图构建的？7. LSD SLAM中闭环检测功能是怎么实现的？

总结

视觉SLAM总结——LSD SLAM中关键知识点总结

这篇LSD SLAM的总结是为了保证我的知识框架的完整性写的，LSD SLAM的源码我目前还并没有花时间去钻研，但是LSD SLAM的地位和ORB SLAM以及SVO（这两个框架的源码是读过了）在经典SLAM框架中是同等重要的，因此我花时间研究了下它的基本算法，算法懂了其实看源码只是一个对照的过程，不过我还是分享出来供大家参考，提供几个参考的资料 LSD-SLAM笔记之SE3Tracking LSD-SLAM笔记之DepthMap LSD-SLAM笔记之一致性约束 这三个博客是同一个博主写的，我觉得原理推导和代码分析都写得比较仔细，主要参考的这个，还有其他一些资料也非常好 LSD-SLAM深入学习（2）-算法解析 lsd-slam源码解读第三篇:算法解析 下面就开始我的总结

1. LSD SLAM的创新点/关键点是什么？

或者说你对LSD SLAM算法中印象最深刻的几个点是什么？ 论文原文指出的创新点有两个：

(1) a novel direct tracking method which operates on sim(3), thereby explicitly detecting scale-drift, (2) an elegant probabilistic solution to include the effect of noisy depth values into tracking.

翻译过来就是 （1）一种新颖的基于（相似变换空间对应的李代数）Sim(3)上的直接跟踪法，从而能够很明确的检测到尺度漂移； （2）使用一种概率方法，对图像跟踪过程中，处理噪声对深度图像信息的影响。

而我个人觉得吧，LSD SLAM中几个比较关键的点是： （1）在前端使用直接法进行Tracking的时候，给出的优化模型进行归一化方差的操作； （2）在后端一直更新和维护的深度地图 （3）在闭环检测时调用的双向Sim3跟踪操作判断是否闭环成功； 下面我会一一分析总结。

2. LSD SLAM的整体框架是怎样的？

如下图所示，LSD SLAM一共由Tracking，Depth Map Estimation和Map Optimization三个线程组成 这里先区分三个概念：关键帧，普通帧，参考关键帧，所谓参考关键帧是就是用来维护深度图的关键帧，所有普通帧的位姿都是基于参考关键帧计算的，在程序中就叫做trackingParent。

（1）Tracking线程：首先将当前图像构造为新的普通帧（当前帧），如果它的参考关键帧不是最近的关键帧的话就先更新参考关键帧（就说我反正要跟踪离我最近的关键帧），把上一帧与参考关键帧的位姿当做初始位姿，然后构建归一化方差的光度残差的代价函数进行优化求解当前帧与参考关键帧之间的位姿变换，然后进行跟踪是否失败的判断和关键帧筛选。

（2）Depth Map Estimation线程：首先判断其是否为关键帧，如果是关键帧，则根据它的参考关键帧构建新的深度图，如果不是关键帧，则用来更新它的参考关键帧的深度图。

（3）Map Optimization线程：如果新关键帧队列为空则在优化过的帧中随机选取图像帧，如果不为空则从新关键帧的第一帧开始选取，主要是根据视差、关键帧连接关系，判断选取的图像帧是否为候选帧，然后对每个候选帧和测试的闭环关键帧之间进行双向Sim3跟踪，如果求解出的两个李代数满足马氏距离在一定范围内，则认为是闭环成功，并且在位姿图中添加边的约束，最后进行全局的图优化。

3. LSD SLAM是怎样完成初始化的？

LSD-SLAM的初始化不需要使用两视图几何，它从第一帧随机初始化场景的深度，然后通过随后的图像不断对场景深度进行修正。图像中梯度明显的像素点的深度被初始化为随机的分布，并赋值为较大的方差后放入系统。第一个初始化的关键帧和后面的图像配准后，跟踪直接开始。图像不断输入，初始特征点的深度测量用滤波方法优化，直到收敛。这种方法不存在两视图几何的退化问题；但在深度收敛之前需要处理大量图像，需要一个中间跟踪过程，此时生成的地图不可靠。

4. LSD SLAM中的跟踪算法有什么特别的地方？

LSD SLAM的前端采用的跟踪算法采用的是直接法，比较特别的地方是，LSD SLAM中的直接法优化的光度残差考虑了方差一致性，这里来详细分下下LSD SLAM中的直接法。 我们知道直接法是基于灰度不变性最小化两幅图像的光度误差，而LSD SLAM的论文中给出的代价函数的公式如下：

E

p

(

ξ

j

i

)

=

∑

p

∈

Ω

D

i

∥

r

p

2

(

p

,

ξ

j

i

)

σ

r

p

(

p

,

ξ

j

)

2

∥

δ

E_{p}\left(\xi_{j i}\right)=\sum_{\mathbf{p} \in \Omega_{D_{i}}}\left\|\frac{r_{p}^{2}\left(\mathbf{p}, \xi_{j i}\right)}{\sigma_{r_{p}\left(\mathbf{p}, \xi_{j}\right)}^{2}}\right\|_{\delta}

Ep​(ξji​)=p∈ΩDi​​∑​∥∥∥∥∥​σrp​(p,ξj​)2​rp2​(p,ξji​)​∥∥∥∥∥​δ​其中，

r

p

2

(

p

,

ξ

j

i

)

r_{p}^{2}\left(\mathbf{p}, \xi_{j i}\right)

rp2​(p,ξji​)为光度误差

r

p

(

p

,

ξ

j

i

)

:

=

I

i

(

p

)

−

I

j

(

ω

(

p

,

D

i

(

p

)

,

ξ

j

i

)

)

r_{p}\left(\mathbf{p}, \xi_{j i}\right) :=I_{i}(\mathbf{p})-I_{j}\left(\omega\left(\mathbf{p}, D_{i}(\mathbf{p}), \xi_{j i}\right)\right)

rp​(p,ξji​):=Ii​(p)−Ij​(ω(p,Di​(p),ξji​))

σ

r

p

2

(

p

,

ξ

j

)

\sigma_{r_{p}}^{2}\left(\mathbf{p}, \xi_{j}\right)

σrp​2​(p,ξj​)为归一化方差

σ

r

p

(

p

.

ξ

j

i

)

2

:

=

2

σ

I

2

+

(

∂

r

p

(

p

,

ξ

j

i

)

∂

D

i

(

p

)

)

2

V

i

(

p

)

\sigma_{r_{p}\left(\mathbf{p} . \xi_{j i}\right)}^{2} :=2 \sigma_{I}^{2}+\left(\frac{\partial r_{p}\left(\mathbf{p}, \xi_{j i}\right)}{\partial D_{i}(\mathbf{p})}\right)^{2} V_{i}(\mathbf{p})

σrp​(p.ξji​)2​:=2σI2​+(∂Di​(p)∂rp​(p,ξji​)​)2Vi​(p)

∥

∗

∥

δ

\|*\|_{\delta}

∥∗∥δ​为Hube范数

∥

r

2

∥

δ

:

=

{

r

2

2

δ

if

∣

r

∣

≤

δ

∣

r

∣

−

δ

2

otherwise

\left\|r^{2}\right\|_{\delta} :=\left\{\begin{array}{ll}{\frac{r^{2}}{2 \delta}} & {\text { if } \quad|r| \leq \delta} \\ {|r|-\frac{\delta}{2}} & {\text { otherwise }}\end{array}\right.

∥∥​r2∥∥​δ​:={2δr2​∣r∣−2δ​​ if ∣r∣≤δ otherwise ​中间还有一些参数进行一一说明：

p

\mathbf{p}

p是参考关键帧

I

i

I_{i}

Ii​观察到的有深度信息的归一化坐标系下的图像点；

D

i

(

p

)

D_{i}(\mathbf{p})

Di​(p)是

p

\mathbf{p}

p在参考帧下的逆深度；

V

i

(

p

)

V_{i}(\mathbf{p})

Vi​(p)是

p

\mathbf{p}

p对对应的逆深度的方差；

ω

(

p

,

D

i

(

p

)

,

ξ

j

i

)

\omega\left(\mathbf{p}, D_{i}(\mathbf{p}), \xi_{j i}\right)

ω(p,Di​(p),ξji​)是把参考关键帧下的归一化坐标系下的图像点

p

\mathbf{p}

p对应的3D点投影到当前帧的归一化平面上，定义如下

ω

(

p

,

D

i

(

p

)

,

ξ

)

:

=

(

x

′

/

z

′

y

′

/

z

′

1

)

with

(

x

′

y

′

z

′

1

)

:

=

exp

⁡

sec

⁡

(

3

)

(

ξ

)

(

p

x

/

d

p

y

/

d

1

/

d

1

)

\omega\left(\mathbf{p}, D_{i}(\mathbf{p}), \xi\right) :=\left(\begin{array}{c}{x^{\prime} / z^{\prime}} \\ {y^{\prime} / z^{\prime}} \\ {1 }\end{array}\right) \quad \text { with } \quad\left(\begin{array}{c}{x^{\prime}} \\ {y^{\prime}} \\ {z^{\prime}} \\ {1}\end{array}\right) :=\exp _{\sec (3)}(\xi)\left(\begin{array}{c}{\mathbf{p}_{x} / d} \\ {\mathbf{p}_{y} / d} \\ {1 / d} \\ {1}\end{array}\right)

ω(p,Di​(p),ξ):=⎝⎛​x′/z′y′/z′1​⎠⎞​ with ⎝⎜⎜⎛​x′y′z′1​⎠⎟⎟⎞​:=expsec(3)​(ξ)⎝⎜⎜⎛​px​/dpy​/d1/d1​⎠⎟⎟⎞​按照论文中的公式直观理解的话

p

\mathbf{p}

p确实是在归一化坐标系下的图像点，但这与我们通常的表达方式是不符的，我们求一个图像点的光度肯定是在图像坐标系下的图像点

u

\mathbf{u}

u进行计算，中间差了一个相机内参

u

=

K

p

=

(

f

x

0

c

x

0

f

y

c

y

0

0

1

)

p

\mathbf{u}=K \mathbf{p}=\left(\begin{array}{ccc}{f x} & {0} & {c x} \\ {0} & {f y} & {c y} \\ {0} & {0} & {1}\end{array}\right) \mathbf{p}

u=Kp=⎝⎛​fx00​0fy0​cxcy1​⎠⎞​p这一点我觉得没必要太纠结，反正我们能理解作者的意思对吧，而且

K

K

K是一个常数矩阵

到目前为止对一般的直接法比较熟悉的同学就会有三个问题（其实就是我的问题）： （1）为什么要进行方差归一化操作？ （2）归一化的方法

σ

r

p

2

(

p

⋅

ξ

j

i

)

\sigma_{r_{p}}^{2}\left(\mathbf{p} \cdot \xi_{j i}\right)

σrp​2​(p⋅ξji​)具体怎么求？ （3）我们知道不带方差的代价函数

E

p

(

ξ

j

i

)

E_{p}\left(\xi_{j i}\right)

Ep​(ξji​)可以表示为：

E

(

ξ

)

=

∑

i

(

I

r

e

f

(

p

i

)

−

I

(

ω

(

p

,

D

r

e

f

(

p

)

,

ξ

)

)

)

2

⏟

=

r

i

2

(

ξ

)

E(\xi)=\sum_{i} \underbrace{\left(I_{r e f}\left(\mathbf{p}_{i}\right)-I\left(\omega\left(\mathbf{p}, D_{r e f}(\mathbf{p}), \xi\right)\right)\right)^{2}}_{=r_{i}^{2}(\xi)}

E(ξ)=i∑​=ri2​(ξ)

(Iref​(pi​)−I(ω(p,Dref​(p),ξ)))2​​那带方差归一化的代价函数实际操作中应该怎么处理呢？ 我们一个一个来回答，参考LSD-SLAM笔记之SE3Tracking

（1）为什么需要进行方差归一化操作呢？ 论文中在估计两帧间位姿变换的时候，把所有有逆深度假设的像素都用上了。但是每个逆深度的确定性不同，也就是有些逆深度比较准确，有些不准确。而准确与否则体现在逆深度的方差上了。因此在残差上除以了方差做了归一化。在论文中考虑了两个方面的方差，一个是由逆深度估计不准确引入的，另外一个是由图像高斯噪声引起的。也即是说上面计算方差的式子前面的是连个图像的图像高斯噪声，后面的是逆深度造成的误差。

（2）归一化的方法

σ

r

p

2

(

p

⋅

ξ

j

i

)

\sigma_{r_{p}}^{2}\left(\mathbf{p} \cdot \xi_{j i}\right)

σrp​2​(p⋅ξji​)具体怎么求？ 公式如下

σ

r

p

(

p

⋅

ξ

i

j

)

2

:

=

2

σ

I

2

+

(

∂

r

p

(

p

,

ξ

j

i

)

∂

D

i

(

p

)

)

2

V

i

(

p

)

\sigma_{r_{p}\left(\mathbf{p} \cdot \xi_{i j}\right)}^{2} :=2 \sigma_{I}^{2}+\left(\frac{\partial r_{p}\left(\mathbf{p}, \xi_{j i}\right)}{\partial D_{i}(\mathbf{p})}\right)^{2} V_{i}(\mathbf{p})

σrp​(p⋅ξij​)2​:=2σI2​+(∂Di​(p)∂rp​(p,ξji​)​)2Vi​(p)图像高斯噪声

σ

I

\sigma_{I}^{}

σI​再程序中就是一个常数，而逆深度的方差

V

i

(

p

)

V_{i}(\mathbf{p})

Vi​(p)就是高斯分布的方差（这里有一个很重要的假设还没提，我们假设图像点的逆深度是满足高斯分布的，在深度滤波时会再一次提到这一点，具体怎么求的也会说明），唯一现在还不知道的就是光度误差对逆深度的导数

∂

r

p

(

p

,

ξ

j

i

)

∂

D

i

(

p

)

\frac{\partial r_{p}\left(\mathbf{p}, \xi_{j i}\right)}{\partial D_{i}(\mathbf{p})}

∂Di​(p)∂rp​(p,ξji​)​，这里根据导数的定义求解，但是在实际操作时论文作者对这个导数求解进行了一个简化操作，假设这里的三维变换只考虑平移，而不考虑旋转（因为两帧相距很近），这样可以大大简化实际求导操作，如下：参考LSD-SLAM权重更新公式推导 为了方便推到，首先我们给几个和论文中不同的定义，对于参考关键帧，其逆深度

d

=

1

/

z

d=1/z

d=1/z，归一化坐标系下的像素坐标

(

u

,

v

)

(u,v)

(u,v)，相机坐标系下是坐标

(

x

,

y

,

z

)

(x,y,z)

(x,y,z)，对于当前帧，其归一化坐标系下的像素坐标

(

u

′

,

v

′

)

(u',v')

(u′,v′)，相机坐标系下是坐标

(

x

′

,

y

′

,

z

′

)

(x',y',z')

(x′,y′,z′)，然后我们将问题从原论文中过渡过来：

∂

r

p

(

p

,

ξ

j

i

)

∂

D

i

(

p

)

=

∂

(

I

i

(

p

)

−

I

j

(

ω

(

p

,

D

i

(

p

)

,

ξ

j

i

)

)

)

∂

D

i

(

p

)

=

−

∂

(

I

j

(

ω

(

p

,

D

i

(

p

)

,

ξ

j

i

)

)

)

∂

D

i

(

p

)

=

−

∂

I

(

u

′

,

v

′

)

∂

d

=

−

∂

I

(

u

′

,

v

′

)

∂

(

u

′

,

v

′

)

∂

(

u

′

,

v

′

)

∂

(

x

′

,

y

′

,

z

′

)

∂

(

x

′

,

y

′

,

z

′

)

∂

d

\begin{aligned} \frac{\partial r_{p}\left(\mathbf{p}, \xi_{j i}\right)}{\partial D_{i}(\mathbf{p})} &=\frac{\partial\left(I_{i}(\mathbf{p})-I_{j}\left(\omega\left(\mathbf{p}, D_{i}(\mathbf{p}), \xi_{j i}\right)\right)\right)}{\partial D_{i}(\mathbf{p})} \\& = -\frac{\partial\left(I_{j}\left(\omega\left(\mathbf{p}, D_{i}(\mathbf{p}), \xi_{j i}\right)\right)\right)}{\partial D_{i}(\mathbf{p})} \\& = -\frac{\partial I\left(u^{\prime}, v^{\prime}\right)}{\partial d} \\& = -\frac{\partial I\left(u^{\prime}, v^{\prime}\right)}{\partial\left(u^{\prime}, v^{\prime}\right)} \frac{\partial\left(u^{\prime}, v^{\prime}\right)}{\partial\left(x^{\prime}, y^{\prime}, z^{\prime}\right)} \frac{\partial\left(x^{\prime}, y^{\prime}, z^{\prime}\right)}{\partial d} \end{aligned}

∂Di​(p)∂rp​(p,ξji​)​​=∂Di​(p)∂(Ii​(p)−Ij​(ω(p,Di​(p),ξji​)))​=−∂Di​(p)∂(Ij​(ω(p,Di​(p),ξji​)))​=−∂d∂I(u′,v′)​=−∂(u′,v′)∂I(u′,v′)​∂(x′,y′,z′)∂(u′,v′)​∂d∂(x′,y′,z′)​​这里有三项，第一项是光度对像素的导数，即求得像素梯度：

∂

I

(

u

′

,

v

′

)

∂

(

u

′

,

v

′

)

=

[

g

x

g

y

]

\frac{\partial I\left(u^{\prime}, v^{\prime}\right)}{\partial\left(u^{\prime}, v^{\prime}\right)}=\left[g_{x} \quad g_{y}\right]

∂(u′,v′)∂I(u′,v′)​=[gx​gy​]第二项是归一化坐标系下的图像点对相机坐标系下的空间点的导数，我们已知：

u

′

=

x

′

z

′

f

x

u'=\frac{x'}{z'}f_x

u′=z′x′​fx​

v

′

=

y

′

z

′

f

y

v'=\frac{y'}{z'}f_y

v′=z′y′​fy​因此

∂

(

u

′

,

v

′

)

∂

(

x

′

,

y

′

,

z

′

)

=

[

∂

u

′

∂

x

′

∂

u

′

∂

y

′

∂

u

′

∂

z

′

∂

v

′

∂

x

′

∂

v

′

∂

y

′

∂

v

′

∂

z

′

]

=

[

1

z

′

f

x

0

−

x

′

z

′

2

f

x

0

1

z

′

f

y

−

y

′

z

′

2

f

y

]

\frac{\partial\left(u^{\prime}, v^{\prime}\right)}{\partial\left(x^{\prime}, y^{\prime}, z^{\prime}\right)}=\left[\begin{array}{lll}{\frac{\partial u^{\prime}}{\partial x^{\prime}}} & {\frac{\partial u^{\prime}}{\partial y^{\prime}}} & {\frac{\partial u^{\prime}}{\partial z^{\prime}}} \\ {\frac{\partial v^{\prime}}{\partial x^{\prime}}} & {\frac{\partial v^{\prime}}{\partial y^{\prime}}} & {\frac{\partial v^{\prime}}{\partial z^{\prime}}}\end{array}\right]=\left[\begin{array}{ccc}{\frac{1}{z^{\prime}} f_{x}} & {0} & {-\frac{x^{\prime}}{z^{\prime 2}} f_{x}} \\ {0} & {\frac{1}{z^{\prime}} f_{y}} & {-\frac{y^{\prime}}{z^{\prime 2}} f_{y}}\end{array}\right]

∂(x′,y′,z′)∂(u′,v′)​=[∂x′∂u′​∂x′∂v′​​∂y′∂u′​∂y′∂v′​​∂z′∂u′​∂z′∂v′​​]=[z′1​fx​0​0z′1​fy​​−z′2x′​fx​−z′2y′​fy​​]第三项是相机坐标系下的空间点对逆深度的导数，我们已知

[

x

′

y

′

z

′

]

=

[

x

y

z

]

+

[

t

x

t

y

t

z

]

=

[

u

d

f

x

v

d

f

y

1

d

]

+

[

t

x

t

y

t

z

]

=

1

d

[

u

f

x

v

f

y

1

]

+

[

t

x

t

y

t

z

]

\left[\begin{array}{l}{x^{\prime}} \\ {y^{\prime}} \\ {z^{\prime}}\end{array}\right]=\left[\begin{array}{c}{x} \\ {y} \\ {z}\end{array}\right]+\left[\begin{array}{c}{t_{x}} \\ {t_{y}} \\ {t_{z}}\end{array}\right]=\left[\begin{array}{c}{\frac{u}{d}f_x} \\ {\frac{v}{d}f_y} \\ {\frac{1}{d}}\end{array}\right]+\left[\begin{array}{c}{t_{x}} \\ {t_{y}} \\ {t_{z}}\end{array}\right]=\frac{1}{d}\left[\begin{array}{c}{uf_x} \\ {vf_y} \\ {1}\end{array}\right]+\left[\begin{array}{c}{t_{x}} \\ {t_{y}} \\ {t_{z}}\end{array}\right]

⎣⎡​x′y′z′​⎦⎤​=⎣⎡​xyz​⎦⎤​+⎣⎡​tx​ty​tz​​⎦⎤​=⎣⎡​du​fx​dv​fy​d1​​⎦⎤​+⎣⎡​tx​ty​tz​​⎦⎤​=d1​⎣⎡​ufx​vfy​1​⎦⎤​+⎣⎡​tx​ty​tz​​⎦⎤​因此

∂

(

x

′

,

y

′

,

z

′

)

∂

d

=

−

1

d

2

[

u

f

x

v

f

y

1

]

\frac{\partial\left(x^{\prime}, y^{\prime}, z^{\prime}\right)}{\partial d}=-\frac{1}{d^2}\left[\begin{array}{c}{uf_x} \\ {vf_y} \\ {1}\end{array}\right]

∂d∂(x′,y′,z′)​=−d21​⎣⎡​ufx​vfy​1​⎦⎤​又

[

u

f

x

v

f

y

1

]

=

d

[

x

′

−

t

x

y

′

−

t

y

z

′

−

t

z

]

\left[\begin{array}{c}{u f_{x}} \\ {v f_{y}} \\ {1}\end{array}\right]=d\left[\begin{array}{l}{x^{\prime}-t_{x}} \\ {y^{\prime}-t_{y}} \\ {z^{\prime}-t_{z}}\end{array}\right]

⎣⎡​ufx​vfy​1​⎦⎤​=d⎣⎡​x′−tx​y′−ty​z′−tz​​⎦⎤​因此

∂

(

x

′

,

y

′

,

z

′

)

∂

d

=

1

d

[

t

x

−

x

′

t

y

−

y

′

t

z

−

z

′

]

\frac{\partial\left(x^{\prime}, y^{\prime}, z^{\prime}\right)}{\partial d}=\frac{1}{d}\left[\begin{array}{c}{t_{x}-x^{\prime}} \\ {t_{y}-y^{\prime}} \\ {t_{z}-z^{\prime}}\end{array}\right]

∂d∂(x′,y′,z′)​=d1​⎣⎡​tx​−x′ty​−y′tz​−z′​⎦⎤​综上所述

∂

I

(

u

′

,

v

′

)

∂

d

=

[

g

x

g

y

]

[

1

z

′

f

x

0

−

x

′

z

2

f

x

0

1

z

′

f

y

−

y

z

2

f

y

]

1

d

[

t

x

−

x

′

t

y

−

y

′

t

z

−

z

′

]

=

1

d

z

′

2

[

g

x

g

y

]

[

z

′

f

x

(

t

x

−

x

′

)

−

x

′

f

x

(

t

z

−

z

′

)

z

′

f

y

(

t

y

−

y

′

)

−

y

′

f

y

(

t

z

−

z

′

)

]

=

1

d

z

′

2

[

g

x

g

y

]

[

z

′

f

x

t

x

−

z

′

f

x

x

′

−

x

′

f

x

t

z

+

x

′

f

x

z

′

z

′

f

y

t

y

−

z

′

f

y

y

′

−

y

′

f

y

t

z

+

y

′

f

y

z

′

]

=

1

d

z

′

2

[

g

x

g

y

]

[

z

′

f

x

t

x

−

x

′

f

x

t

z

z

′

f

y

t

y

−

y

′

f

y

t

z

]

=

g

x

(

z

′

f

x

t

x

−

x

′

f

x

t

z

)

2

+

g

y

(

z

′

f

y

t

y

−

y

′

f

y

t

z

)

d

z

′

2

=

g

x

f

x

(

z

′

t

x

−

x

′

t

z

)

d

z

′

2

+

g

y

f

y

(

z

′

t

y

−

y

′

t

z

)

d

z

′

2

\begin{aligned} \frac{\partial I\left(u^{\prime}, v^{\prime}\right)}{\partial d}&=\left[\begin{array}{ll}{g_{x}} & {g_{y}}\end{array}\right]\left[\begin{array}{ccc}{\frac{1}{z^{\prime}} f_{x}} & {0} & {-\frac{x^{\prime}}{z^{2}} f_{x}} \\ {0} & {\frac{1}{z^{\prime}} f_{y}} & {-\frac{y}{z^{2}} f_{y}}\end{array}\right] \frac{1}{d}\left[\begin{array}{c}{t_{x}-x^{\prime}} \\ {t_{y}-y^{\prime}} \\ {t_{z}-z^{\prime}}\end{array}\right] \\&=\frac{1}{d z^{\prime 2}}\left[\begin{array}{ll}{g_{x}} & {g_{y}}\end{array}\right]\left[\begin{array}{c}{z^{\prime} f_{x}\left(t_{x}-x^{\prime}\right)-x^{\prime} f_{x}\left(t_{z}-z^{\prime}\right)} \\ {z^{\prime} f_{y}\left(t_{y}-y^{\prime}\right)-y^{\prime} f_{y}\left(t_{z}-z^{\prime}\right)}\end{array}\right] \\&=\frac{1}{d z^{\prime 2}}\left[\begin{array}{ll}{g_{x}} & {g_{y}}\end{array}\right]\left[\begin{array}{l}{z^{\prime} f_{x} t_{x}-z^{\prime} f_{x} x^{\prime}-x^{\prime} f_{x} t_{z}+x^{\prime} f_{x} z^{\prime}} \\ {z^{\prime} f_{y} t_{y}-z^{\prime} f_{y} y^{\prime}-y^{\prime} f_{y} t_{z}+y^{\prime} f_{y} z^{\prime}}\end{array}\right] \\&=\frac{1}{d z^{\prime 2}}\left[\begin{array}{ll}{g_{x}} & {g_{y}}\end{array}\right]\left[\begin{array}{l}{z^{\prime} f_{x} t_{x}-x^{\prime} f_{x} t_{z}} \\ {z^{\prime} f_{y} t_{y}-y^{\prime} f_{y} t_{z}}\end{array}\right] \\&=\frac{g_{x}\left(z^{\prime} f_{x} t_{x}-x^{\prime} f_{x} t_{z}\right)^{2}+g_{y}\left(z^{\prime} f_{y} t_{y}-y^{\prime} f_{y} t_{z}\right)}{d z^{\prime 2}} \\&=g_{x} f_{x} \frac{\left(z^{\prime} t_{x}-x^{\prime} t_{z}\right)}{d z^{\prime 2}}+g_{y} f_{y} \frac{\left(z^{\prime} t_{y}-y^{\prime} t_{z}\right)}{d z^{\prime 2}} \end{aligned}

∂d∂I(u′,v′)​​=[gx​​gy​​][z′1​fx​0​0z′1​fy​​−z2x′​fx​−z2y​fy​​]d1​⎣⎡​tx​−x′ty​−y′tz​−z′​⎦⎤​=dz′21​[gx​​gy​​][z′fx​(tx​−x′)−x′fx​(tz​−z′)z′fy​(ty​−y′)−y′fy​(tz​−z′)​]=dz′21​[gx​​gy​​][z′fx​tx​−z′fx​x′−x′fx​tz​+x′fx​z′z′fy​ty​−z′fy​y′−y′fy​tz​+y′fy​z′​]=dz′21​[gx​​gy​​][z′fx​tx​−x′fx​tz​z′fy​ty​−y′fy​tz​​]=dz′2gx​(z′fx​tx​−x′fx​tz​)2+gy​(z′fy​ty​−y′fy​tz​)​=gx​fx​dz′2(z′tx​−x′tz​)​+gy​fy​dz′2(z′ty​−y′tz​)​​到此为止我们就得到了方差的整个计算公式

（3）方差归一化的代价函数实际操作中应该怎么处理呢？ 方差归一化的代价函数可以写作

E

(

ξ

)

=

∑

i

ω

i

(

ξ

)

r

i

2

(

ξ

)

E(\xi)=\sum_{i} \omega_{i}(\xi) r_{i}^{2}(\xi)

E(ξ)=i∑​ωi​(ξ)ri2​(ξ)进行高斯牛顿法迭代优化（这个操作还不熟悉的同学可能需要自己补一下了）时更新量就变为：

δ

ξ

(

n

)

=

−

(

J

T

J

)

−

1

J

T

W

r

(

ξ

(

n

)

)

\delta \xi^{(n)}=-\left(\mathbf{J}^{T} \mathbf{J}\right)^{-1} \mathbf{J}^{T} \mathbf{W} \mathbf{r}\left(\xi^{(n)}\right)

δξ(n)=−(JTJ)−1JTWr(ξ(n))其中

W

=

W

(

ξ

(

n

)

)

\mathbf{W}=\mathbf{W}\left(\xi^{(n)}\right)

W=W(ξ(n))是一个权重矩阵 在实际的操作当中，代价函数分母中的用来归一化的方差是当作一个系数（当然这个系数会按照上面的公式进行更新）来处理，所以公式还是：

δ

ξ

∗

=

arg

⁡

min

⁡

δ

ξ

E

p

(

δ

ξ

∘

ξ

)

=

arg

⁡

min

⁡

δ

ξ

∑

i

r

i

2

(

δ

ξ

∘

ξ

)

\delta \xi^{*}=\arg \min _{\delta \xi} E_{p}(\delta \xi \circ \xi)=\arg \min _{\delta \xi} \sum_{i} r_{i}^{2}(\delta \xi \circ \xi)

δξ∗=argδξmin​Ep​(δξ∘ξ)=argδξmin​i∑​ri2​(δξ∘ξ)所以雅克比的求导还是（参考《视觉SLAM十四讲》就好）

J

=

∂

I

∂

u

∂

u

∂

q

∂

q

∂

δ

ξ

\mathbf{J}=\frac{\partial \boldsymbol{I}}{\partial \boldsymbol{u}} \frac{\partial \boldsymbol{u}}{\partial \boldsymbol{q}} \frac{\partial \boldsymbol{q}}{\partial \delta \xi}

J=∂u∂I​∂q∂u​∂δξ∂q​（1）

∂

I

/

∂

u

\partial I/ \partial_{u}

∂I/∂u​为像素

u

u

u处的像素梯度; （2）

∂

u

/

∂

q

\partial u / \partial q

∂u/∂q为投影方程关于相机坐标系下的三位点的导数

∂

u

∂

q

=

[

∂

u

∂

X

∂

u

∂

Y

∂

u

∂

Z

∂

v

∂

X

∂

v

∂

Y

∂

v

∂

Z

]

=

[

f

x

Z

0

−

f

x

X

Z

2

0

f

y

Z

−

f

y

Y

Z

2

]

\frac{\partial \boldsymbol{u}}{\partial \boldsymbol{q}}=\left[\begin{array}{ccc}{\frac{\partial u}{\partial X}} & {\frac{\partial u}{\partial Y}} & {\frac{\partial u}{\partial Z}} \\ {\frac{\partial v}{\partial X}} & {\frac{\partial v}{\partial Y}} & {\frac{\partial v}{\partial Z}}\end{array}\right]=\left[\begin{array}{ccc}{\frac{f_{x}}{Z}} & {0} & {-\frac{f_{x} X}{Z^{2}}} \\ {0} & {\frac{f_{y}}{Z}} & {-\frac{f_{y} Y}{Z^{2}}}\end{array}\right]

∂q∂u​=[∂X∂u​∂X∂v​​∂Y∂u​∂Y∂v​​∂Z∂u​∂Z∂v​​]=[Zfx​​0​0Zfy​​​−Z2fx​X​−Z2fy​Y​​]（3）

∂

q

/

∂

δ

ξ

\partial \boldsymbol{q} / \partial \delta \boldsymbol{\xi}

∂q/∂δξ为三维点对位姿变换的导数

∂

P

′

∂

δ

ξ

=

[

I

,

−

P

′

′

]

=

[

1

0

0

0

Z

′

−

Y

′

0

1

0

Z

′

0

X

′

0

0

1

Y

′

−

X

′

0

]

\frac{\partial \boldsymbol{P}^{\prime}}{\partial \delta \boldsymbol{\xi}}=\left[\boldsymbol{I},-\boldsymbol{P}^{\prime \prime}\right]=\left[\begin{array}{cccccc}{1} & {0} & {0} & {0} & {Z^{\prime}} & {-Y^{\prime}} \\ {0} & {1} & {0} & {Z^{\prime}} & {0} & {X^{\prime}} \\ {0} & {0} & {1} & {Y^{\prime}} & {-X^{\prime}} & {0}\end{array}\right]

∂δξ∂P′​=[I,−P′′]=⎣⎡​100​010​001​0Z′Y′​Z′0−X′​−Y′X′0​⎦⎤​因此有雅可比矩阵

J

=

−

∂

I

2

∂

u

∂

u

∂

δ

ξ

\boldsymbol{J}=-\frac{\partial \boldsymbol{I}_{2}}{\partial \boldsymbol{u}} \frac{\partial \boldsymbol{u}}{\partial \delta \boldsymbol{\xi}}

J=−∂u∂I2​​∂δξ∂u​而

∂

u

∂

δ

ξ

=

[

f

x

Z

0

−

f

x

X

Z

2

−

f

x

X

Y

Z

2

f

x

+

f

x

X

2

Z

2

−

f

x

Y

Z

0

f

y

Z

−

f

y

Y

Z

2

−

f

y

−

f

y

Y

2

Z

2

f

y

X

Y

Z

2

f

y

X

Z

]

\frac{\partial \boldsymbol{u}}{\partial \delta \boldsymbol{\xi}}=\left[\begin{array}{ccccc}{\frac{f_{x}}{Z}} & {0} & {-\frac{f_{x} X}{Z^{2}}} & {-\frac{f_{x} X Y}{Z^{2}}} & {f_{x}+\frac{f_{x} X^{2}}{Z^{2}}} & {-\frac{f_{x} Y}{Z}} \\ {0} & {\frac{f_{y}}{Z}} & {-\frac{f_{y} Y}{Z^{2}}} & {-f_{y}-\frac{f_{y} Y^{2}}{Z^{2}}} & {\frac{f_{y} X Y}{Z^{2}}} & {\frac{f_{y} X}{Z}}\end{array}\right]

∂δξ∂u​=[Zfx​​0​0Zfy​​​−Z2fx​X​−Z2fy​Y​​−Z2fx​XY​−fy​−Z2fy​Y2​​fx​+Z2fx​X2​Z2fy​XY​​−Zfx​Y​Zfy​X​​]然后用高斯牛顿或者列文伯格进行迭代求解就好，不同的是在更新的时候会把这个前面计算出来的方差作为系数也乘到更新公式中，看这段代码

inline void NormalEquationsLeastSquares::update(const Vector6& J, const float& res, const float& weight)

{

// printf("up: %f, %f, %f, %f, %f, %f; res: %f; w: %f\n",

// J[0],J[1],J[2],J[3],J[4],J[5],res, weight);

A_opt.rankUpdate(J, weight);

//MathSse::addOuterProduct(A, J, factor);

//A += J * J.transpose() * factor;

//MathSse::add(b, J, -res * factor); // not much difference :(

b -= J * (res * weight);

error += res * res * weight;

num_constraints += 1;

}

其中weight就是我们前面计算好的方差，是不是很简单～

5. LSD SLAM是如何进行关键帧选取的？

论文中是根据运动距离来确定，如果当前相机运动距离参考帧过远则把当前帧创建为关键帧。并且给出了距离函数：

dist

⁡

(

ξ

j

1

)

:

=

ξ

j

i

T

W

ξ

j

i

\operatorname{dist}\left(\xi_{\mathrm{j} 1}\right) :=\xi_{\mathrm{ji}}^{T} \mathbf{W} \xi_{\mathrm{ji}}

dist(ξj1​):=ξjiT​Wξji​其中

W

\mathbf{W}

W是权重矩阵,并且距离阈值根据当前帧场景平均逆深度来确定。

6. LSD SLAM是如何进行深度估计和地图构建的？

说实话，这一段快看吐我了…还是来自LSD-SLAM笔记之SE3Tracking中的总结：LSD-SLAM构建的是半稠密逆深度地图（semi-dense inverse depth map），只对有明显梯度的像素位置进行深度估计，用逆深度表示，并且假设逆深度服从高斯分布。一旦一个图像帧被选为关键帧，则用其跟踪的参考关键帧的深度图对其进行深度图构建，之后跟踪到该新建关键帧的图像帧都会用来对其深度图进行更新。当然，追溯到第一帧，肯定是没有深度图的，因此第一帧的深度图是有明显梯度区域随机生成的深度。 总的来说，建图线程可以分为两种情况 （1）不构建关键帧，则更新参考关键帧的深度图（Depth Map Refinement） （2）构建关键帧，则构建新的深度图（Depth Map Creation） 那么这里要关注的问题主要是两个了 （1）怎么更新参考关键帧的深度图？ （2）怎么构建新的深度图？ 下面分别回答

（1）怎么更新参考关键帧的深度图呢？ 更新参考关键帧深度主要从三个步骤讲解： 步骤一：选取用来跟新参考关键帧的图像帧 LSD SLAM里面确定的规则是：如果距离参考关键帧最近的图像帧（在时间序列上会是较老的图像帧）满足视差和观测角要求的话，就选取最近的图像帧，如果极线搜索失败则，使用较远的图像帧进行极线搜索。

步骤二：采用立体匹配策略进行极线搜索 一般的方法是参考关键帧上的点如果已有逆深度的先验假设

N

(

d

,

σ

d

2

)

N(d,σ^2_d)

N(d,σd2​)，则设置的逆深度搜索范围为

d

±

2

σ

d

d±2σ_d

d±2σd​，而LSD SLAM的论文中为了提高搜索效率，在图像帧的对极线上采样5个点，找到最佳的匹配点，匹配的方法使用的是SSD，如下

E

S

S

D

(

u

)

=

∑

i

[

I

1

(

x

i

+

u

)

−

I

0

(

x

i

)

]

2

E_{S S D}(u)=\sum_{i}\left[I_{1}\left(x_{i}+u\right)-I_{0}\left(x_{i}\right)\right]^{2}

ESSD​(u)=i∑​[I1​(xi​+u)−I0​(xi​)]2

步骤三：根据最佳的匹配点进行深度更新 在《视觉SLAM十四讲》中的13章当中介绍了一个高斯分布的深度滤波器，某个像素点的深度

d

d

d服从高斯分布

P

(

d

)

=

N

(

μ

,

σ

2

)

P(d)=N\left(\mu, \sigma^{2}\right)

P(d)=N(μ,σ2)而观测的同样是一个高斯分布

P

(

d

o

b

s

)

=

N

(

μ

o

b

s

,

σ

o

b

s

2

)

P\left(d_{o b s}\right)=N\left(\mu_{o b s}, \sigma_{o b s}^{2}\right)

P(dobs​)=N(μobs​,σobs2​)则融合后的深度同样满足高斯分布为

μ

f

u

s

e

=

σ

o

b

s

2

μ

+

σ

2

μ

o

b

s

σ

2

+

σ

o

b

s

2

,

σ

f

u

s

e

2

=

σ

2

σ

o

b

s

2

σ

2

+

σ

o

b

s

2

\mu_{f u s e}=\frac{\sigma_{o b s}^{2} \mu+\sigma^{2} \mu_{o b s}}{\sigma^{2}+\sigma_{o b s}^{2}}, \quad \sigma_{f u s e}^{2}=\frac{\sigma^{2} \sigma_{o b s}^{2}}{\sigma^{2}+\sigma_{o b s}^{2}}

μfuse​=σ2+σobs2​σobs2​μ+σ2μobs​​,σfuse2​=σ2+σobs2​σ2σobs2​​中间涉及最主要的问题是观测的高斯分布如何求，在书中仅仅是用了一个三角模型就求解了观测高斯分布的方差，而LSD则对其进行了更加精确地建模，下面进行一波分析： 论文中指出，观测的方差主要由三部分构成：（1）因为三维变换

ξ

\xi

ξ和相机误差

π

\pi

π导致的匹配位置的误差，称为几何视差误差，（2）因为参考关键帧和图像帧中图像的噪声导致的匹配位置的误差，称为光度视差误差，还有就是逆深度计算误差，其主要是因为前面两个误差和基线长度引起的误差，下面一一进行分析：

几何视差误差： 我们来看图说话，先给出论文Semi-Dense Visual Odometry for a Monocular Camera中的一个图： 上图中两条黑色的实线从上到下分别表示没有误差和带有误差的极线，可见带有误差的极线仅仅是位置发生了变换而极线的方向保持不变，这与论文中定义的极线的近似表达式有关

L

:

=

{

l

0

+

λ

(

l

x

l

y

)

∣

λ

∈

S

}

L :=\left\{l_{0}+\lambda\left(\begin{array}{c}{l_{x}} \\ {l_{y}}\end{array}\right) | \lambda \in S\right\}

L:={l0​+λ(lx​ly​​)∣λ∈S}其中

l

0

l_0

l0​表示参考关键帧中无穷远点在图像帧中的图像点，三维变换

ξ

\xi

ξ和相机误差

π

\pi

π导致的误差都包含在

l

0

l_0

l0​的位置中，

l

:

=

(

l

x

,

l

y

)

T

l :=\left(l_{x}, l_{y}\right)^{T}

l:=(lx​,ly​)T是归一化的极线方向，

λ

\lambda

λ是表示的是极线搜索的范围，而图中蓝色线段

ϵ

l

\epsilon_{l}

ϵl​表示的就是极线位置误差，蓝色线段

ϵ

l

\epsilon_{l}

ϵl​和带误差的极线的交点表示的是，在考虑了极线位置误差后我的匹配点应该在的位置，而实际上，我们立体匹配过程中采用的是图像灰度进行匹配程度的判断，因此实际我们匹配的结果会是在灰度等值线（黑色虚线）与带误差的极线的交点，因此红线线段

ϵ

λ

\epsilon_{\lambda}

ϵλ​表示的就是我们要求的几何视差误差，下面通过数学表达式描述这一系列关系，我们匹配到的点

λ

∗

\lambda^{*}

λ∗在带误差的极线和灰度等值线上，因此我们有：

l

0

+

λ

∗

(

l

x

l

y

)

=

g

0

+

γ

(

−

g

y

g

x

)

γ

∈

R

l_{0}+\lambda^{*}\left(\begin{array}{c}{l_{x}} \\ {l_{y}}\end{array}\right)=g_{0}+\gamma\left(\begin{array}{c}{-g_{y}} \\ {g_{x}}\end{array}\right) \qquad \gamma \in \mathbb{R}

l0​+λ∗(lx​ly​​)=g0​+γ(−gy​gx​​)γ∈R其中

g

0

g_0

g0​为灰度等值线上一点，

g

:

=

(

g

x

,

g

y

)

g :=\left(g_{x}, g_{y}\right)

g:=(gx​,gy​)为灰度等值线梯度，整理得：

λ

∗

(

l

0

)

=

⟨

g

,

g

0

−

l

0

⟩

⟨

g

,

l

⟩

\lambda^{*}\left(l_{0}\right)=\frac{\left\langle g, g_{0}-l_{0}\right\rangle}{\langle g, l\rangle}

λ∗(l0​)=⟨g,l⟩⟨g,g0​−l0​⟩​然后我们就可以获得几何视差误差的方差

σ

λ

(

ξ

,

π

)

2

=

J

λ

∗

(

l

0

)

(

σ

l

2

0

0

σ

l

2

)

J

λ

∗

(

l

0

)

T

=

σ

l

2

⟨

g

,

l

⟩

2

\sigma_{\lambda(\xi, \pi)}^{2}=J_{\lambda^{*}\left(l_{0}\right)}\left(\begin{array}{cc}{\sigma_{l}^{2}} & {0} \\ {0} & {\sigma_{l}^{2}}\end{array}\right) J_{\lambda^{*}\left(l_{0}\right)}^{T}=\frac{\sigma_{l}^{2}}{\langle g, l\rangle^{2}}

σλ(ξ,π)2​=Jλ∗(l0​)​(σl2​0​0σl2​​)Jλ∗(l0​)T​=⟨g,l⟩2σl2​​其中

J

λ

∗

(

l

0

)

J_{\lambda^{*}\left(l_{0}\right)}

Jλ∗(l0​)​是

λ

∗

(

l

0

)

\lambda^{*}\left(l_{0}\right)

λ∗(l0​)关于

l

0

l_0

l0​的雅克比，这个方差的定义公式可以参见Probability Models in Engineering and Science的第四章。

光度视差误差 在极线搜索的时候，我们找到的点满足SSD误差最小，也就有：

λ

∗

=

min

⁡

λ

(

i

r

e

f

−

I

p

(

λ

)

)

2

\lambda^{*}=\min _{\lambda}\left(i_{r e f}-I_{p}(\lambda)\right)^{2}

λ∗=λmin​(iref​−Ip​(λ))2这里的

i

r

e

f

i_{ref}

iref​是关键帧的灰度，

I

p

(

λ

)

I_p(λ)

Ip​(λ)是参考帧图像，假设在极线上有过彻底的搜索，得到一个很好的初始位置

λ

0

λ_0

λ0​，对(7)式子中的

I

p

(

λ

)

I_p(λ)

Ip​(λ)做一阶泰勒展开，然后对增量

Δ

λ

Δλ

Δλ求导并令式子为零，则可解出

λ

∗

(

I

)

=

λ

0

+

Δ

λ

=

λ

0

+

(

i

r

e

f

−

I

p

(

λ

0

)

)

g

p

−

1

\lambda^{*}(I)=\lambda_{0}+\Delta \lambda=\lambda_{0}+\left(i_{r e f}-I_{p}\left(\lambda_{0}\right)\right) g_{p}^{-1}

λ∗(I)=λ0​+Δλ=λ0​+(iref​−Ip​(λ0​))gp−1​这里的

g

p

−

1

g^{−1}_p

gp−1​是图像

I

p

I_p

Ip​极线上的梯度，因此是一维的，因此有

σ

λ

(

I

)

2

=

J

λ

∗

(

I

)

(

σ

i

2

0

0

σ

i

2

)

J

λ

∗

(

I

)

T

=

2

σ

i

2

g

p

2

\sigma_{\lambda(I)}^{2}=J_{\lambda^{*}(I)}\left(\begin{array}{cc}{\sigma_{i}^{2}} & {0} \\ {0} & {\sigma_{i}^{2}}\end{array}\right) J_{\lambda^{*}(I)}^{T}=\frac{2 \sigma_{i}^{2}}{g_{p}^{2}}

σλ(I)2​=Jλ∗(I)​(σi2​0​0σi2​​)Jλ∗(I)T​=gp2​2σi2​​这里由于同时对关键帧和参考帧的灰度都计算了噪声，因此会有2这个系数，这个方程是一个线性方程，这里的

σ

i

2

σ^2_i

σi2​是图像的高斯噪声的方差

逆深度计算误差 逆深度的观测方差和像素位置以及两图像的基线长度有关，为

σ

d

,

o

b

s

2

=

α

2

(

σ

λ

(

ξ

,

π

)

2

+

σ

λ

(

I

)

2

)

\sigma_{d, \mathrm{obs}}^{2}=\alpha^{2}\left(\sigma_{\lambda(\xi, \pi)}^{2}+\sigma_{\lambda(I)}^{2}\right)

σd,obs2​=α2(σλ(ξ,π)2​+σλ(I)2​)其中

σ

λ

(

ξ

,

π

)

2

+

σ

λ

(

I

)

2

\sigma_{\lambda(\xi, \pi)}^{2}+\sigma_{\lambda(I)}^{2}

σλ(ξ,π)2​+σλ(I)2​为像素位置的误差，

α

\alpha

α为基线长度引入的误差，定义为：

α

:

=

∂

d

∂

λ

\alpha :=\frac{\partial d}{\partial \lambda}

α:=∂λ∂d​考虑到在极线上搜索匹配点的时候，是使用了多个点，因此实际上这里是给出了逆深度误差的上限：

σ

d

,

o

b

s

2

≤

α

2

(

min

⁡

{

σ

λ

(

ξ

,

π

)

2

}

+

min

⁡

{

σ

λ

(

I

)

2

}

)

\sigma_{d, \mathrm{obs}}^{2} \leq \alpha^{2}\left(\min \left\{\sigma_{\lambda(\xi, \pi)}^{2}\right\}+\min \left\{\sigma_{\lambda(I)}^{2}\right\}\right)

σd,obs2​≤α2(min{σλ(ξ,π)2​}+min{σλ(I)2​})

σ

λ

(

ξ

,

π

)

2

\sigma_{\lambda(\xi, \pi)}^{2}

σλ(ξ,π)2​和

σ

λ

(

I

)

2

\sigma_{\lambda(I)}^{2}

σλ(I)2​上面已经给定了求解方式，

α

\alpha

α怎么求呢，首先，我们可以求得两帧图像对应点在归一化平面上的点：

p

k

=

(

x

k

y

k

1

)

p

r

=

(

x

r

y

r

1

)

=

K

−

1

(

u

r

v

r

1

)

p_{k}=\left(\begin{array}{c}{x_{k}} \\ {y_{k}} \\ {1}\end{array}\right) \qquad p_{r}=\left(\begin{array}{c}{x_{r}} \\ {y_{r}} \\ {1}\end{array}\right)=K^{-1}\left(\begin{array}{c}{u_{r}} \\ {v_{r}} \\ {1}\end{array}\right)

pk​=⎝⎛​xk​yk​1​⎠⎞​pr​=⎝⎛​xr​yr​1​⎠⎞​=K−1⎝⎛​ur​vr​1​⎠⎞​根据两帧间的变换，有

p

r

=

R

⋅

p

k

⋅

d

−

1

+

t

=

(

r

0

r

1

r

2

)

p

k

d

−

1

+

(

t

x

t

y

t

z

)

=

(

r

0

⋅

p

k

⋅

d

−

1

+

t

x

r

1

⋅

p

k

⋅

d

−

1

+

t

y

r

2

⋅

p

k

⋅

d

−

1

+

t

z

)

p_{r}=R \cdot p_{k} \cdot d^{-1}+t=\left(\begin{array}{c}{r_{0}} \\ {r_{1}} \\ {r_{2}}\end{array}\right) p_{k} d^{-1}+\left(\begin{array}{c}{t_{x}} \\ {t_{y}} \\ {t_{z}}\end{array}\right)=\left(\begin{array}{c}{r_{0} \cdot p_{k} \cdot d^{-1}+t_{x}} \\ {r_{1} \cdot p_{k} \cdot d^{-1}+t_{y}} \\ {r_{2} \cdot p_{k} \cdot d^{-1}+t_{z}}\end{array}\right)

pr​=R⋅pk​⋅d−1+t=⎝⎛​r0​r1​r2​​⎠⎞​pk​d−1+⎝⎛​tx​ty​tz​​⎠⎞​=⎝⎛​r0​⋅pk​⋅d−1+tx​r1​⋅pk​⋅d−1+ty​r2​⋅pk​⋅d−1+tz​​⎠⎞​由于差一个尺度因子，我们现在相当得到两个方程。现在我们只需要解一个未知数

d

d

d，至少一个方程就可以。作者为了减少误差，在图像

u

，

v

u，v

u，v方向选取了较长的那个方向的方程来解。这里以

u

u

u方向为例：

x

r

=

r

0

⋅

p

k

⋅

d

−

1

+

t

x

r

2

⋅

p

k

⋅

d

−

1

+

t

z

x_{r}=\frac{r_{0} \cdot p_{k} \cdot d^{-1}+t_{x}}{r_{2} \cdot p_{k} \cdot d^{-1}+t_{z}}

xr​=r2​⋅pk​⋅d−1+tz​r0​⋅pk​⋅d−1+tx​​解出有

d

=

r

2

p

k

⋅

x

r

−

r

0

p

k

t

x

−

t

z

⋅

x

r

d=\frac{r_{2} p_{k} \cdot x_{r}-r_{0} p_{k}}{t_{x}-t_{z} \cdot x_{r}}

d=tx​−tz​⋅xr​r2​pk​⋅xr​−r0​pk​​这样就可以求出来

α

\alpha

α

α

:

=

∂

d

∂

λ

=

∂

d

∂

x

∂

x

∂

u

∂

u

∂

λ

=

r

2

p

k

⋅

t

x

−

r

0

p

k

⋅

t

z

(

t

x

−

t

z

⋅

x

)

2

⋅

f

x

i

n

v

⋅

∂

u

∂

λ

\alpha :=\frac{\partial d}{\partial \lambda}=\frac{\partial d}{\partial x} \frac{\partial x}{\partial u} \frac{\partial u}{\partial \lambda}=\frac{r_{2} p_{k} \cdot t_{x}-r_{0} p_{k} \cdot t_{z}}{\left(t_{x}-t_{z} \cdot x\right)^{2}} \cdot f_{x}^{i n v} \cdot \frac{\partial u}{\partial \lambda}

α:=∂λ∂d​=∂x∂d​∂u∂x​∂λ∂u​=(tx​−tz​⋅x)2r2​pk​⋅tx​−r0​pk​⋅tz​​⋅fxinv​⋅∂λ∂u​这里的

f

x

i

n

v

f^{inv}_x

fxinv​是矩阵

K

−

1

K^{−1}

K−1的第一个元素，

r

i

r_i

ri​为旋转矩阵

R

R

R第

i

i

i行的行向量，

∂

u

/

∂

λ

∂u/∂λ

∂u/∂λ表示了极线段在

u

u

u方向上投影与极线段长度的比例。到此所有原本不知道的量都知道怎么求了，分析到此结束

（2）怎么构建新的深度图？ 在构建新的关键帧时，使用其参考关键帧的深度图来构建当前帧的深度图。这里主要考虑构建新的深度图时，逆深度误差的传播。首先假设两帧间的旋转是很小的，逆深度就可以近似为）：

d

1

(

d

0

)

=

(

d

0

−

1

−

t

z

)

−

1

d_{1}\left(d_{0}\right)=\left(d_{0}^{-1}-t_{z}\right)^{-1}

d1​(d0​)=(d0−1​−tz​)−1这里的

t

z

t_z

tz​是相机沿着光轴方向的位移，

d

1

d_1

d1​的方差是

σ

d

1

2

=

J

d

1

σ

d

0

2

J

d

1

T

+

σ

p

2

=

(

d

1

d

0

)

4

σ

d

0

2

+

σ

p

2

\sigma_{d_{1}}^{2}=J_{d_{1}} \sigma_{d_{0}}^{2} J_{d_{1}}^{T}+\sigma_{p}^{2}=\left(\frac{d_{1}}{d_{0}}\right)^{4} \sigma_{d_{0}}^{2}+\sigma_{p}^{2}

σd1​2​=Jd1​​σd0​2​Jd1​T​+σp2​=(d0​d1​​)4σd0​2​+σp2​这里的

σ

p

2

σ^2_p

σp2​为预测不确定性。在实际求逆深度的时候，是考虑旋转的，把参考关键帧上的点通过SE3变换到当前新的关键帧上来，然后求逆深度。但是求方差则是用上述近似方法求的。

好，上面大致说了下怎么更新参考关键帧的深度图和怎么构建新的深度图这两个问题，最后还有一个问题需要注意是一个像素点只能有一个逆深度的先验值，在得到一个新的观测值的时候，根据方差来判断对新的观测值融合或者舍弃。 （1）当两个逆深度观测值的差值小于

2

σ

2σ

2σ的时候，则认为观测有效，进行深度图更新过程 （2）否则，舍弃新的观测，并且增加先验逆深度的不确定性（深度图更新过程）；否则，当新估计的深度使得点远离相机，则认为该位置是阻塞的（逆深度假设无效），并且舍弃逆深度信息。

7. LSD SLAM中闭环检测功能是怎么实现的？

LSD SLAM中的闭环检测主要是根据视差、关键帧连接关系，找出候选帧，然后对每个候选帧和测试的关键帧之间进行双向Sim3跟踪，如果求解出的两个李代数满足马氏距离在一定范围内，则认为是闭环成功，并且在位姿图中添加边的约束，最后进行全局的BA优化即可。 值得注意的如果候选帧离闭环关键帧近采用的是SE3追踪检测，如果相距较远时采用的是Sim3追踪检测（即以相似变换描述两帧之间的关系），下面对Sim3追踪检测进行一个简单分析： 对于两帧归一化坐标系上的图像点通过Sim3变换有：

ω

(

p

,

D

i

(

p

)

,

ξ

)

:

=

(

x

′

/

z

′

y

′

/

z

′

1

)

with

(

x

′

y

′

z

′

1

)

:

=

exp

⁡

s

i

m

(

3

)

(

ξ

)

(

p

x

/

d

p

y

/

d

1

/

d

1

)

\omega\left(\mathbf{p}, D_{i}(\mathbf{p}), \xi\right) :=\left(\begin{array}{c}{x^{\prime} / z^{\prime}} \\ {y^{\prime} / z^{\prime}} \\ {1 }\end{array}\right) \quad \text { with } \quad\left(\begin{array}{c}{x^{\prime}} \\ {y^{\prime}} \\ {z^{\prime}} \\ {1}\end{array}\right) :=\exp _{\mathfrak{sim}(3)}(\xi)\left(\begin{array}{c}{\mathbf{p}_{x} / d} \\ {\mathbf{p}_{y} / d} \\ {1 / d} \\ {1}\end{array}\right)

ω(p,Di​(p),ξ):=⎝⎛​x′/z′y′/z′1​⎠⎞​ with ⎝⎜⎜⎛​x′y′z′1​⎠⎟⎟⎞​:=expsim(3)​(ξ)⎝⎜⎜⎛​px​/dpy​/d1/d1​⎠⎟⎟⎞​这里依旧使用归一化方差有

E

(

ξ

j

i

)

=

∑

p

∈

Ω

D

i

∥

r

p

2

(

p

,

ξ

j

i

)

σ

r

p

(

p

.

ξ

j

i

)

2

+

r

d

2

(

p

,

ξ

j

i

)

σ

r

d

(

p

,

ξ

j

i

)

2

∥

δ

E\left(\xi_{j i}\right)=\sum_{\mathbf{p} \in \Omega_{D_{i}}}\left\|\frac{r_{p}^{2}\left(\mathbf{p}, \xi_{j i}\right)}{\sigma_{r_{p}\left(\mathbf{p} . \xi_{j i}\right)}^{2}}+\frac{r_{d}^{2}\left(\mathbf{p}, \xi_{j i}\right)}{\sigma_{r_{d}\left(\mathbf{p}, \xi_{j i}\right)}^{2}}\right\|_{\delta}

E(ξji​)=p∈ΩDi​​∑​∥∥∥∥∥​σrp​(p.ξji​)2​rp2​(p,ξji​)​+σrd​(p,ξji​)2​rd2​(p,ξji​)​∥∥∥∥∥​δ​光度残差和方差定义如下：

r

p

(

p

,

ξ

j

i

)

:

=

I

i

(

p

)

−

I

j

(

ω

(

p

,

D

i

(

p

)

,

ξ

j

i

)

)

σ

r

p

(

p

,

ξ

j

j

)

2

:

=

2

σ

I

2

+

(

∂

r

p

(

p

,

ξ

j

i

)

∂

D

i

(

p

)

)

2

V

i

(

p

)

\begin{aligned} r_{p}\left(\mathbf{p}, \xi_{j i}\right) & :=I_{i}(\mathbf{p})-I_{j}\left(\omega\left(\mathbf{p}, D_{i}(\mathbf{p}), \xi_{j i}\right)\right) \\ \sigma_{r_{p}\left(\mathbf{p}, \xi_{j j}\right)}^{2} : &=2 \sigma_{I}^{2}+\left(\frac{\partial r_{p}\left(\mathbf{p}, \xi_{j i}\right)}{\partial D_{i}(\mathbf{p})}\right)^{2} V_{i}(\mathbf{p}) \end{aligned}

rp​(p,ξji​)σrp​(p,ξjj​)2​:​:=Ii​(p)−Ij​(ω(p,Di​(p),ξji​))=2σI2​+(∂Di​(p)∂rp​(p,ξji​)​)2Vi​(p)​深度残差和方差定义如下：

r

d

(

p

,

ξ

j

i

)

=

[

p

′

]

3

−

D

j

(

[

p

′

]

1

,

2

)

σ

r

d

(

p

,

ξ

j

j

)

2

=

V

j

(

[

p

′

]

1

,

2

)

(

∂

r

d

(

p

,

ξ

j

i

)

∂

D

j

(

p

′

]

1

,

2

)

)

+

V

i

(

p

)

(

∂

r

d

(

p

,

ξ

i

j

)

∂

D

i

(

p

)

)

\begin{aligned} r_{d}\left(\mathbf{p}, \xi_{j i}\right) &=\left[\mathbf{p}^{\prime}\right]_{3}-D_{j}\left(\left[\mathbf{p}^{\prime}\right]_{1,2}\right) \\ \sigma_{r_{d}\left(\mathbf{p}, \xi_{j j}\right)}^{2} &=V_{j}\left(\left[\mathbf{p}^{\prime}\right]_{1,2}\right)\left(\frac{\partial r_{d}\left(\mathbf{p}, \xi_{j i}\right)}{\partial D_{j}\left(\mathbf{p}^{\prime}\right]_{1,2} )}\right)+V_{i}(\mathbf{p})\left(\frac{\partial r_{d}\left(\mathbf{p}, \xi_{i j}\right)}{\partial D_{i}(\mathbf{p})}\right) \end{aligned}

rd​(p,ξji​)σrd​(p,ξjj​)2​​=[p′]3​−Dj​([p′]1,2​)=Vj​([p′]1,2​)(∂Dj​(p′]1,2​)∂rd​(p,ξji​)​)+Vi​(p)(∂Di​(p)∂rd​(p,ξij​)​)​与方差归一化的SE3跟踪算法相同，利用高斯牛顿法进行迭代求解即可

δ

ξ

∗

=

arg

⁡

min

⁡

δ

ξ

∑

p

∈

Ω

D

i

(

ω

h

σ

r

p

(

p

,

ξ

(

n

)

2

(

r

p

(

p

,

ξ

(

n

)

)

+

J

p

(

ξ

(

n

)

)

δ

ξ

)

2

+

ω

h

σ

r

d

(

p

,

ξ

(

n

)

2

(

r

d

(

p

,

ξ

(

n

)

+

J

d

(

ξ

(

n

)

)

δ

ξ

)

)

2

)

\delta \xi^{*}=\arg \min _{\delta \xi} \sum_{\mathbf{p} \in \Omega_{D_{i}}}\left(\frac{\omega_{h}}{\sigma_{r_{p}\left(\mathbf{p}, \xi^{(n)}\right.}^{2}}\left(r_{p}\left(\mathbf{p}, \xi^{(n)}\right)+\mathbf{J}_{p}\left(\xi^{(n)}\right) \delta \xi\right)^{2}+\frac{\omega_{h}}{\sigma_{r_{d}\left(\mathbf{p}, \xi^{(n)}\right.}^{2}}\left(r_{d}\left(\mathbf{p}, \xi^{(n)}+\mathbf{J}_{d}\left(\xi^{(n)}\right) \delta \xi\right)\right)^{2}\right)

δξ∗=argδξmin​p∈ΩDi​​∑​⎝⎛​σrp​(p,ξ(n)2​ωh​​(rp​(p,ξ(n))+Jp​(ξ(n))δξ)2+σrd​(p,ξ(n)2​ωh​​(rd​(p,ξ(n)+Jd​(ξ(n))δξ))2⎠⎞​而方差的处理方式也相同，最后统一成一个系数

w

h

w_h

wh​，更细节的推导就不在此赘述了，需要了解的朋友可以参考博客LSD-SLAM笔记之一致性约束

上面简单介绍了下Sim3追踪检测，在LSD SLAM的论文中，为了避免误检测，采用的是双向Sim3追踪检测，对每一个候选帧都计算其与检测关键帧彼此跟踪的Sim3，然后计算两者的马氏距离：

e

(

ξ

j

k

i

,

ξ

i

j

k

)

:

=

(

ξ

j

k

i

∘

ξ

i

j

k

)

T

(

Σ

j

k

i

+

Adj

⁡

j

k

i

Σ

i

j

k

Adj

⁡

j

k

i

T

)

−

1

(

ξ

j

k

i

∘

ξ

i

j

k

)

e\left(\xi_{j_{k} i}, \xi_{i j_{k}}\right) :=\left(\xi_{j_{k} i} \circ \xi_{i j_{k}}\right)^{T}\left(\boldsymbol{\Sigma}_{j_{k} i}+\operatorname{Adj}_{j_{k} i} \mathbf{\Sigma}_{i j_{k}} \operatorname{Adj}_{j_{k} i}^{T}\right)^{-1}\left(\xi_{j_{k} i} \circ \xi_{i j_{k}}\right)

e(ξjk​i​,ξijk​​):=(ξjk​i​∘ξijk​​)T(Σjk​i​+Adjjk​i​Σijk​​Adjjk​iT​)−1(ξjk​i​∘ξijk​​)其实双向跟踪在一开始选取临近的候选帧的时候做SE3跟踪也用到了，但是只是对其中的??(3)做了2-Norm距离的判断。

总结

我利用这段时间把ORB SLAM2、SVO和LSD SLAM都总结了一下，膜拜了各位前辈的博客（感谢各位前辈的分享），让感觉数学推导真的是一件很有意思的事情，能用代码把推导结果实现出来就更有意思了，同时感觉要成为一名合格的机器人工程师还有很长的路要走很多东西要学，像到现在，ORB SLAM都和imu结合起来了，SVO都有SVO2.0了，LSD也升级成DSO了，而我仅仅是初步掌握了几个经典框架和一些基本知识而已。 接下来打算花一段时间好好学习下VINS，本科的时候用IMU MPU6050调试过机器人云台，也算一个比较熟悉的硬件了，现在有机会能将新老知识结合到一起想想就觉得激动，一步一个脚印，加油！

此外，对其他SLAM算法感兴趣的同学可以看考我的博客SLAM算法总结——经典SLAM算法框架总结