視覺SLAM：VIO的誤差和誤差雅可比矩陣

1.兩個相機之間的非線性優化

觀測相機方程關於相機位姿與特征點的雅可比矩陣：

1.1 位姿：

1.2 3D特征點

• fx,fy,fz為相機內參
• X',Y',Z'為3D點在相機坐標系下的坐標
• 該誤差是觀測值減去預測值，反過來，預測值減觀測值時，去掉或加上負號即可
• 姿態定義為先平移后旋轉，如果定義為先旋轉后平移，將該矩陣的前3列與后3列對調即可

2.vio滑動窗口的BA優化

1.相機：

相機誤差仍然為重投影誤差：
優化是在機體坐標系下完成，也就是imu系，所以多了一個相機到機體坐標的外參

根據鏈式法則，可以分兩步走，第一步，誤差對\(f_{cj}\)求導，最后再分別相乘即可

2.1 誤差對\(f_{cj}\)求導：

2.2 \(f_{cj}\)對逆深度的求導：

2.3 \(f_{cj}\)對各時刻狀態量的求導：

• 對i時刻的位移求導：
  
  對i時刻的角度增量求導：
  

• 對j時刻的位移求導;
  
  對j時刻的角度增量求導
  

2.4 \(f_{cj}\)對imu和相機的外參求導：

• 對位移求導：
  
• 對角度增量求導：
  分為兩部分求導： \(f_{cj} = f_{cj}^{1} + f_{cj}^{2}\)
  第一部分：
  
  第二部分：
  
  最后相加即可。

注意：最后別忘了分別乘上誤差對\(f_{cj}\)的求導

2.5 程序示例：

double inv_dep_i = verticies_[0]->Parameters()[0];

    VecX param_i = verticies_[1]->Parameters();  //i時刻位姿
    Qd Qi(param_i[6], param_i[3], param_i[4], param_i[5]); //姿態
    Vec3 Pi = param_i.head<3>();  //位移

    VecX param_j = verticies_[2]->Parameters();  //j時刻位姿
    Qd Qj(param_j[6], param_j[3], param_j[4], param_j[5]);
    Vec3 Pj = param_j.head<3>();

    VecX param_ext = verticies_[3]->Parameters();
    Qd qic(param_ext[6], param_ext[3], param_ext[4], param_ext[5]);
    Vec3 tic = param_ext.head<3>()

    Vec3 pts_camera_i = pts_i_ / inv_dep_i;  
    Vec3 pts_imu_i = qic * pts_camera_i + tic;  
    Vec3 pts_w = Qi * pts_imu_i + Pi;
    Vec3 pts_imu_j = Qj.inverse() * (pts_w - Pj);
    Vec3 pts_camera_j = qic.inverse() * (pts_imu_j - tic);

    double dep_j = pts_camera_j.z();   

    Mat33 Ri = Qi.toRotationMatrix();
    Mat33 Rj = Qj.toRotationMatrix();
    Mat33 ric = qic.toRotationMatrix();
    Mat23 reduce(2, 3);        //誤差對f_cj求導
    reduce << 1. / dep_j, 0, -pts_camera_j(0) / (dep_j * dep_j),
        0, 1. / dep_j, -pts_camera_j(1) / (dep_j * dep_j);
//    reduce = information_ * reduce;

    Eigen::Matrix<double, 2, 6> jacobian_pose_i;
    Eigen::Matrix<double, 3, 6> jaco_i;
    jaco_i.leftCols<3>() = ric.transpose() * Rj.transpose(); //位移求導
    jaco_i.rightCols<3>() = ric.transpose() * Rj.transpose() * Ri * - 
Sophus::SO3d::hat(pts_imu_i); //角度增量求導
    jacobian_pose_i.leftCols<6>() = reduce * jaco_i;

    Eigen::Matrix<double, 2, 6> jacobian_pose_j;
    Eigen::Matrix<double, 3, 6> jaco_j;
    jaco_j.leftCols<3>() = ric.transpose() * -Rj.transpose();
    jaco_j.rightCols<3>() = ric.transpose() * Sophus::SO3d::hat(pts_imu_j);
    jacobian_pose_j.leftCols<6>() = reduce * jaco_j;

    Eigen::Vector2d jacobian_feature;
    //逆深度求導
    jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i_ * -1.0 / (inv_dep_i * inv_dep_i);

    //IMU和相機外參求導
    Eigen::Matrix<double, 2, 6> jacobian_ex_pose;
    Eigen::Matrix<double, 3, 6> jaco_ex;
    jaco_ex.leftCols<3>() = ric.transpose() * (Rj.transpose() * Ri - Eigen::Matrix3d::Identity());
    Eigen::Matrix3d tmp_r = ric.transpose() * Rj.transpose() * Ri * ric;
    jaco_ex.rightCols<3>() = -tmp_r * Utility::skewSymmetric(pts_camera_i) + Utility::skewSymmetric(tmp_r * pts_camera_i) + Utility::skewSymmetric(ric.transpose() * (Rj.transpose() * (Ri * tic + Pi - Pj) - tic));
    
    jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex;

    jacobians_[0] = jacobian_feature;   //2行1列
    jacobians_[1] = jacobian_pose_i;
    jacobians_[2] = jacobian_pose_j;
    jacobians_[3] = jacobian_ex_pose;

