欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

VINS-Mono代码阅读笔记(四):vins_estimator中process线程代码分析

程序员文章站 2022-03-22 20:54:00
...

process线程作为estimator中的主要线程,对接收到的imu消息和image消息进行处理,进而通过这些信息估计出相机位姿。

1.process函数介绍

阅读IMU相关代码前,要能够明白IMU都能测量哪些数据。IMU就是惯性传感器的缩写,主要用于测量加速度和旋转运动的传感器。基础的IMU包括加速度计和角速度计,一般由三轴的加速度传感器和三个轴的陀螺仪组成,可以测量IMU在三个轴方向上的线加速度和三个轴方向上的角速度信息。

process函数代码如下。

// 这个是处理measurements的线程
// thread: visual-inertial odometry
void process()
{
    while (true)
    {
        std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;
        std::unique_lock<std::mutex> lk(m_buf);
        /**
         * 这里执行条件变量con.wait函数并且measurements.size==0,就锁定了当前线程,当前线程会一直被阻塞
         * 直到有另一个线程也使用con变量调用notification函数来唤醒当前线程
         * 当其他线程调用了notification后,这里的wait函数中还需要判断(measurements = getMeasurements()).size() != 0是否为true,
         * 只有当为true的时候才会解除当前线程的阻塞
         * */
        con.wait(lk, [&]
                 {
                    //1.获取在时间上“对齐”的IMU和图像数据的组合
            return (measurements = getMeasurements()).size() != 0;
                 });
        lk.unlock();
        m_estimator.lock();
        //2.遍历measurements,其实就是遍历获取每一个img_msg和其对应的imu_msg对数据进行处理
        for (auto &measurement : measurements)
        {
            auto img_msg = measurement.second;
            double dx = 0, dy = 0, dz = 0, rx = 0, ry = 0, rz = 0;
            //遍历和当前img_msg时间上“对齐”的IMU数据
            for (auto &imu_msg : measurement.first)
            {
                double t = imu_msg->header.stamp.toSec();
                double img_t = img_msg->header.stamp.toSec() + estimator.td;
                if (t <= img_t)//imu数据比图像数据早到
                {
                    //第一次的时候current_time值为-1
                    if (current_time < 0)
                        current_time = t;
                    //计算时间间隔
                    double dt = t - current_time;
                    ROS_ASSERT(dt >= 0);
                    current_time = t;
                    //dx,dy,dz分别是IMU在三个轴方向上的加速度
                    dx = imu_msg->linear_acceleration.x;
                    dy = imu_msg->linear_acceleration.y;
                    dz = imu_msg->linear_acceleration.z;
                    //rx,ry,rz分别是IMU在三个轴方向上的角速度
                    rx = imu_msg->angular_velocity.x;
                    ry = imu_msg->angular_velocity.y;
                    rz = imu_msg->angular_velocity.z;
                    //对每一个IMU值进行预积分
                    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);

                }
                else//图像数据比IMU数据早到
                {
                    double dt_1 = img_t - current_time;
                    //t为imu的时间,dt_2表示imu消息的时间戳和图像数据时间戳之间的差值
                    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;
                    //对每一个IMU值进行预积分
                    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);
                }
            }
            // set relocalization frame 设置重定位帧
            sensor_msgs::PointCloudConstPtr relo_msg = NULL;
            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();
                //遍历relo_msg中的points特征点
                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());

            TicToc t_s;
            map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image;
            //遍历img_msg中的特征点
            for (unsigned int i = 0; i < img_msg->points.size(); i++)
            {
                int v = img_msg->channels[0].values[i] + 0.5;
                int feature_id = v / NUM_OF_CAM;
                int camera_id = v % NUM_OF_CAM;
                //获取img_msg中第i个点的x,y,z坐标,这个是归一化后的坐标值
                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];
                //获取像素点在x,y方向上的速度
                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 map,索引为feature_id
                //image中每个特征点在帧中的位置信息和坐标轴上的速度信息按照feature_id为索引存入image中
                image[feature_id].emplace_back(camera_id,  xyz_uv_velocity);
            }
            //图像特征处理,包括初始化和非线性优化
            estimator.processImage(image, img_msg->header);

            double whole_t = t_s.toc();
            printStatistics(estimator, whole_t);
            std_msgs::Header header = img_msg->header;
            header.frame_id = "world";
            //给Rviz发送里程计信息:odometry、path、relo_path这几个topic
            pubOdometry(estimator, header);
            //给Rviz发送关键点位置
            pubKeyPoses(estimator, header);
            //给Rviz发送相机位置
            pubCameraPose(estimator, header);
            //给Rviz发送点云数据
            pubPointCloud(estimator, header);
            //发布tf转换topic
            pubTF(estimator, header);
            //发送关键帧信息
            pubKeyframe(estimator);
            if (relo_msg != NULL)
                pubRelocalization(estimator);
            //ROS_ERROR("conend: %f, at %f", img_msg->header.stamp.toSec(), ros::Time::now().toSec());
        }
        m_estimator.unlock();
        m_buf.lock();
        m_state.lock();
        //3.NON_LINEAR情况下,调用update进行参数更新
        if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
            update();
        m_state.unlock();
        m_buf.unlock();
    }
}

总结来看,process中代码结构如下图所示:

VINS-Mono代码阅读笔记(四):vins_estimator中process线程代码分析

