使用evo评估cartographer使用多线雷达和imu的纯定位精度

gps的定位原始数据的消息结构为vd_msgs::GpsRaw
将其转换为gps里程计，作为groundtruth

#include <deque>
#include <mutex>
#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/Imu.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include "GeographicLib/LocalCartesian.hpp"
#include <vd_msgs/GpsRaw.h>

GeographicLib::LocalCartesian geo_converter;
ros::Publisher  odom_pub_;
ros::Publisher imu_pub_;
vd_msgs::GpsRaw gps_raw;

double lat = 0;
double lon = 0;
double alt = 0;
bool enable_z = false;


void gpsRawCalback(const vd_msgs::GpsRaw msg)
{
  gps_raw = msg;

  nav_msgs::Odometry real_gps_odom;

  real_gps_odom.header.stamp = gps_raw.header.stamp;
  real_gps_odom.header.frame_id = "odom";
  real_gps_odom.child_frame_id  = "gps_link_compare";

  geometry_msgs::Quaternion catch_orientation;

  catch_orientation = tf::createQuaternionMsgFromRollPitchYaw
                            (gps_raw.Vector.x, gps_raw.Vector.y, gps_raw.Vector.z);
/*   catch_orientation = tf::createQuaternionMsgFromRollPitchYaw
                            (0, 0, gps_raw.Vector.z); */
  real_gps_odom.pose.pose.orientation = catch_orientation;

  double local_E = 0;
  double local_N = 0;
  double local_U = 0;
  geo_converter.Forward(gps_raw.Latitude, gps_raw.Longitude, gps_raw.Altitude, local_E, local_N, local_U);

  real_gps_odom.pose.pose.position.x = local_E;
  real_gps_odom.pose.pose.position.y = local_N;
  if(enable_z) {
    real_gps_odom.pose.pose.position.z = local_U;
  } else {
    real_gps_odom.pose.pose.position.z = 0;
  }
  

  real_gps_odom.twist.twist.linear.x = gps_raw.Vv;
  real_gps_odom.twist.twist.linear.y = 0;
  real_gps_odom.twist.twist.linear.z = 0;
  // 应该是角速度，非角度，还需要修改
  // real_gps_odom.twist.twist.angular = gps_raw.Imu.angular;  

  odom_pub_.publish(real_gps_odom);

  sensor_msgs::Imu imu_data;

  imu_data.header.stamp = gps_raw.header.stamp;
  imu_data.header.frame_id = "imu_link";

  imu_data.orientation = catch_orientation;

/*   imu_data.angular_velocity.x = 0;
  imu_data.angular_velocity.y = 0;
  imu_data.angular_velocity.z = gps_raw.Imu.linear.z* M_PI / 180.0;

  imu_data.linear_acceleration.x = gps_raw.Imu.angular.x;
  imu_data.linear_acceleration.y = gps_raw.Imu.angular.y;
  imu_data.linear_acceleration.z = gps_raw.Imu.angular.z; */

  imu_data.angular_velocity.x = gps_raw.Imu.linear.x* M_PI / 180.0;
  imu_data.angular_velocity.y = gps_raw.Imu.linear.y* M_PI / 180.0;
  imu_data.angular_velocity.z = gps_raw.Imu.linear.z* M_PI / 180.0;

  imu_data.linear_acceleration.x = gps_raw.Imu.angular.x;
  imu_data.linear_acceleration.y = gps_raw.Imu.angular.y;
  imu_data.linear_acceleration.z = gps_raw.Imu.angular.z;

  imu_pub_.publish(imu_data);

}

int main(int argc, char* argv[]) 
{
  ros::init(argc, argv, "odom_gps_dy");
  ros::NodeHandle nh;

  ros::Subscriber gps_raw_sub_ = nh.subscribe("/navsat_dy/gps_raw", 100, gpsRawCalback);
  odom_pub_    = nh.advertise<nav_msgs::Odometry>("/odom/by_gps", 100);
  imu_pub_ = nh.advertise<sensor_msgs::Imu>("/imu/data", 100);

  nh.param("/odom_gps_dy/latitude",   lat , 28.1077029);
  nh.param("/odom_gps_dy/longitude",  lon , 112.8735103);
  nh.param("/odom_gps_dy/altitude",   alt , 37.6);
  nh.param("/odom_gps_dy/use3d",      enable_z , false);

/*   nh.param("/odom_gps_dy/latitude",   lat , 28.18593249);112.8735103
  nh.param("/odom_gps_dy/longitude",  lon , 112.9446714);
  nh.param("/odom_gps_dy/altitude",   alt , 37.6);
  nh.param("/odom_gps_dy/use3d",      enable_z , false); */

  if(lat == 28.18593249){
    ROS_WARN_STREAM ( "no set gnss origin!");
  }else{
    ROS_INFO("set gnss origin > lat:%lf, lat:%lf, lat:%lf\n", lat, lon, alt);
  }
  
  geo_converter.Reset(lat, lon, alt);

  ros::spin();
  return 0;
}

