SVO代码分析（二）

附：

processFirstFrame();

processSecondFrame();

processFrame();

relocalizeFrame(SE3(Matrix3d::Identity(),Vector3d::Zero()),map_.getClosestKeyframe(last_frame_))

这4个函数被主函数文件vo_node.cpp中的addImage函数调用，他们与addImage一样都定义在frame_handler_mono.cpp中。

processFirstFrame()

首先看一下processFirstFrame()函数：

1. 新建一个变换矩阵SE3然后赋给new_frame_的T_f_w_（用于表示从世界坐标到相机坐标的变换矩阵）。

2. 然后判断函数klt_homography_init_.addFirstFrame(new_frame_)返回值是否为initialization::FAILURE，如果是，则结束processFirstFrame并返回RESULT_NO_KEYFRAME（意思是当前帧非关键帧）。其中klt_homography_init_是KltHomographyInit（FrameHandlerMono的友元类）类型的类，用于计算单应性矩阵（根据前两个关键帧）来初始化位姿。

       其中addFirstFrame(new_frame_)（定义在initialization.cpp中）工作如下：

       2.1 先执行reset()函数（定义在同文件中）：初始化px_cur_（一个二维点向量，存储当前帧中被用于跟踪的关键点坐标）和frame_ref_（一个frame型智能指针）。

2.2 执行detectFeatures函数，对new_frame进行Fast特征检测（调用FastDetector::detect函数），并将其关键点坐标集和对应的向量集（关键特征点对应的世界坐标系下的向量）分别赋给px_ref_和f_ref_。

       2.3 判断特征数是否小于100，如果是，就结束addFirstFrame并返回FAILURE。

       2.4 若上一步没执行return，则继续执行程序，将传入的new_frame_赋值给frame_ref_

       2.5 将px_ref_的值赋给px_cur_，并结束addFirstFrame返回SUCCESS。

3. 如果上一步未执行return，说明当前帧是关键帧，执行函数new_frame_->setKeyframe()将其设置为关键帧（即将is_keyframe_设置为true，并执行setKeyPoints()）。setKeyPoints函数中通过调用checkKeyPoints函数对当前图像中每个特征点进行遍历和比较，最终选出最具有代表性的5个作为关键点。实质上是1个靠近图像中心的点和4个靠近图像四个角的点（看这段代码时，犯了糊涂，竟然花了半小时时间各种胡乱操作，最后才发现没那么复杂，，，真的是智商下降555……）。

4. 将new_frame_设置为关键帧后，通过addKeyFrame函数把它存入keyframes_中。

5. stage_设置为STAGE_SECOND_FRAME

6. 将信息“Init: Selected first frame”记录至日志。

7. 结束processFirstFrame并返回RESULT_IS_KEYFRAME

processSecondFrame()

然后看processSecondFrame()函数：

1. 类似processFirstFrame函数，processSecondFrame在最开始调用klt_homography_init_子函数，不过这次调用的是addSecondFrame(new_frame_)。然后将返回值保存到InitResult型变量res中。

addSecondFrame(new_frame_)同样定义在initialization.cpp中，完成的工作如下：

1.1 首先调用trackKlt函数跟踪特征（LK光流法）。

1.1.1 创建cv::TermCriteria类型变量termcrit（用于设置迭代算法终止条件），通过重载构造函数实现初始化，其中cv::TermCriteria::COUNT表示达到最大迭代次数终止迭代，cv::TermCriteria::EPS表示达到精度后终止，两个加起来表示两个任意一个满足就终止。klt_max_iter前面设置为30，故最大迭代次数设置为30。klt_eps值为0.001，故精度设置为0.001。

1.1.2 调用cv::calcOpticalFlowPyrLK实现LK光流跟踪。各输入参数：

上一帧图像的金字塔frame_ref->img_pyr_[0]

当前帧图像金字塔frame_cur->img_pyr_[0]

被跟踪特征点（上帧）坐标集合px_ref

当前帧特征点坐标集合px_cur（看英文应该是当前帧的意思，但是既然知道了当前帧中特征的位置，还跟踪个鬼哦，跟踪不就是为了找到这个位置吗╮(╯▽╰)╭算了先往下看），另一方面px_cur用来存储光流法计算出的当前帧中特征点坐标。顺便提一下px_cur是三维向量（确切说应该是一个存储2维坐标的向量）：特征序号、特征坐标。

输出状态向量status，用于表示每个特征是否被找到，找到就置1。

输出错误向量error，用于表示每个特征的错误（距离误差）。

设置搜索窗口的大小为klt_win_size*klt_win_size。

金字塔总层数为4.

迭代终止条件为termcrit（前面设置的那个）

flag变量，用于设置光流跟踪方法。设置为cv::OPTFLOW_USE_INITIAL_FLOW，表示利用px_cur中存储的坐标信息进行初步估计。

                  1.1.3 通过for循环，剔除没有跟踪到的特征点。

1.1.4 将剩余特征点的像素坐标转换为世界坐标，这个通过frame类的函数c2f（调用cam2world函数）实现。然后再存入f_cur中。

                  1.1.5 将特征点移动的像素距离存入disparities_中。

