在博文<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显示：
