ORB-SLAM3读取压缩图ros节点程序备忘

代码备忘,在原本的ORBSLAM3 ROS节点程序中新增以下功能:

1. 将图片的ROS消息格式改为压缩图片类型
2. 增加地图保存的ros服务,地图保存以前在结束程序是自动保存,现在通过ros服务节点随时保存rosservice call /save_map 1
3. 增加纯定位模式和SLAM模式切换的ros服务节点(废物功能)

/**
* 
* Adapted from ORB-SLAM3: Examples/ROS/src/ros_stereo.cc
*
*/

#include "common.h"

using namespace std;

class ImageGrabber
{
public:
    ImageGrabber(ORB_SLAM3::System* pSLAM):mpSLAM(pSLAM)
    {
        // lusx add 0927
        localization_mode = false;//默认处于slam模式
    }

    void GrabStereo(const sensor_msgs::CompressedImageConstPtr& msgLeft, const sensor_msgs::CompressedImageConstPtr& msgRight);
    
    // lusx add 0927
    bool SaveMap(std_srvs::SetBool::Request& map_request, std_srvs::SetBool::Response& map_response);
    bool ChangeSlamMode(std_srvs::SetBool::Request& mode_request, std_srvs::SetBool::Response& mode_response);

    bool do_rectify;
    ORB_SLAM3::System* mpSLAM;
    cv::Mat M1l,M2l,M1r,M2r;

    // lusx add 0927 
    bool localization_mode;//添加使用rosservice发送纯定位模式切换功能

    // lusx add 1010
    std::vector<double> vTrackTime;
};

int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "Stereo");
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
    if (argc > 1)
    {
        ROS_WARN ("Arguments supplied via command line are ignored.");
    }

    ros::NodeHandle node_handler;
    std::string node_name = ros::this_node::getName();
    // 获得词袋向量和配置文件路径
    std::string voc_file, settings_file;
    node_handler.param<std::string>(node_name + "/voc_file", voc_file, "file_not_set");
    node_handler.param<std::string>(node_name + "/settings_file", settings_file, "file_not_set");

    bool isVOMode;
    node_handler.param<bool>(node_name + "/isVOMode", isVOMode, "false");

    if (voc_file == "file_not_set" || settings_file == "file_not_set")
    {
        ROS_ERROR("Please provide voc_file and settings_file in the launch file");       
        ros::shutdown();
        return 1;
    } 
    // 设置地图和位姿对应的FrameID
    node_handler.param<std::string>(node_name + "/map_frame_id", map_frame_id, "map");
    node_handler.param<std::string>(node_name + "/pose_frame_id", pose_frame_id, "pose");
    
    // Create SLAM system. It initializes all system threads and gets ready to process frames.
    // 创建和初始化SLAM系统
    ORB_SLAM3::System SLAM(voc_file, settings_file, ORB_SLAM3::System::STEREO,true,isVOMode);

    // 抓取RGB图像
    ImageGrabber igb(&SLAM);
    node_handler.param<bool>(node_name + "/do_rectify", igb.do_rectify, "false");
    
    // 加载双目相机remap的参数(如果需要remap的话)
    cv::FileStorage fsSettings(settings_file, cv::FileStorage::READ);
    if(!fsSettings.isOpened())
    {
        cerr << "ERROR: Wrong path to settings" << endl;
        return -1;
    }

    if(igb.do_rectify)
    {
        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);
    }

    // lusx add 0927
    ros::ServiceServer map_service = node_handler.advertiseService("/save_map", &ImageGrabber::SaveMap, &igb);
    ros::ServiceServer mode_service = node_handler.advertiseService("/change_slam_mode", &ImageGrabber::ChangeSlamMode, &igb);

    // 左右相机时间戳同步
    message_filters::Subscriber<sensor_msgs::CompressedImage> left_sub(node_handler, "/camera/left/image_raw", 1);
    message_filters::Subscriber<sensor_msgs::CompressedImage> right_sub(node_handler, "/camera/right/image_raw", 1);
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::CompressedImage, sensor_msgs::CompressedImage> sync_pol;
    message_filters::Synchronizer<sync_pol> sync(sync_pol(10), left_sub, right_sub);
    sync.registerCallback(boost::bind(&ImageGrabber::GrabStereo,&igb,_1,_2));

    // 初始化位姿和地图点的publisher
    pose_pub = node_handler.advertise<geometry_msgs::PoseStamped> ("/orb_slam3_ros/camera", 1);
    map_points_pub = node_handler.advertise<sensor_msgs::PointCloud2>("orb_slam3_ros/map_points", 1);
    // 将ORBSLAM的坐标转为ROS里面的tf格式
    setup_tf_orb_to_ros(ORB_SLAM3::System::STEREO);

    ros::spin();
    // Stop all threads
    SLAM.Shutdown();

    // 保存轨迹
    string resultDir="null";
    node_handler.param<string>(node_name + "/resultDir", resultDir, "null");
    if(resultDir!="null")
    {
        SLAM.SaveTrajectoryEuRoC(resultDir);
    }

    // lusx add 1010,计算平均跟踪时间
    double totalTime=0;
    for(int i=0;i<igb.vTrackTime.size();i++)
        totalTime+=igb.vTrackTime[i];
    if(igb.vTrackTime.size() !=0)
        std::cout<<"**Track time average cost "<<totalTime/igb.vTrackTime.size()<<" ms"<<std::endl;

    ros::shutdown();

    return 0;
}

