ORBSLAM3 --- 优化（一）：g2o优化中的节点与边的定义-G2oTypes.h、G2oTypes.cc解析

目录

1.节点类

1.1 ImuCamPose类

1.1.1 类的定义

1.1.2  ImuCamPose::ImuCamPose

1.1.3  ImuCamPose::SetParam

1.1.4  ImuCamPose::isDepthPositive

1.2 VertexPose类

1.2.1 节点定义

1.2.2 ImuCamPose::Update函数解析

1.3 VertexPose4DoF类

1.3.1 节点定义

1.3.2  ImuCamPose::UpdateW函数解析

1.4 速度节点VertexVelocity

1.4.1 节点定义

1.5 陀螺仪偏置节点VertexGyroBias和加速度计偏置节点VertexAccBias

1.5.1 节点定义

1.6 重力方向节点VertexGDir

1.6.1 重力方向类GDirection

1.6.2 节点定义

1.7 尺度节点 VertexScale

1.7.1 节点定义

2.边

2.1 单目重投影边EdgeMono

2.1.1 边的定义

2.1.2 误差计算computeError

2.1.3 单目投影函数 ImuCamPose::Project

2.1.4 雅可比计算 EdgeMono::linearizeOplus

2.3 单目仅优化位姿一元边

2.3.1 边的定义

2.3.2 雅可比的计算

2.4 双目位姿三维点二元边EdgeStereo

2.4.1 边的定义

2.4.2  误差计算方法computeError

2.5 双目纯位姿一元边 EdgeStereoOnlyPose

2.5.1 边的定义

2.6 惯性边EdgeInertial（误差为残差）

2.6.1 边的定义

2.6.2 惯性边的构造函数

2.6.3 误差/残差计算 computeError

2.6.4 雅可比计算

2.7 初始化惯性边（误差为残差）EdgeInertialGS

2.7.1 边的定义

2.7.2 构造函数

2.7.3 误差/残差计算

2.7.4 雅可比计算

2.8 陀螺仪偏置的二元边，除了残差及重投影误差外的第三个边，控制偏置变化EdgeGyroRW

2.8.1 边的定义

2.8.2 误差定义

2.8.3 雅克比的计算

2.9  加速度计偏置的二元边，除了残差及重投影误差外的第三个边，控制偏置变化EdgeAccRW

2.9.1 边的定义

2.10  先验边EdgePriorPoseImu

2.10.1 先验类ConstraintPoseImu

2.10.2 边的定义

2.11 根据给定值的加速度计先验边，目的是把优化量维持在先验附近EdgePriorAcc

2.11.1 边的定义

1.节点类

1.1 ImuCamPose类

1.1.1 类的定义

// 相关节点中使用，存放的是imu与cam的内外参
class ImuCamPose
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    ImuCamPose() {}
    ImuCamPose(KeyFrame *pKF);
    ImuCamPose(Frame *pF);
    ImuCamPose(Eigen::Matrix3d &_Rwc, Eigen::Vector3d &_twc, KeyFrame *pKF);

    // 放参数的函数
    void SetParam(const std::vector<Eigen::Matrix3d> &_Rcw, const std::vector<Eigen::Vector3d> &_tcw, const std::vector<Eigen::Matrix3d> &_Rbc,
                    const std::vector<Eigen::Vector3d> &_tbc, const double &_bf);

    void Update(const double *pu);                                                   // update in the imu reference
    void UpdateW(const double *pu);                                                  // update in the world reference

    // 视觉的时候做重投影误差 对应的投影的过程
    Eigen::Vector2d Project(const Eigen::Vector3d &Xw, int cam_idx = 0) const;       // Mono
    Eigen::Vector3d ProjectStereo(const Eigen::Vector3d &Xw, int cam_idx = 0) const; // Stereo

    // 判断投影的点是否可用
    bool isDepthPositive(const Eigen::Vector3d &Xw, int cam_idx = 0) const;

public:
    // For IMU  IMU坐标系 -> 世界坐标系
    Eigen::Matrix3d Rwb;
    Eigen::Vector3d twb;

    // For set of cameras
    std::vector<Eigen::Matrix3d> Rcw;
    std::vector<Eigen::Vector3d> tcw;
    std::vector<Eigen::Matrix3d> Rcb, Rbc;
    std::vector<Eigen::Vector3d> tcb, tbc;

    // 与基线相关 基线*焦距
    double bf;
    // 相机类
    std::vector<GeometricCamera *> pCamera;

    // For posegraph 4DoF
    Eigen::Matrix3d Rwb0;
    Eigen::Matrix3d DR;

    int its; // 记录更新次数
};

        注释写的很详细，不再赘述。

1.1.2  ImuCamPose::ImuCamPose

        存储IMU和相机的变量 储存关键帧位姿相关的信息，用于优化。就是取数的...没啥可说。关键帧调用和普通帧调用都是一样的效果。

ImuCamPose::ImuCamPose(KeyFrame *pKF):its(0)
{
    // Load IMU pose
    twb = pKF->GetImuPosition().cast<double>();
    Rwb = pKF->GetImuRotation().cast<double>();

    // Load camera poses
    int num_cams;
    if(pKF->mpCamera2)
        num_cams=2;
    else
        num_cams=1;

    tcw.resize(num_cams);
    Rcw.resize(num_cams);
    tcb.resize(num_cams);
    Rcb.resize(num_cams);
    Rbc.resize(num_cams);
    tbc.resize(num_cams);
    pCamera.resize(num_cams);

    // Left camera
    tcw[0] = pKF->GetTranslation().cast<double>();
    Rcw[0] = pKF->GetRotation().cast<double>();
    tcb[0] = pKF->mImuCalib.mTcb.translation().cast<double>();
    Rcb[0] = pKF->mImuCalib.mTcb.rotationMatrix().cast<double>();
    Rbc[0] = Rcb[0].transpose();
    tbc[0] = pKF->mImuCalib.mTbc.translation().cast<double>();
    pCamera[0] = pKF->mpCamera;
    bf = pKF->mbf;

    if(num_cams>1)
    {
        Eigen::Matrix4d Trl = pKF->GetRelativePoseTrl().matrix().cast<double>();
        Rcw[1] = Trl.block<3,3>(0,0) * Rcw[0];
        tcw[1] = Trl.block<3,3>(0,0) * tcw[0] + Trl.block<3,1>(0,3);
        tcb[1] = Trl.block<3,3>(0,0) * tcb[0] + Trl.block<3,1>(0,3);
        Rcb[1] = Trl.block<3,3>(0,0) * Rcb[0];
        Rbc[1] = Rcb[1].transpose();
        tbc[1] = -Rbc[1] * tcb[1];
        pCamera[1] = pKF->mpCamera2;
    }

    // For posegraph 4DoF
    Rwb0 = Rwb;
    DR.setIdentity();
}

1.1.3  ImuCamPose::SetParam

        设置相关数据。

void ImuCamPose::SetParam(
    const std::vector<Eigen::Matrix3d> &_Rcw, const std::vector<Eigen::Vector3d> &_tcw,
    const std::vector<Eigen::Matrix3d> &_Rbc, const std::vector<Eigen::Vector3d> &_tbc, const double &_bf)
{
    Rbc = _Rbc;
    tbc = _tbc;
    Rcw = _Rcw;
    tcw = _tcw;
    const int num_cams = Rbc.size();
    Rcb.resize(num_cams);
    tcb.resize(num_cams);

    for(int i=0; i<tcb.size(); i++)
    {
        Rcb[i] = Rbc[i].transpose();
        tcb[i] = -Rcb[i]*tbc[i];
    }
    Rwb = Rcw[0].transpose()*Rcb[0];
    twb = Rcw[0].transpose()*(tcb[0]-tcw[0]);

    bf = _bf;
}

        设置参数，更新参数用，也没什么好说的。

1.1.4  ImuCamPose::isDepthPositive

        判断深度值是否有效。

bool ImuCamPose::isDepthPositive(const Eigen::Vector3d &Xw, int cam_idx) const
{
    return (Rcw[cam_idx].row(2) * Xw + tcw[cam_idx](2)) > 0.0;
}

        即从世界坐标系投影到相机坐标系之后，看看相机坐标系下的z值是否为0。

1.2 VertexPose类

1.2.1 节点定义

        继承于g2o中的BaseVertex，自由度为6，类为第一节中的ImuCamPose。

class VertexPose : public g2o::BaseVertex<6, ImuCamPose>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    VertexPose() {}
    VertexPose(KeyFrame *pKF)
    {
        // g2o Vertex里面带的功能性函数 ，目的是要把估计的数放入节点里面
        setEstimate(ImuCamPose(pKF));
    }
    VertexPose(Frame *pF)
    {
        setEstimate(ImuCamPose(pF));
    }

    virtual bool read(std::istream &is);
    virtual bool write(std::ostream &os) const;

    // 重置函数,设定被优化变量的原始值
    virtual void setToOriginImpl()
    {
    }

    // 在g2o优化时更新
    // TODO  g2o优化的时候得到一个derta 最後会返回到节点update_中，通过oplusImpl函数自动更新
    virtual void oplusImpl(const double *update_)
    {
        // https://github.com/RainerKuemmerle/g2o/blob/master/doc/README_IF_IT_WAS_WORKING_AND_IT_DOES_NOT.txt
        // 官方讲解cache
        // 需要在oplusImpl与setEstimate函数中添加
        _estimate.Update(update_);

        // 官方推荐在使用完Update后要用updatecache函数
        // 加完之后会提高速度？？？
        updateCache();
    }
};

        我们在了解g2o节点时，要注意两个函数：

        setEstimate：g2oVertex中带的功能性函数，我们要把估计的数放在节点里面。因此我们需要把ImuCamPose类放到里面（ImuCamPose类有两个初始化的构造函数正好对应关键帧和普通帧的）

        oplusImpl：g2o优化的时候我们会得到一个变量（旋转、速度、位移），最后会返回到节点const double *update_中，通过这个函数自动更新的。

