ROS调用ORB-SLAM2

    在博文<ORB-SLAM2编译安装>中,我们编译安装了ORB-SLAM2,也运行了其自带的ROS例程。但是该ROS程序基于rosbuild编译,而我们现在更多的使用catkin,参考作用有限。这篇文章记录了基于ROS调用ORB-SLAM2的过程。所谓的调用是指ORB-SLAM2接收ROS消息作为输入,并由ROS将位姿估计的结果发布出去。


一、新建ROS项目

    <ROS-创建功能包和节点>中我们介绍了ROS工作空间和功能包的创建过程。这里创建slam功能包,如下:

#创建工作空间
(base) chenjianqu@chen:~$ cd ros
(base) chenjianqu@chen:~/ros$ cd project
(base) chenjianqu@chen:~/ros/project$ mkdir -p ws2/src
(base) chenjianqu@chen:~/ros/project$ cd ws2
(base) chenjianqu@chen:~/ros/project/ws2$ catkin_make #初始化工作空间

#创建功能包
(base) chenjianqu@chen:~/ros/project/ws2$ cd src
(base) chenjianqu@chen:~/ros/project/ws2/src$ catkin_create_pkg slam cv_bridge image_transport roscpp sensor_msgs std_msgs tf message_filters


二、编写源代码

    先编写源代码,在src目录下增加orb_slam_run.cpp:

#include <stdio.h>
#include <iostream>
#include <algorithm>
#include <fstream>
#include <chrono>

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <tf/transform_broadcaster.h>
#include <eigen_conversions/eigen_msg.h>

#include <std_msgs/Time.h>
#include <opencv2/core/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>

#include"System.h"

using namespace std;

class ImageGrabber
{
public:
    ImageGrabber(ORB_SLAM2::System* pSLAM);
    void GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD);
protected:
    ORB_SLAM2::System* mpSLAM;
    tf::TransformBroadcaster* br;
};



int main(int argc, char** argv)
{
    ros::init(argc, argv, "orb_slam_node");//初始化节点
    ros::start();//启动节点
    if(argc != 3){
        cout<<"需要传入参数:视觉词典路径 配置文件路径" << endl;
        ros::shutdown();//关闭节点
        return 1;
    }
    
    //初始化ORB-SLAM2
    ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD,true);
    ImageGrabber igb(&SLAM);
    
    ros::NodeHandle nh;
    
    message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/rgb/image_raw", 1);
    message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/camera/depth_registered/image_raw", 1);
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_pol;
    message_filters::Synchronizer<sync_pol> sync(sync_pol(10), rgb_sub,depth_sub);
    sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD,&igb,_1,_2));
    
    ros::spin();
    
    SLAM.Shutdown();
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
    
    ros::shutdown();
    
    return 0;
}



ImageGrabber::ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM)
{
    br=new tf::TransformBroadcaster();
}


void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD)
{
    ros::Time timestamp= msgRGB->header.stamp;//时间戳
        
    cv_bridge::CvImageConstPtr cv_ptrRGB;
    try{
            cv_ptrRGB = cv_bridge::toCvShare(msgRGB);
    }
    catch (cv_bridge::Exception& e){
            ROS_ERROR("cv_bridge exception: %s", e.what());
            return;
    }
    cv_bridge::CvImageConstPtr cv_ptrD;
    try{
            cv_ptrD = cv_bridge::toCvShare(msgD);
    }
    catch (cv_bridge::Exception& e){
            ROS_ERROR("cv_bridge exception: %s", e.what());
            return;
    }
    
    //调用ORB-SLAM2
    cv::Mat Tcw=mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec());
    tf::Transform m;
    
    //设置平移
    m.setOrigin(
        tf::Vector3(
            Tcw.at<float>(0,3),
            Tcw.at<float>(1,3),
            Tcw.at<float>(2,3)
        )
    );
    
    //设置旋转
    tf::Matrix3x3 Rcw;
    Rcw.setValue( //Mat转换为Matrix
        Tcw.at<float>(0,0),Tcw.at<float>(0,1),Tcw.at<float>(0,2),  
        Tcw.at<float>(1,0),Tcw.at<float>(1,1),Tcw.at<float>(1,2),
        Tcw.at<float>(2,0),Tcw.at<float>(2,1),Tcw.at<float>(2,2)
    );
    double roll,pitch,yaw;
    Rcw.getRPY(roll, pitch, yaw, 1);//Matrix转换为RPY
    tf::Quaternion q;
    q.setRPY(roll,pitch,yaw);//RPY转换为四元数
    m.setRotation(q);
    
    //发布坐标
    br->sendTransform(tf::StampedTransform(m, timestamp, "world", "orb_slam2"));
}

    如果阅读过ORB-SLAM2源代码的话,就知道ORB-SLAM2的入口程序定义在System.cc里面,其中TrackRGBD()函数是RGBD版ORB-SLAM2的入口函数,该函数接受opencv格式的rgb图和深度图作为输入,因此需要通过cv_bridge将sensor_msgs/Image格式转换为opencv格式。该回调函数封装成类的原因是便于传参。

    深度图和RGB图是两个不同的消息,因此需要通过message_filters进行时间上的对准。

    ORB-SLAM2返回的位姿是cv::Mat格式的,需要先转为tf::Matrix,再转为旋转角,再转为四元数,最后才能放到tf::Transform里面,最后发布的是world帧到orb_slam2帧的位姿。有关ros中的TF内容可参考<ROS-坐标转换>。

    下面编写CMakeLists.txt:

