MARS_LVIG数据集时间不对称问题，还有ground truth文件提取问题

[1391741823]

郑博你好！ 使用代码在跑MARS_LVIG dataset的HKisland03数据集时，自己跑出来的真值时间戳跟bag包里提取的时间戳不一样，无论是否加减62s秒以下是跑代码的数据时间戳：

下面是从bag包里提取的时间戳与相关的数据：（在乘10-9次方时也对不上 无法进行evo评估）

在bag中对应的真值有以下topic ，且每一项的数据的消息会提出不同的列数据（比如topic：/dji_osdk_ros/rtk_position中存在1828行数据 没有其他旋转的数据与他组成一个txt真值文件表），我应该使用哪些数据来构造我的八维的真值表呢，望郑博点播：

[1391741823]

这个是订阅topic相关： 另外很抱歉郑博我发了很多个贴因为我网卡了，实在抱歉！

[xuankuzcr]

真值只提供了平移，没有旋转，就是这个话题/dji_osdk_ros/rtk_position。我记得是先手动补偿个大概的时间偏移，再用evo_ape的--t_offset逐步微调。

#include <ros/ros.h>
#include "turtlesim/Pose.h"
#include <sensor_msgs/NavSatFix.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>
#include <math.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>
#include <fstream>

// Function to save trajectory in TUM format
void SaveTrajTUM(double timestamp, const Eigen::Vector3d& t) {
    std::ofstream foutC("/home/chunran/Desktop/GpsTrajectory.txt", std::ios::app | std::ios::out);
    foutC.setf(std::ios::fixed, std::ios::floatfield);
    foutC.precision(9); // 9 decimal places for timestamp
    foutC << timestamp << " ";
    foutC.precision(5);

    Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
    Eigen::Quaterniond q(R);
    q.normalized();

    float x = t(0);
    float y = t(1);
    float z = t(2);
    float qx = q.x();
    float qy = q.y();
    float qz = q.z();
    float qw = q.w();

    foutC << x << " " << y << " " << z << " " << qx << " " << qy << " " << qz << " " << qw << std::endl;
}

// Structure to hold GPS pose information
struct MyPose {
    double latitude;
    double longitude;
    double altitude;
};

// Convert degrees to radians
double degToRad(double deg) {
    return deg * M_PI / 180.0;
}

// Global variables
static const double EARTH_RADIUS = 6378.137; // Earth radius in kilometers
ros::Publisher state_pub_;
nav_msgs::Path ros_path_;
bool init = false;
MyPose init_pose;

// Callback function for GPS data
void gpsCallback(const sensor_msgs::NavSatFixConstPtr& gps_msg_ptr) {
    if (!init) {
        // Initialize the initial pose
        init_pose.latitude = gps_msg_ptr->latitude;
        init_pose.longitude = gps_msg_ptr->longitude;
        init_pose.altitude = gps_msg_ptr->altitude;
        init = true;
    } else {
        // Calculate relative position
        double radLat1 = degToRad(init_pose.latitude);
        double radLong1 = degToRad(init_pose.longitude);
        double radLat2 = degToRad(gps_msg_ptr->latitude);
        double radLong2 = degToRad(gps_msg_ptr->longitude);

        // Calculate x (latitude difference)
        double delta_lat = radLat2 - radLat1;
        double x = 2 * asin(sqrt(pow(sin(delta_lat / 2), 2) + cos(radLat1) * cos(radLat2) * pow(sin(0 / 2), 2)));
        x = (delta_lat < 0) ? -x * EARTH_RADIUS * 1000 : x * EARTH_RADIUS * 1000;

        // Calculate y (longitude difference)
        double delta_long = radLong1 - radLong2;
        double y = 2 * asin(sqrt(pow(sin(0 / 2), 2) + cos(radLat2) * cos(radLat2) * pow(sin(delta_long / 2), 2)));
        y = (delta_long < 0) ? -y * EARTH_RADIUS * 1000 : y * EARTH_RADIUS * 1000;

        // Calculate z (altitude difference)
        double z = gps_msg_ptr->altitude - init_pose.altitude;

        // Publish trajectory
        ros_path_.header.frame_id = "path";
        ros_path_.header.stamp = ros::Time::now();

        geometry_msgs::PoseStamped pose;
        pose.header = ros_path_.header;

        pose.pose.position.x = x;
        pose.pose.position.y = y;
        pose.pose.position.z = z;

        double timestamp = gps_msg_ptr->header.stamp.toSec();
        Eigen::Vector3d trans(x, y, z);
        std::cout << "delta dis: " << std::sqrt(x * x + y * y) << std::endl;

        SaveTrajTUM(timestamp, trans);

        ros_path_.poses.push_back(pose);

        ROS_INFO("( x:%0.6f ,y:%0.6f ,z:%0.6f)", x, y, z);

        state_pub_.publish(ros_path_);
    }
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "rtk_subscriber");
    ros::NodeHandle n;
    ros::Subscriber pose_sub = n.subscribe("/dji_osdk_ros/rtk_position", 10, gpsCallback);

    state_pub_ = n.advertise<nav_msgs::Path>("rtk_path", 10);

    ros::spin();
    return 0;
}

