orb_slam3实现保存/加载地图功能and发布位姿功能

1.保存/加载地图

先说方法：在加载的相机参数文件.yaml的最前面加上下面两行就行。

System.LoadAtlasFromFile: "MH01_to_MH05_stereo_inertial.osa"
System.SaveAtlasToFile: "MH01_to_MH05_stereo_inertial.osa" 

第一行表示从本地加载名为"MH01_to_MH05_stereo_inertial.osa"的地图文件，第二行表示保存名为"MH01_to_MH05_stereo_inertial.osa"的地图到本地。第一次运行建图时注释掉第一行，只使用第二行，加载地图重定位时反过来，亲测同时使用会报错

2.发布位姿

以双目节点ros_stereo.cc为例修改。计算的位姿由mpSLAM->TrackStereo函数返回，返回类型是Sophus::SE3f，弄一个变量Tcw_SE3f接受返回值：

Sophus::SE3f Tcw_SE3f=mpSLAM->TrackStereo(imLeft,imRight,cv_ptrLeft->header.stamp.toSec());

Sophus::SE3f不知道是啥，也不知道怎么解析提取想要的数据，但是cv::Mat格式的我知道怎么做，于是将Sophus::SE3f转换成cv::Mat：

cv::Mat Tcw;
Sophus::SE3f Tcw_SE3f=mpSLAM->TrackStereo(imLeft,imRight,cv_ptrLeft->header.stamp.toSec());
Eigen::Matrix4f Tcw_Matrix = Tcw_SE3f.matrix();
cv::eigen2cv(Tcw_Matrix, Tcw);

这时候Tcw就是cv::Mat格式了。
然后将Tcw发布：

PublishPose(Tcw);

构造发布器：

ros::Publisher PosPub = nh.advertise<geometry_msgs::PoseStamped>("ORB_SLAM3/pose", 5);

PublishPose函数中，解析提取数据部分关键代码如下：

geometry_msgs::PoseStamped poseMSG;
    if(!Tcw.empty())
    {
   
        cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t();
        cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3);
        vector<float> q = ORB_SLAM3::Converter::toQuaternion(Rwc);
        poseMSG.pose.position.x = - twc.at<float>(0);
        poseMSG.pose.position.y = -twc.at<float>(2);
        poseMSG.pose.position.z = twc.at<float>(1);
        poseMSG.pose.orientation.x = q[0];
        poseMSG.pose.orientation.y = q[1];
        poseMSG.pose.orientation.z = q[2];
        poseMSG.pose.orientation.w = q[3];
        poseMSG.header.frame_id = "VSLAM";
        poseMSG.header.stamp = ros::Time::now();
        pPosPub->publish(poseMSG);

完整代码如下：

#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>
#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>
#include<ros/ros.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<opencv2/core/core.hpp>
#include"../../../include/System.h"
#include <tf/transform_broadcaster.h>
#include "Converter.h"

using namespace std;

class ImageGrabber
{
   
public:
    ImageGrabber(ORB_SLAM3::System* pSLAM):mpSLAM(pSLAM){
   }
    void GrabStereo(const sensor_msgs::ImageConstPtr& msgLeft,const sensor_msgs::ImageConstPtr& msgRight);
    void PublishPose(cv::Mat Tcw);
    ORB_SLAM3::System* mpSLAM;
    bool do_rectify;
    cv::Mat M1l,M2l,M1r,M2r;
    ros::Publisher* pPosPub;
};

void ImageGrabber::PublishPose(cv::Mat Tcw)
{
   
    geometry_msgs::PoseStamped poseMSG;
    if(!Tcw.empty())
    {
   

        cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t();
        cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3);
        vector<float> q = ORB_SLAM3::Converter::toQuaternion(Rwc);
        poseMSG.pose.position.x = - twc.at<float>(0);
        poseMSG.pose.position.y = -twc.at<float>(2);
        poseMSG.pose.position.z = twc.at<float>(1);
        poseMSG.pose.orientation.x = q[0];
        poseMSG.pose.orientation.y = q[1];
        poseMSG.pose.orientation.z = q[2];
        poseMSG.pose.orientation.w = q[3];
        poseMSG.header.frame_id = "VSLAM";
        poseMSG.header.stamp = ros::Time::now();
        pPosPub->publish(poseMSG);
    }
}

int main(int argc, char **argv)
{
   
    ros::init(argc, argv, "RGBD");
    ros::start();

    if(argc != 4)
    {
   
        cerr << endl << "Usage: rosrun ORB_SLAM3 Stereo path_to_vocabulary path_to_settings do_rectify" << endl;
        ros::shutdown();
        return 1;
    }    

    // Create SLAM system. It initializes all system threads and gets ready to process frames.
    ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::STEREO,true);

    ImageGrabber igb(&SLAM);

    stringstream ss(argv[3]);
	ss >> boolalpha >> igb.do_rectify;

    if(igb.do_rectify)
    {
         
        // Load settings related to stereo calibration
        cv::FileStorage fsSettings(argv[2], cv::FileStorage::READ);
        if(!fsSettings.isOpened())
        {
   
            cerr << "ERROR: Wrong path to settings" << endl;
            return -1;
        }

        cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r;
        fsSettings["LEFT.K"] >> K_l;
        fsSettings["RIGHT.K"] >> K_r;

        fsSettings["LEFT.P"] >> P_l;
        fsSettings["RIGHT.P"] >> P_r;

        fsSettings["LEFT.R"] >> R_l;
        fsSettings["RIGHT.R"] >> R_r;

        fsSettings["LEFT.D"] >> D_l;
        fsSettings["RIGHT.D"] >> D_r;

        int rows_l = fsSettings[