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 calibrationcv::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["LEFT.height"];int cols_l = fsSettings["LEFT.width"];int rows_r = fsSettings["RIGHT.height"];int cols_r = fsSettings["RIGHT.width"];if(K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() ||rows_l==0 || rows_r==0 || cols_l==0 || cols_r==0){cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << endl;return -1;}cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,igb.M1l,igb.M2l);cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,igb.M1r,igb.M2r);}ros::NodeHandle nh;message_filters::Subscriber<sensor_msgs::Image> left_sub(nh, "/zed2i/zed_node/left/image_rect_color", 1);message_filters::Subscriber<sensor_msgs::Image> right_sub(nh, "/zed2i/zed_node/right/image_rect_color", 1);typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_pol;message_filters::Synchronizer<sync_pol> sync(sync_pol(10), left_sub,right_sub);sync.registerCallback(boost::bind(&ImageGrabber::GrabStereo,&igb,_1,_2));ros::Publisher PosPub = nh.advertise<geometry_msgs::PoseStamped>("ORB_SLAM3/pose", 5);igb.pPosPub = &(PosPub);ros::spin();// Stop all threads// Save camera trajectorySLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory_TUM_Format.txt");SLAM.SaveTrajectoryTUM("FrameTrajectory_TUM_Format.txt");SLAM.SaveTrajectoryKITTI("FrameTrajectory_KITTI_Format.txt");SLAM.Shutdown();ros::shutdown();return 0;
}void ImageGrabber::GrabStereo(const sensor_msgs::ImageConstPtr& msgLeft,const sensor_msgs::ImageConstPtr& msgRight)
{// Copy the ros image message to cv::Mat.cv_bridge::CvImageConstPtr cv_ptrLeft;try{cv_ptrLeft = cv_bridge::toCvShare(msgLeft);}catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());return;}cv_bridge::CvImageConstPtr cv_ptrRight;try{cv_ptrRight = cv_bridge::toCvShare(msgRight);}catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());return;}if(do_rectify){cv::Mat Tcw;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);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);// mpSLAM->TrackStereo(imLeft,imRight,cv_ptrLeft->header.stamp.toSec());PublishPose(Tcw);// cout << "Tcw:" << Tcw.matrix() << endl;}else{cv::Mat Tcw;Sophus::SE3f Tcw_SE3f=mpSLAM->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec());Eigen::Matrix4f Tcw_Matrix = Tcw_SE3f.matrix();cv::eigen2cv(Tcw_Matrix, Tcw);// mpSLAM->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec());PublishPose(Tcw);// cout << "Tcw:" << Tcw.matrix() << endl;}}

相应的，ros_stereo_inertial.cc修改后代码如下：

/**
* This file is part of ORB-SLAM3
*
* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
*
* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public
* License as published by the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even
* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along with ORB-SLAM3.
* If not, see <http://www.gnu.org/licenses/>.
*/#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>
#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>
#include<vector>
#include<queue>
#include<thread>
#include<mutex>#include "geometry_msgs/PoseStamped.h"#include<ros/ros.h>
#include<cv_bridge/cv_bridge.h>
#include<sensor_msgs/Imu.h>
#include<opencv2/core/core.hpp>#include"../../../include/System.h"
#include"../include/ImuTypes.h"using namespace std;class ImuGrabber
{public:ImuGrabber(){};void GrabImu(const sensor_msgs::ImuConstPtr &imu_msg);queue<sensor_msgs::ImuConstPtr> imuBuf;std::mutex mBufMutex;
};class ImageGrabber
{public:ImageGrabber(ORB_SLAM3::System* pSLAM, ImuGrabber *pImuGb, const bool bRect, const bool bClahe): mpSLAM(pSLAM), mpImuGb(pImuGb), do_rectify(bRect), mbClahe(bClahe){}void PublishPose(cv::Mat Tcw);void GrabImageLeft(const sensor_msgs::ImageConstPtr& msg);void GrabImageRight(const sensor_msgs::ImageConstPtr& msg);cv::Mat GetImage(const sensor_msgs::ImageConstPtr &img_msg);void SyncWithImu();queue<sensor_msgs::ImageConstPtr> imgLeftBuf, imgRightBuf;std::mutex mBufMutexLeft,mBufMutexRight;ORB_SLAM3::System* mpSLAM;ImuGrabber *mpImuGb;const bool do_rectify;cv::Mat M1l,M2l,M1r,M2r;const bool mbClahe;cv::Ptr<cv::CLAHE> mClahe = cv::createCLAHE(3.0, cv::Size(8, 8));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();// Rwc与Twc分别代表的就是相机在世界坐标系下的旋转与平移cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3);vector<float> q = ORB_SLAM3::Converter::toQuaternion(Rwc);/*cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t();cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3);tf::Matrix3x3 M(Rwc.at<float>(0,0),Rwc.at<float>(0,1),Rwc.at<float>(0,2),Rwc.at<float>(1,0),Rwc.at<float>(1,1),Rwc.at<float>(1,2),Rwc.at<float>(2,0),Rwc.at<float>(2,1),Rwc.at<float>(2,2));tf::Vector3 V(twc.at<float>(0), twc.at<float>(1), twc.at<float>(2));tf::Transform tfTcw(M,V);//mTfBr.sendTransform(tf::StampedTransform(tfTcw,ros::Time::now(), "ORB_SLAM/World", "ORB_SLAM/Camera"));*/poseMSG.pose.position.x = - twc.at<float>(0);cout << "PublishPose position.x = " << poseMSG.pose.position.x << endl;poseMSG.pose.position.y = -twc.at<float>(2);cout << "PublishPose position.y = " << poseMSG.pose.position.y << endl;poseMSG.pose.position.z = twc.at<float>(1);cout << "PublishPose position.z = " << poseMSG.pose.position.z << endl;poseMSG.pose.orientation.x = q[0];cout << "PublishPose orientation.x = " << poseMSG.pose.orientation.x << endl;poseMSG.pose.orientation.y = q[1];cout << "PublishPose orientation.y = " << poseMSG.pose.orientation.y << endl;poseMSG.pose.orientation.z = q[2];cout << "PublishPose orientation.z = " << poseMSG.pose.orientation.z << endl;poseMSG.pose.orientation.w = q[3];cout << "PublishPose orientation.w = " << poseMSG.pose.orientation.w << endl;poseMSG.header.frame_id = "VSLAM";poseMSG.header.stamp = ros::Time::now();// poseMSG.pose.position.x = twc.at<float>(0);// cout << "PublishPose position.x = " << poseMSG.pose.position.x << endl;// poseMSG.pose.position.y = twc.at<float>(2);// cout << "PublishPose position.y = " << poseMSG.pose.position.y << endl;// poseMSG.pose.position.z = twc.at<float>(1);// cout << "PublishPose position.z = " << poseMSG.pose.position.z << endl;// poseMSG.pose.orientation.x = q[0];// cout << "PublishPose orientation.x = " << poseMSG.pose.orientation.x << endl;// poseMSG.pose.orientation.y = q[1];// cout << "PublishPose orientation.y = " << poseMSG.pose.orientation.y << endl;// poseMSG.pose.orientation.z = q[2];// cout << "PublishPose orientation.z = " << poseMSG.pose.orientation.z << endl;// poseMSG.pose.orientation.w = q[3];// cout << "PublishPose orientation.w = " << poseMSG.pose.orientation.w << endl;// poseMSG.header.frame_id = "VSLAM";// poseMSG.header.stamp = ros::Time::now();pPosPub->publish(poseMSG);// geometry_msgs::PoseStamped camera_pose;// camera_pose.header.frame_id="stereo";// camera_pose.header.stamp=ros::Time::now();// camera_pose.pose.position.x = poseMSG.pose.position.y;// camera_pose.pose.position.y = -poseMSG.pose.position.x;// camera_pose.pose.position.z =  poseMSG.pose.position.z;// camera_pose.pose.orientation = poseMSG.pose.orientation;// pPosPub->publish(camera_pose);//mlbLost.push_back(mState==LOST);}
}int main(int argc, char **argv)
{ros::init(argc, argv, "Stereo_Inertial");ros::start();ros::NodeHandle n("~");ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);bool bEqual = false;if(argc < 4 || argc > 5){cerr << endl << "Usage: rosrun ORB_SLAM3 Stereo_Inertial path_to_vocabulary path_to_settings do_rectify [do_equalize]" << endl;ros::shutdown();return 1;}std::string sbRect(argv[3]);if(argc==5){std::string sbEqual(argv[4]);if(sbEqual == "true")bEqual = true;}// 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::IMU_STEREO,true);ImuGrabber imugb;ImageGrabber igb(&SLAM,&imugb,sbRect == "true",bEqual);if(igb.do_rectify){      // Load settings related to stereo calibrationcv::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["LEFT.height"];int cols_l = fsSettings["LEFT.width"];int rows_r = fsSettings["RIGHT.height"];int cols_r = fsSettings["RIGHT.width"];if(K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() ||rows_l==0 || rows_r==0 || cols_l==0 || cols_r==0){cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << endl;return -1;}cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,igb.M1l,igb.M2l);cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,igb.M1r,igb.M2r);}// Maximum delay, 5 secondsros::Subscriber sub_imu = n.subscribe("/zed2i/zed_node/imu/data", 1000, &ImuGrabber::GrabImu, &imugb); ros::Subscriber sub_img_left = n.subscribe("/zed2i/zed_node/left/image_rect_color", 100, &ImageGrabber::GrabImageLeft,&igb);ros::Subscriber sub_img_right = n.subscribe("/zed2i/zed_node/right/image_rect_color", 100, &ImageGrabber::GrabImageRight,&igb);std::thread sync_thread(&ImageGrabber::SyncWithImu,&igb);ros::Publisher PosPub = n.advertise<geometry_msgs::PoseStamped>("ORB_SLAM3_imu/pose", 5);igb.pPosPub = &(PosPub);ros::spin();return 0;
}void ImageGrabber::GrabImageLeft(const sensor_msgs::ImageConstPtr &img_msg)
{mBufMutexLeft.lock();if (!imgLeftBuf.empty())imgLeftBuf.pop();imgLeftBuf.push(img_msg);mBufMutexLeft.unlock();
}void ImageGrabber::GrabImageRight(const sensor_msgs::ImageConstPtr &img_msg)
{mBufMutexRight.lock();if (!imgRightBuf.empty())imgRightBuf.pop();imgRightBuf.push(img_msg);mBufMutexRight.unlock();
}cv::Mat ImageGrabber::GetImage(const sensor_msgs::ImageConstPtr &img_msg)
{// Copy the ros image message to cv::Mat.cv_bridge::CvImageConstPtr cv_ptr;try{cv_ptr = cv_bridge::toCvShare(img_msg, sensor_msgs::image_encodings::MONO8);}catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());}if(cv_ptr->image.type()==0){return cv_ptr->image.clone();}else{std::cout << "Error type" << std::endl;return cv_ptr->image.clone();}
}void ImageGrabber::SyncWithImu()
{const double maxTimeDiff = 0.01;while(1){cv::Mat imLeft, imRight;double tImLeft = 0, tImRight = 0;if (!imgLeftBuf.empty()&&!imgRightBuf.empty()&&!mpImuGb->imuBuf.empty()){tImLeft = imgLeftBuf.front()->header.stamp.toSec();tImRight = imgRightBuf.front()->header.stamp.toSec();this->mBufMutexRight.lock();while((tImLeft-tImRight)>maxTimeDiff && imgRightBuf.size()>1){imgRightBuf.pop();tImRight = imgRightBuf.front()->header.stamp.toSec();}this->mBufMutexRight.unlock();this->mBufMutexLeft.lock();while((tImRight-tImLeft)>maxTimeDiff && imgLeftBuf.size()>1){imgLeftBuf.pop();tImLeft = imgLeftBuf.front()->header.stamp.toSec();}this->mBufMutexLeft.unlock();if((tImLeft-tImRight)>maxTimeDiff || (tImRight-tImLeft)>maxTimeDiff){// std::cout << "big time difference" << std::endl;continue;}if(tImLeft>mpImuGb->imuBuf.back()->header.stamp.toSec())continue;this->mBufMutexLeft.lock();imLeft = GetImage(imgLeftBuf.front());imgLeftBuf.pop();this->mBufMutexLeft.unlock();this->mBufMutexRight.lock();imRight = GetImage(imgRightBuf.front());imgRightBuf.pop();this->mBufMutexRight.unlock();vector<ORB_SLAM3::IMU::Point> vImuMeas;mpImuGb->mBufMutex.lock();if(!mpImuGb->imuBuf.empty()){// Load imu measurements from buffervImuMeas.clear();while(!mpImuGb->imuBuf.empty() && mpImuGb->imuBuf.front()->header.stamp.toSec()<=tImLeft){double t = mpImuGb->imuBuf.front()->header.stamp.toSec();cv::Point3f acc(mpImuGb->imuBuf.front()->linear_acceleration.x, mpImuGb->imuBuf.front()->linear_acceleration.y, mpImuGb->imuBuf.front()->linear_acceleration.z);cv::Point3f gyr(mpImuGb->imuBuf.front()->angular_velocity.x, mpImuGb->imuBuf.front()->angular_velocity.y, mpImuGb->imuBuf.front()->angular_velocity.z);vImuMeas.push_back(ORB_SLAM3::IMU::Point(acc,gyr,t));mpImuGb->imuBuf.pop();}}mpImuGb->mBufMutex.unlock();if(mbClahe){mClahe->apply(imLeft,imLeft);mClahe->apply(imRight,imRight);}if(do_rectify){cv::remap(imLeft,imLeft,M1l,M2l,cv::INTER_LINEAR);cv::remap(imRight,imRight,M1r,M2r,cv::INTER_LINEAR);}cv::Mat Tcw;Sophus::SE3f Tcw_SE3f=mpSLAM->TrackStereo(imLeft,imRight,tImLeft,vImuMeas);Eigen::Matrix4f Tcw_Matrix = Tcw_SE3f.matrix();cv::eigen2cv(Tcw_Matrix, Tcw);// mpSLAM->TrackStereo(imLeft,imRight,cv_ptrLeft->header.stamp.toSec());PublishPose(Tcw);std::chrono::milliseconds tSleep(1);std::this_thread::sleep_for(tSleep);}}
}void ImuGrabber::GrabImu(const sensor_msgs::ImuConstPtr &imu_msg)
{mBufMutex.lock();imuBuf.push(imu_msg);mBufMutex.unlock();return;
}

最后的牢骚
一开始看官网上说orb_slam3 V1.0新增了保存加载地图功能，但我运行一遍程序发现他并没有保存啥地图。到代码里搜savemap也搜不到，研究了好久才发现人家的函数名都变了，地图不叫map叫atlas。。。
还有orb_slam3发布也两年了，就是没有人给他加个发布位姿功能，反正github上我没发现，找到一个好像人家还写错了。也有可能已经有人改进了但就是没开源。。。
另外本人才疏学浅，c++才自学了几个月，如有错误还请指正，反正代码可以运行而且功能也是正常的。