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

摘要(假装这是论文):由于最近在做orb_slam2相关,将这几天的实验过程总结记录,本文主要为傻瓜式记录运行代码的过程,以及中间遇到的问题及解决方法,截止至发文前,只是把单目Mono的功能实现,暂时还未对其进行细致的分析.其实CSDN中已经有很多其他大神对ORB SLAM的程序运行进行了解读,本次实验也是在众多资料的帮助下完成的.
准备工作
 硬件准备
 1.装有ROS Indigo和装有OpenCv的ubuntu 14.04
 3.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

 
 代码准备
 1.由于使用USB摄像头作为图像输入,需要将图像信息作为topic发送出去,ROS官网提供了usb_cam的package代码,从github上下usb_cam的代码,下载链接:https://github.com/bosch-ros-pkg/usb_cam
 2.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文件夹下查看.
 更改后,其实只是换了个1
 完成之后再开一个终端, 运行

 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拍摄一定数量的棋盘图片的程序
因为要在MATLAB上标定,这里就在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)
这里写图片描述

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

运行顺序:
1.第一个终端

roscore

2.第二个终端运行usb_cam

roslaunch usb_cam usb_cam-test.launch

观察camera是否在正常输出图像,正常则继续
3.第三个终端运行

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需要一点时间,等等....
正常情况下,你这个时候应该是看到这样的东西,这里写图片描述
4.第四个终端:
运行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/xx/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

相关推荐
©️2020 CSDN 皮肤主题: 大白 设计师:CSDN官方博客 返回首页