1.2.2 ImuCamPose::Update函数解析

        我们来看一下它是怎么更新的，首先我们要了解的是它是根据扰动进行更新的，Update函数中主要对几个变量进行了更新：

        1.：IMU相对于世界坐标系的平移向量

        2.：IMU相对于世界坐标系的旋转矩阵

        3.：世界坐标系到IMU坐标系的旋转矩阵

        4.世界坐标系到IMU坐标系的平移向量

        5.        

        6.

        我们看一下扰动如何定义：

        代码中pu时优化出的更新值（在后文会讲优化相关知识），包括旋转和平移。

/** 
 * @brief 优化算出更新值，更新到状态中
 * @param pu 更新值
 */
void ImuCamPose::Update(const double *pu)
{
    Eigen::Vector3d ur, ut;
    // 旋转
    ur << pu[0], pu[1], pu[2];
    // 平移
    ut << pu[3], pu[4], pu[5];

    // 更新的是imu位姿,按照imu预积分公式更新
    // 更新平移和旋转
    twb += Rwb * ut;
    Rwb = Rwb * ExpSO3(ur);

    // Normalize rotation after 5 updates
    // 由于节点是不断优化的，随着优化的次数增加，旋转的连乘乘多了可能不符合正交的形式，乘多了需要保证正交性对其标准化
    its++;
    if(its>=3)
    {
        NormalizeRotation(Rwb);
        its=0;
    }

    // Update camera poses
    const Eigen::Matrix3d Rbw = Rwb.transpose();
    const Eigen::Vector3d tbw = -Rbw * twb;

    for(int i=0; i<pCamera.size(); i++)
    {
        Rcw[i] = Rcb[i] * Rbw;
        tcw[i] = Rcb[i] * tbw + tcb[i];
    }

}

        Rcw、tcw是ImuCamPose类内变量，表示某一帧的相机坐标。

1.3 VertexPose4DoF类

1.3.1 节点定义

        继承于g2o中的BaseVertex，自由度为4，类为第一节中的ImuCamPose。它只优化了三个平移和一个航偏角。

// 优化中关于位姿的节点，4自由度  3个平移加一个航偏
class VertexPose4DoF : public g2o::BaseVertex<4, ImuCamPose>
{
    // Translation and yaw are the only optimizable variables
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    VertexPose4DoF() {}
    VertexPose4DoF(KeyFrame *pKF)
    {
        setEstimate(ImuCamPose(pKF));
    }
    VertexPose4DoF(Frame *pF)
    {
        setEstimate(ImuCamPose(pF));
    }
    VertexPose4DoF(Eigen::Matrix3d &_Rwc, Eigen::Vector3d &_twc, KeyFrame *pKF)
    {

        setEstimate(ImuCamPose(_Rwc, _twc, pKF));
    }

    virtual bool read(std::istream &is) { return false; }
    virtual bool write(std::ostream &os) const { return false; }

    virtual void setToOriginImpl()
    {
    }

    // 强制让旋转的前两维的更新量为0
    virtual void oplusImpl(const double *update_)
    {
        double update6DoF[6];
        update6DoF[0] = 0;
        update6DoF[1] = 0;
        update6DoF[2] = update_[0];
        update6DoF[3] = update_[1];
        update6DoF[4] = update_[2];
        update6DoF[5] = update_[3];
        _estimate.UpdateW(update6DoF);
        updateCache();
    }
};

        更新的时候有点不一样，它将除了航偏角之外的俩角度置为了0。

1.3.2  ImuCamPose::UpdateW函数解析

// 更新世界坐标系的坐标
void ImuCamPose::UpdateW(const double *pu)
{
    Eigen::Vector3d ur, ut;
    // 三个旋转向量
    ur << pu[0], pu[1], pu[2];
    // 三个平移向量
    ut << pu[3], pu[4], pu[5];

    // derta R
    // DR 最开始是一个单位矩阵，随着类更新的迭代次数增加，DR就表明从开始到现在的全部的dR的连乘的结果
    const Eigen::Matrix3d dR = ExpSO3(ur);
    DR = dR * DR;
    // Rwb0 最开始的定义 Rwb0 = Rwb 修改世界坐标系的位置
    Rwb = DR * Rwb0;
    // Update body pose
    twb += ut;

    // Normalize rotation after 5 updates
    its++;
    if(its>=5)
    {
        DR(0,2) = 0.0;
        DR(1,2) = 0.0;
        DR(2,0) = 0.0;
        DR(2,1) = 0.0;
        NormalizeRotation(DR);
        its = 0;
    }

    // Update camera pose
    const Eigen::Matrix3d Rbw = Rwb.transpose();
    const Eigen::Vector3d tbw = -Rbw * twb;

    for(int i=0; i<pCamera.size(); i++)
    {
        Rcw[i] = Rcb[i] * Rbw;
        tcw[i] = Rcb[i] * tbw+tcb[i];
    }
}

        const Eigen::Matrix3d dR = ExpSO3(ur); 将变成了一个旋转矩阵。（Exp过后的）

        DR在ImuCamPose类定义：在实现中将其初始化为一个单位矩阵。

/** 
 * @brief 储存位姿相关的信息，用于优化
 */
ImuCamPose::ImuCamPose(Eigen::Matrix3d &_Rwc, Eigen::Vector3d &_twc, KeyFrame* pKF): its(0)
{
    // This is only for posegrpah, we do not care about multicamera
    tcw.resize(1);
    Rcw.resize(1);
    tcb.resize(1);
    Rcb.resize(1);
    Rbc.resize(1);
    tbc.resize(1);
    pCamera.resize(1);

    tcb[0] = pKF->mImuCalib.mTcb.translation().cast<double>();
    Rcb[0] = pKF->mImuCalib.mTcb.rotationMatrix().cast<double>();
    Rbc[0] = Rcb[0].transpose();
    tbc[0] = pKF->mImuCalib.mTbc.translation().cast<double>();
    twb = _Rwc * tcb[0] + _twc;
    Rwb = _Rwc * Rcb[0];
    Rcw[0] = _Rwc.transpose();
    tcw[0] = -Rcw[0] * _twc;
    pCamera[0] = pKF->mpCamera;
    bf = pKF->mbf;

    // For posegraph 4DoF
    Rwb0 = Rwb;
    DR.setIdentity();
}

        DR 最开始是一个单位矩阵，随着类更新的迭代次数增加，DR就表明从开始到现在的全部的dR的连乘的结果。（DR = dR * DR;）

        Rwb0定义为Rwb，即不管Rwb在中间如何变化的，Rwb0就是Rwb的初始值。

        也就是说，这里的Rwb更新方式使用刚开始没有更新的值，乘以这段时间的积分更新的。这里左乘更新的是世界坐标系的坐标，Update中是右乘更新的是IMU的位姿。可以认为DR是一个R_{w2w1}。也就是说UpdateW更新了世界坐标系的位姿。

// 更新世界坐标系的坐标
void ImuCamPose::UpdateW(const double *pu)
{
    Eigen::Vector3d ur, ut;
    // 三个旋转向量
    ur << pu[0], pu[1], pu[2];
    // 三个平移向量
    ut << pu[3], pu[4], pu[5];

    // derta R
    // DR 最开始是一个单位矩阵，随着类更新的迭代次数增加，DR就表明从开始到现在的全部的dR的连乘的结果
    const Eigen::Matrix3d dR = ExpSO3(ur);
    DR = dR * DR;
    // Rwb0 最开始的定义 Rwb0 = Rwb 修改世界坐标系的位置
    Rwb = DR * Rwb0;
    // Update body pose
    twb += ut;

    // Normalize rotation after 5 updates
    its++;
    if(its>=5)
    {
        DR(0,2) = 0.0;
        DR(1,2) = 0.0;
        DR(2,0) = 0.0;
        DR(2,1) = 0.0;
        NormalizeRotation(DR);
        its = 0;
    }

    // Update camera pose
    const Eigen::Matrix3d Rbw = Rwb.transpose();
    const Eigen::Vector3d tbw = -Rbw * twb;

    for(int i=0; i<pCamera.size(); i++)
    {
        Rcw[i] = Rcb[i] * Rbw;
        tcw[i] = Rcb[i] * tbw+tcb[i];
    }
}

1.4 速度节点VertexVelocity

1.4.1 节点定义

        3自由度的速度节点。

/** 
 * @brief 速度节点
 */
class VertexVelocity : public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    VertexVelocity() {}
    VertexVelocity(KeyFrame *pKF);
    VertexVelocity(Frame *pF);

    virtual bool read(std::istream &is) { return false; }
    virtual bool write(std::ostream &os) const { return false; }

    virtual void setToOriginImpl()
    {
    }

    virtual void oplusImpl(const double *update_)
    {
        Eigen::Vector3d uv;
        uv << update_[0], update_[1], update_[2];
        setEstimate(estimate() + uv);
    }
};

        根据我们定义速度的扰动：

        更新速度的时候直接相加即可：

    virtual void oplusImpl(const double *update_)
    {
        Eigen::Vector3d uv;
        uv << update_[0], update_[1], update_[2];
        setEstimate(estimate() + uv);
    }

