VINS-MONO代码解读4----vins_estimator(后端求解部分)

读书健身敲代码

已于 2024-08-21 12:01:06 修改

阅读量898

点赞数 10

文章标签： SLAM VIO

于 2023-12-05 10:57:39 首次发布

本文链接：https://blog.csdn.net/qq_37746927/article/details/134800523

版权

0. 后端整体pipeline

在这里插入图片描述

1. solveOdometry()后端求解 (重点)

完成初始化之后，正式开始后端求解。

包含

在world系下重新三角化
执行非线性优化

重点介绍介绍各个ResidualBolck的构建以及marginalization

需要注意，在ceres中只接受residual，不接受信息矩阵，所以我们要把信息矩阵做分解，构建新的Jacobian和residual，如下所示：
在这里插入图片描述
视觉部分：
以视觉标定误差为1.5pixel设定视觉的信息矩(这里还不太了解为何这样能直接构建 $\sqrt{信息矩阵}$ ，按理来说构建的应该是协方差矩阵，然后LLT分解得 $\sqrt{信息矩阵}$ )：

//视觉测量残差的协方差矩阵
void Estimator::setParameter()
{
    for (int i = 0; i < NUM_OF_CAM; i++)
    {
        tic[i] = TIC[i];
        ric[i] = RIC[i];
    }
    f_manager.setRic(ric);
    //这里假设标定相机内参时的重投影误差△u=1.5 pixel，(Sigma)^(-1)=(1.5/f * I(2x2))^(-1) = (f/1.5 * I(2x2))
    ProjectionFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
    ProjectionTdFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
    td = TD;
}

在Evaluate()中计算Jacobian和residual时使用信息矩阵构建新的Jacobian和residual

ProjectionFactor::Evaluate() {
...

#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;
    
...

#else
        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);
#endif
        reduce = sqrt_info * reduce;

...
//jacobians[0]
jacobian_pose_i.leftCols<6>() = reduce * jaco_i;
//jacobians[1]
jacobian_pose_j.leftCols<6>() = reduce * jaco_j;
//jacobians[2]
jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex;
//jacobians[3]
jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i * -1.0 / (inv_dep_i * inv_dep_i);
...
}

IMU部分：
使用预积分的协方差构建信息矩阵，LLT分解之后得信息矩阵，进而构建Jacobian和residual

bool IMUFactor::Evaluate(){
...

        // 构建IMU残差residual
        Eigen::Map<Eigen::Matrix<double, 15, 1>> residual(residuals);
        residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi,
                                            Pj, Qj, Vj, Baj, Bgj);

        // LLT分解，residual 还需乘以信息矩阵的sqrt_info
        // 因为优化函数其实是d=r^T P^-1 r ，P表示协方差，而ceres只接受最小二乘优化
        // 因此需要把P^-1做LLT分解，使d=(L^T r)^T (L^T r) = r'^T r
        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;

...
        jacobian_pose_i = sqrt_info * jacobian_pose_i;
        jacobian_speedbias_i = sqrt_info * jacobian_speedbias_i;
        jacobian_pose_j = sqrt_info * jacobian_pose_j;
        jacobian_speedbias_j = sqrt_info * jacobian_speedbias_j;
        
...
}

1.1 marg先验部分

此部分和1.7小结均属于marg部分，在下一篇博客中单独讲解。

1.2 视觉部分

1.2.1 处理rolling shutter camera的时间

在这里插入图片描述
为什么要计算时间同步误差，这里看不懂，先挖个坑。

    //特征匀速模型补偿特征的坐标
    //pts_i_td 处理时间同步误差和Rolling shutter时间后，角点在归一化平面的坐标。
    //TR / ROW * row_i就是相机 rolling 到这一行时所用的时间（认为rolling shutter camera同一行像素的曝光时间相同）
    Eigen::Vector3d pts_i_td, pts_j_td;
    pts_i_td = pts_i - (td - td_i + TR / ROW * row_i) * velocity_i;
    pts_j_td = pts_j - (td - td_j + TR / ROW * row_j) * velocity_j;

1.2.2 视觉Jacobian推导

本部分具体推导可参考博客，写的很详细。

在这里插入图片描述

具体记录一下自己手推过程中遇到的问题：

