后端结构

因此,从本节开始,就进入后端的介绍,文章主要是与代码相结合进行介绍,首先,需要熟悉一下后端节点的接受与发布都有哪些?

image-20220628202953111

了解后端节点的接收与发布,再来熟悉一下VINS后端代码的结构:

image-20220628203324555

至此,我们对后端结构有一个大致的印象。

后端ROS节点函数

同样地,看后端代码也是从他的ROS节点函数estimator_node.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);//设置 logger 级别

    // 2、读取参数,设置状态估计器参数
    readParameters(n);
    estimator.setParam eter();
#ifdef EIGEN_DONT_PARALLELIZE
    ROS_DEBUG("EIGEN_DONT_PARALLELIZE");
#endif
    ROS_WARN("waiting for image and imu...");

    // 3、发布用于 RVIZ 显示的 Topic
    registerPub(n);

    // 4、订阅 IMU、feature、restart、match_points 的 topic,执行各自回调函数

    // 接收IMU消息
    ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());
    // 接收前端视觉光流结果
    ros::Subscriber sub_image = n.subscribe("/feature_tracker/feature", 2000, feature_callback);
    // 接收前端重启命令
    ros::Subscriber sub_restart = n.subscribe("/feature_tracker/restart", 2000, restart_callback);
    // 回环检测的fast relocalization 响应
    ros::Subscriber sub_relo_points = n.subscribe("/pose_graph/match_points", 2000, relocalization_callback);
  
    // 5、创建 VIO 主线程
    std::thread measurement_process{process};
    ros::spin();

    return 0;
}

不难看出,代码的重点就是四个回调函数以及一个VIO主线程,回调函数的作用如下:

  • imu_callback: 将imu消息imu_msg存入imu_buf,同时按照imu频率预测位姿并发送,这样就可以提高里程计频率
  • feature_callback:将前端消息 feature_msg 放入 feature_buf
  • restart_callback:将vins估计器复位, 收到 restart 消息时清空 feature_buf 和 imu_buf,估计器重置,时间重置
  • relocalization_callback:将消息 points_msg 放入 relo_buf

以上四个回调函数只是接收消息,并没有对消息做任何的处理,核心的处理过程,在作者定义的主线程process之中。通过代码,可以看出,这个线程是一直循环往复、永久执行的。

这个主线程和前文的回调函数是”生产者——消费者“模型,如下图所示,回调函数imu_callbackfeature_callback通过接收消息,并将消息分别存入imu_buffeature_buf,而在主线程中则需要将这些数据取出来,所以说前者是生产者,而后者是消费者。这就涉及到一个线程安全的问题。

如果imu数据被取出之后,下一个数据还没进入buf,此时主线程process再去取数据则会导致程序崩溃,所以在回调函数里面进行加锁处理,取一个数据加一次锁,下一个数据进来之后再解锁,这样就解决了没有数据的时候被强行”索要“数据导致程序崩溃的问题;

但是主线程process如何知道开没开锁?难道要不停的访问吗?显然这样很占用CPU,因此,引入了条件变量std::condition_variable con;,它的作用就是一个通信的作用,当生产者有新的数据可以被访问了,就通过条件变量”打电话“通知消费者取数据, con.notify_one();,然后消费者端接收到这个消息之后,(接收”来电“: con.wait)这样就解决了线程安全问题。如果您还是不理解或者想谅解更多,详细介绍请戳>>>

image-20220628211730840

至此,我们已经了解到主线程如何接收数据的了,刚刚提到所有的处理都是主线程进行的,那么,处理什么呢?通过下面的流程图,可以对该主线程做的事情有一个清楚的认识。

VINS_VIO主线程

IMU与图像对齐组合

首先就是IMU与图像进行对齐并组合,主要是函数getMeasurements(),它的原理可以通过下图直观的了解。

image-20220628215540780

如上图所示,作者考虑了三种组合情况,分别对应代码中的三个对齐标准:

  • IMU太慢,等待IMU
//对齐标准:IMU最后一个数据的时间(最新数据)要大于第一个图像特征数据的时间(最老数据)
if (!(imu_buf.back()->header.stamp.toSec() > feature_buf.front()->header.stamp.toSec() + estimator.td))
{
    // ROS_WARN("wait for imu, only should happen at the beginning");
    sum_of_wait++;
    return measurements;
}
  • 图像太老,扔掉老的图片
//对齐标准:IMU第一个数据的时间(最老数据)要小于第一个图像特征数据的时间(最新数据)
if (!(imu_buf.front()->header.stamp.toSec() < feature_buf.front()->header.stamp.toSec() + estimator.td))
{
    ROS_WARN("throw img, only should happen at the beginning");
    feature_buf.pop();
    continue;
}

经过上面两种情况的过滤,则最终得到的就是图像中的第三种情况,此时,由于IMU的频率远大于相机频率,所以使用两帧之间的IMU进行预积分,然后在于图像帧组合,这样就形成了一对(img,IMU)组合的约束。

// 此时就保证了图像一定有imu数据   
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);

对所有(img,IMU)组合一一操作

IMU预积分操作

对与IMU预积分的操作,核心函数是processIMU() ,它实现了 IMU 的预积分,通过中值积分得到当前 PVQ 作为优化初值。

image-20220629113926685

/**
 * @brief 对IMU数据进行处理,包括更新预积分量,提供优化状态量的初始值
 * 
 * @param dt 
 * @param linear_acceleration 
 * @param angular_velocity 
 */
void 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;
    }
    // 滑窗中保留11帧,frame_count 表示当前处理第几帧,一般处理到第11帧就保持不变了
    // 由于预积分是帧间约束,因此第1个预积分量实际上是用不到的
    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)
    {
        // 调用push_back接口,进行预积分操作
        pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
        // if(solver_flag != NON_LINEAR)
            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;
}

重定位(回环)操作

待完成......

视觉与IMU的初始化及紧耦合操作

image-20220629115851374

这部分是视觉与IMU的初始化以及非线性优化的紧耦合操作,而它所涉及的程序是非常庞大的,内容也非常多。将再下一讲细细讲解。

🧐 本文作者:
😉 本文链接:https://lukeyalvin.site/archives/75.html
😊 版权说明:本博客所有文章除特别声明外,均默认采用 CC BY-NC-SA 4.0 许可协议。
最后修改:2022 年 06 月 29 日
赏杯咖啡