1.5 陀螺仪偏置节点VertexGyroBias和加速度计偏置节点VertexAccBias

1.5.1 节点定义

/** 
 * @brief 陀螺仪偏置节点
 */
class VertexGyroBias : public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    VertexGyroBias() {}
    VertexGyroBias(KeyFrame *pKF);
    VertexGyroBias(Frame *pF);

    virtual bool read(std::istream &is) { return false; }
    virtual bool write(std::ostream &os) const { return false; }

    virtual void setToOriginImpl()
    {
    }

    virtual void oplusImpl(const double *update_)
    {
        Eigen::Vector3d ubg;
        ubg << update_[0], update_[1], update_[2];
        setEstimate(estimate() + ubg);
    }
};

/** 
 * @brief 加速度计偏置节点
 */
class VertexAccBias : public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    VertexAccBias() {}
    VertexAccBias(KeyFrame *pKF);
    VertexAccBias(Frame *pF);

    virtual bool read(std::istream &is) { return false; }
    virtual bool write(std::ostream &os) const { return false; }

    virtual void setToOriginImpl()
    {
    }

    virtual void oplusImpl(const double *update_)
    {
        Eigen::Vector3d uba;
        uba << update_[0], update_[1], update_[2];
        setEstimate(estimate() + uba);
    }
};

        均是三自由度的Vector3d节点。

        根据我们定义偏置节点的扰动：

        更新速度的时候直接相加即可：

    virtual void oplusImpl(const double *update_)
    {
        Eigen::Vector3d uba;
        uba << update_[0], update_[1], update_[2];
        setEstimate(estimate() + uba);
    }

    virtual void oplusImpl(const double *update_)
    {
        Eigen::Vector3d ubg;
        ubg << update_[0], update_[1], update_[2];
        setEstimate(estimate() + ubg);
    }

1.6 重力方向节点VertexGDir

1.6.1 重力方向类GDirection

// Gravity direction vertex
// 重力方向
class GDirection
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    GDirection() {}
    GDirection(Eigen::Matrix3d pRwg) : Rwg(pRwg) {}

    void Update(const double *pu)
    {
        // 强行优化不可观的数据，会导致不收敛
        // 更新前两维 第三维不要了
        Rwg = Rwg * ExpSO3(pu[0], pu[1], 0.0);
    }

    Eigen::Matrix3d Rwg, Rgw;

    int its;
};

        这个类里面有重力坐标系向世界坐标系的旋转矩阵以及迭代次数its。

        更新方式为更新前两维舍弃第三维。

1.6.2 节点定义

        自由度为2。因为第三维不优化。

/** 
 * @brief 重力方向节点
 */
class VertexGDir : public g2o::BaseVertex<2, GDirection>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    VertexGDir() {}
    VertexGDir(Eigen::Matrix3d pRwg)
    {
        setEstimate(GDirection(pRwg));
    }

    virtual bool read(std::istream &is) { return false; }
    virtual bool write(std::ostream &os) const { return false; }

    virtual void setToOriginImpl()
    {
    }

    virtual void oplusImpl(const double *update_)
    {
        _estimate.Update(update_);
        updateCache();
    }
};

1.7 尺度节点 VertexScale

1.7.1 节点定义

        只需优化s，故自由度为1。

// scale vertex
/** 
 * @brief 尺度节点
 */
class VertexScale : public g2o::BaseVertex<1, double>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    VertexScale()
    {
        setEstimate(1.0);
    }
    VertexScale(double ps)
    {
        setEstimate(ps);
    }

    virtual bool read(std::istream &is) { return false; }
    virtual bool write(std::ostream &os) const { return false; }

    virtual void setToOriginImpl()
    {
        setEstimate(1.0);
    }

    virtual void oplusImpl(const double *update_)
    {
        setEstimate(estimate() * exp(*update_));
    }
};

        尺度的更新方式是乘以exp的格式。要保证尺度是正值。

2.边

2.1 单目重投影边EdgeMono

2.1.1 边的定义

        ORBSLAM2中用的都是g2o里面的边，现在我们因为引入了IMU因此需要重新定义边（g2o定义的边没有考虑到IMU）。

        2元边， 误差维度为2元，误差类型为Eigen::Vector2d， 二元边连接的节点分别是：

        节点1类型为g2o::VertexSBAPointXYZ（g2o自带的三维点的节点）。

        节点2类型为VertexPose（R，t）。

        按照节点顺序取节点_vertices[0] = VertexSBAPointXYZ _vertices[1]=VertexPose

        关于边我们也是有两个点需要注意：一是如何计算边的误差函数computeError、二是linearizeOplus，其存放计算雅可比的矩阵。

/** 
 * @brief 单目视觉重投影的边
 * 这里或许会有疑问，OptimizableTypes.h 里面不是定义了视觉重投影的边么？
 * 原因是这样，那里面定义的边也用，不过只是在纯视觉时，没有imu情况下，因为用已经定义好的节点就好
 * 但是加入imu时，优化要有残差的边与重投影的边同时存在，且两个边可能连接同一个位姿节点，所以需要重新弄一个包含imu位姿的节点
 * 因此，边也需要重新写，并且在imu优化时使用这个边
 */
// 误差为2维， 类型为Eigen::Vector2d， 节点1类型为g2o::VertexSBAPointXYZ，节点二类型为VertexPose
// 二元边 2 代表误差的维度  Eigen::Vector2d代表误差的类型 g2o::VertexSBAPointXYZ（维度为3）, VertexPose代表二元边链接的节点的类型（维度为6）
class EdgeMono : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ, VertexPose>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EdgeMono(int cam_idx_ = 0) : cam_idx(cam_idx_)
    {
    }

    virtual bool read(std::istream &is) { return false; }
    virtual bool write(std::ostream &os) const { return false; }

    // 计算重投影误差
    void computeError()
    {
        // _vertices[0] 代表g2o::VertexSBAPointXYZ节点  _vertices[1] 代表VertexPose节点

        // 先把三维点提取出来
        const g2o::VertexSBAPointXYZ *VPoint = static_cast<const g2o::VertexSBAPointXYZ *>(_vertices[0]);
        // 把节点的pose提取出来
        const VertexPose *VPose = static_cast<const VertexPose *>(_vertices[1]);
	// 定义观测，在定义边的时候就会定义
        const Eigen::Vector2d obs(_measurement);
        // VPose->estimate()返回一个VertexPose类，里面有个投影的函数（3D点信息 + 相机ID）
        // VPose->estimate().Project(VPoint->estimate(), cam_idx)返回的是像素坐标 obs也是像素坐标 两个相减也是像素坐标
        _error = obs - VPose->estimate().Project(VPoint->estimate(), cam_idx);
    }

    // 存放计算雅可比的函数
    virtual void linearizeOplus();

    bool isDepthPositive()
    {
        const g2o::VertexSBAPointXYZ *VPoint = static_cast<const g2o::VertexSBAPointXYZ *>(_vertices[0]);
        const VertexPose *VPose = static_cast<const VertexPose *>(_vertices[1]);
        return VPose->estimate().isDepthPositive(VPoint->estimate(), cam_idx);
    }

    Eigen::Matrix<double, 2, 9> GetJacobian()
    {
        linearizeOplus();
        Eigen::Matrix<double, 2, 9> J;
        J.block<2, 3>(0, 0) = _jacobianOplusXi;
        J.block<2, 6>(0, 3) = _jacobianOplusXj;
        return J;
    }

    // 由2*2的像素点信息矩阵变成了9*9的关于旋转平移与三维点坐标的信息矩阵
    // 可以理解为像素的不确定性给旋转平移跟三维点带来了多大的不确定性
    // 在前段跟踪时使用 滑窗/边缘化的时候使用
    // 像素点误差的信息矩阵为information为已知的，要根据像素点的信息矩阵返回到变量的信息矩阵中（9*9的信息矩阵）
    Eigen::Matrix<double, 9, 9> GetHessian()
    {
        linearizeOplus();
        Eigen::Matrix<double, 2, 9> J;
        // 三维点
        J.block<2, 3>(0, 0) = _jacobianOplusXi;
        // R，t
        J.block<2, 6>(0, 3) = _jacobianOplusXj;
        return J.transpose() * information() * J;
    }

public:
    const int cam_idx;
};

        返回信息矩阵的功能是在前段做跟踪，滑窗和边缘化的时候会用到信息矩阵。

2.1.2 误差计算computeError

    // 计算重投影误差
    void computeError()
    {
        // _vertices[0] 代表g2o::VertexSBAPointXYZ节点  _vertices[1] 代表VertexPose节点

        // 先把三维点提取出来
        const g2o::VertexSBAPointXYZ *VPoint = static_cast<const g2o::VertexSBAPointXYZ *>(_vertices[0]);
        // 把节点的pose提取出来
        const VertexPose *VPose = static_cast<const VertexPose *>(_vertices[1]);
	// 定义观测，在定义边的时候就会定义
        const Eigen::Vector2d obs(_measurement);
        // VPose->estimate()返回一个VertexPose类，里面有个投影的函数（3D点信息 + 相机ID）
        // VPose->estimate().Project(VPoint->estimate(), cam_idx)返回的是像素坐标 obs也是像素坐标 两个相减也是像素坐标
        _error = obs - VPose->estimate().Project(VPoint->estimate(), cam_idx);
    }

        就是计算重投影误差。

        const g2o::VertexSBAPointXYZ *VPoint = static_cast<const g2o::VertexSBAPointXYZ *>(_vertices[0]);就是把_vertices[0] = g2o::VertexSBAPointXYZ的三维点取出来。

        在把节点的pose取出来（R、t）

        观测就是节点的真实值。

        VPose->estimate().Project(VPoint->estimate(), cam_idx);是投影点的像素坐标。

        VPose->estimate()返回了一个VertexPose类。里面有个投影函数，其输入是点的坐标相机的ID。

