ORB-SLAM3 LocalInertialBA代码和公式对照

ORB_SLAM3 Optimizer::LocalInertialBA部分代码阅读

主要流程梳理：

Step 1: 优化的关键帧和地图点选取：

在LocalInertialBA中，需要确定vpOptimizableKFs和lFixedKeyFrames。与纯视觉SLAM不同，因为IMU的测量在连续的关键帧之间建立了约束，所以不能光依据共视关系去选择关键帧。ORB-SLAM3中采取了两种方式结合的方法，首先将相邻的固定大小的关键帧作为优化帧和固定帧，然后利用给你共视图补充优化关键帧lpOptVisKFs。其中在与vpOptimizableKFs或lpOptVisKFs可见的地图点都存放在向量lLocalMapPoints中。然后再利用可观测到lLocalMapPoints中地图点的关键帧补充lFixedKeyFrames（其中，固定的关键帧最大数量代码中设置为200）

Step 2 使用g2o进行优化：

g2o使用方式可以参考计算机视觉life的从零开始学习SLAM推送，链接如下：g2o

Optimizer::LocalInertialBA中采用LinearSolverEigen线性求解器，并使用LM迭代算法。

KeyFrame结点设置

共定义了四种结点：

VextrexPose
VertexVelocity
VertexGyroBias
VertexAccBias

对于vpOptimizableKFs， lFixedKeyFrames， lpOptVisKFs 都需要加入关键帧位姿结点（VextrexPose）。其中前两者需要根据pKFi->bImu是否为真来选择是否加入IMU状态结点（包含速度: VertexVelocity， 陀螺仪bias: VertexGyroBias， 加速度计bias: VertexAccBias）

intertial边设置

共定义了三种边

vector<EdgeInertial*> vei(N,(EdgeInertial*)NULL);
vector<EdgeGyroRW*> vegr(N,(EdgeGyroRW*)NULL);
vector<EdgeAccRW*> vear(N,(EdgeAccRW*)NULL);

添加这些intertial边的前提是当前关键帧和其前一帧都有IMU信息且当前帧完成了预积分．

EdgeInertial建立相邻关键帧之间速度和位置的边

vei[i]->setVertex(0,dynamic_cast<g2o::OptimizableGraph::Vertex*>(VP1));
vei[i]->setVertex(1,dynamic_cast<g2o::OptimizableGraph::Vertex*>(VV1));
vei[i]->setVertex(2,dynamic_cast<g2o::OptimizableGraph::Vertex*>(VG1));
vei[i]->setVertex(3,dynamic_cast<g2o::OptimizableGraph::Vertex*>(VA1));
vei[i]->setVertex(4,dynamic_cast<g2o::OptimizableGraph::Vertex*>(VP2));
vei[i]->setVertex(5,dynamic_cast<g2o::OptimizableGraph::Vertex*>(VV2));//VP2和VV2表示当前帧的位置和速度.

EdgeInertial误差和雅可比计算公式推导:

void EdgeInertial::computeError()
{ 
    /*赋值操作
     * ......
     */
    const Eigen::Vector3d er = LogSO3(dR.transpose()*VP1->estimate().Rwb.transpose()*VP2->estimate().Rwb);
    const Eigen::Vector3d ev = VP1->estimate().Rwb.transpose()*(VV2->estimate()- VV1->estimate()- g*dt)- dV;
    const Eigen::Vector3d ep = VP1->estimate().Rwb.transpose()*(VP2->estimate().twb - VP1->estimate().twb
                               - VV1->estimate()*dt - g*dt*dt/2) - dP;
    _error << er, ev, ep;
}

$\Delta R,\Delta V,\Delta P$分别表示旋转, 速度, 位置的预积分量(在代码中用dR，dV和dP表示)，其中w代表世界坐标系，b代表imu惯性坐标系，c代表相机坐标系。
$$\begin{array}{rl}{e}_R& =\Delta R^T{R}_{wb1}^T{R}_{wb2}\\ e_V& =R_{wb1}^T(V_{wb2}-V_{wb1}-g\Delta t)-\Delta V\\ e_P& =R_{wb1}^T(t_{wb2}-t_{wb1}-V_{wb1}\Delta t-\frac{1}{2}g(\Delta t{)}^2)-\Delta p\end{array}$$
需要注意的是这里的$t_{wb}$表示的是世界坐标系下，世界坐标系原点指向imu位置的向量。它与求导的t满足的关系为$t_{wb}=R_{wb}t$这个公式可以在VertexPose的update部分看到。（t的实际含义我不是很懂，希望明白的人不吝赐教，但可以不管它，只要保证更新公式和雅可比公式里使用一致就行）

