vins中的gps与vio融合

vins的这个部分简短点描述就是两个约束+一个外参：

• vio算出来的两个位姿提供相对位姿约束
• gps数据提供绝对的位姿约束
• 外参：是利用队列里的最后一个数据进行求逆变换获得

相对位姿约束

​ vio测量的两个相邻点之间提供融合后点的相对位姿约束关系，优化部分定义在RelativeRTError类中。

// 相对位置残差
residuals[0] = (t_i_ij[0] - T(t_x)) / T(t_var);// t_i_ij是融合后的ij相对位移在i帧
residuals[1] = (t_i_ij[1] - T(t_y)) / T(t_var);
residuals[2] = (t_i_ij[2] - T(t_z)) / T(t_var);
ceres::QuaternionProduct(relative_q_inv, q_i_j, error_q); 
// 旋转残差
residuals[3] = T(2) * error_q[1] / T(q_var);// error_q是相对位姿的差的李代数
residuals[4] = T(2) * error_q[2] / T(q_var);
residuals[5] = T(2) * error_q[3] / T(q_var);

绝对位置约束

​ gps测量提供融合后点的绝对位姿约束关系， 优化部分定义在TError类中。

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

外参获得

​ 在把优化出的最后的数据从参数块中拷贝出来后，对于最后一组数据进行计算

WGPS_T_WVIO = WGPS_T_body * WVIO_T_body.inverse();

其中 WGPS_T_body是gps约束下把vio数据拉到gps坐标系下的数据

WVIO_T_body是vio发送过来的数据其参考坐标是vio的坐标系