1.2 判断跟踪到的特征点的数目是否小于设定的阈值，是的话就结束addSecondFrame函数，并返回FAILURE。

1.3 调用getMedian函数得到disparities_中的中位数作为平均距离，幅值给变量disparity。然后判断disparity，若值小于设定值，结束addSecondFrame函数，并返回NO_KEYFRAME。

1.4 调用computeHomography函数计算单应矩阵，输入参数为上帧特征点球坐标（这个球坐标应该指世界坐标系下单位向量）（f_ref_），当前帧特征点球坐标（f_cur_），相机参数（貌似是焦距），重投影阈值（小于该阈值则判断为内点），还多了几个输出参数为inliers_（存储内点下标），xyz_in_cur（存储3D点按估计位姿投影得到的像素坐标），T_cur_from_ref_ （存储从上帧到当前帧的位姿变化的变换矩阵）。

1.5 判断内点数量，若小于设定阈值，则结束addSecondFrame函数，并返回FAILURE。

1.6 调节地图大小以使地图平均深度等于指定比例：

首先从xyz_in_cur中提取3D点深度信息，然后选其中位数作为均值。然后计算当前帧的变换矩阵（可表示相机位姿的变化）。SE3中对乘法“*”进行了重载以实现矩阵乘法，所以T_cur_from_ref_ * frame_ref_->T_f_w_表示对后者进行前者形式的变换。

       T_f_w.rotation_matrix()和T_f_w.translation()分别返回SE3型变量的R和t。

       pos()是frame类成员函数（程序注释是返回帧的世界位置坐标），其实它的本质是返回T_f_w_的逆矩阵的平移参数t。

       命名已经通过T_cur_from_ref_和frame_ref_->T_f_w_得到frame_cur的旋转和平移参数了，为什么还要再重新计算frame_cur的平移参数呢？实际上是为了调整地图规模（scale）。进行一个简单（好吧，其实也挺复杂的）的推导就可以推出来，我们把T_cur_from_ref_.rotation_matrix()简称为R1，T_cur_from_ref_.translation()简称为t1，frame_ref_->T_f_w_.translation()简称为t2，那个复杂的计算式子最后结果相当于：

       frame_cur->T_f_w_.translation() =R1*t2 - scale*t1（应该没推错。。。）

1.7 创建欧式群矩阵T_world_cur，赋值为frame_cur->T_f_w_的逆矩阵

1.8通过for循环向两帧都添加内点和对应的特征：

       首先定义二维向量px_cur和px_ref用于存储内点特征点的像素坐标，px_cur_存储的是当前帧中各特征点的序号以及坐标，inliers_存储的是内点对应的下标。

       （查了半天也没查到Vector2d.cast函数，推测是进行强制数据类型转换）

       进行判断，若超出可视范围或者深度为负，则跳过，进行下一轮。

       将内点特征点坐标（相机坐标系）乘以scale后，得到其世界坐标系坐标，存入指针变量new_point。

       新建Feature型指针ftr_cur，并用new_point、px_cur、f_cur_等进行初始化，0指的是金字塔0层。然后调用Frame类成员函数addFeature，将ftr_cur添加进frame_cur的fts_（特征点列表）。然后调用Point类成员函数addFrameRef将ftr_cur添加进new_point的obs_（可以观测到此特征点的帧的指针的列表）。

       新建Feature型指针ftr_ref并进行同上操作。

1.9结束addSecondFrame函数，并返回SUCCESS。

 

2. 判断res的值：

       为FAILURE时，结束processSecondFrame()函数，并返回RESULT_FAILURE。

为NO_KEYFRAME时，结束processSecondFrame()函数，并返回RESULT_NO_KEYFRAME。

3. 条件编译，如果定义了USE_BUNDLE_ADJUSTMENT，就进行BA优化，通过调用ba::twoViewBA函数，这里就不展开了。

4. 执行函数new_frame_->setKeyframe()将其设置为关键帧（即将is_keyframe_设置为true，并执行setKeyPoints()）。setKeyPoints函数中通过调用checkKeyPoints函数对当前图像中每个特征点进行遍历和比较，最终选出最具有代表性的5个作为关键点。实质上是1个靠近图像中心的点和4个靠近图像四个角的点。

5. 通过函数getSceneDepth获取场景平均深度（depth_mean）最小深度（depth_min）。

6. 向深度滤波器depth_filter中添加关键帧（当前帧），传入参数depth_mean、0.5 * depth_min（不知道为啥除以2）进行初始化。

7. 向map_中添加当前关键帧。

8. 设置stage_为STAGE_DEFAULT_FRAME。

9. 调用klt_homography_init_.reset()，初始化px_cur_和frame_ref_。

10. 结束processSecondFrame()函数，返回RESULT_IS_KEYFRAME。

processFrame()

接下来是processFrame()函数：

1. 设置初始位姿，即将上帧（last_frame_）的变换矩阵（T_f_w_）赋给当前帧的变换矩阵（T_f_w_）。

