转载自：https://blog.csdn.net/hltt3838/article/details/109725845
贵在坚持，不忘初心 2020-11-16 20:55:42 237 收藏 5
分类专栏： VINS-Mono and Fusion 程序解读
版权
源代码：VINS - Fusion
数据集：KITTI 数据
程序入口：globalOptNode.cpp
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上。
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 队列里面
ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 100, vio_callback);
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();
}
理解：
localPoseMap[t] ： VIO 的位姿
globalPoseMap[t] ：VIO 的位姿 经过 坐标变换 转到 世界坐标系下的 位姿，也是GPS和VIO融合后的位姿！
这个函数把vio的结果存储在 localPoseMap 中，然后使用外参 WGPS_T_WVIO 把VIO的结果转换到GPS坐标系下存储在 globalPoseMap 中。
注意，此时我们考虑刚开始gps没对齐时，此时外参 WGPS_T_WVIO 是单位矩阵，所以globalPoseMap里的位姿和VIO的结果一样。
globalPoseMap 是存储融合优化后的位姿的，这也符合逻辑：在没有gps数据时，融合结果完全和VIO一样，而且频率也比GPS位姿更新频率高！
而 外参 WGPS_T_WVIO 也是我们需要优化的参数！
while(!gpsQueue.empty())//有GPS数据时执行
{
//获取最早的GPS数据和其时间
sensor_msgs::NavSatFixConstPtr GPS_msg = gpsQueue.front();
double gps_t = GPS_msg->header.stamp.toSec();
printf("vio t: %f, gps t: %f \n", t, gps_t);
// +- 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;
globalEstimator.inputGPS(t, latitude, longitude, altitude, pos_accuracy);
gpsQueue.pop();//注意这里
break;//此处break,意味只存储了一个GPS数据后就break了
}
else if(gps_t < t - 0.01)
gpsQueue.pop();
else if(gps_t > t + 0.01)
break;
}
m_buf.unlock();子程序 globalEstimator.inputGPS(t, latitude, longitude, altitude, pos_accuracy);
void GlobalOptimization::inputGPS(double t, double latitude, double longitude, double altitude, double posAccuracy)
{
double xyz[3];
GPS2XYZ(latitude, longitude, altitude, xyz);
// 存入经纬度计算出的平面坐标，存入GPSPositionMap中
vector<double> tmp{xyz[0], xyz[1], xyz[2], posAccuracy};
GPSPositionMap[t] = tmp;
newGPS = true;
}目的：GPS的 经纬高 转换成 XYZ，并放入全局变量 GPSPositionMap[t] 里面！
注意：经纬度表示的是地球上的坐标，而地球是一个球形， 需要首先把经纬度转化到平面坐标系上，值得一提的是，
GPS2XYZ() 并非把经纬度转化到世界坐标系下(以0经度，0纬度为原点)， 而是以第一帧GPS数据为坐标原点 ！
输入VIO数据的 10ms内 的GPS数据进行坐标系转换,GPS2XYZ函数将GPS的经纬度坐标转换为ENU坐标,并且第一帧
的GPS数据作为原点[0,0,0]，转换之后的gps数据和协方差一起存入到GPSPositionMap中；这时的 GPSPositionMap 便是观测值！
且此时，newGPS = true，意味满足优化的条件！后面我们会感受到程序设计的巧妙！
Eigen::Vector3d global_t;
Eigen:: Quaterniond global_q;
globalEstimator.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);
// 位姿写入文本文件（略）......
std::ofstream foutC("/home/vins_fusion/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();
}子程序 globalEstimator.getGlobalOdom(global_t, global_q);
void GlobalOptimization::getGlobalOdom(Eigen::Vector3d &odomP, Eigen::Quaterniond &odomQ)
{
odomP = lastP;
odomQ = lastQ;
}目的：实现全局位姿 globalPoseMap[t] 的更新
到此为止，订阅VIO的程序结束，而下面 就是GPS/VIO融合后需要发布的数据，如：
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);疑问：GPS和VIO融合的程序在哪？
答：在main 函数 里面的 global_path = &globalEstimator.global_path;
GlobalOptimization::GlobalOptimization()
{
initGPS = false;
newGPS = false;
WGPS_T_WVIO = Eigen::Matrix4d::Identity();
threadOpt = std::thread(&GlobalOptimization::optimize, this);
}这一部分其实在 main() 最前面的，只是被我放进了最后面，即使放在了最前面 融合程序也不会执行，
因为 newGPS = fause, 进不去 GlobalOptimization::optimize() 里面 ！
这一块在进行程序讲解之前先进行理论的梳理！ 如下图：
要估计出 x0、x1、x2 . . . xn 这些时刻的位姿， 我有的是两个方面的观测值，一方面是GPS得到的每个时刻的位置（x,y,z）
（并且GPS信号可以提供在该时刻得到这个位置的精度posAccuracy），没有累计误差；另一方面是VIO得到的每个时刻的位置（x,y,z）
以及对应的姿态四元数（w,x,y,z）,存在累计误差，短时间内精度较高。
为了得到更好的一个融合结果，因此我们采用这个策略：观测值中，初始位置由GPS提供，并且vio观测值信任的是 i到j时刻 的 位移以及姿态变化量，
并不信任vio得到的一个绝对位移量以及绝对的旋转姿态量。只信任短期的i到j的变化量，用这个变化量作为整个代价函数的观测值，进行求解。
因此两个残差项 TError 及 RelativeRTError 分别对应的就是GPS位置信号以及vio短时间内估计的 i到j时刻 的位姿变化量对最终结果的影响。
建议：仔细理解上面一段话的意思，后面看程序会很清晰！还有就是把Ceres 这个工具搞明白！
（1）ceres构建最小二乘问题， 参考
void GlobalOptimization::optimize()
{
while(true)
{
if(newGPS)//得到GPS信号，并且与vio结果时间同步后
{
newGPS = false;
printf("global optimization\n");
TicToc globalOptimizationTime;
//定义损失函数，首先使用ceres构建最小二乘问题，这个没啥可说的状态量赋初值，添加参数块。
ceres::Problem problem;
ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
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();
//add param，迭代的初始值是globalPoseMap中的值，也就是VIO转换到GPS坐标系下的值。
mPoseMap.lock();
int length = localPoseMap.size(); //VIO位姿的个数
// 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);
}理解：
ceres::LocalParameterization* local_parameterization = new ceres::QuaternionParameterization();
这段代码的目的是为了解决 姿态有四个变量，但是只有三个自由度问题，姿态用四元数表示的！
int length = localPoseMap.size();
订阅VIO信息时，VIO位姿的个数，也是后面我们需要优化参数 块的个数！
优化的变量是 旋转q_array[length][4] 和 平移t_array[length][3]; 他们的初值是VIO的第一帧数据，
也等价于 第一个 全局位姿 globalPoseMap.begin()； 然后把他们加入优化参数块即可 ！
（2）添加 VIO两帧之间的残差项
map<double, vector<double>>::iterator iterVIO, iterVIONext, iterGPS;
int i = 0;
//然后添加残差：
for (iterVIO = localPoseMap.begin(); iterVIO != localPoseMap.end(); iterVIO++, i++)
{
//vio factor，添加VIO残差，观测量是两帧VIO数据之差，是相对的。而下面的GPS是绝对的
iterVIONext = iterVIO;
iterVIONext++;
if(iterVIONext != localPoseMap.end())
{
Eigen::Matrix4d wTi = Eigen::Matrix4d::Identity();
Eigen::Matrix4d wTj = Eigen::Matrix4d::Identity();
//i，对应的四元素
wTi.block<3, 3>(0, 0) = Eigen::Quaterniond(iterVIO->second[3], iterVIO->second[4], iterVIO->second[5], iterVIO->second[6]).toRotationMatrix();
//i，对应的位置
wTi.block<3, 1>(0, 3) = Eigen::Vector3d(iterVIO->second[0], iterVIO->second[1], iterVIO->second[2]);
//j，对应的四元素
wTj.block<3, 3>(0, 0) = Eigen::Quaterniond(iterVIONext->second[3], iterVIONext->second[4], iterVIONext->second[5], iterVIONext->second[6]).toRotationMatrix();
//j，对应的位置
wTj.block<3, 1>(0, 3) = Eigen::Vector3d(iterVIONext->second[0], iterVIONext->second[1], iterVIONext->second[2]);
//i 到 j 转换的四元素
Eigen::Matrix4d iTj = wTi.inverse() * wTj;
Eigen::Quaterniond iQj;
iQj = iTj.block<3, 3>(0, 0);
Eigen::Vector3d iPj = iTj.block<3, 1>(0, 3);
//计算两帧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]);
}理解：
wTi：i 时刻VIO结果的旋转R和平移t, 构成 Ti
wTj：i 下一时刻VIO结果的旋转R和平移t, 构成 Tj
iTj: j 时刻 转换到 i时刻 的 4*4 矩阵，包含旋转iQj 和 平移iPj
然后根据上面的信息，构造 相邻两帧 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);
具体的程序在 Factors.h 里面的结构体，如下：
struct RelativeRTError
{
//结构体初始化
RelativeRTError(double t_x, double t_y, double t_z,
double q_w, double q_x, double q_y, double q_z,
double t_var, double q_var)
:t_x(t_x), t_y(t_y), t_z(t_z),
q_w(q_w), q_x(q_x), q_y(q_y), q_z(q_z),
t_var(t_var), q_var(q_var){}
//构建代价函数
template <typename T>
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]; //VIO和GPS优化后，世界坐标系 i、j帧的位置增量
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]; //i帧的四元数逆
QuaternionInverse(w_q_i, i_q_w);
//世界坐标系下，i、j帧的位置增量t_w_ij，经i_q_w 转换到i帧的坐标系，与 t_x 求差！
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]; //传入VIO观测的四元数增量
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;
}
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)));
}
double t_x, t_y, t_z, t_norm;
double q_w, q_x, q_y, q_z;
double t_var, q_var;
};注意：哪些是VIO的 两帧间的变化，哪些是 优化后 globalPoseMap 的位姿态！
思路：VIO和GPS优化后的POS增量 投影到 VIO下，然后和VIO下的POS 增量作差，求残差！
我以前看这些代码很迷糊，但是你把Ceres熟悉之后，在明白VIO和GPS的优化原理，就很简单，下图是自己画的示意图：
VIO的数据和状态量的残差定义为：（j时刻的状态量 - i时刻的状态量）得到的增量-vio增量; 其意味着融合后的位置必须
和GPS位置尽可能重合，而两帧间的增量要和VIO尽可能相等。这对理解坐标转换至关重要，这样的处理意味着vio的数据
并不对融合后的绝对位置做约束，只要求融合后的位置增量和vio的增量误差尽可能小, 所以融合后的位置会在GPS坐标系下。
（3）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坐标系的变换。
ceres::Solve(options, &problem, &summary);再来看一下 struct TError 结构体
struct TError
{
//定义数据的 t_x 、t_y 、 t_z 和 var 的析构函数
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;
};理解：根据程序可知，残差是：VIO/GPS融合后的位置（优化变量） - gps观测值， 到此，优化部分结束！
思考：自己想一下，优化结束了干啥？当然是数据的更新了，这个很简单吧，不然你优化的目的是干嘛的呢！
那我们需要更新哪些数据呢？ 这个其实也不难，我们先看程序部分
（4）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(); //T R t
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]);
//更新世界坐标系与VIO间的外参，不再是单位矩阵
WGPS_T_WVIO = WGPS_T_body * WVIO_T_body.inverse();
}
}
//更新全局位姿函数
updateGlobalPath();
//printf("global time %f \n", globalOptimizationTime.toc());
mPoseMap.unlock();
}
std::chrono::milliseconds dura(2000);
std::this_thread::sleep_for(dura);
}
return;
}理解：
经过VIO和GPS优化后，我们可以得到一个更加准确的全局位姿，globalPose ！因为全局位姿准确了，所以我们可以
用来更新VIO到GPS的转换矩阵WGPS_T_WVIO， 这是很容易理解的事情！因为开始我们的转换矩阵WGPS_T_WVIO
设置的是单位矩阵，有人就问，为啥是单位矩阵，这不就意味这开始VIO和GPS是重和的嘛，上面其实已经解释了，第一：没有GPS数据
情况下，VIO结果就是全局的位姿！第二：不是单位矩阵，那你说是什么矩阵？你说呀！你又说不出来更好的！第三：我们后面不是优化了嘛，
开始的单位矩阵仅仅是一个初始的值，设置合理即可！
注意：
WGPS_T_WVIO = WGPS_T_body * WVIO_T_body.inverse()
这导致了WGPS_T_WVIO这个外参包括了VIO计算的误差。什么意思呢，本来当vio足够精准时，
这样算出的WGPS_T_WVIO肯定是一个固定值。实际把WGPS_T_WVIO输出看，会随着时间一直变化。
变化原因是因为vio的结果和融合后的结果均带误差，WGPS_T_body * WVIO_T_body.inverse() 使得误差包含在里面了。
既然，转换矩阵 WGPS_T_WVIO 更新结束了，下面就需要更新全局位姿了！
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);
}
}到此，VIO和GPS的优化结束！其实是很容易的一件事情，也可能是我看程序亭容易的，但是自己写的话也会有很多的问题！
还要加强程序编写能力！