其他分享
首页 > 其他分享> > DSO学习笔记六 initializeFromInitializer函数

DSO学习笔记六 initializeFromInitializer函数

作者：互联网

目录

一、概述

自己以前写的东西居然快看不懂，半途而废的代价太大了（哭）。
言归正传，如果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=[R0T​t1​]可以得到
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​=[R0T​t∧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} [h1​h3​​h2​h4​​][xy​]=[b1​b2​​]
直接解法就是先消元，如果消去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​−h2​h4−1​h3​h3​​0h4​​][xy​]=[b1​−h2​h4−1​b2​b2​​]
把常数拓展成矩阵，可以应用到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为下一次初始化做准备。

标签：initializeFromInitializer,partial,bmatrix,笔记,DSO,xi,th,frac,位姿
来源： https://blog.csdn.net/weixin_43424002/article/details/114629354