5.vinsdusion globalOptNode.cpp

IO输出的 nav_msgs::Odometry 类型消息，这个定位信息包含了VIO的位置和姿态，其坐标系原点位于VIO的第一帧处。

GPS输出的sensor_msgs::NavSatFixConstPtr 类型消息，这个是全局定位信息，用经纬度来表示，其坐标原点位于该GPS坐标系下定义的0经度0纬度处。

1.消息订阅和发布：

// 订阅GPS
ros::Subscriber sub_GPS = n.subscribe("/gps", 100, GPS_callback);
// 订阅里程计
ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 100, vio_callback);
 
// 发布轨迹，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);

1.1回调函数

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

/**
 * 订阅GPS
*/
void GPS_callback(const sensor_msgs::NavSatFixConstPtr &GPS_msg)
{
    //printf("gps_callback! \n");
    m_buf.lock();
    gpsQueue.push(GPS_msg);
    m_buf.unlock();
}

VIO回调函数，请看注释：

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;
    Eigen::Quaterniond vio_q;
    ......
    /// 位姿传入global Estimator中
    globalEstimator.inputOdom(t, vio_t, vio_q);
    m_buf.lock();
    // 寻找与VIO时间戳相对应的GPS消息
    // 细心的读者可能会疑惑，这里需不需要对GPS和VIO进行硬件上的时间戳同步呢？
    // 这个问题请看总结与讨论
    while(!gpsQueue.empty())
    {
        // 获取最老的GPS数据和其时间
        sensor_msgs::NavSatFixConstPtr GPS_msg = gpsQueue.front();
        double gps_t = GPS_msg->header.stamp.toSec();
        // 10ms sync tolerance
        // +- 10ms的时间偏差
        if(gps_t >= t - 0.01 && gps_t <= t + 0.01)
        {   /// gps的经纬度，海拔高度
            double latitude = GPS_msg->latitude;
            double longitude = GPS_msg->longitude;
            double altitude = GPS_msg->altitude;
            // gps 数据的方差
            double pos_accuracy = GPS_msg->position_covariance[0];
            if(pos_accuracy <= 0)
                pos_accuracy = 1;
            //printf("receive covariance %lf \n", pos_accuracy);
            /// GPS_msg->status.status 这个数字代表了GPS的状态(固定解，浮点解等)
            /// 具体可以谷歌
            // if(GPS_msg->status.status > 8)
            // 向globalEstimator中输入GPS数据
            globalEstimator.inputGPS(t, latitude, longitude, altitude, pos_accuracy);
            gpsQueue.pop();
            break;
        }
        else if(gps_t < t - 0.01)
            gpsQueue.pop();
        else if(gps_t > t + 0.01)
            break;
    }
    m_buf.unlock();
    ......
    // 广播轨迹（略）......
    pub_global_odometry.publish(odometry);
    pub_global_path.publish(*global_path);
    publish_car_model(t, global_t, global_q);
    // 位姿写入文本文件（略）......
}

可以看出，global Fusion的优化策略是收到一帧VIO数据，就寻找相应的GPS数据来进行优化。我们下面主要来看一下globalEstimator中的inputOdom()和inputGPS()这两个函数

首先看下 inputGPS():

void GlobalOptimization::inputGPS(double t, double latitude, 
                                  double longitude, 
                                  double altitude, 
                                  double posAccuracy)
{
    double xyz[3];
        // 因为经纬度表示的是地球上的坐标，而地球是一个球形，
        // 需要首先把经纬度转化到平面坐标系上
        // 值得一提的是，GPS2XYZ()并非把经纬度转化到世界坐标系下(以0经度，0纬度为原点)，
        // 而是以第一帧GPS数据为坐标原点，这一点需要额外注意
    GPS2XYZ(latitude, longitude, altitude, xyz);
        // 存入经纬度计算出的平面坐标，存入GPSPositionMap中
    vector<double> tmp{xyz[0], xyz[1], xyz[2], posAccuracy};
    GPSPositionMap[t] = tmp;
    newGPS = true;
}

再看inputOdom()：

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[t] = globalPose;
    lastP = globalP;
    lastQ = globalQ;
    // 把最新的全局姿态插入轨迹当中(过程略)
    ......
    global_path.poses.push_back(pose_stamped);
    mPoseMap.unlock();
}

现在两种数据都收到以后，万事俱备，我们看一下 void GlobalOptimization::optimize()这个函数：
这个函数开了一个线程来做优化：
首先使用ceres构建最小二乘问题，这个没啥可说的。

ceres::Problem problem;
ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
//options.minimizer_progress_to_stdout = true;
//options.max_solver_time_in_seconds = SOLVER_TIME * 3;
options.max_num_iterations = 5;
ceres::Solver::Summary summary;
ceres::LossFunction *loss_function;
loss_function = new ceres::HuberLoss(1.0);
ceres::LocalParameterization* local_parameterization = new ceres::QuaternionParameterization();

状态量赋初值，添加参数块。可以看出来，迭代的初始值是globalPoseMap中的值，也就是VIO转换到GPS坐标系下的值。

// 遍历历史里程计（转ENU系下了），显式添加变量参数
  map<double, vector<double>>::iterator iter;
  iter = globalPoseMap.begin();
  for (int i = 0; i < length; i++, iter++)
  {
      t_array[i][0] = iter->second[0];
      t_array[i][1] = iter->second[1];
      t_array[i][2] = iter->second[2];
      q_array[i][0] = iter->second[3];
      q_array[i][1] = iter->second[4];
      q_array[i][2] = iter->second[5];
      q_array[i][3] = iter->second[6];
      problem.AddParameterBlock(q_array[i], 4, local_parameterization);
      problem.AddParameterBlock(t_array[i], 3);
  }
 
  // 遍历历史里程计
  map<double, vector<double>>::iterator iterVIO, iterVIONext, iterGPS;

然后添加残差：

for (iterVIO = localPoseMap.begin(); iterVIO != localPoseMap.end(); iterVIO++, i++) {
    //vio factor
    // 添加VIO残差，观测量是两帧VIO数据之差，是相对的。而下面的GPS是绝对的
    iterVIONext = iterVIO;
    iterVIONext++;
    if (iterVIONext != localPoseMap.end()) {
        /// 计算两帧VIO之间的相对差（略）......
        ceres::CostFunction *vio_function = RelativeRTError::Create(iPj.x(), iPj.y(), iPj.z(),
                                                                    iQj.w(), iQj.x(), iQj.y(), iQj.z(),
                                                                    0.1, 0.01);
        problem.AddResidualBlock(vio_function, NULL, q_array[i], t_array[i], q_array[i + 1], t_array[i + 1]);
    }
    // gps factor
    // GPS残差，这个观测量直接就是GPS的测量数据，
    // 残差计算的是GPS和优化变量的差，这个是绝对的差。
    double t = iterVIO->first;
    iterGPS = GPSPositionMap.find(t);
    if (iterGPS != GPSPositionMap.end()) {
        ceres::CostFunction *gps_function = TError::Create(iterGPS->second[0], iterGPS->second[1],
                                                           iterGPS->second[2], iterGPS->second[3]);
        problem.AddResidualBlock(gps_function, loss_function, t_array[i]);
    }
 
}

优化完成后，再根据优化结果更新姿态就ok啦。为了防止VIO漂移过大，每次优化完成还需要计算一下VIO到GPS坐标系的变换。