SVO ros 節點解讀
測試執行
- 使用作者給出的bag文件，運行以下指令
rosbag play airground_rig_s3_2013-03-18_21-38-48.bag這樣會在ros環境中廣播/camera/image_raw 圖像message, svo節點重映射該message到cam_topic，並訂閱該topoic。運行該節點程序
roslaunch svo_ros test_rig3.launch
可視化輸出結果
rosrun rviz rviz -d src/rpg_svo/svo_ros/rviz_config.rviz
svo節點的輸出節點通過一個tf變換和2個marker，這樣可以可視化在rviz中。2個marker其中一個是point顯示的相機的位置（svo/points），另外一個是（表示相機位姿的五面體（/svo/keyframe））
如圖所示爲svo節點廣播的topic，其中svo/points和/svo/keyframe均爲/visualization_msgs/marker類型的message.
運行結果
我使用同一段視頻跑了2遍，發現所走的軌跡基本是完全一樣的，從圖中可以看出所用的背景很複雜，很有利於提取角點，有很多distinctive feature points。
ros node init
- 創建ROS句柄
- 訂閱camera topic（svo/cam_topic）同時指定camera topic的回調函數
- 指定廣播的topics（可視化marker）
- 訂閱remote key用戶輸入，指定回調函數
VO的構造函數
vo節點的成員中包含FrameHandlerMono成員，該類完成svo的核心功能，其中包括image sparse alignment, feature alignment, pose and Structure Optimization
另一個是visualizer,負責輸出message至rviz可視化顯示計算結果，可以借鑑。廣播的信息如下
pub_frames_ = pnh_.advertise<visualization_msgs::Marker>("keyframes", 10);
pub_points_ = pnh_.advertise<visualization_msgs::Marker>("points", 1000);
pub_pose_ = pnh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose",10);初始化（設置）marker的代碼位於vikit中。
parameter設置，首先讀取相機的內參數，使用分別讀取了參數
cam_model: Pinhole
cam_width: 752
cam_height: 480
cam_fx: 414.536145
cam_fy: 414.284429
cam_cx: 348.804988
cam_cy: 240.076451
cam_d0: -0.283076
cam_d1: 0.066674
cam_d2: 0.000896
cam_d3: 0.000778讀取方法是
getParam<int>(ns+"/cam_width"),其中ns爲參數的外層命名空間（svo）。
其次是讀取相機的初始位姿。參數格式如下
<param name="init_rx" value="3.14" />
<param name="init_ry" value="0.00" />
<param name="init_rz" value="0.00" />事實上，並不需要在launch文件中難過給出全部6D數據，因爲如果從參數服務器中讀不到的時候會返回一個默認值0.0.
圖像的回調函數
ROS中首先sensor圖像數據類型轉化爲opencv Mat類型，其次是處理用戶輸入，然後在vo中增加當前傳入的圖像（該該函數中分階段處理圖像（首幀，前2幀，和普通幀，重定位幀）），之後就是發佈可視化message。
具體的流程如下
相關類簡介
類 FrameHandlerMono(VO entity)
| 函數 | 輸入 | 輸出 |
|---|---|---|
| 構造函數 | 抽象相機類 | - |
| 增加圖像 | Mat,時間戳 | - |
| 設置首幀圖像 | FramePtr | - |
| 讀取末幀圖像 | – | FramePtr |
初始化
- 構造特徵檢測子
- 指定深度濾波器回調函數
- 構造深度濾波器
- 開啓深度濾波器線程
processFirstFrame
- 初始化機體到世界的齊次變化(4x4矩陣)
- 單應矩陣得到第一幀圖像
- 增加keyframe
- 增加新幀
- 指示階段變動
processSecondFrame
- 單應矩陣增加第二幀圖像
- 兩視圖bundle adjustment
- 添加新幀
- 得到景深
- 增加關鍵幀
- 單應矩陣歸零
processFrame
- 獲取上幀圖像計算單應矩陣
- 稀疏圖像對齊 spase image alignment
- Feature alignment
- 如果重建誤差大於預設閾值,那麼丟棄計算所得變換,標註當前的計算是無效的.(map reprojection & feature alignment)
- 位姿最優化,optimizeGaussNewton
- 結構優化,optimizeStructure