當前位置：首頁 > 编程资源 > 编程问答 >内容正文

编程问答

DSO窗口优化部分代码注释加理论解析

發(fā)布時間：2023/12/20 编程问答 48 豆豆

生活随笔收集整理的這篇文章主要介紹了 DSO窗口优化部分代码注释加理论解析小編覺得挺不錯的,現(xiàn)在分享給大家,幫大家做個參考.

總綱：

float FullSystem::optimize(int mnumOptIts)
{
? ?0.先清空活動點殘差隊列
? ?1.將所有外面舊成熟點相對新幀計算的殘差和新成熟點相對非所在關鍵幀的殘差加入點的殘差隊列
? ?2.調用linearizeAll()函數(shù)計算相關導數(shù)。
? ?調用linearizeAll()
? ?{
? ? ? ?2.1 linearizeAll_Reductor()
? ? ? ? ? ?{
? ? ? ? ? ? ?linearize()
? ? ? ? ? ? ? {
? ? ? ? ? ? ?具體活在這里干。完成的功能有：給點的state_NewState屬性賦值，計算點的各種雅克比行列式
? ? ? ? ? ? ?所需偏導，這些偏導是組成最終方程矩陣的基本元素。函數(shù)的返回結果是此點的殘差值。
? ? ? ? ? ? ?}
? ? ? ? ? ? }
? ? ? ?2.2 調用一次setNewFrameEnergyTH修改什么閾值。
? ? ? ?2.3 剔除判為外點的殘差。
? ?}
? ?3.調用applyRes_Reductor函數(shù)，這個函數(shù)對好點做一次中間量的計算，輸入數(shù)據(jù)可看成是linearize返回的偏導值，
? ? ? 所謂輸出的中間量就是例如：殘差對位姿的偏導的轉置* 殘差對逆深度的偏導等這類的東西。
? ? ? 最后會在solveSystem函數(shù)里用到這些中間變量，其實所謂中間變量就是雅克比矩陣的一些組成元素。
? ?4.開始迭代優(yōu)化
?? ? for()
?? ? {
?? ??? ? 4.1 保存當前的狀態(tài)，這是為了在優(yōu)化結果不好的情況下可以回退,backupState函數(shù)實現(xiàn)。
?? ??? ? 4.2 求解優(yōu)化增量方程解出的結果有68包含的各項的小增量，所有點深度值的小增量。都
?? ??? ? ? ? 是絕對增量值切記切記，solveSystem函數(shù)實現(xiàn)
?? ??? ? ? ? {
?? ??? ? ? ? ? ? 4.2.1 getNullspaces 零空間申請
?? ??? ? ? ? ? ? 4.2.2 solveSystemF 真正的解方程的位置
?? ??? ? ? ? ? ? ? ? ? {
?? ??? ? ? ? ? ? ? ? ??
?? ??? ? ? ? ? ? ? ? ? ? ?4.2.2.1 計算舒爾消元方程的那些系數(shù) 用到2.1步驟中計算的值以及3步驟計算的值。然后解出第一個方程
?? ??? ? ? ? ? ? ? ? ? ? ? ? ? ? ? 關于位姿和仿射以及相機參數(shù)的方程的解。獲得68 個增量值（8幀的關鍵幀窗口的話）
?? ??? ? ? ? ? ? ? ? ? ? ? ? ? ? ??
?? ??? ? ? ? ? ? ? ? ? ? ?4.2.2.2 根據(jù)上面的解計算解出逆深度方程的解。獲得逆深度值們的增量
?? ??? ? ? ? ? ? ? ? ? ? ?
?? ??? ? ? ? ? ? ? ? ? ? ?4.2.2.3 零空間做一次修正
?? ??? ? ? ? ? ? ? ? ? }
?? ??? ? ? ? }
?? ??? ? 4.3 對計算的增量進一步限制次數(shù)
?? ??? ? 4.4 doStepFromBackup()
?? ??? ? ? ? {
?? ??? ? ? ? ? ?4.4.1 用增量更新關鍵幀窗口中的各項狀態(tài)值，如位姿，仿射，相機參數(shù)，點的逆深度等。輸入為4.2.2.1 ?4.2.2.2這兩步結果
?? ??? ? ? ? ? ?4.4.2 ?setPrecalcValues 將兩幀之間的優(yōu)化前后狀態(tài)值（不包括點的逆深度值）都存儲一份。
?? ??? ? ? ? ? ? ? ? ?然后計算一下狀態(tài)值的增量值（包括逆深度值）
?? ??? ? ? ? }
?? ??? ? 4.5 用上面計算得到的新狀態(tài)值計算一次新的殘差以及偏導數(shù)等，在linearizeAll中實現(xiàn)。
?? ??? ? 4.6 如果新殘差值小于原來殘差值，那么就再調用一次調用applyRes_Reductor函數(shù)，接著調整lambda值，
?? ??? ? 再返回到4.1，接著順序執(zhí)行。否則就將狀態(tài)結果回退，升高lambda值返回4.1順序執(zhí)行。
?? ??? ? 4.7 滿足迭代跳出條件后結束優(yōu)化。
?? ??? ?}
?? ? 5.改變一次最后一幀的位姿仿射是干啥。
?? ? 6.調用setAdjointsF函數(shù)，這個函數(shù)是計算一些中間變量，為將相對值轉為絕對值做準備。
?? ? 7.調一次setPrecalcValues函數(shù)，再調一次linearizeAll函數(shù)，此時調linearizeAll是刪除一些不合理點帶著的殘差項
?? ? ? 下次再做窗口優(yōu)化的時候就不再用它們了。
}

理論部分主要是林突破和途金戈的博客。也貼個圖吧

途金戈的

