DSO windowed optimization 代码 (2)
3 非 Schur Complement 部分信息计算
参考《DSO windowed optimization 公式》,非Schur Complement 部分指 \(H_{XX}\) 和 \(J^T_{X}r\)。
3.1 AccumulatedTopHessianSSE::addPoint()
优化的局部信息计算
EnergyFunctional::accumulateAF_MT() 与 EnergyFunctional::accumulateLF_MT() 遍历每一个点,对每一个点调用 AccumulatedTopHessianSSE::addPoint()。在 AccumulatedTopHessianSSE::addPoint() 中遍历点的每一个 residual。计算所有优化系统的信息,存储在每个点的局部变量和 EnergyFunctional 的局部变量中。
3.1.1 resApprox
首先搞定resApprox
。由 VecNRf 可知,这东西是 8x1 的矩阵(也就是每个 residual 都是八个像素点的组合)。
VecNRf resApprox;
if(mode==0) // active
resApprox = rJ->resF;
if(mode==2) // marginalize
resApprox = r->res_toZeroF;
if(mode==1) // linearized
{
// compute Jp*delta
__m128 Jp_delta_x = _mm_set1_ps(rJ->Jpdxi[0].dot(dp.head<6>())+rJ->Jpdc[0].dot(dc)+rJ->Jpdd[0]*dd);
__m128 Jp_delta_y = _mm_set1_ps(rJ->Jpdxi[1].dot(dp.head<6>())+rJ->Jpdc[1].dot(dc)+rJ->Jpdd[1]*dd);
__m128 delta_a = _mm_set1_ps((float)(dp[6]));
__m128 delta_b = _mm_set1_ps((float)(dp[7]));
for(int i=0;i<patternNum;i+=4)
{
// PATTERN: rtz = resF - [JI*Jp Ja]*delta.
__m128 rtz = _mm_load_ps(((float*)&r->res_toZeroF)+i);
rtz = _mm_add_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JIdx))+i),Jp_delta_x));
rtz = _mm_add_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JIdx+1))+i),Jp_delta_y));
rtz = _mm_add_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JabF))+i),delta_a));
rtz = _mm_add_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JabF+1))+i),delta_b));
_mm_store_ps(((float*)&resApprox)+i, rtz);
}
}
Residual 有三种情况:
- active 情况最简单,直接是 residual。
- marginalize 的情况比较复杂,res_toZeroF 在EFResidual::fixLinearizationF()赋值,而 res_toZeroF 与下面计算的 rtz 是类似的。
- linearized 在这里已经给出了其赋值的方法,下面会说到,linearized residual 是不存在的。
所谓的 linearied residual 是指 EFResidual::isActive() 与 EFResidual::isLinearized 都为 true 的 Residual。初始阶段 isLinearized 为 false,只要搞清楚 isLinearized 在什么时候设置为 true 就可以了解到 linearized residual 是何种意思。查找了 EFResidual::isLinearized 只在 EFResidual::fixLinearizationF 中设置为 true,而 EFResidual::fixLinearizationF() 仅仅只在 FullSystem::flagPointsForRemoval() 中调用。在此处,将那些符合 2 种情况(1. 因为 residual 太少造成了 Out Of Boundary(这里考虑到将要被 marginalize 掉的帧的影响),2. 主帧要被 marginalize 掉)的点的 residual 设置为 linearized。但是这些点紧接着又会在 EnergyFunctional::marginalizePointsF() 中被 marg 掉,被删除掉。最终也没有进入 FullSystem::optimize() 的优化过程中。我在 AccumulatedTopHessianSSE::addPoint() 的这个位置设置了 conditional breakpoint (mode==1),或者assert(mode!=1)
,实验证明 linearized residual 是不存在的。
active residual 时,
resApprox
对应的就是简单的 \(r_{21}\)。linearized residual 时,还要看这个代码是什么意思。
\(\begin{bmatrix} \text{Jp_delta_x} \\ \text{Jp_delta_y} \end{bmatrix} = {\partial x_2 \over \partial \xi_1}{\delta \xi_1} + {\partial x_2 \over \partial \xi_2}{\delta \xi_2} + {\partial x_2 \over \partial C}{\delta C} + {\partial x_2 \over \partial \rho_1}{\delta \rho_1}\)
\(\begin{bmatrix} \text{delta_a} \\ \text{delta_b}\end{bmatrix} = {\partial l_{21} \over \partial l_1}{\delta l_1} + {\partial l_{21} \over \partial l_2}{\delta l_2}\)
\(\text{rtz} = {\partial r_{21} \over \partial \xi_1}{\delta \xi_1} + {\partial r_{21} \over \partial \xi_2}{\delta \xi_2} + {\partial r_{21} \over \partial C}{\delta C} + {\partial r_{21} \over \partial \rho_1}{\delta \rho_1} + {\partial r_{21} \over \partial l_1}{\delta l_1} + {\partial r_{21} \over \partial l_2}{\delta l_2}\)
res_toZeroF
与rtz
相同。resApprox
= res_toZeroF
+ rtz
。
3.1.2 acc
在 AccumulatedTopHessianSSE::addPoint() 函数计算了 Hessian 矩阵。而这里的 Hessian 矩阵是存储了两个帧之间的相互信息,所有的信息存储在 AccumulatedTopHessianSSE::acc 中,acc
是一个数组,大小是 8*8 个,位置 (i, j) 上对应的是 i 帧与 j 帧的相互信息。
AccumulatorApprox 也就是AccumulatedTopHessianSSE::acc 变量的“基础”类型。这个类型对应着 13x13 的矩阵。这个矩阵经过阅读代码,可以知道存储的是以下信息。
\]
\]
\]
&= \begin{bmatrix} {\partial r_{21} \over \partial C}^T_{4\times8} \\ {\partial r_{21} \over \partial \xi_{21}}^T_{6\times8} \\ {\partial r_{21} \over \partial l_{21}}^T_{2\times8} \\ {r_{21}}^T_{1\times8} \end{bmatrix} \begin{bmatrix} {\partial r_{21} \over \partial C}_{8\times4} & {\partial r_{21} \over \partial \xi_{21}}_{8\times6} & {\partial r_{21} \over \partial l_{21}}_{8\times2} & {r_{21}}_{8\times1}\end{bmatrix} \notag \\
&= \begin{bmatrix} {{\partial r_{21} \over \partial C}^T{\partial r_{21} \over \partial C}}_{4\times4} & {{\partial r_{21} \over \partial C}^T{\partial r_{21} \over \partial \xi_{21}}}_{4\times6} & {{\partial r_{21} \over \partial C}^T{\partial r_{21} \over \partial l_{21}}}_{4\times2} & {{\partial r_{21} \over \partial C}^T{r_{21}}}_{4\times1} \\ {{\partial r_{21} \over \partial \xi_{21}}^T{\partial r_{21} \over \partial C}}_{6\times4} & {{\partial r_{21} \over \partial \xi_{21}}^T{\partial r_{21} \over \partial \xi_{21}}}_{6\times6} & {{\partial r_{21} \over \partial \xi_{21}}^T{\partial r_{21} \over \partial l_{21}}}_{6\times2} & {{\partial r_{21} \over \partial \xi_{21}}^T{r_{21}}}_{6\times1} \\ {{\partial r_{21} \over \partial l_{21}}^T{\partial r_{21} \over \partial C}}_{2\times4} & {{\partial r_{21} \over \partial l_{21}}^T{\partial r_{21} \over \partial \xi_{21}}}_{2\times6} & {{\partial r_{21} \over \partial l_{21}}^T{\partial r_{21} \over \partial l_{21}}}_{2\times2} & {{\partial r_{21} \over \partial l_{21}}^T{r_{21}}}_{2\times1} \\ {{r_{21}}^T{\partial r_{21} \over \partial C}}_{1\times4} & {{r_{21}}^T{\partial r_{21} \over \partial \xi_{21}}}_{1\times6} & {{r_{21}}^T{\partial r_{21} \over \partial l_{21}}}_{1\times2} & {{r_{21}}^T{r_{21}}}_{1\times1} \end{bmatrix} \notag \end{align}\]
代码中的BotRight
对应矩阵右下角 3x3 的分块:
\]
TopRight
对应矩阵右上角 10x3 的分块:
\]
Data
对应左上角 10x10 的分块:
\]
这个 AccumulatorApprox 中存储的 13x13 矩阵并不是优化过程中整体的大矩阵,只是对应着窗口中两帧之间的相互信息。注意到代码中计算调用acc
变量时是这么调用的acc[tid][htIDX]
,int htIDX = r->hostIDX + r->targetIDX * nframes[tid];
,不考虑tid
线程编号,acc
共有8*8=64个。
继续讲完 AccumulatedTopHessianSSE::addPoint 函数。
函数的目标除了计算不同帧之间的相互信息(变量acc
),还需要计算每一个点对于所有 residual 的信息和。即EFPoint
中的成员变量Hdd_accAF, bd_accAF, Hcd_accAF, Hdd_accLF, bd_accLF, Hcd_accLF
,如果这个点是 active 点,那么设置AF
相关的变量,否则设置LF
相关变量,如果是 marginalize 点,清除AF
相关变量的信息。这三个成员变量将用于计算逆深度的优化量。
局部变量Hdd_acc, bd_acc, Hcd_acc
对应着这些EFPoint
的成员变量,最后赋值到成员变量。
3.1.3 bd_acc
, Hdd_acc
, Hcd_acc
JI_r[0] += resApprox[i] *rJ->JIdx[0][i];
JI_r[1] += resApprox[i] *rJ->JIdx[1][i];
...
Vec2f Ji2_Jpdd = rJ->JIdx2 * rJ->Jpdd;
bd_acc += JI_r[0]*rJ->Jpdd[0] + JI_r[1]*rJ->Jpdd[1];
Hdd_acc += Ji2_Jpdd.dot(rJ->Jpdd);
Hcd_acc += rJ->Jpdc[0]*Ji2_Jpdd[0] + rJ->Jpdc[1]*Ji2_Jpdd[1];
JI_r
对应 \({\partial r_{21} \over \partial x_2}^T({\partial r_{21} \over \partial \xi_1}{\delta \xi_1} + {\partial r_{21} \over \partial \xi_2}{\delta \xi_2} + {\partial r_{21} \over \partial C}{\delta C} + {\partial r_{21} \over \partial \rho_1}{\delta \rho_1} + {\partial r_{21} \over \partial l_1}{\delta l_1} + {\partial r_{21} \over \partial l_2}{\delta l_2})\),2x1。
Ji2_Jpdd
对应 \({\partial r_{21} \over \partial x_2}^T{\partial r_{21} \over \partial \rho_1}\),2x1。
bd_acc
对应(1)active 时,\({\partial r_{21} \over \partial \rho_1}^Tr_{21}\);(2)marginalize 时,\({\partial r_{21} \over \partial \rho_1}^T({\partial r_{21} \over \partial \xi_1}{\delta \xi_1} + {\partial r_{21} \over \partial \xi_2}{\delta \xi_2} + {\partial r_{21} \over \partial C}{\delta C} + {\partial r_{21} \over \partial \rho_1}{\delta \rho_1} + {\partial r_{21} \over \partial l_1}{\delta l_1} + {\partial r_{21} \over \partial l_2}{\delta l_2})\)。1x1。
Hdd_acc
对应 \({\partial r_{21} \over \partial \rho_1}^T{\partial r_{21} \over \partial \rho_1}\),1x1。
Hcd_acc
对应 \({\partial r_{21} \over \partial C}^T{\partial r_{21} \over \partial \rho_1}\),4x1。
3.2 AccumulatedTopHessianSSE::stitchDoubleInternal()
优化信息统计
循环for(int k=min;k<max;k++)
循环是遍历所有可能的 (host_frame,target_frame) 组合。
内层循环累积计算accH
就不用看了,这个循环是用于累加多个线程的结果,accH
就是acc[h+nframes*t]
,参照 3.1。
下面的H
(对应 \(H_{XX}\))和b
(对应 \(J^T_{X}r\))的累加,使用了 EnergyFunctional::adHost 和 EnergyFunctional::adTarget。这是因为前面计算的 Jacobian 都是对相对状态的偏导,这两个变量存储的是相对状态对绝对状态的偏导。
adHost[h+nframes*t]
下标是 (t,h),对应公式 \({\partial X_R^{(th)} \over \partial X_R^{(h)}}^T\)。
adTarget[h+nframes*t]
下标是 (t,h),对应公式 \({\partial X_R^{(th)} \over \partial X_R^{(t)}}^T\)。
\(X_R^{(i)}\) 是 i 帧的所有状态,包括 se(3) 和 AffLight 参数,即 \(\begin{bmatrix} \xi_i \\ l_i \end{bmatrix}\)。
最新文章
- ReactNative入门 —— 动画篇(下)
- 编译安装nginx
- Android 子线程测试
- TCL:读取表格(xls)中的数据
- 【BZOJ】【2705】【SDOI2012】Longge的问题
- linux环境: shell初始化文件, for TCSH, CSH
- 组件之间使用Prop传递数据
- sencha touch 总结
- socket-WebSocket-HttpListener-TcpListener服务端客户端的具体使用案例
- RescuePro Deluxe恢复软件SanDisk
- Linux->;Windows主机目录和文件名中文乱码恢复
- Django框架----logging配置
- Asp .Net core 2 学习笔记(3) —— 静态文件
- 【转】ZooKeeper学习第二期--Zookeeper命令操作
- PHP操作Redis常用技巧
- java并发编程:线程安全管理类--原子操作类--AtomicStampedReference<;V>;
- 【CSAPP笔记】12. 高速缓存存储器
- JAVA随笔(二)
- 「小程序JAVA实战」小程序的举报功能开发(68)
- Install dotNet Core on Mac
热门文章
- java强引用,软引用,弱引用,虚引用
- python numpy 数组拼接
- IDEA常见设置
- WebSocket安卓客户端实现详解(三)–服务端主动通知
- Leetcode 461.汉明距离 By Python
- 1.Zabbix报错信息:It probably means that the systems requires more physical memory.
- 自学Linux Shell12.1-结构化命令if-then
- 架构师成长之路6.4 DNS服务器搭建(部署主从DNS)
- BZOJ 4785 [Zjoi2017]树状数组 | 二维线段树
- [USACO 2018 December Contest]作业总结