VINS-Fusion 多传感器全局位姿估计的一种通用优化框架

云端舞步

于 2024-08-05 17:16:45 发布

阅读量155

点赞数 1

分类专栏： VINS-FUSION 文章标签： VINS-Fusion 全局位姿图优化 GPS Ceres

本文链接：https://blog.csdn.net/u010196944/article/details/140848255

版权

VINS-FUSION 专栏收录该内容

27 篇文章 4 订阅 ¥19.90 ¥99.00

订阅专栏

超级会员免费看

摘要

对于自动导航的机器人来说，精确的状态估计是基本问题。为了实现局部精确和全局无漂移的位姿估计，通常将具有互补属性的多个传感器进行融合。在一个小的区域内，局部传感器，如相机、IMU、Lidar等，提供了精确的位姿，而在一个大场景环境下，全局传感器，如gps、magnetometer、barometer等，提供有噪声但全局无漂移的定位。在本文中，我们提出一种传感器融合框架，将局部状态和全局传感器融合，实现局部精确和全局无漂移的位姿估计。现有的VO/VIO方法产生的局部估计，在位姿图优化中与全局传感器融合。在图优化中，局部估计被对齐到全局坐标系中。同时，累计漂移被消除。

通用优化框架如下图1所示。

图1 通用优化框架图。全局估计器将局部估计与多种全局传感器融合，为了获得局部精确和全局无偏移的位姿估计。

一、概述

根据测量的参考框架，我们将传感器分为局部和全局类型。

1.局部传感器：摄像头、激光雷达LiDAR、IMU（加速度计和陀螺仪）等。这类传感器不是全局参考的，因此通常需要一个参考框架。一般来说，机器人的第一个姿态被设置为原点，以便启动传感器，机器人的姿态从起点开始逐步演变。因此，累积的漂移会随着与起点的距离增加而增长。

2.全局传感器：全球定位系统GPS、磁力计、气压计等。这类传感器是全局参考的。它总是在一个固定的全局坐标系下工作，例如地球坐标系。参考坐标系的原点是固定的，并且事先已知。它们的测量是全局参考的，但有噪声。误差与行进的距离无关。对于GPS，它测量相对于地球的绝对经度、纬度和高度。经度、纬度和高度可以转换为x、y和z坐标。对于磁力计，它测量磁场的方向和强度，这可以确定方向。对于气压计，它测量气压，这可以转换为高度。我们的框架结构如图1所示。局部传感器（摄像头和IMU）用于局部估计。采用现有的视觉里程计VO/视觉惯性里程计VIO方法产生局部姿态。局部结果和全局传感器输入到全局位姿图中。它们被转换为统一的因素以构建优化问题。全局估计器生成局部精确和全局意识的6自由度姿态结果。

二、理论部分

1.局部位姿估计VO/VIO

只要能产生6-DOF局部位姿的VO/VIO，都能用我们的框架融合。举例利用VINS-FUSION 的VIO估计。

状态量：

式中，第k个 IMU状态xk， $\lambda$ 为特征点逆深度。

构造非线性最小二乘优化问题：

式中，残差构成分别为先验残差(边缘化)、惯性IMU残差和视觉残差(重投影)。

在局部坐标系下，VINS-FUSION VIO 获得了精确实时的6-DOF位姿估计。

2.全局位姿图结构

图2展示了全局位姿图结构的示意图。每个位姿包含世界坐标系中的位置和方向，作为位姿图中的一个节点。节点的密度由最低频率的传感器决定。两个连续节点之间的边是局部约束，来自局部估计（VO/VIO）。这条边限制了一个节点相对于另一个节点的相对位姿。其他边是全局约束，来自全局传感器。位姿图优化的本质是一个最大似然估计（MLE）问题。MLE由机器人在一段时间内的位姿联合概率分布组成。变量是所有节点的全局位姿， $\chi =\left \{ x_{0},x_{1},\cdots ,x_{n} \right \}$ ，其中 $x_{i} = \left \{ p_{i}^{w}, q_{i}^{w}\right \}$ 。 $p^{w}, q^{w}$ 分别是在全局坐标系下的位置和方向。在所有测量概率相互独立的前提下，问题通常被推导为