/** * This file is part of DSO. * * Copyright 2016 Technical University of Munich and Intel. * Developed by Jakob Engel <engelj at in dot tum dot de>, * for more information see <http://vision.in.tum.de/dso>. * If you use this code, please cite the respective publications as * listed on the above website. * * DSO is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * DSO is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with DSO. If not, see <http://www.gnu.org/licenses/>. */#include "FullSystem/FullSystem.h"#include "stdio.h" #include "util/globalFuncs.h" #include <Eigen/LU> #include <algorithm> #include "IOWrapper/ImageDisplay.h" #include "util/globalCalib.h" #include <Eigen/SVD> #include <Eigen/Eigenvalues> #include "FullSystem/ResidualProjections.h"#include "OptimizationBackend/EnergyFunctional.h" #include "OptimizationBackend/EnergyFunctionalStructs.h"#include <cmath>#include <algorithm>namespace dso {void FullSystem::linearizeAll_Reductor(bool fixLinearization, std::vector<PointFrameResidual*>* toRemove, int min, int max, Vec10* stats, int tid) { //循環(huán)所有殘差項for(int k = min; k < max; k++){PointFrameResidual* r = activeResiduals[k];//在這里計算偏導數(shù) 填充雅克比矩陣用包括幾何雅克比圖像雅克比和//逆深度雅克比 ,還對點的state_NewState進行賦值//(*stats)[0]加的這個是殘差值(*stats)[0] += r->linearize(&Hcalib);if(fixLinearization){r->applyRes(true);if(r->efResidual->isActive()){if(r->isNew){PointHessian* p = r->point;//假設無限深度的投影點。Vec3f ptp_inf = r->host->targetPrecalc[r->target->idx].PRE_KRKiTll * Vec3f(p->u,p->v, 1); // projected point assuming infinite depth.//具有真實深度的投影點。Vec3f ptp = ptp_inf + r->host->targetPrecalc[r->target->idx].PRE_KtTll*p->idepth_scaled; // projected point with real depth.float relBS = 0.01*((ptp_inf.head<2>() / ptp_inf[2])-(ptp.head<2>() / ptp[2])).norm(); // 0.01 = one pixel.if(relBS > p->maxRelBaseline)p->maxRelBaseline = relBS;p->numGoodResiduals++;}}else{//壞點需要被剔除的toRemove[tid].push_back(activeResiduals[k]);}}} }void FullSystem::applyRes_Reductor(bool copyJacobians, int min, int max, Vec10* stats, int tid) {for(int k = min; k < max; k++)activeResiduals[k]->applyRes(true); } void FullSystem::setNewFrameEnergyTH() {// collect all residuals and make decision on TH.//收集所有殘差并對TH做出更新吧allResVec.clear();allResVec.reserve(activeResiduals.size() * 2);FrameHessian* newFrame = frameHessians.back();//最后一幀 //printf("=======id = %d\n",newFrame->idx);for(PointFrameResidual* r : activeResiduals)if(r->state_NewEnergyWithOutlier >= 0 && r->target == newFrame){allResVec.push_back(r->state_NewEnergyWithOutlier);//target為back的幀的所有成熟點的殘差}if(allResVec.size()==0){newFrame->frameEnergyTH = 12*12*patternNum;return; // should never happen, but lets make sure.} //printf("=======allResVec.size() = %d\n",allResVec.size());int nthIdx = setting_frameEnergyTHN*allResVec.size();//0.7assert(nthIdx < (int)allResVec.size());assert(setting_frameEnergyTHN < 1);std::nth_element(allResVec.begin(), allResVec.begin()+nthIdx, allResVec.end());float nthElement = sqrtf(allResVec[nthIdx]);newFrame->frameEnergyTH = nthElement*setting_frameEnergyTHFacMedian;newFrame->frameEnergyTH = 26.0f*setting_frameEnergyTHConstWeight + newFrame->frameEnergyTH*(1-setting_frameEnergyTHConstWeight);newFrame->frameEnergyTH = newFrame->frameEnergyTH*newFrame->frameEnergyTH;newFrame->frameEnergyTH *= setting_overallEnergyTHWeight*setting_overallEnergyTHWeight;}//相關導數(shù)計算，并且計算出殘差值來。 Vec3 FullSystem::linearizeAll(bool fixLinearization) {double lastEnergyP = 0;double lastEnergyR = 0;double num = 0;std::vector<PointFrameResidual*> toRemove[NUM_THREADS];for(int i = 0; i < NUM_THREADS; i++) toRemove[i].clear();if(multiThreading){treadReduce.reduce(boost::bind(&FullSystem::linearizeAll_Reductor, this, fixLinearization, toRemove, _1, _2, _3, _4), 0, activeResiduals.size(), 0);lastEnergyP = treadReduce.stats[0];}else{Vec10 stats;//雅克比矩陣元素:偏導數(shù)。還有點屬性都給賦個值linearizeAll_Reductor(fixLinearization, toRemove, 0, activeResiduals.size(),&stats,0); lastEnergyP = stats[0];} //修改閾值frameHessians.back()->frameEnergyTH的值就是修改 target幀的frameEnergyTH值setNewFrameEnergyTH();//這段是剔除優(yōu)化后為外點的殘差if(fixLinearization){for(PointFrameResidual* r : activeResiduals){PointHessian* ph = r->point;if(ph->lastResiduals[0].first == r)ph->lastResiduals[0].second = r->state_state;else if(ph->lastResiduals[1].first == r)ph->lastResiduals[1].second = r->state_state;}int nResRemoved=0;for(int i=0;i<NUM_THREADS;i++){//循環(huán)某個PointFrameResidual點for(PointFrameResidual* r : toRemove[i]){//取其PointHessian點PointHessian* ph = r->point;if(ph->lastResiduals[0].first == r)ph->lastResiduals[0].first=0;else if(ph->lastResiduals[1].first == r)ph->lastResiduals[1].first=0;//該點所有殘差項for(unsigned int k = 0; k < ph->residuals.size(); k++)if(ph->residuals[k] == r){ef->dropResidual(r->efResidual);deleteOut<PointFrameResidual>(ph->residuals,k);nResRemoved++;break;}}}}return Vec3(lastEnergyP, lastEnergyR, num); }// applies step to linearization point.將步驟應用于線性化點。 //優(yōu)化結果生效應該是修改各個待優(yōu)化參數(shù)值 //并通過評估本次增量的大小來判斷是否結束迭代 //待優(yōu)化參數(shù)包括N個逆深度 m*8+4的相機位姿仿射相機參數(shù) bool FullSystem::doStepFromBackup(float stepfacC,float stepfacT,float stepfacR,float stepfacA,float stepfacD) { // float meanStepC=0,meanStepP=0,meanStepD=0; // meanStepC += Hcalib.step.norm();//pstepfac 是預測的增量唄Vec10 pstepfac;//平移pstepfac.segment<3>(0).setConstant(stepfacT);//旋轉pstepfac.segment<3>(3).setConstant(stepfacR);//仿射pstepfac.segment<4>(6).setConstant(stepfacA);float sumA=0, sumB=0, sumT=0, sumR=0, sumID=0, numID=0;float sumNID=0;if(setting_solverMode & SOLVER_MOMENTUM){Hcalib.setValue(Hcalib.value_backup + Hcalib.step);//循環(huán)關鍵幀窗口for(FrameHessian* fh : frameHessians){Vec10 step0 = fh->step;step0.head<6>() += 0.5f*(fh->step_backup.head<6>());//修改fh->setState(fh->state_backup + step0);sumA += step0[6] * step0[6];sumB += step0[7] * step0[7];sumT += step0.segment<3>(0).squaredNorm();sumR += step0.segment<3>(3).squaredNorm();for(PointHessian* ph : fh->pointHessians){float stepx = ph->step+0.5f*(ph->step_backup);ph->setIdepth(ph->idepth_backup + stepx);sumID += stepx * stepx;sumNID += fabsf(ph->idepth_backup);numID++;ph->setIdepthZero(ph->idepth_backup + stepx);}}}else{//相機參數(shù)增量*權重 stepfacC 修改相機參數(shù)1 * 4//修改相機參數(shù)Hcalib.setValue(Hcalib.value_backup + stepfacC*Hcalib.step);//循環(huán)關鍵幀窗口for(FrameHessian* fh : frameHessians){// cwiseProduct 函數(shù)功能 R = P .* Q 對應點相乘//修改位姿仿射參數(shù)等原有值+ 增量fh->setState(fh->state_backup + pstepfac.cwiseProduct(fh->step));sumA += fh->step[6]*fh->step[6];sumB += fh->step[7]*fh->step[7];sumT += fh->step.segment<3>(0).squaredNorm();sumR += fh->step.segment<3>(3).squaredNorm();//循環(huán)點修改逆深度值for(PointHessian* ph : fh->pointHessians){//修改逆深度值ph->setIdepth(ph->idepth_backup + stepfacD * ph->step);sumID += ph->step * ph->step;sumNID += fabsf(ph->idepth_backup);numID++;//修改idepth_zero值ph->setIdepthZero(ph->idepth_backup + stepfacD * ph->step);}}}sumA /= frameHessians.size();sumB /= frameHessians.size();sumR /= frameHessians.size();sumT /= frameHessians.size();sumID /= numID;sumNID /= numID;if(!setting_debugout_runquiet)printf("STEPS: A %.1f; B %.1f; R %.1f; T %.1f. \t",sqrtf(sumA) / (0.0005*setting_thOptIterations),sqrtf(sumB) / (0.00005*setting_thOptIterations),sqrtf(sumR) / (0.00005*setting_thOptIterations),sqrtf(sumT)*sumNID / (0.00005*setting_thOptIterations));EFDeltaValid=false;setPrecalcValues();return sqrtf(sumA) < 0.0005*setting_thOptIterations &&sqrtf(sumB) < 0.00005*setting_thOptIterations &&sqrtf(sumR) < 0.00005*setting_thOptIterations &&sqrtf(sumT)*sumNID < 0.00005*setting_thOptIterations; }// sets linearization point. //保留狀態(tài)有點逆深度idepth 相機參數(shù)Hcalib.value 位姿仿射fh->get_state() //以及它們對應的增量值 ph->step Hcalib.step fh->step void FullSystem::backupState(bool backupLastStep) {if(setting_solverMode & SOLVER_MOMENTUM){//非第一次迭代if(backupLastStep){Hcalib.step_backup = Hcalib.step;Hcalib.value_backup = Hcalib.value;for(FrameHessian* fh : frameHessians){fh->step_backup = fh->step;fh->state_backup = fh->get_state();for(PointHessian* ph : fh->pointHessians){ph->idepth_backup = ph->idepth;ph->step_backup = ph->step;}}}else//第一次迭代{Hcalib.step_backup.setZero();Hcalib.value_backup = Hcalib.value;//循環(huán)關鍵幀窗口for(FrameHessian* fh : frameHessians){fh->step_backup.setZero();fh->state_backup = fh->get_state();//循環(huán)成熟點for(PointHessian* ph : fh->pointHessians){ph->idepth_backup = ph->idepth;ph->step_backup=0;}}}}else{Hcalib.value_backup = Hcalib.value;for(FrameHessian* fh : frameHessians){fh->state_backup = fh->get_state();for(PointHessian* ph : fh->pointHessians)ph->idepth_backup = ph->idepth;}} }// sets linearization point. void FullSystem::loadSateBackup() {Hcalib.setValue(Hcalib.value_backup);for(FrameHessian* fh : frameHessians){fh->setState(fh->state_backup);for(PointHessian* ph : fh->pointHessians){ph->setIdepth(ph->idepth_backup);ph->setIdepthZero(ph->idepth_backup);}}EFDeltaValid=false;setPrecalcValues(); }double FullSystem::calcMEnergy() {if(setting_forceAceptStep) return 0;return ef->calcMEnergyF();}void FullSystem::printOptRes(const Vec3 &res, double resL, double resM, double resPrior, double LExact, float a, float b) {printf("A(%f)=(AV %.3f). Num: A(%'d) + M(%'d); ab %f %f!\n",res[0],sqrtf((float)(res[0] / (patternNum*ef->resInA))),ef->resInA,ef->resInM,a,b);}float FullSystem::optimize(int mnumOptIts) {if(frameHessians.size() < 2) return 0;if(frameHessians.size() < 3) mnumOptIts = 20;if(frameHessians.size() < 4) mnumOptIts = 15;// get statistics and active residuals.獲取統(tǒng)計數(shù)據(jù)和活動殘差。//清空活動點殘差隊列activeResiduals.clear();int numPoints = 0;//沒用int numLRes = 0;//沒用int galdg = 0;//循環(huán)窗口內(nèi)的幀for(FrameHessian* fh : frameHessians)for(PointHessian* ph : fh->pointHessians)//循環(huán)成熟點{//循環(huán)每個點在不同幀投影算的殘差for(PointFrameResidual* r : ph->residuals){//在外面舊成熟點相對新幀計算新殘差 isLinearized為false//在外面新激活成熟點相對非所在幀算新殘差 isLinearized為false//這些殘差點是累積下來的if(!r->efResidual->isLinearized){//加入活動點殘差隊列activeResiduals.push_back(r);//設置了幾個值r->resetOOB();//galdg ++;}elsenumLRes++;}//點總數(shù)numPoints++;}//相關導數(shù)計算Vec3 lastEnergy = linearizeAll(false);//這個函數(shù)返回值設置直接為0了double lastEnergyL = calcLEnergy();//這個函數(shù)返回值設置直接為0了double lastEnergyM = calcMEnergy();//在所有的 residual 中找到 isLinearized 為 false 的 residual，調用 //PointFrameResidual::applyRes(true)，設置它們的 PointFrameResidual::ResState //（按照 FullSystem::optimizeImmaturePoint 的結果），如果是正常的 //點(ResState::IN)調用EFResidual::takeDataF()把 EFResidual::JpJdF 設置一下。 //這就算完成了優(yōu)化的準備工作。還計算了每個殘差的 //JPJDF值是一個8*1的向量if(multiThreading)treadReduce.reduce(boost::bind(&FullSystem::applyRes_Reductor, this, true, _1, _2, _3, _4), 0, activeResiduals.size(), 50);else{//計算了下博客中的4.7.6提到的中間變量 applyRes_Reductor(true, 0, activeResiduals.size(), 0, 0);}if(!setting_debugout_runquiet){printf("Initial Error \t");printOptRes(lastEnergy, lastEnergyL, lastEnergyM, 0, 0, frameHessians.back()->aff_g2l().a, frameHessians.back()->aff_g2l().b);} //調試繪圖跟蹤debugPlotTracking();double lambda = 1e-1;//0.1float stepsize = 1;VecX previousX = VecX::Constant(CPARS + 8 * frameHessians.size(), NAN);//開始迭代優(yōu)化for(int iteration = 0; iteration < mnumOptIts; iteration++){// solve!//保存當前的狀態(tài)，這是為了在優(yōu)化結果不好的情況下可以回退backupState(iteration!=0);//solveSystemNew(0);//求解優(yōu)化增量方程解出的結果有68包含的各項的小增量，所有點深度值//的小增量。都是絕對增量值切記切記solveSystem(iteration, lambda);double incDirChange = (1e-20 + previousX.dot(ef->lastX)) / (1e-20 + previousX.norm() * ef->lastX.norm());previousX = ef->lastX;if(std::isfinite(incDirChange) && (setting_solverMode & SOLVER_STEPMOMENTUM)){float newStepsize = exp(incDirChange * 1.4);if(incDirChange < 0 && stepsize>1) stepsize = 1;stepsize = sqrtf(sqrtf(newStepsize * stepsize * stepsize * stepsize));if(stepsize > 2) stepsize=2;if(stepsize <0.25) stepsize=0.25;} //優(yōu)化結果生效應該是修改各個待優(yōu)化參數(shù)值 //并通過評估本次增量的大小來判斷是否結束迭代 //待優(yōu)化參數(shù)包括M個點的逆深度 m * 8 + 4的相機位姿仿射相機參數(shù)bool canbreak = doStepFromBackup(stepsize, stepsize, stepsize, stepsize, stepsize);// eval new energy!評估新能量//用上面修改的新數(shù)據(jù)計算新的能量值newEnergy，并且重新計算一組//相關導數(shù)Vec3 newEnergy = linearizeAll(false);double newEnergyL = calcLEnergy();//返回0 不用去費腦子了double newEnergyM = calcMEnergy();//返回0不用去費腦子了if(!setting_debugout_runquiet){printf("%s %d (L %.2f, dir %.2f, ss %.1f): \t",(newEnergy[0] + newEnergy[1] + newEnergyL + newEnergyM <lastEnergy[0] + lastEnergy[1] + lastEnergyL + lastEnergyM) ? "ACCEPT" : "REJECT",iteration,log10(lambda),incDirChange,stepsize);printOptRes(newEnergy, newEnergyL, newEnergyM , 0, 0, frameHessians.back()->aff_g2l().a, frameHessians.back()->aff_g2l().b);}if(setting_forceAceptStep || (newEnergy[0] + newEnergy[1] + newEnergyL + newEnergyM <lastEnergy[0] + lastEnergy[1] + lastEnergyL + lastEnergyM)){if(multiThreading)treadReduce.reduce(boost::bind(&FullSystem::applyRes_Reductor, this, true, _1, _2, _3, _4), 0, activeResiduals.size(), 50);else{//中間變量計算applyRes_Reductor(true,0,activeResiduals.size(),0,0);}lastEnergy = newEnergy;lastEnergyL = newEnergyL;lastEnergyM = newEnergyM;lambda *= 0.25;}else{//能量升高了就使用 FullSystem::loadSateBackup() 將結果回滾。loadSateBackup();//重新算一次lastEnergy = linearizeAll(false);lastEnergyL = calcLEnergy();lastEnergyM = calcMEnergy();lambda *= 1e2;}if(canbreak && iteration >= setting_minOptIterations) break;}Vec10 newStateZero = Vec10::Zero();newStateZero.segment<2>(6) = frameHessians.back()->get_state().segment<2>(6);//setStateZero賦值的地方用newStateZero賦值frameHessians.back()->setEvalPT(frameHessians.back()->PRE_worldToCam, newStateZero);EFDeltaValid = false;EFAdjointsValid = false;ef->setAdjointsF(&Hcalib);setPrecalcValues();//在跳出循環(huán)體之后調用一次 FullSystem::linearizeAll(true)， //效果是將優(yōu)化之后成為 outlier 的 residual 剔除，剩下 //正常的 residual 調用一次 EFResidual::takeDataF() 計算新的 //EFResidual::JpJdF （這里涉及到 FEJ）。注意一下 lastEnergy = linearizeAll(true);if(!std::isfinite((double)lastEnergy[0]) || !std::isfinite((double)lastEnergy[1]) || !std::isfinite((double)lastEnergy[2])){printf("KF Tracking failed: LOST!\n");isLost=true;}statistics_lastFineTrackRMSE = sqrtf((float)(lastEnergy[0] / (patternNum*ef->resInA)));if(calibLog != 0){(*calibLog) << Hcalib.value_scaled.transpose() <<" " << frameHessians.back()->get_state_scaled().transpose() <<" " << sqrtf((float)(lastEnergy[0] / (patternNum*ef->resInA))) <<" " << ef->resInM << "\n";calibLog->flush();}//最終在這要更新關鍵幀的位姿和仿射參數(shù) 。用優(yōu)化后的值更新。{boost::unique_lock<boost::mutex> crlock(shellPoseMutex);//循環(huán)關鍵幀窗口for(FrameHessian* fh : frameHessians){fh->shell->camToWorld = fh->PRE_camToWorld;fh->shell->aff_g2l = fh->aff_g2l();}}debugPlotTracking();//返回的這個值是點的平均殘差值的開方return sqrtf((float)(lastEnergy[0] / (patternNum * ef->resInA)));}void FullSystem::solveSystem(int iteration, double lambda) { //零空間給lastNullspaces_pose他們幾個賦值呢，affA affB壓根沒用到不用去理會ef->lastNullspaces_forLogging = getNullspaces(ef->lastNullspaces_pose,ef->lastNullspaces_scale,ef->lastNullspaces_affA,ef->lastNullspaces_affB); //解方程的主函數(shù)ef->solveSystemF(iteration, lambda,&Hcalib); }double FullSystem::calcLEnergy() {if(setting_forceAceptStep) return 0;double Ef = ef->calcLEnergyF_MT();return Ef;}void FullSystem::removeOutliers() {int numPointsDropped=0;for(FrameHessian* fh : frameHessians){for(unsigned int i=0;i<fh->pointHessians.size();i++){PointHessian* ph = fh->pointHessians[i];if(ph==0) continue;if(ph->residuals.size() == 0){fh->pointHessiansOut.push_back(ph);ph->efPoint->stateFlag = EFPointStatus::PS_DROP;fh->pointHessians[i] = fh->pointHessians.back();fh->pointHessians.pop_back();i--;numPointsDropped++;}}}ef->dropPointsF(); }std::vector<VecX> FullSystem::getNullspaces(std::vector<VecX> &nullspaces_pose1,std::vector<VecX> &nullspaces_scale1,std::vector<VecX> &nullspaces_affA,std::vector<VecX> &nullspaces_affB) { // 先做個清空nullspaces_pose1.clear();nullspaces_scale1.clear();nullspaces_affA.clear();nullspaces_affB.clear();int n = CPARS + frameHessians.size() * 8;std::vector<VecX> nullspaces_x0_pre;for(int i = 0; i < 6; i++){VecX nullspace_x0(n);nullspace_x0.setZero();for(FrameHessian* fh : frameHessians){nullspace_x0.segment<6>(CPARS+fh->idx * 8) = fh->nullspaces_pose.col(i);nullspace_x0.segment<3>(CPARS+fh->idx * 8) *= SCALE_XI_TRANS_INVERSE;nullspace_x0.segment<3>(CPARS+fh->idx * 8 + 3) *= SCALE_XI_ROT_INVERSE;}nullspaces_x0_pre.push_back(nullspace_x0);nullspaces_pose1.push_back(nullspace_x0);}for(int i=0;i<2;i++){VecX nullspace_x0(n);nullspace_x0.setZero();for(FrameHessian* fh : frameHessians){nullspace_x0.segment<2>(CPARS+fh->idx*8+6) = fh->nullspaces_affine.col(i).head<2>();nullspace_x0[CPARS+fh->idx*8+6] *= SCALE_A_INVERSE;nullspace_x0[CPARS+fh->idx*8+7] *= SCALE_B_INVERSE;}nullspaces_x0_pre.push_back(nullspace_x0);if(i==0) nullspaces_affA.push_back(nullspace_x0);if(i==1) nullspaces_affB.push_back(nullspace_x0);}VecX nullspace_x0(n);nullspace_x0.setZero();for(FrameHessian* fh : frameHessians){nullspace_x0.segment<6>(CPARS+fh->idx*8) = fh->nullspaces_scale;nullspace_x0.segment<3>(CPARS+fh->idx*8) *= SCALE_XI_TRANS_INVERSE;nullspace_x0.segment<3>(CPARS+fh->idx*8+3) *= SCALE_XI_ROT_INVERSE;}nullspaces_x0_pre.push_back(nullspace_x0);nullspaces_scale1.push_back(nullspace_x0);return nullspaces_x0_pre; }} /** * This file is part of DSO. * * Copyright 2016 Technical University of Munich and Intel. * Developed by Jakob Engel <engelj at in dot tum dot de>, * for more information see <http://vision.in.tum.de/dso>. * If you use this code, please cite the respective publications as * listed on the above website. * * DSO is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * DSO is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with DSO. If not, see <http://www.gnu.org/licenses/>. *//** KFBuffer.cpp** Created on: Jan 7, 2014* Author: engelj*/#include "FullSystem/FullSystem.h"#include "stdio.h" #include "util/globalFuncs.h" #include <Eigen/LU> #include <algorithm> #include "IOWrapper/ImageDisplay.h" #include "util/globalCalib.h" #include <Eigen/SVD> #include <Eigen/Eigenvalues>#include "FullSystem/ResidualProjections.h" #include "OptimizationBackend/EnergyFunctional.h" #include "OptimizationBackend/EnergyFunctionalStructs.h"#include "FullSystem/HessianBlocks.h"namespace dso { int PointFrameResidual::instanceCounter = 0;long runningResID=0;PointFrameResidual::PointFrameResidual(){assert(false); instanceCounter++;}PointFrameResidual::~PointFrameResidual(){assert(efResidual==0); instanceCounter--; delete J;}PointFrameResidual::PointFrameResidual(PointHessian* point_, FrameHessian* host_, FrameHessian* target_) :point(point_),host(host_),target(target_) {efResidual=0;instanceCounter++;resetOOB();J = new RawResidualJacobian();assert(((long)J)%16==0);isNew=true; }//投影點關于各個參數(shù)的導數(shù)。其組成雅克比矩陣 double PointFrameResidual::linearize(CalibHessian* HCalib) {state_NewEnergyWithOutlier = -1;if(state_state == ResState::OOB){ state_NewState = ResState::OOB; return state_energy; }FrameFramePrecalc* precalc = &(host->targetPrecalc[target->idx]);float energyLeft=0;const Eigen::Vector3f* dIl = target->dI;//const float* const Il = target->I;const Mat33f &PRE_KRKiTll = precalc->PRE_KRKiTll;const Vec3f &PRE_KtTll = precalc->PRE_KtTll;const Mat33f &PRE_RTll_0x = precalc->PRE_RTll_0;const Vec3f &PRE_tTll_0 = precalc->PRE_tTll_0;const float * const color = point->color;const float * const weights = point->weights;Vec2f affLL = precalc->PRE_aff_mode;float b0 = precalc->PRE_b0_mode;Vec6f d_xi_x, d_xi_y;Vec4f d_C_x, d_C_y;float d_d_x, d_d_y;{float drescale, u, v, new_idepth;float Ku, Kv;Vec3f KliP; //點越界直接返回if(!projectPoint(point->u, point->v, point->idepth_zero_scaled, 0, 0,HCalib,PRE_RTll_0x,PRE_tTll_0, drescale, u, v, Ku, Kv, KliP, new_idepth)){ state_NewState = ResState::OOB; return state_energy; }centerProjectedTo = Vec3f(Ku, Kv, new_idepth);// diff d_idepth//target幀點坐標對逆深度求偏導d_d_x = drescale * (PRE_tTll_0[0] - PRE_tTll_0[2]*u)*SCALE_IDEPTH*HCalib->fxl();d_d_y = drescale * (PRE_tTll_0[1] - PRE_tTll_0[2]*v)*SCALE_IDEPTH*HCalib->fyl();// diff calib//target幀點坐標對相機參數(shù)求偏導d_C_x[2] = drescale*(PRE_RTll_0x(2,0)*u-PRE_RTll_0x(0,0));d_C_x[3] = HCalib->fxl() * drescale*(PRE_RTll_0x(2,1)*u-PRE_RTll_0x(0,1)) * HCalib->fyli();d_C_x[0] = KliP[0]*d_C_x[2];d_C_x[1] = KliP[1]*d_C_x[3];d_C_y[2] = HCalib->fyl() * drescale*(PRE_RTll_0x(2,0)*v-PRE_RTll_0x(1,0)) * HCalib->fxli();d_C_y[3] = drescale*(PRE_RTll_0x(2,1)*v-PRE_RTll_0x(1,1));d_C_y[0] = KliP[0]*d_C_y[2];d_C_y[1] = KliP[1]*d_C_y[3];d_C_x[0] = (d_C_x[0]+u)*SCALE_F;d_C_x[1] *= SCALE_F;d_C_x[2] = (d_C_x[2]+1)*SCALE_C;d_C_x[3] *= SCALE_C;d_C_y[0] *= SCALE_F;d_C_y[1] = (d_C_y[1]+v)*SCALE_F;d_C_y[2] *= SCALE_C;d_C_y[3] = (d_C_y[3]+1)*SCALE_C;// target幀點坐標對位姿增量求偏導 DSO追蹤與優(yōu)化公式56d_xi_x[0] = new_idepth*HCalib->fxl();//對應論文公式13 dso詳解博客公式d_xi_x[1] = 0;d_xi_x[2] = -new_idepth*u*HCalib->fxl();d_xi_x[3] = -u*v*HCalib->fxl();d_xi_x[4] = (1+u*u)*HCalib->fxl();d_xi_x[5] = -v*HCalib->fxl();d_xi_y[0] = 0;d_xi_y[1] = new_idepth*HCalib->fyl();d_xi_y[2] = -new_idepth*v*HCalib->fyl();d_xi_y[3] = -(1+v*v)*HCalib->fyl();d_xi_y[4] = u*v*HCalib->fyl();d_xi_y[5] = u*HCalib->fyl();}{//位姿參數(shù)雅克比矩陣 =====>坐標對位姿求偏導J->Jpdxi[0] = d_xi_x;J->Jpdxi[1] = d_xi_y;//相機參數(shù)雅克比矩陣 =====>坐標對相機參數(shù)求偏導J->Jpdc[0] = d_C_x;J->Jpdc[1] = d_C_y;//逆深度雅克比矩陣 =====>坐標對逆深度求偏導J->Jpdd[0] = d_d_x;J->Jpdd[1] = d_d_y;}float JIdxJIdx_00=0, JIdxJIdx_11=0, JIdxJIdx_10=0;float JabJIdx_00=0, JabJIdx_01=0, JabJIdx_10=0, JabJIdx_11=0;float JabJab_00=0, JabJab_01=0, JabJab_11=0;float wJI2_sum = 0; //圖像坐標的偏導數(shù) 以及光度仿射變換的偏導，是對8個點求 //是像素坐標關于梯度增量光度增量等的偏導for(int idx = 0; idx < patternNum; idx++){float Ku, Kv;if(!projectPoint(point->u+patternP[idx][0], point->v+patternP[idx][1], point->idepth_scaled, PRE_KRKiTll, PRE_KtTll, Ku, Kv)){ state_NewState = ResState::OOB; return state_energy; }projectedTo[idx][0] = Ku;projectedTo[idx][1] = Kv;Vec3f hitColor = (getInterpolatedElement33(dIl, Ku, Kv, wG[0]));float residual = hitColor[0] - (float)(affLL[0] * color[idx] + affLL[1]); //點坐標對光度變化增量求偏導float drdA = (color[idx] - b0);if(!std::isfinite((float)hitColor[0])){ state_NewState = ResState::OOB; return state_energy; }float w = sqrtf(setting_outlierTHSumComponent / (setting_outlierTHSumComponent + hitColor.tail<2>().squaredNorm()));w = 0.5f * (w + weights[idx]);float hw = fabsf(residual) < setting_huberTH ? 1 : setting_huberTH / fabsf(residual);//hub范數(shù)energyLeft += w * w * hw * residual * residual * (2 - hw);{if(hw < 1) hw = sqrtf(hw);hw = hw*w;hitColor[1]*=hw;hitColor[2]*=hw;J->resF[idx] = residual*hw;//殘差對圖像坐標求偏導J->JIdx[0][idx] = hitColor[1];J->JIdx[1][idx] = hitColor[2];//殘差對仿射求偏導J->JabF[0][idx] = drdA*hw;J->JabF[1][idx] = hw;JIdxJIdx_00+=hitColor[1]*hitColor[1];JIdxJIdx_11+=hitColor[2]*hitColor[2];JIdxJIdx_10+=hitColor[1]*hitColor[2];//轉置計算體現(xiàn)在這 +表示的ΣJabJIdx_00+= drdA*hw * hitColor[1];JabJIdx_01+= drdA*hw* hitColor[2];JabJIdx_10+= hw * hitColor[1];JabJIdx_11+= hw * hitColor[2];JabJab_00+= drdA*hw*drdA*hw;JabJab_01+= drdA*hw*hw;JabJab_11+= hw*hw;wJI2_sum += hw*hw*(hitColor[1]*hitColor[1] + hitColor[2]*hitColor[2]);if(setting_affineOptModeA < 0) J->JabF[0][idx]=0;if(setting_affineOptModeB < 0) J->JabF[1][idx]=0;}} //(殘差 /像素坐標)轉置*(殘差 /像素坐標)J->JIdx2(0,0) = JIdxJIdx_00;J->JIdx2(0,1) = JIdxJIdx_10;J->JIdx2(1,0) = JIdxJIdx_10;J->JIdx2(1,1) = JIdxJIdx_11;//(殘差 /光度參數(shù))轉置*(殘差 /像素坐標)J->JabJIdx(0,0) = JabJIdx_00;J->JabJIdx(0,1) = JabJIdx_01;J->JabJIdx(1,0) = JabJIdx_10;J->JabJIdx(1,1) = JabJIdx_11;//(殘差 /光度參數(shù))轉置*(殘差 /光度參數(shù))J->Jab2(0,0) = JabJab_00;J->Jab2(0,1) = JabJab_01;J->Jab2(1,0) = JabJab_01;J->Jab2(1,1) = JabJab_11;state_NewEnergyWithOutlier = energyLeft;if(energyLeft > std::max<float>(host->frameEnergyTH, target->frameEnergyTH) || wJI2_sum < 2){energyLeft = std::max<float>(host->frameEnergyTH, target->frameEnergyTH);//異常點state_NewState = ResState::OUTLIER;}else{//正常點state_NewState = ResState::IN;}state_NewEnergy = energyLeft;//返回值是殘差值return energyLeft; }void PointFrameResidual::debugPlot() {if(state_state==ResState::OOB) return;Vec3b cT = Vec3b(0,0,0);if(freeDebugParam5==0){float rT = 20*sqrt(state_energy/9);if(rT<0) rT=0; if(rT>255)rT=255;cT = Vec3b(0,255-rT,rT);}else{if(state_state == ResState::IN) cT = Vec3b(255,0,0);else if(state_state == ResState::OOB) cT = Vec3b(255,255,0);else if(state_state == ResState::OUTLIER) cT = Vec3b(0,0,255);else cT = Vec3b(255,255,255);}for(int i=0;i<patternNum;i++){if((projectedTo[i][0] > 2 && projectedTo[i][1] > 2 && projectedTo[i][0] < wG[0]-3 && projectedTo[i][1] < hG[0]-3 ))target->debugImage->setPixel1((float)projectedTo[i][0], (float)projectedTo[i][1],cT);} }//設置它們的 PointFrameResidual::ResState （ //按照 FullSystem::optimizeImmaturePoint 的結果） //，如果是正常的點(ResState::IN)調用 //EFResidual::takeDataF()把 EFResidual::JpJdF 設置 //一下。這就算完成了優(yōu)化的準備工作。 // 計算每個殘差的JPJDF值 void PointFrameResidual::applyRes(bool copyJacobians) {if(copyJacobians){if(state_state == ResState::OOB){assert(!efResidual->isActiveAndIsGoodNEW);return; // can never go back from OOB}if(state_NewState == ResState::IN)// && ){efResidual->isActiveAndIsGoodNEW=true;efResidual->takeDataF();}else{efResidual->isActiveAndIsGoodNEW=false;}}setState(state_NewState);state_energy = state_NewEnergy; } } /** * This file is part of DSO. * * Copyright 2016 Technical University of Munich and Intel. * Developed by Jakob Engel <engelj at in dot tum dot de>, * for more information see <http://vision.in.tum.de/dso>. * If you use this code, please cite the respective publications as * listed on the above website. * * DSO is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * DSO is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with DSO. If not, see <http://www.gnu.org/licenses/>. */#include "FullSystem/FullSystem.h"#include "stdio.h" #include "util/globalFuncs.h" #include <Eigen/LU> #include <algorithm> #include "IOWrapper/ImageDisplay.h" #include "util/globalCalib.h" #include <Eigen/SVD> #include <Eigen/Eigenvalues> #include "FullSystem/ResidualProjections.h"#include "OptimizationBackend/EnergyFunctional.h" #include "OptimizationBackend/EnergyFunctionalStructs.h"#include <cmath>#include <algorithm>namespace dso {void FullSystem::linearizeAll_Reductor(bool fixLinearization, std::vector<PointFrameResidual*>* toRemove, int min, int max, Vec10* stats, int tid) { //循環(huán)所有殘差項for(int k = min; k < max; k++){PointFrameResidual* r = activeResiduals[k];//在這里計算偏導數(shù) 填充雅克比矩陣用包括幾何雅克比圖像雅克比和//逆深度雅克比 ,還對點的state_NewState進行賦值//(*stats)[0]加的這個是殘差值(*stats)[0] += r->linearize(&Hcalib);if(fixLinearization){r->applyRes(true);if(r->efResidual->isActive()){if(r->isNew){PointHessian* p = r->point;//假設無限深度的投影點。Vec3f ptp_inf = r->host->targetPrecalc[r->target->idx].PRE_KRKiTll * Vec3f(p->u,p->v, 1); // projected point assuming infinite depth.//具有真實深度的投影點。Vec3f ptp = ptp_inf + r->host->targetPrecalc[r->target->idx].PRE_KtTll*p->idepth_scaled; // projected point with real depth.float relBS = 0.01*((ptp_inf.head<2>() / ptp_inf[2])-(ptp.head<2>() / ptp[2])).norm(); // 0.01 = one pixel.if(relBS > p->maxRelBaseline)p->maxRelBaseline = relBS;p->numGoodResiduals++;}}else{//壞點需要被剔除的toRemove[tid].push_back(activeResiduals[k]);}}} }void FullSystem::applyRes_Reductor(bool copyJacobians, int min, int max, Vec10* stats, int tid) {for(int k = min; k < max; k++)activeResiduals[k]->applyRes(true); } void FullSystem::setNewFrameEnergyTH() {// collect all residuals and make decision on TH.//收集所有殘差并對TH做出更新吧allResVec.clear();allResVec.reserve(activeResiduals.size() * 2);FrameHessian* newFrame = frameHessians.back();//最后一幀 //printf("=======id = %d\n",newFrame->idx);for(PointFrameResidual* r : activeResiduals)if(r->state_NewEnergyWithOutlier >= 0 && r->target == newFrame){allResVec.push_back(r->state_NewEnergyWithOutlier);//target為back的幀的所有成熟點的殘差}if(allResVec.size()==0){newFrame->frameEnergyTH = 12*12*patternNum;return; // should never happen, but lets make sure.} //printf("=======allResVec.size() = %d\n",allResVec.size());int nthIdx = setting_frameEnergyTHN*allResVec.size();//0.7assert(nthIdx < (int)allResVec.size());assert(setting_frameEnergyTHN < 1);std::nth_element(allResVec.begin(), allResVec.begin()+nthIdx, allResVec.end());float nthElement = sqrtf(allResVec[nthIdx]);newFrame->frameEnergyTH = nthElement*setting_frameEnergyTHFacMedian;newFrame->frameEnergyTH = 26.0f*setting_frameEnergyTHConstWeight + newFrame->frameEnergyTH*(1-setting_frameEnergyTHConstWeight);newFrame->frameEnergyTH = newFrame->frameEnergyTH*newFrame->frameEnergyTH;newFrame->frameEnergyTH *= setting_overallEnergyTHWeight*setting_overallEnergyTHWeight;}//相關導數(shù)計算，并且計算出殘差值來。 Vec3 FullSystem::linearizeAll(bool fixLinearization) {double lastEnergyP = 0;double lastEnergyR = 0;double num = 0;std::vector<PointFrameResidual*> toRemove[NUM_THREADS];for(int i = 0; i < NUM_THREADS; i++) toRemove[i].clear();if(multiThreading){treadReduce.reduce(boost::bind(&FullSystem::linearizeAll_Reductor, this, fixLinearization, toRemove, _1, _2, _3, _4), 0, activeResiduals.size(), 0);lastEnergyP = treadReduce.stats[0];}else{Vec10 stats;//雅克比矩陣元素:偏導數(shù)。還有點屬性都給賦個值linearizeAll_Reductor(fixLinearization, toRemove, 0, activeResiduals.size(),&stats,0); lastEnergyP = stats[0];} //修改閾值frameHessians.back()->frameEnergyTH的值就是修改 target幀的frameEnergyTH值setNewFrameEnergyTH();//這段是剔除優(yōu)化后為外點的殘差if(fixLinearization){for(PointFrameResidual* r : activeResiduals){PointHessian* ph = r->point;if(ph->lastResiduals[0].first == r)ph->lastResiduals[0].second = r->state_state;else if(ph->lastResiduals[1].first == r)ph->lastResiduals[1].second = r->state_state;}int nResRemoved=0;for(int i=0;i<NUM_THREADS;i++){//循環(huán)某個PointFrameResidual點for(PointFrameResidual* r : toRemove[i]){//取其PointHessian點PointHessian* ph = r->point;if(ph->lastResiduals[0].first == r)ph->lastResiduals[0].first=0;else if(ph->lastResiduals[1].first == r)ph->lastResiduals[1].first=0;//該點所有殘差項for(unsigned int k = 0; k < ph->residuals.size(); k++)if(ph->residuals[k] == r){ef->dropResidual(r->efResidual);deleteOut<PointFrameResidual>(ph->residuals,k);nResRemoved++;break;}}}}return Vec3(lastEnergyP, lastEnergyR, num); }// applies step to linearization point.將步驟應用于線性化點。 //優(yōu)化結果生效應該是修改各個待優(yōu)化參數(shù)值 //并通過評估本次增量的大小來判斷是否結束迭代 //待優(yōu)化參數(shù)包括N個逆深度 m*8+4的相機位姿仿射相機參數(shù) bool FullSystem::doStepFromBackup(float stepfacC,float stepfacT,float stepfacR,float stepfacA,float stepfacD) { // float meanStepC=0,meanStepP=0,meanStepD=0; // meanStepC += Hcalib.step.norm();//pstepfac 是預測的增量唄Vec10 pstepfac;//平移pstepfac.segment<3>(0).setConstant(stepfacT);//旋轉pstepfac.segment<3>(3).setConstant(stepfacR);//仿射pstepfac.segment<4>(6).setConstant(stepfacA);float sumA=0, sumB=0, sumT=0, sumR=0, sumID=0, numID=0;float sumNID=0;if(setting_solverMode & SOLVER_MOMENTUM){Hcalib.setValue(Hcalib.value_backup + Hcalib.step);//循環(huán)關鍵幀窗口for(FrameHessian* fh : frameHessians){Vec10 step0 = fh->step;step0.head<6>() += 0.5f*(fh->step_backup.head<6>());//修改fh->setState(fh->state_backup + step0);sumA += step0[6] * step0[6];sumB += step0[7] * step0[7];sumT += step0.segment<3>(0).squaredNorm();sumR += step0.segment<3>(3).squaredNorm();for(PointHessian* ph : fh->pointHessians){float stepx = ph->step+0.5f*(ph->step_backup);ph->setIdepth(ph->idepth_backup + stepx);sumID += stepx * stepx;sumNID += fabsf(ph->idepth_backup);numID++;ph->setIdepthZero(ph->idepth_backup + stepx);}}}else{//相機參數(shù)增量*權重 stepfacC 修改相機參數(shù)1 * 4//修改相機參數(shù)Hcalib.setValue(Hcalib.value_backup + stepfacC*Hcalib.step);//循環(huán)關鍵幀窗口for(FrameHessian* fh : frameHessians){// cwiseProduct 函數(shù)功能 R = P .* Q 對應點相乘//修改位姿仿射參數(shù)等原有值+ 增量fh->setState(fh->state_backup + pstepfac.cwiseProduct(fh->step));sumA += fh->step[6]*fh->step[6];sumB += fh->step[7]*fh->step[7];sumT += fh->step.segment<3>(0).squaredNorm();sumR += fh->step.segment<3>(3).squaredNorm();//循環(huán)點修改逆深度值for(PointHessian* ph : fh->pointHessians){//修改逆深度值ph->setIdepth(ph->idepth_backup + stepfacD * ph->step);sumID += ph->step * ph->step;sumNID += fabsf(ph->idepth_backup);numID++;//修改idepth_zero值ph->setIdepthZero(ph->idepth_backup + stepfacD * ph->step);}}}sumA /= frameHessians.size();sumB /= frameHessians.size();sumR /= frameHessians.size();sumT /= frameHessians.size();sumID /= numID;sumNID /= numID;if(!setting_debugout_runquiet)printf("STEPS: A %.1f; B %.1f; R %.1f; T %.1f. \t",sqrtf(sumA) / (0.0005*setting_thOptIterations),sqrtf(sumB) / (0.00005*setting_thOptIterations),sqrtf(sumR) / (0.00005*setting_thOptIterations),sqrtf(sumT)*sumNID / (0.00005*setting_thOptIterations));EFDeltaValid=false;setPrecalcValues();return sqrtf(sumA) < 0.0005*setting_thOptIterations &&sqrtf(sumB) < 0.00005*setting_thOptIterations &&sqrtf(sumR) < 0.00005*setting_thOptIterations &&sqrtf(sumT)*sumNID < 0.00005*setting_thOptIterations; }// sets linearization point. //保留狀態(tài)有點逆深度idepth 相機參數(shù)Hcalib.value 位姿仿射fh->get_state() //以及它們對應的增量值 ph->step Hcalib.step fh->step void FullSystem::backupState(bool backupLastStep) {if(setting_solverMode & SOLVER_MOMENTUM){//非第一次迭代if(backupLastStep){Hcalib.step_backup = Hcalib.step;Hcalib.value_backup = Hcalib.value;for(FrameHessian* fh : frameHessians){fh->step_backup = fh->step;fh->state_backup = fh->get_state();for(PointHessian* ph : fh->pointHessians){ph->idepth_backup = ph->idepth;ph->step_backup = ph->step;}}}else//第一次迭代{Hcalib.step_backup.setZero();Hcalib.value_backup = Hcalib.value;//循環(huán)關鍵幀窗口for(FrameHessian* fh : frameHessians){fh->step_backup.setZero();fh->state_backup = fh->get_state();//循環(huán)成熟點for(PointHessian* ph : fh->pointHessians){ph->idepth_backup = ph->idepth;ph->step_backup=0;}}}}else{Hcalib.value_backup = Hcalib.value;for(FrameHessian* fh : frameHessians){fh->state_backup = fh->get_state();for(PointHessian* ph : fh->pointHessians)ph->idepth_backup = ph->idepth;}} }// sets linearization point. void FullSystem::loadSateBackup() {Hcalib.setValue(Hcalib.value_backup);for(FrameHessian* fh : frameHessians){fh->setState(fh->state_backup);for(PointHessian* ph : fh->pointHessians){ph->setIdepth(ph->idepth_backup);ph->setIdepthZero(ph->idepth_backup);}}EFDeltaValid=false;setPrecalcValues(); }double FullSystem::calcMEnergy() {if(setting_forceAceptStep) return 0;return ef->calcMEnergyF();}void FullSystem::printOptRes(const Vec3 &res, double resL, double resM, double resPrior, double LExact, float a, float b) {printf("A(%f)=(AV %.3f). Num: A(%'d) + M(%'d); ab %f %f!\n",res[0],sqrtf((float)(res[0] / (patternNum*ef->resInA))),ef->resInA,ef->resInM,a,b);}float FullSystem::optimize(int mnumOptIts) {if(frameHessians.size() < 2) return 0;if(frameHessians.size() < 3) mnumOptIts = 20;if(frameHessians.size() < 4) mnumOptIts = 15;// get statistics and active residuals.獲取統(tǒng)計數(shù)據(jù)和活動殘差。//清空活動點殘差隊列activeResiduals.clear();int numPoints = 0;//沒用int numLRes = 0;//沒用int galdg = 0;//循環(huán)窗口內(nèi)的幀for(FrameHessian* fh : frameHessians)for(PointHessian* ph : fh->pointHessians)//循環(huán)成熟點{//循環(huán)每個點在不同幀投影算的殘差for(PointFrameResidual* r : ph->residuals){//在外面舊成熟點相對新幀計算新殘差 isLinearized為false//在外面新激活成熟點相對非所在幀算新殘差 isLinearized為false//這些殘差點是累積下來的if(!r->efResidual->isLinearized){//加入活動點殘差隊列activeResiduals.push_back(r);//設置了幾個值r->resetOOB();//galdg ++;}elsenumLRes++;}//點總數(shù)numPoints++;}//相關導數(shù)計算Vec3 lastEnergy = linearizeAll(false);//這個函數(shù)返回值設置直接為0了double lastEnergyL = calcLEnergy();//這個函數(shù)返回值設置直接為0了double lastEnergyM = calcMEnergy();//在所有的 residual 中找到 isLinearized 為 false 的 residual，調用 //PointFrameResidual::applyRes(true)，設置它們的 PointFrameResidual::ResState //（按照 FullSystem::optimizeImmaturePoint 的結果），如果是正常的 //點(ResState::IN)調用EFResidual::takeDataF()把 EFResidual::JpJdF 設置一下。 //這就算完成了優(yōu)化的準備工作。還計算了每個殘差的 //JPJDF值是一個8*1的向量if(multiThreading)treadReduce.reduce(boost::bind(&FullSystem::applyRes_Reductor, this, true, _1, _2, _3, _4), 0, activeResiduals.size(), 50);else{//計算了下博客中的4.7.6提到的中間變量 applyRes_Reductor(true, 0, activeResiduals.size(), 0, 0);}if(!setting_debugout_runquiet){printf("Initial Error \t");printOptRes(lastEnergy, lastEnergyL, lastEnergyM, 0, 0, frameHessians.back()->aff_g2l().a, frameHessians.back()->aff_g2l().b);} //調試繪圖跟蹤debugPlotTracking();double lambda = 1e-1;//0.1float stepsize = 1;VecX previousX = VecX::Constant(CPARS + 8 * frameHessians.size(), NAN);//開始迭代優(yōu)化for(int iteration = 0; iteration < mnumOptIts; iteration++){// solve!//保存當前的狀態(tài)，這是為了在優(yōu)化結果不好的情況下可以回退backupState(iteration!=0);//solveSystemNew(0);//求解優(yōu)化增量方程解出的結果有68包含的各項的小增量，所有點深度值//的小增量。都是絕對增量值切記切記solveSystem(iteration, lambda);double incDirChange = (1e-20 + previousX.dot(ef->lastX)) / (1e-20 + previousX.norm() * ef->lastX.norm());previousX = ef->lastX;if(std::isfinite(incDirChange) && (setting_solverMode & SOLVER_STEPMOMENTUM)){float newStepsize = exp(incDirChange * 1.4);if(incDirChange < 0 && stepsize>1) stepsize = 1;stepsize = sqrtf(sqrtf(newStepsize * stepsize * stepsize * stepsize));if(stepsize > 2) stepsize=2;if(stepsize <0.25) stepsize=0.25;} //優(yōu)化結果生效應該是修改各個待優(yōu)化參數(shù)值 //并通過評估本次增量的大小來判斷是否結束迭代 //待優(yōu)化參數(shù)包括M個點的逆深度 m * 8 + 4的相機位姿仿射相機參數(shù)bool canbreak = doStepFromBackup(stepsize, stepsize, stepsize, stepsize, stepsize);// eval new energy!評估新能量//用上面修改的新數(shù)據(jù)計算新的能量值newEnergy，并且重新計算一組//相關導數(shù)Vec3 newEnergy = linearizeAll(false);double newEnergyL = calcLEnergy();//返回0 不用去費腦子了double newEnergyM = calcMEnergy();//返回0不用去費腦子了if(!setting_debugout_runquiet){printf("%s %d (L %.2f, dir %.2f, ss %.1f): \t",(newEnergy[0] + newEnergy[1] + newEnergyL + newEnergyM <lastEnergy[0] + lastEnergy[1] + lastEnergyL + lastEnergyM) ? "ACCEPT" : "REJECT",iteration,log10(lambda),incDirChange,stepsize);printOptRes(newEnergy, newEnergyL, newEnergyM , 0, 0, frameHessians.back()->aff_g2l().a, frameHessians.back()->aff_g2l().b);}if(setting_forceAceptStep || (newEnergy[0] + newEnergy[1] + newEnergyL + newEnergyM <lastEnergy[0] + lastEnergy[1] + lastEnergyL + lastEnergyM)){if(multiThreading)treadReduce.reduce(boost::bind(&FullSystem::applyRes_Reductor, this, true, _1, _2, _3, _4), 0, activeResiduals.size(), 50);else{//中間變量計算applyRes_Reductor(true,0,activeResiduals.size(),0,0);}lastEnergy = newEnergy;lastEnergyL = newEnergyL;lastEnergyM = newEnergyM;lambda *= 0.25;}else{//能量升高了就使用 FullSystem::loadSateBackup() 將結果回滾。loadSateBackup();//重新算一次lastEnergy = linearizeAll(false);lastEnergyL = calcLEnergy();lastEnergyM = calcMEnergy();lambda *= 1e2;}if(canbreak && iteration >= setting_minOptIterations) break;}Vec10 newStateZero = Vec10::Zero();newStateZero.segment<2>(6) = frameHessians.back()->get_state().segment<2>(6);//setStateZero賦值的地方用newStateZero賦值frameHessians.back()->setEvalPT(frameHessians.back()->PRE_worldToCam, newStateZero);EFDeltaValid = false;EFAdjointsValid = false;ef->setAdjointsF(&Hcalib);setPrecalcValues();//在跳出循環(huán)體之后調用一次 FullSystem::linearizeAll(true)， //效果是將優(yōu)化之后成為 outlier 的 residual 剔除，剩下 //正常的 residual 調用一次 EFResidual::takeDataF() 計算新的 //EFResidual::JpJdF （這里涉及到 FEJ）。注意一下 lastEnergy = linearizeAll(true);if(!std::isfinite((double)lastEnergy[0]) || !std::isfinite((double)lastEnergy[1]) || !std::isfinite((double)lastEnergy[2])){printf("KF Tracking failed: LOST!\n");isLost=true;}statistics_lastFineTrackRMSE = sqrtf((float)(lastEnergy[0] / (patternNum*ef->resInA)));if(calibLog != 0){(*calibLog) << Hcalib.value_scaled.transpose() <<" " << frameHessians.back()->get_state_scaled().transpose() <<" " << sqrtf((float)(lastEnergy[0] / (patternNum*ef->resInA))) <<" " << ef->resInM << "\n";calibLog->flush();}//最終在這要更新關鍵幀的位姿和仿射參數(shù) 。用優(yōu)化后的值更新。{boost::unique_lock<boost::mutex> crlock(shellPoseMutex);//循環(huán)關鍵幀窗口for(FrameHessian* fh : frameHessians){fh->shell->camToWorld = fh->PRE_camToWorld;fh->shell->aff_g2l = fh->aff_g2l();}}debugPlotTracking();//返回的這個值是點的平均殘差值的開方return sqrtf((float)(lastEnergy[0] / (patternNum * ef->resInA)));}void FullSystem::solveSystem(int iteration, double lambda) { //零空間給lastNullspaces_pose他們幾個賦值呢，affA affB壓根沒用到不用去理會ef->lastNullspaces_forLogging = getNullspaces(ef->lastNullspaces_pose,ef->lastNullspaces_scale,ef->lastNullspaces_affA,ef->lastNullspaces_affB); //解方程的主函數(shù)ef->solveSystemF(iteration, lambda,&Hcalib); }double FullSystem::calcLEnergy() {if(setting_forceAceptStep) return 0;double Ef = ef->calcLEnergyF_MT();return Ef;}void FullSystem::removeOutliers() {int numPointsDropped=0;for(FrameHessian* fh : frameHessians){for(unsigned int i=0;i<fh->pointHessians.size();i++){PointHessian* ph = fh->pointHessians[i];if(ph==0) continue;if(ph->residuals.size() == 0){fh->pointHessiansOut.push_back(ph);ph->efPoint->stateFlag = EFPointStatus::PS_DROP;fh->pointHessians[i] = fh->pointHessians.back();fh->pointHessians.pop_back();i--;numPointsDropped++;}}}ef->dropPointsF(); }std::vector<VecX> FullSystem::getNullspaces(std::vector<VecX> &nullspaces_pose1,std::vector<VecX> &nullspaces_scale1,std::vector<VecX> &nullspaces_affA,std::vector<VecX> &nullspaces_affB) { // 先做個清空nullspaces_pose1.clear();nullspaces_scale1.clear();nullspaces_affA.clear();nullspaces_affB.clear();int n = CPARS + frameHessians.size() * 8;std::vector<VecX> nullspaces_x0_pre;for(int i = 0; i < 6; i++){VecX nullspace_x0(n);nullspace_x0.setZero();for(FrameHessian* fh : frameHessians){nullspace_x0.segment<6>(CPARS+fh->idx * 8) = fh->nullspaces_pose.col(i);nullspace_x0.segment<3>(CPARS+fh->idx * 8) *= SCALE_XI_TRANS_INVERSE;nullspace_x0.segment<3>(CPARS+fh->idx * 8 + 3) *= SCALE_XI_ROT_INVERSE;}nullspaces_x0_pre.push_back(nullspace_x0);nullspaces_pose1.push_back(nullspace_x0);}for(int i=0;i<2;i++){VecX nullspace_x0(n);nullspace_x0.setZero();for(FrameHessian* fh : frameHessians){nullspace_x0.segment<2>(CPARS+fh->idx*8+6) = fh->nullspaces_affine.col(i).head<2>();nullspace_x0[CPARS+fh->idx*8+6] *= SCALE_A_INVERSE;nullspace_x0[CPARS+fh->idx*8+7] *= SCALE_B_INVERSE;}nullspaces_x0_pre.push_back(nullspace_x0);if(i==0) nullspaces_affA.push_back(nullspace_x0);if(i==1) nullspaces_affB.push_back(nullspace_x0);}VecX nullspace_x0(n);nullspace_x0.setZero();for(FrameHessian* fh : frameHessians){nullspace_x0.segment<6>(CPARS+fh->idx*8) = fh->nullspaces_scale;nullspace_x0.segment<3>(CPARS+fh->idx*8) *= SCALE_XI_TRANS_INVERSE;nullspace_x0.segment<3>(CPARS+fh->idx*8+3) *= SCALE_XI_ROT_INVERSE;}nullspaces_x0_pre.push_back(nullspace_x0);nullspaces_scale1.push_back(nullspace_x0);return nullspaces_x0_pre; }}

