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