ORB_SLAM2 技术详解(3)Part II -- 编译并使用ROS ORB_SLAM2 Package

目录

1. 编译ORB_SLAM2 ROS package

  1.1系统平台

1.2 创建catkin_ws

1.3 安装相关依赖库

cv_bridge的opencv版本冲突的问题解决

2. 使用USB3.0 Camera

2.1 安装ROS usb_cam源码

2.2 编译usb_cam

2.3 测试usb摄像头

2.4 ROS运行.launch文件

2.4 查看rqt节点信息

3. 使用ROS ORB_SLAM2节点

     3.1 rosrun ORB_SLAM2 Mono 节点

3.2 查看rqt_graph

3.3  Topic

4. 测试

5. 实验结果


1. 编译ORB_SLAM2 ROS package

  1.1系统平台

  • 操作系统 :Ubuntu 16.04
  • ROS版本 :kinetic
  • eigen库版本 :3.x.x
  • OpenCV版本:opencv3.4.7
  • USB摄像头:640*480,30帧

1.2 创建catkin_ws

  • 使用ROS创建workspace

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

  • 添加环境变量(修改并按照你的ORB_SLAM2的创建路径配置

echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc

1.3 安装相关依赖库

参考链接:https://www.jianshu.com/p/f39bf76cfc61

  • 安装git cmake
  • sudo apt-get install git cmake

  • 安装一些库
  • sudo apt-get install libblas-dev liblapack-dev

  • 安装Pangolin
  • sudo apt-get install libglew-dev
    sudo apt-get install libboost-dev libboost-thread-dev libboost-filesystem-dev
    sudo apt-get install libpython2.7-dev
    sudo apt-get install build-essential
    cd ~/catkin_ws/ORB-SLAM2
    git clone https://github.com/stevenlovegrove/Pangolin.git
    cd Pangolin

  • 编译安装Pangolin
  • mkdir build
    cd build
    cmake -DCPP11_NO_BOOST=1 ..
    make

  • 安装OpenCV依赖
  • sudo apt-get install cmake git libgtk2.0-dev pkg-config libavcodec-dev libavformat-dev libswscale-dev

  • 下载OpenCV并解压到Ubuntu,然后进入OpenCV文件夹进行编译安装
  • mkdir release
    cd release
    cmake -D CMAKE_BUILD_TYPE=RELEASE -D CMAKE_INSTALL_PREFIX=/usr/local ..
    make
    sudo make install

  • 安装指定版本的eigen库
  • 下载Eigen,进入到在解压后的Eigen文件夹(例如eigen-eigen-07105f7124f9)下

mkdir build
cd build
cmake ..
make
sudo make install

  • 下载ORB_SLAM2:

git clone https://github.com/raulmur/ORB_SLAM2.git ORB_SLAM2

  • 编译安装

cd ORB_SLAM2
chmod +x build.sh
vim build.sh

把build.sh的make -j改成make(这是为了防止多线程编译出错,对于build_ros.sh文件同理

  • 运行build.sh

./build.sh

  • 注意坑1:如果在编译构建的过程中出现error:usleep()函数未定义的错误,要到报错误的代码文件里面加上(或者在头文件加也行):
#include 
#include 
#include 
  • 修改环境变量(user就是用户的名字)

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/(user)/catkin_ws/ORB-SLAM2/Examples/ROS

这个时候 打开~/.bashrc文件 最末端的时候应该是这个样子:

ORB_SLAM2 技术详解(3)Part II -- 编译并使用ROS ORB_SLAM2 Package_第1张图片

ORB_SLAM2 技术详解(3)Part II -- 编译并使用ROS ORB_SLAM2 Package_第2张图片

  • 检验ROS path是否被识别

理论上,这个应该要work的,不过有可能ros不能识别。如何确认有没有work呢?方法是:

echo $ROS_PACKAGE_PATH

  如果终端显示了笔者刚刚加入的路径,说明ROS路径配置成功了

ORB_SLAM2 技术详解(3)Part II -- 编译并使用ROS ORB_SLAM2 Package_第3张图片

如出现路径不能识别的问题,具体可以参考:https://github.com/raulmur/ORB_SLAM2/issues/url

链接:https://www.cnblogs.com/yepeichu/p/10896201.html

最后,验证ros是否识别ORB_SLAM2包,

roscd ORB_SLAM2

如果能进入这个文件夹,说明我们路径设置成功了。ROS配置关,pass。

  • 编译安装ROS版本的ORB-SLAM2

cd ORB_SLAM2
chmod +x build_ros.sh
./build_ros.sh

  • 注意坑2:如果在编译构建的过程中出现关于boost库的错误,要到/Examples/ROS/ORB-SLAM2/CMakeLists.txt文件下修改 加上 -lboost_system,如果依旧不能解决问题,请重新安装boost库。
  • 请参考本人另外一篇专门踩坑的博客:https://blog.csdn.net/hhaowang/article/details/104385286
set(LIBS
${OpenCV_LIBS}
${EIGEN3_LIBS}
${Pangolin_LIBRARIES}
${PROJECT_SOURCE_DIR}/../../../Thirdparty/DBoW2/lib/libDBoW2.so
${PROJECT_SOURCE_DIR}/../../../Thirdparty/g2o/lib/libg2o.so
${PROJECT_SOURCE_DIR}/../../../lib/libORB_SLAM2.so
#加上这一句
-lboost_system
)

 


cv_bridge的opencv版本冲突的问题解决

  请参考本人另外一篇专门踩坑的博客:https://blog.csdn.net/hhaowang/article/details/104385286

内心:Fxxk!!!怎么这么多问题!--->Github ORB_SLAM2 --->issues-->哦,原来不止我一个人啊!!!


 

2. 使用USB3.0 Camera

参考:https://blog.csdn.net/Felaim/article/details/79612504 ROS(三):使用ROS跑通ORB_SLAM2

参考:https://www.jianshu.com/p/f39bf76cfc61 ORB-SLAM2编译安装和USB摄像头例程运行

  • 由于使用USB摄像头作为图像输入,需要将图像信息作为massage 发送到topic usb_cam上去,注意不是/camera/,ROS官网提供了usb_cam的package代码,从github上下usb_cam的代码,下载链接:https://github.com/bosch-ros-pkg/usb_cam

2.1 安装ROS usb_cam源码

将usb_cam 文件夹放在catkin_ws/src目录下,即执行以下命令

cd catkin_ws/src
git clone https://github.com/bosch-ros-pkg/usb_cam.git
cd ..
catkin_make


ORB_SLAM2 技术详解(3)Part II -- 编译并使用ROS ORB_SLAM2 Package_第4张图片

2.2 编译usb_cam

cd usb_cam
mkdir build
cd build
cmake ..
make

 

2.3 测试usb摄像头

首先开个新的终端,运行roscore(如果显示命令没有找到就说明的你没有source source /opt/ros/indigo/setup.bash)
在运行该节点之前,需要先配置一下节点参数,打开src/usb_cam-develop/launch文件中的的launch文件,我这里只需要该设备号即可,笔记本电脑的video0一般是网络摄像头,插入usb摄像头一般是video1,若想看usb摄像头是哪个端口号,可cd到根目录中的dev文件夹下查看。

ls /dev/video*

cd ~/workspace/catkin_ws/src/usb_cam

更改launch文件

cd launch
gedit usb_cam-test.launch

ORB_SLAM2 技术详解(3)Part II -- 编译并使用ROS ORB_SLAM2 Package_第5张图片

2.4 ROS运行.launch文件

roslaunch usb_cam usb_cam-test.launch

如果出现image_view报错的情况,安装image_view 

sudo apt-get install ros-kinetic-image-view

ORB_SLAM2 技术详解(3)Part II -- 编译并使用ROS ORB_SLAM2 Package_第6张图片

当摄像头可以打开后,就能在image_view node上查看实时图像。

ORB_SLAM2 技术详解(3)Part II -- 编译并使用ROS ORB_SLAM2 Package_第7张图片

2.4 查看rqt节点信息

ORB_SLAM2 技术详解(3)Part II -- 编译并使用ROS ORB_SLAM2 Package_第8张图片

可以清晰的看到,/usb_cam发布了一个消息/usb_cam/image_raw/usb_cam Topic上,然后/image_view node订阅了这个Topic以接收响应的消息。

查看相应的节点,

ORB_SLAM2 技术详解(3)Part II -- 编译并使用ROS ORB_SLAM2 Package_第9张图片

ORB_SLAM2 技术详解(3)Part II -- 编译并使用ROS ORB_SLAM2 Package_第10张图片

ORB_SLAM2 技术详解(3)Part II -- 编译并使用ROS ORB_SLAM2 Package_第11张图片

查看相应topic上的带宽,

ORB_SLAM2 技术详解(3)Part II -- 编译并使用ROS ORB_SLAM2 Package_第12张图片

注意:上述Topic、msg拓扑关系为下一步实验失败埋下伏笔!


3. 使用ROS ORB_SLAM2节点

     3.1 rosrun ORB_SLAM2 Mono 节点

根据不同的环境配置和文件路径更改之后再运行,

rosrun ORB_SLAM2 Mono ~/workspace/catkin_ws/ORB_SLAM2/Vocabulary/ORBvoc.txt ~/workspace/catkin_ws/ORB_SLAM2/Examples/Monocular/TUM1.yaml

3.2 查看rqt_graph

此时再看rqt_graph,多出来了/camera topic和 /camera/image_raw msg,以及Mono节点

ORB_SLAM2 技术详解(3)Part II -- 编译并使用ROS ORB_SLAM2 Package_第13张图片

理论上,ROS会启用usb_cam node 和 ORB_SLAM2 Mono node,并接收/image_raw上的图像完成SLAM工作。很遗憾,出现了以下这种情况:WATING FOR IMAGES

ORB_SLAM2 技术详解(3)Part II -- 编译并使用ROS ORB_SLAM2 Package_第14张图片

问题出在了ORB_SLAM2 Mono节点默认启用标准消息类型/camera/image_raw,而我们使用的usb_cam驱动调用camera时将图像数据发布在自定义消息类型/usb_cam/image_raw上。

ORB ROS节点订阅的topic和usb_cam发布的topic名称不同!

 

3.3  Topic

因此,针对以上问题有两种方法可以解决:第一中较费事,但是可以帮助理解ROS的工作过程,第二种很简单,去ORB_SLAM中将其订阅的代码改掉,重新编译。

ORB_SLAM2 技术详解(3)Part II -- 编译并使用ROS ORB_SLAM2 Package_第15张图片

方法一:

编写自定义的ROS包,让ORB-SLAM的ROS节点订阅摄像头发布图像的topic

问题是,ORB-SLAM ROS节点订阅的topic为/camera/image_view,而笔记本摄像头图像流发布topic为/usb_cam/image_raw,这些可以通过rostopic list -v / rosnode list看到.

因此需要自己写一个ROS node程序,将这两个topic联合起来,我们选择自己重新定义一个ros packge

cd catkin_ws/src
catkin_create_pkg orb_image_transport image_transport cv_bridge
cd ..
catkin_make
cd orb_image_transport
gedit orb_image_converter.cpp

 

orb_image_converter.cpp文件负责将笔记本摄像头图像/usb_cam/image_view, publish到同一个topic,让ORB-SLAM订阅这个topic

#include 
#include 
#include 
#include 
#include  //include the headers for OPENCV's image processing and GUI module
#include   //
 
static const std::string OPENCV_WINDOW = "Image window";   //define show image gui
 
class ImageConverter
{
  ros::NodeHandle nh_;                    //define Nodehandle
  image_transport::ImageTransport it_;    //use this to create a publisher or subscriber
  image_transport::Subscriber image_sub_; //
  image_transport::Publisher image_pub_;
   
public:
  ImageConverter()
    : it_(nh_)
  {
    // Subscrive to input video feed and publish output video feed
    image_sub_ = it_.subscribe("/usb_cam/image_raw", 1,
      &ImageConverter::imageCb, this);
    //image_pub_ = it_.advertise("/image_converter/output_video", 1);
    image_pub_ = it_.advertise("/camera/image_raw", 1);
    cv::namedWindow(OPENCV_WINDOW);    //Opencv HighGUI calls to create/destroy a display window on start-up / shutdon
  }
 
  ~ImageConverter()
  {
    cv::destroyWindow(OPENCV_WINDOW);
  }
 
  void imageCb(const sensor_msgs::ImageConstPtr& msg)
  {
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
      cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }
    cv::imshow(OPENCV_WINDOW, cv_ptr->image);
    cv::waitKey(3);
     
    // Output modified video stream
    image_pub_.publish(cv_ptr->toImageMsg());
  }
};
 
int main(int argc, char** argv)
{
  ros::init(argc, argv, "image_converter");
  ImageConverter ic;
  ros::spin();
  return 0;
}

并在CMakeLists.txt文件最后添加

add_executable(orb_image_converter orb_image_converter.cpp)
target_link_libraries(orb_image_converter ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
cmake_minimum_required(VERSION 2.8.3)

project(orb_image_transport)


## Compile as C++11, supported in ROS Kinetic and newer

# add_compile_options(-std=c++11)

## Find catkin macros and libraries

## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)

## is used, also find other catkin packages

set(cv_bridge_DIR /usr/local/share/cv_bridge/cmake)  


find_package(catkin REQUIRED COMPONENTS

  cv_bridge

  image_transport

)
find_package(OpenCV 3.4.7 REQUIRED)

catkin_package()

add_executable(orb_image_converter src/orb_image_converter.cpp)

target_link_libraries(

orb_image_converter 

${catkin_LIBRARIES} 

${OpenCV_LIBRARIES}

-lboost_system

)

## System dependencies are found with CMake's conventions

# find_package(Boost REQUIRED COMPONENTS system)

 

在catkin_ws目录下执行catkin_make后就完成了所有的工作.若编译遇到错误https://blog.csdn.net/qq_31813825/article/details/86532187?utm_source=distribute.pc_relevant.none-task

注意这里没有使用自定义的消息类型,不需要对Package.xml和CMakeLists.txt做别的改动.

最后一次运行就可以完成ORB-SLAM在笔记本摄像头上的运行

 

方法二:

后来发现这种方法太笨,在安装了博世的ROS摄像头驱动包usb_cam以后,摄像头的图像将发布到/usb_cam/image_raw,因此在ORB的代码中将其订阅的topic从/camera/image_raw改为/usb_cam/image_raw即可,在ROS目录下的ros_mono.cc文件中修改即可,双目,深度以及AR demo同理。

 


 

4. 测试

启动
1、首先我们要启动ros内核,新开一个终端,执行

roscore

 

2、启动image topic转换节点

rosrun orb_image_transport orb_image_converter


3、启动usb_cam摄像头节点,新开一个终端,执行

roslaunch usb_cam usb_cam-test.launch

ORB_SLAM2 技术详解(3)Part II -- 编译并使用ROS ORB_SLAM2 Package_第16张图片
4、启动 ORB_SLAM2 的 Mono 节点(单目)

rosrun ORB_SLAM2 Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE
PATH_TO_VOCABULARY是ORBvoc.txt文件路径,PATH_TO_SETTINGS_FILE是你的摄像机内参数文件路径
所以可以在第三个终端输入如下命令:

rosrun ORB_SLAM2 Mono ~/workspace/catkin_ws/ORB_SLAM2/Vocabulary/ORBvoc.txt ~/workspace/catkin_ws/ORB_SLAM2/Examples/Monocular/TUM1.yaml

ORB_SLAM2 技术详解(3)Part II -- 编译并使用ROS ORB_SLAM2 Package_第17张图片

牵线搭桥成功。

5. 实验结果

 

摄像头标定请参考https://www.jianshu.com/p/967a35dbb56a

ORB_SLAM2 技术详解(3)Part II -- 编译并使用ROS ORB_SLAM2 Package_第18张图片

 

 

你可能感兴趣的:(SLAM开源架构详解)