Bootstrap

VINS-Mono 代码解析二、初始化 第1部分

一、前言

VINS的状态估计器模块(estimator),主要在代码中 /vins_estimator节点的相关部分实现。
这个模块可以说是VINS的最核心模块,从论文的内容上来说,里面的内容包括了VINS的估计器初始化、基于滑动窗口的非线性优化实现紧耦合。此外还包括了关键帧的选择内容。该模块的代码放在文件夹vins_estimator中,可以看到,除了上述内容外,还包括有外参标定、可视化等其他功能的实现,内容实在是太多了!

本人主要按照程序的运行步骤来讲解,理论和代码 结合的方式,可能有点多,后面可能会分两部分。

因为这是我第3此仔细看这个代码了,一方面加深理解,另一方面也是一种记录和复习吧!结束后会自己写双目和加GPS数据。

这一部分的整体结构图:

           

其中:

  • factor                                    ——  主要用于非线性优化对各个参数块和残差块的定义,VINS采用的是ceres,所以这部分需要对一些状态量和因子进行继承和重写。
  • initial                                     —— 主要用于初始化,VINS采用的初始化策略是先SfM进行视觉初始化,再与IMU进行松耦合。
  • estimator.cpp                   ——  vins_estimator需要的所有函数都放在这里,是一个鸿篇巨制。
  • estimator_node.cpp     ——  vins_estimator的入口,是一个ROS的node,实际上运行的是这个cpp文件。
  • feature_manager.cpp  ——  负责管理滑窗内的所有特征点。
  • parameters.cpp              ——  读取参数,是一个辅助函数。
  • utility                                   ——   里面放着用于可视化的函数和tictok计时器。

下面是系统的闭环优化流程图:

                                                                                    

上图中,蓝线为正常的闭环优化流程,即通过后端的非线性优化来更新滑窗内所有相机的位姿。紫线为闭环检测模块,当后端优化完成后,会将滑窗内的次新帧进行闭环检测,即首先提取新角点并进行描述,然后与数据库进行检索,寻找闭环帧,并将该帧添加到数据库中。红线为快速重定位模块,当检测到闭环帧后,会将闭环约束添加到后端的整体目标函数中进行非线性优化,得到第 i 帧(注意这里的帧为闭环帧中的老帧)经过滑窗优化后的位姿,这时可以直接根据计算结果修正滑窗内所有相机的位姿,但是因为此处计算的 T i←i_opt  并不准 确 , 因 此 我 们 会 在 检 测 到 闭 环 成 功 后 , 进 行 PoseGraph 的 四 自 由 度 优 化PoseGraph::optimize4DoF(),来计算 T i←i_opt ,并将此值 (r_drift、t_drift) 传回给后端优化线程,来更新滑窗内的相机位姿 Estimator::update_loop_correction()。

 

二、程序解析

程序入口在   /vins_estimator/src/estimator_node.cpp   的main.cpp 里面,代码如下:

int main(int argc, char **argv)
{
    //1、ROS初始化 
    ros::init(argc, argv, "vins_estimator"); //许ROS通过命令行进行名称重映射,指定节点名称的地方
    ros::NodeHandle n("~");                  //创建节点句柄-实际上将执行节点的初始化
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
    
    //2、读取参数,设置状态估计器参数
    readParameters(n);
    //它读取了每一个相机到IMU坐标系的旋转/平移外参数和非线性优化的重投影误差部分的信息矩阵。
    estimator.setParameter();
    
#ifdef EIGEN_DONT_PARALLELIZE
    ROS_DEBUG("EIGEN_DONT_PARALLELIZE");
#endif
    ROS_WARN("waiting for image and imu...");

    //3、发布用于  RVIZ显示 和 pose_graph_node.cpp 接收 的Topic,本模块具体发布的内容详见输入输出
    //这个函数定义在utility/visualization.cpp里面:void registerPub(ros::NodeHandle &n)。
    registerPub(n); 

    //4、创建消息订阅者,订阅IMU、feature、restart、match_points的topic,执行各自回调函数
    ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());
    
    //这一部分接收的是feature_tracker_node发布的在cur帧的所有特征点的信息
    ros::Subscriber sub_image = n.subscribe("/feature_tracker/feature", 2000, feature_callback);
    
    //restart_callback干了一件事,就是把所有状态量归零,把buf里的数据全部清空。
    ros::Subscriber sub_restart = n.subscribe("/feature_tracker/restart", 2000, restart_callback);
    ros::Subscriber sub_relo_points = n.subscribe("/pose_graph/match_points", 2000, relocalization_callback);

    //5、创建VIO主线程process()(VINS核心!),这一部分是最重要的,包含了VINS绝大部分内容和最难的内容
    std::thread measurement_process{process};
    ros::spin();//所有用户的调用程序将从 ros::spin()开始调用

    return 0;
}

 说明:1、2部分我们就不看了,和前端的道理是一样的,ROS的初始化和参数的读取,我们从第3部分开始看!

