14-[LVI-SAM]visual_odometry_callbackAndprocess

阅读量：

2021SC@SDUSC

visual_odometry_callback

在上一章中, 我们主要对 $\texttt{visual\_odometry}$ 的主要函数进行了全面概述, 并对节点在生成回调函数之前的准备工作进行了详细讲解. 我们已经掌握了节点的工作原理及其输入输出关系. 进一步分析表明, 在这一章节中所讨论的内容与我之前研究过的 $\texttt{visual\_feature}$ 具有较强的关联性, 并且需要依赖其发布的视觉特征点.

在此基础上

其中

其中，imu_callback，主线程process是主要部分。

文章目录

visual_odometry_callback
- [1] imu_callback
  - [a] 数据完整性验证
    - [b] 将数据持久化存储并发送通知给主线程处理
    - [c] 计算IMU状态并进行更新
    - [3] 预测算法的调用与初始化
    - d. 完整代码
2. odometry callback
3. feature callback
4. restart callback
5. primary thread process
a. 接收数据并提取measurements数组
- b. getMeasurements() function
  - b. IMU预积分
  - - processIMU()函数
  - c. VINS优化
  - d. 从激光雷达里程计获取初始化信息
  - - getOdometry()函数
  - d. 可视化，传输数据给RVIZ
  - e. 更新IMU参数PQV
  - - update()函数
  - f. 完整代码
  - 附录：引用

1. *`imu_callback`

IMU回调函数会存储imu_msg到imu_buf并推导出包含[P,Q,V,header,failureCount]的状态信息。

a. 数据校验

此处确认所有IMU数据帧的到达时间顺序是否正确；如果存在错误，则会发出警告信息并跳过该数据帧进行处理；若顺序无误，则继续后续操作流程；同时记录当前帧的时间值，并用于下一数据帧的时间验证。

复制代码

    // 判断这一帧的时间是否小于等于上一帧的时间，如果结果为true，则说明乱序，时间顺序错误
    if (imu_msg->header.stamp.toSec() <= last_imu_t)
    {
    ROS_WARN("imu message in disorder!");
    return;
    }
    last_imu_t = imu_msg->header.stamp.toSec();

b. 保存数据，并通知主线程

为了防止多线程同时访问IMU数据缓存队列而引发问题，在获取数据后会将主程序从阻塞状态被唤醒。具体功能描述可参考下文注释

复制代码

    m_buf.lock(); 
    imu_buf.push(imu_msg);
    m_buf.unlock();
    con.notify_one(); // 唤醒作用于process线程中的获取观测值数据的函数
    
    last_imu_t = imu_msg->header.stamp.toSec(); // 不知道为啥又加了一次赋值操作

c. IMU状态递推，并发布

我们首先注意到，在编程过程中，“代码块”通常被大括号包裹以定义作用域。“这种做法背后的逻辑是什么？”其根本原因是，在这段代码的第一行定义了一个互斥锁机制 lock_guard。“在执行这段代码的过程中，默认情况下会在第一行施加一个锁机制 lock_guard。“一旦退出该作用域时，“对应的临时变量会被自动释放。“相应的 lock_guard 对象在构造函数完成时会自动释放锁。”总体而言，在编程中这是一种常见的技术手段。

随后调用预测函数。该函数的功能是基于IMU测得的imu_msg和上一时刻的PVQ状态推导出下一个tmp_Q、tmp_P和tmp_V，并采用中值积分方法进行计算。

最后，发布最新的由IMU直接递推得到的PQV。

复制代码

    {
    // 构造互斥锁m_state，析构时解锁
    std::lock_guard<std::mutex> lg(m_state);
    predict(imu_msg); // 递推得到IMU的PQV
    std_msgs::Header header = imu_msg->header;
    
    // 发布最新的由IMU直接递推得到的PQV
    if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
        pubLatestOdometry(tmp_P, tmp_Q, tmp_V, header, estimator.failureCount);
    }

