VIO的Bundle Adjustment推导

IMU模型和运动积分

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

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

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

其中IMU读数,即测量值（理论值在偏置和噪声的影响下得到的读数）为

${}_{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}}aleft( t right) - {}_{tiny{W}}gright) + b^aleft( t right) + eta^a left( t right) $

 

假设在时间间隔$left[ t,t+Delta{t} right]$中,${}_{tiny{W}}a$和${}_{tiny{B}} omega_{tiny{WB}}$为常数

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

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

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

以上的公式用IMU测量值表示：

$R left( t +Delta{t} right) = R left( t right) Expleft( left(  tilde{ omega } left( t right) - b^gleft( t right) - eta^{gd} left( t right) right) Delta{t}right)$

$V left( t +Delta{t} right) = V left( t right) +gDelta{t}+Rleft( 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) = Pleft( t right) + V left( t right)Delta{t} + frac{1}{2} gDelta{t}^2 +frac{1}{2}Rleft( t right) left(  tilde{ a } left( t right) - b^{a}left( t right) - eta^{ad}left( t right) right) Delta {t}^2$

 

IMU预积分

给定初值，在i和j时刻对IMU的角速度和加速度进行积分，可以计算j时刻相对于i时刻的姿态：

$R_{j} = R_{i}prodlimits_{k=i}limits^{j-1}Expleft( left(  tilde{ omega }_{k}  - b^g_{k}- eta^{gd}_{k}  right) Delta{t} right)$

$V_{j} = V_{i}+ gDelta{t_{ij}}+ sumlimits_{k=i}limits^{j-1}R_kleft( tilde{ a }_{k}  - b^a_{k}- eta^{ad}_{k}  right) Delta{t}$

$P_{j} = P_{i}+ sumlimits_{k=i}limits^{j-1}left[V_kDelta{t} + frac{1}{2}gDelta{t}^2 + frac{1}{2}R_kleft( tilde{ a }_{k}  - b^a_{k}- eta^{ad}_{k}  right) Delta{t}^2 right]$

在preintegration理论中需要初值（$R_i$,$V_i$,$P_i$）和常数项（包含重力g的项）分离出来。

(1)

$Delta{R_{ij}} = R_{i}^{T}R_j=prodlimits_{k=i}limits^{j-1}Expleft( 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 - gDelta{t_{ij}} right)= sumlimits_{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_iDelta{t_{ij}}-frac{1}{2} gDelta{t_{ij}^2} right)=sumlimits_{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}^2right]$

其中$Delta{R_{ij}}$,$Delta{V_{ij}}$,$Delta{P_{ij}}$即为preintegration measurement,即不考虑初值以及重力加速度项的相对测量。注意到这项项包含有噪声$eta$,我们也需要将它们分离出来，在分离的过程中发现preintegration measurement是近似服从高斯分布的，即

(2)

$Deltatilde{R}_{ij} approx Delta{R_{ij}}Expleft( delta phi_{ij} right) =prodlimits_{k=i}limits^{j-1}Expleft( left(  tilde{ omega }_{k}  - b^g_{k} right) Delta{t} right)$

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

$Deltatilde{P}_{ij} approx Delta{P_{ij}}+delta{P_{ij}}=sumlimits_{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}^2right]$ 

最终可得预积分测量模型（其中$Expleft(-deltaphi_{ij}right)^T = Expleft(deltaphi_{ij}right)$）

(3)

$Deltatilde{R}_{ij} = R_{i}^{T}R_jExpleft( delta phi_{ij} right)$

$Deltatilde{V}_{ij} = R_{i}^{T}left( V_j - V_i - gDelta{t_{ij}} right)+delta{V_{ij}}$

$Deltatilde{P}_{ij} = R_{i}^{T}left( P_j - P_i -V_iDelta{t_{ij}}-frac{1}{2} gDelta{t_{ij}^2} right)+delta{P_{ij}}$

偏差更新

(4)

$Deltatilde{R}_{ij}left( b_i^gright) =prodlimits_{k=i}limits^{j-1}Expleft( left(  tilde{ omega }_{k}  -bar{b}^g_{i} -delta{b_i^g}right) Delta{t} right) simeq Deltatilde{R}_{ij}left( bar{b}_i^gright) Expleft(frac{partialDeltabar{R}_{ij}} {partial{b^g}} delta{b^g_i} right)$

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

