SVO: Fast Semi-Direct Monocular Visual Odometry 代碼測試

    我在一個項目中要用到單目視覺里程計，自然想到的是大名鼎鼎的libviso以及SVO了。

    libviso是在x86/64的intel芯片下的算法，兼容ubuntu以及win，具有雙目機單目兩種視覺里程計。但是它不適用於ARM Linux，因爲底層用到了SSE指令集。相關主頁www.cvlibs.net。真的是計算機視覺的大牛了。

  SVO是2014年在IEEE上的文章，其單目定位效果驚人。網址是https://github.com/uzh-rpg/rpg_svo。

  我來講一下這幾天配置遇到的坑。

  首先我是在虛擬機下安裝ubuntu12.04，安裝了ros，版本是groovy（關係不大）。在按照他的要求將部分依賴代碼下載到workspace，以及部分下載到catkin_ws/src中以後，開始編譯workspace裏的文件。但是有一個報錯。說是warning，但是由於在cmakelist有一個-Werror選項，只要是warning就會認爲是error，就會編譯失敗，於是百度，說吧-Werror刪除就好了，果然編譯通過了。之後開始編譯catkin_ws裏的SVO文件，會有error，最後查下去發現是gcc和g++的版本不夠（具體多少我忘了）。我只能百度一下，怎麼升級gcc和g++，升級到最新以後，編譯還是出錯！後來又百度，發現不光要升級gcc和g++，還有一切其他的要升級，有一個操作是要先刪除gcc和g++，，於是我刪除了gcc和g++以及他的依賴。。在刪除的時候。。我發現許多帶有ros的文件也被刪除了，，於是感覺整個人都不好了。轉念一想，乾脆裝一個ubuntu14.04，自帶最新的gcc和g++。

    於是下載了u14的包，虛擬機安裝，再安裝ros indigo，，發現下載ros太慢了，於是去exbot網站上下載了集合indigo的ubuntu14.04lts。之後按照SVO的要求，一步不下來果然沒有問題，全部編譯成功。

    之後要開始使用SVO了，下載了他的數據集，用rosbag play ，和roslaunch svo_ros test_rig3.launch 再 rosrun rviz rviz（先按要求配置好rviz的初始化文件），就可以看到效果了。

    之後是使用live.launch,最好自己再建一個，比如我的是live_xjh.launch。我們來看一下原始的內容：

<launch>
  
    <node pkg="svo_ros" type="vo" name="svo" clear_params="true" output="screen">
    
        <!-- Camera topic to subscribe to -->
        <param name="cam_topic" value="/camera/image_raw" type="str" />
        
        <!-- Camera calibration file -->
        <rosparam file="$(find svo_ros)/param/my_camera.yaml" />
        
        <!-- Default parameter settings: choose between vo_fast and vo_accurate -->
        <rosparam file="$(find svo_ros)/param/vo_fast.yaml" />

    </node>
        
</launch>

仔細閱讀！他要有/camera/image_raw的主題能夠訂閱，還會打開一個/param/my_camera.yaml相機初始化配置，以及/param/vo_fast.yaml里程計的參數配置。

打開/param裏會發現許多文件，因爲他介紹了三種不同的攝像頭內參標定，我一開始只會在matlab下標定，於是使用了第二種方式。羅技網絡攝像頭參數如下：

cam_model: Pinhole
cam_width: 640
cam_height: 480
cam_fx: 690.09102 
cam_fy: 686.25294 
cam_cx: 325.38988 
cam_cy: 286.9196 
cam_d0: -0.02240 
cam_d1: -0.05900 
cam_d2: 0.00894 
cam_d3: -0.00590 

之後要寫一個節點將攝像頭的數據打包成一個/camera/image_raw的topic發佈出來。代碼如下

#include <ros/ros.h>
#include <stdio.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>  
static const char WINDOW[] = "Image window";

