参考网址:

(128条消息) 如何获得gazebo仿真中的机器人位姿真值_寒墨阁的博客-CSDN博客_gazebo获取机器人位置

(128条消息) evo安装、evo使用方法详细介绍使用教程

(128条消息) evo评测VINS-MONO—代码修改、数据格式转换、数据测试_可即的博客-CSDN博客_vins测试

(128条消息) evo测评TUM数据集_dididada~的博客-CSDN博客_evo tum

gazebo获得真实位姿

1
2
3
4
5
6
7
8
9
10
11
12
13
<!-- 在gazebo仿真模型文件中添加： -->
<gazebo>
  <plugin name="p3d_base_controller" filename="libgazebo_ros_p3d.so">
    <alwaysOn>true</alwaysOn>
    <updateRate>50.0</updateRate>
    <bodyName>base_footprint</bodyName>
    <topicName>base_pose_ground_truth</topicName>
    <gaussianNoise>0.0</gaussianNoise>
    <frameName>map</frameName>
    <xyzOffsets>0 0 0</xyzOffsets>
    <rpyOffsets>0 0 0</rpyOffsets>
  </plugin>
</gazebo>

ros生成tum文件

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
#include <ros/ros.h>
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "nav_msgs/Odometry.h"

#include <fstream>
#include <string.h>
#include <sstream>
#include <iomanip>

class EvoCreatTum
{
public:
    EvoCreatTum();
    ~EvoCreatTum();
    void poseCallback(const nav_msgs::Odometry::ConstPtr& msg);
private:
    ros::NodeHandle nh_;
    ros::Subscriber sub_;
    std::string odom_topic_;
    nav_msgs::Odometry pose_;
    std::string outFilename_;
};

EvoCreatTum::EvoCreatTum()
{
    ros::Time::init();
    nh_.param<std::string>("/odom_topic", odom_topic_, "/odom");
    nh_.param<std::string>("outFilename", outFilename_, "/root/catkin_ws/devel/lib/evo_creat_tum/outpose.txt");
    sub_ = nh_.subscribe(odom_topic_, 1, &EvoCreatTum::poseCallback, this);
}


EvoCreatTum::~EvoCreatTum()
{
}

void EvoCreatTum::poseCallback(const nav_msgs::Odometry::ConstPtr &msg)
{
    ros::Time currTime = ros::Time::now();
    double t_s = currTime.toSec();
    // std::cout << std::fixed << std::setprecision(4) << "t_s: " << t_s << std::endl;
    pose_ = *msg;
    double tx = pose_.pose.pose.position.x;
    double ty = pose_.pose.pose.position.y;
    double tz = pose_.pose.pose.position.z;
    double qx = pose_.pose.pose.orientation.x;
    double qy = pose_.pose.pose.orientation.y;
    double qz = pose_.pose.pose.orientation.z;
    double qw = pose_.pose.pose.orientation.w;
    std::fstream outTumpose;
    // 打开并向后添加
    outTumpose.open(outFilename_, std::ios::out | std::ios::app);
    outTumpose << std::fixed << std::setprecision(4) << t_s << " " << std::fixed << std::setprecision(6) << tx << " " << ty << " " << tz << " " << qx << " " << qy << " " << qz << " " << qw << std::endl;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "evo_creat_tum_node"); // 节点的名字
    EvoCreatTum evo_creat_tum;
    ros::spin(); // 程序执行到此处时开始进行等待，每次订阅的消息到来都会执行一次ScanCallback()
    return 0;
}
1
2
3
4
5
6
<launch>
    <!-- creat_tum -->
    <param name="/odom_topic" value="/odom"/>
    <param name="outFilename" value="/root/catkin_ws/devel/lib/evo_creat_tum/outpose.txt"/>
    <node name="evo_creat_tum" pkg="evo_creat_tum" type="evo_creat_tum" />
</launch>

evo使用

1
2
3
4
5
6
# 配准前
evo_traj tum CameraTrajectory.txt groundtruth.txt -p
# 配准后
evo_traj tum CameraTrajectory.txt --ref=groundtruth.txt -p -a
# 输出结果
evo_ape tum groundtruth.txt CameraTrajectory.txt -r full -va -p --save_results results/ORB.zip