This column refers to the PDF of Cui Huakun’s “VINS Paper Derivation and Code Analysis”. According to this framework, the principle of VINS-Mono is explained in detail, and code analysis is attached~
The overall content is as follows:
VINS-Mono principle derivation and code analysis (1) Overall framework
VINS-Mono principle derivation and code analysis (2) IMU pre-integration
VINS-Mono principle derivation and code analysis (3) Back-end nonlinear optimization
VINS-Mono principle derivation and code analysis (4) Front-end visual processing
VINS-Mono principle derivation and code analysis (5) Initialization
VINS-Mono principle derivation and code analysis (6) Marginalization Maginalization and FEJ
VINS-Mono principle derivation and code analysis (7) Closed-loop detection and optimization
VINS-Mono principle derivation and code analysis (8) Other supplements~
1.State vector
The state vector includes a total of n + 1 states of all cameras within the sliding window (position, speed, orientation, acceleration bias and gyroscope bias), external parameters from Camera to IMU, and inverse depth of m + 1 3D points:
![X=[x_0,x_1,\cdotp\cdotp\cdotp x_n,x_c^b,\lambda_0,\lambda_1,\cdotp\cdotp\cdotp\lambda_m]](http://i2.wp.com/latex.csdn.net/eq?X=[x_0,x_1,\cdotp\cdotp\cdotpx_n,x_c^b,\lambda_0,\lambda_1,\cdotp\cdotp\cdotp\lambda_m])
![x_k=[p_{b_k}^w,v_{b_k}^w,q_{b_k}^w,b_a,b_g]](http://i2.wp.com/latex.csdn.net/eq?x_k=[p_{b_k}^w,v_{b_k}^w,q_{b_k}^w,b_a,b_g])
![x_c^b=[p_c^b,q_c^b]](http://i2.wp.com/latex.csdn.net/eq?x_c^b=[p_c^b,q_c^b])
2. Objective function
\right\|_{P_{b_{k+1}}^{b_k}}^2+\Sigma_{(l,j)\inC}\left\|r_C\left(\hat{z}_l^{c_j},X\right)\right\|_{P_l^{c_j}}^2\right\})
(19)
The three residual terms are marginalized prior information, IMU measurement residuals, and visual reprojection residuals. These three residuals are expressed by code distance.
According to the Gauss-Newton method in “Lecture 14”, to calculate the minimum value of the objective function, it can be understood that when the optimization variable has an increment, the value of the objective function is minimum. Taking the IMU residual as an example, it can be written in the following form :
\right\|_{P_{b_{k+1}}^{b_k}}^2=\min_{\deltaX}\left\|r_B\left(\hat{z}_{b_{k+1}}^{b_k},X\right)+J_{b_{k+1}}^{b_k}\DeltaX\right\|_{P_{b_{k+1}}^{b_k}}^2)
Where
calculation formula:
Then, the overall incremental equation of the overall objective function of formula (19) can be written as:
\Delta^{b_k}}^{-1}r_B+\sum{J_l^{C_j}}^T{P_l^{C_j}}^{-1}r_C)
In the above formula, 
Continue to simplify the above equation to:
\DeltaX=b_p+b_B+b_C)
Among them, 
Although the above Jacobian is the first derivative of the error term r with respect to the state vector src=”//i2.wp.com/latex.csdn.net/eq?\DeltaX”> is a small perturbation of the state vector, not the increment solved above
-\gamma(X)}{\deltaX})
3.IMU constraints
(1) Residual error
The IMU residual is the difference between the changes in PVQ and bias between two frames.
![r_B^{15\times1}(\hat{z}_{b_{k + 1}}^{b_k},X)=\begin{bmatrix} \delta\alpha_{b_{k + 1 }}^{b_k} \ \delta\theta_{b_{k + 1}}^{b_k} \ \delta\beta_{b_{k + 1}}^{b_k} \ \delta b_a \ \ delta b_g\end{bmatrix} = \begin{bmatrix} R_w^{b_k}\left(p_{b_{k + 1}}^w-p_{b_k}^w-v_{b_k}^w\Delta t_k + \frac12g^w\Delta t_k^2\right)-\alpha_{b_{k + 1}}^{b_k} \ \left.2\left[\gamma_{b_{k + 1}}^{b_k} \right.^{-1}\otimes q_{b_k}^{w_k-1}\otimes q_{b_{k + 1}}^w\right]_{xyz} \ R_w^{b_k}(v_{ b_{k + 1}}^w-v_{b_k}^w + g^w\Delta t_k)-\beta_{b_{k + 1}}^{b_k} \ b_{a_{b_{k + 1 }}}-b_{a_{b_k}} \ b_{\omega b_{k + 1}}-b_{\omega b_k}\end{bmatrix}](http://i2.wp.com/latex.csdn.net/eq?r_B^{15\times1}(\hat{z}_{b_{k+1}}^{b_k},X)=\begin{bmatrix}\delta\alpha_{b_{k+1}}^{b_k}\\delta\theta_{b_{k+1}}^{b_k}\\delta\beta_{b_{k+1}}^{b_k}\\deltab_a\\deltab_g\end{bmatrix}=\begin{bmatrix}R_w^{b_k}\left(p_{b_{k+1}}^w-p_{b_k}^w-v_{b_k}^w\Deltat_k+\frac12g^w\Deltat_k^2\right)-\alpha_{b_{k+1}}^{b_k}\\left.2\left[\gamma_{b_{k+1}}^{b_k}\right.^{-1}\otimesq_{b_k}^{w_k-1}\otimesq_{b_{k+1}}^w\right]_{xyz}\R_w^{b_k}(v_{b_{k+1}}^w-v_{b_k}^w+g^w\Deltat_k)-\beta_{b_{k+1}}^{b_k}\b_{a_{b_{k+1}}}-b_{a_{b_k}}\b_{\omegab_{k+1}}-b_{\omegab_k}\end{bmatrix})
(20)
The Jacobian of each increment with respect to bias can be obtained from the corresponding position in the large Jacobian of formula (16).
Then look at the implementation of the IMU residual definition in the code, in evaluate in vins_estimator/src/factor/integration_base.h
Eigen::Matrix<double, 15, 1> evaluate(const Eigen::Vector3d & amp;Pi, const Eigen::Quaterniond & amp;Qi, const Eigen::Vector3d & amp;Vi, const Eigen:: Vector3d &Bai, const Eigen::Vector3d &Bgi,
const Eigen::Vector3d & amp;Pj, const Eigen::Quaterniond & amp;Qj, const Eigen::Vector3d & amp;Vj, const Eigen::Vector3d & amp;Baj, const Eigen::Vector3d & amp;Bgj)
{
Eigen::Matrix<double, 15, 1> residuals;
Eigen::Matrix3d dp_dba = jacobian.block<3,3>(O_P, O_BA);
Eigen::Matrix3d dp_dbg = jacobian.block<3,3>(O_P, O_BG);
Eigen::Matrix3d dq_dbg = jacobian.block<3,3>(O_R, O_BG);
Eigen::Matrix3d dv_dba = jacobian.block<3,3>(O_V, O_BA);
Eigen::Matrix3d dv_dbg = jacobian.block<3,3>(O_V, O_BG);
Eigen::Vector3d dba = Bai - linearized_ba;
Eigen::Vector3d dbg = Bgi = linearized_bg;
Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg);
Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg;
Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg;
// Residuals
residuals.block<3,1>(O_P, 0) = Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - P_i - Vi * sum_dt) - corrected_delta_p;
residuals.block<3,1>(O_R, 0) = 2 * (corrected_delta_q.inverse() * (Qi.inverse() * Qj)).vec();
residuals.block<3,1>(O_V, 0) = Qi.inverse() * (G * sum_dt + Vj - Vi) - corrected_delta_v;
residuals.block<3,1>(O_BA, 0) = Baj - Bai;
residuals.block<3,1>(O_BG, 0) = Bgj - Bgi;
return residuals;
}
(2) Optimization variables
![[p_{b_k}^w,q_{b_k}^w],~[v_{b_k}^w,b_{a_k},b_{\omega_k}],~[p_{b_{k + 1}}^w,q_{b_{k + 1}}^w],~[v_{b_{k + 1}}^w,b_{a_{k + 1}},b_{\omega_{k + 1}}]](http://i2.wp.com/latex.csdn.net/eq?[p_{b_k}^w,q_{b_k}^w],~[v_{b_k}^w,b_{a_k},b_{\omega_k}],~[p_{b_{k+1}}^w,q_{b_{k+1}}^w],~[v_{b_{k+1}}^w,b_{a_{k+1}},b_{\omega_{k+1}}])
（3）Jacobian
When calculating the Jacobian, the partial derivative object corresponding to the residual is the above optimization variable, but the calculation is performed using a perturbation method, that is, the perturbation is ![[\delta p_{b_k}^w,\delta\theta_{b_k }^w],~[\delta v_{b_k}^w,\delta b_{a_k},\delta b_{\omega_k}],~[\delta p_{b_{k + 1}}^w,\delta \theta_{b_{k + 1}}^w],~[\delta v_{b_{k + 1}}^w,\delta b_{a_{k + 1}},\delta b_{\omega_{k + 1}}]](http://i2.wp.com/latex.csdn.net/eq?[\deltap_{b_k}^w,\delta\theta_{b_k}^w],~[\deltav_{b_k}^w,\deltab_{a_k},\deltab_{\omega_k}],~[\deltap_{b_{k+1}}^w,\delta\theta_{b_{k+1}}^w],~[\deltav_{b_{k+1}}^w,\deltab_{a_{k+1}},\deltab_{\omega_{k+1}}])
![J[0]^{15\times7}=\left[\frac{\partial r_B}{\partial p_{b_k}^w},\frac{\partial r_B}{\partial q_{b_k }^w}\right]==\begin{bmatrix}-R_w^{b_k} & amp;\left[R_w^{b_k}\left(p_{b_{k + 1}}^w-p_{b_k} ^w-v_{b_k}^w\Delta t_k + \frac12g^w\Delta t_k^2\right)\right]^{\land}\0 & amp;-\mathcal{L}[{q_{b_ {k + 1}}^w}^{-1}\otimes q_{b_k}^w]\mathcal{R}\left[\gamma_{b_{k + 1}}^{b_k}\right]\ 0 & amp;\left[R_w^{b_k}(v_{b_{k + 1}}^w-v_{b_k}^w + g^w\Delta t_k)\right]^{\land}\0 & amp;0\0 & amp;0\end{bmatrix}](http://i2.wp.com/latex.csdn.net/eq?J[0]^{15\times7}=\left[\frac{\partialr_B}{\partialp_{b_k}^w},\frac{\partialr_B}{\partialq_{b_k}^w}\right]==\begin{bmatrix}-R_w^{b_k}&\left[R_w^{b_k}\left(p_{b_{k+1}}^w-p_{b_k}^w-v_{b_k}^w\Deltat_k+\frac12g^w\Deltat_k^2\right)\right]^{\land}\0&-\mathcal{L}[{q_{b_{k+1}}^w}^{-1}\otimesq_{b_k}^w]\mathcal{R}\left[\gamma_{b_{k+1}}^{b_k}\right]\0&\left[R_w^{b_k}(v_{b_{k+1}}^w-v_{b_k}^w+g^w\Deltat_k)\right]^{\land}\\0&0\0&0\end{bmatrix})
![J[1]^{15\times9}=\left[\frac{\partial r_B}{\partial v_{b_k}^w},\frac{\partial r_B}{\partial b_{a_k }},\frac{\partial r_B}{\partial b_{w_k}}\right]=\begin{bmatrix}-R_W^{b_k}\Delta t & amp;-J_{b_a}^\alpha & amp; -J_{b_\omega}^\alpha\0 & amp;0 & amp;-\mathcal{L}\left[q_{b_{k + 1}}^w{}^{-1}\otimes q_ {b_k}^w\otimes\gamma_{b_{k + 1}}^{b_k}\right]J_{b_\omega}^\gamma\-R_W^{b_k} & amp;-J_{b_a}^ \beta & amp;-J_{b_\omega}^\beta\0 & amp;-I & amp;0\0 & amp;0 & amp;-I\end{bmatrix}](http://i2.wp.com/latex.csdn.net/eq?J[1]^{15\times9}=\left[\frac{\partialr_B}{\partialv_{b_k}^w},\frac{\partialr_B}{\partialb_{a_k}},\frac{\partialr_B}{\partialb_{w_k}}\right]=\begin{bmatrix}-R_W^{b_k}\Deltat&-J_{b_a}^\alpha&-J_{b_\omega}^\alpha\0&0&-\mathcal{L}\left[q_{b_{k+1}}^w{}^{-1}\otimesq_{b_k}^w\otimes\gamma_{b_{k+1}}^{b_k}\right]J_{b_\omega}^\gamma\-R_W^{b_k}&-J_{b_a}^\beta&-J_{b_\omega}^\beta\0&-I&0\0&0&-I\end{bmatrix})
![J[2]^{15\times7}=\left[\frac{\partial r_B}{\partial p_{b_{k + 1}}^w},\frac{\partial r_B}{ \partial q_{b_{k + 1}}^w}\right]=\begin{bmatrix}R_w^{b_k} & amp;0\0 & amp;\mathcal{L}\left[{\gamma_{ D_{k + 1}}^{b_k}}^{-1}\otimes{q_{b_k}^{w}}^{-1}\otimes{q_{D_{k + 1}}^{w} }\right]\0 & amp;0\0 & amp;0\0 & amp;0\end{bmatrix}](http://i2.wp.com/latex.csdn.net/eq?J[2]^{15\times7}=\left[\frac{\partialr_B}{\partialp_{b_{k+1}}^w},\frac{\partialr_B}{\partialq_{b_{k+1}}^w}\right]=\begin{bmatrix}R_w^{b_k}&0\0&\mathcal{L}\left[{\gamma_{D_{k+1}}^{b_k}}^{-1}\otimes{q_{b_k}^{w}}^{-1}\otimes{q_{D_{k+1}}^{w}}\right]\0&0\0&0\0&0\end{bmatrix})
![J[3]^{15\times9}=\left[\frac{\partial r_B}{\partial v_{b_{k + 1}}^w},\frac{\partial r_B}{ \partial b_{a_{k + 1}}},\frac{\partial r_B}{\partial b_{w_{k + 1}}}\right]=\begin{bmatrix}0 & amp;0 & amp; 0\0 & amp;0 & amp;0\R_w^{b_k} & amp;0 & amp;0\0 & amp;I & amp;0\0 & amp;0 & amp;I\ end{bmatrix}](http://i2.wp.com/latex.csdn.net/eq?J[3]^{15\times9}=\left[\frac{\partialr_B}{\partialv_{b_{k+1}}^w},\frac{\partialr_B}{\partialb_{a_{k+1}}},\frac{\partialr_B}{\partialb_{w_{k+1}}}\right]=\begin{bmatrix}0&0&0\0&0&0\R_w^{b_k}&0&0\0&I&0\0&0&I\end{bmatrix})
Then take a look at the code implementation part ~ in vins_estimator/src/factor/imu_factor.h
if(jacobians[0])
{
Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMahjor>> jacobian_pose_i(jacobians[0]);
jacobian_pose_i.setZero();
jacobian_pose_i.block<3,3>(O_P, O_P) = -Qi.inverse().toRotationMatirx();
jacobian_pose.i.block<3,3>(O_P, O_R) = Utility::skewSymmetric(Qi.inverse() * (0.5 * G * sum_dt * sum_dt * Pj - Pi - Vi * sum_dt));
#if 0
jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Qj.inverse() * Qi).toRotationMatrix();
#else
Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Utility::Qleft(Qj.inverse() * Qi) * Utility::Qright(corrected_delta_q)).bottomRightCorner<3, 3>();
#endif
jacobian_pose_i.block<3,3>(O_V, O_R) = Utility::skewSymmetric(Qi.inverse() * (G * sum_dt + Vj - Vi));
jacobian_pose_i = sqrt_info * jacobian_pose_i;
}
if(jacobians[1])
{
Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> jacobian_speedbias_i(jacobians[1]);
jacobian_speedbias_i.setZero();
jacobian_speedbias_i.block<3, 3>(O_P, O_V - O_V) = -Qi.inverse().toRotationMatrix() * sum_dt;
jacobian_speedbias_i.block<3, 3>(O_P, O_BA - O_V) = -dp_dba;
jacobian_speedbias_i.block<3, 3>(O_P, O_BG - O_V) = -dp_dbg;
#if 0
jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -dq_dbg;
#else
jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -Utility::Qleft(Qj.inverse() * Qi * pre_integration->delta_q).bottomRightCorner<3, 3>() * dq_dbg;
#endif
jacobian_speedbias_i.block<3, 3>(O_V, O_V - O_V) = -Qi.inverse().toRotationMatrix();
jacobian_speedbias_i.block<3, 3>(O_V, O_BA - O_V) = -dv_dba;
jacobian_speedbias_i.block<3, 3>(O_V, O_BG - O_V) = -dv_dbg;
jacobian_speedbias_i.block<3, 3>(O_BA, O_BA - O_V) = -Eigen::Matrix3d::Identity();
jacobian_speedbias_i.block<3, 3>(O_BG, O_BG - O_V) = -Eigen::Matrix3d::Identity();
jacobian_speedbias_i = sqrt_info * jacobian_speedbias_i;
}
if(jacobians[2])
{
Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMajor>> jacobian_pose_j(jacobians[2]);
jacobian_pose_j.setZero();
jacobian_pose_j.block<3, 3>(O_P, O_P) = Qi.inverse().toRotationMatrix();
#if 0
jacobian_pose_j.block<3, 3>(O_R, O_R) = Eigen::Matrix3d::Identity();
#else
Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
jacobian_pose_j.block<3, 3>(O_R, O_R) = Utility::Qleft(corrected_delta_q.inverse() * Qi.inverse() * Qj).bottomRightCorner<3, 3>();
#endif
jacobian_pose_j = sqrt_info * jacobian_pose_j;
}
if(jacobians[3])
{
Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> jacobian_speedbias_j(jacobians[3]);
jacobian_speedbias_j.setZero();
jacobian_speedbias_j.block<3, 3>(O_V, O_V - O_V) = Qi.inverse().toRotationMatrix();
jacobian_speedbias_j.block<3, 3>(O_BA, O_BA - O_V) = Eigen::Matrix3d::Identity();
jacobian_speedbias_j.block<3, 3>(O_BG, O_BG - O_V) = Eigen::Matrix3d::Identity();
jacobian_speedbias_j = sqrt_info * jacobian_speedbias_j;
}
Residual in the code is also multiplied by an information matrix sqrt_info, because the real optimization term is the Mahalanobis distance: 
)
^T(L^Tr))
Let
(4) Covariance
The IMU covariance P is the covariance of the IMU incremental error iterated out of the IMU pre-integration derived from equation (17).
4. Visual constraints
(1) Residual error
The visual residual is the reprojection error. For the l-th landmark point P, convert P from the i-th camera coordinate system when it was first viewed to the pixel coordinate system under the current j-th camera coordinate system, which can be Define the visual error term as:
=\begin{bmatrix}\vec{b}_1\\vec{b}_2\end{bmatrix}\left(\frac{P_l^{c_j}}{\left\|P_l^{c_j}\right\|}-\bar{P}_l^{c_j}\right))
Among them, 
)
![P_l^{c_j}=R_b^c\left\{R_w^{b_j}\left[R_{b_i}^w\left(R_c^b\frac{1}{\lambda_l}\bar{ P}_l^{c_i} + p_c^b\right) + p_{b_i}^w-p_{b_j}^w\right]-p_c^b\right\}](http://i2.wp.com/latex.csdn.net/eq?P_l^{c_j}=R_b^c\left\{R_w^{b_j}\left[R_{b_i}^w\left(R_c^b\frac{1}{\lambda_l}\bar{P}_l^{c_i}+p_c^b\right)+p_{b_i}^w-p_{b_j}^w\right]-p_c^b\right\})
(2) Optimization variables
![[p_{b_i}^w,q_{b_i}^w],~\left[p_{b_j}^w,q_{b_j}^w\right],~[p_c^b,q_c^ b],~\lambda_l](http://i2.wp.com/latex.csdn.net/eq?[p_{b_i}^w,q_{b_i}^w],~\left[p_{b_j}^w,q_{b_j}^w\right],~[p_c^b,q_c^b],~\lambda_l)
（3）Jacobian
According to the visual residual formula, the Jacobian relative to each optimization variable can be obtained.
![J[0]^{3\times7}=\left[\frac{\partial r_C}{\partial p_{b_i}^{w}},\frac{\partial r_C}{\partial q_ {b_i}^{w}}\right]=\left[R_{b}^{c}R_{w}^{b_j}\quad-R_{b}^{c}R_{w}^{b_j} R_{b_i}^{w}\left(R_{c}^{b}\frac{1}{\lambda_{l}}\bar{P}_{l}^{c_i} + p_{c}^ {b}\right)^{\wedge}\right]](http://i2.wp.com/latex.csdn.net/eq?J[0]^{3\times7}=\left[\frac{\partialr_C}{\partialp_{b_i}^{w}},\frac{\partialr_C}{\partialq_{b_i}^{w}}\right]=\left[R_{b}^{c}R_{w}^{b_j}\quad-R_{b}^{c}R_{w}^{b_j}R_{b_i}^{w}\left(R_{c}^{b}\frac{1}{\lambda_{l}}\bar{P}_{l}^{c_i}+p_{c}^{b}\right)^{\wedge}\right])
![J[1]^{3\times7}=\left[\frac{\partial r_{C}}{\partial p_{b_{j}}^{w}},\frac{\partial r_{C}}{\partial q_{b_{j}}^{w}}\right]=\left[-R_{b}^{c}R_{w}^{b_{j}}\quad R_ {b}^{c}\left\{R_{w}^{b_{j}}\left[R_{b_{i}}^{b}\left(R_{c}^{b}\frac{ \overline{P}_{l}^{c_{i}}}{\lambda_{l}} + p_{c}^{b}\right) + p_{b_{i}}^{w}-p_ {b_{j}}^{w}\right]\right\}^{\wedge}\right]](http://i2.wp.com/latex.csdn.net/eq?J[1]^{3\times7}=\left[\frac{\partialr_{C}}{\partialp_{b_{j}}^{w}},\frac{\partialr_{C}}{\partialq_{b_{j}}^{w}}\right]=\left[-R_{b}^{c}R_{w}^{b_{j}}\quadR_{b}^{c}\left\{R_{w}^{b_{j}}\left[R_{b_{i}}^{b}\left(R_{c}^{b}\frac{\overline{P}_{l}^{c_{i}}}{\lambda_{l}}+p_{c}^{b}\right)+p_{b_{i}}^{w}-p_{b_{j}}^{w}\right]\right\}^{\wedge}\right])
![J[2]^{3\times7}=\left[\frac{\partial r_C}{\partial p_c^b},\frac{\partial r_C}{\partial q_c^b}\right ]\=\begin{bmatrix}R_b^c\left(R_w^{b_j}R_{b_i}^{\prime\prime}-I_{3\times3}\right)\-R_b^cR_w^{b_j }R_{b_i}^{b_j}R_c^{b}\left(\frac{\bar{F}^{c_i}}{\lambda_l}\right)^{\wedge} + \left(R_b^cR_w^ {b_j}R_{b_l}^{w}R_c^{b}\frac{\bar{F}_l^{c_i}}{\lambda_l}\right)^{\wedge} + \left\{R_b^c \left[R_w^{b_j}\left(R_{b_i}^{b_j}p_c^{b} + p_{b_i}^{w}-p_{b_j}^{w}\right)-p_c^{b }\right]\right\}^{\wedge}\end{bmatrix}^T](http://i2.wp.com/latex.csdn.net/eq?J[2]^{3\times7}=\left[\frac{\partialr_C}{\partialp_c^b},\frac{\partialr_C}{\partialq_c^b}\right]\=\begin{bmatrix}R_b^c\left(R_w^{b_j}R_{b_i}^{\prime\prime}-I_{3\times3}\right)\-R_b^cR_w^{b_j}R_{b_i}^{b_j}R_c^{b}\left(\frac{\bar{F}^{c_i}}{\lambda_l}\right)^{\wedge}+\left(R_b^cR_w^{b_j}R_{b_l}^{w}R_c^{b}\frac{\bar{F}_l^{c_i}}{\lambda_l}\right)^{\wedge}+\left\{R_b^c\left[R_w^{b_j}\left(R_{b_i}^{b_j}p_c^{b}+p_{b_i}^{w}-p_{b_j}^{w}\right)-p_c^{b}\right]\right\}^{\wedge}\end{bmatrix}^T)
![J[3]^{3\times1}=\frac{\partial r_{C}}{\partial\lambda_{l}}=-R_{b}^{c}R_{w}^ {b_{j}}R_{b_{i}}^{w}R_{c}^{b}\frac{\bar{P}_{l}^{c_{i}}}{?{\lambda_ {l}}^{2}}](http://i2.wp.com/latex.csdn.net/eq?J[3]^{3\times1}=\frac{\partialr_{C}}{\partial\lambda_{l}}=-R_{b}^{c}R_{w}^{b_{j}}R_{b_{i}}^{w}R_{c}^{b}\frac{\bar{P}_{l}^{c_{i}}}{{\lambda_{l}}^{2}})
The code is implemented in vins_estimator/src/factor/projection_factor.cpp
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
}
(4) Covariance
The noise covariance of visual constraints is related to the reprojection error when calibrating the camera internal parameters. Here, 1.5 pixels are taken. The covariance matrix corresponding to the normalized camera plane needs to be divided by the focal length f, then the information matrix is equal to the inverse of the covariance matrix. ,for:
^{-1}=\fracf{1.5}I_{2\times2})