void EdgeInertial::linearizeOplus()
{
    /*赋值操作
     * ......
     * const Eigen::Matrix3d invJr = InverseRightJacobianSO3(er);
     */
    // Jacobians wrt Pose 1
    _jacobianOplus[0].setZero();
     // rotation
    _jacobianOplus[0].block<3,3>(0,0) = -invJr*Rwb2.transpose()*Rwb1; // 怎么求er关于R^T_w1的导数
    _jacobianOplus[0].block<3,3>(3,0) = Skew(Rbw1*(VV2->estimate() - VV1->estimate() - g*dt)); // OK
    _jacobianOplus[0].block<3,3>(6,0) = Skew(Rbw1*(VP2->estimate().twb - VP1->estimate().twb
                                                   - VV1->estimate()*dt - 0.5*g*dt*dt)); // OK
    // translation
    _jacobianOplus[0].block<3,3>(6,3) = -Eigen::Matrix3d::Identity(); // 为什么不是-Rbw1 ?

    // Jacobians wrt Velocity 1
    _jacobianOplus[1].setZero();
    _jacobianOplus[1].block<3,3>(3,0) = -Rbw1; // OK
    _jacobianOplus[1].block<3,3>(6,0) = -Rbw1*dt; // OK

    // Jacobians wrt Gyro 1
    _jacobianOplus[2].setZero();
    _jacobianOplus[2].block<3,3>(0,0) = -invJr*eR.transpose()*RightJacobianSO3(JRg*dbg)*JRg; // OK
    _jacobianOplus[2].block<3,3>(3,0) = -JVg; // OK
    _jacobianOplus[2].block<3,3>(6,0) = -JPg; // OK

    // Jacobians wrt Accelerometer 1
    _jacobianOplus[3].setZero();
    _jacobianOplus[3].block<3,3>(3,0) = -JVa; // OK
    _jacobianOplus[3].block<3,3>(6,0) = -JPa; // OK

    // Jacobians wrt Pose 2
    _jacobianOplus[4].setZero();
    // rotation
    _jacobianOplus[4].block<3,3>(0,0) = invJr; // OK
    // translation
    _jacobianOplus[4].block<3,3>(6,3) = Rbw1*Rwb2; // OK

    // Jacobians wrt Velocity 2
    _jacobianOplus[5].setZero();
    _jacobianOplus[5].block<3,3>(3,0) = Rbw1; // OK
}

