VINS - Fusion GPS/VIO 融合二、数据融合

最新推荐文章于 2025-11-12 09:52:41 发布

原创

最新推荐文章于 2025-11-12 09:52:41 发布 · 3.5k 阅读

69 ·

CC 4.0 BY-SA版权

https://zhuanlan.zhihu.com/p/75492883

一、简介

源代码：VINS - Fusion

数据集：KITTI 数据

程序入口：globalOptNode.cpp

二、程序解读

2.1 主函数

int main(int argc, char **argv)
{
    //初始化
    ros::init(argc, argv, "globalEstimator");
    ros::NodeHandle n("~");
    //全局优化
    global_path = &globalEstimator.global_path;
    //订阅GPS信息
    ros::Subscriber sub_GPS = n.subscribe("/gps", 100, GPS_callback);
    //订阅VIO信息
    ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 100, vio_callback);
   
   //发布信息， //这些发布的数据，rviz 订阅然后显示，看一下 rviz的配置文件就知道了！
   //配置文件在config里面的 vins_rviz_config.rviz
    pub_global_path = n.advertise<nav_msgs::Path>("global_path", 100);
    pub_global_odometry = n.advertise<nav_msgs::Odometry>("global_odometry", 100);
    pub_car = n.advertise<visualization_msgs::MarkerArray>("car_model", 1000);
    ros::spin();
    return 0;
}

理解：

代码很简单，订阅 topic（/gps）在GPS_callback回调函数中接收并处理gps数据，订阅topic（/vins_estimator/odometry）

在vio_callback回调函数中接收并处理vio的定位数据，并且生成了三个发布器，用于将结果展示在rviz上。

2.2 GPS 消息订阅

ros::Subscriber sub_GPS = n.subscribe("/gps", 100, GPS_callback)

void GPS_callback(const sensor_msgs::NavSatFixConstPtr &GPS_msg)
{
    m_buf.lock();
    gpsQueue.push(GPS_msg);
    m_buf.unlock();
}

理解：GPS回调函数，简单，只是把GPS消息存储到了全局变量 gpsQueue 队列里面

2.3 订阅VIO信息

ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 100, vio_callback);

(1) 获取 VIO位姿 localPoseMap[t] 和世界坐标系下的globalPoseMap[t]

void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
{
    double t = pose_msg->header.stamp.toSec();
    last_vio_t = t;
    
    // 获取VIO输出的位置(三维向量)，姿态(四元数)
    Eigen::Vector3d vio_t(pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y, pose_msg->pose.pose.position.z);
    
    Eigen::Quaterniond vio_q;
    vio_q.w() = pose_msg->pose.pose.orientation.w;
    vio_q.x() = pose_msg->pose.pose.orientation.x;
    vio_q.y() = pose_msg->pose.pose.orientation.y;
    vio_q.z() = pose_msg->pose.pose.orientation.z;
    
    //位姿传入global Estimator中
    globalEstimator.inputOdom(t, vio_t, vio_q);
    m_buf.lock();

globalEstimator.inputOdom(t, vio_t, vio_q) 程序如下：

void GlobalOptimization::inputOdom(double t, Eigen::Vector3d OdomP, Eigen::Quaterniond OdomQ)
{
	mPoseMap.lock();
     // 把vio直接输出的位姿存入 localPoseMap 中
    vector<double> localPose{OdomP.x(), OdomP.y(), OdomP.z(), 
    		             OdomQ.w(), OdomQ.x(), OdomQ.y(), OdomQ.z()};
    localPoseMap[t] = localPose;
    Eigen::Quaterniond globalQ;
    
    // 把VIO转换到GPS坐标系下，准确的说是转换到以第一帧GPS为原点的坐标系下
    // 转换之后的位姿插入到globalPoseMap 中
    globalQ = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomQ;//全局姿态
    
    Eigen::Vector3d globalP = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomP + WGPS_T_WVIO.block<3, 1>(0, 3);
    
    //优化后的位姿
    vector<double> globalPose{globalP.x(), globalP.y(), globalP.z(),
                              globalQ.w(), globalQ.x(), globalQ.y(), globalQ.z()};
                              
    //优化后的位姿赋值给全局变量 globalPoseMap                          
    globalPoseMap[t] = globalPose;
    lastP = globalP;//更新
    lastQ = globalQ;
    
    //把最新的全局姿态插入轨迹当中(第一个VIO数据当作第一个全局位姿)
    geometry_msgs::PoseStamped pose_stamped;
    pose_stamped.header.stamp = ros::Time(t);
    pose_stamped.header.frame_id = "world";
    pose_stamped.pose.position.x = lastP.x();
    pose_stamped.pose.position.y = lastP.y();
    pose_stamped.pose.position.z = lastP.z();
    pose_stamped.pose.orientation.x = lastQ.x();
    pose_stamped.pose.orientation.y = lastQ.y();
    pose_stamped.pose.orientation.z = lastQ.z();
    pose_stamped.pose.orientation.w = lastQ.w();
    global_path.header = pose_stamped.header;
    global_path.poses.push_back(pose_stamped);

    mPoseMap.unlock();
}