1.保存/加載地圖

先說(shuō)方法：在加載的相機(jī)參數(shù)文件.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"的地圖到本地。第一次運(yùn)行建圖時(shí)注釋掉第一行，只使用第二行，加載地圖重定位時(shí)反過(guò)來(lái)，親測(cè)同時(shí)使用會(huì)報(bào)錯(cuò)

2.發(fā)布位姿

以雙目節(jié)點(diǎn)ros_stereo.cc為例修改。計(jì)算的位姿由mpSLAM->TrackStereo函數(shù)返回，返回類(lèi)型是Sophus::SE3f，弄一個(gè)變量Tcw_SE3f接受返回值：

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

Sophus::SE3f不知道是啥，也不知道怎么解析提取想要的數(shù)據(jù)，但是cv::Mat格式的我知道怎么做，于是將Sophus::SE3f轉(zhuǎn)換成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);

這時(shí)候Tcw就是cv::Mat格式了。
然后將Tcw發(fā)布：

PublishPose(Tcw);

構(gòu)造發(fā)布器：

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

PublishPose函數(shù)中，解析提取數(shù)據(jù)部分關(guān)鍵代碼如下：

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);

完整代碼如下：文章來(lái)源地址http://www.zghlxwxcb.cn/news/detail-614534.html

#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[

到了這里，關(guān)于orb_slam3實(shí)現(xiàn)保存/加載地圖功能and發(fā)布位姿功能的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！