实验设计 | ORB_SLAM2订阅amcl_pose话题

代码

//-- 头文件
#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>
#include<string>

#include<ros/ros.h>
#include "geometry_msgs/PoseStamped.h" 
#include <cv_bridge/cv_bridge.h>

#include<Eigen/Dense>

#include<opencv2/core/core.hpp>
#include <opencv2/core/eigen.hpp>

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

//-- 回调函数 新建回调函数
void GrabPose(const geometry_msgs::PoseStamped::ConstPtr & amclpose)
{
   
	cv::Mat amcl_R(3,3,CV_32F);
	cv::Mat amcl_t(3,1,CV_32F);
	Eigen::Matrix3d matrix_R;
	Eigen::Vector3d vector_t(amclpose->pose.position.x,
							 amclpose->pose<