VINS-MONO学习笔记 [基于滑动窗口的非线性优化]

1. 代码

• 代码位置：
  vins_estimator->src->estimator.cpp

if(result)//初始化成功
{
   //先进行一次滑动窗口非线性优化，得到当前帧与第一帧的位姿
   solver_flag = NON_LINEAR;
   // step4:非线性优化求解vio
   solveOdometry();
   // step5：滑动窗口
   slideWindow();
   // step6: 移除无效地图点
   f_manager.removeFailures();
   ROS_INFO("Initialization finish!");
   last_R = Rs[WINDOW_SIZE];
   last_P = Ps[WINDOW_SIZE];
   last_R0 = Rs[0];
   last_P0 = Ps[0];
 }

//三角化求解所有特征点的深度，并进行非线性优化
void Estimator::solveOdometry()
{
    // 保证滑窗中帧数满了
    if (frame_count < WINDOW_SIZE)
        return;
    // 其次要求初始化完成
    if (solver_flag == NON_LINEAR)
    {
        TicToc t_tri;
        // 先把应该三角化但是没有三角化的特征点三角化
        f_manager.triangulate(Ps, tic, ric);
        ROS_DEBUG("triangulation costs %f", t_tri.toc());
        // 非线性优化 
        optimization();
    }
}

2. ceres解析求导

解析求导相比于自动求导，会使程序的速度加快。特别是对于里程计模块这种对于实时性要求很高的程序而言。

需要手动求出每次优化的残差和雅可比

ceres官网

因为在vins中我们知道参数块的大小,所以我在vins中使用SizeCostFunction

• 第一步是新建一个类继承SizeCostFunction
  ceres::SizeCostFunction<1,1> : 前一个1:残差的维度; 后一个1:参数块的维度,因为这里只有一个参数块,所以只有一个1
• 第二步:定义完类之后需要重载Evaluate虚函数
  double const* const* parameters对应各个参数块.由于各个参数块都定义为double数组,如果有好多个参数块就会被定义为数组的数组,也就是这边的双指针.
• residuals是一个向量,也就是一个一维数组,所以这里定义为double *
• jacobians的大小是残差*参数块总数的矩阵,对于矩阵来讲它也是一个二维数组,所以定义为double * *

const double x = parameters[0][0] // 表示x是第0个参数块的第0个参数

LOSS Function 核函数

http://ceres-solver.org/nnls_modeling.html#instances

3. ceres李代数加法代码实现

// 由于位姿（李群）不满足正常的加法，因此需要自己定义他的加法
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();

// 重新定义LocalParameterization局部参数块，继承以下四个函数
class PoseLocalParameterization : public ceres::LocalParameterization
{
    virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const;
    virtual bool ComputeJacobian(const double *x, double *jacobian) const;
    // 3个平移+4个旋转（四元数）
    virtual int GlobalSize() const { return 7; };
    // 实际的自由度
    virtual int LocalSize() const { return 6; };
};

/**
*  x: 参数
*  delta: 增量
*  x_plus_delta：加法之后的结果
*/
bool PoseLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const
{
    // Eigen::Map 把double类型映射为 Eigen::Vector3d 类型
    Eigen::Map<const Eigen::Vector3d> _p(x);       // 取了x的前三维
    Eigen::Map<const Eigen::Quaterniond> _q(x + 3);// 取了x的第四维开始的四个维度，也就是四元数

    Eigen::Map<const Eigen::Vector3d> dp(delta);

    Eigen::Quaterniond dq = Utility::deltaQ(Eigen::Map<const Eigen::Vector3d>(delta + 3));

    Eigen::Map<Eigen::Vector3d> p(x_plus_delta);
    Eigen::Map<Eigen::Quaterniond> q(x_plus_delta + 3);

    p = _p + dp;
    // .normalized(): 只有单位四元数才能表示旋转
    q = (_q * dq).normalized();

    return true;
}

