借鉴
https://github.com/hoangthien94/ORB_SLAM2_CUDA
中发布位姿的方法

发布的msg 类型

发布了一种叫做PoseStamped的数据，官网上的说明文件：
docs.ros.org/melodic/api/geometry_msgs/html/msg/PoseStamped.html
包含两项：

std_msgs/Header header
geometry_msgs/Pose pose

改动ORB_SLAM

具体需要在原版的ORB_SLAM2中需要修改的地方：

添加一个类： SlamData

在上图所示位置，添加 SlamData.h SlamData.cpp
这两个文件由参考网站上得到。

由于我本次实验只想发布位姿，我舍去了原来代码中发布点云和当前帧的代码注视掉了。具体有：

SlamData.h：

/*
//#include <pcl/visualization/cloud_viewer.h> 
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>  
#include <pcl_conversions/pcl_conversions.h> 
#include <pcl_ros/transforms.h>
#include <pcl_ros/point_cloud.h>
....
//void PublishPointCloudForROS(void);
....
//void GetCurrentROSPointCloud(sensor_msgs::PointCloud2 &all_point_cloud, sensor_msgs::PointCloud2 &ref_point_cloud);
*/

SlamData.c：

    //all_point_cloud_pub = (*nodeHandler).advertise<sensor_msgs::PointCloud2>("point_cloud_all",1);
    //ref_point_cloud_pub = (*nodeHandler).advertise<sensor_msgs::PointCloud2>("point_cloud_ref",1);
...

/*
void SlamData::PublishPointCloudForROS(void)
{
    sensor_msgs::PointCloud2 allMapPoints;
    sensor_msgs::PointCloud2 referenceMapPoints;
    GetCurrentROSPointCloud(allMapPoints, referenceMapPoints);
    all_point_cloud_pub.publish(allMapPoints);
    ref_point_cloud_pub.publish(referenceMapPoints);
}

void SlamData::GetCurrentROSPointCloud(sensor_msgs::PointCloud2 &all_point_cloud, sensor_msgs::PointCloud2 &ref_point_cloud)
{
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_all( new pcl::PointCloud<pcl::PointXYZRGBA> );  
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_ref( new pcl::PointCloud<pcl::PointXYZRGBA> );     
    
    const std::vector<MapPoint*> &vpMPs = mpSLAM->GetmpMapAllMapPoints();
    const std::vector<MapPoint*> &vpRefMPs = mpSLAM->GetmpMapReferenceMapPoints();
    set<MapPoint*> spRefMPs(vpRefMPs.begin(), vpRefMPs.end());

    if(vpMPs.empty())
    {
        return;
    }
	
    for(size_t i=0, iend=vpMPs.size(); i<iend;i++)
    {
        if(vpMPs[i]->isBad() || spRefMPs.count(vpMPs[i]))
            continue;
        cv::Mat pos = vpMPs[i]->GetWorldPos();
        pcl::PointXYZRGBA p1;
        Eigen::Vector4f p1_temp, p1_temp_t;
        p1_temp(0) = pos.at<float>(0);
        p1_temp(1) = pos.at<float>(1);
        p1_temp(2) = pos.at<float>(2);
        p1_temp(3) = 1; 
        p1_temp_t = mTrans_cam2ground * p1_temp;	
        p1.x = p1_temp_t(0);
        p1.y = p1_temp_t(1);
        p1.z = p1_temp_t(2);
        p1.b = 255;
        p1.g = 255;
        p1.r = 255;
        p1.a = 255;
        cloud_all->points.push_back( p1 );
    }
    pcl::PCLPointCloud2 pcl_pc1;
    pcl::toPCLPointCloud2(*cloud_all, pcl_pc1);    // pcl::PointXYZRGBA -> pcl::PCLPointCloud2
    pcl_conversions::fromPCL(pcl_pc1, all_point_cloud);  // pcl::PCLPointCloud2 -> sensor_msgs::PointCloud2
    all_point_cloud.header.frame_id = "world";  
    all_point_cloud.header.stamp = ros::Time::now();   
  
    for(set<MapPoint*>::iterator sit=spRefMPs.begin(), send=spRefMPs.end(); sit!=send; sit++)
    {
        if((*sit)->isBad())
            continue;
        cv::Mat pos = (*sit)->GetWorldPos();
        pcl::PointXYZRGBA p2;
        Eigen::Vector4f p2_temp, p2_temp_t;
        p2_temp(0) = pos.at<float>(0);
        p2_temp(1) = pos.at<float>(1);
        p2_temp(2) = pos.at<float>(2);
        p2_temp(3) = 1;
        p2_temp_t = mTrans_cam2ground * p2_temp;	
        p2.x = p2_temp_t(0);
        p2.y = p2_temp_t(1);
        p2.z = p2_temp_t(2);
        p2.b = 0;
        p2.g = 0;
        p2.r = 255;
        p2.a = 255;
        cloud_ref->points.push_back( p2 );
    }

    pcl::PCLPointCloud2 pcl_pc2;
    pcl::toPCLPointCloud2(*cloud_ref, pcl_pc2); // pcl::PointXYZRGBA -> pcl::PCLPointCloud2
    pcl_conversions::fromPCL(pcl_pc2, ref_point_cloud);  // pcl::PCLPointCloud2 -> sensor_msgs::PointCloud2
    ref_point_cloud.header.frame_id = "world";
    ref_point_cloud.header.stamp = ros::Time::now(); 
}
*/
void SlamData::PublishCurrentFrameForROS(void)
{
    cv_bridge::CvImage cvi;
    cv::Mat img;
    cvi.header.frame_id = "frame";
    cvi.encoding = "bgr8";
    cvi.header.stamp = ros::Time::now();
/*
    if (mpFrameDrawer)
    {
        img = mpFrameDrawer->DrawFrame();
        // cv::imshow("Current Frame",img);
        // cv::waitKey(1e3/FPS/2);
        cvi.image = img;
        sensor_msgs::Image im;
        cvi.toImageMsg(im);
        current_frame_pub.publish(im);
    }*/
}

