vins estimator IMU 残差

已于 2022-05-13 16:54:46 修改
阅读量672 收藏
点赞数
分类专栏： # vins 文章标签： 计算机视觉 算法
于 2022-05-07 15:04:08 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/xiaoma_bk/article/details/124629199
版权

文章目录

Imu 预积分

  • imu自身递推即根据 imu的输入，进行位姿的推算(输入：线加速度+角速度)
  • IMU 积分基于起始位姿( b t w P {_{bt}^{w}\textrm{P}} btw​P)进行递推，导致每次更新其位姿，都需重新，因此 IMU积分 改为预积分模式，只与当前观测值有关，需记录：IMU的积分项 α b i b j {\alpha _{b_ib_j}} αbi​bj​​、 β b i b j {\beta _{b_ib_j}} βbi​bj​​、 q b i b j {q _{b_ib_j}} qbi​bj​​
  • IMU的积分项 通过前一时刻的积分项递推而得，且递推中若采用中值积分，则与前一时刻位姿+当前在时刻位姿+角速度偏置+加速度偏置有关，因此可根据其进行方差传递。
  • 对于残差：一段时间内IMU构建的预积分量作为测量值，对两时刻之间状态进行约束。
  • 求解Ceres Jacobian：
    • 即残差分别对其变量的求导，
    • 其中偏差对bias的求导，偏差对预积分项求导*预积分项对偏差的求导。

Imu 推导 --部分直接给出答案

imu_1
imu_2
imu_3

代码实现

Imu 预积分

构造

  • 有参构造：初始加速度，初始角速度，初始bias偏差，其中Jacobian单位矩阵，协方差为0矩阵
  • 记录了第一帧的加速度与角速度，方便更新bias重新进行预积分。
  • 中值积分 中的固有 噪声。
    • n k a , n k g , n k + 1 a , n k + 1 g , n b k a , n b k + 1 a {n^a_k,n^g_k,n^a_{k+1},n^g_{k+1},n^a_{b_k},n^a_{b_{k+1}}} nka​,nkg​,nk+1a​,nk+1g​,nbk​a​,nbk+1​a​
    • 其：如下 18*18矩阵所示
IntegrationBase() = delete;
IntegrationBase(const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,
                const Eigen::Vector3d &_linearized_ba, const Eigen::Vector3d &_linearized_bg)
    : acc_0{_acc_0}, gyr_0{_gyr_0}, linearized_acc{_acc_0}, linearized_gyr{_gyr_0},
      linearized_ba{_linearized_ba}, linearized_bg{_linearized_bg},
        jacobian{Eigen::Matrix<double, 15, 15>::Identity()}, covariance{Eigen::Matrix<double, 15, 15>::Zero()},
      sum_dt{0.0}, delta_p{Eigen::Vector3d::Zero()}, delta_q{Eigen::Quaterniond::Identity()}, delta_v{Eigen::Vector3d::Zero()}

