附：
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代碼分析暫告一段落，哎，累啊~~~~