这篇博客详细解析了DSO（Direct Sparse Odometry）中的窗口优化流程，包括残差队列的处理、线性化计算、求解增量方程以及状态更新等步骤。重点介绍了linearizeAll函数、applyRes_Reductor函数和solveSystem函数的作用，并结合理论知识进行了解释。文章引用了林突破和途金戈的博客作为理论依据。
总纲：
float FullSystem::optimize(int mnumOptIts)
{
0.先清空活动点残差队列
1.将所有外面旧成熟点相对新帧计算的残差和新成熟点相对非所在关键帧的残差加入点的残差队列
2.调用linearizeAll()函数计算相关导数。
调用linearizeAll()
{
2.1 linearizeAll_Reductor()
{
linearize()
{
具体活在这里干。完成的功能有：给点的state_NewState属性赋值，计算点的各种雅克比行列式
所需偏导，这些偏导是组成最终方程矩阵的基本元素。函数的返回结果是此点的残差值。
}
}
2.2 调用一次setNewFrameEnergyTH修改什么阈值。
2.3 剔除判为外点的残差。
}
3.调用applyRes_Reductor函数，这个函数对好点做一次中间量的计算，输入数据可看成是linearize返回的偏导值，
所谓输出的中间量就是例如：残差对位姿的偏导的转置* 残差对逆深度的偏导等这类的东西。
最后会在solveSystem函数里用到这些中间变量，其实所谓中间变量就是雅克比矩阵的一些组成元素。
4.开始迭代优化
for()
{
4.1 保存当前的状态，这是为了在优化结果不好的情况下可以回退,backupState函数实现。
4.2 求解优化增量方程 解出的结果有68包含的各项的小增量，所有点深度值的小增量。都
是绝对增量值切记切记，solveSystem函数实现
{
4.2.1 getNullspaces 零空间申请
4.2.2 solveSystemF 真正的解方程的位置
{
4.2.2.1 计算舒尔消元方程的那些系数 用到2.1步骤中计算的值以及3步骤计算的值。然后解出第一个方程
关于位姿和仿射以及相机参数的方程的解。获得68 个增量值（8帧的关键帧窗口的话）
4.2.2.2 根据上面的解计算解出逆深度方程的解。 获得逆深度值们的增量
4.2.2.3 零空间做一次修正
}
}
4.3 对计算的增量进一步限制次数
4.4 doStepFromBackup()
{
4.4.1 用增量更新关键帧窗口中的各项状态值，如位姿，仿射，相机参数，点的逆深度等。输入为4.2.2.1 4.2.2.2这两步结果
4.4.2 setPrecalcValues 将两帧之间的优化前后状态值（不包括点的逆深度值）都存储一份。
然后计算一下状态值的增量值（包括逆深度值）
}
4.5 用上面计算得到的新状态值计算一次新的残差以及偏导数等，在linearizeAll中实现。
4.6 如果新残差值小于原来残差值，那么就再调用一次调用applyRes_Reductor函数，接着调整lambda值，
再返回到4.1，接着顺序执行。否则就将状态结果回退，升高lambda值返回4.1顺序执行。
4.7 满足迭代跳出条件后结束优化。
}
5.改变一次最后一帧的位姿仿射是干啥。
6.调用setAdjointsF函数，这个函数是计算一些中间变量，为将相对值转为绝对值做准备。
7.调一次setPrecalcValues函数，再调一次linearizeAll函数，此时调linearizeAll是删除一些不合理点带着的残差项
下次再做窗口优化的时候就不再用它们了。
}
理论部分主要是林突破和途金戈的博客。也贴个图吧
途金戈的
/**
* 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)
{
//循环所有残差项
for(int k = min; k < max; k++)
{
PointFrameResidual* r = activeResiduals[k];
//在这里计算偏导数 填充雅克比矩阵用包括几何雅克比图像雅克比和
//逆深度雅克比 ,还对点的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.7
assert(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;
}
//相关导数计算，并且计算出残差值来。
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;
//雅克比矩阵元素:偏导数。还有点属性都给赋个值
linearizeAll_Reductor(fixLinearization, toRemove, 0, activeResiduals.size(),&stats,0);
lastEnergyP = stats[0];
}
//修改阈值frameHessians.back()->frameEnergyTH的值就是修改 target帧的frameEnergyTH值
setNewFrameEnergyTH();
//这段是剔除优化后为外点的残差
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++)
{
//循环某个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.将步骤应用于线性化点。
//优化结果生效应该是修改各个待优化参数值
//并通过评估本次增量的大小来判断是否结束迭代
//待优化参数包括N个逆深度 m*8+4的相机位姿 仿射 相机参数
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);
//循环关键帧窗口
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
{
//相机参数增量*权重 stepfacC 修改相机参数1 * 4
//修改相机参数
Hcalib.setValue(Hcalib.value_backup + stepfacC*Hcalib.step);
//循环关键帧窗口
for(FrameHessian* fh : frameHessians)
{
// cwiseProduct 函数功能 R = P .* Q 对应点相乘
//修改位姿仿射参数等 原有值+ 增量
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();
//循环点修改逆深度值
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.
//保留状态有点逆深度idepth 相机参数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;
//循环关键帧窗口
for(FrameHessian* fh : frameHessians)
{
fh->step_backup.setZero();
fh->state_backup = fh->get_state();
//循环成熟点
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.获取统计数据和活动残差。
//清空活动点残差队列
activeResiduals.clear();
int numPoints = 0;//没用
int numLRes = 0;//没用
int galdg = 0;
//循环窗口内的帧
for(FrameHessian* fh : frameHessians)
for(PointHessian* ph : fh->pointHessians)//循环成熟点
{
//循环每个点在不同帧投影算的残差
for(PointFrameResidual* r : ph->residuals)
{
//在外面旧成熟点相对新帧计算新残差 isLinearized为false
//在外面新激活成熟点相对非所在帧算新残差 isLinearized为false
//这些残差点是累积下来的
if(!r->efResidual->isLinearized)
{
//加入活动点残差队列
activeResiduals.push_back(r);
//设置了几个值
r->resetOOB();//
galdg ++;
}
else
numLRes++;
}
//点总数
numPoints++;
}
//相关导数计算
Vec3 lastEnergy = linearizeAll(false);
//这个函数返回值设置直接为0了
double lastEnergyL = calcLEnergy();
//这个函数返回值设置直接为0了
double lastEnergyM = calcMEnergy();
//在所有的 residual 中找到 isLinearized 为 false 的 residual，调用
//PointFrameResidual::applyRes(true)，设置它们的 PointFrameResidual::ResState
//（按照 FullSystem::optimizeImmaturePoint 的结果），如果是正常的
//点(ResState::IN)调用EFResidual::takeDataF()把 EFResidual::JpJdF 设置一下。
//这就算完成了优化的准备工作。还计算了每个残差的
//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.1
float stepsize = 1;
VecX previousX = VecX::Constant(CPARS + 8 * frameHessians.size(), NAN);
//开始迭代优化
for(int iteration = 0; iteration < mnumOptIts; iteration++)
{
// solve!
//保存当前的状态，这是为了在优化结果不好的情况下可以回退
backupState(iteration!=0);
//solveSystemNew(0);
//求解优化增量方程 解出的结果有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;
}
//优化结果生效应该是修改各个待优化参数值
//并通过评估本次增量的大小来判断是否结束迭代
//待优化参数包括M个点的逆深度 m * 8 + 4的相机位姿 仿射 相机参数
bool canbreak = doStepFromBackup(stepsize, stepsize, stepsize, stepsize, stepsize);
// eval new energy!评估新能量
//用上面修改的新数据计算新的能量值newEnergy，并且重新计算一组
//相关导数
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();
//在跳出循环体之后调用一次 FullSystem::linearizeAll(true)，
//效果是将优化之后成为 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();
}
//最终在这要更新关键帧的位姿和仿射参数 。用优化后的值更新。
{
boost::unique_lock<boost::mutex> crlock(shellPoseMutex);
//循环关键帧窗口
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);
//解方程的主函数
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(targe
最低0.47元/天 解锁文章
扫一扫
1500
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