template <typename Derived>
    static Eigen::Quaternion<typename Derived::Scalar> deltaQ(const Eigen::MatrixBase<Derived> &theta)
    {
        typedef typename Derived::Scalar Scalar_t;

        Eigen::Quaternion<Scalar_t> dq;
        Eigen::Matrix<Scalar_t, 3, 1> half_theta = theta;
        half_theta /= static_cast<Scalar_t>(2.0);
        dq.w() = static_cast<Scalar_t>(1.0);
        dq.x() = half_theta.x();
        dq.y() = half_theta.y();
        dq.z() = half_theta.z();
        return dq;
    }

4. 预积分约束残差计算

预积分约束:

• 如果仅通过IMU积分得到 b k b_k bk​到 b k + 1 b_{k+1} bk+1​的位姿,速度和旋转,那么(25)式是成立的.
• 但实际还会收到最小化重投影误差的约束
• 所以最小化 [ 两帧间位姿(速度/旋转)计算出的增量 - IMU预积分得到的增量 ]，残差如公式（25）下部分 所示。
  

IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]);
problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);

// 计算给定相邻帧状态量的残差
    Eigen::Matrix<double, 15, 1> evaluate(const Eigen::Vector3d &Pi, const Eigen::Quaterniond &Qi, const Eigen::Vector3d &Vi, const Eigen::Vector3d &Bai, const Eigen::Vector3d &Bgi,
                                          const Eigen::Vector3d &Pj, const Eigen::Quaterniond &Qj, const Eigen::Vector3d &Vj, const Eigen::Vector3d &Baj, const Eigen::Vector3d &Bgj)
    {
        Eigen::Matrix<double, 15, 1> residuals;

        Eigen::Matrix3d dp_dba = jacobian.block<3, 3>(O_P, O_BA);
        Eigen::Matrix3d dp_dbg = jacobian.block<3, 3>(O_P, O_BG);

        Eigen::Matrix3d dq_dbg = jacobian.block<3, 3>(O_R, O_BG);

        Eigen::Matrix3d dv_dba = jacobian.block<3, 3>(O_V, O_BA);
        Eigen::Matrix3d dv_dbg = jacobian.block<3, 3>(O_V, O_BG);

        Eigen::Vector3d dba = Bai - linearized_ba;
        Eigen::Vector3d dbg = Bgi - linearized_bg;

        // delta_q,delta_v,delta_p:预积分量
        // corrected_:补偿后的修正量 
        Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg);
        Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg;
        Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg;

        // 公式（25）下半部份
        residuals.block<3, 1>(O_P, 0) = Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt) - corrected_delta_p;
        residuals.block<3, 1>(O_R, 0) = 2 * (corrected_delta_q.inverse() * (Qi.inverse() * Qj)).vec();
        residuals.block<3, 1>(O_V, 0) = Qi.inverse() * (G * sum_dt + Vj - Vi) - corrected_delta_v;
        residuals.block<3, 1>(O_BA, 0) = Baj - Bai;
        residuals.block<3, 1>(O_BG, 0) = Bgj - Bgi;
        return residuals;
    }

因为ceres不像g2o,没有增加信息矩阵的接口.
所以不能直接使用 e T ∗ p ∗ e e^T*p*e eT∗p∗e,而是将p分解为 L ∗ L T L*L^T L∗LT,让式子等于 e T ∗ L ∗ L T ∗ e e^T*L*L^T*e eT∗L∗LT∗e. 认为 L T ∗ e L^T*e LT∗e即为新的残差

        // 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();
        // 这就是带有信息矩阵的残差,e = L^T*e
        residual = sqrt_info * residual;

5. 预积分雅克比计算

• 对于雅可比矩阵的第一个分块（jacobians[0] 15X7）

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));

jacobian_pose_i.block<3, 3>(O_V, O_R) = Utility::skewSymmetric(Qi.inverse() * (G * sum_dt + Vj - Vi));

Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
//.bottomRightCorner<3, 3>():取右下3x3的小块
jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Utility::Qleft(Qj.inverse() * Qi) * Utility::Qright(corrected_delta_q)).bottomRightCorner<3, 3>();

