1.兩個相機之間的非線性優(yōu)化

觀測相機方程關(guān)于相機位姿與特征點的雅可比矩陣：

1.1 位姿：

1.2 3D特征點

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

2.vio滑動窗口的BA優(yōu)化

1.相機：

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

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

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

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

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

對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]); //姿態(tài)
    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測量值有關(guān)，將一段時間的imu數(shù)據(jù)直接積分起來就得到了與積分量

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

其中p為位移，v為速度，q為姿態(tài)，b為bias噪聲

2.1 IMU的與積分誤差：

其中，位移，速度，bias噪聲的誤差都是直接相減，第二項是關(guān)于四元數(shù)的旋轉(zhuǎn)誤差，后綴xyz代表取四元數(shù)的虛部（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):
非線性系統(tǒng)(x_{k} = f(x_{k-1}, u_{k-1})) 的狀態(tài)誤差的線性遞推關(guān)系為：

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


其中的系數(shù)為：

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

2.3 IMU相對于優(yōu)化變量的雅可比矩陣：

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

對i時刻的位移：
對i時刻的旋轉(zhuǎn)：
對i時刻的速度：
對i時刻的加速度bias:
IMU角度誤差相對于優(yōu)化變量的雅可比
角度誤差對i時刻的姿態(tài)求導：

其中[]L 和[]R 為四元數(shù)轉(zhuǎn)為左/右旋轉(zhuǎn)矩陣的算子
角度誤差對j時刻姿態(tài)求導

角度誤差對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;

    }


}

總結(jié)

以上是生活随笔為你收集整理的视觉SLAM：VIO的误差和误差雅可比矩阵的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。