双目DSO代码---仅做留存

部分内容来自:https://blog.csdn.net/huang826144283/article/details/78880675

DSO可执行文件参数传入main_dso_pangolin.cpp,流程如下

参数入口:int main( intargc, char** argv )

通过参数文件,获取图像、内参和光度相机标定模型中的非线性响应函数用到的参数gammaCalib 及涉及光晕的参数:vignette :new ImageFolderReader(source,calib, gammaCalib, vignette) (跑kitti的灰色图像就没有后两项了)

ps: mode=1代表不进行光度标定; preset=0 代表原速度

 main_dso_pangolin.cpp有一个变量:float playbackSpeed=0;    

// 0 for linearize (play as fast as possible, while sequentializing tracking & mapping). otherwise, factor on timestamps.

根据读入的内参计算每层图像金字塔对应的内参(共6层):setGlobalCalibration

ps : 我是输入1391*341 输出640 320 pyramid0到3

扫描二维码关注公众号,回复: 2661180 查看本文章

模式检验:共有3种模式,分别对应PHOTOMETRIC MODE WITH CALIBRATION、PHOTOMETRIC MODE WITHOUT CALIBRATION、PHOTOMETRIC MODE WITH PERFECT IMAGES,用户根据自身提供的数据的类型进行模式设定

倒叙检验:对图像帧反序存储时的操作?(被我删去了)

还有build system

FullSystem* fullSystem = new FullSystem();

Pangolin显示

ps: 这里代码的并行性我有点搞不清楚.感觉明明应该一直困在下面这个循环里了(pangolin显示):

if(!disableAllDisplay)
    {
        viewer = new IOWrap::PangolinDSOViewer(wG[0],hG[0], false);
        fullSystem->outputWrapper.push_back(viewer);
    }

但是getImage包括下面几个步骤都是在std::thread runthread([&]() {内容}) 而且看起来下面那个应该是主循环 应该是pangolin先走了一个循环吧

我要格外关注thread

读图reader->getImage

视频播放速度控制,判断是否跳帧处理

帧处理入口:addActiveFrame(img, i);

 

初始化失败或重置后的操作

Lost后的操作

ps:FullSystem.h里面很多函数的参数都从const Vec3 &res被改成了Vec3 res这样的(虽然不知道用意何在)

五个互斥锁及其用法:

// =================== changed by tracker-thread. protected by trackMutex ============
1) boost::mutex trackMutex;   
   std::vector<FrameShell*> allFrameHistory;
   CoarseInitializer* coarseInitializer;

// ================== changed by mapper-thread. protected by mapMutex ===============
2)  boost::mutex mapMutex;   
   std::vector<FrameShell*> allKeyFramesHistory;

// mutex etc. for tracker exchange.
3) boost::mutex coarseTrackerSwapMutex;     // if tracker sees that there is a new reference, tracker locks [coarseTrackerSwapMutex] and swaps the two.
  CoarseTracker* coarseTracker_forNewKF;            // set as as reference. protected by [coarseTrackerSwapMutex].
    CoarseTracker* coarseTracker;                    // always used to track new frames. protected by [trackMutex].

// mutex for camToWorl's in shells (these are always in a good configuration).
4)	boost::mutex shellPoseMutex;

// tracking / mapping synchronization. All protected by [trackMapSyncMutex].
5)	boost::mutex trackMapSyncMutex;
	boost::condition_variable trackedFrameSignal;
	boost::condition_variable mappedFrameSignal;

帧处理跳转至FullSystem.cpp,入口是void FullSystem::addActiveFrame( ImageAndExposure* image, int id ),流程如下

 

根据传入帧构建fh与shell,之后图像的所有信息都将放在这两个对象里

ps: FrameHessian* fh = new FrameHessian();   FrameHessian* fh_right = new FrameHessian();
    FrameShell* shell = new FrameShell();

    shell->camToWorld = SE3();         // no lock required, as fh is not used anywhere yet.
    shell->aff_g2l = AffLight(0,0);  shell->marginalizedAt = shell->id = allFrameHistory.size();  shell->timestamp = image->timestamp;
    shell->incoming_id = id; // id passed into DSO
    fh->shell = shell;  fh_right->shell=shell;
    allFrameHistory.push_back(shell);

为各层金字塔图像计算梯度makeImages(image->image, &Hcalib):梯度为当前像素上下光强度差方与左右光强差方之和。先计算第0层的梯度,其他层的像素光强通过上一层均值采样得到。

初始化操作(改为双目之后 变化了很多)

 

前端操作trackNewCoarse(fh) 

ps:return Vec4(achievedRes[0], flowVecs[0], flowVecs[1], flowVecs[2]); 

关键帧判断:提供两类关键帧选择策略:每秒取固定帧数或加权论文中提到的三种情况(视野变化、有遮挡产生或有遮挡消失、曝光时间变化显著)判断是否创建新关键帧

将帧传入后端deliverTrackedFrame(fh, needToMakeKF)

 

初始化操作大致可分为两个阶段,第一个阶段是第一帧的处理,第二阶段是后续帧的跟踪,跟踪的帧数为5帧,5帧后初始化完毕,流程如下(ps:改为双目后步骤也变了很多)