雅可比矩阵为，这里的${\varphi }_1\in so(3)$
$$\begin{array}{rlrlrl}\frac{\partial e}{\partial {\varphi }_1}& =\left[\begin{array}{c}-J_r^{-1}{R}_{wb2}^T{R}_{wb1}\\ {\left(R_{wb1}^T(V_{wb2}-V_{wb1}-g\Delta t)\right)}^{\wedge }\\ {\left(R_{wb1}^T(t_{wb2}-t_{wb1}-V_{wb1}\Delta t-\frac{1}{2}g(\Delta t{)}^2)\right)}^{\wedge }\end{array}\right]& \frac{\partial e}{\partial t_1}& =\left[\begin{array}{c}{0}_{3\times 3}\\ 0_{3\times 3}\\ -{\mathbf{I}}_{3\times 3}\end{array}\right]& \frac{\partial e}{\partial V_1}& =\left[\begin{array}{c}-R_{wb1}^T\\ -R_{wb1}^T\Delta t\\ 0_{3\times 3}\end{array}\right]\\ \frac{\partial e}{\partial {\omega }_1}& =\left[\begin{array}{c}-J_r^{-1}{e}_R^T{J}_r(J_{Rg}\Delta b_g)J_{Rg}\\ -J_{Vg}\\ -J_{Pg}\end{array}\right]& & & \frac{\partial e}{\partial a_1}& =\left[\begin{array}{c}{0}_{3\times 3}\\ -J_{Va}\\ -J_{Pa}\end{array}\right]\\ \frac{\partial e}{\partial {\varphi }_2}& =\left[\begin{array}{c}{J}_r^{-1}\\ 0_{3\times 3}\\ 0_{3\times 3}\end{array}\right]& \frac{\partial e}{\partial t_2}& =\left[\begin{array}{c}{0}_{3\times 3}\\ 0_{3\times 3}\\ R_{wb1}^T{R}_{wb2}\end{array}\right]& \frac{\partial e}{\partial V_2}& =\left[\begin{array}{c}{0}_{3\times 3}\\ R_{wb1}^T\\ 0_{3\times 3}\end{array}\right]\end{array}$$
上式中$J_{Vg}$是预积分V关于$\omega$的导数，其中$\frac{\partial Ln(e_R)}{\partial {\varphi }_1}$推导如下, 这里的${\varphi }_1\in so(3)$
$$\begin{array}{rl}\frac{\partial Ln\left(e_R\right)}{\partial {\varphi }_1}& =\underset{\delta {\varphi }_1\to 0}{\lim }\frac{Ln\left(\Delta R^T{\left(R_{wb1}exp(\delta {\varphi }_1^{\wedge })\right)}^T{R}_{wb2}{e}_R^{-1}\right)}{\delta {\varphi }_1}\\ & =\underset{\delta {\varphi }_1\to 0}{\lim }\frac{Ln\left(\Delta R^{T}exp(\delta {\varphi }_1^{\wedge }{)}^T{R}_{wb1}^T{R}_{wb2}{e}_R^{-1}\right)}{\delta {\varphi }_1}\\ & =\underset{\delta {\varphi }_1\to 0}{\lim }\frac{Ln\left(\Delta R^{T}exp(-\delta {\varphi }_1^{\wedge })R_{wb1}^T{R}_{wb2}{e}_R^{-1}\right)}{\delta {\varphi }_1}\\ & =\underset{\delta {\varphi }_1\to 0}{\lim }\frac{Ln\left(\Delta R^T{R}_{wb1}^T{R}_{wb2}exp\left({\left(-{\left(R_{wb1}^T{R}_{wb2}\right)}^T\delta {\varphi }_1\right)}^{\wedge }\right)e_R^{-1}\right)}{\delta {\varphi }_1}\\ & =\underset{\delta {\varphi }_1\to 0}{\lim }\frac{Ln\left(\Delta R^T{R}_{wb1}^T{R}_{wb2}exp\left({\left(-R_{wb2}^T{R}_{wb1}\delta {\varphi }_1\right)}^{\wedge }\right)e_R^{-1}\right)}{\delta {\varphi }_1}\\ & =\underset{\delta {\varphi }_1\to 0}{\lim }\frac{Ln\left(exp({\varphi }^{\wedge })exp\left({\left(-R_{wb2}^T{R}_{wb1}\delta {\varphi }_1\right)}^{\wedge }\right)exp(-{\varphi }^{\wedge })\right)}{\delta {\varphi }_1}\\ & =\underset{\delta {\varphi }_1\to 0}{\lim }\frac{Ln\left(exp\left({\left(-J_r^{-1}(\varphi )R_{wb2}^T{R}_{wb1}\delta {\varphi }_1+\varphi -\varphi \right)}^{\wedge }\right)\right)}{\delta {\varphi }_1}\\ & =-J_r^{-1}{R}_{wb2}^T{R}_{wb1}\end{array}$$
其中$Ln(R):=ln(R{)}^{\vee }$

下面来看一下对$\omega$和$a$的导数，需要注意的是 $\omega$和$a$为IMU测量值, 它们同时也包含在预积分项中. 为了推导$\frac{\partial e^T}{\partial {\omega }_1}$需要先关注一下预积分部分:

预积分的核心代码在ImuTypes中Preintegrated成员函数IntegrateNewMeasurement中：（在预积分的头文件中，会看到有关序列化和归档的操作，例如template <typename Archive>等，不用担心，他们是用来调试的。可以通过 序列化归档 进一步了解.

