OpenVINS代码解析-初探

---

---

1.入口函数（ROS2）

// Create our VIO system
VioManagerOptions params;
params.print_and_load(parser);
params.use_multi_threading_subs = true;
sys = std::make_shared<VioManager>(params);
#if ROS_AVAILABLE == 1
  viz = std::make_shared<ROS1Visualizer>(nh, sys);
  viz->setup_subscribers(parser);
#elif ROS_AVAILABLE == 2
  viz = std::make_shared<ROS2Visualizer>(node, sys);
  viz->setup_subscribers(parser);
#endif

1.1 通过如下方式创建VioManager，此类管理整个VIO系统

sys = std::make_shared<VioManager>(params);

1.2 将sys作为参数，构造ROS2Visualizer接口类，并调用setup_subscribers()进行订阅

viz = std::make_shared<ROS2Visualizer>(node, sys);
viz->setup_subscribers(parser);

接下来具体看看ROS2Visualizer具体做了那些工作

---

2. ROS2Visualizer Class

ROS2Visualizer主要作为ROS接口用来订阅IMU、Camera等数据，同时将VIO处理的中间结果进行发布，方便可视化。

2.1 主要成员变量（以单目模式为例）

/// Global node handler
std::shared_ptr<rclcpp::Node> _node;
/// Core application of the filter system
std::shared_ptr<VioManager> _app;

rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub_imu;
std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr>
      subs_cam;
/// Queue up camera measurements sorted by time and trigger once we have
/// exactly one IMU measurement with timestamp newer than the camera
/// measurement This also handles out-of-order camera measurements, which is
/// rare, but a nice feature to have for general robustness to bad camera
/// drivers.
std::deque<ov_core::CameraData> camera_queue;
std::mutex camera_queue_mtx;
// Last camera message timestamps we have received (mapped by cam id)
std::map<int, double> camera_last_timestamp;

2.1 setup_subscribers()进行订阅

• 订阅IMU数据，调用callback_inertial()回调函数

sub_imu = _node->create_subscription<sensor_msgs::msg::Imu>(
      topic_imu, rclcpp::SensorDataQoS(),
      std::bind(&ROS2Visualizer::callback_inertial, this,
                std::placeholders::_1));

• 单目模式订阅Camera数据，支持多相机

    // Now we should add any non-stereo callbacks here
    for (int i = 0; i < _app->get_params().state_options.num_cameras; i++) {
      // read in the topic
      std::string cam_topic;
      _node->declare_parameter<std::string>(
          "topic_camera" + std::to_string(i),
          "/cam" + std::to_string(i) + "/image_raw");
      _node->get_parameter("topic_camera" + std::to_string(i), cam_topic);
      parser->parse_external("relative_config_imucam",
                             "cam" + std::to_string(i), "rostopic", cam_topic);
      // create subscriber
      // auto sub = _node->create_subscription(
      //    cam_topic, rclcpp::SensorDataQoS(),
      //    std::bind(&ROS2Visualizer::callback_monocular, this,
      //    std::placeholders::_1, i));
      auto sub = _node->create_subscription<sensor_msgs::msg::Image>(
          cam_topic, 10,
          [this, i](const sensor_msgs::msg::Image::SharedPtr msg0) {
            callback_monocular(msg0, i);
          });
      subs_cam.push_back(sub);
      PRINT_INFO("subscribing to cam (mono): %s\n", cam_topic.c_str());
    }

接下来看看两个回调函数分别做了什么

callback_monocular()

// Check if we should drop this image
  double timestamp = msg0->header.stamp.sec + msg0->header.stamp.nanosec * 1e-9;
  double time_delta = 1.0 / _app->get_params().track_frequency;
  if (camera_last_timestamp.find(cam_id0) != camera_last_timestamp.end() &&
      timestamp < camera_last_timestamp.at(cam_id0) + time_delta) {
    return;
  }
  camera_last_timestamp[cam_id0] = timestamp;

  // Get the image
  cv_bridge::CvImageConstPtr cv_ptr;
  try {
    cv_ptr = cv_bridge::toCvShare(msg0, sensor_msgs::image_encodings::MONO8);
  } catch (cv_bridge::Exception &e) {
    PRINT_ERROR("cv_bridge exception: %s", e.what());
    return;
  }

  // Create the measurement
  ov_core::CameraData message;
  message.timestamp =
      cv_ptr->header.stamp.sec + cv_ptr->header.stamp.nanosec * 1e-9;
  message.sensor_ids.push_back(cam_id0);
  message.images.push_back(cv_ptr->image.clone());

  // Load the mask if we are using it, else it is empty
  // TODO(someone): in the future we should get this from external pixel
  // segmentation
  if (_app->get_params().use_mask) {
    message.masks.push_back(_app->get_params().masks.at(cam_id0));
  } else {
    message.masks.push_back(
        cv::Mat::zeros(cv_ptr->image.rows, cv_ptr->image.cols, CV_8UC1));
  }

  // append it to our queue of images
  std::lock_guard<std::mutex> lck(camera_queue_mtx);
  camera_queue.push_back(message);
  std::sort(camera_queue.begin(), camera_queue.end());

callback_inertial()

首先将ROS格式的IMU数据转换为OpenVINS自定义的数据格式。

// convert into correct format
ov_core::ImuData message;
message.timestamp = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9;
message.wm << msg->angular_velocity.x, msg->angular_velocity.y,
msg->angular_velocity.z;
message.am << msg->linear_acceleration.x, msg->linear_acceleration.y,msg->linear_acceleration.z;

紧接着将IMU数据喂到VioManager中

_app->feed_measurement_imu(message);

其次，创建新的Thread，并找到与之对应延迟的相机数据，将相机数据输入VioManager尝试进行更新。

std::thread thread([&] {
    // Lock on the queue (prevents new images from appending)
    std::lock_guard<std::mutex> lck(camera_queue_mtx);

    // Count how many unique image streams
    std::map<int, bool> unique_cam_ids;
    for (const auto &cam_msg : camera_queue) {
      unique_cam_ids[cam_msg.sensor_ids.at(0)] = true;
    }

    // If we do not have enough unique cameras then we need to wait
    // We should wait till we have one of each camera to ensure we propagate in
    // the correct order
    auto params = _app->get_params();
    size_t num_unique_cameras = (params.state_options.num_cameras == 2)
                                    ? 1
                                    : params.state_options.num_cameras;
    if (unique_cam_ids.size() == num_unique_cameras) {
      // Loop through our queue and see if we are able to process any of our
      // camera measurements We are able to process if we have at least one IMU
      // measurement greater than the camera time
      double timestamp_imu_inC =
          message.timestamp - _app->get_state()->_calib_dt_CAMtoIMU->value()(0);
      while (!camera_queue.empty() &&
             camera_queue.at(0).timestamp < timestamp_imu_inC) {
        auto rT0_1 = boost::posix_time::microsec_clock::local_time();
        double update_dt =
            100.0 * (timestamp_imu_inC - camera_queue.at(0).timestamp);
        _app->feed_measurement_camera(camera_queue.at(0));
        visualize();
        camera_queue.pop_front();
        auto rT0_2 = boost::posix_time::microsec_clock::local_time();
        double time_total = (rT0_2 - rT0_1).total_microseconds() * 1e-6;
        PRINT_INFO(
            BLUE "[TIME]: %.4f seconds total (%.1f hz, %.2f ms behind)\n" RESET,
            time_total, 1.0 / time_total, update_dt);
      }
    }
    thread_update_running = false;
  });