在第一帧数据中取点:setFirst(&Hcalib, fh):每层采集密度不一样,取点策略:先将图像划分成32*32的块,计算每块的梯度均值作为选点的阈值makeHists(fh),选点时,第一次选择先把图像分成d*d的块,然后选择梯度最大且大于阈值的点,如果第一次选择结束后选择的点数目未达要求,则分成2d*2d的块,以此类推。

对于除第一帧外的后帧,trackFrame(fh, outputWrapper),直接法(two frame direct image alignment)只利用第一帧与当前帧的数据,用高斯牛顿方法基于最小化光测误差,求解或优化参数,优化之前,变换矩阵初始化为单位阵、点的逆深度初始化为1,在这个过程中,优化的初值都是没有实际意义的,优化的结果也是很不准确的

跟踪了5帧后,调用initializeFromInitializer(fh),把第一帧设置为关键帧,并把该关键帧相关的信息存储起来,其中关键帧存储的点下采样为2000个活动点

调用deliverTrackedFrame(fh, needToMakeKF),将第五帧传入后端,存为关键帧

改变后的步骤:

coarseInitializer->setFirstStereo(&Hcalib, fh,fh_right); 直接利用双目初始化就好了 很简单 具体的之后再看
initialized=true; 直接改变变量了

前端操作trackNewCoarse(fh):此跟踪过程即论文提到的帧跟踪,跟踪针对于完全BA后的最新关键帧而言,流程如下

 

初始化 affine brightness transfer function中的待优化参数ai,bi 为0

获取倒数第二个frame的shell:slast,倒数第三个frame的shell:sprelast

初始化位姿:对于历史关键帧只有两帧的情况,初始化当前帧到追踪关键帧之间的transformation为单位阵lastF_2_fh_tries.push_back(SE3())(假设0移动);对于其他情况,假设当前帧到倒数第一帧的t约(等于/double/half/0)倒数第二帧到倒数第三帧的t(fh_2_slast = slast_2_sprelast),再加上追踪关键帧与倒数第二帧的变换已在之前求出,所以当前帧到追踪关键帧之间的transformation (lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast))

计算26种不同角度rotation与之前计算transformation的组合得到26种转换矩阵(与论文不一样?论文中称只有在跟踪失败时才会使用27种rotation进行暴力重新初始化,且只在最粗的金字塔层进行)

ps: 这后面太复杂了 我必须先看很多类的函数再来看这个

遍历26种变换矩阵及4种纯移动变换矩阵,依次以这些为初值,调用trackNewestCoarse,用直接法(two frame direct image alignment),只利用追踪关键帧与当前帧的数据,用高斯牛顿方法基于最小化光测误差,优化求解各参数

ps: 未改动版本的函数:

Vec4 trackNewCoarse(FrameHessian* fh);
   void traceNewCoarse(FrameHessian* fh);

改动之后:

 void traceNewCoarseNonKey(FrameHessian* fh, FrameHessian* fh_right); (refine地图,开头lock了mapMutex,深度可能在这里)
    Vec4 trackNewCoarse(FrameHessian* fh,FrameHessian* fh_right); (前端)
    void traceNewCoarseKey(FrameHessian* fh,FrameHessian* fh_right);(开头也lock了mapMutex 备注是process KeyFrame,还不知道干嘛的)

deliverTrackedFrame是前端与后端的桥梁,它将已经标记为非关键帧或关键帧的帧传入后端进行处理,对于非关键帧,调用makeNonKeyFrame(fh):用traceNewCoarse(NonKey)进行点跟踪,然后删除该帧;对于关键帧,调用makeKeyFrame(fh):同样先用traceNewCoarse(Key)进行点跟踪,然后调用flagFramesForMarginalization边际化,最后把新关键帧的Hessian矩阵、残差加入到优化中,激活一些新的点,进行窗内BA优化,还调用了makeNewTraces()。traceNewCoarse的流程如下:

遍历所有活动帧(关键帧frameHessians),当前遍历记为host:为了使用逆深度,每个PointHessian必须使用一个主导帧(host Frame),说明这个点是由该帧反投影得到的

计算host与当前帧(target)之前的位姿关系hostToNew,及KRKi、Kt

遍历host帧中的未成熟地图点immaturePoints:在单目slam中,所有地图点在一开始被观测到时,都只有一个二维的像素坐标,其深度未知,随着相机移动,DSO会在每张图像上追踪这些未成熟的地图点

在极线上追踪未成熟点Trace:对于host帧中的一个像素点, ,该点的逆深度为 ,那么该点在target帧中的投影为: ,其中,  即为代码中的KRKi, 即为代码中的Kt

以投影点位置判断投影点的状态(traced well and good/not traced because of bad condition/end tracking & marginalize/energy too high: if happens again: outlier/traced well and good (but not actually traced)/ not even traced once),在极线上做离散搜索,用高斯牛顿迭代优化匹配关系,每次迭代会得到新的深度区间

猜你喜欢

转载自blog.csdn.net/zhengbq_seu/article/details/81118190
DSO