整体代码

			// 第i帧的IMU位姿 pbi、qbi
            if (jacobians[0])
            {
                Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMajor>> jacobian_pose_i(jacobians[0]);
                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));

#if 0
            jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Qj.inverse() * Qi).toRotationMatrix();
#else
                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>();
#endif

                jacobian_pose_i.block<3, 3>(O_V, O_R) = Utility::skewSymmetric(Qi.inverse() * (G * sum_dt + Vj - Vi));

                jacobian_pose_i = sqrt_info * jacobian_pose_i;

                if (jacobian_pose_i.maxCoeff() > 1e8 || jacobian_pose_i.minCoeff() < -1e8)
                {
                    ROS_WARN("numerical unstable in preintegration");
                    //std::cout << sqrt_info << std::endl;
                    //ROS_BREAK();
                }
            }

• 对于雅可比矩阵的第三个分块（jacobians[2] 15X7）

• 对于雅可比矩阵的第二个分块（jacobians[1] 15X9）

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_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();

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;

• 对于雅可比矩阵的第四个分块（jacobians[3] 15X9）

6. 视觉重投影约束

• 第i帧是滑窗中第一个观测到这个3D点的帧
• 3D点的状态量维持的是逆深度,而不是传统的三维向量
• 因为在 J T ∗ J ∗ △ x = − J ∗ f ( x ) J^T*J*△x = -J*f(x) JT∗J∗△x=−J∗f(x)中, J T ∗ J J^T*J JT∗J的维度与 △ x △x △x维度相同,而 x x x在VIO问题中维护的是滑窗中的位姿,外参以及(占比重最大的)3D点,对3D点只存储逆深度(1个维度),可以让x的维度相比维护3D点坐标(3个维度)缩小2/3,加快求解速度
• 为什么用逆深度不用深度:对于极远的点(例如天空),正深度为无穷大,逆深度则为一个很小的值.而且不会存在离相机很近的点,不存在逆深度值很大的情况
• 因为不是存储三维坐标,所以逆深度需要和第i帧绑定
• 优化变量:第i帧IMU的位姿,第j帧IMU的位姿,相机到IMU外参,3D点逆深度

λ \lambda λ表示逆深度， 1 / λ 1/\lambda 1/λ表示深度
推导一下:

ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j); // 构造函数就是同一个特征点在不同帧的观测
// 约束的变量是该特征点的第一观测帧以及其他一个观测帧，加上外参和特征点逆深度
problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]);

残差的计算:
如果把上面的一长串带入到残差，需要把一长串分为x，y，z三个部分。这会非常的复杂，所以我们采用链式求导法则。
以x为例，我们把残差关于待优化变量的导数分解为残差对于第j帧相机坐标系下的点的导数 × 第j帧相机坐标系下点对于各个优化变量的导数

		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();
        }

		// 对第j帧的位姿 pbj,qbj
        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();
        }

		// 对相机到IMU的外参 pbc,qbc (qic,tic)
        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 * -1.0 / (inv_dep_i * inv_dep_i);
        }

7. 滑动窗口边缘化

由于需要边缘化，所以我们要手动计算H矩阵，而不能靠ceres帮我们自动计算

https://blog.csdn.net/heyijia0327/article/details/52822104

• p是位姿，m是地图点，连线表示约束

• X p 1 X_{p1} Xp1​与 X m 1 X_{m1} Xm1​的贡献是{p1,p1},{p1,m1},{m1,p1},{m1,m1}; X p 1 X_{p1} Xp1​与 X p 2 X_{p2} Xp2​的贡献是{p1,p1},{p1,p2},{p2,p1},{p2,p2}

• Λ a Λa Λa ={p1,p1}, Λ b Λb Λb = {p1,p2-m6}, Λ c Λc Λc={p2-m6,p2-m6}

上式==