2. 图像的稀疏对齐（应该是匹配的意思）：

       首先创建SparseImgAlign类型变量img_align，并利用构造函数进行初始化：图像金字塔最大层和最小层、迭代次数、采用高斯牛顿法、display_和verbose。

       调用run函数进行对齐，传入参数为上帧和当前帧指针：

       2.1 执行reset（）函数进行初始化。

       2.2 判断上帧（ref_frame）的特征数是否为0，若是则结束run函数，返回0。

       2.3 将ref_frame赋给ref_frame_，cur_frame赋给cur_frame_。

       2.4 创建cv::Mat对象（行数为ref_frame_->fts_.size(),，列数为patch_area_（16），数值类型为CV_32F）并赋给ref_patch_cache_。

       2.5 调用.resize函数对矩阵jacobian_cache_的大小进行初始化，行数不变（Eigen::NoChange表示不变），列数设置为ref_patch_cache_.rows * 16。

jacobian_cache_定义在sparse_img_align.h中，通过一下语句生成：

Matrix<double, 6, Dynamic, ColMajor> jacobian_cache_;

其中，数值类型为double，行数为6，Dynamic表示动态矩阵（其大小根据运算需要确定），ColMajor表示按列存储。

                  2.6 初始化向量visible_fts_，使其长度为ref_patch_cache_.rows，值均为false。

       2.7 创建SE3型变量T_cur_from_ref（用于存储从上帧到当前帧的变换矩阵），初始化值为当前帧变换矩阵 乘以 上帧变换矩阵逆矩阵。

       2.8  for循环实现对变换矩阵T_cur_from_ref的优化：

              level_为金字塔层数

设置mu_为0.1（作用是什么不知道）。

              jacobian_cache_初值均设为0。

              have_ref_patch_cache_设为false（作用不明）。

              如果verbose_为真，就打印优化信息。

       调用优化函数optimize（它进一步调用optimizeGaossNewton函数，optimizeGaossNewton又调用precomputeReferencePatches、computeResiduals等一系列函数，贼复杂！！不看了！！！），对T_cur_from_ref进行优化迭代。

2.9 得到优化后的当前帧变换矩阵，即用优化后的T_cur_from_ref乘以上帧的变换矩阵。

2.10 返回n_meas_/patch_area_（值为16），并幅值给img_align_n_tracked。

3. （没办法了 ￣へ￣ 项目逼的紧，代码不能一行行看了，所以后面的写的很简略）地图重投影和特征对齐（或许是匹配）。然后进行判断，如果匹配到的特征数小于阈值，则打印没有匹配到足够的特征信息，同时设置当前帧变换矩阵为上帧变换矩阵，设置tracking_quality为TRACKING_INSUFFICIENT，并返回RESULT_FAILURE。

4. 用高斯牛顿法进行优化（应该是特征点位置优化），判断sfba_n_edges_final，若小于20，则返回RESULT_FAILURE。

5. 结构优化（应该是位姿优化）。

6. 将当前帧插入core_kfs_（用于存储附近关键帧）。

7. 将跟踪质量设置为 sfba_n_edges_final

8.  判断tracking_quality_ ，若等于TRACKING_INSUFFICIENT，同时设置当前帧变换矩阵为上帧变换矩阵，并返回RESULT_FAILURE。

9. 获取场景最小和平均深度。根据平均深度判断是否符合关键帧选择标准，若不合适或者tracking_quality_ 值为 TRACKING_BAD，就将当前帧添加入深度滤波器，然后返回RESULT_NO_KEYFRAME。

10. 将当前帧设置为关键帧。

11. 将map_.point_candidates_中与当前帧相关的特征点添加到当前帧。

12. 条件编译，如果定义了USE_BUNDLE_ADJUSTMENT，则进行BA优化。

13. 将当前关键帧添加到深度滤波器。

14. 移除map_中距离较远的关键帧。

15. 添加当前关键帧到map_。

16. 返回RESULT_IS_KEYFRAME。

relocalizeFrame

最后是重定位函数：

relocalizeFrame(SE3(Matrix3d::Identity(),Vector3d::Zero()),map_.getClosestKeyframe(last_frame_))

1. 首先调用了map_.getClosestKeyframe函数查找最近的关键帧并赋给ref_keyframe。

2. 判断ref_keyframe值，若为nullptr，则结束并返回RESULT_FAILURE。

3. 调用img_align进行图像对齐。

4. 如果匹配特征数大于30，就将上,帧变换矩阵赋给T_f_w_last，设置last_frame为ref_keyframe，然后调用processFram()函数，返回值保存到res，然后执行下一步。

5. 判断res，若值不等于RESULT_FAILURE，就将stage_设置为STAGE_DEFAULT_FRAME，并打印 重定位成功 信息。否则，就将当前帧变换矩阵设置为T_f_w_last（即最近关键帧变换矩阵）。结束重定位函数，并返回res。

6. 如果匹配特征数小于等于30，就结束重定位函数，并返回RESULT_FAILURE。

 

SVO代码分析暂告一段落，哎，累啊~~~~