2.1 主题发布

我们看了主函数代码很奇怪,只有订阅的话题,没有发布的话题,其实话题发布在第3步骤!程序如下“

void registerPub(ros::NodeHandle &n)
{
    pub_latest_odometry    = n.advertise<nav_msgs::Odometry>("imu_propagate", 1000);
    pub_path               = n.advertise<nav_msgs::Path>("path", 1000);
    pub_relo_path          = n.advertise<nav_msgs::Path>("relocalization_path", 1000);
    pub_odometry           = n.advertise<nav_msgs::Odometry>("odometry", 1000);
    pub_point_cloud        = n.advertise<sensor_msgs::PointCloud>("point_cloud", 1000);
    pub_margin_cloud       = n.advertise<sensor_msgs::PointCloud>("history_cloud", 1000);
    pub_key_poses          = n.advertise<visualization_msgs::Marker>("key_poses", 1000);
    pub_camera_pose        = n.advertise<nav_msgs::Odometry>("camera_pose", 1000);
    pub_camera_pose_visual = n.advertise<visualization_msgs::MarkerArray>("camera_pose_visual", 1000);
    pub_keyframe_pose      = n.advertise<nav_msgs::Odometry>("keyframe_pose", 1000);
    pub_keyframe_point     = n.advertise<sensor_msgs::PointCloud>("keyframe_point", 1000);
    pub_extrinsic          = n.advertise<nav_msgs::Odometry>("extrinsic", 1000);
    pub_relo_relative_pose =  n.advertise<nav_msgs::Odometry>("relo_relative_pose", 1000);

    cameraposevisual.setScale(1);
    cameraposevisual.setLineWidth(0.05);
    keyframebasevisual.setScale(0.1);
    keyframebasevisual.setLineWidth(0.01);
}

先不用知道每个发布信息的意思,后面在慢慢去理解,知道发布话题在这即可!

2.2 订阅话题信息

 (1)订阅IMU话题信息

ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());
void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)
{
    if (imu_msg->header.stamp.toSec() <= last_imu_t)
    {
        ROS_WARN("imu message in disorder!");
        return;
    }
    last_imu_t = imu_msg->header.stamp.toSec();

    m_buf.lock();
    imu_buf.push(imu_msg);
    m_buf.unlock();
    con.notify_one();

    last_imu_t = imu_msg->header.stamp.toSec();

    {
        std::lock_guard<std::mutex> lg(m_state);
        predict(imu_msg); // 只要有IMU数据就调用这个程序
        std_msgs::Header header = imu_msg->header;
        
        header.frame_id = "world";
        if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
            pubLatestOdometry(tmp_P, tmp_Q, tmp_V, header);
    }
}

以上程序涉及到多线程编成,可以参考:c++11:多线程编程(1)初识_努力努力努力-CSDN博客

子程序:

1、 predict(imu_msg)   // 只要有IMU数据就调用这个程序

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;

    Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - tmp_Bg;
    tmp_Q = tmp_Q * Utility::deltaQ(un_gyr * dt);

    Eigen::Vector3d un_acc_1 = tmp_Q * (linear_acceleration - tmp_Ba) - estimator.g;

    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;
    tmp_V = tmp_V + dt * un_acc;

    //信息传递
    acc_0 = linear_acceleration;
    gyr_0 = angular_velocity;
}

理解:

  • 离散时刻的 IMU 积分公式, 可以分为:欧拉法离散形式 和 中值法的离散形式,上面程序对应的是中值法!只要有IMU数据就运行;
  • tmp_Q, tmp_P,tmp_V 代表的是200HZ的VIO信息(全局变量, 当前时刻世界坐标系(local world frame))

其理论部分如下:

 

2、pubLatestOdometry(tmp_P, tmp_Q, tmp_V, header);   

void pubLatestOdometry(const Eigen::Vector3d &P, const Eigen::Quaterniond &Q, const Eigen::Vector3d &V, const std_msgs::Header &header)
{
    Eigen::Quaterniond quadrotor_Q = Q ;

    nav_msgs::Odometry odometry;
    odometry.header = header;
    odometry.header.frame_id = "world";
    odometry.pose.pose.position.x = P.x();
    odometry.pose.pose.position.y = P.y();
    odometry.pose.pose.position.z = P.z();
    odometry.pose.pose.orientation.x = quadrotor_Q.x();
    odometry.pose.pose.orientation.y = quadrotor_Q.y();
    odometry.pose.pose.orientation.z = quadrotor_Q.z();
    odometry.pose.pose.orientation.w = quadrotor_Q.w();
    odometry.twist.twist.linear.x = V.x();
    odometry.twist.twist.linear.y = V.y();
    odometry.twist.twist.linear.z = V.z();
    pub_latest_odometry.publish(odometry);
}