void Preintegrated::IntegrateNewMeasurement(const cv::Point3f &acceleration, const cv::Point3f &angVel, const float &dt)
{
    mvMeasurements.push_back(integrable(acceleration,angVel,dt));

    // Position is updated firstly, as it depends on previously computed velocity and rotation.
    // Velocity is updated secondly, as it depends on previously computed rotation.
    // Rotation is the last to be updated.

    //Matrices to compute covariance
    cv::Mat A = cv::Mat::eye(9,9,CV_32F);
    cv::Mat B = cv::Mat::zeros(9,6,CV_32F);

    cv::Mat acc = (cv::Mat_<float>(3,1) << acceleration.x-b.bax,acceleration.y-b.bay, acceleration.z-b.baz);
    cv::Mat accW = (cv::Mat_<float>(3,1) << angVel.x-b.bwx, angVel.y-b.bwy, angVel.z-b.bwz);

    avgA = (dT*avgA + dR*acc*dt)/(dT+dt);
    avgW = (dT*avgW + accW*dt)/(dT+dt);

    // Update delta position dP and velocity dV (rely on no-updated delta rotation)
    dP = dP + dV*dt + 0.5f*dR*acc*dt*dt;
    dV = dV + dR*acc*dt;

    // Compute velocity and position parts of matrices A and B (rely on non-updated delta rotation)
    cv::Mat Wacc = (cv::Mat_<float>(3,3) <<        0, -acc.at<float>(2), acc.at<float>(1),
                                                   acc.at<float>(2), 0, -acc.at<float>(0),
                                                   -acc.at<float>(1), acc.at<float>(0), 0);
    A.rowRange(3,6).colRange(0,3) = -dR*dt*Wacc;
    A.rowRange(6,9).colRange(0,3) = -0.5f*dR*dt*dt*Wacc;
    A.rowRange(6,9).colRange(3,6) = cv::Mat::eye(3,3,CV_32F)*dt;
    B.rowRange(3,6).colRange(3,6) = dR*dt;
    B.rowRange(6,9).colRange(3,6) = 0.5f*dR*dt*dt;

    // Update position and velocity jacobians wrt bias correction
    JPa = JPa + JVa*dt -0.5f*dR*dt*dt;
    JPg = JPg + JVg*dt -0.5f*dR*dt*dt*Wacc*JRg;
    JVa = JVa - dR*dt;
    JVg = JVg - dR*dt*Wacc*JRg;

    // Update delta rotation
    IntegratedRotation dRi(angVel,b,dt);
    dR = NormalizeRotation(dR*dRi.deltaR);

    // Compute rotation parts of matrices A and B
    A.rowRange(0,3).colRange(0,3) = dRi.deltaR.t();
    B.rowRange(0,3).colRange(0,3) = dRi.rightJ*dt;

    // Update covariance
    C.rowRange(0,9).colRange(0,9) = A*C.rowRange(0,9).colRange(0,9)*A.t() + B*Nga*B.t();
    C.rowRange(9,15).colRange(9,15) = C.rowRange(9,15).colRange(9,15) + NgaWalk;

    // Update rotation jacobian wrt bias correction
    JRg = dRi.deltaR.t()*JRg - dRi.rightJ*dt;

    // Total integrated time
    dT += dt;
}

公式对照：

1.Update delta position dP and velocity dV (rely on no-updated delta rotation)
$$\begin{array}{rl}\Delta {\tilde{P}}_{i,k+1}& =\Delta {\tilde{P}}_{i,k}+\Delta {\tilde{V}}_{i,k}\Delta t+\frac{1}{2}\Delta {\tilde{R}}_{i,k}{\mathbf{a}}_k(\Delta t{)}^2\\ \Delta {\tilde{V}}_{i,k+1}& =\Delta {\tilde{V}}_{i,k}+{\mathbf{a}}_k\Delta t\end{array}$$

