0. 简介

对于ORB-SLAM3而言。如何将代码融入Wheel和GPS是一个挺有意思的事情。通过GPS和Wheel可以非常有效的约束视觉里程计结果。Wheel这块主要就是将速度等信息融合到前端中，类似IMU和视觉帧间的关系。而GPS由于频率不是很高，所以基本是用于全局修正的作用。这部分我们经常使用松耦合的形式，当然也有工作去做了紧耦合相关的工作。

1. Wheel特征添加

这一部分主要的其实就是将wheel的速度特征传入到imu中，以约束两者之间的位移信息。从而提供针对IMU的约束，并可以作为边约束约束G2O的边

1.1 ImuType.h

这一步主要是得是将轮速信息融合到IMU内部去做联合优化，这里面主要用于处理传感器数据融合和运动状态估计。

Point(const cv::Point3f Acc, const cv::Point3f Gyro, const double &timestamp, const double& encoder_v):
            a(Acc.x,Acc.y,Acc.z), w(Gyro.x,Gyro.y,Gyro.z), t(timestamp), encoder_v(encoder_v){}
.........
void IntegrateNewMeasurement(const Eigen::Vector3f &acceleration, const Eigen::Vector3f &angVel, const float &dt,const double &encoder_v );
.........
    Eigen::DiagonalMatrix<float,6> Nga, NgaWalk;
    Eigen::DiagonalMatrix<float,9> Nga_en, NgaWalk_en;

    // Values for the original bias (when integration was computed)
    Bias b;
    Eigen::Matrix3f dR;
    Eigen::Vector3f dV, dP;
    Eigen::Matrix3f Rbo = Eigen::Matrix3f::Identity();   //encoder 和imu的旋转
    Eigen::Vector3d Tbo = {-0.9810000061988831,0,0};   //encoder 和imu的旋转
    Eigen::Matrix3f JRg, JVg, JVa, JPg, JPa;
    Eigen::Vector3f avgA, avgW;
    Eigen::Vector3f encoder_velocity ;
    Eigen::Matrix<float,18,18> jacobian_enc;
    Eigen::Matrix<float,21,21> covariance_enc;

1.2 ImuType.cc

下面IntegrateNewMeasurement 函数中，该系统整合了惯性测量（来自加速度计和陀螺仪）和可选的编码器读数。此函数的主要目的是根据这些测量数据更新内部状态，以跟踪位置、速度和旋转的变化。

/** 
 * @brief 初始化预积分
 * @param b_ 偏置
 */
void Preintegrated::Initialize(const Bias &b_)
{
    dR.setIdentity();
    dV.setZero();
    dP.setZero();
    JRg.setZero();
    JVg.setZero();
    JVa.setZero();
    JPg.setZero();
    JPa.setZero();
    C.setZero();
    Info.setZero();
    db.setZero();
    encoder_velocity.setZero();
    jacobian_enc.setZero();
    covariance_enc.setZero();
    b = b_;
    bu = b_;  // 更新后的偏置
    avgA.setZero();  // 平均加速度
    avgW.setZero();  // 平均角速度
    dT = 0.0f;
    mvMeasurements.clear();  // 存放imu数据及dt
}

/** 
 * @brief 根据新的偏置重新积分mvMeasurements里的数据 Optimizer::InertialOptimization调用
 */ 
void Preintegrated::Reintegrate()
{
    std::unique_lock<std::mutex> lock(mMutex);
    const std::vector<integrable> aux = mvMeasurements;
    //重新进行预积分
    Initialize(bu);
    bool buseencoder;
    buseencoder = true;
    if(buseencoder){
        for (size_t i = 0; i < aux.size(); i++)
            IntegrateNewMeasurement(aux[i].a, aux[i].w, aux[i].t,aux[i].enc);
    }
    else{
    for (size_t i = 0; i < aux.size(); i++)
        IntegrateNewMeasurement(aux[i].a, aux[i].w, aux[i].t);}
}