2.1.3 单目投影函数 ImuCamPose::Project

/** 
 * @brief 单目投影
 */
Eigen::Vector2d ImuCamPose::Project(const Eigen::Vector3d &Xw, int cam_idx) const
{
    // 将世界坐标系下的点坐标投影到相机坐标系下
    Eigen::Vector3d Xc = Rcw[cam_idx] * Xw + tcw[cam_idx];

    return pCamera[cam_idx]->project(Xc);
}

        将点在世界坐标系下的坐标投影到相机坐标系下。返回的是像素坐标。

2.1.4 雅可比计算 EdgeMono::linearizeOplus

/** 
 * @brief 单目视觉边计算雅克比
 * _jacobianOplusXi对应着_vertices[0] 也就是误差对于三维点的雅克比
 * _jacobianOplusXj就对应着位姿
 */
void EdgeMono::linearizeOplus()
{
    // R、t + 三维点
    const VertexPose* VPose = static_cast<const VertexPose*>(_vertices[1]);
    const g2o::VertexSBAPointXYZ* VPoint = static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);

    const Eigen::Matrix3d &Rcw = VPose->estimate().Rcw[cam_idx];
    const Eigen::Vector3d &tcw = VPose->estimate().tcw[cam_idx];
    const Eigen::Vector3d Xc = Rcw*VPoint->estimate() + tcw;
    const Eigen::Vector3d Xb = VPose->estimate().Rbc[cam_idx]*Xc+VPose->estimate().tbc[cam_idx];
    const Eigen::Matrix3d &Rcb = VPose->estimate().Rcb[cam_idx];

    // 将三维点通过R,t投影到相机坐标系下 有一个雅可比  相机坐标系投影到图像坐标系下也有雅可比（相机里面计算的）
    
    const Eigen::Matrix<double,2,3> proj_jac = VPose->estimate().pCamera[cam_idx]->projectJac(Xc);

    // 误差 = 观测-预测
    // -1 误差相对于预测的
    // proj_jac 像素相对于相机坐标系下的三维点的雅克比
    // Rcw 相机坐标系下的三维点 相对于世界坐标系下的三维点雅克比
    // 误差对于三维点的雅可比
    _jacobianOplusXi = -proj_jac * Rcw;

    Eigen::Matrix<double,3,6> SE3deriv;
    double x = Xb(0);
    double y = Xb(1);
    double z = Xb(2);

    SE3deriv <<  0.0,   z,  -y, 1.0, 0.0, 0.0,
                 -z , 0.0,   x, 0.0, 1.0, 0.0,
                  y ,  -x, 0.0, 0.0, 0.0, 1.0;

    // proj_jac 像素相对于相机坐标系下的三维点的雅克比
    // Rcb 相机坐标系下的三维点 相对于imu坐标系下的三维点雅克比
    // SE3deriv imu坐标系下的三维点 相对于 imu rt的雅克比
    // Rwb.t() * (Pw - twb) = Pb
    // 求Pb对于Rwb与twb的雅克比，使用扰动的方式 Rwb -> Rwb * EXP(φ)  twb -> twb + Rwb * δt
    // 先带入Rwb (Rwb * EXP(φ)).t() * (Pw - twb) - Rwb.t() * (Pw - twb)
    // 打开后算得imu坐标系下的三维点 相对于 imu r的雅克比为Pb^
    // 同理带入t可得imu坐标系下的三维点 相对于 imu t的雅克比为 -I
    // 差了个负号，因为与proj_jac负号相抵，因此是正确的
    // 误差对于R、t的雅可比
    _jacobianOplusXj = proj_jac * Rcb * SE3deriv; // symbol different becasue of update mode
}

        之前我们求R、t都是以相机坐标系为基准，但我们在update更新的时候是IMU的位姿,算的时候要把IMU相对于相机的矩阵乘上。

2.3 单目仅优化位姿一元边

2.3.1 边的定义

/** 
 * @brief 单目纯位姿一元边
 */
 // 2 表示误差的维度 VertexPose表示误差的类型以及节点类型（节点就是待优化变量）
class EdgeMonoOnlyPose : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EdgeMonoOnlyPose(const Eigen::Vector3f &Xw_, int cam_idx_ = 0) : Xw(Xw_.cast<double>()),
                                                                        cam_idx(cam_idx_) {}

    virtual bool read(std::istream &is) { return false; }
    virtual bool write(std::ostream &os) const { return false; }

    // 计算重投影误差
    void computeError()
    {
        const VertexPose *VPose = static_cast<const VertexPose *>(_vertices[0]);
        const Eigen::Vector2d obs(_measurement);
        _error = obs - VPose->estimate().Project(Xw, cam_idx);
    }

    virtual void linearizeOplus();

    bool isDepthPositive()
    {
        const VertexPose *VPose = static_cast<const VertexPose *>(_vertices[0]);
        return VPose->estimate().isDepthPositive(Xw, cam_idx);
    }

    Eigen::Matrix<double, 6, 6> GetHessian()
    {
        linearizeOplus();
        return _jacobianOplusXi.transpose() * information() * _jacobianOplusXi;
    }

public:
    const Eigen::Vector3d Xw;
    const int cam_idx;
};

        和上面类似。

2.3.2 雅可比的计算

        和8节类似，更简单，只需要推导R，t的雅可比即可。

/** 
 * @brief 单目视觉纯位姿边计算雅克比
 * _jacobianOplusXi对应着_vertices[0] 也就是误差对于位姿的雅克比
 */
void EdgeMonoOnlyPose::linearizeOplus()
{
    const VertexPose* VPose = static_cast<const VertexPose*>(_vertices[0]);

    const Eigen::Matrix3d &Rcw = VPose->estimate().Rcw[cam_idx];
    const Eigen::Vector3d &tcw = VPose->estimate().tcw[cam_idx];
    const Eigen::Vector3d Xc = Rcw*Xw + tcw;
    const Eigen::Vector3d Xb = VPose->estimate().Rbc[cam_idx]*Xc+VPose->estimate().tbc[cam_idx];
    const Eigen::Matrix3d &Rcb = VPose->estimate().Rcb[cam_idx];

    Eigen::Matrix<double,2,3> proj_jac = VPose->estimate().pCamera[cam_idx]->projectJac(Xc);

    Eigen::Matrix<double,3,6> SE3deriv;
    double x = Xb(0);
    double y = Xb(1);
    double z = Xb(2);
    SE3deriv << 0.0,   z,   -y, 1.0, 0.0, 0.0,
                -z , 0.0,    x, 0.0, 1.0, 0.0,
                 y , - x,  0.0, 0.0, 0.0, 1.0;
    _jacobianOplusXi = proj_jac * Rcb * SE3deriv; // symbol different becasue of update mode
}

2.4 双目位姿三维点二元边EdgeStereo

2.4.1 边的定义

        二元边，相比于单目，它的误差是三维的，形式为Eigen::Vector3d。

        节点类型与单目类似：

        g2o::VertexSBAPointXYZ 三维在世界坐标系下的地图点。       

        VertexPose 世界坐标系到相机坐标系下的R，t。

/** 
 * @brief 双目位姿三维点二元边
 */

// 二元边误差是三维的 误差形式为Eigen::Vector3d 内部节点类型为g2o::VertexSBAPointXYZ（三维点）, VertexPose（位姿）
class EdgeStereo : public g2o::BaseBinaryEdge<3, Eigen::Vector3d, g2o::VertexSBAPointXYZ, VertexPose>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    // 代码中都是0
    EdgeStereo(int cam_idx_ = 0) : cam_idx(cam_idx_) {}

    virtual bool read(std::istream &is) { return false; }
    virtual bool write(std::ostream &os) const { return false; }

    // 误差计算方法
    void computeError()
    {
	// 将点和pose提取出来
        const g2o::VertexSBAPointXYZ *VPoint = static_cast<const g2o::VertexSBAPointXYZ *>(_vertices[0]);
        const VertexPose *VPose = static_cast<const VertexPose *>(_vertices[1]);
        // 观测坐标obs 前两维是这个点在左相机下的相机坐标 第三维为在右相机下的u值
        const Eigen::Vector3d obs(_measurement);
        _error = obs - VPose->estimate().ProjectStereo(VPoint->estimate(), cam_idx);
    }

    virtual void linearizeOplus();

    Eigen::Matrix<double, 3, 9> GetJacobian()
    {
        linearizeOplus();
        Eigen::Matrix<double, 3, 9> J;
        J.block<3, 3>(0, 0) = _jacobianOplusXi;
        J.block<3, 6>(0, 3) = _jacobianOplusXj;
        return J;
    }

    Eigen::Matrix<double, 9, 9> GetHessian()
    {
        linearizeOplus();
        Eigen::Matrix<double, 3, 9> J;
        J.block<3, 3>(0, 0) = _jacobianOplusXi;
        J.block<3, 6>(0, 3) = _jacobianOplusXj;
        return J.transpose() * information() * J;
    }

public:
    const int cam_idx;
};