void ImageGrabber::GrabStereo(const sensor_msgs::CompressedImageConstPtr& msgLeft,const sensor_msgs::CompressedImageConstPtr& msgRight)
{
    // Copy the ros image message to cv::Mat.
    cv_bridge::CvImagePtr cv_ptrLeft;
    try
    {
        cv_ptrLeft = cv_bridge::toCvCopy(msgLeft,sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    cv_bridge::CvImagePtr cv_ptrRight;
    try
    {
        cv_ptrRight = cv_bridge::toCvCopy(msgRight,sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    cv::Rect roi_ = cv::Rect(279, 179, 720, 360);

    cv::Mat left_, right_;
    // left_ = cv_ptrLeft->image.clone();
    cv::resize(cv_ptrLeft->image.clone(), left_, cv::Size(1280, 720), 1);
    left_ = left_(roi_).clone();
    // left_ = cv_ptrLeft->image(roi_).clone();
    //right_ = cv_ptrRight->image(roi_).clone();
    cv::resize(cv_ptrRight->image.clone(), right_, cv::Size(1280, 720), 1);
    right_= right_(roi_).clone();

    // Main algorithm runs here
    Sophus::SE3f Tcw_SE3f;
    // lusx add 0913 计算跟踪一张图像的时间
    std::chrono::steady_clock::time_point time_StartPosePred = std::chrono::steady_clock::now();

    // Tcw_SE3f = mpSLAM->TrackStereo(left_,right_,cv_ptrLeft->header.stamp.toSec());
    if(do_rectify)
    {
        cv::Mat imLeft, imRight;
        cv::remap(left_,imLeft,M1l,M2l,cv::INTER_LINEAR);
        cv::remap(right_,imRight,M1r,M2r,cv::INTER_LINEAR);

        Tcw_SE3f = mpSLAM->TrackStereo(imLeft,imRight,cv_ptrLeft->header.stamp.toSec());
    }
    else
    {
        Tcw_SE3f = mpSLAM->TrackStereo(left_,right_,cv_ptrLeft->header.stamp.toSec());
    }

    // lusx add 0913 计算跟踪一张图像的时间
    std::chrono::steady_clock::time_point time_EndPosePred = std::chrono::steady_clock::now();
    double timePosePred = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndPosePred - time_StartPosePred).count();
    vTrackTime.push_back(timePosePred);

    //Sophus::SE3f Tcw_SE3f = mpSLAM->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec());
    cv::Mat Tcw = SE3f_to_cvMat(Tcw_SE3f);
    ros::Time current_frame_time = cv_ptrLeft->header.stamp;

    publish_ros_pose_tf(Tcw, current_frame_time, ORB_SLAM3::System::STEREO);
    publish_ros_tracking_mappoints(mpSLAM->GetTrackedMapPoints(), current_frame_time);

    // publish_ros_tracking_img(mpSLAM->GetCurrentFrame(), current_frame_time);
}

// lusx add 0927
// save_map的服务请求回调函数，cmd:rosservice call /save_map 1
bool ImageGrabber::SaveMap(std_srvs::SetBool::Request& map_request, std_srvs::SetBool::Response& map_response)
{
    if(map_request.data)
    {
        //start_save_map = true;
        usleep(5e5);
        mpSLAM->ActivateLocalizationMode();//保存地图时会短暂的进入纯定位模式
        mpSLAM->SaveAtlas(1);//1表示将地图保存为.soa的格式
        map_response.success = 1;
        map_response.message  = "map saved successed !!!";
        mpSLAM->DeactivateLocalizationMode();//保存结束后会再自动切换为SLAM模式
        //start_save_map = false;
    }
    return true;
}

// lusx add 0927
// slam mode change的回调函数，slam->vo保存地图+模式转换;vo->slam模式转换
bool ImageGrabber::ChangeSlamMode(std_srvs::SetBool::Request& mode_request, std_srvs::SetBool::Response& mode_response)
{
    if(mode_request.data)
    {
        if(localization_mode)//当前是纯定位模式
        {
            mpSLAM->DeactivateLocalizationMode();//转换为SLAM模式
            //mpSLAM->Reset();
            localization_mode = false;
            mode_response.success = 1;
            mode_response.message  = "change to mapping mode successed !!!";
        }
        else// 当前是slam模式
        {
            // 转换为定位模式
            // 这里转换完成后，在SLAM的核心模块中还会有一步判断，因为选择重定位模式时
            // 默认会加载先验地图，所以在进行模式转换时会检测地图是否已经成功merge
            // 只有在地图成功merge时才会切换到纯定位模式
            mpSLAM->ActivateLocalizationMode();
            localization_mode = true;
            mode_response.success = 1;
            mode_response.message  = "change to localization mode successed !!!";
        }
    }

    return true;
}