14-[LVI-SAM]visual_odometry_callbackAndprocess_[&] { return (measurements = getmeasurements()).si

2021SC@SDUSC

visual_odometry_callback

​ 在上一章，我们大致把visual_odometry的main函数过了一遍，把节点在生成回调函数之前的准备工作介绍了一遍。我们已经大致了解了节点的输入和输出。其实，通过上一章的分析，可以发现，这个节点和我之前分析的visual_feature的关联还是比较紧密，需要用到visual_feature发布的视觉特征点。

​ 下面，我们开始分析这个节点最重要的回调函数部分，以及主线程process。一共有4个，分别是：imu_callback，feature_callback，odom_callback，restart_callback。

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

1. *imu_callback

​ IMU回调函数，将imu_msg保存到imu_buf，IMU状态递推并发布[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();
1
2
3
4
5
6
7

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

​ 通过加锁，防止多线程访问IMU数据缓存队列出现问题。并在取出数据之后，通知主线程process，从阻塞状态唤醒。唤醒的具体功能，见下面注释。

m_buf.lock(); 
imu_buf.push(imu_msg);
m_buf.unlock();
con.notify_one(); // 唤醒作用于process线程中的获取观测值数据的函数

last_imu_t = imu_msg->header.stamp.toSec(); // 不知道为啥又加了一次赋值操作
1
2
3
4
5
6

c. IMU状态递推，并发布

​ 首先，所有代码都被一个大括号包围住，表示这是一个作用域。那为什么要这么做呢？其实是因为里面的第一行的互斥锁lock_guard。当第一行代码执行的时候，相当于加了锁，当离开这个作用域的时候，临时变量会释放，而lock_guard析构的时候就会解锁。这相当于一个编程的小技巧。

​ 接着执行predict函数，这个函数的作用是从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);
}
1
2
3
4
5
6
7
8
9
10

predict函数

​ 从IMU测量值imu_msg和上一个PQV递推得到下一个tmp_Q，tmp_P，tmp_V。下面的代码就是使用积分中值定理得到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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39

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);
    }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

2. odom_callback

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

void odom_callback(const nav_msgs::Odometry::ConstPtr& odom_msg)
{
    m_odom.lock();
    odomQueue.push_back(*odom_msg);
    m_odom.unlock();
}
1
2
3
4
5
6

3. feature_callback

​ 这个函数也较为简单，先验证是否是第一帧，如果是的话，不处理。因为我前面的blog有分析到，我们对于第一帧图像是认为没有光流速度的，所以，没有办法追踪并找到特征点。后面的话，也是把消息放到缓冲区内，然后，也需要和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();
}
1
2
3
4
5
6
7
8
9
10
11
12
13

4. restart_callback

​ 这里的话，是会从visual_feature节点可能会接受到重启的消息，我之前的blog也有分析到，如果相机的数据流不稳定，是需要发布restart消息。这里可以看我的第11篇blog。然后再重启之前，我们需要清空缓冲区，并且重置所有数据和状态。

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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

5. *主线程process

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

​ process里面只有一个while循环，运行条件为ros::ok()。因此，如果当前节点正在运行，ros::ok()为true，为无限循环。

​ 接下来是while循环里面的功能。首先是等待并获取measurements：(IMUs, img_msg)s，计算dt。接着使用，estimator.processIMU()进行IMU预积分。然后，通过estimator.setReloFrame()设置重定位帧。最后，estimator.processImage()处理图像帧：初始化，紧耦合的非线性优化。

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

​ 一开始，先创建临时变量，用来保存提取到的measurements。然后，对m_buf创建唯一锁，防止访问共享内存时出现问题。然后进入陷入等待。如果唤醒了，会给m_buf加上锁，然后执行getMeasurements函数，获得相关的值，然后判断取出来的measurement的size值是不是不等于0，如果不等于0，则继续往下执行。如果等于0，则继续挂起。

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();
1
2
3
4
5
6
7

getMeasurements()函数

​ 这里的返回值是数据对数组。其中数组对中的第一个数据为IMU数组，第二个数据为img_msg。这里将缓冲区imu_buf和feature_buf的数据都取出来。除此之外，还让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 (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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41

b. IMU预积分

​ 下面开始正式的处理。首先会加上m_estimator.lock()，这里是为了防止正在执行主线程的时候，突然visual_feature节点突然来了一个重启信号，导致同时访问接下来操作要访问到的共享缓冲区以及共享变量。然后，接着为一个for-each操作，把measurements数组的每一个元素取出来，然后循环执行for里面的内容。这一部分的代码就不单独显示，会放在后面的完整代码里面。

​ 接着就是对每一个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);
    }
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47

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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36

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);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

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

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

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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85

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" 关键帧位姿和点云
1
2
3
4
5
6
7

e. 更新IMU参数PQV

m_buf.lock();
m_state.lock();
if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
    update(); // 更新操作
m_state.unlock();
m_buf.unlock();
1
2
3
4
5
6

update()函数

​ 从estimator中得到滑动窗口当前图像帧的imu更新项[P,Q,V,ba,bg,a,g]。然后对imu_buf中剩余的imu_msg进行PVQ递推。predict函数在上面有分析过。至此，主线程函数分析完成！

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());
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

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();
    }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109

附录：引用

LVI-SAM代码– xuwuzhou的SLAM之路 – 静心，慎思，明辨，笃学

ManiiXu/VINS-Mono-Learning: VINS-Mono