2.4.2  误差计算方法computeError

    // 误差计算方法
    void computeError()
    {
	// 将点和pose提取出来
        const g2o::VertexSBAPointXYZ *VPoint = static_cast<const g2o::VertexSBAPointXYZ *>(_vertices[0]);
        const VertexPose *VPose = static_cast<const VertexPose *>(_vertices[1]);
        // 观测坐标obs 前两维是这个点在左相机下的相机坐标 第三维为在右相机下的u值
        const Eigen::Vector3d obs(_measurement);
        _error = obs - VPose->estimate().ProjectStereo(VPoint->estimate(), cam_idx);
    }

        它的误差是三维的就是在这里体现出来的。

/** 
 * @brief 双目投影，都是0
 * @return u v u`
 */
Eigen::Vector3d ImuCamPose::ProjectStereo(const Eigen::Vector3d &Xw, int cam_idx) const
{
    // cam_idx认为=0就行 因为都是以左目为主的
    // 将点投影到左目的坐标系下
    Eigen::Vector3d Pc = Rcw[cam_idx] * Xw + tcw[cam_idx];
    Eigen::Vector3d pc;
    double invZ = 1/Pc(2);
    // 获得图像像素坐标,将相机坐标系下的点反投影到图像像素坐标上，pc 是一个3*1的向量 前两维弄成这样
    pc.head(2) = pCamera[cam_idx]->project(Pc);
    // 第三维定义为右目相机坐标的u值
    pc(2) = pc(0) - bf*invZ;
    return pc;
}

        将点投影到了左目的相机坐标系下，前两维是投影的像素坐标，第三维定义成了在右目下像素坐标系的u值。

        输出三维像素误差。

2.5 双目纯位姿一元边 EdgeStereoOnlyPose

2.5.1 边的定义

/** 
 * @brief 双目纯位姿一元边，只优化Rt不优化三维点
 */
class EdgeStereoOnlyPose : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, VertexPose>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EdgeStereoOnlyPose(const Eigen::Vector3f &Xw_, int cam_idx_ = 0) : Xw(Xw_.cast<double>()), cam_idx(cam_idx_) {}

    virtual bool read(std::istream &is) { return false; }
    virtual bool write(std::ostream &os) const { return false; }

    void computeError()
    {
        const VertexPose *VPose = static_cast<const VertexPose *>(_vertices[0]);
        const Eigen::Vector3d obs(_measurement);
        _error = obs - VPose->estimate().ProjectStereo(Xw, cam_idx);
    }

    virtual void linearizeOplus();

    Eigen::Matrix<double, 6, 6> GetHessian()
    {
        linearizeOplus();
        return _jacobianOplusXi.transpose() * information() * _jacobianOplusXi;
    }

public:
    const Eigen::Vector3d Xw; // 3D point coordinates
    const int cam_idx;
};

        和上面的一样，不做阐述。

2.6 惯性边EdgeInertial（误差为残差）

2.6.1 边的定义

        三个旋转三个速度三个位移，九元边。误差类型为Vector9d。

/** 
 * @brief 惯性边（误差为残差 3旋转 3速度 3位移）
 */
class EdgeInertial : public g2o::BaseMultiEdge<9, Vector9d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EdgeInertial(IMU::Preintegrated *pInt);

    virtual bool read(std::istream &is) { return false; }
    virtual bool write(std::ostream &os) const { return false; }

    void computeError();
    virtual void linearizeOplus();

    // 关于pose1与2 的旋转平移速度，以及之间的偏置的信息矩阵
    Eigen::Matrix<double, 24, 24> GetHessian()
    {
        linearizeOplus();
        Eigen::Matrix<double, 9, 24> J;
        J.block<9, 6>(0, 0) = _jacobianOplus[0];
        J.block<9, 3>(0, 6) = _jacobianOplus[1];
        J.block<9, 3>(0, 9) = _jacobianOplus[2];
        J.block<9, 3>(0, 12) = _jacobianOplus[3];
        J.block<9, 6>(0, 15) = _jacobianOplus[4];
        J.block<9, 3>(0, 21) = _jacobianOplus[5];
        return J.transpose() * information() * J;
    }

    // 没用
    Eigen::Matrix<double, 18, 18> GetHessianNoPose1()
    {
        linearizeOplus();
        Eigen::Matrix<double, 9, 18> J;
        J.block<9, 3>(0, 0) = _jacobianOplus[1];
        J.block<9, 3>(0, 3) = _jacobianOplus[2];
        J.block<9, 3>(0, 6) = _jacobianOplus[3];
        J.block<9, 6>(0, 9) = _jacobianOplus[4];
        J.block<9, 3>(0, 15) = _jacobianOplus[5];
        return J.transpose() * information() * J;
    }

    // 关于pose2 的旋转平移信息矩阵
    Eigen::Matrix<double, 9, 9> GetHessian2()
    {
        linearizeOplus();
        Eigen::Matrix<double, 9, 9> J;
        J.block<9, 6>(0, 0) = _jacobianOplus[4];
        J.block<9, 3>(0, 6) = _jacobianOplus[5];
        return J.transpose() * information() * J;
    }

    // 预积分中对应的状态对偏置的雅可比
    const Eigen::Matrix3d JRg, JVg, JPg;
    const Eigen::Matrix3d JVa, JPa;

    IMU::Preintegrated *mpInt;  // 预积分
    const double dt;  // 预积分时间
    Eigen::Vector3d g;  // 0, 0, -IMU::GRAVITY_VALUE 重力
};

        边里面存放了预积分中对应的状态对偏置的雅可比、预积分、预积分时间、重力都会芳进来。

2.6.2 惯性边的构造函数

/** 
 * @brief 局部地图中imu的局部地图优化（此时已经初始化完毕不需要再优化重力方向与尺度）
 * 构造函数的输入是一段时间的预积分,在构造函数就将预积分进行了拆分,将一些雅可比矩阵和dT取出来了
 * @param pInt 预积分相关内容
 */
EdgeInertial::EdgeInertial(IMU::Preintegrated *pInt):JRg(pInt->JRg.cast<double>()),
    JVg(pInt->JVg.cast<double>()), JPg(pInt->JPg.cast<double>()), JVa(pInt->JVa.cast<double>()),
    JPa(pInt->JPa.cast<double>()), mpInt(pInt), dt(pInt->dT)
{
    // 准备工作，把预积分类里面的值先取出来，包含信息的是两帧之间n多个imu信息预积分来的
    // This edge links 6 vertices
    // 6元边 预积分的两张图像(两个帧),第i,j时刻的旋转平移、速度。两个时间的偏置对应的边
    resize(6);
    // 1. 定义重力
    g << 0, 0, -IMU::GRAVITY_VALUE;

    // 2. 读取协方差矩阵的前9*9部分的逆矩阵，该部分表示的是预积分测量噪声的协方差矩阵
    Matrix9d Info = pInt->C.block<9,9>(0,0).cast<double>().inverse();
    // 3. 强制让其成为对角矩阵
    Info = (Info+Info.transpose())/2;
    // 4. 让特征值很小的时候置为0，再重新计算信息矩阵（暂不知这么操作的目的是什么，先搞清楚操作流程吧）
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double,9,9> > es(Info);
    Eigen::Matrix<double,9,1> eigs = es.eigenvalues();
    for(int i=0;i<9;i++)
        if(eigs[i]<1e-12)
            eigs[i]=0;
    // asDiagonal 生成对角矩阵
    Info = es.eigenvectors()*eigs.asDiagonal()*es.eigenvectors().transpose();
    setInformation(Info);
}

2.6.3 误差/残差计算 computeError

        在预积分我们推到过，如果忘记了请查看我的博客：ORBSLAM3 IMU理论推导部分https://blog.csdn.net/qq_41694024/article/details/129374894        我们拿位置残差举例：

 ep = VP1->estimate().Rwb.transpose()*(VP2->estimate().twb - VP1->estimate().twb
- VV1->estimate()*dt - g*dt*dt/2) - dP;

         VP1->estimate()是第时刻左面的帧，将其Rwb^T取出，对应着。

         VP2->estimate().twb - VP1->estimate().twb对应着。......

        其实都是一一对应的，可以慢慢验证。