其中 S 是测量集合，包括局部测量（视觉里程计VO/视觉惯性里程计VIO）和全局测量（全球定位系统GPS、磁力计、气压计等）。我们假设测量的不确定性是具有均值和协方差的高斯分布，可表示为 $p(z_{t}^{k}\mid \chi ) \sim \eta (\tilde{z}_{t}^{k}, \Omega_{t}^{k} )$ 。因此，上述方程推导为：

马氏距离是 $\left \| r\right \|_{\Omega }^{2}=r^{T}\Omega^{-1}r$ 。然后状态估计被转换为一个非线性最小二乘问题，这也被称为束调整（Bundle Adjustment, BA）。

3.传感器因子

a.局部因子

由于局部估计（视觉里程计VO/VIO）在一个小区域内是准确的，我们利用两个帧 t-1 和帧 t 之间的相对位姿。考虑到两个连续的帧 t-1 和帧 t，局部因子被推导为：

其中， $(q_{t-1}^{l},p_{t-1}^{l})$ 和 $(q_{t}^{l},p_{t}^{l})$ 分别是在时间 t-1 和 t 的局部坐标系中由视觉里程计（VO/VIO）得到的姿态。 $\ominus$ 表示四元数误差状态的减法运算。第一行表示两个位姿之间的相对位置误差，第二行表示两个位姿之间的相对旋转误差。如果视觉里程计（VO/VIO）算法产生了位姿的协方差矩阵，我们将其用作局部测量的协方差。否则，我们使用统一的协方差来表示所有局部测量的协方差。

b.GPS因子

GPS的原始测量值是经度、纬度和高度，这些并不直接对应于x、y和z轴坐标。通常，我们可以将经度、纬度和高度转换为地心地固（ECEF, Earth Centred Earth Fixed）坐标、ENU（局部东-北-上）坐标或NED（局部北-东-下）坐标。这里，我们以ENU坐标为例。通过将第一次GPS测量设为原点，我们得到ENU世界坐标系中的GPS测量值 $p_{t}^{GPS}=\left [ x_{t}^{w} ,y_{t}^{w},z_{t}^{w}\right ]^{T}$ 。GPS因子被推导为：

GPS测量值直接限制了每个节点的位置。协方差由接收测量时的卫星数量决定。接收到的卫星越多，协方差就越小。

c.magnetometer因子

磁力计可以测量磁场强度的矢量。这个矢量的方向可以帮助确定在世界坐标系中的方向。我们假设磁力计已经过离线校准，没有偏移或偏差。首先，我们通过查找表得到当地区域在ENU坐标中的磁场强度 $z^{w}$ 。我们假设该区域内的磁力强度 $z^{w}$ 是恒定的。我们的测量表示为 $z_{t}^{m}$ 。如果我们将传感器与ENU坐标重合， $z_{t}^{m}$ 的方向应该与 $z^{w}$ 相匹配。受此启发，推导出的因子如下：

其中 $q_{b}^{m}$ 是从机器人中心到磁力计中心的变换，这是已知的并且已经离线校准过。由于磁场很容易受到环境的影响，我们只使用归一化的矢量而不使用长度。长度用于确定协方差。如果测量值的长度与 $z^{w}$ 的长度相差很大，我们设置一个较大的协方差。否则，我们使用较小的协方差。

d.barometer因子

气压计测量一个区域的气压。我们假设在一段时间内同一海拔高度的气压是恒定的。因此，气压可以线性地转换为高度。与GPS一样，我们将第一次测量的高度设为原点高度。然后我们得到了高度测量值 $z_{t}^{m}$ 。直观地说，因子是高度估计的残差，写成如下形式：