2.IMU：

IMU的真實值為 w,a, 測量值為\(w^{~}, a^{~}\)，則有：

其中： b為bias隨機游走誤差，n為白噪聲。

預積分：
預積分僅僅與imu測量值有關，將一段時間的imu數據直接積分起來就得到了與積分量

則j時刻的PVQ積分積分方程為：

其中p為位移，v為速度，q為姿態，b為bias噪聲

2.1 IMU的與積分誤差：

其中，位移，速度，bias噪聲的誤差都是直接相減，第二項是關於四元數的旋轉誤差，后綴xyz代表取四元數的虛部（x, y, z）組成的三維向量。

void EdgeImu::ComputeResidual() {
    VecX param_0 = verticies_[0]->Parameters();
    Qd qi(param_0[6], param_0[3], param_0[4], param_0[5]);
    Vec3 pi = param_0.head<3>();
    SO3d ri(qi);
    SO3d ri_inv = ri.inverse();

    VecX param_1 = verticies_[1]->Parameters();
    Vec3 vi = param_1.head<3>();
    Vec3 bai = param_1.segment(3, 3);
    Vec3 bgi = param_1.tail<3>();

    VecX param_2 = verticies_[2]->Parameters();
    Qd qj(param_2[6], param_2[3], param_2[4], param_2[5]);
    Vec3 pj = param_2.head<3>();

    VecX param_3 = verticies_[3]->Parameters();
    Vec3 vj = param_3.head<3>();
    Vec3 baj = param_3.segment(3, 3);
    Vec3 bgj = param_3.tail<3>();
    SO3d rj(qj);

    double dt = pre_integration_->GetSumDt();
    double dt2 = dt * dt;
    SO3d dr;
    Vec3 dv;
    Vec3 dp;
    pre_integration_->GetDeltaRVP(dr, dv, dp);  //獲取預積分值

    SO3d res_r = dr.inverse() * ri_inv * rj;
    residual_.block<3, 1>(0, 0) = SO3d::log(res_r);
    residual_.block<3, 1>(3, 0) = ri_inv * (vj - vi - gravity_ * dt) - dv;
    residual_.block<3, 1>(6, 0) = ri_inv * (pj - pi - vi * dt - 0.5 * gravity_ * dt2) - dp;
    residual_.block<3, 1>(9, 0) = baj - bai;
    residual_.block<3, 1>(12, 0) = bgj - bgi;
}

2.2 IMU的誤差雅可比矩陣：

基於泰勒展開的誤差傳遞(EKF):
非線性系統\(x_{k} = f(x_{k-1}, u_{k-1})\) 的狀態誤差的線性遞推關系為：

其中，F是狀態量\(x_{k}\)對狀態量\(x_{k-1}\)的雅可比矩陣，G是狀態量\(x_{k}對輸入量\)u_{k-1}$的雅可比矩陣。
IMU的誤差傳遞方程為：


其中的系數為：

• 速度預積分對各狀態量的雅可比，為F的第三行，分別是：位移預積分，旋轉預積分，速度預積分，陀螺儀bias噪聲，加速度bias噪聲
  f33： 速度預積分量對上一時刻速度預積分量的雅可比，為I
  f32： 速度預積分量對角度預積分量的雅可比
  f35： 速度預積分量對k時刻角速度bias噪聲的雅可比
  f22： 前一時刻的旋轉誤差如何影響當前旋轉誤差

2.3 IMU相對於優化變量的雅可比矩陣：

在求解非線性方程式，我們需要知道IMU誤差對兩個關鍵幀i,j的狀態p,q,v,\(b^{a}, b^{g}\)的雅可比

• 對i時刻的位移：
• 對i時刻的旋轉：
• 對i時刻的速度：
• 對i時刻的加速度bias:
  IMU角度誤差相對於優化變量的雅可比
• 角度誤差對i時刻的姿態求導：
  
  其中[]L 和[]R 為四元數轉為左/右旋轉矩陣的算子
• 角度誤差對j時刻姿態求導
  
• 角度誤差對i時刻陀螺儀bias噪聲求導
  

