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文件 最末端的时候应该是这个样子：

• 检验ROS path是否被识别

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

echo $ROS_PACKAGE_PATH

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

如出现路径不能识别的问题，具体可以参考：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

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

2.4 ROS运行.launch文件

roslaunch usb_cam usb_cam-test.launch

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

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

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

2.4 查看rqt节点信息

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

查看相应的节点，

查看相应topic上的带宽，

注意：上述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节点

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

问题出在了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中将其订阅的代码改掉，重新编译。

方法一：

编写自定义的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

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

牵线搭桥成功。

5. 实验结果

 

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