/** * This file is part of DSO. * * Copyright 2016 Technical University of Munich and Intel. * Developed by Jakob Engel <engelj at in dot tum dot de>, * for more information see <http://vision.in.tum.de/dso>. * If you use this code, please cite the respective publications as * listed on the above website. * * DSO is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * DSO is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with DSO. If not, see <http://www.gnu.org/licenses/>. */#include "OptimizationBackend/EnergyFunctional.h" #include "OptimizationBackend/EnergyFunctionalStructs.h" #include "FullSystem/FullSystem.h" #include "FullSystem/HessianBlocks.h" #include "FullSystem/Residuals.h" #include "OptimizationBackend/AccumulatedSCHessian.h" #include "OptimizationBackend/AccumulatedTopHessian.h"#if !defined(__SSE3__) && !defined(__SSE2__) && !defined(__SSE1__) #include "SSE2NEON.h" #endifnamespace dso {bool EFAdjointsValid = false; bool EFIndicesValid = false; bool EFDeltaValid = false;// 這個函數(shù)得到博客<<跟蹤與優(yōu)化>>4.5節(jié)所述。優(yōu)化窗口中兩幀 //之間都會計算一個這個矩陣 void EnergyFunctional::setAdjointsF(CalibHessian* Hcalib) {if(adHost != 0) delete[] adHost;if(adTarget != 0) delete[] adTarget;adHost = new Mat88[nFrames*nFrames];adTarget = new Mat88[nFrames*nFrames];//遍歷隊列中的幀。for(int h=0;h<nFrames;h++)//nFrames幀數(shù)for(int t=0;t<nFrames;t++){FrameHessian* host = frames[h]->data;FrameHessian* target = frames[t]->data; //博客<<跟蹤與優(yōu)化>>H幀到T幀的相對位姿變換矩陣 4.5公式SE3 hostToTarget = target->get_worldToCam_evalPT() * host->get_worldToCam_evalPT().inverse();//初始化一個單位矩陣Mat88 AH = Mat88::Identity();Mat88 AT = Mat88::Identity();//求hostToTarget的伴隨矩陣的轉置//4.5.3AH.topLeftCorner<6,6>() = -hostToTarget.Adj().transpose();//4.5.2AT.topLeftCorner<6,6>() = Mat66::Identity();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]; //block提取AH矩陣(0,0)為起始位大小為3行*8 列的塊AH.block<3,8>(0,0) *= SCALE_XI_TRANS;AH.block<3,8>(3,0) *= SCALE_XI_ROT;AH.block<1,8>(6,0) *= SCALE_A;AH.block<1,8>(7,0) *= SCALE_B;AT.block<3,8>(0,0) *= SCALE_XI_TRANS;AT.block<3,8>(3,0) *= SCALE_XI_ROT;AT.block<1,8>(6,0) *= SCALE_A;AT.block<1,8>(7,0) *= SCALE_B;//th增量對hw增量的偏導 th屬于相對位姿增量hw為絕對位姿增量下同adHost[h + t * nFrames] = AH;//th增量對tw增量的偏導adTarget[h + t * nFrames] = AT;}cPrior = VecC::Constant(setting_initialCalibHessian);if(adHostF != 0) delete[] adHostF;if(adTargetF != 0) delete[] adTargetF;adHostF = new Mat88f[nFrames*nFrames];adTargetF = new Mat88f[nFrames*nFrames]; //下面是將結果傳出去。 //結果內(nèi)容為相對位姿增量對絕對位姿增量的求導 . 還有仿射系數(shù) 矩陣 //維數(shù)是 8*8 //絕對位姿分別是host幀和target幀for(int h = 0; h < nFrames; h++)for(int t = 0; t < nFrames; t++){adHostF[h + t * nFrames] = adHost[h + t * nFrames].cast<float>();adTargetF[h + t * nFrames] = adTarget[h + t * nFrames].cast<float>();}cPriorF = cPrior.cast<float>();EFAdjointsValid = true; }EnergyFunctional::EnergyFunctional() {adHost=0;adTarget=0;red=0;adHostF=0;adTargetF=0;adHTdeltaF=0;nFrames = nResiduals = nPoints = 0;HM = MatXX::Zero(CPARS,CPARS);bM = VecX::Zero(CPARS);accSSE_top_L = new AccumulatedTopHessianSSE();accSSE_top_A = new AccumulatedTopHessianSSE();accSSE_bot = new AccumulatedSCHessianSSE();resInA = resInL = resInM = 0;currentLambda=0; } EnergyFunctional::~EnergyFunctional() {for(EFFrame* f : frames){for(EFPoint* p : f->points){for(EFResidual* r : p->residualsAll){r->data->efResidual=0;delete r;}p->data->efPoint=0;delete p;}f->data->efFrame=0;delete f;}if(adHost != 0) delete[] adHost;if(adTarget != 0) delete[] adTarget;if(adHostF != 0) delete[] adHostF;if(adTargetF != 0) delete[] adTargetF;if(adHTdeltaF != 0) delete[] adHTdeltaF;delete accSSE_top_L;delete accSSE_top_A;delete accSSE_bot; }void EnergyFunctional::setDeltaF(CalibHessian* HCalib) { //重新創(chuàng)建adHTdeltaFif(adHTdeltaF != 0) delete[] adHTdeltaF;//位姿與光度仿射系數(shù) 維度為1 *8 * 64(關鍵幀窗口內(nèi)有8幀的話)adHTdeltaF = new Mat18f[nFrames * nFrames];//遍歷關鍵幀for(int h = 0; h < nFrames; h++)for(int t = 0; t <nFrames; t++){//關鍵幀窗口配對的標簽例如窗口共有6幀那么idx從0~35int idx = h + t * nFrames;//位姿與光度仿射變換的delta值 //-----------------------計算的是f(x) 對hw的偏導 + f(x)對tw的偏導不太理解這塊//get_state_minus_stateZero().head<8>() 取的就是位姿和仿射變換的差值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];}//相機的delta值cDeltaF = HCalib->value_minus_value_zero.cast<float>();//點逆深度的delta值for(EFFrame* f : frames){f->delta = f->data->get_state_minus_stateZero().head<8>();f->delta_prior = (f->data->get_state() - f->data->getPriorZero()).head<8>();for(EFPoint* p : f->points)p->deltaF = p->data->idepth - p->data->idepth_zero;}EFDeltaValid = true; }// accumulates & shifts L. void EnergyFunctional::accumulateAF_MT(MatXX &H, VecX &b, bool MT) {if(MT){red->reduce(boost::bind(&AccumulatedTopHessianSSE::setZero, accSSE_top_A, nFrames, _1, _2, _3, _4), 0, 0, 0);red->reduce(boost::bind(&AccumulatedTopHessianSSE::addPointsInternal<0>,accSSE_top_A, &allPoints, this, _1, _2, _3, _4), 0, allPoints.size(), 50);accSSE_top_A->stitchDoubleMT(red,H,b,this,false,true);resInA = accSSE_top_A->nres[0];//點總數(shù)應該是// printf("====resInA====%d\n", resInA);}else{accSSE_top_A->setZero(nFrames);//遍歷幀參與優(yōu)化的幀其就是關鍵幀窗口內(nèi)的幀for(EFFrame* f : frames)for(EFPoint* p : f->points)//遍歷點accSSE_top_A->addPoint<0>(p,this);//具體計算H b的部分accSSE_top_A->stitchDoubleMT(red,H,b,this,false,false);resInA = accSSE_top_A->nres[0];} }// accumulates & shifts L. void EnergyFunctional::accumulateLF_MT(MatXX &H, VecX &b, bool MT) {if(MT){red->reduce(boost::bind(&AccumulatedTopHessianSSE::setZero, accSSE_top_L, nFrames, _1, _2, _3, _4), 0, 0, 0);red->reduce(boost::bind(&AccumulatedTopHessianSSE::addPointsInternal<1>,accSSE_top_L, &allPoints, this, _1, _2, _3, _4), 0, allPoints.size(), 50);accSSE_top_L->stitchDoubleMT(red,H,b,this,true,true);resInL = accSSE_top_L->nres[0];}else{accSSE_top_L->setZero(nFrames);for(EFFrame* f : frames)for(EFPoint* p : f->points)accSSE_top_L->addPoint<1>(p,this);accSSE_top_L->stitchDoubleMT(red,H,b,this,true,false);resInL = accSSE_top_L->nres[0];} }void EnergyFunctional::accumulateSCF_MT(MatXX &H, VecX &b, bool MT) {if(MT){red->reduce(boost::bind(&AccumulatedSCHessianSSE::setZero, accSSE_bot, nFrames, _1, _2, _3, _4), 0, 0, 0);red->reduce(boost::bind(&AccumulatedSCHessianSSE::addPointsInternal,accSSE_bot, &allPoints, true, _1, _2, _3, _4), 0, allPoints.size(), 50);accSSE_bot->stitchDoubleMT(red,H,b,this,true);}else{accSSE_bot->setZero(nFrames);for(EFFrame* f : frames)for(EFPoint* p : f->points)accSSE_bot->addPoint(p, true);accSSE_bot->stitchDoubleMT(red, H, b,this,false);} }void EnergyFunctional::resubstituteF_MT(VecX x, CalibHessian* HCalib, bool MT) {assert(x.size() == CPARS+nFrames*8);//計算相機參數(shù) 位姿仿射變換參數(shù)增量部分 schur消元后的gαVecXf xF = x.cast<float>();//gβ//相機參數(shù)的增量值HCalib->step = - x.head<CPARS>();Mat18f* xAd = new Mat18f[nFrames * nFrames];//相機參數(shù)增量VecCf cstep = xF.head<CPARS>();//遍歷窗口內(nèi)的幀for(EFFrame* h : frames){//相機位姿以及仿射增量h->data->step.head<8>() = - x.segment<8>(CPARS + 8 * h->idx);h->data->step.tail<2>().setZero();//遍歷窗口內(nèi)的幀for(EFFrame* t : frames){//絕對位姿增量* 相對位姿轉置對絕對位姿求偏導xAd[nFrames * h->idx + t->idx] = xF.segment<8>(CPARS + 8 * h->idx).transpose() * adHostF[h->idx+nFrames * t->idx]+ xF.segment<8>(CPARS + 8 * t->idx).transpose() * adTargetF[h->idx+nFrames * t->idx];}} //schur消元的另外一個增量求解 schur消元后的gβif(MT){red->reduce(boost::bind(&EnergyFunctional::resubstituteFPt,this, cstep, xAd, _1, _2, _3, _4), 0, allPoints.size(), 50);}else{//計算逆深度的增量部分的代碼resubstituteFPt(cstep, xAd, 0, allPoints.size(), 0, 0);}delete[] xAd; } //這個函數(shù)里計算的是逆深度的增量值 gα void EnergyFunctional::resubstituteFPt(const VecCf &xc, Mat18f* xAd, int min, int max, Vec10* stats, int tid) { //遍歷點for(int k = min; k < max; k++){EFPoint *p = allPoints[k];int ngoodres = 0;for(EFResidual *r : p->residualsAll) if(r->isActive()) ngoodres++;if(ngoodres==0){p->data->step = 0;continue;}//接著在這里幾行計算逆深度值的增量//b是殘差*殘差對逆深度的偏導 ---->之和float b = p->bdSumF;//xc是相機參數(shù)增量b -= xc.dot(p->Hcd_accAF + p->Hcd_accLF);//循環(huán)殘差項一個點對應多個殘差for(EFResidual* r : p->residualsAll){if(!r->isActive()) continue;//JpJdF 殘差對位姿的偏導的轉置* 殘差對逆深度的偏導b -= xAd[r->hostIDX * nFrames + r->targetIDX] * r->JpJdF;}// 逆深度的增量存儲于p->data->step中p->data->step = -b * p->HdiF; assert(std::isfinite(p->data->step));} }double EnergyFunctional::calcMEnergyF() {assert(EFDeltaValid);assert(EFAdjointsValid);assert(EFIndicesValid);VecX delta = getStitchedDeltaF();return delta.dot(2*bM + HM*delta); }void EnergyFunctional::calcLEnergyPt(int min, int max, Vec10* stats, int tid) {Accumulator11 E;E.initialize();VecCf dc = cDeltaF;for(int i=min;i<max;i++){EFPoint* p = allPoints[i];float dd = p->deltaF;for(EFResidual* r : p->residualsAll){if(!r->isLinearized || !r->isActive()) continue;Mat18f dp = adHTdeltaF[r->hostIDX+nFrames*r->targetIDX];RawResidualJacobian* rJ = r->J;// compute Jp*deltafloat Jp_delta_x_1 = rJ->Jpdxi[0].dot(dp.head<6>())+rJ->Jpdc[0].dot(dc)+rJ->Jpdd[0]*dd;float Jp_delta_y_1 = rJ->Jpdxi[1].dot(dp.head<6>())+rJ->Jpdc[1].dot(dc)+rJ->Jpdd[1]*dd;__m128 Jp_delta_x = _mm_set1_ps(Jp_delta_x_1);__m128 Jp_delta_y = _mm_set1_ps(Jp_delta_y_1);__m128 delta_a = _mm_set1_ps((float)(dp[6]));__m128 delta_b = _mm_set1_ps((float)(dp[7]));for(int i=0;i+3<patternNum;i+=4){// PATTERN: E = (2*res_toZeroF + J*delta) * J*delta.__m128 Jdelta = _mm_mul_ps(_mm_load_ps(((float*)(rJ->JIdx))+i),Jp_delta_x);Jdelta = _mm_add_ps(Jdelta,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JIdx+1))+i),Jp_delta_y));Jdelta = _mm_add_ps(Jdelta,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JabF))+i),delta_a));Jdelta = _mm_add_ps(Jdelta,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JabF+1))+i),delta_b));__m128 r0 = _mm_load_ps(((float*)&r->res_toZeroF)+i);r0 = _mm_add_ps(r0,r0);r0 = _mm_add_ps(r0,Jdelta);Jdelta = _mm_mul_ps(Jdelta,r0);E.updateSSENoShift(Jdelta);}for(int i=((patternNum>>2)<<2); i < patternNum; i++){float Jdelta = rJ->JIdx[0][i]*Jp_delta_x_1 + rJ->JIdx[1][i]*Jp_delta_y_1 +rJ->JabF[0][i]*dp[6] + rJ->JabF[1][i]*dp[7];E.updateSingleNoShift((float)(Jdelta * (Jdelta + 2*r->res_toZeroF[i])));}}E.updateSingle(p->deltaF*p->deltaF*p->priorF);}E.finish();(*stats)[0] += E.A; }double EnergyFunctional::calcLEnergyF_MT() {assert(EFDeltaValid);assert(EFAdjointsValid);assert(EFIndicesValid);double E = 0;//printf("=====EFFrame.size====%d\n", frames.size());for(EFFrame* f : frames)E += f->delta_prior.cwiseProduct(f->prior).dot(f->delta_prior);E += cDeltaF.cwiseProduct(cPriorF).dot(cDeltaF);red->reduce(boost::bind(&EnergyFunctional::calcLEnergyPt,this, _1, _2, _3, _4), 0, allPoints.size(), 50);return E+red->stats[0]; }EFResidual* EnergyFunctional::insertResidual(PointFrameResidual* r) {EFResidual* efr = new EFResidual(r, r->point->efPoint, r->host->efFrame, r->target->efFrame);efr->idxInAll = r->point->efPoint->residualsAll.size();r->point->efPoint->residualsAll.push_back(efr);connectivityMap[(((uint64_t)efr->host->frameID) << 32) + ((uint64_t)efr->target->frameID)][0]++;nResiduals++;r->efResidual = efr;return efr; } //只有關鍵幀才會nFrames自加 frames隊列中會增加成員 //此函數(shù)實現(xiàn)功能: 創(chuàng)建新幀的能量窗口目標，將其加入窗口隊列，改變BM HMD等尺寸 //重新計算窗口內(nèi)兩幀之間的相對增量對絕對增量的偏導，將新幀的點納入窗口總 //點隊列中。 EFFrame* EnergyFunctional::insertFrame(FrameHessian* fh, CalibHessian* Hcalib) {//創(chuàng)建EFFrame對象分配空間EFFrame* eff = new EFFrame(fh);eff->idx = frames.size();//將eff對象插入EFFrame類的frames的隊列中frames.push_back(eff);nFrames++;fh->efFrame = eff; //CPARSS是表示相機的四個內(nèi)參數(shù)吧，8 表示位姿加仿射，nFrames就是關鍵幀窗口中的幀數(shù)assert(HM.cols() == 8 * nFrames + CPARS-8);//顯然就是給舒爾消元矩陣分配空間呢啊bM.conservativeResize(8 * nFrames + CPARS);HM.conservativeResize(8 * nFrames + CPARS, 8 * nFrames + CPARS);//最新幀的最后8位置為0bM.tail<8>().setZero();////最后8列置為0HM.rightCols<8>().setZero();//最下面8行置為0HM.bottomRows<8>().setZero();EFIndicesValid = false;EFAdjointsValid=false;EFDeltaValid=false; //每次插入關鍵幀時計算。H T兩幀之間相對增量對二者絕對增量 //的偏導值，因為最終窗口優(yōu)化過程是計算各項的絕對增量值的偏導 //通過遍歷兩兩之間的關系，提前計算好adHostF和adTargetF //8*8的矩陣一共有64個。每兩幀之間都會有一個結果setAdjointsF(Hcalib);//新入隊的點加入優(yōu)化窗口的總點隊列里去makeIDX();for(EFFrame* fh2 : frames){connectivityMap[(((uint64_t)eff->frameID) << 32) + ((uint64_t)fh2->frameID)] = Eigen::Vector2i(0,0);if(fh2 != eff)connectivityMap[(((uint64_t)fh2->frameID) << 32) + ((uint64_t)eff->frameID)] = Eigen::Vector2i(0,0);} //返回創(chuàng)建的這個EFFrame類的對象return eff; } EFPoint* EnergyFunctional::insertPoint(PointHessian* ph) {EFPoint* efp = new EFPoint(ph, ph->host->efFrame);//idxInPoints 存的是efp->idxInPoints = ph->host->efFrame->points.size();//efp入隊ph->host->efFrame->points.push_back(efp);nPoints++;ph->efPoint = efp;EFIndicesValid = false;return efp; }void EnergyFunctional::dropResidual(EFResidual* r) {EFPoint* p = r->point;assert(r == p->residualsAll[r->idxInAll]);p->residualsAll[r->idxInAll] = p->residualsAll.back();p->residualsAll[r->idxInAll]->idxInAll = r->idxInAll;p->residualsAll.pop_back();if(r->isActive())r->host->data->shell->statistics_goodResOnThis++;elser->host->data->shell->statistics_outlierResOnThis++;connectivityMap[(((uint64_t)r->host->frameID) << 32) + ((uint64_t)r->target->frameID)][0]--;nResiduals--;r->data->efResidual=0;delete r; } void EnergyFunctional::marginalizeFrame(EFFrame* fh) {assert(EFDeltaValid);assert(EFAdjointsValid);assert(EFIndicesValid);assert((int)fh->points.size()==0);int ndim = nFrames*8+CPARS-8;// new dimensionint odim = nFrames*8+CPARS;// old dimension// VecX eigenvaluesPre = HM.eigenvalues().real(); // std::sort(eigenvaluesPre.data(), eigenvaluesPre.data()+eigenvaluesPre.size()); //if((int)fh->idx != (int)frames.size()-1){int io = fh->idx*8+CPARS; // index of frame to move to endint ntail = 8*(nFrames-fh->idx-1);assert((io+8+ntail) == nFrames*8+CPARS);Vec8 bTmp = bM.segment<8>(io);VecX tailTMP = bM.tail(ntail);bM.segment(io,ntail) = tailTMP;bM.tail<8>() = bTmp;MatXX HtmpCol = HM.block(0,io,odim,8);MatXX rightColsTmp = HM.rightCols(ntail);HM.block(0,io,odim,ntail) = rightColsTmp;HM.rightCols(8) = HtmpCol;MatXX HtmpRow = HM.block(io,0,8,odim);MatXX botRowsTmp = HM.bottomRows(ntail);HM.block(io,0,ntail,odim) = botRowsTmp;HM.bottomRows(8) = HtmpRow;}// // marginalize. First add prior here, instead of to active.HM.bottomRightCorner<8,8>().diagonal() += fh->prior;bM.tail<8>() += fh->prior.cwiseProduct(fh->delta_prior);// std::cout << std::setprecision(16) << "HMPre:\n" << HM << "\n\n";VecX SVec = (HM.diagonal().cwiseAbs()+VecX::Constant(HM.cols(), 10)).cwiseSqrt();VecX SVecI = SVec.cwiseInverse();// std::cout << std::setprecision(16) << "SVec: " << SVec.transpose() << "\n\n"; // std::cout << std::setprecision(16) << "SVecI: " << SVecI.transpose() << "\n\n";// scale!MatXX HMScaled = SVecI.asDiagonal() * HM * SVecI.asDiagonal();VecX bMScaled = SVecI.asDiagonal() * bM;// invert bottom part!Mat88 hpi = HMScaled.bottomRightCorner<8,8>();hpi = 0.5f*(hpi+hpi);hpi = hpi.inverse();hpi = 0.5f*(hpi+hpi);// schur-complement!MatXX bli = HMScaled.bottomLeftCorner(8,ndim).transpose() * hpi;HMScaled.topLeftCorner(ndim,ndim).noalias() -= bli * HMScaled.bottomLeftCorner(8,ndim);bMScaled.head(ndim).noalias() -= bli*bMScaled.tail<8>();//unscale!HMScaled = SVec.asDiagonal() * HMScaled * SVec.asDiagonal();bMScaled = SVec.asDiagonal() * bMScaled;// set.HM = 0.5*(HMScaled.topLeftCorner(ndim,ndim) + HMScaled.topLeftCorner(ndim,ndim).transpose());bM = bMScaled.head(ndim);// remove from vector, without changing the order!for(unsigned int i=fh->idx; i+1<frames.size();i++){frames[i] = frames[i+1];frames[i]->idx = i;}frames.pop_back();nFrames--;fh->data->efFrame=0;assert((int)frames.size()*8+CPARS == (int)HM.rows());assert((int)frames.size()*8+CPARS == (int)HM.cols());assert((int)frames.size()*8+CPARS == (int)bM.size());assert((int)frames.size() == (int)nFrames);// VecX eigenvaluesPost = HM.eigenvalues().real(); // std::sort(eigenvaluesPost.data(), eigenvaluesPost.data()+eigenvaluesPost.size());// std::cout << std::setprecision(16) << "HMPost:\n" << HM << "\n\n";// std::cout << "EigPre:: " << eigenvaluesPre.transpose() << "\n"; // std::cout << "EigPost: " << eigenvaluesPost.transpose() << "\n";EFIndicesValid = false;EFAdjointsValid=false;EFDeltaValid=false;makeIDX();delete fh; }void EnergyFunctional::marginalizePointsF() {assert(EFDeltaValid);assert(EFAdjointsValid);assert(EFIndicesValid);allPointsToMarg.clear();for(EFFrame* f : frames){for(int i=0;i<(int)f->points.size();i++){EFPoint* p = f->points[i];if(p->stateFlag == EFPointStatus::PS_MARGINALIZE){p->priorF *= setting_idepthFixPriorMargFac;for(EFResidual* r : p->residualsAll)if(r->isActive())connectivityMap[(((uint64_t)r->host->frameID) << 32) + ((uint64_t)r->target->frameID)][1]++;allPointsToMarg.push_back(p);}}}accSSE_bot->setZero(nFrames);accSSE_top_A->setZero(nFrames);for(EFPoint* p : allPointsToMarg){accSSE_top_A->addPoint<2>(p,this);accSSE_bot->addPoint(p,false);removePoint(p);}MatXX M, Msc;VecX Mb, Mbsc;accSSE_top_A->stitchDouble(M,Mb,this,false,false);accSSE_bot->stitchDouble(Msc,Mbsc,this);resInM+= accSSE_top_A->nres[0];MatXX H = M-Msc;VecX b = Mb-Mbsc;if(setting_solverMode & SOLVER_ORTHOGONALIZE_POINTMARG){// have a look if prior is there.bool haveFirstFrame = false;for(EFFrame* f : frames) if(f->frameID==0) haveFirstFrame=true;if(!haveFirstFrame)orthogonalize(&b, &H);}HM += setting_margWeightFac*H;bM += setting_margWeightFac*b;if(setting_solverMode & SOLVER_ORTHOGONALIZE_FULL){orthogonalize(&bM, &HM);}EFIndicesValid = false;makeIDX(); }void EnergyFunctional::dropPointsF() {for(EFFrame* f : frames){for(int i=0;i<(int)f->points.size();i++){EFPoint* p = f->points[i];if(p->stateFlag == EFPointStatus::PS_DROP){removePoint(p);i--;}}}EFIndicesValid = false;makeIDX(); }void EnergyFunctional::removePoint(EFPoint* p) {for(EFResidual* r : p->residualsAll)dropResidual(r);EFFrame* h = p->host;h->points[p->idxInPoints] = h->points.back();h->points[p->idxInPoints]->idxInPoints = p->idxInPoints;h->points.pop_back();nPoints--;p->data->efPoint = 0;EFIndicesValid = false;delete p; }void EnergyFunctional::orthogonalize(VecX* b, MatXX* H) { //決定對哪些空間進行正交化 // decide to which nullspaces to orthogonalize.std::vector<VecX> ns; //位姿零空間ns.insert(ns.end(), lastNullspaces_pose.begin(), lastNullspaces_pose.end()); //尺度零空間ns.insert(ns.end(), lastNullspaces_scale.begin(), lastNullspaces_scale.end());//構建零空間矩陣// make Nullspaces matrixMatXX N(ns[0].rows(), ns.size());for(unsigned int i = 0; i < ns.size(); i++)N.col(i) = ns[i].normalized();// compute Npi := N * (N' * N)^-1 = pseudo inverse of N.Eigen::JacobiSVD<MatXX> svdNN(N, Eigen::ComputeThinU | Eigen::ComputeThinV);VecX SNN = svdNN.singularValues();double minSv = 1e10, maxSv = 0;for(int i=0;i<SNN.size();i++){if(SNN[i] < minSv) minSv = SNN[i];if(SNN[i] > maxSv) maxSv = SNN[i];}for(int i = 0;i < SNN.size(); i++){ if(SNN[i] > setting_solverModeDelta*maxSv) SNN[i] = 1.0 / SNN[i]; else SNN[i] = 0; }MatXX Npi = svdNN.matrixU() * SNN.asDiagonal() * svdNN.matrixV().transpose(); // [dim] x 9.MatXX NNpiT = N*Npi.transpose(); // [dim] x [dim].MatXX NNpiTS = 0.5*(NNpiT + NNpiT.transpose()); // = N * (N' * N)^-1 * N'.//減去零空間部分得到正交化結果就是博客圖藍色小三角的位置吧if(b!=0) *b -= NNpiTS * *b;if(H!=0) *H -= NNpiTS * *H * NNpiTS;}//求解結果就是lastX 以及逆深度點的絕對增量值 //此結果中包含了相機位姿光度參數(shù)相機參數(shù)共4 + 8 * N(關鍵幀個數(shù)) //還包含了M個點的逆深度信息 ------------------------->的增量 void EnergyFunctional::solveSystemF(int iteration, double lambda, CalibHessian* HCalib) {if(setting_solverMode & SOLVER_USE_GN) lambda=0;if(setting_solverMode & SOLVER_FIX_LAMBDA) lambda = 1e-5;assert(EFDeltaValid);assert(EFAdjointsValid);assert(EFIndicesValid);MatXX HL_top, HA_top, H_sc;VecX bL_top, bA_top, bM_top, b_sc;//計算HA_top 矩陣內(nèi)容大小68*68 就是增量方程中的Hββ部分//bA_top是JTXr部分舒爾消元后方程組的第二行計算所需的參數(shù)//計算Hxx與Jxtr兩個值accumulateAF_MT(HA_top, bA_top,multiThreading);//這個函數(shù)沒啥用因為里面的某個條件永遠得不到滿足accumulateLF_MT(HL_top, bL_top,multiThreading); //計算 schur部分參數(shù)存放在H_sc() b_sc()中 //舒爾消元后方程組的第二行計算所需的參數(shù) //最終得到的 Hsc 是 68x68 的矩陣，bsc 是 68x1 的矩陣。accumulateSCF_MT(H_sc, b_sc,multiThreading);bM_top = (bM+ HM * getStitchedDeltaF());MatXX HFinal_top;VecX bFinal_top;if(setting_solverMode & SOLVER_ORTHOGONALIZE_SYSTEM){//調試看不走這個分支// have a look if prior is there.bool haveFirstFrame = false;for(EFFrame* f : frames) if(f->frameID==0) haveFirstFrame=true;//對應公式MatXX HT_act = HL_top + HA_top - H_sc;VecX bT_act = bL_top + bA_top - b_sc;if(!haveFirstFrame)orthogonalize(&bT_act, &HT_act);HFinal_top = HT_act + HM;bFinal_top = bT_act + bM_top;lastHS = HFinal_top;lastbS = bFinal_top;for(int i=0;i<8*nFrames+CPARS;i++) HFinal_top(i,i) *= (1+lambda);}else{HFinal_top = HL_top + HM + HA_top;//后半部分是舒爾消元等式右下角值 bL_top是0bFinal_top = bL_top + bM_top + bA_top - b_sc;//這個是舒爾消元后等式左lastHS = HFinal_top - H_sc;//舒爾消元等式右lastbS = bFinal_top;for(int i = 0; i < 8 * nFrames + CPARS; i++) HFinal_top(i,i) *= (1 + lambda);HFinal_top -= H_sc * (1.0f/(1 + lambda));}VecX x;if(setting_solverMode & SOLVER_SVD){//調試看不走這個分支VecX SVecI = HFinal_top.diagonal().cwiseSqrt().cwiseInverse();MatXX HFinalScaled = SVecI.asDiagonal() * HFinal_top * SVecI.asDiagonal();VecX bFinalScaled = SVecI.asDiagonal() * bFinal_top;Eigen::JacobiSVD<MatXX> svd(HFinalScaled, Eigen::ComputeThinU | Eigen::ComputeThinV);VecX S = svd.singularValues();double minSv = 1e10, maxSv = 0;for(int i = 0; i < S.size(); i++){if(S[i] < minSv) minSv = S[i];if(S[i] > maxSv) maxSv = S[i];}VecX Ub = svd.matrixU().transpose()*bFinalScaled;int setZero=0;for(int i = 0;i < Ub.size(); i++){if(S[i] < setting_solverModeDelta*maxSv){ Ub[i] = 0; setZero++; }if((setting_solverMode & SOLVER_SVD_CUT7) && (i >= Ub.size()-7)){ Ub[i] = 0; setZero++; }else Ub[i] /= S[i];}x = SVecI.asDiagonal() * svd.matrixV() * Ub;}else{//std::cout<<"==="<<VecX::Constant(HFinal_top.cols(), 10)<<std::endl;//asDiagonal矩陣轉置 ldlt求解線性方程組 Ax = b cwiseSqrt 單個元素開方 cwiseInverse 1/單個元素 //Constant 常量矩陣值都為10VecX SVecI = (HFinal_top.diagonal()+VecX::Constant(HFinal_top.cols(), 10)).cwiseSqrt().cwiseInverse();MatXX HFinalScaled = SVecI.asDiagonal() * HFinal_top * SVecI.asDiagonal();x = SVecI.asDiagonal() * HFinalScaled.ldlt().solve(SVecI.asDiagonal() * bFinal_top);// SVec.asDiagonal() * svd.matrixV() * Ub;}if((setting_solverMode & SOLVER_ORTHOGONALIZE_X) || (iteration >= 2 && (setting_solverMode & SOLVER_ORTHOGONALIZE_X_LATER))){VecX xOld = x;//零空間就當對位姿和尺度做了次修正吧 orthogonalize(&x, 0);} //這個應該是相機參數(shù) 位姿仿射的絕對增量值lastX = x;currentLambda= lambda;//計算一次點的逆深度的絕對增量值resubstituteF_MT(x, HCalib,multiThreading);currentLambda=0;} // 來新幀的時候就把frames中的所有幀的點重新排一次 void EnergyFunctional::makeIDX() { //重新將順序排一下frames中按序號0 ~ frames.size()來排號for(unsigned int idx=0;idx<frames.size();idx++)frames[idx]->idx = idx;//allPoints隊列清空 allPoints.clear();//遍歷frames 所有幀for(EFFrame* f : frames)for(EFPoint* p : f->points)//遍歷幀上的點{//重新對allPoints隊列賦值allPoints.push_back(p);//遍歷點的所有殘差for(EFResidual* r : p->residualsAll){//改變殘差的ID號r->hostIDX = r->host->idx;r->targetIDX = r->target->idx;}}EFIndicesValid=true; }VecX EnergyFunctional::getStitchedDeltaF() const {VecX d = VecX(CPARS+nFrames*8); d.head<CPARS>() = cDeltaF.cast<double>();for(int h=0;h<nFrames;h++) d.segment<8>(CPARS+8*h) = frames[h]->delta;return d; }} /** * This file is part of DSO. * * Copyright 2016 Technical University of Munich and Intel. * Developed by Jakob Engel <engelj at in dot tum dot de>, * for more information see <http://vision.in.tum.de/dso>. * If you use this code, please cite the respective publications as * listed on the above website. * * DSO is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * DSO is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with DSO. If not, see <http://www.gnu.org/licenses/>. */#include "OptimizationBackend/EnergyFunctionalStructs.h" #include "OptimizationBackend/EnergyFunctional.h" #include "FullSystem/FullSystem.h" #include "FullSystem/HessianBlocks.h" #include "FullSystem/Residuals.h"#if !defined(__SSE3__) && !defined(__SSE2__) && !defined(__SSE1__) #include "SSE2NEON.h" #endifnamespace dso {//博客4.7.6 列出代表的實際意義很明確 void EFResidual::takeDataF() {std::swap<RawResidualJacobian*>(J, data->J);Vec2f JI_JI_Jd = J->JIdx2 * J->Jpdd;for(int i = 0; i < 6; i++)JpJdF[i] = J->Jpdxi[0][i] * JI_JI_Jd[0] + J->Jpdxi[1][i] * JI_JI_Jd[1]; //JpJdF 殘差對位姿的偏導的轉置* 殘差對逆深度的偏導JpJdF.segment<2>(6) = J->JabJIdx * J->Jpdd; }void EFFrame::takeData() {prior = data->getPrior().head<8>();delta = data->get_state_minus_stateZero().head<8>();delta_prior = (data->get_state() - data->getPriorZero()).head<8>();assert(data->frameID != -1);frameID = data->frameID; }void EFPoint::takeData() {priorF = data->hasDepthPrior ? setting_idepthFixPrior*SCALE_IDEPTH*SCALE_IDEPTH : 0;if(setting_solverMode & SOLVER_REMOVE_POSEPRIOR) priorF=0;deltaF = data->idepth-data->idepth_zero; }void EFResidual::fixLinearizationF(EnergyFunctional* ef1) {Vec8f dp = ef1->adHTdeltaF[hostIDX+ef1->nFrames*targetIDX];// compute Jp*delta__m128 Jp_delta_x = _mm_set1_ps(J->Jpdxi[0].dot(dp.head<6>())+J->Jpdc[0].dot(ef1->cDeltaF)+J->Jpdd[0]*point->deltaF);__m128 Jp_delta_y = _mm_set1_ps(J->Jpdxi[1].dot(dp.head<6>())+J->Jpdc[1].dot(ef1->cDeltaF)+J->Jpdd[1]*point->deltaF);__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*)&J->resF)+i);rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JIdx))+i),Jp_delta_x));rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JIdx+1))+i),Jp_delta_y));rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JabF))+i),delta_a));rtz = _mm_sub_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(J->JabF+1))+i),delta_b));_mm_store_ps(((float*)&res_toZeroF)+i, rtz);}isLinearized = true; }} /** * This file is part of DSO. * * Copyright 2016 Technical University of Munich and Intel. * Developed by Jakob Engel <engelj at in dot tum dot de>, * for more information see <http://vision.in.tum.de/dso>. * If you use this code, please cite the respective publications as * listed on the above website. * * DSO is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * DSO is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with DSO. If not, see <http://www.gnu.org/licenses/>. *//** KFBuffer.cpp** Created on: Jan 7, 2014* Author: engelj*/#include "FullSystem/FullSystem.h"#include "stdio.h" #include "util/globalFuncs.h" #include <Eigen/LU> #include <algorithm> #include "IOWrapper/ImageDisplay.h" #include "util/globalCalib.h"#include <Eigen/SVD> #include <Eigen/Eigenvalues> #include "FullSystem/ImmaturePoint.h" #include "math.h"namespace dso {//point是未成熟點，用優(yōu)化的方法算了次currentIdepth來更新未成熟點的 //一些深度相關的屬性，并算了下該點對應的殘差 PointHessian* FullSystem::optimizeImmaturePoint(ImmaturePoint* point, int minObs,ImmaturePointTemporaryResidual* residuals) { //外面創(chuàng)建了這么個結構體residuals 下面這段就開始對這個結構體賦值//未成熟點在非自身幀之外的幀投影的總個數(shù)int nres = 0; //遍歷關鍵幀窗口，每個未成熟點與其非所在的關鍵幀都有個殘差存在for(FrameHessian* fh : frameHessians){//點與非點所在幀計算殘差if(fh != point->host){residuals[nres].state_NewEnergy = residuals[nres].state_energy = 0;residuals[nres].state_NewState = ResState::OUTLIER;residuals[nres].state_state = ResState::IN;//非點所在幀residuals[nres].target = fh;nres++;}}assert(nres == ((int)frameHessians.size())-1);bool print = false;//rand()%50==0;float lastEnergy = 0;float lastHdd=0;float lastbd=0;float currentIdepth=(point->idepth_max + point->idepth_min) * 0.5f;//遍歷某個未成熟點計算一次殘差for(int i = 0; i < nres; i++){lastEnergy += point->linearizeResidual(&Hcalib, 1000, residuals + i,lastHdd, lastbd, currentIdepth);residuals[i].state_state = residuals[i].state_NewState;residuals[i].state_energy = residuals[i].state_NewEnergy;}if(!std::isfinite(lastEnergy) || lastHdd < setting_minIdepthH_act){if(print)printf("OptPoint: Not well-constrained (%d res, H=%.1f). E=%f. SKIP!\n",nres, lastHdd, lastEnergy);return 0;}if(print) printf("Activate point. %d residuals. H=%f. Initial Energy: %f. Initial Id=%f\n" ,nres, lastHdd,lastEnergy,currentIdepth);float lambda = 0.1;//高斯牛頓方法優(yōu)化只有一個變量就是idepthfor(int iteration = 0; iteration < setting_GNItsOnPointActivation; iteration++){float H = lastHdd;H *= 1 + lambda;//解方程算得逆深度增量為stepfloat step = (1.0 / H) * lastbd;float newIdepth = currentIdepth - step;float newHdd = 0; float newbd = 0; float newEnergy = 0;for(int i = 0; i < nres; i++){//計算殘差主函數(shù)newEnergy += point->linearizeResidual(&Hcalib, 1, residuals + i, newHdd, newbd, newIdepth);}if(!std::isfinite(lastEnergy) || newHdd < setting_minIdepthH_act){if(print) printf("OptPoint: Not well-constrained (%d res, H=%.1f). E=%f. SKIP!\n",nres,newHdd,lastEnergy);return 0;}if(print) printf("%s %d (L %.2f) %s: %f -> %f (idepth %f)!\n",(true || newEnergy < lastEnergy) ? "ACCEPT" : "REJECT",iteration,log10(lambda),"",lastEnergy, newEnergy, newIdepth);//新的優(yōu)化結果可接受更新state_state state_energyif(newEnergy < lastEnergy){currentIdepth = newIdepth;lastHdd = newHdd;lastbd = newbd;lastEnergy = newEnergy;for(int i=0;i<nres;i++){residuals[i].state_state = residuals[i].state_NewState;residuals[i].state_energy = residuals[i].state_NewEnergy;}lambda *= 0.5;}else{lambda *= 5;}if(fabsf(step) < 0.0001*currentIdepth)break;}if(!std::isfinite(currentIdepth)){printf("MAJOR ERROR! point idepth is nan after initialization (%f).\n", currentIdepth);return (PointHessian*)((long)(-1)); // yeah I'm like 99% sure this is OK on 32bit systems.}//符合條件的點太少了int numGoodRes = 0;for(int i=0;i<nres;i++)if(residuals[i].state_state == ResState::IN) numGoodRes++;if(numGoodRes < minObs){if(print) printf("OptPoint: OUTLIER!\n");return (PointHessian*)((long)(-1)); // yeah I'm like 99% sure this is OK on 32bit systems.}//更新成熟點的深度信息 //新建一個PointHessian點p。下面是對這個點內(nèi)容各種賦值 //包括逆深度點相對其他關鍵幀的殘差等等PointHessian* p = new PointHessian(point, &Hcalib);if(!std::isfinite(p->energyTH)) {delete p; return (PointHessian*)((long)(-1));}p->lastResiduals[0].first = 0;p->lastResiduals[0].second = ResState::OOB;p->lastResiduals[1].first = 0;p->lastResiduals[1].second = ResState::OOB;//currentIdepth是上面算出來的p->setIdepthZero(currentIdepth);p->setIdepth(currentIdepth);p->setPointStatus(PointHessian::ACTIVE); //初始化該點相對除自己所在幀的每幀的殘差數(shù)據(jù)rfor(int i = 0; i < nres; i++)if(residuals[i].state_state == ResState::IN){PointFrameResidual* r = new PointFrameResidual(p, p->host, residuals[i].target);r->state_NewEnergy = r->state_energy = 0;r->state_NewState = ResState::OUTLIER;//給屬性state_state賦值為ResState::INr->setState(ResState::IN);//p對應的殘差入隊p->residuals.push_back(r);if(r->target == frameHessians.back()){p->lastResiduals[0].first = r;p->lastResiduals[0].second = ResState::IN;}else if(r->target == (frameHessians.size()<2 ? 0 : frameHessians[frameHessians.size()-2])){p->lastResiduals[1].first = r;p->lastResiduals[1].second = ResState::IN;}}if(print) printf("point activated!\n");statistics_numActivatedPoints++;return p; }} /** * This file is part of DSO. * * Copyright 2016 Technical University of Munich and Intel. * Developed by Jakob Engel <engelj at in dot tum dot de>, * for more information see <http://vision.in.tum.de/dso>. * If you use this code, please cite the respective publications as * listed on the above website. * * DSO is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * DSO is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with DSO. If not, see <http://www.gnu.org/licenses/>. */#pragma once #define MAX_ACTIVE_FRAMES 100#include "util/globalCalib.h" #include "vector"#include <iostream> #include <fstream> #include "util/NumType.h" #include "FullSystem/Residuals.h" #include "util/ImageAndExposure.h"namespace dso {inline Vec2 affFromTo(const Vec2 &from, const Vec2 &to) // contains affine parameters as XtoWorld. {return Vec2(from[0] / to[0], (from[1] - to[1]) / to[0]); }struct FrameHessian; struct PointHessian;class ImmaturePoint; class FrameShell;class EFFrame; class EFPoint;#define SCALE_IDEPTH 1.0f // scales internal value to idepth. #define SCALE_XI_ROT 1.0f #define SCALE_XI_TRANS 0.5f #define SCALE_F 50.0f #define SCALE_C 50.0f #define SCALE_W 1.0f #define SCALE_A 10.0f #define SCALE_B 1000.0f#define SCALE_IDEPTH_INVERSE (1.0f / SCALE_IDEPTH) #define SCALE_XI_ROT_INVERSE (1.0f / SCALE_XI_ROT) #define SCALE_XI_TRANS_INVERSE (1.0f / SCALE_XI_TRANS) #define SCALE_F_INVERSE (1.0f / SCALE_F) #define SCALE_C_INVERSE (1.0f / SCALE_C) #define SCALE_W_INVERSE (1.0f / SCALE_W) #define SCALE_A_INVERSE (1.0f / SCALE_A) #define SCALE_B_INVERSE (1.0f / SCALE_B)struct FrameFramePrecalc {EIGEN_MAKE_ALIGNED_OPERATOR_NEW;// static valuesstatic int instanceCounter;FrameHessian* host; // defines row 定義行FrameHessian* target; // defines column定義列// precalc values預測值Mat33f PRE_RTll;//對應郭瑞瑞的Mat33f PRE_KRKiTll;Mat33f PRE_RKiTll;Mat33f PRE_RTll_0;Vec2f PRE_aff_mode;float PRE_b0_mode;Vec3f PRE_tTll;Vec3f PRE_KtTll;Vec3f PRE_tTll_0;float distanceLL;inline ~FrameFramePrecalc() {}inline FrameFramePrecalc() {host=target=0;}void set(FrameHessian* host, FrameHessian* target, CalibHessian* HCalib); };//關鍵幀單幀結構struct FrameHessian {EIGEN_MAKE_ALIGNED_OPERATOR_NEW;EFFrame* efFrame;//// constant info & pre-calculated values//DepthImageWrap* frame;FrameShell* shell; isOOB//跟蹤，精細跟蹤。用于方向選擇（不適用于梯度直方圖等）Eigen::Vector3f* dI; // trace, fine tracking. Used for direction select (not for gradient histograms etc.)Eigen::Vector3f* dIp[PYR_LEVELS]; // coarse tracking / coarse initializer. NAN in [0] only.float* absSquaredGrad[PYR_LEVELS]; // only used for pixel select (histograms etc.). no NAN.//關鍵幀歷史隊列的中幀的編號，int frameID; // incremental ID for keyframes only!static int instanceCounter;//關鍵幀窗口隊列中幀的編號int idx;// Photometric Calibration Stuff光度校準材料float frameEnergyTH; // set dynamically depending on tracking residual根據(jù)跟蹤殘差動態(tài)設置float ab_exposure;bool flaggedForMarginalization; //是所有活躍點的信息。所謂活躍點，是指它們在相機的視野中， //其殘差項仍在參與優(yōu)化部分的計算；就是成熟點std::vector<PointHessian*> pointHessians; // contains all ACTIVE points.包含所有ACTIVE點。//是已經(jīng)邊緣化的地圖點。std::vector<PointHessian*> pointHessiansMarginalized; // contains all MARGINALIZED points (= fully marginalized, usually because point went OOB.) //是被判為外點（outlier）的地圖點std::vector<PointHessian*> pointHessiansOut; // contains all OUTLIER points (= discarded.).包含所有OUTLIER點（=丟棄。）。 //為未成熟地圖點的信息std::vector<ImmaturePoint*> immaturePoints; // contains all OUTLIER points (= discarded.).Mat66 nullspaces_pose;Mat42 nullspaces_affine;Vec6 nullspaces_scale;// variable info.SE3 worldToCam_evalPT;//Vec10 state_zero;Vec10 state_scaled;//世界到相機的位姿變換+ 光度仿射變換 (前 8位的內(nèi)容)// [0-5: worldToCam-leftEps. 6-7: a,b]Vec10 state; Vec10 step;Vec10 step_backup;Vec10 state_backup;EIGEN_STRONG_INLINE const SE3 &get_worldToCam_evalPT() const {return worldToCam_evalPT;}EIGEN_STRONG_INLINE const Vec10 &get_state_zero() const {return state_zero;}EIGEN_STRONG_INLINE const Vec10 &get_state() const {return state;}EIGEN_STRONG_INLINE const Vec10 &get_state_scaled() const {return state_scaled;}EIGEN_STRONG_INLINE const Vec10 get_state_minus_stateZero() const {return get_state() - get_state_zero();}// precalc values 預測值SE3 PRE_worldToCam;SE3 PRE_camToWorld;std::vector<FrameFramePrecalc,Eigen::aligned_allocator<FrameFramePrecalc>> targetPrecalc;MinimalImageB3* debugImage;inline Vec6 w2c_leftEps() const {return get_state_scaled().head<6>();}inline AffLight aff_g2l() const {return AffLight(get_state_scaled()[6], get_state_scaled()[7]);}inline AffLight aff_g2l_0() const {return AffLight(get_state_zero()[6]*SCALE_A, get_state_zero()[7]*SCALE_B);}void setStateZero(const Vec10 &state_zero1);inline void setState(const Vec10 &state0){this->state = state0;state_scaled.segment<3>(0) = SCALE_XI_TRANS * state0.segment<3>(0);state_scaled.segment<3>(3) = SCALE_XI_ROT * state0.segment<3>(3);state_scaled[6] = SCALE_A * state0[6];state_scaled[7] = SCALE_B * state0[7];state_scaled[8] = SCALE_A * state0[8];state_scaled[9] = SCALE_B * state0[9];//w2c_leftEps返回的是state_scaled。PRE_worldToCam = SE3::exp(w2c_leftEps()) * get_worldToCam_evalPT();PRE_camToWorld = PRE_worldToCam.inverse();//setCurrentNullspace();};inline void setStateScaled(const Vec10 &state_scaled0){this->state_scaled = state_scaled0;state.segment<3>(0) = SCALE_XI_TRANS_INVERSE * state_scaled0.segment<3>(0);//0 1 2state.segment<3>(3) = SCALE_XI_ROT_INVERSE * state_scaled0.segment<3>(3);// 3 4 5state[6] = SCALE_A_INVERSE * state_scaled0[6];state[7] = SCALE_B_INVERSE * state_scaled0[7];state[8] = SCALE_A_INVERSE * state_scaled0[8];state[9] = SCALE_B_INVERSE * state_scaled0[9]; //PRE_worldToCam 預測的世界到相機的位姿變換//w2c_leftEps返回的是state_scaled的前6個數(shù) 而在上面state_scaled0對其賦值了，而//state_scaled0又恰好前6個是0啊。所以PRE_worldToCam就是沒變化啊 ------看代碼分析PRE_worldToCam = SE3::exp(w2c_leftEps()) * get_worldToCam_evalPT(); //世界到相機//下面那個就是預測的相機到世界的位姿變換了PRE_camToWorld = PRE_worldToCam.inverse();//相機到世界//setCurrentNullspace();};//* 設置當前位姿, 和狀態(tài)增量, 同時設置了FEJ點// 這個是優(yōu)化函數(shù)內(nèi)部用的真正改變狀態(tài)變量的地方inline void setEvalPT(const SE3 &worldToCam_evalPT0, const Vec10 &state0){this->worldToCam_evalPT = worldToCam_evalPT0;setState(state0);//給setStateZero賦值的地方setStateZero(state0);};//設置當前位姿, 光度仿射系數(shù) 這個是初始化用的在執(zhí)行一些操作之前賦值inline void setEvalPT_scaled(const SE3 &worldToCam_evalPT0, const AffLight &aff_g2l){Vec10 initial_state = Vec10::Zero();// 直接設置光度系數(shù)a和binitial_state[6] = aff_g2l.a;initial_state[7] = aff_g2l.b;//世界到相機的位姿變換this->worldToCam_evalPT = worldToCam_evalPT0;//修改過PRE_worldToCam和另一個的值//initial_state關于位姿的值為0setStateScaled(initial_state); //給setStateZero賦值的地方setStateZero(this->get_state());//this->get_state()就是返回state };void release();inline ~FrameHessian(){assert(efFrame==0);release(); instanceCounter--;for(int i=0;i<pyrLevelsUsed;i++){delete[] dIp[i];delete[] absSquaredGrad[i];}if(debugImage != 0) delete debugImage;};inline FrameHessian(){instanceCounter++;flaggedForMarginalization=false;frameID = -1;efFrame = 0;frameEnergyTH = 8*8*patternNum;debugImage=0;};void makeImages(float* color, CalibHessian* HCalib);inline Vec10 getPrior(){Vec10 p = Vec10::Zero();if(frameID==0){p.head<3>() = Vec3::Constant(setting_initialTransPrior);p.segment<3>(3) = Vec3::Constant(setting_initialRotPrior);if(setting_solverMode & SOLVER_REMOVE_POSEPRIOR) p.head<6>().setZero();p[6] = setting_initialAffAPrior;p[7] = setting_initialAffBPrior;}else{if(setting_affineOptModeA < 0)p[6] = setting_initialAffAPrior;elsep[6] = setting_affineOptModeA;if(setting_affineOptModeB < 0)p[7] = setting_initialAffBPrior;elsep[7] = setting_affineOptModeB;}p[8] = setting_initialAffAPrior;p[9] = setting_initialAffBPrior;return p;}inline Vec10 getPriorZero(){return Vec10::Zero();}};struct CalibHessian {EIGEN_MAKE_ALIGNED_OPERATOR_NEW;static int instanceCounter;VecC value_zero;VecC value_scaled;VecCf value_scaledf;VecCf value_scaledi;VecC value;VecC step;VecC step_backup;VecC value_backup;VecC value_minus_value_zero;inline ~CalibHessian() {instanceCounter--;}inline CalibHessian(){VecC initial_value = VecC::Zero();initial_value[0] = fxG[0];initial_value[1] = fyG[0];initial_value[2] = cxG[0];initial_value[3] = cyG[0];setValueScaled(initial_value);value_zero = value;value_minus_value_zero.setZero();instanceCounter++;for(int i=0;i<256;i++)Binv[i] = B[i] = i; // set gamma function to identity};// normal mode: use the optimized parameters everywhere!inline float& fxl() {return value_scaledf[0];}inline float& fyl() {return value_scaledf[1];}inline float& cxl() {return value_scaledf[2];}inline float& cyl() {return value_scaledf[3];}inline float& fxli() {return value_scaledi[0];}inline float& fyli() {return value_scaledi[1];}inline float& cxli() {return value_scaledi[2];}inline float& cyli() {return value_scaledi[3];}inline void setValue(const VecC &value){// [0-3: Kl, 4-7: Kr, 8-12: l2r]this->value = value;value_scaled[0] = SCALE_F * value[0];value_scaled[1] = SCALE_F * value[1];value_scaled[2] = SCALE_C * value[2];value_scaled[3] = SCALE_C * value[3];this->value_scaledf = this->value_scaled.cast<float>();this->value_scaledi[0] = 1.0f / this->value_scaledf[0];this->value_scaledi[1] = 1.0f / this->value_scaledf[1];this->value_scaledi[2] = - this->value_scaledf[2] / this->value_scaledf[0];this->value_scaledi[3] = - this->value_scaledf[3] / this->value_scaledf[1];this->value_minus_value_zero = this->value - this->value_zero;};inline void setValueScaled(const VecC &value_scaled){this->value_scaled = value_scaled;this->value_scaledf = this->value_scaled.cast<float>();value[0] = SCALE_F_INVERSE * value_scaled[0];value[1] = SCALE_F_INVERSE * value_scaled[1];value[2] = SCALE_C_INVERSE * value_scaled[2];value[3] = SCALE_C_INVERSE * value_scaled[3];this->value_minus_value_zero = this->value - this->value_zero;this->value_scaledi[0] = 1.0f / this->value_scaledf[0];this->value_scaledi[1] = 1.0f / this->value_scaledf[1];this->value_scaledi[2] = - this->value_scaledf[2] / this->value_scaledf[0];this->value_scaledi[3] = - this->value_scaledf[3] / this->value_scaledf[1];};float Binv[256];float B[256];EIGEN_STRONG_INLINE float getBGradOnly(float color){int c = color+0.5f;if(c<5) c=5;if(c>250) c=250;return B[c+1]-B[c];}EIGEN_STRONG_INLINE float getBInvGradOnly(float color){int c = color+0.5f;if(c<5) c=5;if(c>250) c=250;return Binv[c+1]-Binv[c];} };// hessian component associated with one point. //與一點相關的hession 分量 struct PointHessian {EIGEN_MAKE_ALIGNED_OPERATOR_NEW;static int instanceCounter;EFPoint* efPoint;// static valuesfloat color[MAX_RES_PER_POINT]; // colors in host framefloat weights[MAX_RES_PER_POINT]; // host-weights for respective residuals.float u,v;int idx;float energyTH;FrameHessian* host;//幀bool hasDepthPrior;float my_type;float idepth_scaled;//inverse depth of point.float idepth_zero_scaled;float idepth_zero;float idepth;//深度增量值吧應該是float step;float step_backup;float idepth_backup;float nullspaces_scale;float idepth_hessian;float maxRelBaseline;int numGoodResiduals;enum PtStatus {ACTIVE=0, INACTIVE, OUTLIER, OOB, MARGINALIZED};PtStatus status;inline void setPointStatus(PtStatus s) {status=s;}inline void setIdepth(float idepth) {this->idepth = idepth;this->idepth_scaled = SCALE_IDEPTH * idepth;}inline void setIdepthScaled(float idepth_scaled) {this->idepth = SCALE_IDEPTH_INVERSE * idepth_scaled;this->idepth_scaled = idepth_scaled;}inline void setIdepthZero(float idepth) {idepth_zero = idepth;idepth_zero_scaled = SCALE_IDEPTH * idepth;nullspaces_scale = -(idepth*1.001 - idepth/1.001)*500;} //僅包含良好的殘差（不是OOB與OUTLIER）。任意順序。std::vector<PointFrameResidual*> residuals; // only contains good residuals (not OOB and not OUTLIER). Arbitrary order. //包含有關最后兩個幀的殘差的信息。（[0] =最新，[1] =前一個）。std::pair<PointFrameResidual*, ResState> lastResiduals[2]; // contains information about residuals to the last two (!) frames. ([0] = latest, [1] = the one before).void release();PointHessian(const ImmaturePoint* const rawPoint, CalibHessian* Hcalib);inline ~PointHessian() {assert(efPoint==0); release(); instanceCounter--;}inline bool isOOB(const std::vector<FrameHessian*>& toKeep, const std::vector<FrameHessian*>& toMarg) const{int visInToMarg = 0;for(PointFrameResidual* r : residuals){if(r->state_state != ResState::IN) continue;for(FrameHessian* k : toMarg)if(r->target == k) visInToMarg++;}if((int)residuals.size() >= setting_minGoodActiveResForMarg &&numGoodResiduals > setting_minGoodResForMarg+10 &&(int)residuals.size()-visInToMarg < setting_minGoodActiveResForMarg)return true;if(lastResiduals[0].second == ResState::OOB) return true;if(residuals.size() < 2) return false;if(lastResiduals[0].second == ResState::OUTLIER && lastResiduals[1].second == ResState::OUTLIER) return true;return false;}inline bool isInlierNew(){return (int)residuals.size() >= setting_minGoodActiveResForMarg&& numGoodResiduals >= setting_minGoodResForMarg;}};} /** * This file is part of DSO. * * Copyright 2016 Technical University of Munich and Intel. * Developed by Jakob Engel <engelj at in dot tum dot de>, * for more information see <http://vision.in.tum.de/dso>. * If you use this code, please cite the respective publications as * listed on the above website. * * DSO is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * DSO is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with DSO. If not, see <http://www.gnu.org/licenses/>. */#pragma once#include "util/NumType.h"namespace dso { //下面這個結構體這些分量都在linearize函數(shù)中求得 struct RawResidualJacobian//原始殘余雅可比 {EIGEN_MAKE_ALIGNED_OPERATOR_NEW;// ================== new structure: save independently =============.VecNRf resF;//1個點對應的8個鄰域點的殘差值// the two rows of d[x,y]/d[xi].//位姿雅克比矩陣 ==像素坐標對相對位姿求偏導Vec6f Jpdxi[2]; // 2x6 // the two rows of d[x,y]/d[C].// ==像素坐標對相機相對內(nèi)參求導VecCf Jpdc[2]; // 2x4// the two rows of d[x,y]/d[idepth].//逆深度雅克比矩陣 ==像素坐標對host幀的逆深度求導Vec2f Jpdd; // 2x1// the two columns of d[r]/d[x,y]. ==殘差對target幀上的像素坐標求導 8個點VecNRf JIdx[2]; // 8x2// = the two columns of d[r] / d[ab] ==殘差對a21 b21求導就是對光度系數(shù)增量求導數(shù)8個點VecNRf JabF[2]; // 8x2// = JIdx^T * JIdx (inner product). Only as a shorthand. 寫的很清晰Mat22f JIdx2; // 2x2// = Jab^T * JIdx (inner product). Only as a shorthand.Mat22f JabJIdx; // 2x2// = Jab^T * Jab (inner product). Only as a shorthand.Mat22f Jab2; // 2x2}; }

總結

以上是生活随笔為你收集整理的DSO窗口优化部分代码注释加理论解析的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。

日韩av黄I国产麻豆传媒I国产91av视频在线观看I日韩一区二区三区在线看I美女国产在线I麻豆视频国产在线观看I成人黄色短片

编程问答

DSO窗口优化部分代码注释加理论解析

總結