功能:

把当前最新的位姿 存入   pub_latest_odometry;我并不是没事干,把所有的程序都贴上去,你有没有发现  pub_latest_odometry  有点眼熟!对的,  就是  前面 “2.1 主题发布”  中的第一个,看看吧!

 

 问:为啥订阅IMU信息放在这里,为啥不妨在前端?

答:放在这里是为了和视觉信息联合初始化,再者,前端订阅了也没用用呀,前端主要是视觉特征提取和跟踪,在前端订阅,还要发布出去,不如直接放在这里!品一下,应该很容易明白的!

(2) 订阅前端发布的特征点话题信息

ros::Subscriber sub_image = n.subscribe("/feature_tracker/feature", 2000, feature_callback);
void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg)
{
    if (!init_feature)
    {
        //skip the first detected feature, which doesn't contain optical flow speed
        init_feature = 1;
        return;
    }
    m_buf.lock();
    feature_buf.push(feature_msg);
    m_buf.unlock();
    con.notify_one();
}

 分析:

  1. 这一部分接收的是 feature_tracker_node 发布的在  cur帧  的所有特征点的信息,有疑问的可以去看看之前写的博客!
  2. 把订阅的特征点信息放在 全局变量 feature_buf 里面!
  3. 涉及到多线程,看一下前面的连接

(3)订阅前端发布 重新开始 话题信息

当图像数据错误时,比如时间间隔有问题等,系统不运行,状态量清零等,应对前端故障的一种机制!

//restart回调函数,收到restart消息时清空feature_buf和imu_buf,估计器重置,时间重置
void restart_callback(const std_msgs::BoolConstPtr &restart_msg)
{
    if (restart_msg->data == true)
    {
        ROS_WARN("restart the estimator!");
        m_buf.lock();
        while(!feature_buf.empty())
            feature_buf.pop();
        while(!imu_buf.empty())
            imu_buf.pop();
        m_buf.unlock();
        m_estimator.lock();
        estimator.clearState();
        estimator.setParameter();
        m_estimator.unlock();
        current_time = -1;
        last_imu_t = 0;
    }
    return;
}

 

(4)订阅 重定位 话题信息

void relocalization_callback(const sensor_msgs::PointCloudConstPtr &points_msg)
{
    //printf("relocalization callback! \n");
    m_buf.lock();
    relo_buf.push(points_msg);
    m_buf.unlock();
}

功能:把重定位信息放到全局变量 relo_buf 里面!

解析:这一段代码是订阅 重定位 模块对应的信息,当检测到闭环帧后,会将闭环约束添加到后端的整体目标函数中进行非线性优化,得到第 i 帧(注意这里的帧为闭环帧中的老帧)经过滑窗优化后的位姿,这时可以直接根据计算结果修正滑窗内所有相机的位姿。

疑问:为什么要进行重定位?
答:    因为随着相机的运行,下一时刻相机的位姿是根据前时刻推算过来的,这样的坏处是:会有误差的累积(因为噪声的存在嘛,优化的过程噪声是减弱的过程,不会消除),但是如果我们找到开始时刻对应的位置,把开始的信息的约束加到后端优化里面,这时候会把这个误差拉回来,重而提高精度。

2.3 创建VIO主线程  process()  (VINS核心)

开启一个线程:thread: visual-inertial odometry---VIO = 视觉惯导里程计,没有回环部分;先上代码:

