關於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終止 信息。
這就是主程序了，幾個處理 Frame 的函數見下篇（好像是上篇。。。。）的 附。
Word 格式在拷貝的時候，有些會出現問題，所以格式有點亂，還好當時有編號，湊合着看吧親……