predict函数

从IMU测量值imu_msg与前一时刻的状态PQV经过推导得出下一状态tmp_Q、tmp_P与tmp_V。这段代码正是基于积分中值定理来计算这些状态量的变化。然而我对这一部分的具体实现细节还不是很清楚。

复制代码

    void predict(const sensor_msgs::ImuConstPtr &imu_msg)
    {
    double t = imu_msg->header.stamp.toSec();
    
    // 如果是第一个IMU的数据，啧init_imu为1，不处理，直接返回
    if (init_imu)
    {
        latest_time = t;
        init_imu = 0;
        return;
    }
    double dt = t - latest_time;
    latest_time = t;
    
    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;
    }

d. 完整代码

复制代码

    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);
        std_msgs::Header header = imu_msg->header;
        if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
            pubLatestOdometry(tmp_P, tmp_Q, tmp_V, header, estimator.failureCount);
    }
    }

2. `odom_callback`

这个函数就是简单的把重定位后的里程计信息放入缓存队列中。

复制代码

    void odom_callback(const nav_msgs::Odometry::ConstPtr& odom_msg)
    {
    m_odom.lock();
    odomQueue.push_back(*odom_msg);
    m_odom.unlock();
    }

3. `feature_callback`

这个函数相对简单，在开始处理之前先判断当前是否为初始帧。
如果当前是初始帧，则不做处理。
由于之前在博客中已经阐述过，
我们假设初始帧图像不具备光流速度信息，
因此无法进行特征点追踪。
同样地，
在后续步骤中也会将消息存入缓冲区中，
接着也需要与imu_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();
    }

4. `restart_callback`

这部分内容会涉及视觉特征节点可能接收的重启消息。在我的上一篇博客中已详细分析指出：当相机的数据流出现不稳定现象时会触发重置机制并发送restart消息。如前所述第11篇博客中对此进行了深入探讨。在执行重启操作之前建议先清空缓冲区并重置所有数据与状态以确保系统的稳定性。

复制代码

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

5. *主线程process

这个线程是整个节点最重要的部分。

在该过程中仅包含一个while循环；其运行条件由 $ros::ok()$ 决定。由此可见，在当前节点处于运行状态时；若 $ros::ok()$ 返回true，则会导致无限循环。

在while循环内部实现的功能如下：首先，在循环开始时执行以下步骤：1. 等待接收测量数据包measurements：(IMUs, img_msg)s；2. 计算当前时间间隔dt；3. 调用estimator.processIMU()函数完成IMU预积分计算；4. 通过调用estimator.setReloFrame()方法完成重置参考坐标系的操作；5. 最后调用estimator.processImage()函数完成图像数据处理工作流程：包括初始化图像处理流程以及进行紧耦合的非线性优化运算以获得最优解。

a. 接受数据，提取measurements数组

开始时, 我们会先创建一个临时变量, 用于存储我们刚刚获取到的measurements. 接着, 会对 $m_buf$ 施加唯一锁定机制, 这是为了防止在访问共享内存时出现异常. 这种状态会一直保持下去, 直到被唤醒为止. 当被唤醒后, 我们会再次对 $m_buf$ 施加锁定, 并调用getMeasurements函数来获取相关信息. 这个过程会持续下去, 直到我们从返回的对象中获得测量数据为止. 如果返回的对象没有测量数据（即测量结果为空），那么系统就会再次陷入等待状态.

复制代码

    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 array），而第二个部分则对应图像消息（image message）。为了进一步处理这些信息，在缓冲区imu_buf及feature_buf中提取了相关数据，并通过整合实现时间同步功能。此外，在整合过程中要求确保所有时间戳按顺序排列，并将各部分结合在一起。具体来说，在时间同步过程中要求所有记录必须按照发生顺序排列（ chronological ordering）。可参考下方代码注释部分获取详细实现方法。