carto使用多线雷达和imu纯定位如下，获得carto里程计
https://blog.csdn.net/zhaohaowu/article/details/120889458
将carto里程计和gps里程计分别转换为tum数据的轨迹，方便使用evo评估
自定义消息结构为carto_test::odom2tum

float32 x
float32 y
float32 z
float32 q_x
float32 q_y
float32 q_z
float32 q_w

代码如下：

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include "carto_test/odom2tum.h"
ros::Publisher pub_gps;
ros::Publisher pub_carto;

void callback_gps(const nav_msgs::Odometry::ConstPtr& odom)
{
    carto_test::odom2tum msg;
    msg.x = odom->pose.pose.position.x;
    msg.y = odom->pose.pose.position.y;
    msg.z = odom->pose.pose.position.z;
    msg.q_x = odom->pose.pose.orientation.x;
    msg.q_y = odom->pose.pose.orientation.y;
    msg.q_z = odom->pose.pose.orientation.z;
    msg.q_w = odom->pose.pose.orientation.w;
    pub_gps.publish(msg);
    ROS_INFO("gps ok");
}
void callback_carto(const nav_msgs::Odometry::ConstPtr& odom)
{
    carto_test::odom2tum msg;
    msg.x = odom->pose.pose.position.x;
    msg.y = odom->pose.pose.position.y;
    msg.z = odom->pose.pose.position.z;
    msg.q_x = odom->pose.pose.orientation.x;
    msg.q_y = odom->pose.pose.orientation.y;
    msg.q_z = odom->pose.pose.orientation.z;
    msg.q_w = odom->pose.pose.orientation.w;
    pub_carto.publish(msg);
    ROS_INFO("carto ok");
}
int main (int argc, char **argv)
{
    ros::init (argc, argv, "odom2tum");
    ros::NodeHandle n;

    pub_gps = n.advertise<carto_test::odom2tum>("tum_gps", 10, true);
    pub_carto = n.advertise<carto_test::odom2tum>("tum_carto", 10, true);
    ros::Subscriber sub_gps = n.subscribe<nav_msgs::Odometry>("/odom/by_gps", 10, callback_gps);
    ros::Subscriber sub_carto = n.subscribe<nav_msgs::Odometry>("/carto_odom", 10, callback_carto);
    ros::Rate loop_rate(50);
    while(ros::ok())
    {
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

然后将转换后的话题tum_gps和tum_gps转换为tum格式的txt

rostopic echo -b tum_gps_carto.bag -p /tum_carto > tum_carto.txt
rostopic echo -b tum_gps_carto.bag -p /tum_gps > tum_gps.txt

在txt中，ctrl+h替换所有的逗号,为空格
使用evo评估，evo使用方法如下
测量指标：
evo_ape：绝对位姿误差
evo_rpe：相对位姿误差
常用命令：
evo_traj：绘制轨迹，一条或多条，支持kitti，eurco，tum三种格式
evo_res：根据指标比较分析不同SLAM方案轨迹输出结果
不常用命令：
evo_fig：重新打开序列化图的工具
evo_config：用于全局设置和配置文件操作的工具
参数：
-p或–plot: 绘图
-v或–verbose: 输出相关信息（均值，方差等）
-f或–full_check: 检查相关信息（时间戳是否对应，四元数是不是单位四元数）
-a或–align: 对轨迹进行对齐，用ICP的方法，并不是仅仅将起点对齐
–correct_scale: 尺度校正

evo_traj tum --ref=tum_gps.txt tum_carto.txt --plot --plot_mode=xy

使用evo进行校正，原理为icp

evo_traj tum --ref=tum_gps.txt tum_carto.txt --plot --plot_mode=xy --align --correct_scale 

绝对位姿误差ape

evo_ape tum tum_gps.txt tum_carto.txt --plot --plot_mode=xy

校正后的绝对位姿误差

evo_ape tum tum_gps.txt tum_carto.txt --plot --plot_mode=xy --align --correct_scale

相对位姿误差rpe

ape和rpe含义
ape绝对位姿误差：比较估计轨迹和参考轨迹并计算整个轨迹的统计数据，适用于测试轨迹的全局一致性
rpe相对位姿误差：相对位姿误差可以给出局部精度，例如slam系统每米的平移或者旋转漂移量

https://zhuanlan.zhihu.com/p/105428199
https://www.codenong.com/cs105316835/