SLAM程式碼(SVO ros )

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

  • 初始化機體到世界的齊次變化(4×4矩陣)
  • 單應矩陣得到第一幀影象
  • 增加keyframe
  • 增加新幀
  • 指示階段變動

processSecondFrame

  • 單應矩陣增加第二幀影象
  • 兩檢視bundle adjustment
  • 新增新幀
  • 得到景深
  • 增加關鍵幀
  • 單應矩陣歸零

processFrame

  • 獲取上幀影象計算單應矩陣
  • 稀疏影象對齊 spase image alignment
  • Feature alignment
  • 如果重建誤差大於預設閾值,那麼丟棄計算所得變換,標註當前的計算是無效的.(map reprojection & feature alignment)
  • 位姿最優化,optimizeGaussNewton
  • 結構優化,optimizeStructure