void process()
{
    while (true)
    {
       // (1)、对imu和图像数据进行对齐并配对这一部分卡了我很久很久,终于最近彻底弄明白了。
        std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;
        
//等待上面两个接收数据完成就会被唤醒,在执行getMeasurements()提取measurements时互斥锁m_buf会锁住,此时无法接收数据。getMeasurements()的作用是对imu和图像数据进行对齐并组合,之后会具体分析
        std::unique_lock<std::mutex> lk(m_buf);
        con.wait(lk, [&]
                 {
            return (measurements = getMeasurements()).size() != 0;
                 });
        lk.unlock();
        
//(2)、对IMU数据的处理
        m_estimator.lock();
        //对measurements中的每一个measurement (IMUs,IMG)组合进行操作
        for (auto &measurement : measurements)
        {
            //2.1、对于measurement中的每一个imu_msg,计算dt并执行processIMU()-实现了IMU的预积分,通过中值积分得到当前PQV作为优化初值
            auto img_msg = measurement.second;
            double dx = 0, dy = 0, dz = 0, rx = 0, ry = 0, rz = 0;
            for (auto &imu_msg : measurement.first)
            {
                double t = imu_msg->header.stamp.toSec();
                //相机和IMU同步校准得到的时间差
                double img_t = img_msg->header.stamp.toSec() + estimator.td;
                //对于大多数情况,IMU的时间戳都会比img的早,此时直接选取IMU的数据就行
                if (t <= img_t)//http://wiki.ros.org/sensor_msgs
                { 
                    if (current_time < 0)
                        current_time = t;
                    double dt = t - current_time;
                    ROS_ASSERT(dt >= 0);
                    current_time = t;
                    dx = imu_msg->linear_acceleration.x;
                    dy = imu_msg->linear_acceleration.y;
                    dz = imu_msg->linear_acceleration.z;
                    rx = imu_msg->angular_velocity.x;
                    ry = imu_msg->angular_velocity.y;
                    rz = imu_msg->angular_velocity.z;
                    //这里干了2件事,IMU粗略地预积分,然后把值传给一个新建的IntegrationBase对象
                    estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
                    //printf("imu: dt:%f a: %f %f %f w: %f %f %f\n",dt, dx, dy, dz, rx, ry, rz);

                }
            //对于处于边界位置的IMU数据,是被相邻两帧共享的,而且对前一帧的影响会大一些,在这里,对数据线性分配
                else//每个大于图像帧时间戳的第一个imu_msg是被两个图像帧共用的(出现次数少)
                {
                    double dt_1 = img_t - current_time;//current_time < img_time < t
                    double dt_2 = t - img_t;
                    current_time = img_t;
                    ROS_ASSERT(dt_1 >= 0);
                    ROS_ASSERT(dt_2 >= 0);
                    ROS_ASSERT(dt_1 + dt_2 > 0);
                    
                     //以下操作其实就是简单的线性分配
                    double w1 = dt_2 / (dt_1 + dt_2);
                    double w2 = dt_1 / (dt_1 + dt_2);
                    dx = w1 * dx + w2 * imu_msg->linear_acceleration.x;
                    dy = w1 * dy + w2 * imu_msg->linear_acceleration.y;
                    dz = w1 * dz + w2 * imu_msg->linear_acceleration.z;
                    rx = w1 * rx + w2 * imu_msg->angular_velocity.x;
                    ry = w1 * ry + w2 * imu_msg->angular_velocity.y;
                    rz = w1 * rz + w2 * imu_msg->angular_velocity.z;
                    estimator.processIMU(dt_1, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
                    //printf("dimu: dt:%f a: %f %f %f w: %f %f %f\n",dt_1, dx, dy, dz, rx, ry, rz);
                }
            }//这个if-else的操作技巧非常值得学习!这一部分的核心代码是processIMU(),它在estomator.cpp里面,它的作用就是IMU预积分
            
            //(3)、重定位/回环检测操作
            // set relocalization frame
            sensor_msgs::PointCloudConstPtr relo_msg = NULL;
             //在relo_buf中取出最后一个重定位帧,拿出其中的信息并执行setReloFrame()
            while (!relo_buf.empty())
            {
                relo_msg = relo_buf.front();
                relo_buf.pop();
            }
            if (relo_msg != NULL)
            {
                vector<Vector3d> match_points;
                double frame_stamp = relo_msg->header.stamp.toSec();
                for (unsigned int i = 0; i < relo_msg->points.size(); i++)
                {
                    Vector3d u_v_id;
                    u_v_id.x() = relo_msg->points[i].x;
                    u_v_id.y() = relo_msg->points[i].y;
                    u_v_id.z() = relo_msg->points[i].z;
                    match_points.push_back(u_v_id);
                }
                Vector3d relo_t(relo_msg->channels[0].values[0], relo_msg->channels[0].values[1], relo_msg->channels[0].values[2]);
                Quaterniond relo_q(relo_msg->channels[0].values[3], relo_msg->channels[0].values[4], relo_msg->channels[0].values[5], relo_msg->channels[0].values[6]);
                Matrix3d relo_r = relo_q.toRotationMatrix();
                int frame_index;
                frame_index = relo_msg->channels[0].values[7];
                estimator.setReloFrame(frame_stamp, frame_index, match_points, relo_t, relo_r);
            }

            ROS_DEBUG("processing vision data with stamp %f \n", img_msg->header.stamp.toSec());

            
            //建立每个特征点的(camera_id,[x,y,z,u,v,vx,vy])s的map,索引为feature_id
            TicToc t_s;
            
            //(4)、对img信息进行处理(核心!)
            //一开始,就定义了一个新的数据结构,解释一下:
            /*数据结构: map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image
             1、虽然它叫image,但是这个容器里面存放的信息是每一个特征点的!
             2、索引值是feature_id;
             3、value值是一个vector,如果系统是多目的,那么同一个特征点在不同摄像头下会有不同的观测信息,那么这个vector,就是存储着某个特征点在所有摄像头上的信息。对于VINS-mono来说,value它不是vector,仅仅是一个pair,其实是可以的。
             4、接下来看这个vector里面的每一pair。int对应的是camera_id,告诉我们这些数据是当前特征点在哪个摄像头上获得的。
             5、Matrix<double, 7, 1>是一个7维向量,依次存放着当前feature_id的特征点在camera_id的相机中的归一化坐标,像素坐标和像素运动速度,这些信息都是在feature_tracker_node.cpp中获得的*/
            map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image;
            
            
            //这个数据结构定义完之后,接下来就是往这个容器中放数据。
            //遍历img_msg里面的每一个特征点的归一化坐标
            for (unsigned int i = 0; i < img_msg->points.size(); i++)
            {
                //把img的信息提取出来放在image容器里去,通过这里,可以理解img信息里面装的都是些什么
                int v = img_msg->channels[0].values[i] + 0.5;
                int feature_id = v / NUM_OF_CAM;
                int camera_id  = v % NUM_OF_CAM;
                double x = img_msg->points[i].x;
                double y = img_msg->points[i].y;
                double z = img_msg->points[i].z;
                double p_u = img_msg->channels[1].values[i];
                double p_v = img_msg->channels[2].values[i];
                double velocity_x = img_msg->channels[3].values[i];
                double velocity_y = img_msg->channels[4].values[i];
                ROS_ASSERT(z == 1);
                Eigen::Matrix<double, 7, 1> xyz_uv_velocity;
                xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
                image[feature_id].emplace_back(camera_id,  xyz_uv_velocity);
            }
            
            //处理图像processImage() (核心!)
            //这一部分的内容非常多,将一层层剥开讲解!这里实现了视觉与IMU的初始化以及非线性优化的紧耦合。
            estimator.processImage(image, img_msg->header);

            // (5)、可视化
            //向RVIZ发布里程计信息、关键位姿、相机位姿、点云和TF关系,
            //这些函数都定义在中utility/visualization.cpp里,都是ROS相关代码。
            double whole_t = t_s.toc();
            printStatistics(estimator, whole_t);
            std_msgs::Header header = img_msg->header;
            header.frame_id = "world";

            pubOdometry(estimator, header);
            pubKeyPoses(estimator, header);
            pubCameraPose(estimator, header);
            pubPointCloud(estimator, header);
            pubTF(estimator, header);
            pubKeyframe(estimator);
            if (relo_msg != NULL)
                pubRelocalization(estimator);
            //ROS_ERROR("end: %f, at %f", img_msg->header.stamp.toSec(), ros::Time::now().toSec());
        }
        m_estimator.unlock();
        
        //(6)、IMU的PVQ信息更新
        //更新IMU参数[P,Q,V,ba,bg,a,g],需要上锁,注意线程安全。这里对应的知识点是4.1.1最后一个公式。
        m_buf.lock();
        m_state.lock();
        if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
            update();//更新IMU参数[P,Q,V,ba,bg,a,g]
        m_state.unlock();
        m_buf.unlock();
    }
}

 理解:

首先利用 getMeasurements() 把IMU,相机数据对齐,在进行预积分等操作;

 

(1)imu和图像数据进行对齐并组合

 std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, 
sensor_msgs::PointCloudConstPtr>> measurements;
        
        std::unique_lock<std::mutex> lk(m_buf);
        con.wait(lk, [&]
                 {
            return (measurements = getMeasurements()).size() != 0;
                 });
        lk.unlock();

子程序:getMeasurements()

作用:根据时间戳,挑选当前帧和上一帧图像之间的imu数据,用于后续的IMU积分;

std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>>
getMeasurements()
{
    std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;

    while (true)
    {   //边界判断:数据取完了,说明配对完成
        //直到把缓存中的图像特征数据或者IMU数据取完,才能够跳出此函数
        if (imu_buf.empty() || feature_buf.empty())
            return measurements;

//情况1:IMU buf里面所有数据的时间戳都比img buf第一个帧时间戳要早,说明缺乏图像数据,需要等待图像数据
        if (!(imu_buf.back()->header.stamp.toSec() > feature_buf.front()->header.stamp.toSec() + estimator.td))
        {
            ROS_WARN("wait for image, only should happen at the beginning");
            sum_of_wait++;//统计等待的次数
            return measurements;
        }
//情况2: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信息
        //图像数据(img_msg),对应多组在时间戳内的imu数据,然后塞入measurements
        std::vector<sensor_msgs::ImuConstPtr> IMUs;
        while (imu_buf.front()->header.stamp.toSec() < img_msg->header.stamp.toSec() + estimator.td)
        {
             //emplace_back相比push_back能更好地避免内存的拷贝与移动
            IMUs.emplace_back(imu_buf.front());
            imu_buf.pop();
        }
        
        //注意:把最后一个IMU帧又放回到imu_buf里去了
        //原因:最后一帧IMU信息是被相邻2个视觉帧共享的
        IMUs.emplace_back(imu_buf.front());
        if (IMUs.empty())
            ROS_WARN("no imu between two image");
        measurements.emplace_back(IMUs, img_msg);
    }
    return measurements;
}

/--------------------------------------------------代码---------------------------------------------------------/

理解:(最好自己画个图吧)

imu_buf  和 feature_buf 就是之前我们订阅的IMU和图像话题信息(两个传感器同时上电,但IMU频率高,第一个IMU采集时间应该在第一帧图像时间的前面);

该函数的主要功能是对 imu 和 图像数据 进行 对齐并组合,返回的是(IMUs, img_msg),即图像帧所对应的所有IMU数据,并将其放入一个容器vector中。IMU和图像帧的对应关系在新版的代码中有变化:对图像帧j,每次取完imu_buf中所有时间戳小于它的imu_msg,以及第一个时间戳大于图像帧时间戳的imu_msg (这里还需要加上同步时间存在的延迟td)。因此在新代码中,每个大于图像帧时间戳的第一个imu_msg是被两个图像帧共用的,而产生的差别在processIMU()前进行了对应的处理 --- 认真理解这段话啥意思!

情况一:最后一个IMU数据对应的时间都在第一帧图像的前面,当然说明没有image信息啦,所以需要等待啦!;

情况二:IMU第一个数据的时间在第一帧图像时间的后面,不符合IMU采样频率高的特性,那就把第一帧图像 进行 pop() 删除栈顶元素,这时候第二帧图像变成栈顶元素;

后面  while (imu_buf.front()->header.stamp.toSec() < img_msg->header.stamp.toSec() + estimator.td) 条件满足,那就把imu数据放入 IMUs 里面呀,然后执行imu_buf.pop(),第二个IMU数据变成了 栈顶元素,一直这样循环,直到 跳出 while, 说明这个时候IMU数据对应的时间大于第一帧图像的时间,但是又保存到了 IMUs 里面,这就是我们说的“ 每个大于图像帧时间戳的第一个imu_msg是被两个图像帧共用”, 后面就是这样一直读数据,一直保存, 形成了下面的数据格式!             品品,你细品!

(2)对IMU数据的处理

/--------------------------------------------------代码---------------------------------------------------------/

//(2)、对IMU数据的处理
        m_estimator.lock();
        //对measurements中的每一个measurement (IMUs,IMG)组合进行操作
        for (auto &measurement : measurements)
        {
            //2.1、对于measurement中的每一个imu_msg,计算dt并执行processIMU()-实现了IMU的预积分,通过中值积分得到当前PQV作为优化初值
            auto img_msg = measurement.second;
            double dx = 0, dy = 0, dz = 0, rx = 0, ry = 0, rz = 0;
            for (auto &imu_msg : measurement.first)
            {
                double t = imu_msg->header.stamp.toSec();
                //相机和IMU同步校准得到的时间差
                double img_t = img_msg->header.stamp.toSec() + estimator.td;
                //对于大多数情况,IMU的时间戳都会比img的早,此时直接选取IMU的数据就行
                if (t <= img_t)//http://wiki.ros.org/sensor_msgs
                { 
                    if (current_time < 0)
                        current_time = t;
                    double dt = t - current_time;
                    ROS_ASSERT(dt >= 0);
                    current_time = t;
                    dx = imu_msg->linear_acceleration.x;
                    dy = imu_msg->linear_acceleration.y;
                    dz = imu_msg->linear_acceleration.z;
                    rx = imu_msg->angular_velocity.x;
                    ry = imu_msg->angular_velocity.y;
                    rz = imu_msg->angular_velocity.z;
                    //这里干了2件事,IMU粗略地预积分,然后把值传给一个新建的IntegrationBase对象
                    estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
                    //printf("imu: dt:%f a: %f %f %f w: %f %f %f\n",dt, dx, dy, dz, rx, ry, rz);

                }
            //对于处于边界位置的IMU数据,是被相邻两帧共享的,而且对前一帧的影响会大一些,在这里,对数据线性分配
                else//每个大于图像帧时间戳的第一个imu_msg是被两个图像帧共用的(出现次数少)
                {
                    double dt_1 = img_t - current_time;//current_time < img_time < t
                    double dt_2 = t - img_t;
                    current_time = img_t;
                    ROS_ASSERT(dt_1 >= 0);
                    ROS_ASSERT(dt_2 >= 0);
                    ROS_ASSERT(dt_1 + dt_2 > 0);
                    
                     //以下操作其实就是简单的线性分配
                    double w1 = dt_2 / (dt_1 + dt_2);
                    double w2 = dt_1 / (dt_1 + dt_2);
                    dx = w1 * dx + w2 * imu_msg->linear_acceleration.x;
                    dy = w1 * dy + w2 * imu_msg->linear_acceleration.y;
                    dz = w1 * dz + w2 * imu_msg->linear_acceleration.z;
                    rx = w1 * rx + w2 * imu_msg->angular_velocity.x;
                    ry = w1 * ry + w2 * imu_msg->angular_velocity.y;
                    rz = w1 * rz + w2 * imu_msg->angular_velocity.z;
                    estimator.processIMU(dt_1, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
                    //printf("dimu: dt:%f a: %f %f %f w: %f %f %f\n",dt_1, dx, dy, dz, rx, ry, rz);
                }
            }//这个if-else的操作技巧非常值得学习!这一部分的核心代码是processIMU(),它在estomator.cpp里面,它的作用就是IMU预积分

前面一步我们已经把IMU和图像的数据对齐了,后面那就开始使用了!这里的核心思想是对IMU进行积分,但和之前我们分析的predict()是不一样的!下面是分析:

程序  for (auto &imu_msg : measurement.first) 一直读取IMU数据,如果满足:if (t <= img_t),  执行 estimator.processIMU(); 这个是很容易理解的,IMU数据在图像数据的前面,就执行积分呀!关在在于当 else 的时候,即 if (t <= img_t) 的时候,此时的 第一帧IMU在当前帧图像时间后的第一个IMU数据,下一帧的第一个IMU)就会被两帧图像共用!在这时候就需要线性的分配,很简单!我们用如下的图例和公式推导一下:

                                                                          

 

子程序:processIMU()   

相关的概念:
                                    dt:两次IMU测量的时间间隔
linear_acceleration:线性加速度
angular_velocity     :角速度

IMU预积分程序,这里是要和纯视觉SFM结果对齐后求解IMU参数,开始IMU零偏是未知的,用此方法求解。处理加速度,角速度,时间间隔 = IMU数据,进行预积分。
--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
数据结构:
1、Rs[frame_count],Ps[frame_count],Vs[frame_count]:是从IMU系转到world系的PVQ,数据是由IMU预积分得到的,目前在这里存放的是没有用bias修正过的值。
2、frame_count:这个值让我很疑惑,它只在processImage()里有过++操作,而且在estimator.hpp声明的时候,没有加上static关键字。它是在h文件中声明,在cpp文件里初始化的,后续需要再关注一下。
3、dt_buf,linear_acceleration_buf,angular_velocity_buf:帧数和IMU测量值的缓存,而且它们是对齐的。
3、pre_integrations[frame_count],它是IntegrationBase的一个实例,在factor/integration_base.h中定义,它保存着frame_count帧中所有跟IMU预积分相关的量,包括F矩阵,Q矩阵,J矩阵等。   

 


void Estimator::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)
{
    /*这一段作用就是就是给以下数据提供初始值/初始化:
    
   pre_integrations[frame_count]
   dt_buf[frame_count]
   linear_acceleration_buf[frame_count]
   angular_velocity_buf[frame_count]
   Rs[frame_count]
   PS[frame_count]
   Vs[frame_count] 
   关于frame_count的更新,目前只在process_img里的solver_flag == INITIAL这里看到?
    
   */
    
    //1.初始化,如果当前帧不是第一帧IMU,那么就把它看成第一个IMU,而且把他的值取出来作为初始值
    if (!first_imu)
    {
        first_imu = true;
        acc_0 = linear_acceleration;
        gyr_0 = angular_velocity;
    }

    //边界判断:如果当前IMU帧没有构造IntegrationBase,那就构造一个,后续会用上
     //当滑窗不满的时候,把当前测量值加入到滑窗指定位置,所以在这个阶段做预积分的时候相当于是在和自己做预积分
    if (!pre_integrations[frame_count])
    {
        pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
        //注意此处是使用上一个历元的IMU数据!因此first_imu的设置还是有作用的
        //初始化滑动窗口预积分时,bias都是给零的,如果知道bias值,上面的代码需要修改
    }
    
    //进入预积分环节
    //2. 预积分,核心操作
    if (frame_count != 0)//滑窗内关键帧的个数
    {
        //添加IMU数据,进行预积分  Prei-Second
        pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
        //if(solver_flag != NON_LINEAR)
        //这里进行2次预积分,计算量是否有点大,可以考虑优化
            tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);

      //linear_acceleration, angular_velocity分别是当前的测量值,上述两个函数执行同样的操作,计算预计分,
      //因为k,k+1时刻之间有很多IMU数据,每到来一个IMU数据,就会计算并累加一次预计分。
      //3. dt、加速度、角速度加到buf中,为什么?和后面的滑动窗口。边缘花有关!
        dt_buf[frame_count].push_back(dt);
        linear_acceleration_buf[frame_count].push_back(linear_acceleration);
        angular_velocity_buf[frame_count].push_back(angular_velocity);

        //4. 基于中值积分对当前的位姿进行估计
        //这个地方又拿IMU的数据做了一次积分, Prei-Third,更新估计器内系统的位姿
        int j = frame_count;     
        //注意啊,这块对j的操作看似反智,是因为j时刻的值都拷贝了j-1时刻的值!!
        //第一次使用实际上就是使用的是j-1时刻的值,所以在这些地方写上j-1是没有关系的!
        //noise是zero mean Gauss,在这里忽略了
        //把j改成j-1,看看效果是一样
        
        Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;
        //下面都采用的是中值积分的传播方式,noise被忽略了
        // 把j改成j-1,看看效果是一样
        Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];//w=0.5(w0+w1)-bg
        
        //应该改为Rs[j-1],不需要改!!!下面Rs[j]进行了姿态更新
        Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
        Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;//a1 当前帧imu速度
        Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);// 中值积分下的加速度a=1/2(a0+a1)
        Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;// P=P+v*t+1/2*a*t^2
        Vs[j] += dt * un_acc;
         /*
           其中,acc_0,gyr_0是上一时刻的测量值,代码分别与前述中值积分的公式相对应。这里所得到的位姿估计是用于后面作为非线性优化的初始值。
        */
    }
    // 6.更新上一帧的加速度和角速度, 数据传递
    acc_0 = linear_acceleration;
    gyr_0 = angular_velocity;
}