$Deltatilde{P}_{ij}left( b_i^g,b_i^a right)= sumlimits_{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^gright)left( tilde{ a }_{k}  - bar{b}^a_{i} -delta{b}_i^a right) Delta{t}^2right]    simeq Deltatilde{P}_{ij}left( bar{b}_i^g,bar{b}_i^aright) +frac{partialDeltabar{P}_{ij}} {partial{b^g}} delta{b^g_i} + frac{partialDeltabar{P}_{ij}} {partial{b^a}} delta{b^a_i}$

$Deltatilde{R}_{ij}left( bar{b}_i^gright)$,$Deltatilde{V}_{ij}left( bar{b}_i^g,bar{b}_i^aright)$,$Deltatilde{P}_{ij}left( bar{b}_i^g,bar{b}_i^aright)$为偏置未更新的时的值，后半部分为偏置更新后的影响。

在之前的预积分推导中我们假设i和j之间的偏置是不变的(即偏置的下标为i而不是会变化的k)，但是在优化过程中偏置的估计会被一个小增量$delta{b}$更新，将$bgetsbar{b}+delta{b}$代入（2）中得（4）的左半部分，对i和j之间的测量进行积分，但是这不是最高效的，所以我们采取用一阶泰勒展开的方式得（4）的右半部分,其中右半部分中的雅可比（在$bar{b_i}$处计算所得）描述了由于估计的偏置的变化而引起的变化。

残差

