在ROS中使用usb摄像头跑ORB SLAM2

摘要（假装这是论文）：由于最近在做orb_slam2相关，将这几天的实验过程总结记录，本文主要为傻瓜式记录运行代码的过程，以及中间遇到的问题及解决方法，截止至发文前，只是把单目Ｍono的功能实现，暂时还未对其进行细致的分析．其实ＣＳＤＮ中已经有很多其他大神对ORB SLAM的程序运行进行了解读，本次实验也是在众多资料的帮助下完成的．
准备工作
　硬件准备
　１．装有ROS Indigo和装有OpenCv的ubuntu 14.04
　３．USB 摄像头，TB来的，100度广角,640*480,30帧
　环境准备
　安装几个依赖项和库
　Boost
　

sudo apt-get install libboost-all-dev 

　General Graph Optimization　：g2o
　关于g2o的详细解释可以看高博的blog：http://www.cnblogs.com/gaoxiang12/p/5304272.html
　为了编译g2o需要安装如下三个库

　sudo apt-get install libblas-dev
　sudo apt-get install liblapack-dev
　sudo apt-get install libeigen3-dev

　
　代码准备
　１．由于使用USB摄像头作为图像输入，需要将图像信息作为topic发送出去，ROS官网提供了usb_cam的package代码，从github上下usb_cam的代码，下载链接：https://github.com/bosch-ros-pkg/usb_cam
　２．ORB_SLAM的代码下载： https://github.com/raulmur/ORB_SLAM2
工程建立
建立一个catkin_ws 工程

mkdir  -p ~/catkin_ws/src
cd ~/catkin_ws/
catkin_make

将代码拷到src中

编译usb_cam

mkdir build 
cd build 
cmake ..
make 

编译成功后可先测试一下usb摄像头
　　首先开个新的终端，运行roscore（如果显示命令没有找到就说明的你没有source source /opt/ros/indigo/setup.bash 我是直接添加到了根目录文件中的.bashrc中了，详情请下载＜机器人操作系统浅析＞百度文库中可搜到，见Page16）
　在运行该节点之前，需要先配置一下节点参数，打开src/usb_cam-develop/launch文件中的的launch文件，我这里只需要该设备号即可，笔记本电脑的video0一般是网络摄像头，插入usb摄像头一般是video1，若想看usb摄像头是哪个端口号，可cd到根目录中的dev文件夹下查看．
　
　完成之后再开一个终端， 运行

　roslaunch usb_cam usb_cam-test.launch　

　
　运行后，正常的情况下可打开摄像头，看到新建窗口将图像信息显示出来．同时命令窗口会有一部分warning ，直观显示是没有找到head_camera.yaml，应该是相机标定后的内参信息，我尝试将标定（标定之后也会说）后的数据写入yaml文件中让程序去读，打开失败，猜测应该是我的yaml文件中参数命名或者其他原因与程序不符合，因为还没有开始扒源码，暂且放一放，立个flag在这。
　
　
　(打开新的终端)运行rqt_graph可以看usb_cam节点向/usb_cam/image_raw发布了消息，image_view订阅了该消息，消息内容即为图像帧，由image_view显示．
　
编译g2o

cd ~/catkin_ws/ORB_SLAM2/Thirdparty/g2o/
mkdir build
cmake ..
make

编译DBoW2

cd ~/catkin_ws/ORB_SLAM2/Thridparty/DBoW2
mkdir build 
cmake ..
make

编译ORB_SLAM2

cd catkin_ws/src/ORB_SLAM2
mkdir build 
cd build 
cmake ..
make 

编译ROS的example

cd ~/catkin_ws/src/ORB_SLAM2/Example/ROS/ORB_SLAM2
mkdir build
cd build
cmake ..
make 

编译完成后，让我们看下官方文档上关于运行orb需要的两个文件

PATH_TO_VOCABULARY
PATH_TO_SETTINGS_FILE
第一个是VOCABULARY，在catkin_ws/src/ORB_SLAM2/Vocabulary中，将压缩包解压即可
第二个相机内参设置文件由于使用外部usb摄像头的时，需要输入相机内参进行运算，所以还需要对camera进行标定，这里采用的matlab工具箱
下载链接为：http://www.vision.caltech.edu/bouguetj/calib_doc/
（点击进入下载频道即可）
具体过程参考这个大神的博客
http://blog.csdn.net/hyacinthkiss/article/details/41317087
可能还需要一个写使用该camera拍摄一定数量的棋盘图片的程序
因为要在ＭＡＴＬＡＢ上标定，这里就在windows下VS2013中写了个简单的调取摄像头，鼠标左键当快门，保留当前帧为jpg图片到指定路径下的简单程序，需者自取．

#include"cv.h"
#include"highgui.h"
#include"iostream"
using namespace std;
using namespace cv;