复制代码

    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 (ros::ok())
    {
        if (imu_buf.empty() || feature_buf.empty())
            return measurements;
    
        // 对齐标准：IMU最后一个数据的时间要大于第一个图像特征数据的时间
        if (!(imu_buf.back()->header.stamp.toSec() > feature_buf.front()->header.stamp.toSec() + estimator.td))
        {
            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();
    
        std::vector<sensor_msgs::ImuConstPtr> IMUs;
        // 图像数据(img_msg)，对应多组在时间戳内的imu数据,然后塞入measurements
        while (imu_buf.front()->header.stamp.toSec() < img_msg->header.stamp.toSec() + estimator.td)
        {
            IMUs.emplace_back(imu_buf.front());
            imu_buf.pop();
        }
        // 这里把下一个imu_msg也放进去了,但没有pop，因此当前图像帧和下一图像帧会共用这个imu_msg
        IMUs.emplace_back(imu_buf.front());
        if (IMUs.empty())
            ROS_WARN("no imu between two image");
        measurements.emplace_back(IMUs, img_msg);
    }
    return measurements;
    }

b. IMU预积分

在处理主线程时会先调用m_estimator.lock()
主要是为了避免在主线程运行中突然出现visual_feature节点重启的情况
从而使得后续操作需要访问的共享缓冲区和共享变量不会受到影响
随后会执行一个for-each循环操作
这部分代码不会单独展示
在完整的代码文件中会有详细的实现

接下来是对每一个measurement元素的操作开始，在处理过程中涉及到了IMU的预积分阶段。首先是将数据提取出来，并在此基础上立即执行预积分操作；具体实现细节可参考下方代码注释部分。

复制代码

    auto img_msg = measurement.second;
    
    // IMU的预积分
    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();
    double img_t = img_msg->header.stamp.toSec() + estimator.td;
    if (t <= img_t)
    { 
        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;
        // 这里就是执行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
    {
        double dt_1 = img_t - current_time;
        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);
    }
    }

`processIMU()`函数

本节内容主要专注于处理IMU数据。通过让IMU进行预积分运算，并结合中值积分运算的方法计算得到当前PQV，并将其作为优化过程的初始估计值。

复制代码

    oid Estimator::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)
    {
    if (!first_imu)
    {
        first_imu = true;
        acc_0 = linear_acceleration;
        gyr_0 = angular_velocity;
    }
    
    if (!pre_integrations[frame_count])
    {
        pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
    }
    if (frame_count != 0)
    {
        pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
    
        tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);
    
        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);
    
        // 采用的是中值积分的传播方式
        int j = frame_count;         
        Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;
        Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
        Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
        Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
        Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
        Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
        Vs[j] += dt * un_acc;
    }
    acc_0 = linear_acceleration;
    gyr_0 = angular_velocity;
    }

c. VINS优化

这里建立每个特征点的(camera_id,[x,y,z,u,v,vx,vy])s的map，索引为feature_id。

复制代码

    map<int, vector<pair<int, Eigen::Matrix<double, 8, 1>>>> image;
    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;
    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];
    double depth = img_msg->channels[5].values[i];
    
    ROS_ASSERT(z == 1);
    Eigen::Matrix<double, 8, 1> xyz_uv_velocity_depth;
    xyz_uv_velocity_depth << x, y, z, p_u, p_v, velocity_x, velocity_y, depth;
    image[feature_id].emplace_back(camera_id,  xyz_uv_velocity_depth);
    }

d. 从激光雷达里程计获取初始化信息

复制代码

    vector<float> initialization_info;
    m_odom.lock();
    initialization_info = odomRegister->getOdometry(odomQueue, img_msg->header.stamp.toSec() + estimator.td);
    m_odom.unlock();

`getOdometry()`函数

此处在初始化阶段进行处理，并可参考我的同学所做的分析。我这边仅作为辅助展示提供了一些代码片段。简而言之，该函数的作用是将里程计信息从激光雷达帧转换至VINS图像帧中完成数据对准过程。下面是我对这部分代码的具体解析与说明。

