VINS后端优化之imu预积分残差和视觉测量残差

参考资料:

[1]https://blog.csdn.net/q597967420/article/details/76099443

IMU测量残差:

class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9>
//15:残差向量的长度(包括p,q,v,ba,bg)
//7:第1个优化参数的长度(para_Pose[i])
//9:第2个优化参数的长度(para_SpeedBias[i])
//7:第3个优化参数的长度(para_Pose[j])
//9:第4个优化参数的长度(para_SpeedBias[j])
//对于Evaluate函数的输入double const *const *parameters依次对应以上4个参数

  //计算IMU测量残差
 residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi,
                                            Pj, Qj, Vj, Baj, Bgj);

//这里要注意为什么残差要乘以sqrt_info:
//参考:https://blog.csdn.net/q597967420/article/details/76099443
Eigen::Matrix<double, 15, 15> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 15, 15>>(pre_integration->covariance.inverse()).matrixL().transpose();
        //sqrt_info.setIdentity();
        residual = sqrt_info * residual;

视觉测量残差:

class ProjectionFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1>

//2:残差的长度(err_x,err_y)
//7:第一个优化参数的长度para_Pose[imu_i]
//7:第二个优化参数的长度para_Pose[imu_j]
//7:第3个优化参数的长度para_Ex_Pose[0]
//1:第4个优化参数feature_inverse_depth的长度para_Feature[feature_index]

 //！将第i frame下的3D点转到第j frame坐标系下
//！转到归一化平面，得到归一化平面上点P
Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i;
//! P'=Rcb''(Rj''(Ri(Rcb P+t_i)-t_j)-t_cb)
Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;
Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;
Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);
Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);
Eigen::Map<Eigen::Vector2d> residual(residuals);

 //！求取切平面上的误差
#ifdef UNIT_SPHERE_ERROR 
    residual =  tangent_base * (pts_camera_j.normalized() - pts_j.normalized());
    //！求取普通的误差
#else
    double dep_j = pts_camera_j.z();
    residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>();
#endif

residual = sqrt_info * residual;

雅克比矩阵:

//! 因为残差是2， 四个参数块对应的雅克比矩阵分别是：2*7， 2*7, 2*7， 2*1