目录
一、概述
自己以前写的东西居然快看不懂，半途而废的代价太大了（哭）。
言归正传，如果trackFrame认为光流符合要求，大概是位移足够大，那么再为之后5帧优化光度误差后，trackFrame返回true。
此时程序将当前帧输入initializeFromInitializer。
二、insertFrame()
2.1 原理
a、伴随表示
参考《视觉SLAM十四讲》、Adjoint of SE(3)和DSO windowed optimization 公式。
李群对乘法封闭，李群和李代数之间一一对应，李代数描述局部导数关系。
伴随表示定义式：
E
x
p
(
A
d
T
⋅
ξ
)
≐
T
E
x
p
(
ξ
)
T
−
1
\mathrm{Exp}(Ad_T\cdot \xi)\doteq T\mathrm{Exp}(\xi)T^{-1}
Exp(AdT⋅ξ)≐TExp(ξ)T−1然后这个
A
d
T
Ad_T
AdT可以保持定义在群中的二元运算，就是说李群的伴随表示还是李群。
把伴随表示用到特殊欧氏群SE(3)，给定
T
=
[
R
t
0
T
1
]
T=\begin{bmatrix} R & t \\ 0^T & 1 \end{bmatrix}
T=[R0Tt1]可以得到
A
d
T
⋅
[
ρ
ϕ
]
=
[
ρ
ˉ
ϕ
ˉ
]
Ad_T\cdot \begin{bmatrix} \rho \\ \phi \end{bmatrix}= \begin{bmatrix} \bar{\rho} \\ \bar{\phi} \end{bmatrix}
AdT⋅[ρϕ]=[ρˉϕˉ]其中
A
d
T
=
[
R
t
∧
R
0
T
R
]
Ad_T= \begin{bmatrix} R & t^{\wedge }R \\ 0^T & R \end{bmatrix}
AdT=[R0Tt∧RR]calcResAndGS函数里，求的是光度残差对相对位姿的偏导，而利用伴随表示可以求相对位姿对绝对位姿的导数（推导略）：
∂
ξ
t
h
∂
ξ
h
=
−
A
d
T
t
h
\frac{\partial \xi_{th}}{\partial \xi_h}=-Ad_{T_{th}}
∂ξh∂ξth=−AdTth
∂
ξ
t
h
∂
ξ
t
=
I
\frac{\partial \xi_{th}}{\partial \xi_t}=I
∂ξt∂ξth=I
这里的伴随表示对应程序里的hostToTarget.Adj()。
b、边缘化
边缘化原理不算难。比如对二元一次方程组
[
h
1
h
2
h
3
h
4
]
[
x
y
]
=
[
b
1
b
2
]
\begin{bmatrix} h_1 & h_2 \\ h_3 & h_4 \end{bmatrix}\begin{bmatrix} x \\ y \end{bmatrix}=\begin{bmatrix} b_1 \\ b_2 \end{bmatrix}
[h1h3h2h4][xy]=[b1b2]
直接解法就是先消元，如果消去y，那方程可以变成
[
h
1
−
h
2
h
4
−
1
h
3
0
h
3
h
4
]
[
x
y
]
=
[
b
1
−
h
2
h
4
−
1
b
2
b
2
]
\begin{bmatrix} h_1-h_2h_4^{-1}h_3 & 0 \\ h_3 & h_4 \end{bmatrix}\begin{bmatrix} x \\ y \end{bmatrix}=\begin{bmatrix} b_1-h_2h_4^{-1}b_2 \\ b_2 \end{bmatrix}
[h1−h2h4−1h3h30h4][xy]=[b1−h2h4−1b2b2]
把常数拓展成矩阵，可以应用到SLAM优化的高斯牛顿方程中。
目的一方面是方便求解线性方程组，另一方面是在尽量保证信息不丢失的前提下，减少变量个数。
参考DSO边缘化理论与代码对应做个笔记、DSO 中的Windowed Optimization。
从上面的方程看出，如果直接丢掉y，x会失去所有约束。所以在丢掉y之前，要为x添加对应的约束z。
写成概率
P
(
x
,
y
)
=
P
(
x
)
P
(
y
∣
x
)
P(x,y)=P(x)P(y|x)
P(x,y)=P(x)P(y∣x)
约束z就是条件概率。P(x)被称为边缘概率，这也被称为边缘化。
原理明明很简单，放到dso里就很复杂，可能看到代码就慢慢能懂吧大概。
2.2 代码
FrameHessian* firstFrame = coarseInitializer->firstFrame;
firstFrame->idx = frameHessians.size();
frameHessians.push_back(firstFrame);
firstFrame->frameID = allKeyFramesHistory.size();
allKeyFramesHistory.push_back(firstFrame->shell);
将初始化结果coarseInitializer保存到关键帧容器frameHessians中后，程序调用insertFrame()。
bM.conservativeResize(8*nFrames+CPARS);//调整矩阵大小
HM.conservativeResize(8*nFrames+CPARS,8*nFrames+CPARS);
这里应该是初始化两个矩阵用于边缘化，8包括6*度位姿、2个光度变换参数，另外CPARS=4表示4个相机内参。
然后程序调用setAdjointsF()。
SE3 hostToTarget = target->get_worldToCam_evalPT() * host->get_worldToCam_evalPT().inverse();
计算 T t h = T t w ∗ T h w − 1 T_{th}=T_{tw}*T_{hw}^{-1} Tth=Ttw∗Thw−1，这里给定的是从世界坐标系到相机坐标系的变换。
AH.topLeftCorner<6,6>() = -hostToTarget.Adj().transpose();//\frac{\partial \xi_{th}}{\partial \xi_h}=-Ad_{T_{th}}
AT.topLeftCorner<6,6>() = Mat66::Identity();
计算 ∂ ξ t h ∂ ξ h \frac{\partial \xi_{th}}{\partial \xi_h} ∂ξh∂ξth和 ∂ ξ t h ∂ ξ t \frac{\partial \xi_{th}}{\partial \xi_t} ∂ξt∂ξth，寻求绝对位姿和相对位姿之间的关系。
Vec2f affLL = AffLight::fromToVecExposure(host->ab_exposure, target->ab_exposure, host->aff_g2l_0(), target->aff_g2l_0()).cast<float>();
AT(6,6) = -affLL[0];
AH(6,6) = affLL[0];
AT(7,7) = -1;
AH(7,7) = affLL[0];
因为这里的
a
i
a_i
ai已经不是第一帧，所以光度残差
r
=
I
t
+
a
I
h
+
b
r=I_t+aI_h+b
r=It+aIh+b中的参数写成
a
=
−
E
x
p
(
a
t
−
a
h
)
Δ
t
t
Δ
t
h
a=-Exp(a_t-a_h)\frac{\Delta t_t}{\Delta t_h}
a=−Exp(at−ah)ΔthΔtt
b
=
−
b
t
+
a
b
h
b=-b_t+ab_h
b=−bt+abh然后计算
∂
a
∂
a
t
\frac{\partial a}{\partial a_t}
∂at∂a、
∂
a
∂
a
h
\frac{\partial a}{\partial a_h}
∂ah∂a、
∂
b
∂
b
t
\frac{\partial b}{\partial b_t}
∂bt∂b和
∂
b
∂
b
h
\frac{\partial b}{\partial b_h}
∂bh∂b保存到AT和AH中。
最后得到AH（AT同理）
A
H
=
d
i
a
g
(
∂
ξ
t
h
∂
ξ
h
,
∂
a
∂
a
h
,
∂
b
∂
b
h
)
AH=diag( \frac{\partial \xi_{th}}{\partial \xi_h},\frac{\partial a}{\partial a_h},\frac{\partial b}{\partial b_h} )
AH=diag(∂ξh∂ξth,∂ah∂a,∂bh∂b)
adHost[h+t*nFrames] = AH;
adTarget[h+t*nFrames] = AT;
这里是保存窗口内所有帧之间的AH和AT。
三、其他部分
for(FrameHessian* fh : frameHessians)
{
fh->targetPrecalc.resize(frameHessians.size());
for(unsigned int i=0;i<frameHessians.size();i++)
fh->targetPrecalc[i].set(fh, frameHessians[i], &Hcalib);
}
insertFrame后程序调用setPrecalcValues。
遍历所有关键帧，resize()设置容器大小；
set()计算窗口内，所有关键帧之间的位姿变换和光度变换参数，好像还包括自己对自己。
adHTdeltaF[idx] = frames[h]->data->get_state_minus_stateZero().head<8>().cast<float>().transpose() * adHostF[idx]
+frames[t]->data->get_state_minus_stateZero().head<8>().cast<float>().transpose() * adTargetF[idx];
程序调用setDeltaF，因为在trackFrame中优化的状态是六*度姿态 ξ 21 \xi_{21} ξ21和光度仿射变换参数[a,b]，这里利用伴随矩阵计算绝对位姿和光度变换参数的变化。
firstFrame->pointHessians.reserve(wG[0]*hG[0]*0.2f);
firstFrame->pointHessiansMarginalized.reserve(wG[0]*hG[0]*0.2f);
firstFrame->pointHessiansOut.reserve(wG[0]*hG[0]*0.2f);
reserve修改容器预留空间为20%图像大小。这里应该是限制计算量。
float sumID=1e-5, numID=1e-5;
for(int i=0;i<coarseInitializer->numPoints[0];i++)
{
sumID += coarseInitializer->points[0][i].iR;
numID++;
}
float rescaleFactor = 1 / (sumID / numID);
iR我原来理解是特征的逆深度，在propagateDown和propagateUp里经过各种加权和计算得到。所以这里应该是求图像平均深度。
float keepPercentage = setting_desiredPointDensity / coarseInitializer->numPoints[0];
if(!setting_debugout_runquiet)
printf("Initialization: keep %.1f%% (need %d, have %d)!\n", 100*keepPercentage,
(int)(setting_desiredPointDensity), coarseInitializer->numPoints[0] );
计算目标特征数/实际特征数，并输出到终端。
//遍历所有特征
ph->setIdepthScaled(point->iR*rescaleFactor);//为所有特征设置尺度
ph->setIdepthZero(ph->idepth); //用到FEJ中，暂时超纲
程序为每个特征，调用insertPoint()，点特征准备用于后端优化。
SE3 firstToNew = coarseInitializer->thisToNext;//当前帧相对第一帧的位姿
//addActiveFrame中最开始出现的代码
firstFrame->shell->camToWorld = SE3();
...
newFrame->shell->camToWorld = firstToNew.inverse();
...
重设待优化量，这里也看出dso初始化过程是针对当前帧和第一帧进行光度残差优化。在选取位移比较大的第一帧以后，再往后运行5帧。
printf("INITIALIZE FROM INITIALIZER (%d pts)!\n", (int)firstFrame->pointHessians.size());
终端输出第一帧的特征点数。
四、总结
initializeFromInitializer函数是对trackFrame的结果进行一些处理，然后发送给其他的模块，类似中转站，主要作用如下：
首先调用insertFrame()，插入第一帧到能量帧容器，并且计算相对位姿和绝对位姿的关系；
调用setPrecalcValues()，set()计算关键帧之间的状态作为预计算值；setDeltaF()计算状态改变量
δ
x
\delta x
δx；
计算图像平均尺度，用iR计算，但iR的实际意义存疑；
删除多余点，限制计算量，为后端优化做准备；
初始化shell为下一次初始化做准备。