Vins-Mono代码中关于一些理论的思考以及理论如何与代码一一对应起来的

最新推荐文章于 2023-11-06 11:40:21 发布

Jack Ju

最新推荐文章于 2023-11-06 11:40:21 发布

阅读量561

点赞数

分类专栏： VIO

本文链接：https://blog.csdn.net/qq_40464599/article/details/112392925

版权

VIO 专栏收录该内容

9 篇文章 1 订阅

订阅专栏

estimator_node.cpp

1、解释predict函数

void predict(const sensor_msgs::ImuConstPtr &imu_msg)
{
    //获取当前时间
    double t = imu_msg->header.stamp.toSec();
   //首帧判断
    if (init_imu)
    {
        latest_time = t;
        init_imu = 0;
        return;
    }
    //获取dt并传递时间
    double dt = t - latest_time;
    latest_time = t;
    //获取当前时刻的IMU采样数据
    double dx = imu_msg->linear_acceleration.x;
    double dy = imu_msg->linear_acceleration.y;
    double dz = imu_msg->linear_acceleration.z;
    Eigen::Vector3d linear_acceleration{dx, dy, dz};
     
    double rx = imu_msg->angular_velocity.x;
    double ry = imu_msg->angular_velocity.y;
    double rz = imu_msg->angular_velocity.z;
    Eigen::Vector3d angular_velocity{rx, ry, rz};
    //注意，以下数据都是世界坐标系下的
    Eigen::Vector3d un_acc_0 = tmp_Q * (acc_0 - tmp_Ba) - estimator.g;//参见vio公式21，上一时刻的加速度、tem_Q等数据

    Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - tmp_Bg;//上一时刻的数据
    tmp_Q = tmp_Q * Utility::deltaQ(un_gyr * dt);//Q的更新就是公式26  //Q为body系到world系之间的变换，此时求出的是此时刻得Q

    Eigen::Vector3d un_acc_1 = tmp_Q * (linear_acceleration - tmp_Ba) - estimator.g;//当前时刻的P当前时刻的加速度

    Eigen::Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);//加速度用的是中值法

    tmp_P = tmp_P + dt * tmp_V + 0.5 * dt * dt * un_acc;//求出此时刻的P
    tmp_V = tmp_V + dt * un_acc;//求出此时刻的V
    ///注意，这里就是关于IMU的普通的运动积分，不涉及与积分
    acc_0 = linear_acceleration;
    gyr_0 = angular_velocity;
}

本段程序涉及到的公式如下两个图

2、getMeasurements()函数

getMeasurements()
{     //这个函数的作用就是把图像帧和对应的IMU数据们 配对起来,而且IMU数据时间是在图像帧的前面
    std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;

    while (true)
    {
        //边界判断：数据取完了，说明配对完成
        if (imu_buf.empty() || feature_buf.empty())
            return measurements;
        //边界判断：IMU buf里面所有数据的时间戳都比img buf第一个帧时间戳要早，说明缺乏IMU数据，需要等待IMU数据
        if (!(imu_buf.back()->header.stamp.toSec() > feature_buf.front()->header.stamp.toSec() + estimator.td))
        {
            //ROS_WARN("wait for imu, only should happen at the beginning");
            sum_of_wait++;//统计等待的次数
            return measurements;
        }
         //边界判断：IMU第一个数据的时间要大于第一个图像特征数据的时间(说明图像帧有多的)
        if (!(imu_buf.front()->header.stamp.toSec() < feature_buf.front()->header.stamp.toSec() + estimator.td))
        {
            ROS_WARN("throw img, only should happen at the beginning");
            feature_buf.pop();
            continue;
        }
        //核心操作：装入视觉帧信息
        sensor_msgs::PointCloudConstPtr img_msg = feature_buf.front();
        feature_buf.pop();
        //核心操作：转入IMU信息
        std::vector<sensor_msgs::ImuConstPtr> IMUs;
        while (imu_buf.front()->header.stamp.toSec() < img_msg->header.stamp.toSec() + estimator.td)
        {
            IMUs.emplace_back(imu_buf.front());//加入IMUS
            imu_buf.pop();
        }
        IMUs.emplace_back(imu_buf.front());
        if (IMUs.empty())
            ROS_WARN("no imu between two image");
        measurements.emplace_back(IMUs, img_msg);
        //注意：把最后一个IMU帧又放回到imu_buf里去了
        //原因：最后一帧IMU信息是被相邻2个视觉帧共享的
    }
    return measurements;
}

getMeasurements()函数，从imu_buf和feature_buf中读入IMU数据和图像特征点数据，并在该函数中对IMU数据和图像特征点数据进行初步的时间戳对齐。