int main(int argc, char** argv)
{
  int number = 0;
  ros::init(argc, argv, "image_converter");

  //Reading an image from the file
  CvCapture* pCapture = cvCreateCameraCapture(0);
  IplImage* pFrame = cvQueryFrame( pCapture );
  IplImage* dst = cvCreateImage(cvGetSize(pFrame), pFrame->depth, 1);
  cvCvtColor(pFrame, dst, CV_BGR2GRAY);
  cv::Mat cv_image = cv::Mat(dst);

  //Convert OpenCV image to ROS message
  ros::NodeHandle node;
  image_transport::ImageTransport transport(node);
  image_transport::Publisher image_pub; 
  image_pub=transport.advertise("/camera/image_raw", 1);
 // image_pub=transport.advertise("/image", 1);
  ros::Time time=ros::Time::now(); 

  cv_bridge::CvImage cvi;
  cvi.header.stamp = time;
  cvi.header.frame_id = "image";
 // cvi.encoding = "bgr8";
  cvi.encoding = "mono8";
  cvi.image = cv_image;

  sensor_msgs::Image im;
  cvi.toImageMsg(im);
  ros::Rate loop_rate(30);
while(ros::ok())
{
  pFrame=cvQueryFrame( pCapture );
  cvCvtColor(pFrame, dst, CV_BGR2GRAY);
  cv_image = cv::Mat(dst);
  cvi.toImageMsg(im);

  image_pub.publish(im);
 // ROS_INFO("Converted Successfully!number = %d",number++);
  ros::spinOnce();
  loop_rate.sleep();

}
  //Show the image
ROS_INFO("END");
  //ros::spin();
  return 0;
}

尤其注意，他需要的是單通道的灰度圖，我的攝像頭本來就是640*480所以不用resize了。

之後就可以打開攝像頭節點，以及roslaunch svo_ros live_xjh.launch 和rosrun rviz rviz 。

嘿嘿，然後你會發現根本跟不住特徵點。這個在github上很多人都在問。我看了他的配置才發現有個地方漏掉了。我們去看test_rig3.launch

<launch>

  <node pkg="svo_ros" type="vo" name="svo" clear_params="true" output="screen">

    <!-- Camera topic to subscribe to -->
    <param name="cam_topic" value="/camera/image_raw" type="str" />
    
    <!-- Camera calibration file -->
    <rosparam file="$(find svo_ros)/param/camera_atan.yaml" />

    <!-- Default parameter settings: choose between vo_fast and vo_accurate -->
    <rosparam file="$(find svo_ros)/param/vo_fast.yaml" />
    
    <!-- Initial camera orientation, make it point downwards -->
    <param name="init_rx" value="3.14" />
    <param name="init_ry" value="0.00" />
    <param name="init_rz" value="0.00" />

  </node>

</launch>

是不是發現多了三行

    <param name="init_rx" value="3.14" />
    <param name="init_ry" value="0.00" />
    <param name="init_rz" value="0.00" />

這個是旋轉參數的初始化，他將x轉了180度，也就是朝下了，視頻一開始攝像頭就是朝下的！爲什麼這個參數設置很重要？是因爲這個算法更新關鍵幀是要高度變化百分之15纔會刷新，如果我們把朝向弄錯了，就很難跟新了。（這也說明，這個算法，攝像頭不能向前，要向下）。將這段代碼加到我們的·launch文件中再次執行會發現能跟得住特徵點了。

    下面講講效果。

    作者使用的是德國MX的bluefox攝像頭，是全局曝光，我們實驗室有point grey，但是我不知道怎麼在linux裏驅動這些工業攝像頭，用了最簡單的辦法，在win下面拍好視頻，放到linux裏，用opencv讀取每一幀再打包成topic放出來。有關代碼有需要可以留言，不過最好自己寫，也不難。。比較了工業攝像頭和羅技的網絡攝像頭，我並沒有看出多大的區別，動作太大時都會跟蹤失敗。可能是我拍的地方紋理信息不夠多吧，，，但是桌子已經很亂了。。應該紋理很豐富呀，嘿嘿。

    有關攝像頭標定，作者希望使用第一種方式，於是要下載PTAM的包，這個標定過程很順利，要注意的是他標定程序需要讀取的topic的名字是什麼，以及他需要的是mono8圖像。在矯正過程中，工業相機能夠將error控制到0.3以下，羅技的只能到0.45。

  

  最後吐槽以下，國內沒有linux下的usb全局曝光相機，都是做win下面的，arm linux就更少了，GPIO輸出的全局相機還是有的，不過不能傳輸到電腦中。國外的工業相機動則上萬。。真心有點坑