SVO代码理解

SVO代码理解

基本流程

VO 对象创建

调用initialize(), 创建depth_filter线程，对应函数DepthFilter::updateSeedsLoop

FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp)

1. 程序入口传入图像数据及时间戳
2. FrameHandlerBase::startFrameProcessingCommon(), 根据当前状态进行相关操作, set_start 的情况下 进行重置resetAll()
3. 清空core_kfs(type: set< FramePtr>), overlao_kfs(type: vector< pair< FramePtr, size_t>>).
4. 通过img构造new_frame, 在Frame的构造函数中同时创建了影像金字塔
5. 根据当前的状态处理图像. 4种模式: default, first_frame, second_frame, relocalizing
6. 当前frame和lastFrame数据交换，delete new_frame, finishFrameProcessingCommon():根据状态决定时候reset或者relocalize

细节描述

初始化过程

First_frame

KltHomographyInit::addFirstFrame(), 通过detectFeatures()检测Fast特征，Fast特征均匀分布在图像中. 当前帧setKeyFrame(), 同时会设置key_pts,主要用于关键帧共视关系的判断, 并添加当前帧到map中(map->addKeyFrame())

Second_frame

KltHomographyInit::addSecondFrame(), 通过平均视差的大小判断是否为关键帧, 通过H矩阵(实际情况中)恢复R, T,计算出三维点坐标, 并由配置尺度和深度中值设定, feature由frame, px, f, point构造, curFrame & refFrame->addFeature(feature), point->addFrameRef(refFeature & curFeature), 将当前帧设置为关键帧, 并添加当前帧到map中，DepthFilter->addKeyframe(frame, depth_mean, 0.5 * depth)

一般过程

processFrame()

1. 设置当前初始Pose为上一帧的Pose
2. sparse image align, 通过vk::NLLSSolver::optimize()， 优化出当前帧的Pose
3. Reprojector::reprojectMap(frame, overlap_kfs)
   
   1. resetGrid()，新的frame需要将之前的grid清空
   2. map->getCloseKeyFrames()得到close_kfs(type: list< pair< FramePtr, double>>), 主要过程是遍历map中所有keyframe, 通过keyframe中的5个feature判断其是否在当前帧中. 得到pair< 满足条件的keyframe, keyframe到当前帧的距离>
   3. 对close_kfs通过距离值进行排序, 遍历close_kfs，对其feature->point 进行投影, 如果在frame中， 对Reprojector中的grid 进行统计,对应的grid.cells push_back(Candidate(point, px)), 得到overlap_kfs(type: vector< pair< FramePtr, size_t>>)
   4. 对map的point_candidates.candidates进行投影, 如果candidates未投影到frame上超过30次，在map中删除该点，
   5. reprojectCell(), 对grid->cells中的point 进行投影，如果点的类型为DELETED，在cell中删除该点， matcher.findMatchDirect()得到point在frame中的投影点(主要过程为Point::getCloseViewObs()得到与当前frame共视关系最好的feature)，frame->addFeature(feature). 在cell中删除该点，此处只挑选一次(cell中的点已经随机分布)
4. 根据reprojectMap()得到的matches数量进行当前状态的判断是否失败, 进行优化optimizeGaussNewton(), optimizeStructure()
5. needNewKf()进行关键帧判断,DepthFilter->addFrame(), 如果不是关键帧return
6. 当前帧setKeyFrame(), frame对应feature->addFrameRef(frame)
7. map.point_candidate.addCandidatePointToFrame(frame), 将PointCandidateList中的feature对应frame为当前帧的特征添加到当前帧
8. 可选择的使用g2o BA优化, DepthFilter->addKeyframe()
9. 当地图中关键帧的数量大于配置的关键帧数量时，删除距离当前frame最远的keyframe, map.addKeyframe()

后端过程

后端过程主要是在DepthFilter中完成(DepthFilter::updateSeedsLoop()), 前端传入数据的主要方式是DepthFilter::addKeyframe(frame), DepthFilter::addFrame()
1. addFrame()
主要是将frame 放入到frame_queue队列中, 当有新的frame进入时会notify updateSeedLoop线程开始处理frame数据
2. addKeyframe()
获得new_keyframe_min_depth, new_keyframe_mean_depth，new_keyframe
3. updateSeedsLoop()
1. 当传入为keyframe时, frame = new_keyframe, 并且清空frame_queue, 否则 frame = frame_queue.front(). updateSeeds(frame)
2. updateSeeds(frame)主要通过seeds点初始的u_depth…等通过极线范围内的匹配方式得到对应当前frame中匹配点, 更新seed的初始值, 当前对应网格的值设置为true,防止在frame detectFeature时重复提取区域内的fast特征点, 理论上这里frame应该addFeature(), 原始代码中注释了这一块，说是不能保证线程安全
3. 如果满足设定条件将其放入map.candidatelist中, 并从seed中删除, 此时已经计算出新的深度值
4. initializeSeeds(frame)
如果frame为关键帧，更新seeds
1. 检查frame中已经存在的feature, 将对应网格设置为ture, 开始提取fast特征
2. 将新检测出的特征点放入seeds中

重定位过程

FrameHandlerMono::relocalizeFrame(…map.getClosestKeyframe)
1. 找到last_frame可视范围内最近的关键帧
2. 开始一般过程