半直接法视觉里程计（SVO）实践

本文主要分两部分，编译安装SVO后对官方数据集的测试以及实验室摄像头的测试。

一．SVO安装及测试

在官方github首页上有比较详细的安装说明，不过部分步骤略有问题，此处给出成功安装的步骤。操作系统为ubuntu16.04并且安装ROS系统。

共创建两个工作空间，一个存放各种库，包括fast、g2o以及Sophus，另一个存放SVO代码。

1.安装Sophus库

按照步骤即可

cd workspace

git clone https://github.com/strasdat/Sophus.git

cd Sophus

git checkout a621ff

mkdir build

cd build

cmake ..

make

2.安装角点检测库

同样按照步骤即可

cd workspace

git clone https://github.com/uzh-rpg/fast.git

cd fast

mkdir build

cd build

cmake ..

make

3.安装g2o库（选择性）

这一步同样按照官网步骤即可，另外需要安装的依赖库有cmake, libeigen3-dev, libsuitesparse-dev, libqt4-dev, qt4-qmake, libqglviewer-qt4-dev，使用apt-get安装即可。

cd workspace

git clone https://github.com/RainerKuemmerle/g2o.git

cd g2o

mkdir build

cd build

cmake ..

make

sudo make install

需要注意的是，按照官方说明安装的eigen库并不能在当前SVO版本中工作，解决办法是安装eigen3.2.10。（此问题在github上的issue栏中有说明）eigen下载链接

http://bitbucket.org/eigen/eigen/get/3.2.10.tar.bz2

下载后运行如下命令编译安装

tar -xf [name of the file]

cd [name of the file]

mkdir build

cd build

cmake ..

sudo make install

4.按照官网安装vikit和ROS依赖项

cd catkin_ws/src

git clone https://github.com/uzh-rpg/rpg_vikit.git

sudo apt-get install ros-hydro-cmake-modules

命令中将hydro替换为自己对应的发行版即可，此处为kinect。

5.编译代码

cd catkin_ws/src

git clone https://github.com/uzh-rpg/rpg_svo.git

如果安装了g2o则需要将svo/CMakeLists.txt文件中的HAVE_G2O变量置为TRUE，添加环境变量G2O_ROOT。

gedit  ~/.bashrc

添加export G2O_ROOT = $HOME/SLAM/SVO/g2o

使用catkin_make编译SVO代码。

6.测试数据集

下载地址 

rpg.ifi.uzh.ch/datasets/airground_rig_s3_2013-03-18_21-38-48.bag

一共需要开启四个命令行窗口，首先启动ROS，运行roscore。

然后是roslaunch svo_ros test_rig3.launch，运行此命令前需要运行source devel/setup.bash。

再然后是运行

rosrun rviz rviz -d <PATH TO rpg_svo>/svo_ros/rviz_config.rviz

开启GUI查看结果。

最后运行

rosbag play airground_rig_s3_2013-03-18_21-38-48.bag

加载数据集，可以在GUI中看到数据集运行的效果。

二．摄像头测试

测试摄像头前需要标定，使用atan模型的效果较好，可以在PTAM中进行摄像头的标定，不过由于PTAM没有安装成功，因此这里使用SVO中原有的参数，只要在参数文件中把图像宽度和高度做修改即可。

1.图像采集

使用ROS提供的usb camera节点程序采集摄像头图像，下载编译后运行

rosrun usb_cam usb_cam_node开始采集图像并发布，运行如下命令查看原始彩色图像

rosrun image_view image_view image:=/usb_cam/image_raw

2.图像转换

SVO使用的图像为单通道灰度图，摄像头输出为RGB彩色图像，因此还需要一个节点程序订阅彩色图像后转换成灰度图并发布。创建ROS节点程序可以参考：

http://www.cnblogs.com/blue35sky/p/6078771.html

注意创建时添加对应的依赖项，如opencv相关的依赖项（cv_bridge、image_transport）。转换后的图像为/mono_image，运行图像转换程序

rosrun cam_test cam_test_node并在第一步查看图像命令中修改需要查看的图像主题名即可看到灰度图像。

转换代码：

#include <ros/ros.h>
#include <opencv2/opencv.hpp>
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include "ros/ros.h"
#include "std_msgs/String.h"
 
ros::Publisher image_pub ;
 
void chatterCallback(const sensor_msgs::ImageConstPtr& msg)
{
    cv_bridge::CvImagePtr  cv_ptr;
    cv_ptr = cv_bridge::toCvCopy(msg,sensor_msgs::image_encodings::BGR8);
    cv::Mat image_gray;
    cv::cvtColor(cv_ptr->image, image_gray,CV_BGR2GRAY);//灰度化
 
    cv_bridge::CvImage  cvi;
    sensor_msgs::Image  ros_img;
    ros::Time time=ros::Time::now();
    cvi.header.stamp = time;
    cvi.header.frame_id = "image";
    cvi.encoding = "mono8";
    cvi.image = image_gray;
    cvi.toImageMsg(ros_img);
    image_pub.publish(cvi.toImageMsg());
}
 
int main(int argc, char **argv)
{
  ros::init(argc, argv, "img_tran");
  ros::NodeHandle n;
 
  ros::Subscriber sub = n.subscribe("/usb_cam/image_raw", 1000, chatterCallback);
  image_pub = n.advertise<sensor_msgs::Image>("/mono_image", 1000);
 
  ros::spin();
  return 0;
}

3.启动SVO测试

可以直接在原有的test_rig3.launch文件中稍作修改后启动SVO，将摄像头主题的值修改为/mono_image，即上一步发布的灰度图。下面的摄像头校准文件，如果做了校准，可以写成param文件夹中文件的格式，之后在启动文件中将摄像头校准文件参数改成自己的文件就好。一般情况下只有这两处需要修改，之后就可以启动SVO，启动GUI查看位姿估计结果。这里由于摄像头没有校准，因此成功率不高，经常提示特征点太少，不过成功的情况下位姿估计效果还是不错的。