ORB_SLAM2保存地图详细流程(ros版本)

1.开发背景
首先,之前提到过的博主最近换了工作,以后打算努力学习slam,之前做激光嘛,当然,也是个菜鸟,现在突然做视觉SLAM-orbslam,接触起来的第一感觉是:哇,好大一个工程。反正既然做了,就认真学嘛,项目老哥要求把地图保存并加载,所以,这一篇就记录一下保存地图的流程。

2. 软硬件环境
传感器:azure-kinect-sdk,驱动用的是v1.4(当初搭建这个ros版本的驱动可真是折磨死我了,不过后来总结了一下经验,搭建起来也不是太费劲了,需要的话,后面我再写一篇讲搭建linux下ros驱动的过程)
开发环境:ros-melodic,ubuntu18.04(虚拟机),RGBD_ROS
视觉框架:orb_slam3(目前初步成功了,只是在orbslam2上,后面还是要移到orbslam3上的,这里就记录在orbslam2上改动的流程)

3.Let’s go!!!
先上图吧!看一下大致场景,下一篇博客讲重定位流程。

这是之前我们自己采的数据跑出来的图,室内的话还好,rgb+depimg,不过加上Imu的版本才是最稳定的,这个图就是我最终保存下来的,下一篇加载出来的也是它,下篇再讲加载的。

- 修改Map.h和Map.cc
修改Map.h

//头文件需要导入Converter.h
#include "Converter.h"
 
public:
    //保存地图信息函数
    void Save(const string &filename);
    //获取地图点ID
    void GetMapPointsIdx();
    
protected:
    //添加成员变量,这个私有公有都可以
    std::map<MapPoint*, unsigned long int> mmpnMapPointsIdx;
     //保存地图点和关键帧 
    void SaveMapPoint( ofstream &f, MapPoint* mp);
    void SaveKeyFrame( ofstream &f, KeyFrame* kf);
 

修改Map.cc

//保存地图信息
void Map::Save ( const string& filename )
{
    //Print the information of the saving map
    cerr<<"Map.cc :: Map Saving to "<<filename <<endl;
    ofstream f;
    f.open(filename.c_str(), ios_base::out|ios::binary);
 
    //Number of MapPoints
    unsigned long int nMapPoints = mspMapPoints.size();
    f.write((char*)&nMapPoints, sizeof(nMapPoints) );
    //Save MapPoint sequentially
    for ( auto mp: mspMapPoints ){
        //Save MapPoint
        SaveMapPoint( f, mp );
        // cerr << "Map.cc :: Saving map point number: " << mp->mnId << endl;
    }
 
    //Print The number of MapPoints
    cerr << "Map.cc :: The number of MapPoints is :"<<mspMapPoints.size()<<endl;
        
 
    //Grab the index of each MapPoint, count from 0, in which we initialized mmpnMapPointsIdx  
    GetMapPointsIdx(); 
 
    //Print the number of KeyFrames
    cerr <<"Map.cc :: The number of KeyFrames:"<<mspKeyFrames.size()<<endl;
    //Number of KeyFrames
    unsigned long int nKeyFrames = mspKeyFrames.size();
    f.write((char*)&nKeyFrames, sizeof(nKeyFrames));
 
    //Save KeyFrames sequentially
    for ( auto kf: mspKeyFrames )
        SaveKeyFrame( f, kf );
 
    for (auto kf:mspKeyFrames )
    {
        //Get parent of current KeyFrame and save the ID of this parent
        KeyFrame* parent = kf->GetParent();
        unsigned long int parent_id = ULONG_MAX;
        if ( parent )
            parent_id = parent->mnId;
        f.write((char*)&parent_id, sizeof(parent_id));
 
        //Get the size of the Connected KeyFrames of the current KeyFrames
        //and then save the ID and weight of the Connected KeyFrames
        unsigned long int nb_con = kf->GetConnectedKeyFrames().size();
        f.write((char*)&nb_con, sizeof(nb_con));
        for ( auto ckf: kf->GetConnectedKeyFrames())
        {
            int weight = kf->GetWeight(ckf);
            f.write((char*)&ckf->mnId, sizeof(ckf->mnId));
            f.write((char*)&weight, sizeof(weight));
        }
    }
 
    // Save last Frame ID
    // SaveFrameID(f);
 
    f.close();
    cerr<<"Map.cc :: Map Saving Finished!"<<endl;
}
 