void Preintegrated::IntegrateNewMeasurement(const Eigen::Vector3f &acceleration, const Eigen::Vector3f &angVel, const float &dt,const double &encoder_v)
    {
        // 保存imu数据，利用中值积分的结果构造一个预积分类保存在mvMeasurements中
        mvMeasurements.push_back(integrable(acceleration, angVel, dt,encoder_v));

        // 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
        // Step 1.构造协方差矩阵
        // 噪声矩阵的传递矩阵，这部分用于计算i到j-1历史噪声或者协方差
        Eigen::Matrix<float, 9, 9> A;
        A.setIdentity();
        // 噪声矩阵的传递矩阵，这部分用于计算j-1新的噪声或协方差，这两个矩阵里面的数都是当前时刻的，计算主要是为了下一时刻使用
        Eigen::Matrix<float, 9, 6> B;
        B.setZero();

        // 考虑偏置后的加速度、角速度
        Eigen::Vector3f acc, accW;
        acc << acceleration(0) - b.bax, acceleration(1) - b.bay, acceleration(2) - b.baz;
        accW << angVel(0) - b.bwx, angVel(1) - b.bwy, angVel(2) - 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)
        // 根据没有更新的dR来更新dP与dV  eq.(38)
        dP = dP + dV * dt + 0.5f * dR * acc * dt * dt;
        dV = dV + dR * acc * dt;
        Eigen::Vector3f encoder_cast = { static_cast<float>(encoder_v),0,0};
        encoder_velocity = encoder_velocity + dR*Rbo* encoder_cast*dt;
        //std::cerr<<"encoder_velocity "<<encoder_velocity;


        // Compute velocity and position parts of matrices A and B (rely on non-updated delta rotation)
        // 根据η_ij = A * η_i,j-1 + B_j-1 * η_j-1中的Ａ矩阵和Ｂ矩阵对速度和位移进行更新
        Eigen::Matrix<float, 3, 3> Wacc = Sophus::SO3f::hat(acc);

        A.block<3, 3>(3, 0) = -dR * dt * Wacc;
        A.block<3, 3>(6, 0) = -0.5f * dR * dt * dt * Wacc;
        A.block<3, 3>(6, 3) = Eigen::DiagonalMatrix<float, 3>(dt, dt, dt);
        B.block<3, 3>(3, 3) = dR * dt;
        B.block<3, 3>(6, 3) = 0.5f * dR * dt * dt;

        // Update position and velocity jacobians wrt bias correction
        // 因为随着时间推移，不可能每次都重新计算雅克比矩阵，所以需要做J(k+1) = j(k) + (~)这类事，分解方式与AB矩阵相同
        // 论文作者对forster论文公式的基础上做了变形，然后递归更新，参见 https://github.com/UZ-SLAMLab/ORB_SLAM3/issues/212
        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
        // Step 2. 构造函数，会根据更新后的bias进行角度积分
        IntegratedRotation dRi(angVel, b, dt);
        // 强行归一化使其符合旋转矩阵的格式
        auto dR_past = dR;
        dR = NormalizeRotation(dR * dRi.deltaR);

        //std::cerr<<"dR_past"<<dR_past<<std::endl;
        //std::cerr<<"dR"<<dR<<std::endl;
        // Compute rotation parts of matrices A and B
        // 补充AB矩阵
        A.block<3, 3>(0, 0) = dRi.deltaR.transpose();
        B.block<3, 3>(0, 0) = dRi.rightJ * dt;

        // 小量delta初始为0，更新后通常也为0，故省略了小量的更新
        // Update covariance
        // Step 3.更新协方差，frost经典预积分论文的第63个公式，推导了噪声（ηa, ηg）对dR dV dP 的影响
        C.block<9, 9>(0, 0) = A * C.block<9, 9>(0, 0) * A.transpose() + B * Nga * B.transpose();  // B矩阵为9*6矩阵 Nga 6*6对角矩阵，3个陀螺仪噪声的平方，3个加速度计噪声的平方
        // 这一部分最开始是0矩阵，随着积分次数增加，每次都加上随机游走，偏置的信息矩阵
        C.block<6, 6>(9, 9) += NgaWalk;

        // Update rotation jacobian wrt bias correction
        // 计算偏置的雅克比矩阵，r对bg的导数，∂ΔRij/∂bg = (ΔRjj-1) * ∂ΔRij-1/∂bg - Jr(j-1)*t
        // 论文作者对forster论文公式的基础上做了变形，然后递归更新，参见 https://github.com/UZ-SLAMLab/ORB_SLAM3/issues/212
        // ? 为什么先更新JPa、JPg、JVa、JVg最后更新JRg? 答：这里必须先更新dRi才能更新到这个值，但是为什么JPg和JVg依赖的上一个JRg值进行更新的？
        JRg = dRi.deltaR.transpose() * JRg - dRi.rightJ * dt;

        // Total integrated time
        // 更新总时间
        dT += dt;

        bool buseencoder;
        buseencoder = true;
// TODO
        if(buseencoder){
            Eigen::MatrixXf F = Eigen::MatrixXf ::Zero(12,12);
//            F.block<3,3>(0,0) = Eigen::Matrix3f::Identity();
//            F.block<3,3>(0,3) = A.block<3, 3>(6, 0);
//            F.block<3,3>(0,6) = Eigen::DiagonalMatrix<float, 3>(dt, dt, dt);
//
//            F.block<3,3>(3,3) = dRi.deltaR.transpose();
//
//            F.block<3,3>(6,3) = A.block<3, 3>(3, 0);
//            F.block<3,3>(6,6) = Eigen::Matrix3f::Identity();

            //轮速

            Eigen::Vector3f encoder_fi = Rbo*encoder_velocity;
            Eigen::Matrix3f R_encoder;
            R_encoder<< 0, -encoder_fi(2), encoder_fi(1),encoder_fi(2), 0, -encoder_fi(0),-encoder_fi(1), encoder_fi(0), 0;
//            F.block<3,3>(9,3) =-dR_past*dt*R_encoder;
//            F.block<3,3>(9,9) = Eigen::Matrix3f::Identity();


            Eigen::MatrixXf V = Eigen::MatrixXf ::Zero(12,9);
//            V.block<3,3>(0,0) = 0.5f * dR_past * dt * dt;
//            V.block<3,3>(3,3) = dRi.rightJ * dt;
//            V.block<3,3>(6,0) = dR_past * dt;
//            V.block<3,3>(9,6) = dR_past*Rbo*dt;

            F.block<9,9>(0,0) = A;
            F.block<3,3>(9,3) = -dR_past*dt*R_encoder;
            F.block<3,3>(9,9) = Eigen::Matrix3f::Identity();

            V.block<9,6>(0,0) = B;
            V.block<3,3>(9,6) = dR_past*Rbo*dt;


            covariance_enc.block<12,12>(0,0) = F*covariance_enc.block<12,12>(0,0)*F.transpose()+V*Nga_en*V.transpose();
            covariance_enc.block<9,9>(12,12) += NgaWalk_en;



        }
    }

1.3 Tracking.cc

…详情请参照古月居