Example/ROS/ROS_SLAM2/CMakeList.txt

可执行文件中加入lib/SlamData.cpp

rosbuild_add_executable(Stereo
lib/SlamData.cpp
src/ros_stereo.cc
)

include 目录中加入 ${PROJECT_SOURCE_DIR}/lib

include_directories(
${PROJECT_SOURCE_DIR}
${PROJECT_SOURCE_DIR}/lib
${PROJECT_SOURCE_DIR}/../../../
${PROJECT_SOURCE_DIR}/../../../include
${Pangolin_INCLUDE_DIRS}
)

Example/ROS/ROS_SLAM2/src/ros_stereo.cc

实例化system 对象之后，实例化一个Slamdata 对象，并将刚刚新生的System 传递给它。

    ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::STEREO,true);
    ORB_SLAM2::SlamData SLAMDATA(&SLAM, &nodeHandler, true);
    ImageGrabber igb(&SLAM, &SLAMDATA);

修改 ImageGrabber ，通过它访问SlamData

class ImageGrabber
{
public:
    ImageGrabber(ORB_SLAM2::System* pSLAM, ORB_SLAM2::SlamData* pSLAMDATA)
    {
        mpSLAM = pSLAM;
        mpSLAMDATA = pSLAMDATA;        
    }

    void GrabStereo(const sensor_msgs::ImageConstPtr& msgLeft,const sensor_msgs::ImageConstPtr& msgRight);

    ORB_SLAM2::System* mpSLAM;
    bool do_rectify;
    cv::Mat M1l,M2l,M1r,M2r;
    ORB_SLAM2::SlamData* mpSLAMDATA;
};

记录下每次TrackStereo() 的返回值。
TrackStereo() 有返回值，为当前帧的位姿，之前原版的ORB_SLAM没有利用这个返回值。

    cv::Mat Tcw;
    if(do_rectify)
    {
        cv::Mat imLeft, imRight;
        cv::remap(cv_ptrLeft->image,imLeft,M1l,M2l,cv::INTER_LINEAR);
        cv::remap(cv_ptrRight->image,imRight,M1r,M2r,cv::INTER_LINEAR);
        Tcw = mpSLAM->TrackStereo(imLeft,imRight,cv_ptrLeft->header.stamp.toSec());
    }
    else
    {

        Tcw = mpSLAM->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec());
    }

记录下这个返回值之后，剩下的工作就是发布了。借鉴参考网站的操作，利用SLAMDATA 对象发布位姿信息。

    if (Tcw.empty()) {
      return;
    }
    cout << "mpSLAMDATA->EnablePublishROSTopics()" << mpSLAMDATA->EnablePublishROSTopics() << endl;
    if (mpSLAMDATA->EnablePublishROSTopics())
    {
        cout << "PublishTFForROS" << endl;
        mpSLAMDATA->PublishTFForROS(Tcw, cv_ptrLeft);
        cout << "PublishTFForROS" << endl;
        mpSLAMDATA->PublishPoseForROS(cv_ptrLeft);
        cout << "PublishPoseForROS" << endl;
        //mpSLAMDATA->PublishPointCloudForROS();
        cout << "PublishCurrentFrameForROS" << endl;
        mpSLAMDATA->PublishCurrentFrameForROS();
    }