其中${\mathbf{a}}_{j-1}={\tilde{\mathbf{a}}}_{j-1}-{\mathbf{b}}_i^g$其中$\tilde{\mathbf{a}}$为加速度测量值，其中包含了加速度计偏移${\mathbf{b}}_i^g$和测量噪声${\eta }_a$, i表示第一个帧（关键帧）的位置。因为在void Preintegrated::Reintegrate()，或者在void Preintegrated::MergePrevious(Preintegrated* pPrev)会根据从i帧起接收的IMU测量帧数，循环调用IntegrateNewMeasurement。所以这部分其实是在计算预积分的测量值（包含噪声）：
$$\Delta {\tilde{\mathbf{p}}}_{ij}\triangleq \sum _{k=i}^{j-1}\left[\Delta {\tilde{\mathbf{V}}}_{ik}\Delta t+\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}\cdot {\mathbf{a}}_k\Delta t^2\right]$$

$$\Delta {\tilde{\mathbf{v}}}_{jj}\triangleq \sum _{k=1}^{j-1}\left[\Delta {\tilde{\mathbf{R}}}_{ik}\cdot {\mathbf{a}}_k\cdot \Delta t\right]$$

2.Compute velocity and position parts of matrices A and B (rely on non-updated delta rotation) A，B的第二第三行块。
$${\mathbf{A}}_{j-1}=\left[\begin{array}{ccc}\Delta {\tilde{\mathbf{R}}}_{jj-1}& 0& 0\\ -\Delta {\tilde{\mathbf{R}}}_{ij-1}\cdot {\left({\tilde{\mathbf{a}}}_{j-1}-{\mathbf{b}}_i^a\right)}^{\wedge }\Delta t& \mathbf{I}& 0\\ -\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ij-1}\cdot {\left({\tilde{\mathbf{a}}}_{j-1}-{\mathbf{b}}_i^a\right)}^{\wedge }\Delta t^2& \Delta t\mathbf{I}& \mathbf{I}\end{array}\right]$$

$${\mathbf{B}}_{j-1}=\left[\begin{array}{cc}{\mathbf{J}}_r^{j-1}\Delta t& 0\\ 0& \Delta {\tilde{\mathbf{R}}}_{ij-1}\Delta t\\ 0& \frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ij-1}\Delta t^2\end{array}\right]$$

有了A， B就有了预积分的递推公式：
$${\mathbf{\eta }}_{ij}^{\Delta }={\mathbf{A}}_{j-1}{\mathbf{\eta }}_{ij-1}^{\Delta }+{\mathbf{B}}_{j-1}{\mathbf{\eta }}_{j-1}^d$$

3.Update position and velocity jacobians wrt bias correction

