?

?关于SVO算法原理，不少前辈们的文章都介绍了，这里主要是分享一下自己看代码时记的一些笔记（仅个人理解，如有错误敬请指正 ^_^ ）

文件结构

先给出一个大致的文件列表，仅是部分主要文件。

rpg_svo

├── rqt_svo ? ? 为与显示界面有关的功能插件

├── svo ? ? ? ? ? 主程序文件，编译 svo_ros 时需要

│ ? ├── include

│ ? │ ? └── svo

│ ? │ ? ? ? ├── bundle_adjustment.h ? ? ? ? ? ? ?光束法平差（图优化）

│ ? │ ? ? ? ├── config.h ? ? ? ? ? ? ? ? ? ? ? ? SVO的全局配置

│ ? │ ? ? ? ├── depth_filter.h ? ? ? ? ? ? ? ? 像素深度估计（基于概率）

│ ? │ ? ? ? ├── feature_alignment.h ? ? ? ?特征匹配

│ ? │ ? ? ? ├── feature_detection.h ? ? ? ? 特征检测

│ ? │ ? ? ? ├── feature.h（无对应cpp） 特征定义

│ ? │ ? ? ? ├── frame.h ? ? ? ? ? ? ? ? ? ? ? ? frame定义

│ ? │ ? ? ? ├── frame_handler_base.h ? ? 视觉前端基础类

│ ? │ ? ? ? ├── frame_handler_mono.h ? 视觉前端原理

│ ? │ ? ? ? ├── global.h（无对应cpp） 有关全局的一些配置

│ ? │ ? ? ? ├── initialization.h ? ? ? ? ? ? ? ?初始化

│ ? │ ? ? ? ├── map.h ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? 地图的生成与管理

│ ? │ ? ? ? ├── matcher.h ? ? ? ? ? ? ? ? ? ? ? 重投影匹配与极线搜索

│ ? │ ? ? ? ├── point.h ? ? ? ? ? ? ? ? ? ? ? ? ?3D点的定义

│ ? │ ? ? ? ├── pose_optimizer.h ? ? ? ? ? ?图优化（光束法平差最优化重投影误差）

│ ? │ ? ? ? ├── reprojector.h ? ? ? ? ? ? ? ? ?重投影

│ ? │ ? ? ? └── sparse_img_align.h ? ? ? ? 直接法优化位姿（最小化光度误差）

│ ? ├── src

│ ? │ ? ├── bundle_adjustment.cpp

│ ? │ ? ├── config.cpp

│ ? │ ? ├── depth_filter.cpp

│ ? │ ? ├── feature_alignment.cpp

│ ? │ ? ├── feature_detection.cpp

│ ? │ ? ├── frame.cpp

│ ? │ ? ├── frame_handler_base.cpp

│ ? │ ? ├── frame_handler_mono.cpp

│ ? │ ? ├── initialization.cpp

│ ? │ ? ├── map.cpp

│ ? │ ? ├── matcher.cpp

│ ? │ ? ├── point.cpp

│ ? │ ? ├── pose_optimizer.cpp

│ ? │ ? ├── reprojector.cpp

│ ? │ ? └── sparse_img_align.cpp

├── svo_analysis ? ? 未知

├── svo_msgs ? ? ? ? 一些配置文件，编译 svo_ros 时需要

└── svo_ros ? ? ? ? ? ?为与ros有关的程序，包括 launch 文件

? ? ?├── CMakeLists.txt ? ? ? ? ? ? ? ? ? ? 定义ROS节点并指导rpg_svo的编译

├── include

│ ? └── svo_ros

│ ? ?└── visualizer.h ? ? ? ? ? ? ? ?

├── launch

│ ? └── test_rig3.launch ? ? ? ? ? ?ROS启动文件

├── package.xml

├── param ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? 摄像头等一些配置文件

├── rviz_config.rviz ? ? ? ? ? ? ? ? ? Rviz配置文件（启动Rviz时调用）

└── src

? ? ? ? ?├── benchmark_node.cpp

? ? ? ? ?├── visualizer.cpp ? ? ? ? ? ? ? ?地图可视化

? ? ? ? ?└── vo_node.cpp ? ? ? ? ? ? ? ? VO主节点

?rpg_svo/svo_ros/ CMakeLists.txt

交代了编译包 rpg_svo 所需要的编译工具、各种库等。

定义了ROS的节点的编译方式：

? ? ? ?ADD_EXECUTABLE(vo src/vo_node.cpp)

　　TARGET_LINK_LIBRARIES(vo svo_visualizer)

　　ADD_EXECUTABLE(benchmark src/benchmark_node.cpp)