复制代码

    vector<float> getOdometry(deque<nav_msgs::Odometry>& odomQueue, double img_time)
    {
    vector<float> odometry_channel;
    odometry_channel.resize(18, -1); // 重新设置 id(1), P(3), Q(4), V(3), Ba(3), Bg(3), gravity(1)
    
    nav_msgs::Odometry odomCur;
    
    // 把旧的里程计信息扔掉，类似我前面的blog中的分析。
    while (!odomQueue.empty()) 
    {
        if (odomQueue.front().header.stamp.toSec() < img_time - 0.05)
            odomQueue.pop_front();
        else
            break;
    }
    
    if (odomQueue.empty())
    {
        return odometry_channel;
    }
    
    // 找到最接近图像时间的里程计时间
    for (int i = 0; i < (int)odomQueue.size(); ++i)
    {
        odomCur = odomQueue[i];
    
        if (odomCur.header.stamp.toSec() < img_time - 0.002) // 500Hz imu
            continue;
        else
            break;
    }
    
    // 时间戳差异仍然太大，进行处理
    if (abs(odomCur.header.stamp.toSec() - img_time) > 0.05)
    {
        return odometry_channel;
    }
    
    // 将里程计旋转从激光雷达 ROS 框架转换为 VINS 相机框架（仅旋转，假设激光雷达、相机和 IMU 足够接近）
    tf::Quaternion q_odom_lidar;
    tf::quaternionMsgToTF(odomCur.pose.pose.orientation, q_odom_lidar);
    
    tf::Quaternion q_odom_cam = tf::createQuaternionFromRPY(0, 0, M_PI) * (q_odom_lidar * q_lidar_to_cam); // 全局旋转 pi // 注意：相机 - 激光雷达
    tf::quaternionTFToMsg(q_odom_cam, odomCur.pose.pose.orientation);
    
    // 将里程计位置从激光雷达 ROS 框架转换为 VINS 相机框架
    Eigen::Vector3d p_eigen(odomCur.pose.pose.position.x, odomCur.pose.pose.position.y, odomCur.pose.pose.position.z);
    Eigen::Vector3d v_eigen(odomCur.twist.twist.linear.x, odomCur.twist.twist.linear.y, odomCur.twist.twist.linear.z);
    Eigen::Vector3d p_eigen_new = q_lidar_to_cam_eigen * p_eigen;
    Eigen::Vector3d v_eigen_new = q_lidar_to_cam_eigen * v_eigen;
    
    odomCur.pose.pose.position.x = p_eigen_new.x();
    odomCur.pose.pose.position.y = p_eigen_new.y();
    odomCur.pose.pose.position.z = p_eigen_new.z();
    
    odomCur.twist.twist.linear.x = v_eigen_new.x();
    odomCur.twist.twist.linear.y = v_eigen_new.y();
    odomCur.twist.twist.linear.z = v_eigen_new.z();
    
    // odomCur.header.stamp = ros::Time().fromSec(img_time);
    // odomCur.header.frame_id = "vins_world";
    // odomCur.child_frame_id = "vins_body";
    // pub_latest_odometry.publish(odomCur);
    
    odometry_channel[0] = odomCur.pose.covariance[0];
    odometry_channel[1] = odomCur.pose.pose.position.x;
    odometry_channel[2] = odomCur.pose.pose.position.y;
    odometry_channel[3] = odomCur.pose.pose.position.z;
    odometry_channel[4] = odomCur.pose.pose.orientation.x;
    odometry_channel[5] = odomCur.pose.pose.orientation.y;
    odometry_channel[6] = odomCur.pose.pose.orientation.z;
    odometry_channel[7] = odomCur.pose.pose.orientation.w;
    odometry_channel[8]  = odomCur.twist.twist.linear.x;
    odometry_channel[9]  = odomCur.twist.twist.linear.y;
    odometry_channel[10] = odomCur.twist.twist.linear.z;
    odometry_channel[11] = odomCur.pose.covariance[1];
    odometry_channel[12] = odomCur.pose.covariance[2];
    odometry_channel[13] = odomCur.pose.covariance[3];
    odometry_channel[14] = odomCur.pose.covariance[4];
    odometry_channel[15] = odomCur.pose.covariance[5];
    odometry_channel[16] = odomCur.pose.covariance[6];
    odometry_channel[17] = odomCur.pose.covariance[7];
    
    return odometry_channel;
    }