// 计算雅克比矩阵
void EdgeInertial::linearizeOplus()
{
    const VertexPose* VP1 = static_cast<const VertexPose*>(_vertices[0]);
    const VertexVelocity* VV1= static_cast<const VertexVelocity*>(_vertices[1]);
    const VertexGyroBias* VG1= static_cast<const VertexGyroBias*>(_vertices[2]);
    const VertexAccBias* VA1= static_cast<const VertexAccBias*>(_vertices[3]);
    const VertexPose* VP2 = static_cast<const VertexPose*>(_vertices[4]);
    const VertexVelocity* VV2= static_cast<const VertexVelocity*>(_vertices[5]);
    const IMU::Bias b1(VA1->estimate()[0],VA1->estimate()[1],VA1->estimate()[2],VG1->estimate()[0],VG1->estimate()[1],VG1->estimate()[2]);
    const IMU::Bias db = mpInt->GetDeltaBias(b1);
    Eigen::Vector3d dbg;
    dbg << db.bwx, db.bwy, db.bwz;

    const Eigen::Matrix3d Rwb1 = VP1->estimate().Rwb;  // Ri
    const Eigen::Matrix3d Rbw1 = Rwb1.transpose();     // Ri.t()
    const Eigen::Matrix3d Rwb2 = VP2->estimate().Rwb;  // Rj

    const Eigen::Matrix3d dR = mpInt->GetDeltaRotation(b1).cast<double>();
    const Eigen::Matrix3d eR = dR.transpose()*Rbw1*Rwb2;        // r△Rij
    const Eigen::Vector3d er = LogSO3(eR);                      // r△φij
    const Eigen::Matrix3d invJr = InverseRightJacobianSO3(er);  // Jr^-1(log(△Rij))

    // 就很神奇，_jacobianOplus个数等于边的个数，里面的大小等于观测值维度（也就是残差）× 每个节点待优化值的维度
    // Jacobians wrt Pose 1
    // _jacobianOplus[0] 9*6矩阵 总体来说就是三个残差分别对pose1的旋转与平移（p）求导
    _jacobianOplus[0].setZero();
    // rotation
    // (0,0)起点的3*3块表示旋转残差对pose1的旋转求导
    _jacobianOplus[0].block<3,3>(0,0) = -invJr*Rwb2.transpose()*Rwb1; // OK
    // (3,0)起点的3*3块表示速度残差对pose1的旋转求导
    _jacobianOplus[0].block<3,3>(3,0) = Sophus::SO3d::hat(Rbw1*(VV2->estimate() - VV1->estimate() - g*dt)); // OK
    // (6,0)起点的3*3块表示位置残差对pose1的旋转求导
    _jacobianOplus[0].block<3,3>(6,0) = Sophus::SO3d::hat(Rbw1*(VP2->estimate().twb - VP1->estimate().twb
                                                   - VV1->estimate()*dt - 0.5*g*dt*dt)); // OK
    // translation
    // (6,3)起点的3*3块表示位置残差对pose1的位置求导
    _jacobianOplus[0].block<3,3>(6,3) = -Eigen::Matrix3d::Identity(); // OK

    // Jacobians wrt Velocity 1
    // _jacobianOplus[1] 9*3矩阵 总体来说就是三个残差分别对pose1的速度求导
    _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] 9*3矩阵 总体来说就是三个残差分别对陀螺仪偏置的速度求导
    _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] 9*3矩阵 总体来说就是三个残差分别对加速度计偏置的速度求导
    _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] 9*6矩阵 总体来说就是三个残差分别对pose2的旋转与平移（p）求导
    _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] 9*3矩阵 总体来说就是三个残差分别对pose2的速度求导
    _jacobianOplus[5].setZero();
    _jacobianOplus[5].block<3,3>(3,0) = Rbw1; // OK
}

2.6.4 雅可比计算

// 计算雅克比矩阵
void EdgeInertial::linearizeOplus()
{
    const VertexPose* VP1 = static_cast<const VertexPose*>(_vertices[0]);
    const VertexVelocity* VV1= static_cast<const VertexVelocity*>(_vertices[1]);
    const VertexGyroBias* VG1= static_cast<const VertexGyroBias*>(_vertices[2]);
    const VertexAccBias* VA1= static_cast<const VertexAccBias*>(_vertices[3]);
    const VertexPose* VP2 = static_cast<const VertexPose*>(_vertices[4]);
    const VertexVelocity* VV2= static_cast<const VertexVelocity*>(_vertices[5]);
    const IMU::Bias b1(VA1->estimate()[0],VA1->estimate()[1],VA1->estimate()[2],VG1->estimate()[0],VG1->estimate()[1],VG1->estimate()[2]);
    const IMU::Bias db = mpInt->GetDeltaBias(b1);
    Eigen::Vector3d dbg;
    dbg << db.bwx, db.bwy, db.bwz;

    const Eigen::Matrix3d Rwb1 = VP1->estimate().Rwb;  // Ri
    const Eigen::Matrix3d Rbw1 = Rwb1.transpose();     // Ri.t()
    const Eigen::Matrix3d Rwb2 = VP2->estimate().Rwb;  // Rj

    const Eigen::Matrix3d dR = mpInt->GetDeltaRotation(b1).cast<double>();
    const Eigen::Matrix3d eR = dR.transpose()*Rbw1*Rwb2;        // r△Rij
    const Eigen::Vector3d er = LogSO3(eR);                      // r△φij
    const Eigen::Matrix3d invJr = InverseRightJacobianSO3(er);  // Jr^-1(log(△Rij))

    // 就很神奇，_jacobianOplus个数等于边的个数，里面的大小等于观测值维度（也就是残差）× 每个节点待优化值的维度
    // Jacobians wrt Pose 1
    // _jacobianOplus[0] 9*6矩阵 总体来说就是三个残差分别对pose1的旋转与平移（p）求导
    _jacobianOplus[0].setZero();
    // rotation
    // (0,0)起点的3*3块表示旋转残差对pose1的旋转求导
    _jacobianOplus[0].block<3,3>(0,0) = -invJr*Rwb2.transpose()*Rwb1; // OK
    // (3,0)起点的3*3块表示速度残差对pose1的旋转求导
    _jacobianOplus[0].block<3,3>(3,0) = Sophus::SO3d::hat(Rbw1*(VV2->estimate() - VV1->estimate() - g*dt)); // OK
    // (6,0)起点的3*3块表示位置残差对pose1的旋转求导
    _jacobianOplus[0].block<3,3>(6,0) = Sophus::SO3d::hat(Rbw1*(VP2->estimate().twb - VP1->estimate().twb
                                                   - VV1->estimate()*dt - 0.5*g*dt*dt)); // OK
    // translation
    // (6,3)起点的3*3块表示位置残差对pose1的位置求导
    _jacobianOplus[0].block<3,3>(6,3) = -Eigen::Matrix3d::Identity(); // OK

    // Jacobians wrt Velocity 1
    // _jacobianOplus[1] 9*3矩阵 总体来说就是三个残差分别对pose1的速度求导
    _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] 9*3矩阵 总体来说就是三个残差分别对陀螺仪偏置的速度求导
    _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] 9*3矩阵 总体来说就是三个残差分别对加速度计偏置的速度求导
    _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] 9*6矩阵 总体来说就是三个残差分别对pose2的旋转与平移（p）求导
    _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] 9*3矩阵 总体来说就是三个残差分别对pose2的速度求导
    _jacobianOplus[5].setZero();
    _jacobianOplus[5].block<3,3>(3,0) = Rbw1; // OK
}

        这与我们推的IMU一样，不再赘述。

2.7 初始化惯性边（误差为残差）EdgeInertialGS

2.7.1 边的定义

// Edge inertial whre gravity is included as optimizable variable and it is not supposed to be pointing in -z axis, as well as scale
/** 
 * @brief 初始化惯性边（误差为残差）
 */
class EdgeInertialGS : public g2o::BaseMultiEdge<9, Vector9d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    // EdgeInertialGS(IMU::Preintegrated* pInt);
    EdgeInertialGS(IMU::Preintegrated *pInt);

    virtual bool read(std::istream &is) { return false; }
    virtual bool write(std::ostream &os) const { return false; }

    void computeError();
    virtual void linearizeOplus();

    const Eigen::Matrix3d JRg, JVg, JPg;
    const Eigen::Matrix3d JVa, JPa;
    IMU::Preintegrated *mpInt;
    const double dt;
    Eigen::Vector3d g, gI;

    // 关于pose1与2 的旋转平移速度，以及之间的偏置，还有重力方向与尺度的信息矩阵
    Eigen::Matrix<double, 27, 27> GetHessian()
    {
        linearizeOplus();
        Eigen::Matrix<double, 9, 27> J;
        J.block<9, 6>(0, 0) = _jacobianOplus[0];
        J.block<9, 3>(0, 6) = _jacobianOplus[1];
        J.block<9, 3>(0, 9) = _jacobianOplus[2];
        J.block<9, 3>(0, 12) = _jacobianOplus[3];
        J.block<9, 6>(0, 15) = _jacobianOplus[4];
        J.block<9, 3>(0, 21) = _jacobianOplus[5];
        J.block<9, 2>(0, 24) = _jacobianOplus[6];
        J.block<9, 1>(0, 26) = _jacobianOplus[7];
        return J.transpose() * information() * J;
    }

    // 与上面摆放不同
    Eigen::Matrix<double, 27, 27> GetHessian2()
    {
        linearizeOplus();
        Eigen::Matrix<double, 9, 27> J;
        J.block<9, 3>(0, 0) = _jacobianOplus[2];
        J.block<9, 3>(0, 3) = _jacobianOplus[3];
        J.block<9, 2>(0, 6) = _jacobianOplus[6];
        J.block<9, 1>(0, 8) = _jacobianOplus[7];
        J.block<9, 3>(0, 9) = _jacobianOplus[1];
        J.block<9, 3>(0, 12) = _jacobianOplus[5];
        J.block<9, 6>(0, 15) = _jacobianOplus[0];
        J.block<9, 6>(0, 21) = _jacobianOplus[4];
        return J.transpose() * information() * J;
    }

    // 关于偏置，重力方向与尺度的信息矩阵
    Eigen::Matrix<double, 9, 9> GetHessian3()
    {
        linearizeOplus();
        Eigen::Matrix<double, 9, 9> J;
        J.block<9, 3>(0, 0) = _jacobianOplus[2];
        J.block<9, 3>(0, 3) = _jacobianOplus[3];
        J.block<9, 2>(0, 6) = _jacobianOplus[6];
        J.block<9, 1>(0, 8) = _jacobianOplus[7];
        return J.transpose() * information() * J;
    }

    // 下面的没有用到，其实也是获取状态的信息矩阵
    Eigen::Matrix<double, 1, 1> GetHessianScale()
    {
        linearizeOplus();
        Eigen::Matrix<double, 9, 1> J = _jacobianOplus[7];
        return J.transpose() * information() * J;
    }

    Eigen::Matrix<double, 3, 3> GetHessianBiasGyro()
    {
        linearizeOplus();
        Eigen::Matrix<double, 9, 3> J = _jacobianOplus[2];
        return J.transpose() * information() * J;
    }

    Eigen::Matrix<double, 3, 3> GetHessianBiasAcc()
    {
        linearizeOplus();
        Eigen::Matrix<double, 9, 3> J = _jacobianOplus[3];
        return J.transpose() * information() * J;
    }

    Eigen::Matrix<double, 2, 2> GetHessianGDir()
    {
        linearizeOplus();
        Eigen::Matrix<double, 9, 2> J = _jacobianOplus[6];
        return J.transpose() * information() * J;
    }
};

        初始化之前使用，我们需要优化重力方向和尺度。和上面差不多。

