VINS-FUSION 优化-视觉误差因子

大家好，欢迎来到IT知识分享网。

一、视觉误差因子

VINS-FUSION 优化问题可以统一用损失函数表示如下：

式中，包括先验误差因子（滑窗边缘化产生），IMU预积分误差因子，视觉误差因子。本文将结合论文和对应源码详细叙述。

在经典的视觉slam算法中，视觉误差通常用重投影误差表示。
重投影误差：指的真实三维空间点在图像平面上的投影（也就是图像上的像素点）和重投影（其实是用我们的计算值得到的虚拟的像素点）的差值。
在计算重投影误差时，有两种特征参数化方式：特征点3D位置和特征点深度/逆深度。

1.特征点3D位置

其中：3D位置 $P_{l} = [x_{l},y_{l},z_{l}]^{T}$ ；

$z_{l}^{k}$ 表示特征点l在第k帧中的观测；

$((R_{c_{k}}^{w})^{T}, p_{c_{k}}^{w})$ 相机在global位姿；

$\pi (\cdot )$ 表示相机重投影模型，将3D特征点去畸变。

2.深度/逆深度

从第i帧到j帧重投影误差可以表示如下：

其中： $\lambda _{i}$ 表示第i帧深度；

实质上两者是一致的，可以相互转化。

可以把 $P_{l}$ 看成是i帧特征在global位姿，计算如下：

$P_{l} =(R_{c_{i}}^{w})^{T}\lambda_{i} \begin{pmatrix} z_{l}^{i}\\ 1 \end{pmatrix} + p_{c_{i}}^{w}$

二、vins-fusion源码中视觉误差因子

1.求视觉残差（重投影误差）

bool ProjectionFactor::Evaluate(double const *const *parameters, double *residuals, double jacobians) const { TicToc tic_toc; Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]); Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]); Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]); double inv_dep_i = parameters[3][0]; Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i; Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj); Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic); Eigen::Map<Eigen::Vector2d> residual(residuals); #ifdef UNIT_SPHERE_ERROR residual = tangent_base * (pts_camera_j.normalized() - pts_j.normalized()); #else double dep_j = pts_camera_j.z(); residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>(); #endif ... }

其中：Pi,Qi表示i帧位姿，对应公式为 $(p_{b_{i}}^{w}, q_{b_{i}}^{w})$ ；

Pj,Qj表示i帧位姿，对应公式为 $(p_{b_{j}}^{w}, q_{b_{j}}^{w})$ ；

tic,qic表示外参，对应公式为 $(p_{c}^{b},q_{c}^{b})$ ；

inv_dep_i 表示特征点l的逆深度值;

pts_i表示为该特征点在第i帧被观测到；

pts_j表示为该特征点在第j帧被观测到；

pts_i，pts_j表示为global的特征点被i, j帧同时观测到，是同一个global的特征点。

将上述代码转译成公式如下：

对比如下公式：

其实就是上述深度/逆深度参数，从第i帧到j帧重投影误差公式的展开形式。

2.雅可比矩阵

残差对优化变量的雅可比矩阵。

残差：

优化变量 $\chi$ ： $(p_{b_{i}}^{w}, q_{b_{i}}^{w})$ ， $(p_{b_{j}}^{w}, q_{b_{j}}^{w})$ ， $(p_{c}^{b},q_{c}^{b})$ , $\lambda _{l}$

根据链式求导法则：

$jacobians = \frac{\delta (e)}{\delta (p)} \frac{\delta (p)}{\delta (\chi)}$

其中： $p=[u_{i},v_{i},z_{j}]$

（1）残差对p的雅可比矩阵

$\frac{\delta (e)}{\delta (p)}=\begin{vmatrix} \frac{1}{z_{j}} & 0 & - \frac{p_{l}^{c_{j}}(0)}{z_{j}^2}\\ 0&\frac{1}{z_{j}} & - \frac{p_{l}^{c_{j}}(1)}{z_{j}^2}\end{vmatrix}$

(2)残差对 $(p_{b_{i}}^{w}, q_{b_{i}}^{w})$ 的雅可比矩阵

(3)残差对 $(p_{b_{j}}^{w}, q_{b_{j}}^{w})$ 的雅可比矩阵

(3)残差对 $(p_{c}^{b},q_{c}^{b})$ 的雅可比矩阵

$\begin{pmatrix} \frac{\partial r_{c}}{\partial p_{c}^{b}}& \frac{\partial r_{c}}{\partial q_{c}^{b}} \end{pmatrix}$

(4)残差对 $\lambda _{l}$ 的雅可比矩阵

雅克比矩阵对应源码：

