手写VIO后端分析（手写VIO总结五）

手写VIO后端分析（手写VIO总结五）

手写VIO作业总结(一)
手写VIO作业总结(二)
手写VIO作业总结(三)
手写VIO作业总结(四)

1、计算视觉重投影残差理论部分

/*    std::vector<std::shared_ptr<Vertex>> verticies_; // 该边对应的顶点
    VecX residual_;                 // 残差
    std::vector<MatXX> jacobians_;  // 雅可比，每个雅可比维度是 residual x vertex[i]
    MatXX information_;             // 信息矩阵
    VecX observation_;              // 观测信息
    */

void EdgeReprojection::ComputeResidual() {
   
  /**
   * note: Qi,Pi;Qj,Pj不是相机到世界坐标系的变换矩阵，而是imu坐标系到世界坐标系的变换矩阵
   */
    double inv_dep_i = verticies_[0]->Parameters()[0];
    std::cout<<"inv_dep_i: "<<inv_dep_i<<std::endl;
    VecX param_i = verticies_[1]->Parameters();
    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();
    Qd Qj(param_j[6], param_j[3], param_j[4], param_j[5]);
    Vec3 Pj = param_j.head<3>();
/**
 * pts_i_:相机在第i帧图像中的归一化坐标
 * pts_camera_i：相机坐标
 * pts_w:世界坐标
 * pts_imu_i:把相机坐标系下的坐标变换到imu坐标系下
 */
    Vec3 pts_camera_i = pts_i_ / inv_dep_i;
//    std::cout<<"pts_i_: "<<pts_i_<<std::endl;
//    std::cout<<"pts_camera_i: "<<pts_camera_i.transpose()<<std::endl;
    Vec3 pts_imu_i = qic * pts_camera_i + tic;//camrea->imu
    Vec3 pts_w = Qi * pts_imu_i + Pi;//imu->word
    Vec3 pts_imu_j = Qj.inverse() * (pts_w - Pj);//word->imu
    Vec3 pts_camera_j = qic.inverse() * (pts_imu_j - tic);//imu->camera

    double dep_j = pts_camera_j.z();
    residual_ = (pts_camera_j / dep_j).head<2>() - pts_j_.head<2>();   /// J^t * J * delta_x = - J^t * r
//    std::cout<<"residual_: "<<residual_<<std::endl;
//    residual_ = information_ * residual_;   // remove information here, we multi information matrix in problem solver
}

2、残差雅可比的推导

具体代码实现：

void EdgeReprojection::ComputeJacobians() {
   
    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>();

    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