void my_mouse_callback(int event, int x, int y, int flags, void* param);
bool take_photo = false;
const char* arry[2][35] = { "K:\\CVproject\\FUCKOPENCV\\test\\right1.jpg", "K:\\CVproject\\FUCKOPENCV\\test\\right2.jpg", "K:\\CVproject\\FUCKOPENCV\\test\\right3.jpg", "K:\\CVproject\\FUCKOPENCV\\test\\right4.jpg", "K:\\CVproject\\FUCKOPENCV\\test\\right5.jpg", "K:\\CVproject\\FUCKOPENCV\\test\\right6.jpg", "K:\\CVproject\\FUCKOPENCV\\test\\right7.jpg", "K:\\CVproject\\FUCKOPENCV\\test\\right8.jpg", "K:\\CVproject\\FUCKOPENCV\\test\\right9.jpg", "K:\\CVproject\\FUCKOPENCV\\test\\right10.jpg", "K:\\CVproject\\FUCKOPENCV\\test\\right11.jpg", "K:\\CVproject\\FUCKOPENCV\\test\\right12.jpg", "K:\\CVproject\\FUCKOPENCV\\test\\right13.jpg", "K:\\CVproject\\FUCKOPENCV\\test\\right14.jpg", "K:\\CVproject\\FUCKOPENCV\\test\\right15.jpg", "K:\\CVproject\\FUCKOPENCV\\test\\right16.jpg", "K:\\CVproject\\FUCKOPENCV\\test\\right17.jpg", "K:\\CVproject\\FUCKOPENCV\\test\\right18.jpg", "K:\\CVproject\\FUCKOPENCV\\test\\right19.jpg", "K:\\CVproject\\FUCKOPENCV\\test\\right20.jpg" };

int take_times = 0;
int main(int argc, char* argv[])
{
    CvCapture* cap;

    cap = cvCaptureFromCAM(0);

    if (!cap)
    {
        cout << "create camera capture error" << endl;
        system("pause");
        exit(-1);
    }
    cvNamedWindow("img",0);
    IplImage* img; 
    img = cvQueryFrame(cap);
    cvSetMouseCallback("img", my_mouse_callback, (void*) img);

    while (1)
    {
        img = cvQueryFrame(cap); 
        if (!img)
            break;
        if (img->origin == IPL_ORIGIN_TL)
        {
            cvFlip(img, img);
        }
        cvShowImage("img", img);
        if (take_photo)
        {

            cvSaveImage(arry[0][take_times], img);

            take_times++;
            if (take_times == 19)
                take_times = 0;

                take_photo = false;
        }
        cvWaitKey(3);

    }
    cvReleaseCapture(&cap);
    cvDestroyAllWindows();
    cvReleaseImage(&img);
    return 0;
}

void my_mouse_callback(int event, int x, int y, int flags, void* param)
{

    IplImage* image = (IplImage*) param;
    switch (event)
    {
        case CV_EVENT_LBUTTONDOWN:
        {
        　 cout << "set take photo flag" << endl;
　　　　　　take_photo = true;

        }break;
    }

}

将内参数据标定出来之后填写到Example/ROS/ORB_SLAM2路径下的yaml文件中（其中有两个参数我还不确定是否设置正确，搞清楚了再解释一下，先立个flag）

两个文件都准备好了之后就可以开始运行了．

运行顺序：
１．第一个终端

roscore

２．第二个终端运行usb_cam

roslaunch usb_cam usb_cam-test.launch

观察camera是否在正常输出图像，正常则继续
３．第三个终端运行

rosrun ORB_SLAM2 Mono /home/xx/catkin_ws/src/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/xx/catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/Asus.yaml 

因为现在是单目SLAM,采用的mono，第二个参数是标定参数保存的路径．
读vocabulary需要一点时间，等等．．．．
正常情况下，你这个时候应该是看到这样的东西，
４．第四个终端：
运行rqt_graph
你应该可以看到这样的东西

话题没有接上，所以当然是waiting for images咯　．

关掉第三个终端，进到catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/src 打开ros_mono.cc
将subscribe的话题改为/usb_cam/image_raw,将话题接上

    ros::Subscriber sub = nodeHandler.subscribe("/usb_cam/image_raw", 1, &ImageGrabber::GrabImage,&igb);

重新编译ROS的example

cd ~/catkin_ws/src/ORB_SLAM2/Example/ROS/ORB_SLAM2
mkdir build
cd build
cmake ..
make 

再来一次：

rosrun ORB_SLAM2 Mono /home/ｘｘ/catkin_ws/src/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/xx/catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/Asus.yaml 

路径请写自己的路径咯，xx是我的用户目录

到这里应该就可以跑起来了

总结：刚刚开始一段时间，还没有来得及对代码进行深扒，其中若有错误的地方，如有大神路过发现的还请帮忙及时提出改正．

参考了以下两位大佬的blog完成以上实验，最后才整理出来此篇博文，贴出链接，以供各位需要的同学看看．
http://blog.csdn.net/qq_18661939/article/details/51829157
http://blog.csdn.net/dourenyin/article/details/48055441