对1中的式子分别关于a和g求导可得：
$$\begin{array}{rl}{J}_{\tilde{P}a(k)}& =J_{\tilde{P}a(k-1)}+J_{\tilde{V}a(k-1)}\Delta t-\frac{1}{2}\Delta {\tilde{R}}_{k-1}(\Delta t{)}^2\\ J_{\tilde{P}g(k)}& =J_{\tilde{P}g(k-1)}+J_{\tilde{V}g(k-1)}\Delta t-\frac{1}{2}\Delta {\tilde{R}}_{k-1}(\Delta t{)}^2({\mathbf{a}}_{k-1}{)}^{\wedge }{J}_{Rg(k-1)}\\ J_{\tilde{V}a(k)}& =J_{\tilde{V}a(k-1)}-\Delta {\tilde{R}}_{k-1}\Delta t\\ J_{\tilde{V}g(k)}& =J_{\tilde{V}g(k-1)}-\Delta {\tilde{R}}_{k-1}\Delta t({\mathbf{a}}_{k-1}{)}^{\wedge }{J}_{Rg(k-1)}\end{array}$$
其中需要注意，代码中关于$g$的导数只指的是关于${\omega }_k$的导数，在下一部分可以看到，在求$\Delta R_k$时，需要用到${\omega }_k$，推导如下(省略了下标i)：
$$\begin{array}{rl}\frac{\partial \Delta {\tilde{R}}_{k-1}{\mathbf{a}}_{k-1}}{\partial {\omega }_{k-1}}& =\underset{\delta {\tilde{\mathbf{\omega }}}_{k-1}\to 0}{\lim }\frac{\Delta {\tilde{R}}_{k-2}\mathrm{Exp}\left(\left({\tilde{\mathbf{\omega }}}_{k-1}+\delta {\tilde{\mathbf{\omega }}}_{k-1}-{\mathbf{b}}_i^g\right)\Delta t\right){\mathbf{a}}_{k-1}-\Delta {\tilde{R}}_{k-2}\mathrm{Exp}\left(\left({\tilde{\mathbf{\omega }}}_{k-1}-{\mathbf{b}}_i^g\right)\Delta t\right){\mathbf{a}}_{k-1}}{\delta {\tilde{\mathbf{\omega }}}_{k-1}}\\ & =\underset{\delta {\tilde{\mathbf{\omega }}}_{k-1}\to 0}{\lim }\frac{\Delta {\tilde{R}}_{k-2}\mathrm{Exp}\left(\left({\tilde{\mathbf{\omega }}}_{k-1}-{\mathbf{b}}_i^g\right)\Delta t\right)\mathrm{Exp}\left(J_{\Delta {\tilde{R}}_{k-1}}\delta {\tilde{\mathbf{\omega }}}_{k-1}\Delta t\right){\mathbf{a}}_{k-1}-\Delta {\tilde{R}}_{k-2}\mathrm{Exp}\left(\left({\tilde{\mathbf{\omega }}}_{k-1}-{\mathbf{b}}_i^g\right)\Delta t\right){\mathbf{a}}_{k-1}}{\delta {\tilde{\mathbf{\omega }}}_{k-1}}\\ & =\underset{\delta {\tilde{\mathbf{\omega }}}_{k-1}\to 0}{\lim }\frac{\Delta {\tilde{R}}_{k-2}\mathrm{Exp}\left(\left({\tilde{\mathbf{\omega }}}_{k-1}-{\mathbf{b}}_i^g\right)\Delta t\right)\left(\text{I}+{\left(J_{\Delta {\tilde{R}}_{k-1}}\delta {\tilde{\mathbf{\omega }}}_{k-1}\Delta t\right)}^{\wedge }\right){\mathbf{a}}_{k-1}-\Delta {\tilde{R}}_{k-2}\mathrm{Exp}\left(\left({\tilde{\mathbf{\omega }}}_{k-1}-{\mathbf{b}}_i^g\right)\Delta t\right){\mathbf{a}}_{k-1}}{\delta {\tilde{\mathbf{\omega }}}_{k-1}}\\ & =\underset{\delta {\tilde{\mathbf{\omega }}}_{k-1}\to 0}{\lim }\frac{{\left(J_{\Delta {\tilde{R}}_{k-1}}\delta {\tilde{\mathbf{\omega }}}_{k-1}\Delta t\right)}^{\wedge }{\mathbf{a}}_{k-1}}{\delta {\tilde{\mathbf{\omega }}}_{k-1}}\\ & ={\left({\mathbf{a}}_{k-1}\right)}^{\wedge }{J}_{\Delta {\tilde{R}}_{k-1}}\Delta t\\ & ={\left({\mathbf{a}}_{k-1}\right)}^{\wedge }{J}_{Rg}\Delta t\end{array}$$

4.Update delta rotation

这一部分实例化了一个IntegratedRotation，叫做dRi。IntegratedRotation构造函数如下：

IntegratedRotation::IntegratedRotation(const cv::Point3f &angVel, const Bias &imuBias, const float &time):
    deltaT(time)
{
    const float x = (angVel.x-imuBias.bwx)*time;
    const float y = (angVel.y-imuBias.bwy)*time;
    const float z = (angVel.z-imuBias.bwz)*time;

    cv::Mat I = cv::Mat::eye(3,3,CV_32F);

    const float d2 = x*x+y*y+z*z;
    const float d = sqrt(d2);

    cv::Mat W = (cv::Mat_<float>(3,3) << 0, -z, y,
                 z, 0, -x,
                 -y,  x, 0);
    if(d<eps)
    {
        deltaR = I + W;
        rightJ = cv::Mat::eye(3,3,CV_32F);
    }
    else
    {
        deltaR = I + W*sin(d)/d + W*W*(1.0f-cos(d))/d2;
        rightJ = I - W*(1.0f-cos(d))/d2 + W*W*(d-sin(d))/(d2*d);
    }
}

