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.

Leave a Reply

Your email address will not be published. Required fields are marked *