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: Pinholecam_width: 640cam_height: 480cam_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 imageROS_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输出的全局相机还是有的，不过不能传输到电脑中。国外的工业相机动则上万。。真心有点坑