SVO代码分析（一）

关于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">

</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终止信息。

这就是主程序了，几个处理 Frame 的函数见下篇（好像是上篇。。。。）的附。

Word 格式在拷贝的时候，有些会出现问题，所以格式有点乱，还好当时有编号，凑合着看吧亲……

posted @ 2018-03-20 10:25 花心葬空魂阅读(5120) 评论(0) 编辑收藏举报

刷新页面返回顶部

花心葬空魂