在推导 $q_{wb_j}$ 的Jacobian时，就把 $p_{wb_j}$ 当做常量，不过最好的方法是不把 $f_{c_j}$ 展开，把 $R_{wb_j}^T$ 提出来，这样就不会

因为 $R_{wb_j}^Tp_{wb_j}$ 而担心后面的 $R_{wb_j}^T$ 了。

在这里插入图片描述

关于时间戳 $t_d$ 的Jacobian看不懂，可以看看VIO课程第8章作业，而且此部分有之前沈老师组的另一项工作，后续可具体参考。

//对time offset求导(2x1)
if (jacobians[4])
{
    Eigen::Map<Eigen::Vector2d> jacobian_td(jacobians[4]);
    jacobian_td = reduce * ric.transpose() * Rj.transpose() * Ri * ric * velocity_i / inv_dep_i * -1.0  +
                  sqrt_info * velocity_j.head(2);  //后面的项看不懂，为啥要管第j帧的速度？
}

1.2.2 协方差 setParameter()

在这里插入图片描述

//视觉测量残差的协方差矩阵
void Estimator::setParameter()
{
    for (int i = 0; i < NUM_OF_CAM; i++)
    {
        tic[i] = TIC[i];
        ric[i] = RIC[i];
    }
    f_manager.setRic(ric);
    //这里假设标定相机内参时的重投影误差△u=1.5 pixel，(Sigma)^(-1)=(1.5/f * I(2x2))^(-1) = (f/1.5 * I(2x2))
    ProjectionFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
    ProjectionTdFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
    td = TD;
}

1.3 IMU部分

1.3.1 Jacobian推导

具体推导可参考VIO课程或者可参考博客

在这里插入图片描述

IMU部分所有的Jacobian：

要区分清楚残差 $r$ 对待优化量的求导(Jacobian) 和 预计分量对上一时刻预计分量的求导(Jacobian)，前者是为了优化状态量，而后者是为了递推求解下一时刻的预计分量，二者求导的目的不一样。
注意，预积分传递是 $i, ... k, k + 1, ... j$ 期间的每相邻两个时刻间传递，而不是 $w or l d$ -> $j$ 时刻传递，所以之前推导的F只能在与 $i$ -> $j$ 有关的量使用，与world系有关的视为常量，而前面对 $i$ -> $j$ 时刻的预积分量进行了一阶泰勒近似，所以可以直接对用 $i$ 时刻的bias的导数，即 $F$

在这里插入图片描述

注意四元数乘法符号 $\otimes$ 与四元数左右乘算子，在四元数转为旋转矩阵之后需要变为四元数算子。
取虚部的操作直接提出一个 $\begin{bmatrix}0\\I\end{bmatrix}$ 的矩阵，0表示实部为0， $I$ 表示虚部，这个矩阵就表示取虚部。
最后是向量对常量求导，与向量保持相同维度即可。

在这里插入图片描述

此处Jacobian涉及到了使用短时IMU积分来补偿预计分量，对应论文IV.A节，在Jacobian阶段使用预积分补偿，与论文中说的“只用于KF selection，不参与rotation calculation”有些出入。

//使用短时gyro的数据对预积分q进行补偿，对应论文IV.A节
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>();
...
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>();

1.3.2 IMU整体Jacobian

可视化看的更直观：
在这里插入图片描述

1.3.3 协方差

在这里插入图片描述

在midPointIntegration中

//jacobian传递
jacobian = F * jacobian;
//协方差传递
covariance = F * covariance * F.transpose() + V * noise * V.transpose();

1.4 重定位部分

对应崔华坤PDF中7.2节，需结合pose graph来理解，主要是msg的理解和构建residualBlock的选点策略，具体见pose_graph博客。
在这里插入图片描述

1.6 数据格式转换，重定位后处理 double2vector()

防止优化结果在零空间变化，固定第一帧的位姿。(暂时还不知道为什么只固定yaw而不管tilt)

包括求取yaw方向旋转rot_diff，这里对快速重定位的输出进行了处理，后续会用：

loop closure frame v优化前后的 $T_{prev\_now}$ ：drift_correct_r, drift_correct_t
loop closure frame v相对于local loop帧的relative pose(暂时还不知道在哪用)： relo_relative_t, relo_relative_q, relo_relative_yaw