我主要想解释那两个条件，imu保证在2个图像之间的，初步的时间戳对齐需要满足两个条件： 1. IMU缓存队列中队尾数据的时间戳，要晚于图像特征点数据缓存队列中队首数据的时间戳； 2. IMU缓存队列中队首数据的时间戳，要早于图像特征点数据缓存队列中队首数据的时间戳。

满足两个条件，才进行装载数据：
1.当最新的imu帧（imu_buf.back()）新过图像帧（feature_buf.front()+ td ）的时间戳（保证了图像帧之前已经没有imu数据了）
2.最旧的imu帧（imu_buf.front()）旧过图像帧（feature_buf.front()+ td ）的时间戳的（保证了图像帧之前一定要有imu帧）

时间戳初步对齐的结果是：一帧图像特征点数据与多帧IMU数据对应，这些IMU数据为：该帧图像与上一帧图像的时间间隔内的所有IMU数据。这里只保证了相邻的feature数据之间有完整的IMU数据。每一组IMU数据和对应的图像特征点数据组成measurement，多组measurement组成measurements，作为getMeasurements()的返回值。

值得一提的是，在获得观测数据的过程中，使用了互斥锁和条件等待的功能。当回调函数imu_callback()或者feature_callback()从话题消息中接收数据时，互斥锁会锁住imu_buf或者feature_buf，此时getMeasurements()就无法从imu_buf、feature_buf中获取观测数据。当imu_callback()或者feature_callback()接收话题数据完成时，条件等待就会唤醒getMeasurements()从imu_buf、feature_buf中读取观测数据，在读取过程中两个回调函数imu_callback()和feature_callback()是无法接收数据的。当getMeasurements()提取完成后，回调函数就可以继续接收话题消息数据。

图解说明比较清晰：
在这里插入图片描述

参考：https://blog.csdn.net/huanghaihui_123/article/details/103032782 吧 https://qingsimon.github.io/post/2018-12-10-vins-mono%E4%BA%8Cvins_estimator_node/

3、预积分中的表达式部分----（IntegrationBase类详解中的中值积分函数（midPointIntegration））预积分是vins的精髓，这段代码找了半天才找到。

所使用的公式即为下图中的公式，一一对应即可，注意，代码中的delta_q是IMU的body系下的相邻两帧之间的变换。

更加需要指出：代码中delta_p就是下图中αbibk+1，

代码中的delta_p就是下图中βbibk+1，

代码中的dela_q就是下图中的qbibk+1。

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)
    {
        /**该函数用于求解IMU的预积分状态递推和更新对应的Jacobian矩阵，
         * 函数接口参数分别为：
            t时刻的IMU量测：_acc_0, _gyr_0
            t时刻的位置、姿态、速度预积分估计值：delta_p, delta_q, delta_v
            t+1时刻的IMU量测：_acc_1, _gyr_1
            t+1时刻的位置、姿态、速度预积分估计值：result_delta_p, result_delta_q, result_delta_v
            t和t+1时刻加速度计和陀螺仪零偏估计值：result_linearized_ba, result_linearized_ba, result_linearized_ba, result_linearized_bg
            Jacobian矩阵更新选项：update_jacobian***/


        //ROS_INFO("midpoint integration");
        //注意这里不涉及世界坐标系，全在body坐标系下。公式见与积分离散形式（vio从零开始写vi0中公式33）
        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;

midPointIntegration函数剩下一部分就是预积分的误差递推函数公式。

于是就得到了F和G表达式。


        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();
            //这部分应该就是预积分误差传递的形式，
        }

最重要的是得到了这段代码：

 covariance = F * covariance * F.transpose() + V * noise * V.transpose();
            //这部分应该就是预积分误差传递的形式，

为什么要这么做递推？见下图！

就是用概率的方法保证信息传递的准确性，毕竟，预积分通俗解释，就是在相邻两个相机的图片之间有很多imu的数据，通过预积分简化运算，但是将这些imu数据合并的过程中就需要以概率的形式给他们加权处理，置信度越高，比例就越大，权重就越大。这是我个人理解。

20200109 于研究室

Jack Ju

关注

0
点赞
踩
5

收藏

觉得还不错? 一键收藏
打赏
3
评论
Vins-Mono代码中关于一些理论的思考以及理论如何与代码一一对应起来的

estimator_node.cpp1、解释predict函数void predict(const sensor_msgs::ImuConstPtr &imu_msg){ //获取当前时间 double t = imu_msg->header.stamp.toSec(); //首帧判断 if (init_imu) { latest_time = t; init_imu = 0; return;
复制链接

扫一扫