由于baromometer测量是有噪声的，我们计算短时间内多个测量值的方差，并将其用于代价函数中。

e.其它全局因子

虽然我们只详细指定了GPS、磁力计和气压计因子，但我们的系统不仅限于这些全局传感器。其他全局传感器甚至一些人为的传感器，如运动捕捉系统、WiFi和蓝牙指纹，也可以在我们的系统中使用。关键是要在同一个全局坐标系下将这些测量值建模为残差因子。

4.位姿图优化

一旦构建了位姿图，优化它就等于寻找尽可能匹配所有边的节点配置。Ceres Solver 被用于解决这个非线性问题，它以迭代的方式利用高斯-牛顿和Levenberg-Marquardt方法。我们以低频率（1Hz）运行位姿图优化。每次优化后，我们得到从局部坐标系到全局坐标系的转换。因此，我们可以通过这种转换将连续的高频率局部位姿（视觉里程计VO/视觉惯性里程计VIO，200Hz）转换为实时的高频率全局位姿。由于位姿图相当稀疏，计算复杂度与位姿的数量成线性增长。我们可以保持一个巨大的窗口进行位姿图优化，以获得准确且全局无漂移的姿态估计。当计算复杂度超过实时处理能力时，我们将丢弃旧的姿态和测量数据，并将窗口保持在有限的大小内。

三、代码解析

1.订阅数据

a.GPS_callback订阅全局传感器gps数据

订阅的数据存于类型为sensor_msgs::NavSatFixConstPtr队列gpsQueue中;

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

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

b.vio_callback订阅局部VIO数据

void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
{
    //处理里程计数据VIO
    ...

    global_estimator.inputOdom(t, vio_t, vio_q);

    
    //新一帧gps数据来时
    m_buf.lock();
    while(!gpsQueue.empty())
    {
        ...
        global_estimator.inputGPS(t, latitude, longitude, altitude, pos_accuracy);
    }
    m_buf.unlock();


    //发布位姿
    ...
    pub_global_odometry.publish(odometry);
    pub_global_path.publish(*global_path);
    publish_car_model(t, global_t, global_q);

    //将全局位姿写入文件
    std::ofstream foutC("/home/.../output/vio_global.csv", ios::app);
    ...
    foutC.close();
}

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

aa.inputOdom

将VIO位姿存于局部位姿map：localPoseMap。

将VIO位姿，通过转换矩阵WGPS_T_WVIO( $T_{VIO}^{GPS}$ )，得到对齐到全局坐标系下位姿globalPose，并存于全局位姿map: gloablPoseMap。同时将globalPose，压入global_path，用于发布。

void GlobalOptimization::inputOdom(double t, Eigen::Vector3d OdomP, Eigen::Quaterniond OdomQ)
{
	mPoseMap.lock();
    vector<double> localPose{OdomP.x(), OdomP.y(), OdomP.z(), 
    					     OdomQ.w(), OdomQ.x(), OdomQ.y(), OdomQ.z()};
    localPoseMap[t] = localPose;


    Eigen::Quaterniond globalQ;
    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;

    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();
}

bb.inputGPS

新一帧gps数据到来(!gpsQueue.empty())时，根据时间戳找到VIO数据10ms以内的gps数据

使用库geo函数geoConverter.Forward将gps经纬高转化为xyz数据，同时将xyz数据及pos精度存于GPS位置map：GPSPositionMap，置标志newGPS为true。

void GlobalOptimization::inputGPS(double t, double latitude, double longitude, double altitude, double posAccuracy)
{
	double xyz[3];
	GPS2XYZ(latitude, longitude, altitude, xyz);
	vector<double> tmp{xyz[0], xyz[1], xyz[2], posAccuracy};
    //printf("new gps: t: %f x: %f y: %f z:%f \n", t, tmp[0], tmp[1], tmp[2]);
	GPSPositionMap[t] = tmp;
    newGPS = true;

}

cc.发布全局位姿