cmake_minimum_required(VERSION 2.8.3)
project(ps_slam)

set(CMAKE_BUILD_TYPE Release)

add_compile_options(-std=c++11)

find_package(catkin REQUIRED COMPONENTS
  cv_bridge
  image_transport
  roscpp
  sensor_msgs
  std_msgs
  tf
  message_filters
)

#ORB_SLAM的路径
set(ORB_SLAM_PATH /home/chen/ros/ORB_SLAM2)
message("ORB_SLAM_PATH = ${ORB_SLAM_PATH} ")
LIST(APPEND CMAKE_MODULE_PATH ${ORB_SLAM_PATH}/cmake_modules)

find_package(OpenCV)
find_package(Eigen3 3.1.0 REQUIRED)
find_package(Pangolin REQUIRED)

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES serve_test
#  CATKIN_DEPENDS roscpp rospy std_msgs
#  DEPENDS system_lib
)

include_directories(
    include
    ${catkin_INCLUDE_DIRS}
    ${OpenCV_INCLUDE_DIRS}
    ${ORB_SLAM_PATH}
    ${ORB_SLAM_PATH}/include
    ${Pangolin_INCLUDE_DIRS}
)

set(LIBS 
    ${OpenCV_LIBS} 
    ${EIGEN3_LIBS}
    ${Pangolin_LIBRARIES}
    ${ORB_SLAM_PATH}/Thirdparty/DBoW2/lib/libDBoW2.so
    ${ORB_SLAM_PATH}/Thirdparty/g2o/lib/libg2o.so
    ${ORB_SLAM_PATH}/lib/libORB_SLAM2.so
     -lboost_system
)

add_executable(orb_slam_node src/orb_slam_run.cpp)

target_link_libraries(orb_slam_node 
    ${catkin_LIBRARIES} 
    ${LIBS}
)

    上面应该不用怎么解释。


三、运行并测试

    上面写好源代码后,在工作空间目录下进行编译:

catkin_make -j4

    然后刷新路径:

source devel/setup.bash

    接着把词典文件和配置文件准备好,ORB词典路径是ORB-SLAM2/Vocabulary/ORBvoc.txt,配置文件路径是ORB-SLAM2/Examples/ROS/ORB-SLAM2/Asus.yaml,我把它们放在工作空间的data目录下,跑起来:

#先开一个终端:
roscore

#在当前终端
rosrun ps_slam orb_slam_node data/ORBvoc.txt data/Asus.yaml

    接着运行数据集:

rosbag play /media/chen/chen/SLAM/datasets/rgbd_dataset_freiburg1_desk.bag \
/camera/rgb/image_color:=/camera/rgb/image_raw  \
/camera/depth/image:=/camera/depth_registered/image_raw

    上面运行数据包的时候,将发布节点名称 重映射到 我们上面程序中设置接收的名称。

    这个时候就可以看到ORB_SLAM2可视化界面出现地图点和关键帧了。此时再开一个终端,查看tf发布的情况:

rosrun tf tf_monitor

    即可看到orb_slam2存在一个坐标。再查看具体的内容:

 rosrun tf tf_echo world orb_slam2

    执行后可以看到orb_slam2估计的位姿情况。


四、Rviz显示

    可以通过rviz显示当前相机的运动情况,启动rviz:

ros rviz rviz

    然后通过Axis显示:

B.jpg





上一篇:
下一篇:

首页 所有文章 机器人 计算机视觉 自然语言处理 机器学习 编程随笔 关于