rot_diff指 $R_{before\_after}).yaw$ 对应的旋转矩阵，这里用于在优化后保持第[0]帧的yaw不变(补偿 Rs[0],Ps[0],Vs[0])，防止零空间发生变化，后面的RPV跟着[0]相应改变。

其他的fix第[0]帧的方法参考之前的博客第4节

// 优化一次之后，求出优化前后的第一帧的T，用于后面作用到所有的轨迹上去
// 同时这里为防止优化结果往零空间变化，会根据优化前后第一帧的位姿差进行修正。
void Estimator::double2vector()
{
    //窗口第一帧优化前的位姿
    Vector3d origin_R0 = Utility::R2ypr(Rs[0]);//R[0]
    Vector3d origin_P0 = Ps[0];

    if (failure_occur)
    {
        origin_R0 = Utility::R2ypr(last_R0);
        origin_P0 = last_P0;
        failure_occur = 0;
    }
    //窗口第一帧优化后的位姿 q(wxyz)
    Vector3d origin_R00 = Utility::R2ypr(Quaterniond(para_Pose[0][6],
                                                      para_Pose[0][3],
                                                      para_Pose[0][4],
                                                      para_Pose[0][5]).toRotationMatrix());
    double y_diff = origin_R0.x() - origin_R00.x();//(R_before_after).yaw（指向被减，变换到before）
    //TODO：了解欧拉角奇异点
    Matrix3d rot_diff = Utility::ypr2R(Vector3d(y_diff, 0, 0));
    if (abs(abs(origin_R0.y()) - 90) < 1.0 || abs(abs(origin_R00.y()) - 90) < 1.0)
    {
        ROS_DEBUG("euler singular point!");
        rot_diff = Rs[0] * Quaterniond(para_Pose[0][6],
                                       para_Pose[0][3],
                                       para_Pose[0][4],
                                       para_Pose[0][5]).toRotationMatrix().transpose();
    }
    // 根据位姿差做修正，即保证第一帧优化前后位姿不变(似乎只是yaw不变，那tilt呢？)
    for (int i = 0; i <= WINDOW_SIZE; i++)
    {

        Rs[i] = rot_diff * Quaterniond(para_Pose[i][6], para_Pose[i][3], para_Pose[i][4], para_Pose[i][5]).normalized().toRotationMatrix();
        
        Ps[i] = rot_diff * Vector3d(para_Pose[i][0] - para_Pose[0][0],
                                para_Pose[i][1] - para_Pose[0][1],
                                para_Pose[i][2] - para_Pose[0][2]) + origin_P0;

        Vs[i] = rot_diff * Vector3d(para_SpeedBias[i][0],
                                    para_SpeedBias[i][1],
                                    para_SpeedBias[i][2]);

        Bas[i] = Vector3d(para_SpeedBias[i][3],
                          para_SpeedBias[i][4],
                          para_SpeedBias[i][5]);

        Bgs[i] = Vector3d(para_SpeedBias[i][6],
                          para_SpeedBias[i][7],
                          para_SpeedBias[i][8]);
    }

    //外参
    for (int i = 0; i < NUM_OF_CAM; i++)
    {
        tic[i] = Vector3d(para_Ex_Pose[i][0],
                          para_Ex_Pose[i][1],
                          para_Ex_Pose[i][2]);
        ric[i] = Quaterniond(para_Ex_Pose[i][6],
                             para_Ex_Pose[i][3],
                             para_Ex_Pose[i][4],
                             para_Ex_Pose[i][5]).toRotationMatrix();
    }

    //转为逆深度，并置flag
    VectorXd dep = f_manager.getDepthVector();
    for (int i = 0; i < f_manager.getFeatureCount(); i++)
        dep(i) = para_Feature[i][0];
    f_manager.setDepth(dep);
    //time offset
    if (ESTIMATE_TD)
        td = para_Td[0][0];

    // relative info between two loop frame
    if(relocalization_info)
    { 
        Matrix3d relo_r;
        Vector3d relo_t;
        relo_r = rot_diff * Quaterniond(relo_Pose[6], relo_Pose[3], relo_Pose[4], relo_Pose[5]).normalized().toRotationMatrix();
        relo_t = rot_diff * Vector3d(relo_Pose[0] - para_Pose[0][0],
                                     relo_Pose[1] - para_Pose[0][1],
                                     relo_Pose[2] - para_Pose[0][2]) + origin_P0;//保证第[0]帧不变之后，+origin_P0转为世界系下的t
        //优化前后loop closure frame v 的drift T_prev_now
        double drift_correct_yaw;
        //loop closure frame优化前后的yaw drift, prev-now = (R_prev_now).yaw
        drift_correct_yaw = Utility::R2ypr(prev_relo_r).x() - Utility::R2ypr(relo_r).x();
        //r变化R_prev_now
        drift_correct_r = Utility::ypr2R(Vector3d(drift_correct_yaw, 0, 0));
        //t变化t_now_prev = w_t_prev - Rprev_now * w_t_now
        drift_correct_t = prev_relo_t - drift_correct_r * relo_t;

        //loop closure frame v与relo frame local(窗口内的local loop帧)的relative pose：T_local_v，可能用于快速重定位
        //Rw_local^(-1)*( (w)tw_v - (w)t_w_local ) = R_local_w * (w)t_local_v = (local)t_local_v（表示Tlocal_v的平移部分，指向是从local指向v）
        relo_relative_t = relo_r.transpose() * (Ps[relo_frame_local_index] - relo_t);
        //Rw_local^(-1)*Rw_v = Rlocal_v
        relo_relative_q = relo_r.transpose() * Rs[relo_frame_local_index];
        //TODO：(R_local_v).yaw
        relo_relative_yaw = Utility::normalizeAngle(Utility::R2ypr(Rs[relo_frame_local_index]).x() - Utility::R2ypr(relo_r).x());
        //cout << "vins relo " << endl;
        //cout << "vins relative_t " << relo_relative_t.transpose() << endl;
        //cout << "vins relative_yaw " <<relo_relative_yaw << endl;
        relocalization_info = 0;    
    }
}