分析:单纯的看processIMU()  我们可以知道其主要的作用是实现IMU的预积分, 注释很详细,很容易明白,但是需要注意的是:放在 (2)对IMU数据的处理  整体去看,你会发现(2) 的作用是什么?就是获得当前图像帧之前的所有IMU预积分的值 作为 当前图像帧的初始值,

所以IMU预积分可以看作两帧图像之间的一个约束关系,也是后面我们需要优化的项和联合初始化的内容!为啥 会这样想?因为纯视觉可以进行两帧之间的距离求解,IMU也可以求两帧之间的距离,但是IMU有漂移,不准确,而相机相对准确,那就可以根据公式去解出IMU的bias了呀!对应的理论后面说联合初始化的时候再讲吧。

具体程序细节:

(3)重定位

//三、重定位/回环检测操作
            // set relocalization frame
            sensor_msgs::PointCloudConstPtr relo_msg = NULL;
             //在relo_buf中取出最后一个重定位帧,拿出其中的信息并执行setReloFrame()
            while (!relo_buf.empty())
            {
                relo_msg = relo_buf.front();
                relo_buf.pop();
            }
            if (relo_msg != NULL)
            {
                vector<Vector3d> match_points;
                double frame_stamp = relo_msg->header.stamp.toSec();
                for (unsigned int i = 0; i < relo_msg->points.size(); i++)
                {
                    Vector3d u_v_id;
                    u_v_id.x() = relo_msg->points[i].x;
                    u_v_id.y() = relo_msg->points[i].y;
                    u_v_id.z() = relo_msg->points[i].z;
                    match_points.push_back(u_v_id);
                }
                Vector3d relo_t(relo_msg->channels[0].values[0], relo_msg->channels[0].values[1], relo_msg->channels[0].values[2]);
                Quaterniond relo_q(relo_msg->channels[0].values[3], relo_msg->channels[0].values[4], relo_msg->channels[0].values[5], relo_msg->channels[0].values[6]);
                Matrix3d relo_r = relo_q.toRotationMatrix();
                int frame_index;
                frame_index = relo_msg->channels[0].values[7];
                estimator.setReloFrame(frame_stamp, frame_index, match_points, relo_t, relo_r);
            }

