ORB-SLAM3使用压缩图（ROS）

ORB-SLAM3中使用的是/camera/image_raw，这是一种无损的图片格式，通常使用D435等相机采集图片的时候，会导致rosbag包巨大。因此，在实际中，会采集压缩格式的。

具体以ros中的ros_mono.cc中为例子

代码修改

#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>

#include<ros/ros.h>
#include <cv_bridge/cv_bridge.h>

#include<opencv2/core/core.hpp>

#include"../../../include/System.h"

using namespace std;

class ImageGrabber
{
public:
    ImageGrabber(ORB_SLAM3::System* pSLAM):mpSLAM(pSLAM){}

    void GrabImage(const sensor_msgs::CompressedImageConstPtr& msg);

    ORB_SLAM3::System* mpSLAM;
};

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

    if(argc != 3)
    {
        cerr << endl << "Usage: rosrun ORB_SLAM3 Mono path_to_vocabulary path_to_settings" << 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::MONOCULAR,true);

    ImageGrabber igb(&SLAM);

    ros::NodeHandle nodeHandler;
    ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, &ImageGrabber::GrabImage,&igb);

    ros::spin();

    // Stop all threads
    SLAM.Shutdown();

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

    ros::shutdown();

    return 0;
}

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

    mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec());
}

注意，我们修改了原来的void GrabImage(const sensor_msgs::ImageConstPtr& msg);中的ImageConstPtr为CompressedImageConstPtr。下面的函数实现也是需要修改的，要将原有的cv_bridge::CvImageConstPtr cv_ptr;修改为cv_bridge::CvImagePtr cv_ptr;；其次，将cv_ptr = cv_bridge::toCvShare(msg);修改为cv_ptr = cv_bridge::toCvCopy(msg,sensor_msgs::image_encodings::BGR8);OK了。

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

    mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec());
}

参考

https://blog.youkuaiyun.com/guanjing_dream/article/details/129716750