Article From:https://www.cnblogs.com/112358nizhipeng/p/9057943.html

## IMUModel and motion integral

$R_{\tiny{WB}} \left( t +\Delta{t} \right) = R_{\tiny{WB}} \left( t \right) Exp\left( \int_{t} ^{t+\Delta{t}} {}_{\tiny{B}} \omega_{\tiny{WB}} \left( \tau \right) d\tau \right)$

${}_{\tiny{W}}V \left(t+\Delta{t} \right) = {}_{\tiny{W}}V\left( t \right) + \int _{t} ^{t+\Delta{t}} {}_{\tiny{W}}a \left( \tau \right)d\tau$

${}_{\tiny{W}}P \left(t+\Delta{t} \right) = {}_{\tiny{W}}P\left( t \right) + \int _{t} ^{t+\Delta{t}} {}_{\tiny{W}}V \left( \tau \right)d\tau \,+\int \int _{t} ^{t+\Delta{t}}{}_{\tiny{W}}a \left( \tau \right)d\tau^2$

Among them, the IMU reading, that is, the measured value (the theoretical value under the influence of bias and noise) is

${}_{\tiny{B}} \tilde{ \omega }_{\tiny{WB}} \left( t \right) = {}_{\tiny{B}} \omega_{\tiny{WB}} \left( t \right) + b^{g} \left( t \right) +\eta^{g}\left( t \right)$

${}_{\tiny{B}} \tilde{ a } \left( t \right) = R_{\tiny{WB}}^{T} \left( t \right) \left( {}_{\tiny{W}}a\left( t \right) – {}_{\tiny{W}}g\right) + b^a\left( t \right) + \eta^a \left( t \right)$

