LVI-SAM 代码学习-2

最新推荐文章于 2023-04-14 19:01:34 发布

wangwangwang001

最新推荐文章于 2023-04-14 19:01:34 发布

阅读量382

点赞数

分类专栏：多传感器融合SLAM 文章标签：自动驾驶人工智能机器学习

本文链接：https://blog.csdn.net/jyb1234/article/details/121052774

版权

多传感器融合SLAM 专栏收录该内容

2 篇文章 0 订阅

订阅专栏

视觉里程计中的visual_estimator

main函数：

在main函数中接收了imu数据，lio部分预积分结果，视觉特征点和特征跟踪重启标志

ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 5000, imu_callback, ros::TransportHints().tcpNoDelay());
ros::Subscriber sub_odom = n.subscribe("odometry/imu", 5000, odom_callback);
ros::Subscriber sub_image = n.subscribe(PROJECT_NAME + "/vins/feature/feature", 1, feature_callback);
ros::Subscriber sub_restart = n.subscribe(PROJECT_NAME + "/vins/feature/restart", 1, restart_callback);

imu_callback：

把imu数据加入imu_buf中，使用中值积分预测后续的位置

  // 判断时间间隔是否为正
  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();  // 唤醒作用于process线程

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

在pubLatestOdometry中

1.发布名为PROJECT_NAME + "/vins/odometry/imu_propagate_ros"的topic，表示vins_body在vins_world坐标系中的位置

2.发布一个vins_world->vins_body_ros的tf，提供给点云深度配准使用

3.通过监听odom和base_link之间的转换，计算出odom和vins_world坐标系之间的转换，让激光和相机的坐标系可以对齐

未完待续····