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的坐标系