其中,第二部分遍历measurements(其实就是遍历IMU数据和img_msg数据)进行处理的部分是process中最主要的部分,也是我要花时间和功夫重点研究的核心部分,这部分留到后边再慢慢说。这里先把第一部分和第三部分介绍一下。

2.getMeasurements函数解析

process函数中是一个大的while循环一直在运行,当线程创建后,进入线程入口函数process后在while(true)中首先就是将当前线程通过wait方法阻塞了,此时的(measurements = getMeasurements()).size() != 0取值是false,所以wait方法能够阻塞住当前线程。

其实这里说简单点就是生产者-消费者的问题,当生产者还没有生产出产品的时候消费者就在那里等着,等有产品的时候再工作。当前线程作为消费者,在当前线程运行后由于还没有IMU和图像数据,所以暂时将该线程阻塞住,等接收imu数据的回调函数和接收feature的回调函数中收到数据的时候再将该线程唤醒,这时候有数据了线程就可以执行了。

imu_callback和feature_callback中用于唤醒当前线程的语句如下,不清楚的可以返回 VINS-Mono代码阅读笔记(三):vins_estimator中main函数分析 查看相关部分的代码。

con.notify_one();

getMeasurements函数主要工作就是将接收到的IMU数据和相关的图像数据进行“时间上的对齐”,并将对齐的IMU数据和图像数据进行组合供后边IMU和图像数据处理的时候使用。由于IMU的更新频率比相机要高出好多,所以和一帧图像时间上“对齐”的IMU数据是一个vector类型的容器中存放的IMU数据,在阅读代码的时候这个也需要知道。

getMeasurements函数代码如下:

//对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信息接收到后会在imu_callback回调中存入imu_buf,feature消息收到后会在feature_callback中存入feature_buf中
        if (imu_buf.empty() || feature_buf.empty())
            return measurements;
        //下面两个if判断都是对imu数据和image数据在时间戳上的判断(时间要对齐才能进行组合)
        //imu_buf中最后一个imu消息的时间戳比feature_buf中第一个feature消息的时间戳还要小,说明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");
            //imu数据比图像数据要早,所以返回,等待时间上和图像对齐的imu数据到来
            sum_of_wait++;
            return measurements;
        }
        //imu_buf中第一个数据的时间戳比feature_buf的第一个数据的时间戳还要大
        if (!(imu_buf.front()->header.stamp.toSec() < feature_buf.front()->header.stamp.toSec() + estimator.td))
        {
            //imu数据比图像数据滞后,所以图像数据要出队列
            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();

        std::vector<sensor_msgs::ImuConstPtr> IMUs;
        //imu_buf的最前边的数据时间戳小于图像数据的时间戳的话,就将imu_buf中最前边的数据存入IMUs当中
        while (imu_buf.front()->header.stamp.toSec() < img_msg->header.stamp.toSec() + estimator.td)
        {
            //将imu_buf中最上边的元素插入到IMUs的尾部
            IMUs.emplace_back(imu_buf.front());
            imu_buf.pop();
        }
        //这行代码后难道不缺少一句imu_buf.pop();吗???
        IMUs.emplace_back(imu_buf.front());
        if (IMUs.empty())
            ROS_WARN("no imu between two image");
        //将和该img_msg时间上“对齐”的imu数据连同该img_msg放入measurements的尾部
        //所以这里的measurements中存放的就是在时间上“对齐”的IMU数据和图像数据
        measurements.emplace_back(IMUs, img_msg);
    }
    return measurements;
}

3.update函数

在process函数中遍历完measurements之后,说明当前接收到的IMU和图像数据已经处理完了,那么我们可以思考一下接下来做什么呢?因为后边还有可能继续接收IMU和图像数据,所以一些参数的初始值就变成了刚才处理完的数据的值(简单来说,因为IMU和图像数据是连续的,所以前一个状态的数值是后一个状态的初始值)。这个工作就是在update中来完成的。

update函数代码如下:

//得到窗口最后一个图像帧的imu项[P,Q,V,ba,bg,a,g],对imu_buf中剩余imu_msg进行PVQ递推
void update()
{
    TicToc t_predict;
    latest_time = current_time;
    tmp_P = estimator.Ps[WINDOW_SIZE];
    tmp_Q = estimator.Rs[WINDOW_SIZE];
    tmp_V = estimator.Vs[WINDOW_SIZE];
    tmp_Ba = estimator.Bas[WINDOW_SIZE];
    tmp_Bg = estimator.Bgs[WINDOW_SIZE];
    acc_0 = estimator.acc_0;
    gyr_0 = estimator.gyr_0;

    queue<sensor_msgs::ImuConstPtr> tmp_imu_buf = imu_buf;
    for (sensor_msgs::ImuConstPtr tmp_imu_msg; !tmp_imu_buf.empty(); tmp_imu_buf.pop())
        predict(tmp_imu_buf.front());

}

其中的predict函数代码如下:

//从IMU测量值imu_msg和上一个PVQ递推得到当前PVQ
void predict(const sensor_msgs::ImuConstPtr &imu_msg)
{
    double t = imu_msg->header.stamp.toSec();
    //init_imu初始化的值为1
    if (init_imu)
    {
        latest_time = t;
        init_imu = 0;
        return;
    }
    //计算当前imu_msg距离上一个imu_msg的时间间隔
    double dt = t - latest_time;
    latest_time = t;
    //获取x,y,z三个方向上的线加速度
    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};
    //获取x,y,z三个方向上的角速度
    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;
}

 

相关标签: SLAM VINS-Mono