2.7.2 构造函数

// localmapping中imu初始化所用的边，除了正常的几个优化变量外还优化了重力方向与尺度
EdgeInertialGS::EdgeInertialGS(IMU::Preintegrated *pInt):JRg(pInt->JRg.cast<double>()),
    JVg(pInt->JVg.cast<double>()), JPg(pInt->JPg.cast<double>()), JVa(pInt->JVa.cast<double>()),
    JPa(pInt->JPa.cast<double>()), mpInt(pInt), dt(pInt->dT)
{
    // 准备工作，把预积分类里面的值先取出来，包含信息的是两帧之间n多个imu信息预积分来的
    // This edge links 8 vertices
    // 8元边 多了重力方向和尺度
    resize(8);
    // 1. 定义重力
    gI << 0, 0, -IMU::GRAVITY_VALUE;

    // 2. 读取协方差矩阵的前9*9部分的逆矩阵，该部分表示的是预积分测量噪声的协方差矩阵
    Matrix9d Info = pInt->C.block<9,9>(0,0).cast<double>().inverse();
    // 3. 强制让其成为对角矩阵
    Info = (Info+Info.transpose())/2;
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double,9,9> > es(Info);
    // 4. 让特征值很小的时候置为0，再重新计算信息矩阵（暂不知这么操作的目的是什么，先搞清楚操作流程吧）
    Eigen::Matrix<double,9,1> eigs = es.eigenvalues();
    for(int i=0;i<9;i++)
        if(eigs[i]<1e-12)
            eigs[i]=0;
    // asDiagonal 生成对角矩阵
    Info = es.eigenvectors()*eigs.asDiagonal()*es.eigenvectors().transpose();
    setInformation(Info);
}

        8元边 多了重力方向和尺度

2.7.3 误差/残差计算

// 计算误差
void EdgeInertialGS::computeError()
{
    // TODO Maybe Reintegrate inertial measurments when difference between linearization point and current estimate is too big
    const VertexPose* VP1 = static_cast<const VertexPose*>(_vertices[0]);
    const VertexVelocity* VV1= static_cast<const VertexVelocity*>(_vertices[1]);
    const VertexGyroBias* VG= static_cast<const VertexGyroBias*>(_vertices[2]);
    const VertexAccBias* VA= static_cast<const VertexAccBias*>(_vertices[3]);
    const VertexPose* VP2 = static_cast<const VertexPose*>(_vertices[4]);
    const VertexVelocity* VV2 = static_cast<const VertexVelocity*>(_vertices[5]);
    const VertexGDir* VGDir = static_cast<const VertexGDir*>(_vertices[6]);
    const VertexScale* VS = static_cast<const VertexScale*>(_vertices[7]);
    const IMU::Bias b(VA->estimate()[0],VA->estimate()[1],VA->estimate()[2],VG->estimate()[0],VG->estimate()[1],VG->estimate()[2]);
    g = VGDir->estimate().Rwg*gI;
    const double s = VS->estimate();
    const Eigen::Matrix3d dR = mpInt->GetDeltaRotation(b).cast<double>();
    const Eigen::Vector3d dV = mpInt->GetDeltaVelocity(b).cast<double>();
    const Eigen::Vector3d dP = mpInt->GetDeltaPosition(b).cast<double>();

    // 计算残差。广义上讲都是真实值 = 残差 + imu，旋转为imu*残差=真实值
    // dR.transpose() 为imu预积分的值，VP1->estimate().Rwb.transpose() * VP2->estimate().Rwb 为相机的Rwc在乘上相机与imu的标定外参矩阵
    const Eigen::Vector3d er = LogSO3(dR.transpose()*VP1->estimate().Rwb.transpose()*VP2->estimate().Rwb);
    const Eigen::Vector3d ev = VP1->estimate().Rwb.transpose()*(s*(VV2->estimate() - VV1->estimate()) - g*dt) - dV;
    const Eigen::Vector3d ep = VP1->estimate().Rwb.transpose()*(s*(VP2->estimate().twb - VP1->estimate().twb - VV1->estimate()*dt) - g*dt*dt/2) - dP;

    _error << er, ev, ep;
}

        对应论文。

2.7.4 雅可比计算

// 计算雅克比矩阵
void EdgeInertialGS::linearizeOplus()
{
    const VertexPose* VP1 = static_cast<const VertexPose*>(_vertices[0]);
    const VertexVelocity* VV1= static_cast<const VertexVelocity*>(_vertices[1]);
    const VertexGyroBias* VG= static_cast<const VertexGyroBias*>(_vertices[2]);
    const VertexAccBias* VA= static_cast<const VertexAccBias*>(_vertices[3]);
    const VertexPose* VP2 = static_cast<const VertexPose*>(_vertices[4]);
    const VertexVelocity* VV2 = static_cast<const VertexVelocity*>(_vertices[5]);
    const VertexGDir* VGDir = static_cast<const VertexGDir*>(_vertices[6]);
    const VertexScale* VS = static_cast<const VertexScale*>(_vertices[7]);
    // 1. 获取偏置的该变量，因为要对这个东西求导
    const IMU::Bias b(VA->estimate()[0],VA->estimate()[1],VA->estimate()[2],VG->estimate()[0],VG->estimate()[1],VG->estimate()[2]);
    const IMU::Bias db = mpInt->GetDeltaBias(b);

    // 陀螺仪的偏置改变量
    Eigen::Vector3d dbg;
    dbg << db.bwx, db.bwy, db.bwz;

    const Eigen::Matrix3d Rwb1 = VP1->estimate().Rwb;   // Ri
    const Eigen::Matrix3d Rbw1 = Rwb1.transpose();      // Ri.t()
    const Eigen::Matrix3d Rwb2 = VP2->estimate().Rwb;   // Rj
    const Eigen::Matrix3d Rwg = VGDir->estimate().Rwg;  // Rwg
    Eigen::MatrixXd Gm = Eigen::MatrixXd::Zero(3,2);
    Gm(0,1) = -IMU::GRAVITY_VALUE;
    Gm(1,0) = IMU::GRAVITY_VALUE;
    const double s = VS->estimate();
    const Eigen::MatrixXd dGdTheta = Rwg*Gm;
    const Eigen::Matrix3d dR = mpInt->GetDeltaRotation(b).cast<double>();
    const Eigen::Matrix3d eR = dR.transpose()*Rbw1*Rwb2;        // r△Rij
    const Eigen::Vector3d er = LogSO3(eR);                      // r△φij
    const Eigen::Matrix3d invJr = InverseRightJacobianSO3(er);  // Jr^-1(log(△Rij))

    // 就很神奇，_jacobianOplus个数等于边的个数，里面的大小等于观测值维度（也就是残差）× 每个节点待优化值的维度
    // Jacobians wrt Pose 1
    // _jacobianOplus[0] 9*6矩阵 总体来说就是三个残差分别对pose1的旋转与平移（p）求导
    _jacobianOplus[0].setZero();
    // rotation
    // (0,0)起点的3*3块表示旋转残差对pose1的旋转求导
    _jacobianOplus[0].block<3,3>(0,0) = -invJr*Rwb2.transpose()*Rwb1;
    // (3,0)起点的3*3块表示速度残差对pose1的旋转求导
    _jacobianOplus[0].block<3,3>(3,0) = Sophus::SO3d::hat(Rbw1*(s*(VV2->estimate() - VV1->estimate()) - g*dt));
    // (6,0)起点的3*3块表示位置残差对pose1的旋转求导
    _jacobianOplus[0].block<3,3>(6,0) = Sophus::SO3d::hat(Rbw1*(s*(VP2->estimate().twb - VP1->estimate().twb
                                                   - VV1->estimate()*dt) - 0.5*g*dt*dt));
    // translation
    // (6,3)起点的3*3块表示位置残差对pose1的位置求导
    _jacobianOplus[0].block<3,3>(6,3) = Eigen::DiagonalMatrix<double,3>(-s,-s,-s);

    // Jacobians wrt Velocity 1
    // _jacobianOplus[1] 9*3矩阵 总体来说就是三个残差分别对pose1的速度求导
    _jacobianOplus[1].setZero();
    _jacobianOplus[1].block<3,3>(3,0) = -s*Rbw1;
    _jacobianOplus[1].block<3,3>(6,0) = -s*Rbw1*dt;

    // Jacobians wrt Gyro bias
    // _jacobianOplus[2] 9*3矩阵 总体来说就是三个残差分别对陀螺仪偏置的速度求导
    _jacobianOplus[2].setZero();
    _jacobianOplus[2].block<3,3>(0,0) = -invJr*eR.transpose()*RightJacobianSO3(JRg*dbg)*JRg;
    _jacobianOplus[2].block<3,3>(3,0) = -JVg;
    _jacobianOplus[2].block<3,3>(6,0) = -JPg;

    // Jacobians wrt Accelerometer bias
    // _jacobianOplus[3] 9*3矩阵 总体来说就是三个残差分别对加速度计偏置的速度求导
    _jacobianOplus[3].setZero();
    _jacobianOplus[3].block<3,3>(3,0) = -JVa;
    _jacobianOplus[3].block<3,3>(6,0) = -JPa;

    // Jacobians wrt Pose 2
    // _jacobianOplus[3] 9*6矩阵 总体来说就是三个残差分别对pose2的旋转与平移（p）求导
    _jacobianOplus[4].setZero();
    // rotation
    _jacobianOplus[4].block<3,3>(0,0) = invJr;
    // translation
    _jacobianOplus[4].block<3,3>(6,3) = s*Rbw1*Rwb2;

    // Jacobians wrt Velocity 2
    // _jacobianOplus[3] 9*3矩阵 总体来说就是三个残差分别对pose2的速度求导
    _jacobianOplus[5].setZero();
    _jacobianOplus[5].block<3,3>(3,0) = s*Rbw1;

    // Jacobians wrt Gravity direction
    // _jacobianOplus[3] 9*2矩阵 总体来说就是三个残差分别对重力方向求导
    _jacobianOplus[6].setZero();
    _jacobianOplus[6].block<3,2>(3,0) = -Rbw1*dGdTheta*dt;
    _jacobianOplus[6].block<3,2>(6,0) = -0.5*Rbw1*dGdTheta*dt*dt;

    // Jacobians wrt scale factor
    // _jacobianOplus[3] 9*1矩阵 总体来说就是三个残差分别对尺度求导
    _jacobianOplus[7].setZero();
    _jacobianOplus[7].block<3,1>(3,0) = Rbw1*(VV2->estimate()-VV1->estimate()) * s;
    _jacobianOplus[7].block<3,1>(6,0) = Rbw1*(VP2->estimate().twb-VP1->estimate().twb-VV1->estimate()*dt) * s;
}

