标签：initializeFromInitializer partial bmatrix 笔记 DSO xi th frac 位姿

目录

• 一、概述
• 二、insertFrame()
• • 2.1 原理
  • • a、伴随表示
    • b、边缘化
  • 2.2 代码
• 三、其他部分
• 四、总结

一、概述

自己以前写的东西居然快看不懂，半途而废的代价太大了（哭）。
言归正传，如果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

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。