bool ProjectionFactor::Evaluate(double const *const *parameters, double *residuals, double jacobians) const { ... #ifdef UNIT_SPHERE_ERROR residual = tangent_base * (pts_camera_j.normalized() - pts_j.normalized()); #else double dep_j = pts_camera_j.z(); residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>(); #endif residual = sqrt_info * residual; if (jacobians) { Eigen::Matrix3d Ri = Qi.toRotationMatrix(); Eigen::Matrix3d Rj = Qj.toRotationMatrix(); Eigen::Matrix3d ric = qic.toRotationMatrix(); Eigen::Matrix<double, 2, 3> reduce(2, 3); #ifdef UNIT_SPHERE_ERROR double norm = pts_camera_j.norm(); Eigen::Matrix3d norm_jaco; double x1, x2, x3; x1 = pts_camera_j(0); x2 = pts_camera_j(1); x3 = pts_camera_j(2); norm_jaco << 1.0 / norm - x1 * x1 / pow(norm, 3), - x1 * x2 / pow(norm, 3), - x1 * x3 / pow(norm, 3), - x1 * x2 / pow(norm, 3), 1.0 / norm - x2 * x2 / pow(norm, 3), - x2 * x3 / pow(norm, 3), - x1 * x3 / pow(norm, 3), - x2 * x3 / pow(norm, 3), 1.0 / norm - x3 * x3 / pow(norm, 3); reduce = tangent_base * norm_jaco; #else reduce << 1. / dep_j, 0, -pts_camera_j(0) / (dep_j * dep_j), 0, 1. / dep_j, -pts_camera_j(1) / (dep_j * dep_j); #endif reduce = sqrt_info * reduce; if (jacobians[0]) { Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_pose_i(jacobians[0]); Eigen::Matrix<double, 3, 6> jaco_i; jaco_i.leftCols<3>() = ric.transpose() * Rj.transpose(); jaco_i.rightCols<3>() = ric.transpose() * Rj.transpose() * Ri * -Utility::skewSymmetric(pts_imu_i); jacobian_pose_i.leftCols<6>() = reduce * jaco_i; jacobian_pose_i.rightCols<1>().setZero(); } if (jacobians[1]) { Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_pose_j(jacobians[1]); Eigen::Matrix<double, 3, 6> jaco_j; jaco_j.leftCols<3>() = ric.transpose() * -Rj.transpose(); jaco_j.rightCols<3>() = ric.transpose() * Utility::skewSymmetric(pts_imu_j); jacobian_pose_j.leftCols<6>() = reduce * jaco_j; jacobian_pose_j.rightCols<1>().setZero(); } if (jacobians[2]) { Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_ex_pose(jacobians[2]); Eigen::Matrix<double, 3, 6> jaco_ex; jaco_ex.leftCols<3>() = ric.transpose() * (Rj.transpose() * Ri - Eigen::Matrix3d::Identity()); Eigen::Matrix3d tmp_r = ric.transpose() * Rj.transpose() * Ri * ric; jaco_ex.rightCols<3>() = -tmp_r * Utility::skewSymmetric(pts_camera_i) + Utility::skewSymmetric(tmp_r * pts_camera_i) + Utility::skewSymmetric(ric.transpose() * (Rj.transpose() * (Ri * tic + Pi - Pj) - tic)); jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex; jacobian_ex_pose.rightCols<1>().setZero(); } if (jacobians[3]) { Eigen::Map<Eigen::Vector2d> jacobian_feature(jacobians[3]); #if 1 jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i * -1.0 / (inv_dep_i * inv_dep_i); #else jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i; #endif ... }

其中：

reduce与 $\frac{\delta (e)}{\delta (p)}$ 相对应；

jacobians[0]与 $\begin{pmatrix} \frac{\partial r_{c}}{\partial p_{b_{i}}^{w}}& \frac{\partial r_{c}}{\partial q_{b_{i}}^{w}} \end{pmatrix}$ 相对应；

jacobians[1]与 $\begin{pmatrix} \frac{\partial r_{c}}{\partial p_{b_{j}}^{w}}& \frac{\partial r_{c}}{\partial q_{b_{j}}^{w}} \end{pmatrix}$ 相对应；

jacobians[2]与 $\begin{pmatrix} \frac{\partial r_{c}}{\partial p_{c}^{b}}& \frac{\partial r_{c}}{\partial q_{c}^{b}} \end{pmatrix}$ 相对应；

jacobians[3]与 $\frac{\delta (r_{c})}{\delta (\lambda_{l})}$ 相对应。

3.协方差

视觉约束的噪声协方差与标定相机内参的重投影误差相关。即视觉约束的噪声协方差可以根据标定内参时重投影误差来取定。代码中选取的是偏移1.5个像素，对应到归一化相机平面的协方差矩阵需要除以焦距f。而信息矩阵sqrt_info等于协方差矩阵的逆，如下：

sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();

至此，论文公式和源码就一一对应起来了。

三、参考

1.VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator

2.Online Temporal Calibration for Monocular Visual-Inertial Systems

3.https://github.com/HKUST-Aerial-Robotics/VINS-Fusion

免责声明：本站所有文章内容,图片，视频等均是来源于用户投稿和互联网及文摘转载整编而成，不代表本站观点，不承担相关法律责任。其著作权各归其原作者或其出版社所有。如发现本站有涉嫌抄袭侵权/违法违规的内容,侵犯到您的权益，请在线联系站长,一经查实,本站将立刻删除。本文来自网络,若有侵权，请联系删除，如若转载，请注明出处：https://haidsoft.com/148387.html