一、建立数据保存的文件夹

我的GVINS程序是保存在主目录下的文件夹： ~/GVINS/src 中，如下图：

打开 ~/GVINS/src/GVINS-main/config/visensor_f9p 文件夹下的visensor_left_f9p_config.yaml 文件，其中 output_dir: "~/GVINS/output" 这样设置；

output 文件夹我们事先要建立好！如下图：

这样的话我们后面要保存的文件就在 "~/GVINS/output" 文件夹下面了！

二、导航数据输入到特定的 .txt 文件中

打开 ~/GVINS/src/GVINS-main/estimator/src 下的 parameter.cpp 程序，然后把对应的程序修改如下即可：

VINS_RESULT_PATH = OUTPUT_DIR + "/vins_result_no_loop.txt";

FACTOR_GRAPH_RESULT_PATH = OUTPUT_DIR + "/factor_graph_result.txt";

GNSS_RESULT_PATH = OUTPUT_DIR + "/gvins_ecef_result.txt";

因为本人比较关心GVINS程序的 e坐标系下的位置信息，所以详细介绍一下 GNSS_RESULT_PATH；

打开 ~/GVINS/src/GVINS-main/estimator/src/utility 文件夹下面的 visualization.cpp 程序，找到下面的具体程序：

// 伪距定位得到的gnss经纬度和高度；
void pubGnssResult(const Estimator &estimator, const std_msgs::Header &header)
{
    if (!estimator.gnss_ready)      return;
    
    // publish GNSS LLA
    const double gnss_ts = estimator.Headers[WINDOW_SIZE].stamp.toSec() + 
        estimator.diff_t_gnss_local;
        
    Eigen::Vector3d lla_pos = ecef2geo(estimator.ecef_pos);
    printf("global time: %f\n", gnss_ts);
    printf("latitude longitude altitude: %f, %f, %f\n", lla_pos.x(), lla_pos.y(), lla_pos.z());
    sensor_msgs::NavSatFix gnss_lla_msg;
    gnss_lla_msg.header.stamp = ros::Time(gnss_ts);
    gnss_lla_msg.header.frame_id = "geodetic";
    gnss_lla_msg.latitude = lla_pos.x();
    gnss_lla_msg.longitude = lla_pos.y();
    gnss_lla_msg.altitude = lla_pos.z();
    pub_gnss_lla.publish(gnss_lla_msg);

    // publish anchor LLA
    const Eigen::Vector3d anc_lla = ecef2geo(estimator.anc_ecef);
    sensor_msgs::NavSatFix anc_lla_msg;
    anc_lla_msg.header = gnss_lla_msg.header;
    anc_lla_msg.latitude = anc_lla.x();
    anc_lla_msg.longitude = anc_lla.y();
    anc_lla_msg.altitude = anc_lla.z();
    pub_anc_lla.publish(anc_lla_msg);

    // publish ENU pose and path
    geometry_msgs::PoseStamped enu_pose_msg;
    // camera-front orientation
    Eigen::Matrix3d R_s_c;
    R_s_c <<  0,  0,  1,
             -1,  0,  0,
              0, -1,  0;
    Eigen::Matrix3d R_w_sensor = estimator.Rs[WINDOW_SIZE] * estimator.ric[0] * R_s_c.transpose();
    Eigen::Quaterniond enu_ori(estimator.R_enu_local * R_w_sensor);
    enu_pose_msg.header.stamp = header.stamp;
    enu_pose_msg.header.frame_id = "world";     // "enu" will more meaningful, but for viz
    enu_pose_msg.pose.position.x = estimator.enu_pos.x();
    enu_pose_msg.pose.position.y = estimator.enu_pos.y();
    enu_pose_msg.pose.position.z = estimator.enu_pos.z();
    enu_pose_msg.pose.orientation.x = enu_ori.x();
    enu_pose_msg.pose.orientation.y = enu_ori.y();
    enu_pose_msg.pose.orientation.z = enu_ori.z();
    enu_pose_msg.pose.orientation.w = enu_ori.w();
    pub_enu_pose.publish(enu_pose_msg);

    enu_path.header = enu_pose_msg.header;
    enu_path.poses.push_back(enu_pose_msg);
    pub_enu_path.publish(enu_path);

    // publish ENU-local tf
    Eigen::Quaterniond q_enu_world(estimator.R_enu_local);
    static tf::TransformBroadcaster br;
    tf::Transform transform_enu_world;

    transform_enu_world.setOrigin(tf::Vector3(0, 0, 0));
    tf::Quaternion tf_q;
    tf_q.setW(q_enu_world.w());
    tf_q.setX(q_enu_world.x());
    tf_q.setY(q_enu_world.y());
    tf_q.setZ(q_enu_world.z());
    transform_enu_world.setRotation(tf_q);
    br.sendTransform(tf::StampedTransform(transform_enu_world, header.stamp, "enu", "world"));

    
    // write GNSS result to file
    
    ofstream gnss_output(GNSS_RESULT_PATH, ios::app);
    gnss_output.setf(ios::fixed, ios::floatfield);
    gnss_output.precision(3);
   // gnss_output << header.stamp.toSec() * 1e9 << ',';
    gnss_output <<(gnss_ts-1606690000.000) << ' ';
    gnss_output.precision(5);
    gnss_output << estimator.ecef_pos(0) << ' '
                << estimator.ecef_pos(1) << ' '
                << estimator.ecef_pos(2) << '\n';
              //  << estimator.yaw_enu_local << ' '
              //  << estimator.para_rcv_dt[(WINDOW_SIZE)*4+0] << ' '
              //  << estimator.para_rcv_dt[(WINDOW_SIZE)*4+1] << ' '
              //  << estimator.para_rcv_dt[(WINDOW_SIZE)*4+2] << ' '
              //  << estimator.para_rcv_dt[(WINDOW_SIZE)*4+3] << ' '
              //  << estimator.para_rcv_ddt[WINDOW_SIZE] << ','
              //  << estimator.anc_ecef(0) << ' '
              //  << estimator.anc_ecef(1) << ' '
              //  << estimator.anc_ecef(2) << '\n';
    gnss_output.close();

}

以上变是我在原版的GVINS上修改程序，得到的自己想要的位置信息；其中VIO的结果也在visualization.cpp 中，自己看一下就明白了！

打开上面我们说的 "~/GVINS/output" 文件夹，当跑完程序后，数据如下所示：

GVINS 导航结果（PVQ）保存到 txt 文件中

一、建立数据保存的文件夹

二、导航数据输入到特定的 .txt 文件中

猜你喜欢