将ORB编译成ROS工程后，就可以利用ROS的节点通信实现ORB和REMODE的数据传输。

步骤：

1 仿照SVO-REMODE的编译方式。安装googletest 和rpg_open_remode至～/catkin_ws/src 目录下。

2 修改 ORB的主函数，以便发送数据。

2.1 加入头文件

#include
    
           #include
     
       #include
      
       //以上几个头文件是从程序外从参数用的，需要下载vikit源码#include
       
        #include 
        
         #include 
         
          //以上3个头文件是发送图片用的#include
          
           #include
           
            #include"std_msgs/String.h"#include"sstream"#include"DenseInput.h"//这个头文件 说明了传输数据的结构，需要从svo工程svo_msgs里的拷出来。#include 
            
             //位姿矩阵转四元素#include 
             
              #include 
              
             
            
           
          
         
        
       
      
     
    

2.2 在main函数中完成通信

 

以下修改在main函数中

开启ros节点，vk::getParam才能找到launch文件的参数

ros::init(argc,argv,"ORB_SLAM");   ros::NodeHandle SendMessage;// 发布   ros::Publisher pub_dense_=SendMessage.advertise
    
     ("dense_input",10);   //利用vikit读取参数    const string dataset_path(vk::getParam
     
      ("ORB_SLAM/Img_Path"));    const string CamPara_Path(vk::getParam
      
       ("ORB_SLAM/CamPara_Path"));    const string BOW_Path(vk::getParam
       
        ("ORB_SLAM/BOW_Path"));
       
      
     
    

发送数据：每执行一次slam，发送一次数据。数据包括 

 

1 frame的id

2 左图

3 四元数格式的位姿

4 当前场景的最大最小深度。

 

/与REMODE通信//	svo_msgs::DenseInput msg;	msg.header.stamp=ros::Time(20);//svo中设置20可以正常运行	msg.header.frame_id="/world";	msg.frame_id=framecnt;	cv_bridge::CvImage img_msg;	img_msg.header.stamp=msg.header.stamp;	img_msg.header.frame_id="camera";	img_msg.image=imRGB;	img_msg.encoding = sensor_msgs::image_encodings::MONO8;	msg.image = *img_msg.toImageMsg();	double min_z = std::numeric_limits
    
     ::max();	double max_z = std::numeric_limits
     
      ::min();	SLAM.mpTracker->getSceneDepth(SLAM.mpTracker->mCurrentFrame,max_z,min_z);	cout<<"min_z-----------------------------------------  "<
      
       <
       
        mCurrentFrame.mTcw.inv();	cv::Mat RWC=TWC.rowRange(0,3).colRange(0,3);	cv::Mat tWC=TWC.rowRange(0,3).col(3);	vector
        
          q=ORB_SLAM2::Converter::toQuaternion(RWC);	    msg.pose.position.x = tWC.at
         
          (0,0); msg.pose.position.y = tWC.at
          
           (1,0); msg.pose.position.z = tWC.at
           
            (2,0); msg.pose.orientation.w = q[3];//q.w(); msg.pose.orientation.x = q[0];//q.x(); msg.pose.orientation.y = q[1];//q.y(); msg.pose.orientation.z = q[2];//q.z(); pub_dense_.publish(msg);
           
          
         
        
       
      
     
    

 

opencv的cv::Mat  和ros中的sensor_msgs/Image 需要cv_bridge::CvImage  做个转换。

用法：http://wiki.ros.org/cv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages

3 仿照

写一个脚本，目的是在运行工程的时候方便，快捷。脚本内容为

 

gnome-terminal -x bash -c "roscore"gnome-terminal -x bash -c "rosrun rviz rviz -d /home/baohua/project/SLAM/REMODE_ORB/catkin_ws/src/rpg_open_remode/open_remode.rviz  "gnome-terminal -x bash -c " source '/home/baohua/project/SLAM/REMODE_ORB/catkin_ws/devel/setup.sh';roslaunch ORB_SLAM MH01.launch "gnome-terminal -x bash -c " source '/home/baohua/project/SLAM/REMODE_ORB/catkin_ws/devel/setup.sh';roslaunch rpg_open_remode remode_EuRoC.launch "

至此,运行脚本就可以运行REMODE+ORBSLAM工程了。