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。我们来看一下原始的内容：
仔细阅读！他要有/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
#include
#include
#include
#include
#include
#include
#include
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;
} 之后就可以打开摄像头节点，以及roslaunch svo_ros live_xjh.launch 和rosrun rviz rviz 。
嘿嘿，然后你会发现根本跟不住特征点。这个在github上很多人都在问。我看了他的配置才发现有个地方漏掉了。我们去看test_rig3.launch
是不是发现多了三行
这个是旋转参数的初始化，他将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输出的全局相机还是有的，不过不能传输到电脑中。国外的工业相机动则上万。。真心有点坑