ORB-SLAM2发布位姿ROS话题

最新推荐文章于 2023-04-26 21:01:52 发布

Mr.Qin_

最新推荐文章于 2023-04-26 21:01:52 发布

阅读量3.4k

点赞数 13

分类专栏： SLAM ROS 文章标签：自动驾驶计算机视觉人工智能

本文链接：https://blog.csdn.net/qinqinxiansheng/article/details/108288646

版权

ROS 同时被 2 个专栏收录

16 篇文章 15 订阅

订阅专栏

SLAM

13 篇文章 19 订阅

订阅专栏

问题描述

在ORB-SLAM2中，如何发布位姿和轨迹的ros话题，并在Rviz显示

思考

ORB-SLAM2中的位姿信息由下列函数返回，将其转换为所需的数据类型发布出去即可

mpSLAM->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec())

参考：https://github.com/raulmur/ORB_SLAM2/issues/597

实现

1 引入所需的数据类型：

#include<tf/transform_broadcaster.h>
#include "../../../include/Converter.h"
#include <nav_msgs/Path.h>

2 定义和声明发布者

ros::Publisher pose_pub;
nav_msgs::Path stereo_path;
ros::Publisher stereo_path_pub;

 pose_pub = nh.advertise<geometry_msgs::PoseStamped>("ORB_SLAM/pose", 5);//
 stereo_path_pub = nh.advertise<nav_msgs::Path>("ORB_SLAM/path",10);//

3 发布位姿

 geometry_msgs::PoseStamped pose;
 pose.header.stamp = ros::Time::now();
 pose.header.frame_id ="path";

 cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t(); // Rotation information
 cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3); // translation information
 vector<float> q = ORB_SLAM2::Converter::toQuaternion(Rwc);

 tf::Transform new_transform;
 new_transform.setOrigin(tf::Vector3(twc.at<float>(0, 0), twc.at<float>(0, 1), twc.at<float>(0, 2)));
 tf::Quaternion quaternion(q[0], q[1], q[2], q[3]);
 new_transform.setRotation(quaternion);
 tf::poseTFToMsg(new_transform, pose.pose);
 pose_pub.publish(pose);

4 运行结果
打开rviz，按照topic添加要显示的内容，这里为path与pose

在这里插入图片描述
运行orbslam2，即可看到运动轨迹与当前位姿势

在这里插入图片描述

完整代码

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

#include<tf/transform_broadcaster.h>///
#include "../../../include/Converter.h"///
#include <nav_msgs/Path.h>///

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

using namespace std;

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

    void GrabStereo(const sensor_msgs::ImageConstPtr& msgLeft,const sensor_msgs::ImageConstPtr& msgRight);

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

ros::Publisher pose_pub;
nav_msgs::Path stereo_path;
ros::Publisher stereo_path_pub;

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

    if(argc != 4)
    {
        cerr << endl << "Usage: rosrun ORB_SLAM2 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_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::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["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, "/camera/left/image_raw", 1);
    //message_filters::Subscriber<sensor_msgs::Image> right_sub(nh, "camera/right/image_raw", 1);
    message_filters::Subscriber<sensor_msgs::Image> left_sub(nh, "/stereo_depth_perception/rectified_vga_front_left_image", 1);
    message_filters::Subscriber<sensor_msgs::Image> right_sub(nh, "/stereo_depth_perception/rectified_vga_front_right_image", 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));
    pose_pub = nh.advertise<geometry_msgs::PoseStamped>("ORB_SLAM/pose", 5);//
    stereo_path_pub = nh.advertise<nav_msgs::Path>("ORB_SLAM/path",10);//


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

    // Save camera trajectory
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory_TUM_Format.txt");
    SLAM.SaveTrajectoryTUM("FrameTrajectory_TUM_Format.txt");
    SLAM.SaveTrajectoryKITTI("FrameTrajectory_KITTI_Format.txt");

    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 imLeft, imRight;
        cv::remap(cv_ptrLeft->image,imLeft,M1l,M2l,cv::INTER_LINEAR);
        cv::remap(cv_ptrRight->image,imRight,M1r,M2r,cv::INTER_LINEAR);
        mpSLAM->TrackStereo(imLeft,imRight,cv_ptrLeft->header.stamp.toSec()).clone();

    }
    else
    {
        cv::Mat Tcw;
        Tcw = mpSLAM->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec());

        geometry_msgs::PoseStamped pose;
        pose.header.stamp = ros::Time::now();
        pose.header.frame_id ="path";

        cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t(); // Rotation information
        cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3); // translation information
        vector<float> q = ORB_SLAM2::Converter::toQuaternion(Rwc);

        tf::Transform new_transform;
        new_transform.setOrigin(tf::Vector3(twc.at<float>(0, 0), twc.at<float>(0, 1), twc.at<float>(0, 2)));
        tf::Quaternion quaternion(q[0], q[1], q[2], q[3]);
        new_transform.setRotation(quaternion);
        tf::poseTFToMsg(new_transform, pose.pose);
        pose_pub.publish(pose);
        
        stereo_path.header.frame_id="path";
        stereo_path.header.stamp=ros::Time::now();
        stereo_path.poses.push_back(pose);
        stereo_path_pub.publish(stereo_path);
    }
}