Eigen::Vector3d global_t;
Eigen:: Quaterniond global_q;
global_estimator.getGlobalOdom(global_t, global_q);

nav_msgs::Odometry odometry;
odometry.header = pose_msg->header;
odometry.header.frame_id = "world";
odometry.child_frame_id = "world";
odometry.pose.pose.position.x = global_t.x();
odometry.pose.pose.position.y = global_t.y();
odometry.pose.pose.position.z = global_t.z();
odometry.pose.pose.orientation.x = global_q.x();
odometry.pose.pose.orientation.y = global_q.y();
odometry.pose.pose.orientation.z = global_q.z();
odometry.pose.pose.orientation.w = global_q.w();
pub_global_odometry.publish(odometry);
pub_global_path.publish(*global_path);
publish_car_model(t, global_t, global_q);

dd.全局位姿写入到文件

// write result to file
std::ofstream foutC("/home/.../output/vio_global.csv", ios::app);
foutC.setf(ios::fixed, ios::floatfield);
foutC.precision(0);
foutC << pose_msg->header.stamp.toSec() * 1e9 << ",";
foutC.precision(5);
foutC << global_t.x() << ","
		<< global_t.y() << ","
		<< global_t.z() << ","
		<< global_q.w() << ","
		<< global_q.x() << ","
		<< global_q.y() << ","
		<< global_q.z() << endl;
foutC.close();

2.创建发布主题

创建消息分别为"global_path"、"global_odomery"、"car_model"的主题pub_global_path，pub_global_odometry，pub_car用以发布数据。

至此，循环订阅，处理后得到的数据：

a.局部位姿map：localPoseMap；

b.全局位姿map: gloablPoseMap，需要利用局部位姿对齐到全局位姿的转换矩阵WGPS_T_WVIO( $T_{VIO}^{GPS}$ )；

c.GPS位置map：GPSPositionMap(每个gps位置数据10ms以内有VIO位姿)，标志newGPS为true。

3.位姿图优化

在创建类对象global_estimator时，在其构造函数GlobalOptimization中会开启位姿图优化2s线程。

GlobalOptimization::GlobalOptimization()
{
	initGPS = false;
    newGPS = false;
	WGPS_T_WVIO = Eigen::Matrix4d::Identity();
    threadOpt = std::thread(&GlobalOptimization::optimize, this);
}

析构函数

GlobalOptimization::~GlobalOptimization()
{
    threadOpt.detach();
}

在析构函数中，调用 threadOpt.detach() 将线程与 std::thread 对象分离。这表示线程不再与任何 std::thread 对象关联，它将继续执行，无法通过 std::thread 对象来控制。即 GlobalOptimization对象的生命周期结束时，其析构函数被调用，对象的资源被释放。但是，由于线程已经被分离，它不会等待线程结束，也不会尝试加入线程。

当GPS位置map：GPSPositionMap有新一帧数据时，标志newGPS为true，开启一次全局位姿图优化。

a.ceres优化参数设置

设置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();

b.AddParameterBlock添加优化参数

将全局位姿globalPoseMap作为优化状态，通过AddParameterBlock添加。

//add param
mPoseMap.lock();
int length = localPoseMap.size();
// w^t_i   w^q_i
double t_array[length][3];
double q_array[length][4];
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);
}

c.添加VIO因子

aa.计算局部位姿的相对位姿 $T_{j}^{i}$

Eigen::Matrix4d wTi = Eigen::Matrix4d::Identity();
Eigen::Matrix4d wTj = Eigen::Matrix4d::Identity();
wTi.block<3, 3>(0, 0) = Eigen::Quaterniond(iterVIO->second[3], iterVIO->second[4], 
										   iterVIO->second[5], iterVIO->second[6]).toRotationMatrix();