//保存地图点
void Map::SaveMapPoint( ofstream& f, MapPoint* mp)
{   
    //Save ID and the x,y,z coordinates of the current MapPoint
    f.write((char*)&mp->mnId, sizeof(mp->mnId));
    cv::Mat mpWorldPos = mp->GetWorldPos();
    f.write((char*)& mpWorldPos.at<float>(0),sizeof(float));
    f.write((char*)& mpWorldPos.at<float>(1),sizeof(float));
    f.write((char*)& mpWorldPos.at<float>(2),sizeof(float));
}
 
//保存关键帧
void Map::SaveKeyFrame( ofstream &f, KeyFrame* kf )
{
    //Save the ID and timesteps of current KeyFrame
    f.write((char*)&kf->mnId, sizeof(kf->mnId));
    // cout << "saving kf->mnId = " << kf->mnId <<endl;
    f.write((char*)&kf->mTimeStamp, sizeof(kf->mTimeStamp));
    //Save the Pose Matrix of current KeyFrame
    cv::Mat Tcw = kf->GetPose();
 
    Save the rotation matrix
    // for ( int i = 0; i < Tcw.rows; i ++ )
    // {
    //     for ( int j = 0; j < Tcw.cols; j ++ )
    //     {
    //         f.write((char*)&Tcw.at<float>(i,j), sizeof(float));
    //         //cerr<<"Tcw.at<float>("<<i<<","<<j<<"):"<<Tcw.at<float>(i,j)<<endl;
    //     }
    // }
 
    //Save the rotation matrix in Quaternion
    std::vector<float> Quat = Converter::toQuaternion(Tcw);
    for ( int i = 0; i < 4; i ++ )
        f.write((char*)&Quat[i],sizeof(float));
    //Save the translation matrix
    for ( int i = 0; i < 3; i ++ )
        f.write((char*)&Tcw.at<float>(i,3),sizeof(float));
 
 
 
 
    //Save the size of the ORB features current KeyFrame
    //cerr<<"kf->N:"<<kf->N<<endl;
    f.write((char*)&kf->N, sizeof(kf->N));
    //Save each ORB features
    for( int i = 0; i < kf->N; i ++ )
    {
        cv::KeyPoint kp = kf->mvKeys[i];
        f.write((char*)&kp.pt.x, sizeof(kp.pt.x));
        f.write((char*)&kp.pt.y, sizeof(kp.pt.y));
        f.write((char*)&kp.size, sizeof(kp.size));
        f.write((char*)&kp.angle,sizeof(kp.angle));
        f.write((char*)&kp.response, sizeof(kp.response));
        f.write((char*)&kp.octave, sizeof(kp.octave));
 
        //Save the Descriptors of current ORB features
        f.write((char*)&kf->mDescriptors.cols, sizeof(kf->mDescriptors.cols)); //kf->mDescriptors.cols is always 32 here.
        for (int j = 0; j < kf->mDescriptors.cols; j ++ )
            f.write((char*)&kf->mDescriptors.at<unsigned char>(i,j), sizeof(char));
 
        //Save the index of MapPoints that corresponds to current ORB features
        unsigned long int mnIdx;
        MapPoint* mp = kf->GetMapPoint(i);
        if (mp == NULL  )
            mnIdx = ULONG_MAX;
        else
            mnIdx = mmpnMapPointsIdx[mp];
 
        f.write((char*)&mnIdx, sizeof(mnIdx));
    }
 
    // Save BoW for relocalization.
    // f.write((char*)&kf->mBowVec, sizeof(kf->mBowVec));
}
 
//获取地图点ID
void Map::GetMapPointsIdx()
{
    unique_lock<mutex> lock(mMutexMap);
    unsigned long int i = 0;
    for ( auto mp: mspMapPoints )
    {
        mmpnMapPointsIdx[mp] = i;
        i += 1;
    }
}
 

- 修改Converter.h和Converter.cc
修改Converter.h
添加函数声明:

public:
static cv::Mat Converter::toCvMat(const std::vector<float>& v);

修改Converter.cc
添加函数定义:

cv::Mat toCvMat(const std::vector<float>& v)
{
    Eigen::Quaterniond q;
    q.x() = v[0];
    q.y() = v[1];
    q.z() = v[2];
    q.w() = v[3];
 
    Eigen::Matrix<double,3,3> eigMat(q);
    cv::Mat M = toCvMat(eigMat);
    return M;
}

- 修改System.cc和System.h
修改System.h

添加函数声明:

public:
void SaveMap(const string &filename); 

修改System.cc
添加函数定义:

//地图保存
void System::SaveMap(const string &filename)
{
    mpMap->Save(filename);
}

- 入口程序修改