分析:relo_buf   应该熟悉,就是我们上面订阅的重定位信息;这段代码的作用是获得重定位的信息,然后送给estimator.setReloFrame(frame_stamp, frame_index, match_points, relo_t, relo_r) 函数,因此我们必须了解 estimator.setReloFrame() 是干啥的,下面看他的程序!

子程序 estimator.setReloFrame()

/--------------------------------------------------代码---------------------------------------------------------/

void Estimator::setReloFrame(double _frame_stamp, int _frame_index, vector<Vector3d> &_match_points, Vector3d _relo_t, Matrix3d _relo_r)
{
    relo_frame_stamp = _frame_stamp;
    relo_frame_index = _frame_index;
    match_points.clear();
    match_points = _match_points;
    prev_relo_t = _relo_t;
    prev_relo_r = _relo_r;
    for(int i = 0; i < WINDOW_SIZE; i++)
    {
        if(relo_frame_stamp == Headers[i].stamp.toSec())
        {
            relo_frame_local_index = i;
            relocalization_info = 1;
            for (int j = 0; j < SIZE_POSE; j++)
                relo_Pose[j] = para_Pose[i][j];
        }
    }
}

分析:重定位 是当前滑窗内的帧 与  Keyframe database中旧帧匹配;这一部分的代码就是 匹配帧的一个赋值;match_points 表示匹配点的信息;后面用到的时候我们在具体的分析吧!

由于篇幅有限,也不想写太长,VINS-mono的估计的第一部分就到这!

;