紧耦合后端非线性优化-局部优化（Marginalization）

妖狐艹你老母 2022-09-06 10:14 170阅读 0赞

# 0.前言 #

局部优化作为VSLAM当中常用的策略，其作用相当于激光SLAM中的局部地图的ICP or NDT优化（[scan2localmap][]）。如下图所示，在VIO当中，随着时间的推移，路标特征点(landmark)和相机的位姿pose越来越多，BA的计算量随着变量的增加而增加，即使BA的H矩阵是稀疏的，也吃不消。因此，我们要限制优化变量的多少，不能只一味的增加待优化的变量到BA里，而应该去掉一些变量。那么如何丢变量就成了一个很重要的问题！在vins mono和vins fusion中都存在`void Estimator::optimization()`函数用来用**滑窗执行Ceres优化，边缘化，更新滑窗内图像帧的状态（位姿、速度、偏置、外参、逆深度、相机与IMU时差）**，但是这个之前在[VINS-FUSION 前端后端代码全详解][VINS-FUSION]忽略了这一重要的部分。所以在写完回环后发现，关注了全局优化的同时，局部优化却没有关注。为此写着一篇文章来分析vins中的局部优化部分  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2xvdmVseV95b3NoaW5v_size_16_color_FFFFFF_t_70]

# 1. VIO中的状态向量与代价函数 #

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2xvdmVseV95b3NoaW5v_size_16_color_FFFFFF_t_70 1]  
视觉惯性BA：这三项依次为边缘化的先验信息、IMU的测量残差、视觉的重投影误差  
BA优化模型分为三部分：

1、**视觉误差函数部分**（滑动窗口中特征点在相机下视觉重投影残差）

2、**IMU残差部分**（滑动窗口中相邻帧间的IMU产生）

3、**Marg边缘化残差部分**（滑动窗口中去掉位姿和特征点约束）代码中使用Google开源的Ceres solver解决。

# 2. 视觉约束 #

这部分要拟合的目标可以通过重投影误差约束，求解的是对同一个路标点的观测值和估计值之间的误差，注意是在**归一化平面**上表示。  
![在这里插入图片描述][e00b923d1e174f748aa3368e172151ba.png]  
当某路标点在第i帧观测到并进行初始化操作得到路标点逆深度，当其在第j帧也被观测到时，**估计其在第j帧中的坐标为**：  
![在这里插入图片描述][f5be14e837864eaba350db7849ef064d.png]  
此时的视觉残差为：（左侧为根据i帧反推估计的位置，右侧为观测值）  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2xvdmVseV95b3NoaW5v_size_16_color_FFFFFF_t_70 2]  
**逆深度作为参数原因：**  
1）观测到的特征点深度可能非常大，难以进行优化；  
2）可以减少实际优化的参数变量；  
3）逆深度更加服从高斯分布。这里特征点的逆深度在第i帧初始化操作中得到。

我们可以看到在vins fusion中

//左相机在i时刻和j时刻分别观测到路标点
        ProjectionTwoFrameOneCamFactor *f_td = new ProjectionTwoFrameOneCamFactor(pts_i, pts_j, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocity,
                                                         it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td);
        problem.AddResidualBlock(f_td, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]);
    }
    
    if(STEREO && it_per_frame.is_stereo)
    {
                        
        Vector3d pts_j_right = it_per_frame.pointRight;
        if(imu_i != imu_j)
        {
           //左相机在i时刻、右相机在j时刻分别观测到路标点
            ProjectionTwoFrameTwoCamFactor *f = new ProjectionTwoFrameTwoCamFactor(pts_i, pts_j_right, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocityRight,
                                                         it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td);
            problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Ex_Pose[1], para_Feature[feature_index], para_Td[0]);
        }
        else
        {
           //左相机和右相机在i时刻分别观测到路标点
            ProjectionOneFrameTwoCamFactor *f = new ProjectionOneFrameTwoCamFactor(pts_i, pts_j_right, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocityRight,
                                                         it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td);
            problem.AddResidualBlock(f, loss_function, para_Ex_Pose[0], para_Ex_Pose[1], para_Feature[feature_index], para_Td[0]);
        }
       
    }

目前我们将以`ProjectionTwoFrameOneCamFactor`为例子介绍特征点逆深度求解