我用的是rgbd_ros,所以我修改的也是Example/ROS/ORBSLAM2下的ros_rgbd.cc文件:

    // Save camera trajectory
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
    SLAM.SaveMap("MapPointandKeyFrame.bin");

地图保存倒还好,没什么坑,ros::spin()和SLAM.Shutdown()的存在注定了这个地图的保存只有在你按ctrl+c的时候才进行地图保存。当然,你可以把地图保存这句话放在ros::spin或者SLAM.Shutdown()的前面,但是,建图卡死也是分分钟的事。保存完成后会在运行目录下多一个文件,如图:
在这里插入图片描述
也要注意按下ctrl+c时终端打印的信息,因为SaveMap函数中加了很多打印来提示地图保存到哪一步了,做的时候看一下终端也大致可以判断出来是否保存成功了!

参考文献:(感谢各位大神)
Felaim
达达MFZ

  • 10
    点赞
  • 82
    收藏
    觉得还不错? 一键收藏
  • 23
    评论
### 回答1: 要订阅usb_cam并使用orb_slam_2_ros,你需要在ROS中使用以下命令:"rosrun orb_slam_2_ros orb_slam_2_ros __name:=<orb_slam_2> __params:=<path to orb_slam_2_ros.yaml> __image:=/usb_cam/image_raw __camera_info:=/usb_cam/camera_info". ### 回答2: 要使用orb_slam_2_ros订阅usb_cam,您可以按照以下步骤进行操作: 1. 首先,确保已经安装了rosorb_slam_2_ros软件包。您可以使用以下命令进行安装: ``` sudo apt-get install ros-<your_ros_version>-usb-cam sudo apt-get install ros-<your_ros_version>-rtabmap-ros ``` 请将`<your_ros_version>`替换为您正在使用的ROS版本。 2. 创建一个新的ROS工作空间并进行初始化: ``` mkdir -p ~/catkin_ws/src cd ~/catkin_ws/src catkin_init_workspace ``` 3. 克隆orb_slam_2_ros软件包到src目录下: ``` git clone https://github.com/appliedAI-Initiative/orb_slam_2_ros.git ``` 4. 回到catkin_ws并进行构建: ``` cd ~/catkin_ws catkin_make ``` 5. 设置摄像头参数。打开`~/catkin_ws/src/orb_slam_2_ros/launch/usb_cam_v2_2.launch`文件,并根据您的USB相机配置修改`video_device`,`image_width`和`image_height`等参数。 6. 启动usb_cam节点: ``` roslaunch orb_slam_2_ros usb_cam_v2_2.launch ``` 7. 启动ORB-SLAM2节点: ``` roslaunch orb_slam_2_ros orb_slam2_ros.launch ``` 现在,您应该能够订阅usb_cam并使用ORB-SLAM2进行定位和建图。要查看ORB-SLAM2的输出,您可以使用rviz或image_view等工具。 ### 回答3: 使用ORB_SLAM2_ROS订阅USB_CAM的方法如下: 首先,确保按照ORB_SLAM2_ROS的安装说明正确安装了ROSORB_SLAM2。 1. 在终端中,打开新的工作区并创建一个新的工作空间: ``` mkdir -p orb_cam_ws/src cd orb_cam_ws/src ``` 2. 将ORB_SLAM2_ROS软件包克隆到工作空间的src文件夹中: ``` git clone https://github.com/raulmur/ORB_SLAM2.git ``` 3. 编译ORB_SLAM2_ROS软件包: ``` cd .. catkin_make ``` 4. 打开orb_slam2_ros.launch文件: ``` roscd orb_slam2_ros/launch gedit orb_slam2_ros.launch ``` 5. 在文件中添加以下行: ``` <arg name="camera_topic" default="/usb_cam/image_raw"/> <arg name="camera_info_topic" default="/usb_cam/camera_info"/> ``` 6. 保存并关闭文件。 7. 运行ORB_SLAM2_ROS节点: ``` roslaunch orb_slam2_ros orb_slam2_ros.launch ``` 现在,ORB_SLAM2_ROS节点将订阅名为/usb_cam/image_raw的图像主题和/usb_cam/camera_info的摄像机信息主题。您可以在ORB_SLAM2_ROS节点发布的话题中查看三维重构的结果。 请注意,在运行ORB_SLAM2_ROS之前,您需要先启动USB_CAM节点以发布相机图像和相机信息主题。可以使用以下命令启动USB_CAM节点: ``` roslaunch usb_cam usb_cam-test.launch ``` 这样就可以成功订阅USB_CAM并使用ORB_SLAM2_ROS进行视觉定位和三维重建。
评论 23
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值