之前调试了ORB_SLAM2的gazebo仿真,现在需要在ROS中使用到ORB_SLAM2的位姿,但是ORB_SLAM2本身是没有位姿的ROS话题输出的,参考了github上相关问题的探讨:Get Pose Information from ORBSLAM最后成功得到了ORB_SLAM2运行时机器人的位姿反馈。
分析源代码最终会发现对于ORB_SLAM2而言,位姿由以下处理函数返回:
mpSLAM->TrackStereo(imLeft,imRight,cv_ptrLeft->header.stamp.toSec());
所以我们可以调用该处理函数:
Tcw = mpSLAM->TrackStereo(imLeft,imRight,cv_ptrLeft->header.stamp.toSec());
这里Tcw为定义的一个矩阵:
cv::Mat Tcw;
返回得到一个4x4的矩阵信息,该矩阵包含了三个旋转分量以及三个平移分量。我们对该矩阵进行处理并发布出最后的位姿信息:
geometry_msgs::PoseStamped pose;
pose.header.stamp = ros::Time::now();
pose.header.frame_id ="map";
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);
注意这里使用了publish需要在前面声明:
ros::Publisher pose_pub = nh.advertise<geometry_msgs::PoseStamped>("orb_pose", 100);
以及几个相关的头文件:
#include <geometry_msgs/PoseStamped.h>
#include <tf/tf.h>
#include <tf/transform_datatypes.h>
#include"../../../include/Converter.h"
所有的这些程序添加在ros_stereo.cc文件中。最后修改完的文件如下:
#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>
#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"
#include <geometry_msgs/PoseStamped.h> //modify
#include <tf/tf.h>
#include <tf/transform_datatypes.h>
#include"../../../include/Converter.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;
};
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);
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));
ros::Publisher pose_pub = nh.advertise<geometry_msgs::PoseStamped>("orb_pose", 100);//modify
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());
}
else
{
cv::Mat Tcw;//modify
//mpSLAM->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec());
Tcw = mpSLAM->TrackStereo(imLeft,imRight,cv_ptrLeft->header.stamp.toSec());
geometry_msgs::PoseStamped pose;
pose.header.stamp = ros::Time::now();
pose.header.frame_id ="map";
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);
}
}