　　TARGET_LINK_LIBRARIES(benchmark svo_visualizer)

? 指明了最后编译的两个可执行文件为 vo 和 benchmark，以及编译时所需要的库。其中节点vo的源码文件为src/vo_node.cpp。

? 其中svo_visualizer 是前面自定义的一个库：

? ? ? ? ? ?ADD_LIBRARY(svo_visualizer src/visualizer.cpp)

　　　TARGET_LINK_LIBRARIES(svo_visualizer ${LINK_LIBS})

? ? 而这里的LINK_LIBS 在前面代码中已经定义，具体可查看源代码。

?rpg_svo/svo_ros/ launch/ test_rig3.launch

?CMakeLists中定义了节点，而launch文件则描述了节点的启动方式：
? ? ? ? ? ? ? <node pkg="svo_ros" type="vo" name="svo" clear_params="true" output="screen">

? ? ? ? ? ? ?<param name="cam_topic" value="/camera/image_raw" type="str" />

? ? ? ? ? <rosparam file="$(find svo_ros)/param/camera_atan.yaml" />

? ? ? ? ? ? ? <rosparam file="$(find svo_ros)/param/vo_fast.yaml" />

? ? ? ? ? ?<param name="init_rx" value="3.14" />

? ? ? ? ? ? ? ?<param name="init_ry" value="0.00" />

? ? ? ? ? ? ? ?<param name="init_rz" value="0.00" />

? ? ? ? ? ?</node>

? ? ROS包的名称为"svo_ros"，要运行的节点为"vo"（恰是CMakeLists中定义的节点），name="svo"的意思是将节点名字改为svo（也就是说launch文件中可重新命名节点），后面配置了一些配置文件的路径和一些程序运行需要的参数的值。

rpg_svo/svo_ros/ src/vo_node.cpp ? ??

此为主节点程序代码

? ? 包括主程序main和类VoNode两部分。

?VoNode构造函数
其中类VoNode中的构造函数完成了多种功能：

? ? 1. 首先开辟了一个线程用于监听控制台输入（用到了boost库）：

? ? ? ? ? ?user_input_thread_ = boost::make_shared<vk::UserInputThread>();

? ? 2. 然后加载摄像机参数（svo文件夹），用到了vikit工具库

? ? 3. 初始化位姿，用到了Sophus、vikit

? ? ? ? ? ?其中，Sophus::SE3(R，t) 用于构建一个欧式群SE3，R，t为旋转矩阵和平移向量

? ? ? ? ? ?vk::rpy2dcm(const Vector3d &rpy) 可将欧拉角 rpy 转换为旋转矩阵

? ? 4. 初始化视觉前端VO（通过定义变量vo_以及svo::FrameHandlerMono构造函数完成）

? ? ? ? ? ?svo::FrameHandlerMono定义在frame_handler_mono.cpp中。

? ? ? ? ? ?4.1 FrameHandlerMono函数初始化时，先调用了FrameHandlerBase（定义在frame_handler_base.cpp中）

? ? ? ? ? ? ? ? ? FrameHandlerBase先完成了一些设置，如最近10帧中特征数量等。

? ? ? ? ? ? ? ? ? 然后初始化了一系列算法性能监视器

? ? ? ? ? ?4.2 然后进行重投影的初始化，由Reprojector（定义在reprojector.cpp中）构造函数完成（initializeGrid）：

? ? ? ? ? ? ? ? ? initializeGrid ，

注：ceil为取整函数

grid_ 为Grid类变量，Grid中定义了CandidateGrid型变量cells，而CandidateGrid是一个Candidate型的list（双向链表）组成的vector（向量）。

grid_.cells.resize是设置了cells（vector）的大小。即将图像划分成多少个格子。

然后通过for_each函数对cells每个链表（即图像每个格子）申请一块内存。

之后通过for函数给每个格子编号。

最后调用random_shuffle函数将格子的顺序打乱。

? ? ? ? ? ?4.3 通过DepthFilter（深度滤波器）构造函数完成初始化

设置了特征检测器指针、回调函数指针、随机种子、线程、新关键帧深度的初值。

? ? ? ? ? ?4.4 调用initialize初始化函数

? ? ? ? ? ? ? ? ? ? ? ? ?设置了特征检测器指针类型为fast特征检测器，

? ? ? ? ? ? ? ? ? ? ? ? ?设置了回调函数指针所指内容，即由bind函数生成的指针。

? ? ? ?bind函数将newCandidatePoint型变量point_candidates_（map_的成员变量）与输入参数绑定在一起构成函数指针depth_filter_cb。

? ? ? ?最终depth_filter_cb有两个输入参数，这两个参数被传入point_candidates进行计算。