[1391741823]

真值只提供了平移，没有旋转，就是这个话题/dji_osdk_ros/rtk_position。我记得是先手动补偿个大概的时间偏移，再用evo_ape的--t_offset逐步微调。

#include <ros/ros.h> #include "turtlesim/Pose.h" #include <sensor_msgs/NavSatFix.h> #include <geometry_msgs/PoseStamped.h> #include <nav_msgs/Path.h> #include <math.h> #include <Eigen/Core> #include <Eigen/Geometry> #include #include

// Function to save trajectory in TUM format void SaveTrajTUM(double timestamp, const Eigen::Vector3d& t) { std::ofstream foutC("/home/chunran/Desktop/GpsTrajectory.txt", std::ios::app | std::ios::out); foutC.setf(std::ios::fixed, std::ios::floatfield); foutC.precision(9); // 9 decimal places for timestamp foutC << timestamp << " "; foutC.precision(5);

Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
Eigen::Quaterniond q(R);
q.normalized();

float x = t(0);
float y = t(1);
float z = t(2);
float qx = q.x();
float qy = q.y();
float qz = q.z();
float qw = q.w();

foutC << x << " " << y << " " << z << " " << qx << " " << qy << " " << qz << " " << qw << std::endl;

}

// Structure to hold GPS pose information struct MyPose { double latitude; double longitude; double altitude; };

// Convert degrees to radians double degToRad(double deg) { return deg * M_PI / 180.0; }

// Global variables static const double EARTH_RADIUS = 6378.137; // Earth radius in kilometers ros::Publisher state_pub_; nav_msgs::Path ros_path_; bool init = false; MyPose init_pose;

// Callback function for GPS data void gpsCallback(const sensor_msgs::NavSatFixConstPtr& gps_msg_ptr) { if (!init) { // Initialize the initial pose init_pose.latitude = gps_msg_ptr->latitude; init_pose.longitude = gps_msg_ptr->longitude; init_pose.altitude = gps_msg_ptr->altitude; init = true; } else { // Calculate relative position double radLat1 = degToRad(init_pose.latitude); double radLong1 = degToRad(init_pose.longitude); double radLat2 = degToRad(gps_msg_ptr->latitude); double radLong2 = degToRad(gps_msg_ptr->longitude);

    // Calculate x (latitude difference)
    double delta_lat = radLat2 - radLat1;
    double x = 2 * asin(sqrt(pow(sin(delta_lat / 2), 2) + cos(radLat1) * cos(radLat2) * pow(sin(0 / 2), 2)));
    x = (delta_lat < 0) ? -x * EARTH_RADIUS * 1000 : x * EARTH_RADIUS * 1000;

    // Calculate y (longitude difference)
    double delta_long = radLong1 - radLong2;
    double y = 2 * asin(sqrt(pow(sin(0 / 2), 2) + cos(radLat2) * cos(radLat2) * pow(sin(delta_long / 2), 2)));
    y = (delta_long < 0) ? -y * EARTH_RADIUS * 1000 : y * EARTH_RADIUS * 1000;

    // Calculate z (altitude difference)
    double z = gps_msg_ptr->altitude - init_pose.altitude;

    // Publish trajectory
    ros_path_.header.frame_id = "path";
    ros_path_.header.stamp = ros::Time::now();

    geometry_msgs::PoseStamped pose;
    pose.header = ros_path_.header;

    pose.pose.position.x = x;
    pose.pose.position.y = y;
    pose.pose.position.z = z;

    double timestamp = gps_msg_ptr->header.stamp.toSec();
    Eigen::Vector3d trans(x, y, z);
    std::cout << "delta dis: " << std::sqrt(x * x + y * y) << std::endl;

    SaveTrajTUM(timestamp, trans);

    ros_path_.poses.push_back(pose);

    ROS_INFO("( x:%0.6f ,y:%0.6f ,z:%0.6f)", x, y, z);

    state_pub_.publish(ros_path_);
}

}

int main(int argc, char **argv) { ros::init(argc, argv, "rtk_subscriber"); ros::NodeHandle n; ros::Subscriber pose_sub = n.subscribe("/dji_osdk_ros/rtk_position", 10, gpsCallback);

state_pub_ = n.advertise<nav_msgs::Path>("rtk_path", 10);

ros::spin();
return 0;

}

感谢郑博答疑解惑！