{
    noise = Eigen::Matrix<double, 18, 18>::Zero();
    noise.block<3, 3>(0, 0) =  (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();
    noise.block<3, 3>(3, 3) =  (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();
    noise.block<3, 3>(6, 6) =  (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();
    noise.block<3, 3>(9, 9) =  (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();
    noise.block<3, 3>(12, 12) =  (ACC_W * ACC_W) * Eigen::Matrix3d::Identity();
    noise.block<3, 3>(15, 15) =  (GYR_W * GYR_W) * Eigen::Matrix3d::Identity();
}

添加 Imu 数据

  • 将 delta_t，线加速度，角速度 放入各自的队列中
    • 目的：Bias会更新，需要根据新的bias重新计算预积分
  • 进行 IMU预积分传播方程

IMU预积分传播方程

  • 积分计算两个关键帧之间IMU测量的变化量：
    • 旋转delta_q 速度delta_v 位移delta_p
    • 加速度的biaslinearized_ba 陀螺仪的Biaslinearized_bg
    • 同时维护更新预积分的Jacobian和Covariance,计算优化时必要的参数
  • 全局变量：
    • delta_p 对应公式： α b i b k { \alpha_{b_ib_{k}}} αbi​bk​​
    • delta_q 对应公式： q b i b k {q_{b_ib_{k}}} qbi​bk​​
    • delta_v 对应公式： β b i b k {\beta_{b_ib_{k}}} βbi​bk​​
    • linearized_ba 对应公式： b k a {b_k^a} bka​
    • linearized_bg 对应公式： b k g {b_k^g} bkg​
void propagate(double _dt, const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1)
{
    dt = _dt;
    acc_1 = _acc_1;
    gyr_1 = _gyr_1;
    Vector3d result_delta_p;
    Quaterniond result_delta_q;
    Vector3d result_delta_v;
    Vector3d result_linearized_ba;
    Vector3d result_linearized_bg;

    midPointIntegration(_dt, acc_0, gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v,
                        linearized_ba, linearized_bg,
                        result_delta_p, result_delta_q, result_delta_v,
                        result_linearized_ba, result_linearized_bg, 1);

    //checkJacobian(_dt, acc_0, gyr_0, acc_1, gyr_1, delta_p, delta_q, delta_v,
    //                    linearized_ba, linearized_bg);
    delta_p = result_delta_p;
    delta_q = result_delta_q;
    delta_v = result_delta_v;
    linearized_ba = result_linearized_ba;
    linearized_bg = result_linearized_bg;
    delta_q.normalize();
    sum_dt += dt;
    acc_0 = acc_1;
    gyr_0 = gyr_1;  
 
}

IMU预积分中采用中值积分递推Jacobian和Covariance

void midPointIntegration(double _dt, 
                           const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,
                           const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1,
                           const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v,
                           const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg,
                           Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,
                           Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg, bool update_jacobian)
   {
       //ROS_INFO("midpoint integration");
       Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
       // 角速度中值，计算出 角度的变化量
       Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
       result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
		
	   // 角速度中值，计算位置的变化量
       Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
       Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
       result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
       result_delta_v = delta_v + un_acc * _dt;

	   // 零偏不变
       result_linearized_ba = linearized_ba;
       result_linearized_bg = linearized_bg;         

	   // 计算F_k 和  G_k ，与公式一样
       if(update_jacobian)
       {
           Vector3d w_x = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
           Vector3d a_0_x = _acc_0 - linearized_ba;
           Vector3d a_1_x = _acc_1 - linearized_ba;
           Matrix3d R_w_x, R_a_0_x, R_a_1_x;

           //反对称矩阵
           R_w_x<<0, -w_x(2), w_x(1),
               w_x(2), 0, -w_x(0),
               -w_x(1), w_x(0), 0;
           R_a_0_x<<0, -a_0_x(2), a_0_x(1),
               a_0_x(2), 0, -a_0_x(0),
               -a_0_x(1), a_0_x(0), 0;
           R_a_1_x<<0, -a_1_x(2), a_1_x(1),
               a_1_x(2), 0, -a_1_x(0),
               -a_1_x(1), a_1_x(0), 0;

           MatrixXd F = MatrixXd::Zero(15, 15);
           F.block<3, 3>(0, 0) = Matrix3d::Identity();
           F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt + 
                                 -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;
           F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt;
           F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;
           F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;
           F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;
           F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt;
           F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt + 
                                 -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;
           F.block<3, 3>(6, 6) = Matrix3d::Identity();
           F.block<3, 3>(6, 9) = -0.5 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt;
           F.block<3, 3>(6, 12) = -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * -_dt;
           F.block<3, 3>(9, 9) = Matrix3d::Identity();
           F.block<3, 3>(12, 12) = Matrix3d::Identity();
           //cout<<"A"<<endl<<A<<endl;

           MatrixXd V = MatrixXd::Zero(15,18);
           V.block<3, 3>(0, 0) =  0.25 * delta_q.toRotationMatrix() * _dt * _dt;
           V.block<3, 3>(0, 3) =  0.25 * -result_delta_q.toRotationMatrix() * R_a_1_x  * _dt * _dt * 0.5 * _dt;
           V.block<3, 3>(0, 6) =  0.25 * result_delta_q.toRotationMatrix() * _dt * _dt;
           V.block<3, 3>(0, 9) =  V.block<3, 3>(0, 3);
           V.block<3, 3>(3, 3) =  0.5 * MatrixXd::Identity(3,3) * _dt;
           V.block<3, 3>(3, 9) =  0.5 * MatrixXd::Identity(3,3) * _dt;
           V.block<3, 3>(6, 0) =  0.5 * delta_q.toRotationMatrix() * _dt;
           V.block<3, 3>(6, 3) =  0.5 * -result_delta_q.toRotationMatrix() * R_a_1_x  * _dt * 0.5 * _dt;
           V.block<3, 3>(6, 6) =  0.5 * result_delta_q.toRotationMatrix() * _dt;
           V.block<3, 3>(6, 9) =  V.block<3, 3>(6, 3);
           V.block<3, 3>(9, 12) = MatrixXd::Identity(3,3) * _dt;
           V.block<3, 3>(12, 15) = MatrixXd::Identity(3,3) * _dt;

           //step_jacobian = F;
           //step_V = V;
           jacobian = F * jacobian;
           covariance = F * covariance * F.transpose() + V * noise * V.transpose();
       }

   }

计算 残差

  • 可通过上述推导看出
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;

    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;

    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 求解器

  • 继承：ceres::SizedCostFunction<15,7,9,7,9>
    • 15维 残差：
      • P 3
      • Q 3
      • V 3
      • Ba 3
      • Bg 3
    • 79/79 分别为 i,j 变量的维数
      • 7 位姿 3+4(四元素)
      • 9 v ba bg 3+3+3
  • 残差：
    • sqrt_information*残差 = sqrt(cov) * 残差
  • 雅克比：
    • 残差分别对 变量求导

代码实现：

virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{

    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 Vi(parameters[1][0], parameters[1][1], parameters[1][2]);
    Eigen::Vector3d Bai(parameters[1][3], parameters[1][4], parameters[1][5]);
    Eigen::Vector3d Bgi(parameters[1][6], parameters[1][7], parameters[1][8]);

    Eigen::Vector3d Pj(parameters[2][0], parameters[2][1], parameters[2][2]);
    Eigen::Quaterniond Qj(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);

    Eigen::Vector3d Vj(parameters[3][0], parameters[3][1], parameters[3][2]);
    Eigen::Vector3d Baj(parameters[3][3], parameters[3][4], parameters[3][5]);
    Eigen::Vector3d Bgj(parameters[3][6], parameters[3][7], parameters[3][8]);

//Eigen::Matrix<double, 15, 15> Fd;
//Eigen::Matrix<double, 15, 12> Gd;

//Eigen::Vector3d pPj = Pi + Vi * sum_t - 0.5 * g * sum_t * sum_t + corrected_delta_p;
//Eigen::Quaterniond pQj = Qi * delta_q;
//Eigen::Vector3d pVj = Vi - g * sum_t + corrected_delta_v;
//Eigen::Vector3d pBaj = Bai;
//Eigen::Vector3d pBgj = Bgi;

//Vi + Qi * delta_v - g * sum_dt = Vj;
//Qi * delta_q = Qj;

//delta_p = Qi.inverse() * (0.5 * g * sum_dt * sum_dt + Pj - Pi);
//delta_v = Qi.inverse() * (g * sum_dt + Vj - Vi);
//delta_q = Qi.inverse() * Qj;

#if 0
    if ((Bai - pre_integration->linearized_ba).norm() > 0.10 ||
        (Bgi - pre_integration->linearized_bg).norm() > 0.01)
    {
        pre_integration->repropagate(Bai, Bgi);
    }
#endif
    // 构建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();
    residual = sqrt_info * residual;

    if (jacobians)
    {
        // 获取预积分的误差递推函数中pqv关于ba、bg的Jacobian
        double sum_dt = pre_integration->sum_dt;
        Eigen::Matrix3d dp_dba = pre_integration->jacobian.template block<3, 3>(O_P, O_BA);
        Eigen::Matrix3d dp_dbg = pre_integration->jacobian.template block<3, 3>(O_P, O_BG);

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

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

        if (pre_integration->jacobian.maxCoeff() > 1e8 || pre_integration->jacobian.minCoeff() < -1e8)
        {
            LOG(INFO)<<"numerical unstable in preintegration";
            //std::cout << pre_integration->jacobian << std::endl;
///                ROS_BREAK();
        }

        // 第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 * pre_integration->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() * (pre_integration->G * sum_dt + Vj - Vi));

            jacobian_pose_i = sqrt_info * jacobian_pose_i;

            if (jacobian_pose_i.maxCoeff() > 1e8 || jacobian_pose_i.minCoeff() < -1e8)
            {
                LOG(INFO)<<"numerical unstable in preintegration";
                //std::cout << sqrt_info << std::endl;
                //ROS_BREAK();
            }
        }
        // 第i帧的imu速度vbi、bai、bgi
        if (jacobians[1])
        {
            Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> jacobian_speedbias_i(jacobians[1]);
            jacobian_speedbias_i.setZero();
            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;

#if 0
        jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -dq_dbg;
#else
            //Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
            //jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -Utility::Qleft(Qj.inverse() * Qi * corrected_delta_q).bottomRightCorner<3, 3>() * dq_dbg;
            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;
#endif

            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 = sqrt_info * jacobian_speedbias_i;

            //ROS_ASSERT(fabs(jacobian_speedbias_i.maxCoeff()) < 1e8);
            //ROS_ASSERT(fabs(jacobian_speedbias_i.minCoeff()) < 1e8);
        }
        // 第j帧的IMU位姿 pbj、qbj
        if (jacobians[2])
        {
            Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMajor>> jacobian_pose_j(jacobians[2]);
            jacobian_pose_j.setZero();

            jacobian_pose_j.block<3, 3>(O_P, O_P) = Qi.inverse().toRotationMatrix();

#if 0
        jacobian_pose_j.block<3, 3>(O_R, O_R) = Eigen::Matrix3d::Identity();
#else
            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>();
#endif

            jacobian_pose_j = sqrt_info * jacobian_pose_j;

            //ROS_ASSERT(fabs(jacobian_pose_j.maxCoeff()) < 1e8);
            //ROS_ASSERT(fabs(jacobian_pose_j.minCoeff()) < 1e8);
        }
        // 第j帧的IMU速度vbj、baj、bgj
        if (jacobians[3])
        {
            Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> jacobian_speedbias_j(jacobians[3]);
            jacobian_speedbias_j.setZero();

            jacobian_speedbias_j.block<3, 3>(O_V, O_V - O_V) = Qi.inverse().toRotationMatrix();

            jacobian_speedbias_j.block<3, 3>(O_BA, O_BA - O_V) = Eigen::Matrix3d::Identity();

            jacobian_speedbias_j.block<3, 3>(O_BG, O_BG - O_V) = Eigen::Matrix3d::Identity();

            jacobian_speedbias_j = sqrt_info * jacobian_speedbias_j;

            //ROS_ASSERT(fabs(jacobian_speedbias_j.maxCoeff()) < 1e8);
            //ROS_ASSERT(fabs(jacobian_speedbias_j.minCoeff()) < 1e8);
        }
    }

    return true;
}

旋转矩阵对其角度求导

  • ∂ R p ∂ R = − R p ∧ {\frac{\partial Rp}{\partial R} = -Rp^\wedge } ∂R∂Rp​=−Rp∧
  • ∂ R − 1 p ∂ R = − ( R − 1 p ) ∧ \frac{\partial R^{-1} p}{\partial R} = -(R^{-1}p)^\wedge ∂R∂R−1p​=−(R−1p)∧
xiaoma_bk
关注
  • 0
    点赞
  • 踩
  • 0
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
（一）VINS系统框架--残差剖析
qq_43526137的博客
09-22 429
组成VINS的残差，待优化变量，以及对应雅可比推导
视觉slam vins 双目 第一讲 IMU预积分及其残差
weixin_40347021的博客
07-06 1930
视觉slam vins 双目 第一讲 IMU预积分及其残差 视觉slam入门者，欢迎大佬指正错误 主要以vins为切入点分析vio（视觉惯性耦合系统） 第一讲主要分系imu。 1. imu预积分 预积分就是为了简便计算，对每两帧图像之间的imu数据进行积分（vins中采用中值积分），并且以这两帧图像之间的第一帧imu数据为标准系进行积分： 2. 预积分离散化 3. 预积分的误差传递以及协防差矩阵 ４. IMU残差（放入ceres求解） vins中对应的代码及其注释 // An highlight
彻底弄懂IMU预积分原理及代码实现
adas_l5的博客
05-10 1200
预积分量对bias的jacobian。相邻两个imu数据之间的递推。和其他传感器融合时的残差项。bias的一阶线性展开。IMU 预积分示意图。增量式预积分迭代计算。
IMU残差函数及雅可比公式推导（二）
CSDN_XCS的博客
07-18 791
IMU残差函数及雅可比公式推导（一）
VINS全局优化残差函数分析
fb_help的专栏
07-24 871
VINS全局优化残差函数分析 VINS的全局优化包含有与GPS低耦合的优化．即GPS作为一个观测(边)加入到整体的优化中去． 其优化的依据是： 帧与帧之间相对平移和旋转在局部是准确的 含有gps的帧其位置应与gps重合． 第一个约束对应的是RelativeRTError,第二个约束对应的是TError 总体的优化由于包括姿态，因此为非线性优化． 参数是：每一帧的位置和姿态 观测是：某帧的gps...
VINS-Mono 代码详细解读——IMU预积分的残差、Jacobian和协方差
Alex的博客
07-08 2773
前言： 对第k帧和第k+1帧之间所有的IMU进行积分，可得到第K+1帧的PVQ（位置、速度、旋转），作为视觉估计的初始值。 每次qwbt优化更新后，都要重新进行积分，运算量较大。将积分模型转为预积分模型： PVQ积分公式中的积分项变为相对于第i时刻的姿态，而不是相对于世界坐标系的姿态。 目录 一、IMU离散中值预积分理论基础 1、没...
VINS 中的IMU预积分推导和代码解读.zip
06-06
在自动驾驶领域，视觉惯性导航系统（Visual-Inertial Navigation System，简称VINS）是一种重要的定位技术，它结合了摄像头的视觉信息与惯性测量单元（IMU）的数据，为移动平台提供高精度的实时定位。本文将深入探讨...
VINS-Mono学习与程序解读(5)：estimator.processIMU.pdf
05-09
VINS-Mono学习与程序解读(5)：estimator.processIMU.pdf
VIO+VINS-mono中的IMU预积分和代码实现.pdf
05-09
[VIO]+VINS-mono中的IMU预积分和代码实现.pdf
VINS demo android APP安装包
09-07
vins 空间定位demo
pgm 1 map ws vins
04-22
pgm 1 map ws vins
七、VINS-mono 代码解析——紧耦合后端非线性优化 IMU+视觉的残差residual、Jacobian、协方差、基于舒尔补的边缘化
Alex的博客
07-16 1011
目录 一、VIO残差函数的构建 1、需要优化的状态向量： 2、目标函数为： 二、视觉约束（细致） 1.视觉重投影误差residual（归一化平面） 2、优化变量 3、Jacobian 4、协方差 三、IMU约束（粗略，祥细见之前） 1、残差： 2、优化变量： 3、IMU测量残差公式推导 4、残差对状态量的Jacobian 5、残差对状态量的协方差 四、基于舒尔补的边缘化 1、论文部分 2、基本公式 3、舒尔补 4、marg后形成的先验 5 具体例子 5.1 原来的信息矩阵H的构成 5.2 舒尔补后形成新的
VIO(notes) —— （3）VIO残差构建与IMU预积分
曾记否@
04-22 1239
VIO残差构建与IMU预积分VIO残差函数的构建系统所需的状态变量视觉重投影误差视觉重投影误差逆深度参数化VIO 中基于逆深度的重投影误差预积分模型由来及意义为什么需要预积分？怎么预积分？预积分是什么？预积分量方差的计算预积分离散方法预积分量的方差状态误差线性递推公式的推导基于泰勒展开的误差传送(应用于 `EKF 的协方差`预测)基于误差随时间变化的递推方程预积分的误差递推公式推导残差Jacobian的推导视觉重投影残差的JacobianIMU预积分残差的雅克比参考文献 VIO残差函数的构建 系统所需的状态
VINS-Mono 理论详细解读——紧耦合后端非线性优化 IMU+视觉的残差residual、Jacobian、协方差、基于舒尔补的边缘化
诗筱涵的博客
12-30 1909
转载自：VINS-Mono 理论详细解读——紧耦合后端非线性优化 IMU+视觉的残差residual、Jacobian、协方差、基于舒尔补的边缘化_努力努力努力-CSDN博客 VINS-Mono 理论详细解读——紧耦合后端非线性优化 IMU+视觉的残差residual、Jacobian、协方差、基于舒尔补的边缘化 贵在坚持，不忘初心 2020-04-07 19:58:54 1531 收藏 10 分类专栏： VINS-Mono and Fusion 程序解读 版权 ​VINS-Mono.
VINS-Mono-紧耦合VIO中IMU残差的优化原理和代码解析
chennuo0125的博客
12-12 784
前言 VINS-Mono算法中VIO部分是基于优化的方法来做的，其优化的目标函数为: min⁡χ{∥rp−Hpχ∥2+∑k∈B∥rB(z^bk+1bk,χ)∥Pbk+1bk2+∑(l,j)∈Cρ(∥rC(z^lcj,χ)∥Plcj2)} \min_{\boldsymbol{\chi}} \left\{ {\left\| \mathbf{r}_{p}-\mathbf{H}_{p}\boldsymbol{\chi} \right\|}^{2} + {\sum_{k\in B}\left\| \mathbf{
IMU预积分及残差雅克比计算
xiaoshuiyisheng的博客
09-05 6658
前段时间推了泡泡机器人邱笑晨博士的IMU预积分公式,收获很大,再看到VIORB的时候,大都用的这一套公式. 后再看VINS的时候,VINS用的是连续时间的预积分,看一些大佬的博客,说代码里写的是离散时间的.这里将所看到的做一下整理. 要求的是: 即要求 这里要求的三个临时状态量只与IMU的...
VINS-Mono论文笔记(中)
hu_hao的博客
09-02 587
VINS-Mono论文笔记-中前言1.初始化过程1.1 视觉重构1.2 视觉惯性联合 前言 本篇内容涉及VINS-Mono第五至第气节内容，包括： 第五节：VINS-Mono的鲁棒性初始化过程 第六节：紧耦合、自校准、基于非线性优化的单眼VIO 第七节：紧耦合的重定位 1.初始化过程 单目惯性系统是高度非线性系统，系统的正常运行高度依赖准确的初始化过程。 初始化过程由松耦合下的imu预积分辅助纯视觉结构还原过程。 1.1 视觉重构 使用滑动窗口中纯视觉的sfm方法。 初始化过程从一个sfm过程开始，还原一个
【SLAM】IMU预积分的理解、手把手推导（1/4）
Yngz_Miao的博客
09-11 9419
在工程实践中我们常常使用视觉与IMU融合的解决方案，即视觉惯性里程计（VIO）来估计运动，而不是简单地使用视觉里程计（VO）。当使用到IMU时，预积分就是绕不开的一个话题。IMU，中文名叫惯性测量单元，英文名：Inertial measurement unit。它可以测量两个东西，加速度aaa是沿三个轴ax、​ay、az​方向的线加速度，而角速度w就是这三个方向的角速度wx、​wy、​wz​。IMU的频率比较高，一般都在100HZ以上。在IMU内部，除了通常的白噪声，还有个特别的量零偏bias。
VINS 双目+IMU
最新发布
05-28
VINS（Visual-Inertial Navigation System）双目+IMU是一种基于视觉和惯性测量单元的导航系统。其主要应用于无人机、自主移动车辆和机器人等领域，可以实现无需GPS信号的室内和室外定位和导航。 VINS双目+IMU系统利用双目摄像头采集环境图像信息，并结合惯性测量单元（IMU）测量的角速度和加速度信息，通过滤波算法对图像与IMU数据进行融合，从而实现对无人机或机器人的定位和导航。这种方法在室内环境中的精度要比GPS导航更高，而且可以克服GPS信号被遮挡或干扰的问题。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值