? ? ? ?最后的最后，特征检测指针和回调函数指针在一起生成一个深度滤波器depth_filter_。

? ? ? ?深度滤波器启动线程。

? ? 5. 调用svo::FrameHandlerMono的start函数

? ? ? ? ? ?注：你可能在frame_handler_mono.cpp中找不到start函数，因为start原本是定义在frame_handler_base.h中，而FrameHandlerMono是继承自FrameHandlerBase，所以可以调用start函数。

? ? ? ? ? ?frame_handler_base.h中FrameHandlerBase::start()定义为：
? ? ? ? ? ? ? ? ? ? ?void start() { set_start_ = true; }

? ? ? ? ? ?所以通过start函数将set_start_置为true。

?

? 至此，VoNode的构造函数就介绍完了，知道了VoNode构造函数才能知道main函数中创建VoNode实例时发生了什么。
??
??
main()函数
下面是main函数，也就是主函数：

1. 首先调用ros::init完成了ros的初始化

2.创建节点句柄NodeHandle ，名为nh（创建节点前必须要有NodeHandle）

3.创建节点VoNode，名为vo_node，同时VoNode的构造函数完成一系列初始化操作。

4.订阅cam消息：

? ? ? ?先定义topic的名称；

? ? ? ?创建图片发布/订阅器，名为it，使用了之前创建的节点句柄nh；

? ? ? ?调用image_transport::ImageTransport中的subscribe函数：

? ? ? ? ? ? ? it.subscribe(cam_topic, 5, &svo::VoNode::imgCb, &vo_node)

? ? ? ?意思是，对于节点vo_node，一旦有图像（5代表队列长度，应该是5张图片）发布到主题cam_topic时，就执行svo::VoNode::imgCb函数。

然后it.subscribe返回值保存到image_transport::Subscriber型变量it_sub。

其中svo::VoNode::imgCb工作如下：

4.1 首先完成了图片的读取

? ? ? ?img = cv_bridge::toCvShare(msg, "mono8")->image

? ? ? ?这句将ROS数据转变为OpenCV中的图像数据。

4.2 执行processUserActions()函数（定义在同文件中），开辟控制台输入线程，并根据输入的字母进行相应的操作。

4.3 调用FrameHandlerMono：：addImage函数（定义在frame_handler_mono.cpp中）

? ? ? ?其中，msg->header.stamp.toSec()可获取系统时间（以秒为单位）

? ? ? ?获取的图片img和转换的系统时间被传入函数addImage，addImage过程：

? ? ? ?4.3.1 首先进行if判断，如果startFrameProcessingCommon返回false，则addImage结束，直接执行return。

? ? ? ? ? ? ? startFrameProcessingCommon函数过程：

? ? ? ?首先判断set_start_，值为true时，（在创建vo_node时就已通过VoNode构造函数将set_start_设置为true），然后执行resetAll（），resetAll函数定义在frame_handler_base.h中：virtual void resetAll(){resetCommon();}，且resetCommon()定义在同文件中，主要完成了Map型变量map_的初始化（包括关键帧和候选点的清空等），同时stage_被改为STAGE_PAUSED，set_reset和set_start都被设置为false，还有其它一些设置。执行resetAll后，设置stage_为STAGE_FIRST_FRAME。

? ? ? ?然后判断stage是否为STAGE_PAUSED，若是则结束startFrameProcessingCommon，并返回false。

? ? ? ?经过两个if后，将传进函数的系统时间和“New Frame”等信息记录至日志文件中。并启动vk::Timer型变量timer_（用于计量程序执行时间，可精确到毫秒级）。

? ? ? ?最后清空map_的垃圾箱trash，其实前面resetAll函数里已经完成这个功能了，不知道为什么还要再来一次。

? ? ? ?执行完这些后，startFrameProcessingCommon返回一个true。

4.3.2 清空core_kfs_和overlap_kfs_，这两个都是同种指针的集合。core_kfs_是Frame类型的智能指针shared_ptr，用于表示一帧周围的关键帧。overlap_kfs_是一个向量，存储的是由一个指针和一个数值构成的组合变量，用于表示具有重叠视野的关键帧，数值代表了重叠视野中的地标数。

4.3.3 创建新帧并记录至日志文件。新构造一个Frame对象，然后用Frame型智能指针变量new_frame指向它。.reset函数将智能指针原来所指对象销毁并指向新对象。

? ? ? ?创建Frame对象时，发生了一系列操作，通过其构造函数完成。

? ? ? ?Frame构造函数定义在frame.cpp中：
? ? ? ?首先完成了一些变量的初始化，如id、系统时间、相机模型、用于判两帧是否有重叠视野的特征数量、关键帧标志等。