void EdgeImu::ComputeJacobians() {

    VecX param_0 = verticies_[0]->Parameters();
    Qd Qi(param_0[6], param_0[3], param_0[4], param_0[5]);
    Vec3 Pi = param_0.head<3>();

    VecX param_1 = verticies_[1]->Parameters();
    Vec3 Vi = param_1.head<3>();
    Vec3 Bai = param_1.segment(3, 3);
    Vec3 Bgi = param_1.tail<3>();

    VecX param_2 = verticies_[2]->Parameters();
    Qd Qj(param_2[6], param_2[3], param_2[4], param_2[5]);
    Vec3 Pj = param_2.head<3>();

    VecX param_3 = verticies_[3]->Parameters();
    Vec3 Vj = param_3.head<3>();
    Vec3 Baj = param_3.segment(3, 3);
    Vec3 Bgj = param_3.tail<3>();

    double sum_dt = pre_integration_->sum_dt;
    Eigen::Matrix3d dp_dba = pre_integration_->jacobian.template block<3, 3>(O_P, O_BA);
    Eigen::Matrix3d dp_dbg = pre_integration_->jacobian.template block<3, 3>(O_P, O_BG);

    Eigen::Matrix3d dq_dbg = pre_integration_->jacobian.template block<3, 3>(O_R, O_BG);

    Eigen::Matrix3d dv_dba = pre_integration_->jacobian.template block<3, 3>(O_V, O_BA);
    Eigen::Matrix3d dv_dbg = pre_integration_->jacobian.template block<3, 3>(O_V, O_BG);

    if (pre_integration_->jacobian.maxCoeff() > 1e8 || pre_integration_->jacobian.minCoeff() < -1e8)
    {
        // ROS_WARN("numerical unstable in preintegration");
    }

//    if (jacobians[0])
    {
        Eigen::Matrix<double, 15, 6, Eigen::RowMajor> jacobian_pose_i;
        jacobian_pose_i.setZero();

        jacobian_pose_i.block<3, 3>(O_P, O_P) = -Qi.inverse().toRotationMatrix();
        jacobian_pose_i.block<3, 3>(O_P, O_R) = Utility::skewSymmetric(Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt));

        Eigen::Quaterniond corrected_delta_q = pre_integration_->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration_->linearized_bg));
        jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Utility::Qleft(Qj.inverse() * Qi) * Utility::Qright(corrected_delta_q)).bottomRightCorner<3, 3>();
        jacobian_pose_i.block<3, 3>(O_V, O_R) = Utility::skewSymmetric(Qi.inverse() * (G * sum_dt + Vj - Vi));

        if (jacobian_pose_i.maxCoeff() > 1e8 || jacobian_pose_i.minCoeff() < -1e8)
        {
        //     ROS_WARN("numerical unstable in preintegration");
        }
        jacobians_[0] = jacobian_pose_i;
    }
//    if (jacobians[1])
    {
        Eigen::Matrix<double, 15, 9, Eigen::RowMajor> jacobian_speedbias_i;
        jacobian_speedbias_i.setZero();
        jacobian_speedbias_i.block<3, 3>(O_P, O_V - O_V) = -Qi.inverse().toRotationMatrix() * sum_dt;
        jacobian_speedbias_i.block<3, 3>(O_P, O_BA - O_V) = -dp_dba;
        jacobian_speedbias_i.block<3, 3>(O_P, O_BG - O_V) = -dp_dbg;

        jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -Utility::Qleft(Qj.inverse() * Qi * pre_integration_->delta_q).bottomRightCorner<3, 3>() * dq_dbg;

        jacobian_speedbias_i.block<3, 3>(O_V, O_V - O_V) = -Qi.inverse().toRotationMatrix();
        jacobian_speedbias_i.block<3, 3>(O_V, O_BA - O_V) = -dv_dba;
        jacobian_speedbias_i.block<3, 3>(O_V, O_BG - O_V) = -dv_dbg;

        jacobian_speedbias_i.block<3, 3>(O_BA, O_BA - O_V) = -Eigen::Matrix3d::Identity();

        jacobian_speedbias_i.block<3, 3>(O_BG, O_BG - O_V) = -Eigen::Matrix3d::Identity();

        jacobians_[1] = jacobian_speedbias_i;
    }
//    if (jacobians[2])
    {
        Eigen::Matrix<double, 15, 6, Eigen::RowMajor> jacobian_pose_j;
        jacobian_pose_j.setZero();

        jacobian_pose_j.block<3, 3>(O_P, O_P) = Qi.inverse().toRotationMatrix();
        Eigen::Quaterniond corrected_delta_q = pre_integration_->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration_->linearized_bg));
        jacobian_pose_j.block<3, 3>(O_R, O_R) = Utility::Qleft(corrected_delta_q.inverse() * Qi.inverse() * Qj).bottomRightCorner<3, 3>();

        jacobians_[2] = jacobian_pose_j;

    }
//    if (jacobians[3])
    {
        Eigen::Matrix<double, 15, 9, Eigen::RowMajor> jacobian_speedbias_j;
        jacobian_speedbias_j.setZero();

        jacobian_speedbias_j.block<3, 3>(O_V, O_V - O_V) = Qi.inverse().toRotationMatrix();

        jacobian_speedbias_j.block<3, 3>(O_BA, O_BA - O_V) = Eigen::Matrix3d::Identity();

        jacobian_speedbias_j.block<3, 3>(O_BG, O_BG - O_V) = Eigen::Matrix3d::Identity();

        jacobians_[3] = jacobian_speedbias_j;

    }


}