2.8 陀螺仪偏置的二元边，除了残差及重投影误差外的第三个边，控制偏置变化EdgeGyroRW

2.8.1 边的定义

        二元边，误差形式是三维，误差类型为Vector3d。二元边两个节点都是偏置节点VertexGyroBias。

/** 
 * @brief 陀螺仪偏置的二元边，除了残差及重投影误差外的第三个边，控制偏置变化
 */
class EdgeGyroRW : public g2o::BaseBinaryEdge<3, Eigen::Vector3d, VertexGyroBias, VertexGyroBias>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EdgeGyroRW() {}

    virtual bool read(std::istream &is) { return false; }
    virtual bool write(std::ostream &os) const { return false; }

    // 将两个节点相减
    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();
    }

    virtual void linearizeOplus()
    {
        _jacobianOplusXi = -Eigen::Matrix3d::Identity();
        _jacobianOplusXj.setIdentity();
    }

    Eigen::Matrix<double, 6, 6> GetHessian()
    {
        linearizeOplus();
        Eigen::Matrix<double, 3, 6> J;
        J.block<3, 3>(0, 0) = _jacobianOplusXi;
        J.block<3, 3>(0, 3) = _jacobianOplusXj;
        return J.transpose() * information() * J;
    }

    Eigen::Matrix3d GetHessian2()
    {
        linearizeOplus();
        return _jacobianOplusXj.transpose() * information() * _jacobianOplusXj;
    }
};

2.8.2 误差定义

    // 将两个节点相减
    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();
    }

        emmm，就是把两个节点相减。意思可能是想让两次偏置差别不要很大（尽量保持在0附近）。

2.8.3 雅克比的计算

        对于第一个节点，对error的VG1求导，结果就是-1。换算成矩阵的形式就是-E。

        对于第二个节点，对error的VG2求导，结果就是1。换算成矩阵的形式就是E。

    virtual void linearizeOplus()
    {
        _jacobianOplusXi = -Eigen::Matrix3d::Identity();
        _jacobianOplusXj.setIdentity();
    }

2.9  加速度计偏置的二元边，除了残差及重投影误差外的第三个边，控制偏置变化EdgeAccRW

2.9.1 边的定义

/** 
 * @brief 加速度计偏置的二元边，除了残差及重投影误差外的第三个边，控制偏置变化
 */
class EdgeAccRW : public g2o::BaseBinaryEdge<3, Eigen::Vector3d, VertexAccBias, VertexAccBias>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EdgeAccRW() {}

    virtual bool read(std::istream &is) { return false; }
    virtual bool write(std::ostream &os) const { return false; }

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

    Eigen::Matrix<double, 6, 6> GetHessian()
    {
        linearizeOplus();
        Eigen::Matrix<double, 3, 6> J;
        J.block<3, 3>(0, 0) = _jacobianOplusXi;
        J.block<3, 3>(0, 3) = _jacobianOplusXj;
        return J.transpose() * information() * J;
    }

    Eigen::Matrix3d GetHessian2()
    {
        linearizeOplus();
        return _jacobianOplusXj.transpose() * information() * _jacobianOplusXj;
    }
};

        和陀螺仪的误差计算方式、雅可比矩阵计算方式完全相同。

2.10  先验边EdgePriorPoseImu

2.10.1 先验类ConstraintPoseImu

        前端Tracking的时候用到，边缘化会使用到上一帧的信息，对于上一帧的信息会加一个先验的边来控制上一帧的位姿变化不会超过很大不会飘飞。

/** 
 * @brief 先验类，在前端Tracking的时候 前端Tracking的时候用到，边缘化会使用到上一帧的信息，对于上一帧的信息会加一个先验的边来控制上一帧的位姿变化不会超过很大 不会飘飞
 */
class ConstraintPoseImu
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    // H为信息矩阵 从上文推导得出
    ConstraintPoseImu(
        const Eigen::Matrix3d &Rwb_, const Eigen::Vector3d &twb_, const Eigen::Vector3d &vwb_,
        const Eigen::Vector3d &bg_, const Eigen::Vector3d &ba_, const Matrix15d &H_)
        : Rwb(Rwb_), twb(twb_), vwb(vwb_), bg(bg_), ba(ba_), H(H_)
    {
        H = (H + H) / 2;  // 对称化
        // 令特征值小的位置变为0
        Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 15, 15>> es(H);
        Eigen::Matrix<double, 15, 1> eigs = es.eigenvalues();
        for (int i = 0; i < 15; i++)
            if (eigs[i] < 1e-12)
                eigs[i] = 0;
        H = es.eigenvectors() * eigs.asDiagonal() * es.eigenvectors().transpose();
    }

    Eigen::Matrix3d Rwb;
    Eigen::Vector3d twb;
    Eigen::Vector3d vwb;
    Eigen::Vector3d bg;
    Eigen::Vector3d ba;
    Matrix15d H;
};

        在构造函数的时候将某一帧的信息全部输入过来了（位姿、偏置、信息矩阵）

        将信息矩阵对称了一下，令特征值小的位置变为0并重新求了下H。

2.10.2 边的定义

/** 
 * @brief 先验边，前端Tracking的时候用到，边缘化会使用到上一帧的信息，对于上一帧的信息会加一个先验的边来控制上一帧的位姿变化不会超过很大 不会飘飞
 */
class EdgePriorPoseImu : public g2o::BaseMultiEdge<15, Vector15d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    EdgePriorPoseImu(ConstraintPoseImu *c);

    virtual bool read(std::istream &is) { return false; }
    virtual bool write(std::ostream &os) const { return false; }

    void computeError();
    virtual void linearizeOplus();

    Eigen::Matrix<double, 15, 15> GetHessian()
    {
        linearizeOplus();
        Eigen::Matrix<double, 15, 15> J;
        J.block<15, 6>(0, 0) = _jacobianOplus[0];
        J.block<15, 3>(0, 6) = _jacobianOplus[1];
        J.block<15, 3>(0, 9) = _jacobianOplus[2];
        J.block<15, 3>(0, 12) = _jacobianOplus[3];
        return J.transpose() * information() * J;
    }

    Eigen::Matrix<double, 9, 9> GetHessianNoPose()
    {
        linearizeOplus();
        Eigen::Matrix<double, 15, 9> J;
        J.block<15, 3>(0, 0) = _jacobianOplus[1];
        J.block<15, 3>(0, 3) = _jacobianOplus[2];
        J.block<15, 3>(0, 6) = _jacobianOplus[3];
        return J.transpose() * information() * J;
    }
    Eigen::Matrix3d Rwb;
    Eigen::Vector3d twb, vwb;
    Eigen::Vector3d bg, ba;
};

2.11 根据给定值的加速度计先验边，目的是把优化量维持在先验附近EdgePriorAcc

2.11.1 边的定义

        这里是一元边，误差计算方式和先前略有不同。

// Priors for biases
/** 
 * @brief 根据给定值的加速度计先验边，目的是把优化量维持在先验附近
 */
class EdgePriorAcc : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, VertexAccBias>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EdgePriorAcc(const Eigen::Vector3f &bprior_) : bprior(bprior_.cast<double>()) {}

    virtual bool read(std::istream &is) { return false; }
    virtual bool write(std::ostream &os) const { return false; }

    void computeError()
    {
        const VertexAccBias *VA = static_cast<const VertexAccBias *>(_vertices[0]);
        _error = bprior - VA->estimate();
    }
    virtual void linearizeOplus();

    Eigen::Matrix<double, 3, 3> GetHessian()
    {
        linearizeOplus();
        return _jacobianOplusXi.transpose() * information() * _jacobianOplusXi;
    }

    const Eigen::Vector3d bprior;
};

        输入-节点的数。文章来源地址https://www.toymoban.com/news/detail-772612.html

到了这里，关于ORBSLAM3 --- 优化（一）：g2o优化中的节点与边的定义-G2oTypes.h、G2oTypes.cc解析的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！