借鉴
https://github.com/hoangthien94/ORB_SLAM2_CUDA
中发布位姿的方法
发布的msg 类型
发布了一种叫做PoseStamped的数据,官网上的说明文件:
docs.ros.org/melodic/api/geometry_msgs/html/msg/PoseStamped.html
包含两项:
std_msgs/Header header
geometry_msgs/Pose pose
改动ORB_SLAM
具体需要在原版的ORB_SLAM2中需要修改的地方:
添加一个类: SlamData
在上图所示位置,添加 SlamData.h SlamData.cpp
这两个文件由参考网站上得到。
由于我本次实验只想发布位姿,我舍去了原来代码中发布点云和当前帧的代码注视掉了。具体有:
- SlamData.h:
/*
//#include <pcl/visualization/cloud_viewer.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl_ros/point_cloud.h>
....
//void PublishPointCloudForROS(void);
....
//void GetCurrentROSPointCloud(sensor_msgs::PointCloud2 &all_point_cloud, sensor_msgs::PointCloud2 &ref_point_cloud);
*/
- SlamData.c:
//all_point_cloud_pub = (*nodeHandler).advertise<sensor_msgs::PointCloud2>("point_cloud_all",1);
//ref_point_cloud_pub = (*nodeHandler).advertise<sensor_msgs::PointCloud2>("point_cloud_ref",1);
...
/*
void SlamData::PublishPointCloudForROS(void)
{
sensor_msgs::PointCloud2 allMapPoints;
sensor_msgs::PointCloud2 referenceMapPoints;
GetCurrentROSPointCloud(allMapPoints, referenceMapPoints);
all_point_cloud_pub.publish(allMapPoints);
ref_point_cloud_pub.publish(referenceMapPoints);
}
void SlamData::GetCurrentROSPointCloud(sensor_msgs::PointCloud2 &all_point_cloud, sensor_msgs::PointCloud2 &ref_point_cloud)
{
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_all( new pcl::PointCloud<pcl::PointXYZRGBA> );
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_ref( new pcl::PointCloud<pcl::PointXYZRGBA> );
const std::vector<MapPoint*> &vpMPs = mpSLAM->GetmpMapAllMapPoints();
const std::vector<MapPoint*> &vpRefMPs = mpSLAM->GetmpMapReferenceMapPoints();
set<MapPoint*> spRefMPs(vpRefMPs.begin(), vpRefMPs.end());
if(vpMPs.empty())
{
return;
}
for(size_t i=0, iend=vpMPs.size(); i<iend;i++)
{
if(vpMPs[i]->isBad() || spRefMPs.count(vpMPs[i]))
continue;
cv::Mat pos = vpMPs[i]->GetWorldPos();
pcl::PointXYZRGBA p1;
Eigen::Vector4f p1_temp, p1_temp_t;
p1_temp(0) = pos.at<float>(0);
p1_temp(1) = pos.at<float>(1);
p1_temp(2) = pos.at<float>(2);
p1_temp(3) = 1;
p1_temp_t = mTrans_cam2ground * p1_temp;
p1.x = p1_temp_t(0);
p1.y = p1_temp_t(1);
p1.z = p1_temp_t(2);
p1.b = 255;
p1.g = 255;
p1.r = 255;
p1.a = 255;
cloud_all->points.push_back( p1 );
}
pcl::PCLPointCloud2 pcl_pc1;
pcl::toPCLPointCloud2(*cloud_all, pcl_pc1); // pcl::PointXYZRGBA -> pcl::PCLPointCloud2
pcl_conversions::fromPCL(pcl_pc1, all_point_cloud); // pcl::PCLPointCloud2 -> sensor_msgs::PointCloud2
all_point_cloud.header.frame_id = "world";
all_point_cloud.header.stamp = ros::Time::now();
for(set<MapPoint*>::iterator sit=spRefMPs.begin(), send=spRefMPs.end(); sit!=send; sit++)
{
if((*sit)->isBad())
continue;
cv::Mat pos = (*sit)->GetWorldPos();
pcl::PointXYZRGBA p2;
Eigen::Vector4f p2_temp, p2_temp_t;
p2_temp(0) = pos.at<float>(0);
p2_temp(1) = pos.at<float>(1);
p2_temp(2) = pos.at<float>(2);
p2_temp(3) = 1;
p2_temp_t = mTrans_cam2ground * p2_temp;
p2.x = p2_temp_t(0);
p2.y = p2_temp_t(1);
p2.z = p2_temp_t(2);
p2.b = 0;
p2.g = 0;
p2.r = 255;
p2.a = 255;
cloud_ref->points.push_back( p2 );
}
pcl::PCLPointCloud2 pcl_pc2;
pcl::toPCLPointCloud2(*cloud_ref, pcl_pc2); // pcl::PointXYZRGBA -> pcl::PCLPointCloud2
pcl_conversions::fromPCL(pcl_pc2, ref_point_cloud); // pcl::PCLPointCloud2 -> sensor_msgs::PointCloud2
ref_point_cloud.header.frame_id = "world";
ref_point_cloud.header.stamp = ros::Time::now();
}
*/
void SlamData::PublishCurrentFrameForROS(void)
{
cv_bridge::CvImage cvi;
cv::Mat img;
cvi.header.frame_id = "frame";
cvi.encoding = "bgr8";
cvi.header.stamp = ros::Time::now();
/*
if (mpFrameDrawer)
{
img = mpFrameDrawer->DrawFrame();
// cv::imshow("Current Frame",img);
// cv::waitKey(1e3/FPS/2);
cvi.image = img;
sensor_msgs::Image im;
cvi.toImageMsg(im);
current_frame_pub.publish(im);
}*/
}
Example/ROS/ROS_SLAM2/CMakeList.txt
- 可执行文件中加入lib/SlamData.cpp
rosbuild_add_executable(Stereo
lib/SlamData.cpp
src/ros_stereo.cc
)
- include 目录中加入 ${PROJECT_SOURCE_DIR}/lib
include_directories(
${PROJECT_SOURCE_DIR}
${PROJECT_SOURCE_DIR}/lib
${PROJECT_SOURCE_DIR}/../../../
${PROJECT_SOURCE_DIR}/../../../include
${Pangolin_INCLUDE_DIRS}
)
Example/ROS/ROS_SLAM2/src/ros_stereo.cc
- 实例化system 对象之后,实例化一个Slamdata 对象,并将刚刚新生的System 传递给它。
ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::STEREO,true);
ORB_SLAM2::SlamData SLAMDATA(&SLAM, &nodeHandler, true);
ImageGrabber igb(&SLAM, &SLAMDATA);
- 修改 ImageGrabber , 通过它访问SlamData
class ImageGrabber
{
public:
ImageGrabber(ORB_SLAM2::System* pSLAM, ORB_SLAM2::SlamData* pSLAMDATA)
{
mpSLAM = pSLAM;
mpSLAMDATA = pSLAMDATA;
}
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;
ORB_SLAM2::SlamData* mpSLAMDATA;
};
- 记录下每次TrackStereo() 的返回值。
TrackStereo() 有返回值,为当前帧的位姿,之前原版的ORB_SLAM没有利用这个返回值。
cv::Mat Tcw;
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);
Tcw = mpSLAM->TrackStereo(imLeft,imRight,cv_ptrLeft->header.stamp.toSec());
}
else
{
Tcw = mpSLAM->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec());
}
- 记录下这个返回值之后,剩下的工作就是发布了。借鉴参考网站的操作,利用SLAMDATA 对象发布位姿信息。
if (Tcw.empty()) {
return;
}
cout << "mpSLAMDATA->EnablePublishROSTopics()" << mpSLAMDATA->EnablePublishROSTopics() << endl;
if (mpSLAMDATA->EnablePublishROSTopics())
{
cout << "PublishTFForROS" << endl;
mpSLAMDATA->PublishTFForROS(Tcw, cv_ptrLeft);
cout << "PublishTFForROS" << endl;
mpSLAMDATA->PublishPoseForROS(cv_ptrLeft);
cout << "PublishPoseForROS" << endl;
//mpSLAMDATA->PublishPointCloudForROS();
cout << "PublishCurrentFrameForROS" << endl;
mpSLAMDATA->PublishCurrentFrameForROS();
}
订阅发布的位姿
我直接在同一个ORB_SLAM 中订阅发布的位姿
在 ros_stereo.cc 中,在定义完 ros::NodeHandle nh; 之后添加代码:
///////////////////////////////////////////
string filename = "ROS_sub.txt";
ofstream f;
f.open(filename.c_str()); // delete previos data from last run.
f.close();
ros::Subscriber sub = nh.subscribe("MyPose", 1, chatterCallback);
//////////////////////////////////
其中,文件打开和关闭的操作是为了清除原有的内容。
再写个函数chatterCallback:
void chatterCallback(const geometry_msgs::PoseStamped::ConstPtr& msg) //Note it is geometry_msgs::PoseStamped, not std_msgs::PoseStamped
{
/*
ROS_INFO("I heard the pose from the robot");
ROS_INFO("the position(x,y,z) is %f , %f, %f", msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
ROS_INFO("the orientation(x,y,z,w) is %f , %f, %f, %f", msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, msg->pose.orientation.w);
ROS_INFO("the time we get the pose is %f", msg->header.stamp.sec + 1e-9*msg->header.stamp.nsec);
std::cout<<"\n \n"<<std::endl; //add two more blank row so that we can see the message more clearly*/
string filename = "ROS_sub.txt";
ofstream f;
f.open(filename.c_str(), ios::app);
f << fixed;
f << msg->header.stamp.sec << "." << msg->header.stamp.nsec << " " <<
setprecision(9) << msg->pose.position.x << " " << msg->pose.position.y << " " << msg->pose.position.z << " " <<
msg->pose.orientation.x << " " << msg->pose.orientation.y << " " << msg->pose.orientation.z << " " << msg->pose.orientation.w << endl;
f.close();
}
ios::app 使得输出文字在原文件后面,不覆盖。
f.open(filename.c_str(), ios::app);
由于我打算將得到位姿信息和保存的FrameTrajectory_TUM_Format.txt 做对比,所以, 输出的格式参照 System.cc 中的 SaveTrajectoryTUM()
运行和结果
在 ORN_SLAM 路径下运行euroc/V1_01_easy.bag:
rosrun ORB_SLAM2 Stereo Vocabulary/ORBvoc.txt Examples/Stereo/EuRoC.yaml true
rosbag play /media/txt/txtDisk2/euroc/V1_01_easy.bag /cam0/image_raw:=/camera/left/image_raw /cam1/image_raw:=/camera/right/image_raw
运行一个可视化文件,查看关键帧的轨迹和订阅得到的轨迹:
#!/usr/bin/env python3
#-*-coding:utf-8-*-
from tvtk.api import tvtk
import mayavi.mlab as mlab
import numpy as np
# 返回嵌套列表,每个元素都是一个float类型
def parseFile(filepath):
with open(filepath) as f:
data=f.read()
lines=data.split('\n')
list=[ [float(v.strip()) for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0]
return np.array(list)
def plotTraj(points1,points2):
x1 = points1[:, 1]
y1 = points1[:, 2]
z1 = points1[:, 3]
x2 = points2[:, 1]
y2 = points2[:, 2]
z2 = points2[:, 3]
mlab.plot3d(x1, y1, z1,
color=(1, 0, 0), #red=points1
line_width=0.0002, #green=points2
)
mlab.plot3d(x2, y2, z2,
color=(0, 1, 0),
line_width=0.0002,
)
mlab.show()
def addPlotTraj(mlab,points,colortype):
x = points[:, 1]
y = points[:, 2]
z = points[:, 3]
mlab.plot3d(x, y, z,
color=colortype, #red=points1
line_width=0.0002, #green=points2
)
return mlab
if __name__=='__main__':
lego_np=parseFile("/home/txt/slam/ORB_SLAM2-master/KeyFrameTrajectory_TUM_Format.txt") #red
lego_hm_np=parseFile("/home/txt/slam/ORB_SLAM2-master/ROS_sub.txt") #green
#orb_np=parseFile("/home/atom/Scripts/PythonVisual/scene12/stereoorb/KeyFrameTrajectory.txt") #blue
addPlotTraj(mlab,lego_np,(1,0,0))
addPlotTraj(mlab, lego_hm_np, (0, 1, 0))
#addPlotTraj(mlab, orb_np, (0, 0, 1))
mlab.show()
得到结果:
可见订阅结果正确。