wTi.block<3, 1>(0, 3) = Eigen::Vector3d(iterVIO->second[0], iterVIO->second[1], iterVIO->second[2]);
wTj.block<3, 3>(0, 0) = Eigen::Quaterniond(iterVIONext->second[3], iterVIONext->second[4], 
										   iterVIONext->second[5], iterVIONext->second[6]).toRotationMatrix();
wTj.block<3, 1>(0, 3) = Eigen::Vector3d(iterVIONext->second[0], iterVIONext->second[1], iterVIONext->second[2]);
Eigen::Matrix4d iTj = wTi.inverse() * wTj;

bb.构建VIO因子代价函数

同样和闭环因子图优化一样，全局因子图优化也是利用ceres自动求得。

Eigen::Quaterniond iQj;
iQj = iTj.block<3, 3>(0, 0);
Eigen::Vector3d iPj = iTj.block<3, 1>(0, 3);

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]);

代价函数CostFunctor：在类中RelativeRTError实现creat，creat函数中创建自动求导代价函数AutoDiffCostFunction，其实参为代价函数类RelativeRTError，并在代价函数类中操作符operator中实现残差的计算。数字6，4，3，4，3分别表示残差维度，优化参数为全局前后两帧位姿：i帧四元数维度，i帧位置维度，j帧四元数维度，j帧位置维度。

static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z,
								   const double q_w, const double q_x, const double q_y, const double q_z,
								   const double t_var, const double q_var) 
{
  return (new ceres::AutoDiffCostFunction<
		  RelativeRTError, 6, 4, 3, 4, 3>(
			new RelativeRTError(t_x, t_y, t_z, q_w, q_x, q_y, q_z, t_var, q_var)));
}

接着操作符operator中实现残差的计算。

其中：

前后两帧位置残差 = 前后两帧全局优化相对位置 - VIO前后两帧相对位置。

形参w_q_i, ti, w_q_j, tj为优化参数，全局前后两帧位姿，计算得到优化参数前后两帧相对位置t_i_ij；t_x，t_y，t_z为VIO前后两帧相对位置。

前后两帧失准角残差 = 2*(VIO前后两帧相对四元数^T * 前后两帧全局优化相对四元数）(乘积结果取旋转向量*2，作为失准角，因为失准角误差是小量，旋转向量为isin(theta/2), jsin(theta/2), ksin(theta/2))。

bool operator()(const T* const w_q_i, const T* ti, const T* w_q_j, const T* tj, T* residuals) const
{
	T t_w_ij[3];
	t_w_ij[0] = tj[0] - ti[0];
	t_w_ij[1] = tj[1] - ti[1];
	t_w_ij[2] = tj[2] - ti[2];

	T i_q_w[4];
	QuaternionInverse(w_q_i, i_q_w);

	T t_i_ij[3];
	ceres::QuaternionRotatePoint(i_q_w, t_w_ij, t_i_ij);

	residuals[0] = (t_i_ij[0] - T(t_x)) / T(t_var);
	residuals[1] = (t_i_ij[1] - T(t_y)) / T(t_var);
	residuals[2] = (t_i_ij[2] - T(t_z)) / T(t_var);

	T relative_q[4];
	relative_q[0] = T(q_w);
	relative_q[1] = T(q_x);
	relative_q[2] = T(q_y);
	relative_q[3] = T(q_z);

	T q_i_j[4];
	ceres::QuaternionProduct(i_q_w, w_q_j, q_i_j);

	T relative_q_inv[4];
	QuaternionInverse(relative_q, relative_q_inv);

	T error_q[4];
	ceres::QuaternionProduct(relative_q_inv, q_i_j, error_q); 

	residuals[3] = T(2) * error_q[1] / T(q_var);
	residuals[4] = T(2) * error_q[2] / T(q_var);
	residuals[5] = T(2) * error_q[3] / T(q_var);

	return true;
}

c.添加GPS因子

gps因子只有位置相对比较简单，从GPS位置map：GPSPositionMap中取GPS位置作为观测量。

同样构建GPS残差代价函数CostFunctor：在类中TError实现creat，creat函数中创建自动求导代价函数AutoDiffCostFunction，其实参为代价函数类TError，并在代价函数类中操作符operator中实现残差的计算。数字3，3分别表示残差维度，优化参数为全局i帧位置维度。

接着操作符operator中实现残差的计算。

其中：

位置残差 = i帧全局优化位置 - VIO i 帧位置。

struct TError
{
	TError(double t_x, double t_y, double t_z, double var)
				  :t_x(t_x), t_y(t_y), t_z(t_z), var(var){}

	template <typename T>
	bool operator()(const T* tj, T* residuals) const
	{
		residuals[0] = (tj[0] - T(t_x)) / T(var);
		residuals[1] = (tj[1] - T(t_y)) / T(var);
		residuals[2] = (tj[2] - T(t_z)) / T(var);

		return true;
	}

	static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z, const double var) 
	{
	  return (new ceres::AutoDiffCostFunction<
	          TError, 3, 3>(
	          	new TError(t_x, t_y, t_z, var)));
	}

	double t_x, t_y, t_z, var;

};