d. 可视化，传输数据给RVIZ

随后将数据通过话题传递至RVIZ系统，并具体传输内容可参考下方注释部分。关于发布信息函数的具体实现细节，则不做深入探讨。即简单地提取所需数据，并将其打包后发送至指定话题位置完成处理流程至此已完成for-each循环迭代处理，并随后执行锁解除操作：调用$m_estimator.unlock();

复制代码

    std_msgs::Header header = img_msg->header;
    pubOdometry(estimator, header); // "odometry" 里程计信息PQV
    pubKeyPoses(estimator, header); // "key_poses" 关键点三维坐标
    pubCameraPose(estimator, header); // "camera_pose" 相机位姿
    pubPointCloud(estimator, header); // "history_cloud" 点云信息
    pubTF(estimator, header); // "extrinsic" 相机到IMU的外参
    pubKeyframe(estimator); // "keyframe_point"、"keyframe_pose" 关键帧位姿和点云

e. 更新IMU参数PQV

复制代码

    m_buf.lock();
    m_state.lock();
    if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
    update(); // 更新操作
    m_state.unlock();
    m_buf.unlock();

`update()`函数

从estimator获取滑动窗口当前图像帧的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());
    }

f. 完整代码

复制代码

    void process()
    {
    while (ros::ok())
    {
        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();
    
        m_estimator.lock();
        for (auto &measurement : measurements)
        {
            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();
                double img_t = img_msg->header.stamp.toSec() + estimator.td;
                if (t <= img_t)
                { 
                    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;
                    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
                {
                    double dt_1 = img_t - current_time;
                    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);
                }
            }
    
            // TicToc t_s;
            map<int, vector<pair<int, Eigen::Matrix<double, 8, 1>>>> image;
            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;
                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];
                double depth = img_msg->channels[5].values[i];
    
                ROS_ASSERT(z == 1);
                Eigen::Matrix<double, 8, 1> xyz_uv_velocity_depth;
                xyz_uv_velocity_depth << x, y, z, p_u, p_v, velocity_x, velocity_y, depth;
                image[feature_id].emplace_back(camera_id,  xyz_uv_velocity_depth);
            }
    
            vector<float> initialization_info;
            m_odom.lock();
            initialization_info = odomRegister->getOdometry(odomQueue, img_msg->header.stamp.toSec() + estimator.td);
            m_odom.unlock();
    
    
            estimator.processImage(image, initialization_info, img_msg->header);
            // double whole_t = t_s.toc();
            // printStatistics(estimator, whole_t);
    
            std_msgs::Header header = img_msg->header;
            pubOdometry(estimator, header);
            pubKeyPoses(estimator, header);
            pubCameraPose(estimator, header);
            pubPointCloud(estimator, header);
            pubTF(estimator, header);
            pubKeyframe(estimator);
        }
        m_estimator.unlock();
    
        m_buf.lock();
        m_state.lock();
        if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
            update();
        m_state.unlock();
        m_buf.unlock();
    }
    }

附录：引用

LVI-SAM代码– xuwuzhou探索SLAM技术的道路 – 以静制动, 善思, 明辨, 扎实求学

ManiiXu/VINS-Mono-Learning: VINS-Mono

全部评论 (0)

还没有任何评论哟~

14-[LVI-SAM]visual_odometry_callbackAndprocess