上述代码中的$deltaR$就是$exp((w\Delta t{)}^{\wedge })$, R的积分公式如下：
$${\mathbf{R}}_{b(t+\Delta t)}^W={\mathbf{R}}_{b(t)}^W\mathrm{Exp}\left(\left({\tilde{\mathbf{\omega }}}_k-{\mathbf{b}}_i^g\right)\cdot \Delta t\right)$$
它可由运动微分方程${\dot{\mathbf{R}}}_b^w={\mathbf{R}}_b^w{\left({\mathbf{\omega }}_{wb}^b\right)}^{\wedge }$通过Euler Integration得到。

与1相同，通过循环调用，完成如下操作：
$$\Delta {\tilde{\mathbf{R}}}_{ij}=\prod _{k=1}^{j-1}\mathrm{Exp}\left(\left({\tilde{\mathbf{\omega }}}_k-{\mathbf{b}}_i^g\right)\Delta t\right)$$

5.Update rotation jacobian wrt bias correction

可以用四中的结果计算A ，B的第一行块。

6.Update covariance

可以用2中的递推公式来计算预积分的雅可比：
$${\mathbf{\Sigma }}_{ij}={\mathbf{A}}_{j-1}{\mathbf{\Sigma }}_{ij-1}{\mathbf{A}}_{j-1}^T+{\mathbf{B}}_{j-1}{\mathbf{\Sigma }}_{\mathbf{\eta }}{\mathbf{B}}_{j-1}^T{\mathbf{\Sigma }}_{ij}={\mathbf{A}}_{j-1}{\mathbf{\Sigma }}_{ij-1}{\mathbf{A}}_{j-1}^T+{\mathbf{B}}_{j-1}{\mathbf{\Sigma }}_{\mathbf{\eta }}{\mathbf{B}}_{j-1}^T$$

7.Update rotation jacobian wrt bias correction
$$J_{Rg}=J_{Rg}\Delta t-J_r(deltaR)\Delta t;$$

有了预积分的雅可比矩阵，那么EdgeInertial中关于$\omega$和a的雅可比就很好求了。

EdgeGyroRW边建立相邻关键帧之间陀螺仪偏移的边

vegr[i] = new EdgeGyroRW();
vegr[i]->setVertex(0,VG1);
vegr[i]->setVertex(1,VG2);

EdgeAccRW建立相邻关键帧之间加速度偏移的边

vear[i] = new EdgeAccRW();
vear[i]->setVertex(0,VA1);
vear[i]->setVertex(1,VA2);

以上两个边的更新和雅可比矩阵计算就很简单了，误差项就为两个值相减

//角速度误差计算
void computeError(){
    const VertexGyroBias* VG1= static_cast<const VertexGyroBias*>(_vertices[0]);
    const VertexGyroBias* VG2= static_cast<const VertexGyroBias*>(_vertices[1]);
    _error = VG2->estimate()-VG1->estimate();
}
// 加速度误差计算
void computeError(){
        const VertexAccBias* VA1= static_cast<const VertexAccBias*>(_vertices[0]);
        const VertexAccBias* VA2= static_cast<const VertexAccBias*>(_vertices[1]);
        _error = VA2->estimate()-VA1->estimate();
    }

误差项的形式很简单所以雅可比也就很简单：

//角速度雅可比
virtual void linearizeOplus(){
    _jacobianOplusXi = -Eigen::Matrix3d::Identity();
    _jacobianOplusXj.setIdentity();
}
//加速度雅可比
virtual void linearizeOplus(){
    _jacobianOplusXi = -Eigen::Matrix3d::Identity();
    _jacobianOplusXj.setIdentity();
}

视觉边设置

根据使用的相机种类定义了单目边和双目边：

EdgeMono
EdgeStereo

这部分就是通过重投影误差进行约束，具体就没细看了，但推导起来，视觉SLAM14讲就够了。

参考：《视觉slam14讲》

​ 《机器人学中的状态估计》

​ 预积分推导——邱笑晨