d. ceres Solve求解

ceres::Solve(options, &problem, &summary);

e.更正全局位姿转换矩阵WGPS_T_WVIO( $T_{VIO}^{GPS}$ )

iter = globalPoseMap.begin();
for (int i = 0; i < length; i++, iter++)
{
	vector<double> globalPose{t_array[i][0], t_array[i][1], t_array[i][2],
							  q_array[i][0], q_array[i][1], q_array[i][2], q_array[i][3]};
	iter->second = globalPose;
	if(i == length - 1)
	{
		Eigen::Matrix4d WVIO_T_body = Eigen::Matrix4d::Identity(); 
		Eigen::Matrix4d WGPS_T_body = Eigen::Matrix4d::Identity();
		double t = iter->first;
		WVIO_T_body.block<3, 3>(0, 0) = Eigen::Quaterniond(localPoseMap[t][3], localPoseMap[t][4], 
														   localPoseMap[t][5], localPoseMap[t][6]).toRotationMatrix();
		WVIO_T_body.block<3, 1>(0, 3) = Eigen::Vector3d(localPoseMap[t][0], localPoseMap[t][1], localPoseMap[t][2]);
		WGPS_T_body.block<3, 3>(0, 0) = Eigen::Quaterniond(globalPose[3], globalPose[4], 
															globalPose[5], globalPose[6]).toRotationMatrix();
		WGPS_T_body.block<3, 1>(0, 3) = Eigen::Vector3d(globalPose[0], globalPose[1], globalPose[2]);
		WGPS_T_WVIO = WGPS_T_body * WVIO_T_body.inverse();
	}
}

计算全局位姿是转化矩阵WGPS_T_WVIO将VIO位姿转化为全局位姿(GPS系)，当Ceres全局位姿图优化后，需要将转化矩阵更新WGPS_T_WVIO( $T_{VIO}^{GPS}$ )，即VIO2GPS的外参。

f.发布全局位姿

发布map中全局位姿globalPoseMap。

void GlobalOptimization::updateGlobalPath()
{
    global_path.poses.clear();
    map<double, vector<double>>::iterator iter;
    for (iter = globalPoseMap.begin(); iter != globalPoseMap.end(); iter++)
    {
        geometry_msgs::PoseStamped pose_stamped;
        pose_stamped.header.stamp = ros::Time(iter->first);
        pose_stamped.header.frame_id = "world";
        pose_stamped.pose.position.x = iter->second[0];
        pose_stamped.pose.position.y = iter->second[1];
        pose_stamped.pose.position.z = iter->second[2];
        pose_stamped.pose.orientation.w = iter->second[3];
        pose_stamped.pose.orientation.x = iter->second[4];
        pose_stamped.pose.orientation.y = iter->second[5];
        pose_stamped.pose.orientation.z = iter->second[6];
        global_path.poses.push_back(pose_stamped);
    }
}

updateGlobalPath();