2021SC@SDUSC visualodometrycallback 在上一章，我们大致把visualodometry的main函数过了一遍，把节点在生成回调函数之前的准备工作介绍了一遍。我们已经...

13-[LVI-SAM]visual_odometry_初探

2021SC@SDUSC visualodometry初探上一章已经把visualfeature的大部分代码和功能，虽然没有完全把那一个节点讲明白，但是已经大致清楚整一份代码的思路。

11-[LVI-SAM]visual_feature_img_callback浅析

2021SC@SDUSC visualfeatureimgcallback浅析上一blog，我们分析了LVISAM中的其中一个回调函数lidarcallback。

15-[LVI-SAM]分析总结

2021SC@SDUSC LVISAM分析总结这是我的LVISAM代码分析的最后一篇。在这一篇，我会做一个总结，对我前面的Blog做一个综述，分享在分析的过程中，我的收获。

ubunut20.04 复现lvi-sam 实测

文章目录前言一、ceres1.40安装二、gtsam安装 2.lvisam 修改前言哈哈，这是一篇临时其兴的记录，托了好久，之前是配置了LIOSAM，这次是在ubuntu20.04上配置LV...

Ubuntu20.04部署编译LVI-SAM

基于Ubuntu20.04编译LVI-SAM的步骤及需要注意的点。

【LVI-SAM论文全文翻译】： LVI-SAM: Tightly-coupled Lidar-Visual-Inertial Odometry via Smoothing and Mapping

文章目录写在前面摘要一、引言二、完整的激光视觉惯导SLAM系统 A.系统概述 B.视觉惯导系统 C.雷达惯导系统三、实验 A.消融研究 B.JackalDataset C.HandheldD...

3-LVI-SAM源码分析_utility

2021SC@SDUSC LVISAM源码分析lidarodometry/utility.h 这个头文件虽然不是LVISAM中的核心部分，但却在每个地方都有引入。对于还是初学者的我们来说，还是有必...

多传感器融合 SLAM LVI-SAM

目录 LVISAM简介 A.系统概述 B.视觉惯导系统 C.雷达惯导系统 LVISAM安装编译编译LVISAM 常见问题 LVISAM工程化建议 LVISAM简介源码地址：https://gith...

LVI-SAM编译指南安装详情

github地址： https://github.com/TixiaoShan/LVISAM 大佬已经在github上列出了详细的步骤了，但在我电脑上还有几处报错，那么在此提供给大家：本机ros版本...

是否确定退出登录?

14-[LVI-SAM]visual_odometry_callbackAndprocess

visual_odometry_callback

文章目录

1. *imu_callback

a. 数据校验

b. 保存数据，并通知主线程

c. IMU状态递推，并发布

predict函数

d. 完整代码

2. odom_callback

3. feature_callback

4. restart_callback

5. *主线程process

a. 接受数据，提取measurements数组

getMeasurements()函数

b. IMU预积分

processIMU()函数

c. VINS优化

d. 从激光雷达里程计获取初始化信息

getOdometry()函数

d. 可视化，传输数据给RVIZ

e. 更新IMU参数PQV

update()函数

f. 完整代码

附录：引用

全部评论 (0)

相关文章推荐

14-[LVI-SAM]visual_odometry_callbackAndprocess

13-[LVI-SAM]visual_odometry_初探

11-[LVI-SAM]visual_feature_img_callback浅析

15-[LVI-SAM]分析总结

ubunut20.04 复现lvi-sam 实测

Ubuntu20.04部署编译LVI-SAM

【LVI-SAM论文全文翻译】： LVI-SAM: Tightly-coupled Lidar-Visual-Inertial Odometry via Smoothing and Mapping

3-LVI-SAM源码分析_utility

多传感器融合 SLAM LVI-SAM

LVI-SAM编译指南安装详情

1. *`imu_callback`

2. `odom_callback`

3. `feature_callback`

4. `restart_callback`

`getMeasurements()`函数

`processIMU()`函数

`getOdometry()`函数

`update()`函数