DSO程式碼筆記【待整理】
經過一系列準備工作後
fullSystem->addActiveFrame(img,id);
正式跑DSO。
每個幀的資料儲存於FrameHessian結構體中，FrameHessian即是一個帶著狀態變數與Hessian資訊的幀。 Jk = [JI*Jgeo,Jphoto]，分別偉影象、幾何、光照雅可比
幾何和光度的雅可比，相對自變數來說通常是光滑函式；而影象雅可比則依賴影象資料，不夠光滑；所以，在優化過程中，幾何和光度的雅可比僅在迭代開始時計算一次，此後固定不變[1]。而影象雅可比則隨著迭代更新。這種做法稱為First-Estimate-Jacobian（FEJ），在VIO中也會經常用到[8]。它可以減小計算量、防止優化往錯誤的地方走太多，也可以在邊緣化過程中保證零空間的維度不會降低
FrameShell.h
類似連結串列？包含camToTrackingRef和FrameShell*
MatrixAccumulators.hpp
一個累積器有三層快取如下所示。SSE的話每個快取器的大小是4*(1+2+…+X)，比如acc9就是4*45，普通的話沒有4×
| name | usage | num |
|---|---|---|
| SSEData | 一級快取 | numIn1 1000 |
| SSEData1 | 二級快取 | numIn1k |
| SSEData1 | 三級快取 | numIn1m |
update函式進行更新，每個累加器過載。大致都是H= J^T * W * J
void shiftUp(bool force) 是 向上級儲存器轉換， 每次update後都檢查一次，如果低階快取數量過大或force=true，數值付給高層並且清空低層 finish函式就是強制shiftUp後，全部資料累加到最上層，最後取得資料也就是1m的資料。A=1m。 以上是普通的情況，如果有SSE的話就是4位一起加速，原理一樣。
updateSingle函式就是普通更新，updateSSE就是加速更新。 #- [ ] updateSSENoShift存疑 就用到一次，為什麼不shift。
各個累加器： acc9在初始化和track中都有，初始化中，acc9的update輸入是dp0-7和r，前8個是光度誤差r對se3的六個導數和對a和b的導數, acc9SC
PixelSelector.cpp
makemaps在setFirst和makeNewTraces的時候用到。 對於高層，選點直接是grid中梯度最大的點makePixelStatus，只在setFirst的時候用 int PixelSelector::makeMaps( const FrameHessian* const fh, float* map_out, float density, int recursionsLeft, bool plot, float thFactor) { makeHists(fh); Eigen::Vector3i n = this->select(fh, map_out,currentPotential, thFactor); n[0]+n[1]+n[2]就是選中的點數，和density決定quotia。 然後判斷quotia，不合適的話改步長pot再遞迴。 }
void PixelSelector::makeHists(const FrameHessian* const fh)
對absSquaredGrad處理，先分割成32*32的塊，在每塊中，遍歷每個點{直方圖統計梯度0-50，賦值給gradHist. }，
然後給每塊生成一個動態閾值，這個閾值就是直方圖中1梯度開始求和，直到=0梯度點的數量 × setting_minGradHistCut時的梯度 ，
再+setting_minGradHistAdd， 賦值給ths
然後對32*32的塊做閾值平滑處理，賦值給thsSmoothed，這就是最終的動態閾值。後續select處理的時候點>>5，正好是對32做一個取值處理。
遞迴函式實現點管理，即step1，過程如下： 輸入引數pot決定了步長。
Eigen::Vector3i PixelSelector::select(const FrameHessian* const fh,
float* map_out, int pot, float thFactor)
4倍步長遍歷
{
在4倍這個大塊中2倍步長遍歷
{
在2倍大塊中1倍步長遍歷
{
挑點的原則是在隨機選取的方向上投影模最大，優先選光度，其次dx、dy。
分別增加返回的Vector3i的數量。
}
}
}
ImmaturePoint.cpp
class ImmaturePoint: 建構函式：hostFrame、生成點的位置、梯度和權重
traceOn():– 初始化後的第一幀的所有候選點idepth_min/max都是1； 更新host上的深度？？這個函式是將關鍵幀上的點與非關鍵幀影像進行匹配，得到在非關鍵幀影像上的位置，知道這個資訊就可以更新深度了。先進行極線搜尋，搜尋得到一個比較好的值（energy最小），再使用高斯牛頓優化，找個更好的匹配。最後更新一波深度–只有一個範圍沒有確切之，idepth_min~idepth_max，
在單目SLAM中，所有地圖點在一開始被觀測到時，都只有一個2D的畫素座標，其深度是未知的。這種點在DSO中稱為未成熟的地圖點：Immature Points。隨著相機的運動，DSO會在每張影象上追蹤這些未成熟的地圖點，這個過程稱為trace——實際上是一個沿著極線搜尋的過程，十分類似於svo的depth filter。Trace的過程會確定每個Immature Point的逆深度和它的變化範圍。如果Immature Point的深度（實際中為深度的倒數，即逆深度）在這個過程中收斂，那麼我們就可以確定這個未成熟地圖點的三維座標，形成了一個正常的地圖點。
linearizeResidual() calcResidual() 不成熟點過程： –初始化後，在initialforminitialer時建立，然後在makeKeyFrame中，FullSystem::traceNewCoarse()中通過traceon()設定每個未成熟點的性質，然後activatePointsMT()啟用點（實際上是通過FullSystem::optimizeImmaturePoint將好的未成熟點轉化為PointHessian），然後後面優化的都是成熟點了。然後FullSystem::makeNewTraces()建立新的未成熟點，
HessianBlock.cpp
struct CalibHessian:相機內參
struct PointHessian:–活躍點 建構函式：由ImmaturePoint得到，並設定狀態、IdepthScaled vector<PointFrameResidual*> residuals isOB()–is out boundary isInlierNew()
struct FrameHessian:–一個帶著狀態變數與Hessian資訊的幀 vector<PointHessian*> pointHessian pointHessianMarginalized pointHessianOut–包含的所有active點 邊緣化點 outlier點 Vec10 state_** 狀態變數 前6個是xi(t rot) 後4個是光度a b EFFrame* efFrame setState() 更新T
struct FrameFramePrecalc:–恢復RT並計算各種預算量 set()–根據兩個FrameHessian計算host到target的點座標變換矩陣和AffLight – [R t;0 1]=TjTi^-1
Residuals.cpp
class PointFrameResidual: --某點在兩幀間的殘差 建構函式:param in: PointHessian, host, target 包括 PointHessian和兩個FrameHessian-host and target RawResualJacobian* J的形式見PDF(1.71)
核心函式linearize():計算雅可比J 推導和(1.51-1.70)對應，程式碼實現對應1.71-1.92，需要遍歷每一個 PointFrameResidual將與這個 Residual 相關的導數計算出來，再進行優化。而這些計算出來的相關導數被儲存在 RawResidualJacobian 中
CoarseInitialize.cpp
setFirst()–設定第一幀–makeMaps() in PiexlSelector2.cpp–遞迴函式實現點管理step1，選取候選點。然後makeNN(),
makeNN(): k-最近鄰建立kd-tree 每一層是一個KD樹，對每層的點找到最近鄰的10個點和歸一化距離 除了最高層外給每層的點賦值parent，l層的點在l+1層找parent。
trackFrame():–初始化的track，propagateDown以後，**calcResAndGS()**計算殘差和海塞、Jb，new frame 相對 ref frame 之間存在 8 個引數需要確定，前 6 個引數是 se(3)，後 2 個引數是光度仿射變換的引數。；然後算H，高斯迭代更新姿態和Rt 點的lastHessian就是b的雅可比的最後一位
propagateDown(LVL) – 如果是壞點就用上層parent的深度，好點的話用parent和自己的加權 calcRes():
Vec3f CoarseInitializer::calcResAndGS(
int lvl, Mat88f &H_out, Vec8f &b_out,
Mat88f &H_out_sc, Vec8f &b_out_sc,
const SE3 &refToNew, AffLight refToNew_aff,
bool plot)
{
Accumulator11 E; emmm這裡的11代表1*1 即A是一個數
對篩選後N個點的每一個點
{
算出重投影誤差後，求r對[xi a b]的偏導--dp0-7.
acc9的H=[Jx21^T*Jx21(8*8) Jx21^T*r21(8*1)
r21^T*Jx21(1*8) r21^T*r21(1*1) ]
Tx21是N*(N+8),到了每一個點，dp0-7就是Tx21的每一行,r就是r21的每一行
對於H的更新來說，推出：
acc9.updateSSE(dp0-7,r);
acc9SC.updateSingleWeighted()用的是JbBuffer_new，具體見calcResAndGS推導筆記
}
acc9.finish(); //可以由上面推出左上角的8*8是xi相關的H，右上角8*1是對應的b
總之acc9用的是[ap0-7,r],acc9SC用的是[ap0-7*dd,r*dd]和weight dd
之後就輸出E，是res的總能量，代表總誤差，alpha能量，和點數。
裡面的alpha好像是加上了和深度1平面的約束
}
包括後面calcEC裡面似乎也是和深度1平面的約束。
FullSystem.cpp
makeKeyFrame(): traceNewCoarse()遍歷所有active幀，所有 不成熟點分別呼叫traceOn()，更新不成熟點的逆深度。到這為止和NonKeyFrame一樣/。呼叫FullSystem::flagFramesForMarginalization 標記需要被 marg 掉的幀。向FrameHessian結構中插入新的幀。遍歷視窗中所有幀的pointHessians，建立它們連結自己 hostframe 與當前幀的 PointFrameResidual，加入到EnergyFunctional 中。呼叫 FullSystem::activatePointsMT 遍歷視窗中所有幀的immaturePoints並激活合適的點。接下來就是呼叫 FullSystem::optimize 視窗優化。視窗優化完，就是marg掉不需要的幀和點。去外點後，setCoarseTrackingRef去建立以後要用的參考幀，並通過 makeCoarseDepthL0() 建立coarse tracking lastRef的模板。做完這些操作之後，有一個重要的操作是 FullSystem::makeNewTraces 在當前關鍵幀提取 immaturePoints並生成新的殘差，為後面做準備。
trackNewCoarse()：先假設勻速運動，fh2slast=slast2preslast ，用此得到 lastF_2_fh=fh2slast^-1*lastF2slast, 再依次按照一幀、兩幀、半幀、zero motion的假設，並進行了 a TON of 旋轉作為假設，一起加入到總的假設（lastF_2_fh_tries）中。每個假設再用trackNewestCoarse()與最新關鍵幀匹配。
traceNewCoarse(): 對所有active Frame的不成熟點traceOn()極線搜尋，更新未成熟點
activatePointsMT(): 遍歷frameHessians裡面所有不成熟點，判斷canActitive，啟用那些離現有點距離最遠的候選點，轉換成地圖點並把剩餘的不成熟點刪除。
FullSystem::optimize(): 遍歷所有成熟點，找到非residuals的殘差點，全都放進activeResiduals。然後呼叫 FullSystem::linearizedAll 計算所有residual 的誤差，Energy 求解（1.190-1.200）。然後用for 迴圈優化了，在這個優化過程中，呼叫了若干次 FullSystem::linearizedAll()迭代
FullSystem::linearizedAll，這個函式用來計算系統能量，並且對所有的 residual 求導數。
FullSystem::activePointMT(): 用coarseDistanceMap->makeDistanceMap(0)建立距離圖（同心圓，所以類似BFS），然後從所有幀的所有未成熟點中篩選出好的，組成toOptimize，並用coarseDistanceMap->addIntoDistFinal()建立最終距離圖。 然後new出成熟點PointHessian,呼叫FullSystem::activePointMT_Reductor()即optimizeImmaturePoint(),優化未成熟點。得到成熟點。然後將這些成熟點加入FrameHessian中，，並加入energyfunction的ef->insertPoint()，ef->insertResidual()
FullSystemOptPoint.cpp
FullSystem::optimizeImmaturePoint(): 對每個未成熟點進行優化，
CoarseTracker.cpp
trackNewestCoarse(): 與最新關鍵幀匹配。依舊是高斯牛頓 優化，用calcRes函式進行優化。而這個優化只優化相兩幀的相對狀態（相對位姿 6 + 光度仿射變換 2）。當前 假設得到的結果與前面假設得到的結果比較，當前結果與前面一幀匹配的結果比較（這個跨度有點大，是上一幀），滿足條件 就可以跳出了。
calcRes(lvl ……): 計算當前層各點匹配後的殘差，當是第0層是要計算一堆sumSquaredShift的東西？？，並累加的能量函式
FullSystemOptimize.cpp
FullSystem::optimize(): FullSystem::linearizeAll()::計算所有residual的誤差和（整個系統優化需要降低的能量），這裡也順便進行了 導數準備 工作。 FullSystem::linearizeAll_Reductor():對每個PointFrameResidual呼叫linearize()，計算導數，返回能量stats[0]。
EnergyFuctional.cpp
calcLEnergyF(): calcMEnergyF(): sloveSyestmF():
AccumulatedTopHessianSSE.cpp
addPoint():計算Hessian矩陣 mode=0:ac,1:lineariize,2:marginalize. AccumulatorApprox 也就是AccumulatedTopHessianSSE::acc 變數的“基 礎”型別。這個型別對應著 13x13 的矩陣。 程式碼中通過 stitchDoubleInternal() 進行幀幀之間的所有的 Point 點對的 accH 的求和（1.121）
流程：
各種引數準備
CoarseInitialize->setFrist()-設定第一幀
{
通過makeMaps和makePixelStatus作點管理，根據梯度選點
makeNN()建立層內10個最近點和層間parent點的索引
}
CoarseInitialize->trackFrameWithScaleEstimation()-粗初始化-第二幀開始直接初始化成功
{
bool ac = trackFrameMain(newFrameHessian); //左相機
bool CoarseInitializer::trackFrameMain(FrameHessian* newFrameHessian)
{
resOld = calcResAndGS();
while(1)
{
之前的calc函式算出的H b Hsc bsc，做L-M迭代，算inc
LM的u越大越接近最速演算法，越小越接近GN演算法。如果accept, lambda減半，否則增大4倍，u起著步長縮短的阻尼作用。
對比resNew和resOld，如果新的誤差小於舊的，accept，滿足條件後跳出
跳出條件是迭代次數太多、失敗太多或inc小於閾值
}
}
}
初始化成功後
{
initializeFromInitializer()--為第一幀生成pointHessians 是成熟點，具有逆深度資訊的點
diliverFrame(fh,**true**)--作為關鍵幀新增
}
-初始化結束，追蹤
FullSystem::trackNewCoarse()--返回的res包括ResRMSE和Flow，可以用來判斷是否makeKF
{
得到總假設lastF_2_fh_tries，每次假設用trackNewestCoarse()與最新幀匹配，高斯優化8維inc，找到合適的假設後跳出，得到lastF_2_fh和lastCoarseRMSE等,
}
needToMakeKF,用res判斷
diliverFrame(fh,**needToMakeKF**)---makeKeyFrame() or makeNonKeyFrame()
-如果makeKeyFrame()：
FullSystem::makeKeyFrame()
{
FullSystem::traceNewCoarse()-這一步不管是不是KF都做
{
for(active Frame)
{
for(host->ImmaturePoint)
{
ImmaturePoint:traceOn()-更新不成熟點的逆深度。
}
}
}
FullSystem::flagFramesForMarginalization 標記需要被 marg 掉的幀
向FrameHessian結構中插入新的幀。
ef->insertFrame();
{
setAdjointsF();
connectivityMap;
}
ef->setDeltaF();
遍歷視窗中所有幀的 pointHessians，建立它們連結自己 hostframe 與當前幀的 PointFrameResidual，加入到 EnergyFunctional 中:
for(fh1)
{
fo(ph)
{
PointFrameResidual* r = new PointFrameResidual(ph,fh1,fh);
//fh--當前 fh1-其他
ef->insertResidual(r);
}
}
呼叫 FullSystem::activatePointsMT遍歷視窗中所有幀的immaturePoints並激活合適的點:
activatePointsMT()
{
for(ph : immaturePoints)
{
makeDistanceMap();
bool canActive;
if(dist>=)
toOptimize.push_back(ph);
}
activatePointsMT_Reductor(&optimized,&toOptimize);--optimizeImmaturePoint(optimize[k])
for(newpoint:optimize)
{
ef->insertPoint(newpoint);
for(r:newpoint->residuals)
ef->insertResidual(r);
}
}
ef->makeIDX();
> 感覺，至此為止，把所有FrameHessian(即keyframe)裡面的PointHessian(即成熟點)裡面的所有PointFrameResidual
加入到EnergyFunctional裡面，分別對應EFFrame,EFPoint,EFResidual,接下來可以對此優化了。
接下來就是呼叫 FullSystem::optimize 視窗優化。
{
FullSystem::optimize()
{
在所有幀的所有點的所有 PointFrameResidual 中找到非 linearized的 ，將他們放入變數 activeResiduals 中
linearizeAll(false)
{
linearizeAll_Reductor()
{
對上述每個PointFrameResidual呼叫linearize()，分別計算兩相機的ref和new的殘差和雅可比，返回能量stats[0]=lastEnergyP。
setNewFrameEnergyTH_full()根據linearize後的殘差獲得新的閾值-對good點殘差排序後前70%
}
}
calcLEnergy();--linearized energy, EL=Eab+Ec+Ep
calcMEnergy();--marginalized energy, EM=(19)
applyRes_Reductor();--更新變數,efResidual->takeDataF(),計算JpJd
X的維度是C的4+8*幀數，每幀提供8維變數(xi,a,b)
for(0:mnumOptIts) //迭代
{
backupState();備份一波，如果迭代結果不好了可以退回。
solveSystem();--EnergyFunctional::sloveSyestmF()，迭代更新，
最終得到新的x ef->lastX, 並通過resubstituteF() 將 x 作用到step上(14)
{
EnergyFuntional::accumulateAF_MT和EnergyFunctional::accumulateLF_MT區別是addPoint的mode，分別是active和linearize對應0和1
EnergyFunctional::accumulateSCF_MT 消除Hδ中的點部分，只剩下相機位姿和相機內參。在求解 ξ; c後，利用 SC 更新單個 Point 的深度 d。
}
previousX=ef->lastX
doStepFromBackup(),apply setp to linearization point
linearizeAll(false)、calcLEnergy()、calcMEnergy()計算新能量
判斷退出迴圈條件
}
ef->setAdjointsF() ?不懂伴隨矩陣的意義：it is often necessary to transform a tangent vector from the tangent space around one element to the tangent space of another. The adjoint performs this transformation
從李群G到其李代數上的一個線性變換叫做這個李群的伴隨表示
linearizeAll(true)--fixLinearization=true
}
}
removeOutliers();
flagPointsForRemoval();
ef->dropPointsF();
getNullspaces();
ef->marginalizePointsF();
FullSystem::makeNewTraces在當前關鍵幀提取immaturePoints，這樣下一個關鍵幀處理的時候可以生成 pointHessians 加入到視窗優化過程中。
marginalizeFrame();
}