读取kitti数据集并显示groundtruth是检验自己slam算法的必须品。
读取rosbag里的tf::tfMessage就是groundtruth。
数据下载:
链接: https://pan.baidu.com/s/1JD2G4f29pyn--x5RBMpYkg 提取码: hx9y
下载后使用下面代码进行合并之后解压(百度云盘不让上传4G以上的文件。。)
cat 2011_09_30_0027.bag.tar.gz* > 2011_09_30_0027.bag.tar.gz
可以使用info读取到包内的数据结构第一个就是tf::tfMessage
读取tf::tfMessage数据需要在CMakelist和package.xml里添加tf
find_package(catkin REQUIRED COMPONENTS
tf
)
<build_depend>tf</build_depend>
在sub数据时注意名字。
ros::Subscriber tf_sub = n.subscribe("/tf", 100, tf_Callhandle);
在会掉函数时注意结构。tansforms是一个vector,第0号是正确位姿,后面都没用。具体结构可以看tf/tfMessage 和 tf 树 理解
nav_msgs::Path groundPath;
void tf_Callhandle(const tf::tfMessage msg_tf)
{
//输出轨迹
nav_msgs::Odometry laserOdometry;
laserOdometry.header.frame_id = "map";
laserOdometry.child_frame_id = "map_child";
laserOdometry.header.stamp = msg_tf.transforms[0].header.stamp;
laserOdometry.pose.pose.orientation.x = msg_tf.transforms[0].transform.rotation.x;
laserOdometry.pose.pose.orientation.y = msg_tf.transforms[0].transform.rotation.y;
laserOdometry.pose.pose.orientation.z = msg_tf.transforms[0].transform.rotation.z;
laserOdometry.pose.pose.orientation.w = msg_tf.transforms[0].transform.rotation.w;
laserOdometry.pose.pose.position.x = msg_tf.transforms[0].transform.translation.x;
laserOdometry.pose.pose.position.y = msg_tf.transforms[0].transform.translation.y;
laserOdometry.pose.pose.position.z = msg_tf.transforms[0].transform.translation.z;
geometry_msgs::PoseStamped laserPose;
laserPose.header = laserOdometry.header;
laserPose.pose = laserOdometry.pose.pose;
groundPath.header.stamp = laserOdometry.header.stamp;
groundPath.poses.push_back(laserPose);
groundPath.header.frame_id = "map";
pubgroundPath.publish(groundPath);
}