订阅发布的位姿

我直接在同一个ORB_SLAM 中订阅发布的位姿
在 ros_stereo.cc 中，在定义完 ros::NodeHandle nh; 之后添加代码:

    ///////////////////////////////////////////
    string filename = "ROS_sub.txt";
    ofstream f;
    f.open(filename.c_str()); // delete previos data from last run.
    f.close();
    ros::Subscriber sub = nh.subscribe("MyPose", 1, chatterCallback);
    //////////////////////////////////

其中，文件打开和关闭的操作是为了清除原有的内容。
再写个函数chatterCallback：

void chatterCallback(const geometry_msgs::PoseStamped::ConstPtr& msg) //Note it is geometry_msgs::PoseStamped, not std_msgs::PoseStamped
{
    /*
    ROS_INFO("I heard the pose from the robot"); 
    ROS_INFO("the position(x,y,z) is %f , %f, %f", msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
    ROS_INFO("the orientation(x,y,z,w) is %f , %f, %f, %f", msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, msg->pose.orientation.w);
    ROS_INFO("the time we get the pose is %f",  msg->header.stamp.sec + 1e-9*msg->header.stamp.nsec);

    std::cout<<"\n \n"<<std::endl; //add two more blank row so that we can see the message more clearly*/
    string filename = "ROS_sub.txt";
    ofstream f;
    f.open(filename.c_str(), ios::app);
    f << fixed;
    f  << msg->header.stamp.sec << "." << msg->header.stamp.nsec << " " <<
      setprecision(9) <<  msg->pose.position.x << " " <<  msg->pose.position.y << " " <<  msg->pose.position.z << " " <<
       msg->pose.orientation.x << " " << msg->pose.orientation.y << " " << msg->pose.orientation.z << " " << msg->pose.orientation.w << endl;
    f.close();


}

ios::app 使得输出文字在原文件后面，不覆盖。

f.open(filename.c_str(), ios::app);

由于我打算將得到位姿信息和保存的FrameTrajectory_TUM_Format.txt 做对比，所以，输出的格式参照 System.cc 中的 SaveTrajectoryTUM()

运行和结果

在 ORN_SLAM 路径下运行euroc/V1_01_easy.bag：

rosrun ORB_SLAM2 Stereo Vocabulary/ORBvoc.txt Examples/Stereo/EuRoC.yaml true
rosbag play /media/txt/txtDisk2/euroc/V1_01_easy.bag /cam0/image_raw:=/camera/left/image_raw /cam1/image_raw:=/camera/right/image_raw

运行一个可视化文件，查看关键帧的轨迹和订阅得到的轨迹：

#!/usr/bin/env python3
#-*-coding:utf-8-*-

from tvtk.api import tvtk
import mayavi.mlab as mlab
import numpy as np

# 返回嵌套列表，每个元素都是一个float类型
def parseFile(filepath):
    with open(filepath) as f:
        data=f.read()
        lines=data.split('\n')
        list=[  [float(v.strip()) for v in line.split(" ") if v.strip()!=""]  for line in lines if len(line)>0]
    return np.array(list)


def plotTraj(points1,points2):
    x1 = points1[:, 1]
    y1 = points1[:, 2]
    z1 = points1[:, 3]
    x2 = points2[:, 1]
    y2 = points2[:, 2]
    z2 = points2[:, 3]
    mlab.plot3d(x1, y1, z1,
                color=(1, 0, 0),    #red=points1
                line_width=0.0002,  #green=points2
                )
    mlab.plot3d(x2, y2, z2,
                color=(0, 1, 0),
                line_width=0.0002,
                )
    mlab.show()

def addPlotTraj(mlab,points,colortype):
    x = points[:, 1]
    y = points[:, 2]
    z = points[:, 3]
    mlab.plot3d(x, y, z,
                color=colortype,    #red=points1
                line_width=0.0002,  #green=points2
                )
    return mlab

if __name__=='__main__':
    lego_np=parseFile("/home/txt/slam/ORB_SLAM2-master/KeyFrameTrajectory_TUM_Format.txt")    #red
    lego_hm_np=parseFile("/home/txt/slam/ORB_SLAM2-master/ROS_sub.txt")    #green
    #orb_np=parseFile("/home/atom/Scripts/PythonVisual/scene12/stereoorb/KeyFrameTrajectory.txt")     #blue


    addPlotTraj(mlab,lego_np,(1,0,0))
    addPlotTraj(mlab, lego_hm_np, (0, 1, 0))
    #addPlotTraj(mlab, orb_np, (0, 0, 1))
    mlab.show()

得到结果：

可见订阅结果正确。

ORB_SLAM2 双目ros 发布实时位姿