1.7 marg处理(难点)

marg部分比较复杂，另开一帖单独讲解，详见：marg的博客

2. failureDetection()检测初始化和求解状态

不赘述。

3. clearState() setParameter()状态管理

不赘述。

4. slideWindow()滑动窗口

在marg的博客中详细讲解。

5. removeFailures()去除

去掉未三角化出正深度的landmark。

6. 发布各种结果

pubOdometry()
1. pub里程计输出(相关msg：nav_msgs::Odometry)
2. 发布带时间戳的pose (相关msg：geometry_msgs::PoseStamped)
3. 使用快速重定位时计算出的被loop帧的现在与之前的drift，把现在的拉回到之前的上面去，并发布发布重定位的path(是(R|t)，相关msg：nav_msgs::Path)
4. estimator输出的PQV输出到文件

其他的不再赘述，简单注释：

 //发布后端优化后的1.PQV  2.带时间戳的pose(PQ)  3.重定位后的pose
 pubOdometry(estimator, header);//"odometry"
 //历史KF的t（应该是那10个大红点）
 pubKeyPoses(estimator, header);//"key_poses"
 //使用外参转为camera pose
 pubCameraPose(estimator, header);//"camera_pose"
 //发布现有的和即将被marg掉的landmark
 pubPointCloud(estimator, header);//"history_cloud"
 //发布transform Twb和Tic
 pubTF(estimator, header);//"extrinsic" Tbc
 //当2nd时KF时，发布2nd的Twb 和 2nd中的feature的2D,归一化3D，camera 3D坐标
 pubKeyframe(estimator);//"keyframe_point"、"keyframe_pose"
 //在double2vector()中求得relo_relative_t，relo_relative_q,代表T_local_v，后面用于重定位
 if (relo_msg != NULL)
     pubRelocalization(estimator);//"relo_relative_pose"

7. 更新状态 update()

主要是更新IMU参数[P,Q,V,ba,bg,a,g]
在前面取出本次measurement之后，由于IMU频率较高，buf中可能还剩有一些imu数据，需要对这些数据先predit进行预积分，结果保存在全局变量tmp_P, tmp_Q, tmp_V, tmp_Ba, tmp_Bg, acc_0, gyr_0（预积分初值）中。

下一篇重点讲解Marginalization。