/**
     * 迭代优化每一步调用，计算变量x在当前状态下的残差，以及残差对变量的Jacobian，用于计算delta_x，更新变量x
     * 优化重投影误差
     * 1、优化变量：前一帧位姿，当前帧位姿，相机与IMU外参，特征点逆深度，相机与IMU时差。对匹配点构建重投影误差
     * 2、计算残差对优化变量的Jacobian
     * @param parameters    优化变量的值
     * @param residuals     output 残差
     * @param jacobians     output 残差对优化变量的Jacobian
    */
    bool ProjectionTwoFrameOneCamFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
    {
        
        // 优化变量：前一帧位姿7，当前位姿7，相机与IMU外参7，特征点逆深度1，相机与IMU时差1
        TicToc tic_toc;
        Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);
        Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);
    
        Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]);
        Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);
    
        Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]);
        Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);
    
        double inv_dep_i = parameters[3][0];
    
        double td = parameters[4][0];
    
        Eigen::Vector3d pts_i_td, pts_j_td;
        // 匹配点ij, 归一化相机点时差校正，对应到采集时刻的位置，因为IMU数据是对应图像采集时刻的
        pts_i_td = pts_i - (td - td_i) * velocity_i;
        pts_j_td = pts_j - (td - td_j) * velocity_j;
        // 转到相机系
        Eigen::Vector3d pts_camera_i = pts_i_td / inv_dep_i;
        // 转到IMU系
        Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;
        // 转到世界系
        Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;
        // 转到j的IMU系
        Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);
        // 转到j的相机系
        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_td.normalized());
    #else
        // 计算归一化相机平面上的两点误差
        double dep_j = pts_camera_j.z();
        residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>();
    #endif
    
        residual = sqrt_info * residual;
    
        if (jacobians)
        {
        
            Eigen::Matrix3d Ri = Qi.toRotationMatrix();
            Eigen::Matrix3d Rj = Qj.toRotationMatrix();
            Eigen::Matrix3d ric = qic.toRotationMatrix();
            Eigen::Matrix<double, 2, 3> reduce(2, 3);
    #ifdef UNIT_SPHERE_ERROR
            double norm = pts_camera_j.norm();
            Eigen::Matrix3d norm_jaco;
            double x1, x2, x3;
            x1 = pts_camera_j(0);
            x2 = pts_camera_j(1);
            x3 = pts_camera_j(2);
            norm_jaco << 1.0 / norm - x1 * x1 / pow(norm, 3), - x1 * x2 / pow(norm, 3),            - x1 * x3 / pow(norm, 3),
                         - x1 * x2 / pow(norm, 3),            1.0 / norm - x2 * x2 / pow(norm, 3), - x2 * x3 / pow(norm, 3),
                         - x1 * x3 / pow(norm, 3),            - x2 * x3 / pow(norm, 3),            1.0 / norm - x3 * x3 / pow(norm, 3);
            reduce = tangent_base * norm_jaco;
    #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;
    
            // 下面是计算残差对优化变量的Jacobian
            if (jacobians[0])
            {
        
                Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_pose_i(jacobians[0]);
    
                Eigen::Matrix<double, 3, 6> jaco_i;
                jaco_i.leftCols<3>() = ric.transpose() * Rj.transpose();
                jaco_i.rightCols<3>() = ric.transpose() * Rj.transpose() * Ri * -Utility::skewSymmetric(pts_imu_i);
    
                jacobian_pose_i.leftCols<6>() = reduce * jaco_i;
                jacobian_pose_i.rightCols<1>().setZero();
            }
    
            if (jacobians[1])
            {
        
                Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_pose_j(jacobians[1]);
    
                Eigen::Matrix<double, 3, 6> jaco_j;
                jaco_j.leftCols<3>() = ric.transpose() * -Rj.transpose();
                jaco_j.rightCols<3>() = ric.transpose() * Utility::skewSymmetric(pts_imu_j);
    
                jacobian_pose_j.leftCols<6>() = reduce * jaco_j;
                jacobian_pose_j.rightCols<1>().setZero();
            }
            if (jacobians[2])
            {
        
                Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_ex_pose(jacobians[2]);
                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;
                jacobian_ex_pose.rightCols<1>().setZero();
            }
            if (jacobians[3])
            {
        
                Eigen::Map<Eigen::Vector2d> jacobian_feature(jacobians[3]);
                jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i_td * -1.0 / (inv_dep_i * inv_dep_i);
            }
            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);
            }
        }
        sum_t += tic_toc.toc();
    
        return true;
    }

## …详情请参照[古月居][Link 1] ##

[scan2localmap]: https://www.guyuehome.com/34082
[VINS-FUSION]: https://www.guyuehome.com/34507
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2xvdmVseV95b3NoaW5v_size_16_color_FFFFFF_t_70]: /images/20220829/5ac45353982944a1826b39648111eb9d.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2xvdmVseV95b3NoaW5v_size_16_color_FFFFFF_t_70 1]: /images/20220829/eb12e8e8f4c3432f855aab60c91b4f88.png
[e00b923d1e174f748aa3368e172151ba.png]: /images/20220829/092c03d290214b0a9b66790a93f50242.png
[f5be14e837864eaba350db7849ef064d.png]: /images/20220829/ce59d02e69314e2e9d2856b1bb7f49e0.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2xvdmVseV95b3NoaW5v_size_16_color_FFFFFF_t_70 2]: /images/20220829/07dd4bce73bc405ea01e27ca3e395c1d.png
[Link 1]: https://www.guyuehome.com/35372