$r_{Delta{R_{ij}}} = Logleft( left( Deltatilde{R}_{ij}left( bar{b}_i^gright) Expleft(frac{partialDeltabar{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 - gDelta{t_{ij}} right) - left[Deltatilde{V}_{ij}left( bar{b}_i^g,bar{b}_i^aright) +frac{partialDeltabar{V}_{ij}} {partial{b^g}} delta{b^g} + frac{partialDeltabar{V}_{ij}} {partial{b^a}} delta{b^a} right]$

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

其中被减数为（1）的左半部分，减数为（4）的右半部分。

 迭代噪声传播

噪声向量$eta_{ij}^Delta = left[ deltaphi^T_{ij}, delta{V}^T_{ij},delta{P}^T_{ij} right]^T sim mathcal{N} left( 0_{9X1},sum_{ij} right)$

给出递推结果：

$deltaphi_{i,j} backsimeq Delta tilde{R}_{j-1,j}^Tdeltaphi_{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}deltaphi_{i,j-1}Delta{t}+Deltatilde{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}deltaphi_{i,j-1}Delta{t}^2 + frac{1}{2}Deltatilde{R}_{i,j-1}eta_{j-1}^{ad}Delta{t}^2$

写成矩阵形式：

$begin{bmatrix}deltaphi_{i,j} \delta{V}_{i,j} \delta{P}_{i,j}end{bmatrix}= A_{j-1}begin{bmatrix}deltaphi_{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}$这是线性模型

其中

$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} \ Deltatilde{R}_{i,j-1}Delta{t} \ frac{1}{2}Deltatilde{R}_{i,j-1} Delta{t}^2end{bmatrix}_{9X3}$

而写成协方差形式为：

$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)的偏差更新中雅可比递推形式如下：

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

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

推导：$frac{partialDeltabar{R}_{i,j+1}}{partial{b^g}} = -sum^{j}_{k=i}left[ Deltatilde{R}_{j+1,k+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_rDelta{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^jDelta{t}$

$= Deltatilde{R}^T_{j,j+1}frac{partialDeltabar{R}_{ij}}{partial{b^g}}-J_r^jDelta{t}$

 

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

推导：$frac{partialDeltabar{V}_{i,j+1}}{partial{b^a}} = -sum^{j}_{k=i} Deltabar{R}_{ik}Delta{t}$

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

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

 

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

推导：$frac{partialDeltabar{V}_{i,j+1}}{partial{b^g}} = -sum^{j}_{k=i} Deltabar{R}_{ik} left(tilde{a}_k - bar{b}_i^aright)^{wedge}   frac{partialDeltabar{R}_{ik}}{partial{b^g}}Delta{t}$

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

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

 

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

推导：$frac{partialDeltabar{P}_{i,j+1}}{partial{b^a}} = sum^{j}_{k=i} frac{partialDeltabar{V}_{ik}}{partial{b^a}}Delta{t}-frac{1}{2}Deltabar{R}_{ik}Delta{t^2}$

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

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

 

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

推导：$frac{partialDeltabar{P}_{i,j+1}}{partial{b^g}} = sum^{j}_{k=i} left( frac{partialDeltabar{V}_{ik}}{partial{b^g}}Delta{t}-frac{1}{2}Deltabar{R}_{ik}left(tilde{a}_k - bar{b}_i^aright)^{wedge}   frac{partialDeltabar{R}_{ik}}{partial{b^g}}Delta{t}^2right)$

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

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

不含噪声的递推公式

$Deltatilde{P}_{i,j+1} = Deltatilde{P}_{i,j} + Deltatilde{V}_{i,j}Delta{t}+frac{1}{2}Deltatilde{R}_{i,j}left( tilde{a}_j - bar{b}^a_iright)^{wedge}Delta{t^2}$

$Deltatilde{V}_{i,j+1} = Deltatilde{V}_{i,j}+Deltatilde{R}_{i,j}left( tilde{a}_j - bar{b}^a_iright)^{wedge}Delta{t} $

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

 

到此已经知道了delta measurements，jacobians，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);//上一次的测试
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;
//协方差
_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;
}
}

 

下面按照图优化的思路，建立VIO的图模型

图优化的模型如上图所示。

红色圆形节点中的量为$delta{b^a}$,$delta{b^g}$，因为$bgetsbar{b}+delta{b}$，所以$delta{b}$被优化后相当于偏置也被更新了。

三角形黑色节点的量为IMU的状态，（R,P,V）。

四边形蓝色节点的量为世界坐标下的三维点坐标，（X,Y,Z）。

青色的五边形节点的量为（R,P,V，$delta{b^a}$，$delta{b^g}$）

黑色的圆形节点的量为世界坐标系下的重力加速度g。

紫色的圆形节点的量为陀螺仪的偏置$b^g$

 各边的误差，及雅可比计算

 参考ORB-YGZ-SLAM中设置节点与边的方式

误差函数为论文【1】中公式45

$r_{Delta{R_{ij}}} = Logleft( left( Deltatilde{R}_{ij}left( bar{b}_i^gright) Expleft(frac{partialDeltabar{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 - gDelta{t_{ij}} right) - left[Deltatilde{V}_{ij}left( bar{b}_i^g,bar{b}_i^aright) +frac{partialDeltabar{V}_{ij}} {partial{b^g}} delta{b^g} + frac{partialDeltabar{V}_{ij}} {partial{b^a}} delta{b^a} right]$

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

误差程序实现

 

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;
//预积分类，实际值
double dTij = M.getDeltaTime();
// Delta Time
double dT2 = dTij * dTij;
Vector3d dPij = M.getDeltaP();
// Delta Position pre-integration measurement //测量出来的实际deltaP
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;
}

 

雅克比

对3个部分的误差$left[r_{Delta{P_{ij}}},r_{Delta{V_{ij}}} ,  r_{Delta{R_{ij}}}right]$求8个部分的被优化项$left[{P_i}, {V_i},{phi_i},{P_j}, {V_j},{phi_j},tilde{delta}b_i^g,tilde{delta}b_i^aright]$的雅克比,总共24个部分。

i:

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

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

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

j:

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

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

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

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

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

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

其中$alpha = -J_r^{-1}left( r_{Delta{R_{ij}}} left( delta{b}_i^gright)right) Expleft( r_{Delta{R}_{ij}}left(delta{b}_i^gright)right)^T {J}^b_rfrac{partialDeltabar{R}_{ij}}{partial{b^g}}$

雅克比程序实现

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();//陀螺仪
//
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;
}

 

 

偏置误差

$r = begin{bmatrix} left(b_j^g+delta b_j^gright) - left( b_i^g+delta b_i^gright) \ left(b_j^a+delta b_j^aright) - left( b_i^a+delta b_i^aright) end{bmatrix}$

误差程序实现

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()) //不是估计值与实际值之差，而是前后之差
- (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:
//error是六维的
// deltabiasGyr_i-deltabiasAcc_i
// rBiasGi - rBiasAi
err.segment<3>(0) = rBiasG;
// bias gyro error
err.segment<3>(3) = rBiasA;
// bias acc error

_error = err;
}

被优化项

节点i:  $left[ delta b_i^g,delta b_i^aright]$,节点j:  $left[ delta b_j^g, delta b_j^a right]$

偏置雅克比

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

雅克比代码实现

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();
}

 

 

世界坐标系中空间点三维坐标经IMU坐标系转为像素二维坐标：

$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]$

投影误差

 _error = _measurement（测量值） - p(像素坐标估计值)

 设$P_w = left[ X, Y, Zright]$,$P_c = left[X^{'},Y^{'},Z^{'}right]$

$p = begin{bmatrix} u \ v end{bmatrix} = begin{bmatrix} f_xleft( frac{X^{'}}{Z^{'}}right)+c_x \ f_yleft( frac{Y^{'}}{Z^{'}}right)+c_y end{bmatrix} $

投影误差代码实现

void computeError() {
Vector3d Pc = computePc();
Vector2d obs(_measurement);//像素坐标,实际
_error = obs - cam_project(Pc);//Pc为在相机坐标系下三维点，cam_project()将Pc转为像素坐标，误差为二维

}
bool isDepthPositive() {
Vector3d Pc = computePc();
return Pc(2) > 0.0;
}
Vector3d computePc() {
const VertexSBAPointXYZ *vPoint = static_cast<const VertexSBAPointXYZ *>(_vertices[0]);//三维点
const VertexNavStatePVR *vNavState = static_cast<const VertexNavStatePVR *>(_vertices[1]);//imu，p,v,r
const NavState &ns = vNavState->estimate();
Matrix3d Rwb = ns.Get_RotMatrix(); //矩阵形式
Vector3d Pwb = ns.Get_P();
const Vector3d &Pw = vPoint->estimate();
Matrix3d Rcb = Rbc.transpose();//相机与imu之间的关系
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;
}

雅克比

优化项：$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_xfrac{1}{Z^{'}} & 0 & -f_xfrac{X^{'}}{Z^{'2}} \ 0 & f_yfrac{1}{Z^{'}} & -f_yfrac{Y^{'}}{Z^{'2}} end{bmatrix} $, $frac{partial P_c}{partial P_w} = R_{cb}R_{wb}^T$

优化项：$left[ delta P , delta V , delta R right]  = left[ delta P_{wb}, delta V_{wb} , 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$为世界坐标系下三维点坐标。

$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}Expleft(   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( Expleft(  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}^Tleft( 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}$        推导用到伴随矩阵的性质，和论文公式（2）

雅克比程序实现：

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();//空间三维点对误差函数求偏导
// Jacobian of Pc/error w.r.t dPwb
Matrix<double, 2, 3> JdPwb = -Jpi * (-Rcb);//求NavState中P的偏导
??
// 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 NavState
// order in 'update_': dP, dV, dPhi
Matrix<double, 2, 9> JNavState = Matrix<double, 2, 9>::Zero();
JNavState.block<2, 3>(0, 0) = JdPwb;//跳过了（0.3），其实为对V求偏导，雅克比为0
JNavState.block<2, 3>(0, 6) = JdRwb;
// Jacobian of error w.r.t NavState
_jacobianOplusXj = JNavState;
}

 

推导同上

误差程序实现：

void computeError() {
Vector3d Pc = computePc();
Vector2d obs(_measurement);
_error = obs - cam_project(Pc);
}
bool isDepthPositive() {//是否为正深度
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_;
//Pw是参数？

}
protected:
// Camera intrinsics
double fx, fy, cx, cy;
// Camera-IMU extrinsics

Matrix3d Rbc;
Vector3d Pbc;
// Point position in world frame

Vector3d Pw;
};

雅克比程序实现：

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;
}

 

 不好意思，烂尾了，欢迎交流

 

 

 

参考论文

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