Assume that in the time interval $left [t, t + Delta {t} right]$, ${{ tiny {W}$and ${{ tiny {B}} \ Omega { tiny {WB}$are constants

$R_{\tiny{WB}} \left( t +\Delta{t} \right) = R_{\tiny{WB}} \left( t \right) Exp\left( {}_{\tiny{B}} \omega_{\tiny{WB}} \left( t \right) \Delta{t} \right)$

${}_{\tiny{W}}V\left( t + \Delta{t} \right) ={}_{\tiny{W}}V\left( t \right) + {}_{\tiny{W}}a \left( t \right)\Delta{t}$

${}_{\tiny{W}}P \left(t+\Delta{t} \right) = {}_{\tiny{W}}P\left( t \right)+{}_{\tiny{W}}V \left( t \right) \Delta{t} + \frac{1}{2}{}_{\tiny{W}}a \left( t \right)\Delta{t}^2$

The above formula is expressed in terms of IMU measurements:

$R \left( t +\Delta{t} \right) = R \left( t \right) Exp\left( \left( \tilde{ \omega } \left( t \right) – b^g\left( t \right) – \eta^{gd} \left( t \right) \right) \Delta{t}\right)$

$V \left( t +\Delta{t} \right) = V \left( t \right) +g\Delta{t}+R\left( t \right) \left( \tilde{ a } \left( t \right) – b^{a}\left( t \right) – \eta^{ad}\left( t \right) \right) \Delta {t}$

$P \left(t+\Delta{t} \right) = P\left( t \right) + V \left( t \right)\Delta{t} + \frac{1}{2} g\Delta{t}^2 +\frac{1}{2}R\left( t \right) \left( \tilde{ a } \left( t \right) – b^{a}\left( t \right) – \eta^{ad}\left( t \right) \right) \Delta {t}^2$

## IMUPre integral

Given the initial value, the angular velocity and acceleration of the IMU are integrated at the time I and j to calculate the attitude of the moment J relative to the time i:

$R_{j} = R_{i}\prod\limits_{k=i}\limits^{j-1}Exp\left( \left( \tilde{ \omega }_{k} – b^g_{k}- \eta^{gd}_{k} \right) \Delta{t} \right)$

$V_{j} = V_{i}+ g\Delta{t_{ij}}+ \sum\limits_{k=i}\limits^{j-1}R_k\left( \tilde{ a }_{k} – b^a_{k}- \eta^{ad}_{k} \right) \Delta{t}$

$P_{j} = P_{i}+ \sum\limits_{k=i}\limits^{j-1}\left[V_k\Delta{t} + \frac{1}{2}g\Delta{t}^2 + \frac{1}{2}R_k\left( \tilde{ a }_{k} – b^a_{k}- \eta^{ad}_{k} \right) \Delta{t}^2 \right]$

In preintegration theory, the initial value ($R_i$, $V_i$, $P_i$) and the constant term (including the term of gravity g) are separated.

(1)

$\Delta{R_{ij}} = R_{i}^{T}R_j=\prod\limits_{k=i}\limits^{j-1}Exp\left( \left( \tilde{ \omega }_{k} – b^g_{k}- \eta^{gd}_{k} \right) \Delta{t} \right)$

$\Delta{V_{ij}} = R_{i}^{T}\left( V_j – V_i – g\Delta{t_{ij}} \right)= \sum\limits_{k=i}\limits^{j-1}\Delta{R_{ik}}\left( \tilde{ a }_{k} – b^a_{k}- \eta^{ad}_{k} \right) \Delta{t}$

$\Delta{P_{ij}} = R_{i}^{T}\left( P_j – P_i -V_i\Delta{t_{ij}}-\frac{1}{2} g\Delta{t_{ij}^2} \right)=\sum\limits_{k=i}\limits^{j-1}\left[\Delta{V_{ik}}\Delta{t}+\frac{1}{2}\Delta{R_{ik}}\left( \tilde{ a }_{k} – b^a_{k}- \eta^{ad}_{k} \right) \Delta{t}^2\right]$

Where $Delta {R {ij}$, $Delta {V {ij}$, $Delta {P {ij}$, $Delta {P {ij}$are preintegration measurements, i.e. relative measurements of initial values and gravity acceleration terms are not considered.. Notice that this term contains noise $\ ETA$, and we also need to separate them. In the process of separation, we find that the preintegration measure is approximately Gaussian.

(2)

$\Delta\tilde{R}_{ij} \approx \Delta{R_{ij}}Exp\left( \delta \phi_{ij} \right) =\prod\limits_{k=i}\limits^{j-1}Exp\left( \left( \tilde{ \omega }_{k} – b^g_{k} \right) \Delta{t} \right)$

$\Delta\tilde{V}_{ij} \approx \Delta{V_{ij}}+\delta{V_{ij}} = \sum\limits_{k=i}\limits^{j-1}\Delta{\tilde{R}_{ik}}\left( \tilde{ a }_{k} – b^a_{k} \right) \Delta{t}$

$\Delta\tilde{P}_{ij} \approx \Delta{P_{ij}}+\delta{P_{ij}}=\sum\limits_{k=i}\limits^{j-1}\left[\Delta{\tilde{V}_{ik}}\Delta{t}+\frac{1}{2}\Delta{\tilde{R}_{ik}}\left( \tilde{ a }_{k} – b^a_{k} \right) \Delta{t}^2\right]$

Finally, a pre-integral measurement model (where $Exp left (- delta phi_{ij} right) ^ T = Exp left (\ delta\ phi_{ij} right)$) is obtained.

(3)

$\Delta\tilde{R}_{ij} = R_{i}^{T}R_jExp\left( \delta \phi_{ij} \right)$

$\Delta\tilde{V}_{ij} = R_{i}^{T}\left( V_j – V_i – g\Delta{t_{ij}} \right)+\delta{V_{ij}}$

$\Delta\tilde{P}_{ij} = R_{i}^{T}\left( P_j – P_i -V_i\Delta{t_{ij}}-\frac{1}{2} g\Delta{t_{ij}^2} \right)+\delta{P_{ij}}$

## Deviation update

(4)

$\Delta\tilde{R}_{ij}\left( b_i^g\right) =\prod\limits_{k=i}\limits^{j-1}Exp\left( \left( \tilde{ \omega }_{k} -\bar{b}^g_{i} -\delta{b_i^g}\right) \Delta{t} \right) \simeq \Delta\tilde{R}_{ij}\left( \bar{b}_i^g\right) Exp\left(\frac{\partial\Delta\bar{R}_{ij}} {\partial{b^g}} \delta{b^g_i} \right)$

$\Delta\tilde{V}_{ij}\left( b_i^g,b_i^a \right) =\sum\limits_{k=i}\limits^{j-1}\Delta{\tilde{R}_{ik}}\left(b_i^g\right)\left( \tilde{ a }_{k} – \bar{b}^a_{i} -\delta{b}_i^a \right) \Delta{t} \simeq \Delta\tilde{V}_{ij}\left( \bar{b}_i^g,\bar{b}_i^a\right) +\frac{\partial\Delta\bar{V}_{ij}} {\partial{b^g}} \delta{b^g_i} + \frac{\partial\Delta\bar{V}_{ij}} {\partial{b^a}} \delta{b^a_i}$

$\Delta\tilde{P}_{ij}\left( b_i^g,b_i^a \right)= \sum\limits_{k=i}\limits^{j-1}\left[\Delta{\tilde{V}_{ik}}\left( b_i^g,b_i^a \right)\Delta{t}+\frac{1}{2}\Delta{\tilde{R}_{ik}}\left( b_i^g\right)\left( \tilde{ a }_{k} – \bar{b}^a_{i} -\delta{b}_i^a \right) \Delta{t}^2\right] \simeq \Delta\tilde{P}_{ij}\left( \bar{b}_i^g,\bar{b}_i^a\right) +\frac{\partial\Delta\bar{P}_{ij}} {\partial{b^g}} \delta{b^g_i} + \frac{\partial\Delta\bar{P}_{ij}} {\partial{b^a}} \delta{b^a_i}$

$\Delta\tilde{R}_{ij}\left( \bar{b}_i^g\right)$,$\Delta\tilde{V}_{ij}\left( \bar{b}_i^g,\bar{b}_i^a\right)$,$\Delta\tilde{P}_{ij}\left( \bar{b}_i^g,\bar{b}_i^a\right)$To offset the value of the non updated, the latter part is the offset update effect.

In the previous pre-integral derivation, we assumed that the offset between I and J was constant (i.e., the subscript of the offset is I rather than the variable k), but the estimate of the offset was updated by a small increment of $b\ gets\ bar {b} +\ delt during the optimization processA {b}$is substituted into the left half of (4) to integrate the measurements between I and j, but this is not the most efficient, so we use the first-order Taylor expansion to get the right half of (4), where the Jacobi in the right half (calculated at $\ bar {b_i}$)The changes caused by the estimated bias are described.

## residual

$r_{\Delta{R_{ij}}} = Log\left( \left( \Delta\tilde{R}_{ij}\left( \bar{b}_i^g\right) Exp\left(\frac{\partial\Delta\bar{R}_{ij}} {\partial{b^g}} \delta{b^g} \right) \right) ^T R_i^T{R_j}\right)$

$r_{\Delta{V_{ij}}} = R_{i}^{T}\left( V_j – V_i – g\Delta{t_{ij}} \right) – \left[\Delta\tilde{V}_{ij}\left( \bar{b}_i^g,\bar{b}_i^a\right) +\frac{\partial\Delta\bar{V}_{ij}} {\partial{b^g}} \delta{b^g} + \frac{\partial\Delta\bar{V}_{ij}} {\partial{b^a}} \delta{b^a} \right]$

$r_{\Delta{P_{ij}}} = R_{i}^{T}\left( P_j – P_i -V_i\Delta{t_{ij}}-\frac{1}{2} g\Delta{t_{ij}^2} \right) – \left[ \Delta\tilde{P}_{ij}\left( \bar{b}_i^g,\bar{b}_i^a\right) +\frac{\partial\Delta\bar{P}_{ij}} {\partial{b^g}} \delta{b^g} + \frac{\partial\Delta\bar{P}_{ij}} {\partial{b^a}} \delta{b^a} \right]$

The subtractive part is the left half of (1), and the reduction is the right half of (4).

## Iterative noise propagation

Noise vectors $eta_{ij} ^\ Delta = left [delta phi {ij}, Delta {V} ^ T_{ij}, Delta {P} ^ T {ij} \ right ^ T_ SIM SimMathcal{N} \left (0_{9X1}, \sum_{ij} \right)$

The recurrence results are given.

$\delta\phi_{i,j} \backsimeq \Delta \tilde{R}_{j-1,j}^T\delta\phi_{i,j-1}+J_r^{j-1}\eta_{j-1}^{gd}\Delta{t}$

$\delta{V_{i,j}} \backsimeq \delta{V_{i,j-1}} – \Delta{\tilde{R}_{i,j-1}} \left( \tilde{a}_{j-1}-b^a_i \right)^{\wedge}\delta\phi_{i,j-1}\Delta{t}+\Delta\tilde{R}_{i,j-1}\eta_{j-1}^{ad}\Delta{t}$

$\delta{P_{i,j}} \backsimeq \delta{P_{i,j-1}} + \delta{V_{i,j-1}}\Delta{t} – \frac{1}{2}\Delta{\tilde{R}_{i,j-1}} \left( \tilde{a}_{j-1}-b^a_i \right)^{\wedge}\delta\phi_{i,j-1}\Delta{t}^2 + \frac{1}{2}\Delta\tilde{R}_{i,j-1}\eta_{j-1}^{ad}\Delta{t}^2$

Written in matrix form:

$\begin{bmatrix}\delta\phi_{i,j} \\\delta{V}_{i,j} \\\delta{P}_{i,j}\end{bmatrix}= A_{j-1}\begin{bmatrix}\delta\phi_{i,j-1} \\\delta{V}_{i,j-1} \\\delta{P}_{i,j-1}\end{bmatrix}+B_{j-1}\eta_{j-1}^{gd}+C_{j-1}\eta_{j-1}^{ad}$This is a linear model.

among

$A_{j-1}=\begin{bmatrix} \Delta \tilde{R}_{j-1,j}^T & 0_{3X3} & 0_{3X3} \\ -\Delta{\tilde{R}_{i,j-1}} \left( \tilde{a}_{j-1}-b^a_i \right)^{\wedge}\Delta{t} & 0_{3X3} & I_{3X3} \\ – \frac{1}{2}\Delta{\tilde{R}_{i,j-1}} \left( \tilde{a}_{j-1}-b^a_i \right)^{\wedge}\Delta{t}^2 & I_{3X3} & I_{3X3}\Delta{t} \end{bmatrix}_{9X9}$

$B_{j-1} = \begin{bmatrix}J_r^{j-1}\Delta{t} \\ 0_{3X3} \\ 0_{3X3}\end{bmatrix}_{9X3}$

$C_{j-1}=\begin{bmatrix}0_{3X3} \\ \Delta\tilde{R}_{i,j-1}\Delta{t} \\ \frac{1}{2}\Delta\tilde{R}_{i,j-1} \Delta{t}^2\end{bmatrix}_{9X3}$

And the covariance form is:

$\sum_{ij}= A_{j-1}\sum_{i,j-1}A_{j-1}^T + B_{j-1}\eta_{j-1}^{gd}B_{j-1}^T + C_{j-1}\eta_{j-1}^{ad}C_{j-1}^T$

## (4)Deviation updateJacobianThe recurrence forms are as follows:

$\frac{\partial\Delta\bar{R}_{ij}}{\partial{b^g}} = -\sum^{j-1}_{k=i}\left[ \Delta\tilde{R}_{k+1,j}\left(\bar{b}_i\right)^T{J_r^k}\Delta{t}\right]$

$= -\sum^{j-1}_{k=i}\left[ \Delta\tilde{R}_{j,k+1}{J_r^k}\Delta{t}\right]$

Derivation: $frac { partial Delta bar {R} {i, j + 1} { partial {b ^ g}}= – \ sum {j} {k = i} leftK+1}{J_r^k}\Delta{t}\right]$

$=- \Delta{\tilde{R}_{j+1,j}}\left[ \sum_{k=i}^j \Delta{\tilde{R}_{j,k+1}}J_r^k \Delta{t}\right]$

$=- \Delta{\tilde{R}_{j+1,j}}\left[ \sum_{k=i}^{j-1} \Delta{\tilde{R}_{j,k+1}}J_r^k \Delta{t} + \Delta{\tilde{R}_{j,j+1}}J^j_r\Delta{t}\right]$

$= \Delta{\tilde{R}_{j+1,j}}\left[- \sum_{k=i}^{j-1} \Delta{\tilde{R}_{k+1,j}^T}J_r^k \Delta{t}\right]-J_r^j\Delta{t}$

$= \Delta\tilde{R}^T_{j,j+1}\frac{\partial\Delta\bar{R}_{ij}}{\partial{b^g}}-J_r^j\Delta{t}$

$\frac{\partial\Delta\bar{V}_{ij}}{\partial{b^a}} = -\sum^{j-1}_{k=i} \Delta\bar{R}_{ik}\Delta{t}$

Derivation: $frac { partial Delta bar {V} {i, j + 1} { partial {b ^ a}}= – \ sum {j} {k = I} Delta bar {R} {I k}\ Delta {t {t}]$

$=-\left(\Delta\bar{R}_{ij}\Delta{t} + \sum^{j-1}_{k=i} \Delta\bar{R}_{ik}\Delta{t}\right)$

$= \frac{\partial\Delta\bar{V}_{ij}}{\partial{b^a}}-\Delta\bar{R}_{ij}\Delta{t}$

$\frac{\partial\Delta\bar{V}_{ij}}{\partial{b^g}} = -\sum^{j-1}_{k=i} \Delta\bar{R}_{ik} \left(\tilde{a}_k – \bar{b}_i^a\right)^{\wedge} \frac{\partial\Delta\bar{R}_{ik}}{\partial{b^g}}\Delta{t}$

Derivation: $frac { partial Delta bar {V} {i, j + 1} { partial {b ^ g}}= – \ sum {j} {k = I} Delta \ bar {R} {I k} leftTilde {a}_k- bar {b}_i ^ a right { wedge} frac { partial Delta bar {R} {i k} { partial {b ^ g} \ Delta {t}$

$=-\Delta\bar{R}_{ij} \left(\tilde{a}_j – \bar{b}_i^a\right)^{\wedge} \frac{\partial\Delta\bar{R}_{ij}}{\partial{b^g}}\Delta{t}-\sum^{j-1}_{k=i} \Delta\bar{R}_{ik} \left(\tilde{a}_k – \bar{b}_i^a\right)^{\wedge} \frac{\partial\Delta\bar{R}_{ik}}{\partial{b^g}}\Delta{t}$

$= \frac{\partial\Delta\bar{V}_{ij}}{\partial{b^g}}-\Delta\bar{R}_{ij} \left(\tilde{a}_j – \bar{b}_i^a\right)^{\wedge} \frac{\partial\Delta\bar{R}_{ij}}{\partial{b^g}}\Delta{t}$

$\frac{\partial\Delta\bar{P}_{ij}}{\partial{b^a}} = \sum^{j-1}_{k=i} \frac{\partial\Delta\bar{V}_{ik}}{\partial{b^a}}\Delta{t}-\frac{1}{2}\Delta\bar{R}_{ik}\Delta{t^2}$

Derivation: $frac { partial Delta bar {P} {i, j + 1} { partial {b ^ a}} = sum {j} {k = i} frac { partial \ Delta \ bar {V}_{ik}}{\partial{b^a}}\Delta{t}-\frac{1}{2}\Delta\bar{R}_{ik}\Delta{t^2}$

$=\frac{\partial\Delta\bar{V}_{ij}}{\partial{b^a}}\Delta{t}-\frac{1}{2}\Delta\bar{R}_{ij}\Delta{t^2}+\sum^{j-1}_{k=i} \left(\frac{\partial\Delta\bar{V}_{ik}}{\partial{b^a}}\Delta{t}-\frac{1}{2}\Delta\bar{R}_{ik}\Delta{t^2}\right)$

$= \frac{\partial\Delta\bar{P}_{ij}}{\partial{b^a}}+\left( \frac{\partial\Delta\bar{V}_{ij}}{\partial{b^a}}\Delta{t}-\frac{1}{2}\Delta\bar{R}_{ij}\Delta{t^2} \right)$

$\frac{\partial\Delta\bar{P}_{ij}}{\partial{b^g}} = \sum^{j-1}_{k=i} \frac{\partial\Delta\bar{V}_{ik}}{\partial{b^g}}\Delta{t}-\frac{1}{2}\Delta\bar{R}_{ik}\left(\tilde{a}_k – \bar{b}_i^a\right)^{\wedge} \frac{\partial\Delta\bar{R}_{ik}}{\partial{b^g}}\Delta{t}^2$

Derivation: $frac { partial Delta bar {P {i, j + 1} { partial {b ^ g}} = sum {j} {k = I} left\bar{V}_{ik}}{\partial{b^g}}\Delta{t}-\frac{1}{2}\Delta\bar{R}_{ik}\left (\tilde{a}_k – \bar{b}_i^a\r)Ight) ^{\wedge} \frac{\partial\Delta\bar{R}_{ik}}{\partial{b^g}}\Delta{t}^2\right)$

$=\left(\frac{\partial\Delta\bar{V}_{ij}}{\partial{b^g}}\Delta{t}-\frac{1}{2}\Delta\bar{R}_{ij}\left(\tilde{a}_j – \bar{b}_i^a\right)^{\wedge} \frac{\partial\Delta\bar{R}_{ij}}{\partial{b^g}}\Delta{t}^2\right) + \sum^{j-1}_{k=i} \left( \frac{\partial\Delta\bar{V}_{ik}}{\partial{b^g}}\Delta{t}-\frac{1}{2}\Delta\bar{R}_{ik}\left(\tilde{a}_k – \bar{b}_i^a\right)^{\wedge} \frac{\partial\Delta\bar{R}_{ik}}{\partial{b^g}}\Delta{t}^2 \right)$

$=\frac{\partial\Delta\bar{P}_{ij}}{\partial{b^g}}+ \left( \frac{\partial\Delta\bar{V}_{ij}}{\partial{b^g}}\Delta{t}-\frac{1}{2}\Delta\bar{R}_{ij}\left(\tilde{a}_j – \bar{b}_i^a\right)^{\wedge} \frac{\partial\Delta\bar{R}_{ij}}{\partial{b^g}}\Delta{t}^2 \right)$

## Recurrence formula without noise

$\Delta\tilde{P}_{i,j+1} = \Delta\tilde{P}_{i,j} + \Delta\tilde{V}_{i,j}\Delta{t}+\frac{1}{2}\Delta\tilde{R}_{i,j}\left( \tilde{a}_j – \bar{b}^a_i\right)^{\wedge}\Delta{t^2}$

$\Delta\tilde{V}_{i,j+1} = \Delta\tilde{V}_{i,j}+\Delta\tilde{R}_{i,j}\left( \tilde{a}_j – \bar{b}^a_i\right)^{\wedge}\Delta{t}$

$\Delta\tilde{R}_{i,j+1} = \Delta\tilde{R}_{i,j}Exp\left[ \left( \tilde{\omega_j} – \bar{b_i^g}\right)^{\wedge}\Delta{t}\right]$

So far you’ve seen the updates to the delta measurements, jacobians, and covariance matrix.

// incrementally update 1)delta measurements, 2)jacobians, 3)covariance matrix
// acc: acc_measurement - bias_a,     last measurement!! not current measurement
// omega: gyro_measurement - bias_g,      last measurement!! not current measurement{
void IMUPreintegrator::update(const Vector3d &omega, const Vector3d &acc, const double &dt) {
double dt2 = dt * dt;

Matrix3d dR = Expmap(omega * dt);//Last test
Matrix3d Jr = JacobianR(omega * dt);
// noise covariance propagation of delta measurements
// err_k+1 = A*err_k + B*err_gyro + C*err_acc
Matrix3d I3x3 = Matrix3d::Identity();
Matrix<double, 9, 9> A = Matrix<double, 9, 9>::Identity();
A.block<3, 3>(6, 6) = dR.transpose();
A.block<3, 3>(3, 6) = -_delta_R * skew(acc) * dt;
A.block<3, 3>(0, 6) = -0.5 * _delta_R * skew(acc) * dt2;
A.block<3, 3>(0, 3) = I3x3 * dt;

Matrix<double, 9, 3> Bg = Matrix<double, 9, 3>::Zero();
Bg.block<3, 3>(6, 0) = Jr * dt;

Matrix<double, 9, 3> Ca = Matrix<double, 9, 3>::Zero();
Ca.block<3, 3>(3, 0) = _delta_R * dt;
Ca.block<3, 3>(0, 0) = 0.5 * _delta_R * dt2;
//covariance
_cov_P_V_Phi = A * _cov_P_V_Phi * A.transpose() +
Bg * IMUData::getGyrMeasCov() * Bg.transpose() +
Ca * IMUData::getAccMeasCov() * Ca.transpose();
// jacobian of delta measurements w.r.t bias of gyro/acc
// update P first, then V, then R
_J_P_Biasa += _J_V_Biasa * dt - 0.5 * _delta_R * dt2;
_J_P_Biasg += _J_V_Biasg * dt - 0.5 * _delta_R * skew(acc) * _J_R_Biasg * dt2;
_J_V_Biasa += -_delta_R * dt;
_J_V_Biasg += -_delta_R * skew(acc) * _J_R_Biasg * dt;
_J_R_Biasg = dR.transpose() * _J_R_Biasg - Jr * dt;

// delta measurements, position/velocity/rotation(matrix)
// update P first, then V, then R. because P's update need V&R's previous state

_delta_P += _delta_V * dt + 0.5 * _delta_R * acc * dt2;    // P_k+1 = P_k + V_k*dt + R_k*a_k*dt*dt/2
_delta_V += _delta_R * acc * dt;
_delta_R = normalizeRotationM(_delta_R * dR);  // normalize rotation, in case of numerical error accumulation
//    // noise covariance propagation of delta measurements
//    // err_k+1 = A*err_k + B*err_gyro + C*err_acc
//    Matrix3d I3x3 = Matrix3d::Identity();
//    MatrixXd A = MatrixXd::Identity(9,9);
//    A.block<3,3>(6,6) = dR.transpose();
//    A.block<3,3>(3,6) = -_delta_R*skew(acc)*dt;
//    A.block<3,3>(0,6) = -0.5*_delta_R*skew(acc)*dt2;
//    A.block<3,3>(0,3) = I3x3*dt;
//    MatrixXd Bg = MatrixXd::Zero(9,3);
//    Bg.block<3,3>(6,0) = Jr*dt;
//    MatrixXd Ca = MatrixXd::Zero(9,3);
//    Ca.block<3,3>(3,0) = _delta_R*dt;
//    Ca.block<3,3>(0,0) = 0.5*_delta_R*dt2;
//    _cov_P_V_Phi = A*_cov_P_V_Phi*A.transpose() +
//        Bg*IMUData::getGyrMeasCov*Bg.transpose() +
//        Ca*IMUData::getAccMeasCov()*Ca.transpose();

// delta time
_delta_time += dt;

}}

## Following the idea of graph optimization, a graph model of VIO is established.

The graph optimization model is shown in the figure above.

The quantities in the red circle node are $Delta {b ^ a}$, $Delta {b ^ g}$, because $b gets bar {b}+\ Delta {b}$, so Delta {b}$is optimized and the offset is updated. The state of the triangle black node is IMU, (R, P, V). The amount of the quadrilateral blue node is the coordinates of three dimensional points in the world coordinates, (X, Y, Z). The amount of green pentagonal nodes is (R, P, V,$\delta{b^a}$,$\delta{b^g}$). The amount of black circular nodes is g of gravity acceleration in the world coordinate system. The amount of purple circular node is$b^g$of gyro bias. ## The error of every side and Jacobi’s calculation Refer to the way to set nodes and edges in ORB-YGZ-SLAM. The error function is the formula 45 in [1].$r_{\Delta{R_{ij}}} = Log\left( \left( \Delta\tilde{R}_{ij}\left( \bar{b}_i^g\right) Exp\left(\frac{\partial\Delta\bar{R}_{ij}} {\partial{b^g}} \delta{b^g} \right) \right) ^T R_i^T{R_j}\right)r_{\Delta{V_{ij}}} = R_{i}^{T}\left( V_j – V_i – g\Delta{t_{ij}} \right) – \left[\Delta\tilde{V}_{ij}\left( \bar{b}_i^g,\bar{b}_i^a\right) +\frac{\partial\Delta\bar{V}_{ij}} {\partial{b^g}} \delta{b^g} + \frac{\partial\Delta\bar{V}_{ij}} {\partial{b^a}} \delta{b^a} \right]r_{\Delta{P_{ij}}} = R_{i}^{T}\left( P_j – P_i -V_i\Delta{t_{ij}}-\frac{1}{2} g\Delta{t_{ij}^2} \right) – \left[ \Delta\tilde{P}_{ij}\left( \bar{b}_i^g,\bar{b}_i^a\right) +\frac{\partial\Delta\bar{P}_{ij}} {\partial{b^g}} \delta{b^g} + \frac{\partial\Delta\bar{P}_{ij}} {\partial{b^a}} \delta{b^a} \right]$Error program implementation  void EdgeNavStatePVR::computeError() { // const VertexNavStatePVR *vPVRi = static_cast<const VertexNavStatePVR *>(_vertices[0]); const VertexNavStatePVR *vPVRj = static_cast<const VertexNavStatePVR *>(_vertices[1]); const VertexNavStateBias *vBiasi = static_cast<const VertexNavStateBias *>(_vertices[2]); // terms need to computer error in vertex i, except for bias error const NavState &NSPVRi = vPVRi->estimate(); Vector3d Pi = NSPVRi.Get_P(); Vector3d Vi = NSPVRi.Get_V(); SO3d Ri = NSPVRi.Get_R(); // Bias from the bias vertex const NavState &NSBiasi = vBiasi->estimate(); Vector3d dBgi = NSBiasi.Get_dBias_Gyr(); Vector3d dBai = NSBiasi.Get_dBias_Acc(); // terms need to computer error in vertex j, except for bias error const NavState &NSPVRj = vPVRj->estimate(); Vector3d Pj = NSPVRj.Get_P(); Vector3d Vj = NSPVRj.Get_V(); SO3d Rj = NSPVRj.Get_R(); // IMU Preintegration measurement const IMUPreintegrator &M = _measurement; //Preclassification, actual value double dTij = M.getDeltaTime(); // Delta Time double dT2 = dTij * dTij; Vector3d dPij = M.getDeltaP(); // Delta Position pre-integration measurement //Actual deltaP measured Vector3d dVij = M.getDeltaV(); // Delta Velocity pre-integration measurement Sophus::SO3d dRij = Sophus::SO3(M.getDeltaR()); // Delta Rotation pre-integration measurement // tmp variable, transpose of Ri Sophus::SO3d RiT = Ri.inverse(); // residual error of Delta Position measurement Vector3d rPij = RiT * (Pj - Pi - Vi * dTij - 0.5 * GravityVec * dT2) - (dPij + M.getJPBiasg() * dBgi + M.getJPBiasa() * dBai); // this line includes correction term of bias change. // residual error of Delta Velocity measurement Vector3d rVij = RiT * (Vj - Vi - GravityVec * dTij) - (dVij + M.getJVBiasg() * dBgi + M.getJVBiasa() * dBai); //this line includes correction term of bias change // residual error of Delta Rotation measurement Sophus::SO3d dR_dbg = Sophus::SO3d::exp(M.getJRBiasg() * dBgi); Sophus::SO3d rRij = (dRij * dR_dbg).inverse() * RiT * Rj; Vector3d rPhiij = rRij.log(); Vector9d err; // typedef Matrix<double, D, 1> ErrorVector; ErrorVector _error; D=9 err.setZero(); // 9-Dim error vector order: // position-velocity-rotation // rPij - rVij - rPhiij err.segment<3>(0) = rPij; // position error err.segment<3>(3) = rVij; // velocity error err.segment<3>(6) = rPhiij; // rotation phi error _error = err; } Jacobi For the error of three parts$ left [r {Delta {P {ij}}, R {Delta {V {ij}}, R {Delta {R {ij}} u right]$, the optimized term$left [{left {ij}} right]$for eight parts is obtained.P_i}, V_i}, { phi_i}, {P_j}, V_j}, { phi_j}, tilde {\ delta} b_i ^ g, tilde {\ delta} b_i ^ a \ right$, a total of 24 JacobiPart.

i:

$\frac{\partial{r}_{\Delta{P_{ij}}}}{\partial\delta{P_i}} = -I_{3X1}$ , $\frac{\partial{r}_{\Delta{V_{ij}}}}{\partial\delta{P_i}} = 0$, $\frac{\partial{r}_{\Delta{R_{ij}}}}{\partial\delta{P_i}} = 0$

$\frac{\partial{r}_{\Delta{P_{ij}}}}{\partial\delta{V_i}} = -R_i^T\Delta{t}_{ij}$, $\frac{\partial{r}{_\Delta{V_{ij}}}}{\partial\delta{V_i}} = -R_i^T$, $\frac{\partial{r}_{\Delta{R_{ij}}}}{\partial\delta{V_i}} = 0$

$\frac{\partial{r}_{\Delta{P_{ij}}}}{\partial\delta{\phi_i}} = \left( R_i^T \left( P_j-P_i-V_i\Delta{t_{ij}}-\frac{1}{2}g\Delta{t_{ij}^2}\right)\right)^{\wedge}$, $\frac{\partial{r}_{\Delta{V_{ij}}}}{\partial\delta{\phi_i}}=\left(R_i^T\left( V_j- V_i-g\Delta{t_{ij}}\right)\right)^{\wedge}$, $\frac{\partial{r}_{\Delta{R_{ij}}}}{\partial\delta{\phi_i}} = -J_r^{-1}\left(r{}_{\Delta{R}}\left(R_i\right)\right)R^T_j{R_i}$

j:

$\frac{\partial{r}_{\Delta{P_{ij}}}}{\partial\delta{P_j}} = R_i^T{R_j}$, $\frac{\partial{r}_{\Delta{V_{ij}}}}{\partial\delta{P_j}} = 0$, $\frac{\partial{r}_{\Delta{R_{ij}}}}{\partial\delta{P_j}} = 0$

$\frac{\partial{r}_{\Delta{P_{ij}}}}{\partial\delta{V_j}} = 0$, $\frac{\partial{r}_{\Delta{V_{ij}}}}{\partial\delta{V_j}} = R_i^T$, $\frac{\partial{r}_{\Delta{R_{ij}}}}{\partial\delta{V_j}} = 0$

$\frac{\partial{r}_{\Delta{P_{ij}}}}{\partial\delta{\phi_j}} = 0$, $\frac{\partial{r}_{\Delta{V_{ij}}}}{\partial\delta{\phi_j}} = 0$, $\frac{\partial{r}_{\Delta{R_{ij}}}}{\partial\delta{\phi_j}} = J_r^{-1}\left(r{}_{\Delta{R}}\left(R_j\right)\right)$

$\tilde{\delta}{b^g_i}$,$\tilde{\delta}{b^a_i}$:

$\frac{\partial{r_{\Delta{P_{ij}}}}}{\partial\tilde{\delta}{b^g_i}}=-\frac{\partial\Delta\bar{P}_{ij}}{\partial{b_i^g}}$, $\frac{\partial{r_{\Delta{V_{ij}}}}}{\partial\tilde{\delta}{b^g_i}}=-\frac{\partial\Delta\bar{V}_{ij}}{\partial{b_i^g}}$,$\frac{\partial{r_{\Delta{R_{ij}}}}}{\partial\tilde{\delta}{b^g_i}}=\alpha$

$\frac{\partial{r_{\Delta{P_{ij}}}}}{\partial\tilde{\delta}{b^a_i}}=-\frac{\partial\Delta\bar{P}_{ij}}{\partial{b_i^a}}$, $\frac{\partial{r_{\Delta{V_{ij}}}}}{\partial\tilde{\delta}{b^a_i}}=-\frac{\partial\Delta\bar{V}_{ij}}{\partial{b_i^a}}$,$\frac{\partial{r_{\Delta{R_{ij}}}}}{\partial\tilde{\delta}{b^a_i}}=0$

Where $alpha = J_r ^ {- 1} left (r_{Delta {R_{ij}}} \ left ( Delta {b}_i ^ G right) Exp\ left (r_{ Delt right)A{R}_{ij}}\left (\delta{b}_i^g\right) \right) ^T {J}^b_r\frac{\partial\Delta\bar{R}_{ij}}{\partial{b^g}]$

Jacobian programming

void EdgeNavStatePVR::linearizeOplus() {
//
const VertexNavStatePVR *vPVRi = static_cast<const VertexNavStatePVR *>(_vertices[0]);
const VertexNavStatePVR *vPVRj = static_cast<const VertexNavStatePVR *>(_vertices[1]);
const VertexNavStateBias *vBiasi = static_cast<const VertexNavStateBias *>(_vertices[2]);

// terms need to computer error in vertex i, except for bias error
const NavState &NSPVRi = vPVRi->estimate();
Vector3d Pi = NSPVRi.Get_P();
Vector3d Vi = NSPVRi.Get_V();
Matrix3d Ri = NSPVRi.Get_RotMatrix();
// bias
const NavState &NSBiasi = vBiasi->estimate();
Vector3d dBgi = NSBiasi.Get_dBias_Gyr();//gyroscope//    Vector3d dBai = NSBiasi.Get_dBias_Acc();

// terms need to computer error in vertex j, except for bias error
const NavState &NSPVRj = vPVRj->estimate();
Vector3d Pj = NSPVRj.Get_P();
Vector3d Vj = NSPVRj.Get_V();
Matrix3d Rj = NSPVRj.Get_RotMatrix();

// IMU Preintegration measurement
const IMUPreintegrator &M = _measurement;
double dTij = M.getDeltaTime();   // Delta Time
double dT2 = dTij * dTij;

// some temp variable
Matrix3d I3x3 = Matrix3d::Identity();   // I_3x3
Matrix3d O3x3 = Matrix3d::Zero();       // 0_3x3
Matrix3d RiT = Ri.transpose();          // Ri^T
Matrix3d RjT = Rj.transpose();          // Rj^T
Vector3d rPhiij = _error.segment<3>(6); // residual of rotation, rPhiij
Matrix3d JrInv_rPhi = Sophus::SO3::JacobianRInv(rPhiij);    // inverse right jacobian of so3 term #rPhiij#
Matrix3d J_rPhi_dbg = M.getJRBiasg();              // jacobian of preintegrated rotation-angle to gyro bias i
// 1.
// increment is the same as Forster 15'RSS
// pi = pi + Ri*dpi,    pj = pj + Rj*dpj
// vi = vi + dvi,       vj = vj + dvj
// Ri = Ri*Exp(dphi_i), Rj = Rj*Exp(dphi_j)
//      Note: the optimized bias term is the 'delta bias'
// dBgi = dBgi + dbgi_update,    dBgj = dBgj + dbgj_update
// dBai = dBai + dbai_update,    dBaj = dBaj + dbaj_update

// 2.
// 9-Dim error vector order in PVR:
// position-velocity-rotation
// rPij - rVij - rPhiij
//      Jacobian row order:
// J_rPij_xxx
// J_rVij_xxx
// J_rPhiij_xxx

// 3.
// order in 'update_' in PVR
// Vertex_i : dPi, dVi, dPhi_i
// Vertex_j : dPj, dVj, dPhi_j
// 6-Dim error vector order in Bias:
// dBiasg_i - dBiasa_i

// 4.
// For Vertex_PVR_i
Matrix<double, 9, 9> JPVRi;
JPVRi.setZero();

// 4.1
// J_rPij_xxx_i for Vertex_PVR_i
JPVRi.block<3, 3>(0, 0) = -I3x3;      //J_rP_dpi
JPVRi.block<3, 3>(0, 3) = -RiT * dTij;  //J_rP_dvi
JPVRi.block<3, 3>(0, 6) = Sophus::SO3::hat(
RiT * (Pj - Pi - Vi * dTij - 0.5 * GravityVec * dT2));    //J_rP_dPhi_i

// 4.2
// J_rVij_xxx_i for Vertex_PVR_i
JPVRi.block<3, 3>(3, 0) = O3x3;    //dpi
JPVRi.block<3, 3>(3, 3) = -RiT;    //dvi
JPVRi.block<3, 3>(3, 6) = Sophus::SO3::hat(RiT * (Vj - Vi - GravityVec * dTij));    //dphi_i

// 4.3
// J_rPhiij_xxx_i for Vertex_PVR_i
Matrix3d ExprPhiijTrans = Sophus::SO3::exp(rPhiij).inverse().matrix();
Matrix3d JrBiasGCorr = Sophus::SO3::JacobianR(J_rPhi_dbg * dBgi);
JPVRi.block<3, 3>(6, 0) = O3x3;    //dpi
JPVRi.block<3, 3>(6, 3) = O3x3;    //dvi
JPVRi.block<3, 3>(6, 6) = -JrInv_rPhi * RjT * Ri;    //dphi_i
// 5.
// For Vertex_PVR_j
Matrix<double, 9, 9> JPVRj;
JPVRj.setZero();

// 5.1
// J_rPij_xxx_j for Vertex_PVR_j
JPVRj.block<3, 3>(0, 0) = RiT * Rj;  //dpj
JPVRj.block<3, 3>(0, 3) = O3x3;    //dvj
JPVRj.block<3, 3>(0, 6) = O3x3;    //dphi_j

// 5.2
// J_rVij_xxx_j for Vertex_PVR_j
JPVRj.block<3, 3>(3, 0) = O3x3;    //dpj
JPVRj.block<3, 3>(3, 3) = RiT;    //dvj
JPVRj.block<3, 3>(3, 6) = O3x3;    //dphi_j

// 5.3
// J_rPhiij_xxx_j for Vertex_PVR_j
JPVRj.block<3, 3>(6, 0) = O3x3;    //dpj
JPVRj.block<3, 3>(6, 3) = O3x3;    //dvj
JPVRj.block<3, 3>(6, 6) = JrInv_rPhi;    //dphi_j

// 6.
// For Vertex_Bias_i
Matrix<double, 9, 6> JBiasi;
JBiasi.setZero();

// 5.1
// J_rPij_xxx_j for Vertex_Bias_i
JBiasi.block<3, 3>(0, 0) = -M.getJPBiasg();     //J_rP_dbgi
JBiasi.block<3, 3>(0, 3) = -M.getJPBiasa();     //J_rP_dbai

// J_rVij_xxx_j for Vertex_Bias_i
JBiasi.block<3, 3>(3, 0) = -M.getJVBiasg();    //dbg_i
JBiasi.block<3, 3>(3, 3) = -M.getJVBiasa();    //dba_i

// J_rPhiij_xxx_j for Vertex_Bias_i
JBiasi.block<3, 3>(6, 0) = -JrInv_rPhi * ExprPhiijTrans * JrBiasGCorr * J_rPhi_dbg;    //dbg_i
JBiasi.block<3, 3>(6, 3) = O3x3;    //dba_i

// Evaluate _jacobianOplus
_jacobianOplus[0] = JPVRi;
_jacobianOplus[1] = JPVRj;
_jacobianOplus[2] = JBiasi;
}

Bias error

$r = \begin{bmatrix} \left(b_j^g+\delta b_j^g\right) – \left( b_i^g+\delta b_i^g\right) \\ \left(b_j^a+\delta b_j^a\right) – \left( b_i^a+\delta b_i^a\right) \end{bmatrix}$

Error program implementation

    void EdgeNavStateBias::computeError() {
const VertexNavStateBias *vBiasi = static_cast<const VertexNavStateBias *>(_vertices[0]);
const VertexNavStateBias *vBiasj = static_cast<const VertexNavStateBias *>(_vertices[1]);

const NavState &NSi = vBiasi->estimate();
const NavState &NSj = vBiasj->estimate();

// residual error of Gyroscope's bias, Forster 15'RSS
Vector3d rBiasG = (NSj.Get_BiasGyr() + NSj.Get_dBias_Gyr())
- (NSi.Get_BiasGyr() + NSi.Get_dBias_Gyr());

// residual error of Accelerometer's bias, Forster 15'RSS
Vector3d rBiasA = (NSj.Get_BiasAcc() + NSj.Get_dBias_Acc()) //It is not the difference between the estimated value and the actual value, but the difference between before and after.
- (NSi.Get_BiasAcc() + NSi.Get_dBias_Acc());

Vector6d err;  // typedef Matrix<double, D, 1> ErrorVector; ErrorVector _error; D=6
err.setZero();
// 6-Dim error vector order:  //errorIt's six dimensional.// deltabiasGyr_i-deltabiasAcc_i
// rBiasGi - rBiasAi
err.segment<3>(0) = rBiasG;     // bias gyro error
err.segment<3>(3) = rBiasA;    // bias acc error

_error = err;
}

Be optimized

Node i: $left [delta b_i ^ g, Delta b_i ^ a right]$, node j: $left [ Delta b_j ^ g, Delta b_j ^ a \ right]$

Offset Jacobi

$\frac{\partial r}{\partial \left[ \delta b_i^g,\delta b_i^a\right] } = \begin{bmatrix} -I_3 & 0 \\ 0 & -I_3 \end{bmatrix}$,$\frac{\partial r}{\partial \left[ \delta b_j^g,\delta b_j^a\right] } = \begin{bmatrix} I_3 & 0 \\ 0 & I_3 \end{bmatrix}$

Jacobian code implementation

    void EdgeNavStateBias::linearizeOplus() {
// 6-Dim error vector order:
// deltabiasGyr_i-deltabiasAcc_i
// rBiasGi - rBiasAi

_jacobianOplusXi = -Matrix<double, 6, 6>::Identity();
_jacobianOplusXj = Matrix<double, 6, 6>::Identity();
}

Three dimensional coordinates of space points in the world coordinate system are transformed into two dimensional coordinates by IMU coordinate system.

$P_b = \left(R_{bc}P_c + t_{bc}\right), P_w = \left( R_{wb}P_b + t_{wb}\right)$

$P_w = R_{wb}\left( R_{bc}P_c + t_{bc}\right) + t_{wb}$

$P_c = R_{cb}\left[ R_{wb}^T \left( P_w – t_{wb}\right) – t_{bc}\right]$

Projection error

_error = _measurement（Measurement value) – p(Pixel coordinates estimate

Let $P_w = \left[X, Y, Z\right]$, $P_c = \left[X^{‘}, Y^{‘}, Z^{‘}\right]$

$p = \begin{bmatrix} u \\ v \end{bmatrix} = \begin{bmatrix} f_x\left( \frac{X^{‘}}{Z^{‘}}\right)+c_x \\ f_y\left( \frac{Y^{‘}}{Z^{‘}}\right)+c_y \end{bmatrix}$

Implementation of projection error code

        void computeError() {
Vector3d Pc = computePc();
Vector2d obs(_measurement);//Pixel coordinates, actual
_error = obs - cam_project(Pc);//PcFor the three-dimensional point in the camera coordinate system, cam_project () transforms Pc into pixel coordinates with two dimensional errors.
}
bool isDepthPositive() {
Vector3d Pc = computePc();
return Pc(2) > 0.0;
}
Vector3d computePc() {
const VertexSBAPointXYZ *vPoint = static_cast<const VertexSBAPointXYZ *>(_vertices[0]);//Three dimensional point
const VertexNavStatePVR *vNavState = static_cast<const VertexNavStatePVR *>(_vertices[1]);//imu，p,v,r

const NavState &ns = vNavState->estimate();
Matrix3d Rwb = ns.Get_RotMatrix(); //Matrix form
Vector3d Pwb = ns.Get_P();
const Vector3d &Pw = vPoint->estimate();

Matrix3d Rcb = Rbc.transpose();//The relationship between camera and IMU
Vector3d Pc = Rcb * Rwb.transpose() * (Pw - Pwb) - Rcb * Pbc;

return Pc;
}

inline Vector2d project2d(const Vector3d &v) const {//Three dimensional points are transformed into uniform coordinates in camera coordinate system.
Vector2d res;
res(0) = v(0) / v(2);
res(1) = v(1) / v(2);
return res;
}

Jacobi

Optimization: $P_w$

$\frac{\partial{error}}{\partial{P_w}}=-\frac{\partial{p}}{\partial{P_w}} =-\frac{\partial{p}}{\partial{P_c}}\frac{\partial P_c}{\partial P_w}$

$\frac{\partial p}{\partial P_c} = \begin{bmatrix} f_x\frac{1}{Z^{‘}} & 0 & -f_x\frac{X^{‘}}{Z^{‘2}} \\ 0 & f_y\frac{1}{Z^{‘}} & -f_y\frac{Y^{‘}}{Z^{‘2}} \end{bmatrix}$, $\frac{\partial P_c}{\partial P_w} = R_{cb}R_{wb}^T$

Optimizations: $left [delta P, Delta V, Delta R right] = left [delta P_{wb}, Delta V_{wb}, Delta R_ Delta R_{wb} \right]$

$\frac{\partial{error}}{\partial{ \left[ \delta P_{wb}, \delta V_{wb} , \delta R_{wb} \right] }}=-\frac{\partial{p}}{\partial{ \left[ \delta P_{wb}, \delta V_{wb} , \delta R_{wb} \right] }} = -\frac{\partial{p}}{\partial{P_c}}\frac{\partial P_c}{\partial \left[ \delta P_{wb}, \delta V_{wb} , \delta R_{wb} \right] }$

$\frac{\partial P_c}{\partial \delta P_{wb}} = \lim_\limits{\delta P_{wb}\to 0}\frac{ R_{cb}\left[ R_{wb}^T \left( P_w – \left( P_{wb} + R_{wb}\delta P_{wb} \right) \right) – P_{bc}\right] -R_{cb}\left[ R_{wb}^T \left( P_w – P_{wb} \right) – P_{bc}\right] } {\delta P_{wb}} = -R_{cb}$, $P_w$Coordinates of three dimensional points for the world coordinate system.

$\frac{\partial P_c}{\partial \delta V_{wb}} = 0$

$\frac{\partial P_c}{\partial \delta \phi_{wb}} = \lim_\limits{\delta \phi_{wb}\to 0}\frac{ R_{cb}\left[ \left( R_{wb}Exp\left( \delta \phi_{wb}^{\wedge} \right) \right)^T \left( P_w – P_{wb} \right) – P_{bc}\right] -R_{cb}\left[ R_{wb}^T \left( P_w – P_{wb} \right) – P_{bc}\right] } {\delta \phi_{wb}} = \lim_\limits{\delta \phi_{wb}\to 0}\frac{ R_{cb}\left[ \left( Exp\left( \delta \phi _{wb}^{\wedge} \right)\right)^T R_{wb}^T \left( P_w – P_{wb} \right) – P_{bc}\right] -R_{cb}\left[ R_{wb}^T \left( P_w – P_{wb} \right) – P_{bc}\right] } {\delta \phi_{wb}}$

$= \lim_\limits{\delta \phi_{wb}\to 0}\frac{ R_{cb}\left[ \left( I – \delta \phi_{wb} ^{\wedge} \right) R_{wb}^T \left( P_w – P_{wb} \right) – P_{bc}\right] -R_{cb}\left[ R_{wb}^T \left( P_w – P_{wb} \right) – P_{bc}\right] } {\delta \phi_{wb}} = \lim_\limits{\delta \phi_{wb}\to 0}\frac{ -R_{cb}\left[ \delta \phi_{wb} ^{\wedge} R_{wb}^T \left( P_w – P_{wb} \right) \right] } {\delta \phi_{wb}}=\lim_\limits{\delta \phi_{wb}\to 0}\frac{ -R_{cb} R_{wb}^T \left( R_{wb} \delta \phi_{wb} \right)^{\wedge} \left( P_w – P_{wb} \right) } {\delta \phi_{wb}}$

$= \lim_\limits{\delta \phi_{wb}\to 0}\frac{ R_{cb} R_{wb}^T \left( P_w – P_{wb} \right) ^{\wedge} \left( R_{wb} \delta \phi_{wb} \right) } {\delta \phi_{wb}} = \lim_\limits{\delta \phi_{wb}\to 0}\frac{ \left[ R_{cb} R_{wb}^T \left( P_w – P_{wb} \right) \right] ^{\wedge} R_{cb} R_{wb}^T\left( R_{wb} \delta \phi_{wb} \right) } {\delta \phi_{wb}}$

$= \left[ R_{cb}R_{wb}^T \left(P_w-P_{wb}\right)\right]^{\wedge}R_{cb}$        The properties of the adjoint matrix and the formula of the paper (2) are derived.

Jacobian procedure:

    void EdgeNavStatePVRPointXYZ::linearizeOplus() {
const VertexSBAPointXYZ *vPoint = static_cast<const VertexSBAPointXYZ *>(_vertices[0]);
const VertexNavStatePVR *vNavState = static_cast<const VertexNavStatePVR *>(_vertices[1]);

const NavState &ns = vNavState->estimate();
Matrix3d Rwb = ns.Get_RotMatrix();
Vector3d Pwb = ns.Get_P();
const Vector3d &Pw = vPoint->estimate();

Matrix3d Rcb = Rbc.transpose();
Vector3d Pc = Rcb * Rwb.transpose() * (Pw - Pwb) - Rcb * Pbc;

double x = Pc[0];
double y = Pc[1];
double z = Pc[2];

// Jacobian of camera projection
Matrix<double, 2, 3> Maux;
Maux.setZero();
Maux(0, 0) = fx;
Maux(0, 1) = 0;
Maux(0, 2) = -x / z * fx;
Maux(1, 0) = 0;
Maux(1, 1) = fy;
Maux(1, 2) = -y / z * fy;
Matrix<double, 2, 3> Jpi = Maux / z;

// error = obs - pi( Pc )
// Pw <- Pw + dPw,          for Point3D
// Rwb <- Rwb*exp(dtheta),  for NavState.R
// Pwb <- Pwb + Rwb*dPwb,   for NavState.P

// Jacobian of error w.r.t Pw
_jacobianOplusXi = -Jpi * Rcb * Rwb.transpose();//Spatial 3D point pair error function for partial derivation/ / Jacobian of Pc/error w.r.t dPwbMatrix< double, 2, 3> JdPwb =-Jpi * (-Rcb); / / seek the partial derivative of P in NavState?/ / Jacobian of Pc/error w.r.t dRwbVector3d PaUX = Rcb * Rwb.transpose () * (Pw - Pwb);Matrix< double, 2, 3> JdRwb = -Jpi * (Sophus::SO3:: Hat (Paux) * Rcb); / /????/ / Jacobian of Pc w.r.t NavState/ / order in'updaTe_': dP, dV, dPhiMatrix< double, 2, 9> JNavState = Matrix< double, 2, 9>:: Zero ();JNavState. Block & lt; 2, 3 & gt; (0, 0) = JdPwb; / / / skipped (0.3), which is actually a partial derivative of V with Jacobian ratio of 0JNavState.blocK< 2, 3> (0, 6) = JdRwb;/ / Jacobian of error w.r.t NavState_jacobianOplusXj =JNavState;}

Derivation of the same upper

Error program implementation:

        void computeError() {
Vector3d Pc = computePc();
Vector2d obs(_measurement);

_error = obs - cam_project(Pc);
}

bool isDepthPositive() {//Is it a positive depth?
Vector3d Pc = computePc();
return Pc(2) > 0.0;
}

Vector3d computePc() {
const VertexNavStatePVR *vNSPVR = static_cast<const VertexNavStatePVR *>(_vertices[0]);

const NavState &ns = vNSPVR->estimate();
Matrix3d Rwb = ns.Get_RotMatrix();
Vector3d Pwb = ns.Get_P();
//const Vector3d& Pw = vPoint->estimate();

Matrix3d Rcb = Rbc.transpose();
Vector3d Pc = Rcb * Rwb.transpose() * (Pw - Pwb) - Rcb * Pbc;

return Pc;
}

inline Vector2d project2d(const Vector3d &v) const {
Vector2d res;
res(0) = v(0) / v(2);
res(1) = v(1) / v(2);
return res;
}

Vector2d cam_project(const Vector3d &trans_xyz) const {
Vector2d proj = project2d(trans_xyz);
Vector2d res;
res[0] = proj[0] * fx + cx;
res[1] = proj[1] * fy + cy;
return res;
}

virtual void linearizeOplus();

void SetParams(const double &fx_, const double &fy_, const double &cx_, const double &cy_,
const Matrix3d &Rbc_, const Vector3d &Pbc_, const Vector3d &Pw_) {
fx = fx_;
fy = fy_;
cx = cx_;
cy = cy_;
Rbc = Rbc_;
Pbc = Pbc_;
Pw = Pw_;
}

void SetParams(const double &fx_, const double &fy_, const double &cx_, const double &cy_,
const SO3d &Rbc_, const Vector3d &Pbc_, const Vector3d &Pw_) {
fx = fx_;
fy = fy_;
cx = cx_;
cy = cy_;
Rbc = Rbc_.matrix();
Pbc = Pbc_;
Pw = Pw_;     //PwIs it a parameter?
}
protected:
// Camera intrinsics
double fx, fy, cx, cy;
// Camera-IMU extrinsics
Matrix3d Rbc;
Vector3d Pbc;
// Point position in world frame
Vector3d Pw;
};

Jacobian procedure:

    void EdgeNavStatePVRPointXYZOnlyPose::linearizeOplus() {
const VertexNavStatePVR *vNSPVR = static_cast<const VertexNavStatePVR *>(_vertices[0]);

const NavState &ns = vNSPVR->estimate();
Matrix3d Rwb = ns.Get_RotMatrix();
Vector3d Pwb = ns.Get_P();

Matrix3d Rcb = Rbc.transpose();
Vector3d Pc = Rcb * Rwb.transpose() * (Pw - Pwb) - Rcb * Pbc;

double x = Pc[0];
double y = Pc[1];
double z = Pc[2];

// Jacobian of camera projection
Matrix<double, 2, 3> Maux;
Maux.setZero();
Maux(0, 0) = fx;
Maux(0, 1) = 0;
Maux(0, 2) = -x / z * fx;
Maux(1, 0) = 0;
Maux(1, 1) = fy;
Maux(1, 2) = -y / z * fy;
Matrix<double, 2, 3> Jpi = Maux / z;

// error = obs - pi( Pc )
// Pw <- Pw + dPw,          for Point3D
// Rwb <- Rwb*exp(dtheta),  for NavState.R
// Pwb <- Pwb + Rwb*dPwb,   for NavState.P

// Jacobian of Pc/error w.r.t dPwb
//Matrix3d J_Pc_dPwb = -Rcb;
Matrix<double, 2, 3> JdPwb = -Jpi * (-Rcb);   //????????????
// Jacobian of Pc/error w.r.t dRwb
Vector3d Paux = Rcb * Rwb.transpose() * (Pw - Pwb);
Matrix<double, 2, 3> JdRwb = -Jpi * (Sophus::SO3::hat(Paux) * Rcb);   //??????????????

// Jacobian of Pc w.r.t NavStatePVR
// order in 'update_': dP, dV, dPhi
Matrix<double, 2, 9> JNavState = Matrix<double, 2, 9>::Zero();
JNavState.block<2, 3>(0, 0) = JdPwb;
JNavState.block<2, 3>(0, 6) = JdRwb;

// Jacobian of error w.r.t NavStatePVR
_jacobianOplusXi = JNavState;
}

Sorry, it’s over. Welcome to communicate.

Reference paper

[1]Christian Forster, Luca Carlone, Frank Dellaert, Davide Scaramuzza,“On-Manifold Preintegration for Real-Time Visual-Inertial Odometry”,in IEEE Transactions on Robotics, 2016.