? ? ? ?然后调用intFrame（img）函数。对传入的img创建图像金字塔img_pyr_（一个Mat型向量）。

? ? ? ? ? ? ? ? ? 4.3.4 处理帧。首先设置UpdateResult型枚举变量res，值为RESULT_FAILURE。

? ? ? ? ? ? ? ? ? ? ? ? ?然后判断stage_：

? ? ? ? ? ? ? ? ? ? ? ? ?值为STAGE_FIRST_FRAME，执行processFirstFrame()；

? ? ? ? ? ? ? ? ? ? ? ? ?值为STAGE_SECOND_FRAME，执行processSecondFrame()；

? ? ? ? ? ? ? ? ? ? ? ? ?值为STAGE_DEFAULT_FRAME，执行processFrame()；

? ? ? ? ? ? ? ? ? ? ? ? ?值为STAGE_RELOCALIZING，执行relocalizeFrame。

? ? ? ?其中processFirstFrame作用是处理第1帧并将其设置为关键帧；processSecondFrame作用是处理第1帧后面所有帧，直至找到一个新的关键帧；processFrame作用是处理两个关键帧之后的所有帧；relocalizeFrame作用是在相关位置重定位帧以提供关键帧（直译过来是这样，不太理解，看了后面的代码也许就知道了。。。。心疼自己~#@￥%&&我又回来了，看了后面后觉得它的作用是重定位）。由于这4个函数比较大，所以它们的解析放在最后附里面吧。

4.3.5 将new_frame_赋给last_frame_，然后将new_frame_给清空，供下次使用。

4.3.6 执行finishFrameProcessingCommon，传入的参数为last_frame_的id号和图像中的特征数nOb的值、res的值。

? ? ? ?finishFrameProcessingCommon工作如下：

? ? ? ?将last_frame_信息记录至日志。

? ? ? ?统计当前帧处理时间并压入acc_frame_timings_以捅进最近10帧的总处理时间。如果stage_值为STAGE_DEFAULT_FRAME，还将nOb传入的值压入acc_num_obs_以统计最近10帧的检测出的特征总数。

? ? ? ?然后是一个条件编译判断，如果定义了SVO_TRACE，就会调用PerformanceMonitor::writeToFile()（定义在performance_monitor.cpp中），writeToFile先调用trace()（定义在同文件中）将系统时间记录至文件（logs_是干什么的没看懂，也没注释）。然后用互斥锁对线程进行写保护，再将特征点数量记录至日志。

? ? ? ?将传入的参数res值赋给dropout，然后判断dropout：

值为RESULT_FAILURE，同时stage_为STAGE_DEFAULT_FRAME或stage_ == STAGE_RELOCALIZING，就执行：

stage_=STAGE_RELOCALIZING

tracking_quality_ = TRACKING_INSUFFICIENT

当只有dropout == RESULT_FAILURE时，就执行resetAll()：进行Map型变量map_的初始化（包括关键帧和候选点的清空等），同时stage_被改为STAGE_PAUSED，set_reset和set_start都被设置为false，还有其它一些设置。

? ? ? ? ? ? ? ? ? ? ? ? ?判断dropout后判断set_reset_，如果为真，同样执行resetAll()。

? ? ? ? ? ? ? ? ? ? ? ? ?最后返回0，结束finishFrameProcessingCommon。

? ? ? ? ? ? ? ? ? 4.3.7 结束addImag函数。

4.4 调用Visualizer类成员函数publishMinimal（进行ROS消息有关的设置）。

4.5 调用Visualizer类成员函数visualizeMarkers（里面又调用了publishTfTransform、publishCameraMarke、publishPointMarker、publishMapRegion等函数），进行Marker和关键帧的显示。

4.6调用Visualizer类成员函数exportToDense函数（稠密显示特征点）。

4.7 判断stage_，若值为STAGE_PAUSED，则将线程挂起100000微秒（0.1秒）。

4.8 结束imgCb函数。

? ? 5. 订阅远程输入消息（应该指的就是键盘输入）

? ? ? ? ? ?nh.subscribe("svo/remote_key", 5, &svo::VoNode::remoteKeyCb, &vo_node)

意思跟上面差不多，有消息发布到主题svo/remote_key（队列长度是5，如果输入6个数据，那么第6个就会被舍弃），就执行svo::VoNode::remoteKeyCb函数。

返回值保存到vo_node.sub_remote_key_。

? ? 6. 无图像信息输入或者键盘输入q，则停止程序，打印 SVO终止 信息。
? ??
? ?这就是主程序了。
? ?
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。

## processSecondFrame()
接下来是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。

cs