1、安装 evo
pip install evo --upgrade --no-binary evo --user
即可直接安装成功
如果说需要更新则更新即可
/usr/local/bin/python3.7 -m pip install --upgrade pip
2、测试
evo_traj euroc 2.txt --plot
报错：
[ERROR] EuRoC format ground truth must have at least 8 entries per row and no trailing delimiter at the end of the rows (comma)
出现这个问题的原因是生成的原始文件中偶尔存在空格等不是完全规范的tum结果文件
解决办法：运行如下命令可以清除多余的空格
使用这个方法尝试解决：
cat results.txt | tr -s [:space:] > results_new.txt
但是报错
[ERROR] TUM trajectory files must have 8 entries per row and no trailing delimiter at the end of the rows (space)
我打开我的txt数据，发现只有七列而要求是八列，因此需要改成八列数据
要求的八列数据是这些：
时间戳+三维坐标+四元数
我之前路径输出的是三个旋转角，哭泣，所以这里需要把他转换成四元数来输出，重新修改代码运行，输出八列数据
就可以顺利运行啦：
把我的真实轨迹文件进行处理，保留八列数据
如图所示，为了使用evo进行绘图，需要格式准确，这样还是报错
首先，需要顶格，不能空格
其次对空格的要求太高，需要删除空格，转换成逗号更保险，于是需要将上面的格式转换成下面图所示
代码放在下面啦：
新建一个create_txt.cpp
#include <ros/ros.h>
#include <ros/time.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointCloud.h>
#include <pcl/point_types.h> //pcl点云格式头文件
#include <pcl_conversions/pcl_conversions.h> //转换
#include <pcl/point_cloud.h>
#include <pcl/filters/voxel_grid.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include "Eigen/Core"
#include <boost/foreach.hpp>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>
#include <stdlib.h>
#include <ctime>
#include <fstream>
#include <iostream>
#include <vector>
#include <cmath>
#define foreach BOOST_FOREACH
using namespace std;
std::string txt_path;
struct TRAJECTORY
{
double time;
double position_x;
double position_y;
double position_z;
double orientation_w;
double orientation_x;
double orientation_y;
double orientation_z;
};
void read_trajectory_file(std::string filepathIn)
{
std::ifstream inFile(filepathIn);
std::ofstream traj_ofs;
traj_ofs.open( txt_path.c_str(), std::ios::app);
if (!inFile)
{
cout << "open write file fail!" << endl;
}
// temp v
char content[2000];
std::string content_str = "";
while (!inFile.eof())
{
content_str = "";
inFile.getline(content, 2000);
if (strlen(content) < 2)
break;
std::istringstream _Readstr(content);
std::string partOfstr;
double data[9];
for (int i = 0; i < 9; i++)
{
getline(_Readstr, partOfstr, ' ');
data[i] = strtold(partOfstr.c_str(), NULL);
}
TRAJECTORY trajectory;
trajectory.time = data[0];
trajectory.position_x = data[1];
trajectory.position_y = data[2];
trajectory.position_z = data[3];
trajectory.orientation_w = data[4];
trajectory.orientation_x = data[5];
trajectory.orientation_y = data[6];
trajectory.orientation_z = data[7];
traj_ofs << std::setprecision(16) << trajectory.time << "," << trajectory.position_x << "," << trajectory.position_y<< "," << trajectory.position_z << "," << trajectory.orientation_w << "," << trajectory.orientation_x << "," << trajectory.orientation_y << "," << trajectory.orientation_z<<std::endl;
}
inFile.close();
traj_ofs.close();
cout << "read trajectory file end!" << endl;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "create_txt");
ros::NodeHandle nh;
string traj_path = "";
nh.param<string>("input_trajectory_path", traj_path, "");
nh.param<string>("output_txt_path", txt_path, "");
read_trajectory_file(traj_path);
return 0;
}
这样就可以成功转换啦
接下来使用该命令
evo_traj euroc ground.txt --plot
就可以成功绘图啦
但是我使用evo_traj tum ground.txt --plot就一直报错
应该还是格式的问题
格式转化
evo_traj euroc test.txt --save_as_tum
就可以转化成tum格式了，其他的都类似
3. 轨迹对齐
采用对齐指令将两条轨迹进行对齐。为此需要通过–ref参数指定参考轨迹，并增加参数-a（或–align）进行对齐（旋转与平移）
evo_traj euroc test.txt --ref=ground.txt -p --plot_mode xyz -a --correct_scale
但是报错：
[ERROR] found no matching timestamps between reference and test.txt with max. time diff 0.01 (s) and time offset 0.0 (s)
时间戳对应不上，这可怎么办？
需要时间戳对应才可以进行对比，这就很无语了
这样都把所有的txt文件时间戳改成0，1，2，3…个数一样就可以对齐啦
修改上面代码读取文件把时间戳进行修改
代码如下：
#include <ros/ros.h>
#include <ros/time.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointCloud.h>
#include <pcl/point_types.h> //pcl点云格式头文件
#include <pcl_conversions/pcl_conversions.h> //转换
#include <pcl/point_cloud.h>
#include <pcl/filters/voxel_grid.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include "Eigen/Core"
#include <boost/foreach.hpp>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>
#include <stdlib.h>
#include <ctime>
#include <fstream>
#include <iostream>
#include <vector>
#include <cmath>
#define foreach BOOST_FOREACH
using namespace std;
std::string txt_path;
struct TRAJECTORY
{
double time;
double position_x;
double position_y;
double position_z;
double orientation_w;
double orientation_x;
double orientation_y;
double orientation_z;
};
void read_trajectory_file(std::string filepathIn)
{
std::ifstream inFile(filepathIn);
std::ofstream traj_ofs;
traj_ofs.open( txt_path.c_str(), std::ios::app);
if (!inFile)
{
cout << "open write file fail!" << endl;
}
// temp v
char content[2000];
std::string content_str = "";
int m=0;
int n= -1;
while (!inFile.eof())
{
content_str = "";
inFile.getline(content, 2000);
n++;
if(n%40==0)
{
if (strlen(content) < 2)
break;
std::istringstream _Readstr(content);
std::string partOfstr;
double data[9];
for (int i = 0; i < 9; i++)
{
getline(_Readstr, partOfstr, ',');
data[i] = strtold(partOfstr.c_str(), NULL);
}
TRAJECTORY trajectory;
trajectory.time = m;
trajectory.position_x = data[1];
trajectory.position_y = data[2];
trajectory.position_z = data[3];
trajectory.orientation_w = data[4];
trajectory.orientation_x = data[5];
trajectory.orientation_y = data[6];
trajectory.orientation_z = data[7];
traj_ofs << std::setprecision(16) << trajectory.time << "," << trajectory.position_x << "," << trajectory.position_y<< "," << trajectory.position_z << "," << trajectory.orientation_w << "," << trajectory.orientation_x << "," << trajectory.orientation_y << "," << trajectory.orientation_z<<std::endl;
m++;
}
}
inFile.close();
traj_ofs.close();
cout << "read trajectory file end!" << endl;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "create_txt");
ros::NodeHandle nh;
string traj_path = "";
nh.param<string>("input_trajectory_path", traj_path, "");
nh.param<string>("output_txt_path", txt_path, "");
read_trajectory_file(traj_path);
return 0;
}
这样结果就是：
一共5008个数据，再次输入该语句进行轨迹对齐
evo_traj euroc trial.txt --ref=truth.txt -p --plot_mode xyz -a --correct_scale
这个效果与下面效果相同
evo_traj euroc trial.txt --ref truth.txt -p -a
这里有个bug
因为他是完全按照时间戳进行对齐的
要是想让他自动配准再显示出来这个就做不到
因此即使尺度进行了修改，要是点对应不上还是无法进行对比
因此需要在使用evo前自己进行点云轨迹的处理
这里我使用了cloudcompare进行处理进行配准之后在进行轨迹的显示对比
不知道有没有更简单的方法，希望大家多多分享
如果想投影到xy平面进行显示，则输入下面的命令
evo_traj euroc trial.txt --ref=truth.txt -p --plot_mode xy -a --correct_scale
4.计算轨迹的绝对误差（evo_ape）
直接使用下面的语句
evo_ape euroc true.txt test.txt --plot --plot_mode xy --save_results result.zip
会报错：
[ERROR] TUM trajectory files must have 8 entries per row and no trailing delimiter at the end of the rows (space)
于是我使用上面的格式转换把数据格式转成tum格式
再进行比较
evo_ape tum true.tum test.tum --plot --plot_mode xy --save_results result.zip
5.计算轨迹的绝对误差（evo_ape）
evo_rpe tum true.tum test.tum --plot --plot_mode xy --save_results resultrpe.zip
注意！
这里还要分平移误差和旋转误差，结果可能有区别！
其中-r表示ape所基于的姿态关系
不添加-r/–pose_relation和可选项，则默认为trans_part。
-r/–pose_relation可选参数 含义
full 表示同时考虑旋转和平移误差得到的ape,无单位（unit-less）
trans_part 考虑平移部分得到的ape，单位为m
rot_part 考虑旋转部分得到的ape，无单位（unit-less）
angle_deg 考虑旋转角得到的ape,单位°（deg）
angle_rad 考虑旋转角得到的ape,单位弧度（rad）
–d/–delta表示相对位姿之间的增量，–u/–delta_unit表示增量的单位，可选参数为[f, d, r, m],分别表示[frames, deg, rad, meters]。–d/–delta -u/–delta_unit合起来表示衡量局部精度的单位，如每米，每弧度，每百米等。其中–delta_unit为f时，–delta的参数必须为整形，其余情况下可以为浮点型。–delta 默认为1，–delta_unit默认为f。
因此这里仅仅统计平移的相对精度：
evo_rpe tum true.tum test.tum -r trans_part --delta 1 --delta_unit m --plot --plot_mode xy
全部：
evo_rpe tum true.tum test.tum -r full --delta 1 --delta_unit m -vas --plot --plot_mode xy
我实验的最小的结果：
evo_rpe tum true.tum test.tum -r trans_part --plot --plot_mode xy
但是这个效果对所有实验都很小。。。
有点搞不清楚，这些语句有什么影响了