OpenVINS Note

[TOC]

Overview

• docs: https://docs.openvins.com/

• code: https://github.com/rpng/open_vins

• paper

  @Conference{Geneva2020ICRA,
  Title      = {OpenVINS: A Research Platform for Visual-Inertial Estimation},
  Author     = {Patrick Geneva and Kevin Eckenhoff and Woosik Lee and Yulin Yang and Guoquan Huang},
  Booktitle  = {Proc. of the IEEE International Conference on Robotics and Automation},
  Year       = {2020},
  Address    = {Paris, France},
  Url        = {\url{https://github.com/rpng/open_vins}}
  }

Project Features

• Sliding window visual-inertial MSCKF

• First-Estimate Jacobian Estimators

  • OC-EKF1

• Modular covariance type system

  • Inspired by graph-based optimization frameworks such as Georgia Tech Smoothing and Mapping library (GTSAM), the included filter has modularity allowing for convenient covariance management with a proper type-based state system.

    class Type {
    protected:
        // Current best estimate
        Eigen::MatrixXd _value;
        // Location of error state in covariance
        int _id = -1;
        // Dimension of error state
        int _size = -1;
    };

• Different feature representations (ref: https://zhuanlan.zhihu.com/p/94380129)

  • 直角坐标系
    • Global XYZ
    • Anchored XYZ
  • 球坐标系
    • Global inverse depth
    • Anchored inverse depth
  • 逆深度 + normalized UV: Anchored MSCKF inverse depth
  • 逆深度 + Bearing Vector: Anchored single inverse depth

• Calibration of sensor intrinsics and extrinsics

  • Camera to IMU transform: state->_calib_IMUtoCAM
  • Camera to IMU time offset: state->_calib_dt_CAMtoIMU $$t_{imu} = t_{cam} + t_d$$
  • Camera intrinsics: state->_cam_intrinsics

• Environmental SLAM feature

  • OpenCV ARUCO tag SLAM features
    • class TrackAruco
  • Sparse feature SLAM features

• Visual tracking support

  • Monocular camera
  • Stereo camera
  • Binocular camera

• KLT or descriptor based

• Static IMU initialization (sfm will be open sourced later)

• Zero velocity detection and updates (ZUPT)

  • UpdaterZeroVelocity::try_update()

• Extendable visual-inertial simulator

  • On manifold SE(3) B-Spline
  • Arbitrary number of cameras
  • Arbitrary sensor rate
  • Automatic feature generation

• continuous preintegration integrators

  • CpiBase, CpiV1, CpiV2

• Out of the box evaluation on EurocMav and TUM-VI datasets

• Extensive evaluation suite (ATE, RPE, NEES, RMSE, etc..)

• Others

  • Givens Rotation
    • nullspace_project_inplace
    • measurement_compress_inplace
  • Diagonal Matrix
    • triangularView

monocular VIO Code Process

Start

• init

  VioManagerOptions params = parse_ros_nodehandler(nh);
  sys = std::make_shared<VioManager>(params);
  viz = std::make_shared<RosVisualizer>(nh, sys);

• callback loop

  • callback_inertial: sys->feed_measurement_imu(message);
  • callback_monocular: sys->feed_measurement_camera(message);

VioManager::feed_measurement_camera

void feed_measurement_camera(const ov_core::CameraData &message) {
    camera_queue.push_back(message);
    std::sort(camera_queue.begin(), camera_queue.end());
}

VioManager::feed_measurement_imu

feed_imu:

• propagator
• initializer
• updaterZUPT

VioManager::track_image_and_update

• image downsample

• ZUPT update: updaterZUPT->try_update()

• feature tracking

  • trackFEATS->feed_monocular()
  • trackDATABASE->append_new_measurements(trackFEATS->get_feature_database());

• init vio

  if(!is_initialized_vio) {
    is_initialized_vio = try_to_initialize();
    if(!is_initialized_vio) return;
  }

• propagate and update: do_feature_propagate_update(message);

  • propagate and clone: propagator->propagate_and_clone(state, message.timestamp);

    • Propagator::select_imu_readings(imu_data,time0,time1);
      • IMU linear interpolation: Propagator::interpolate_data()
    • predict_and_compute(): $F, Q_d$
      • predict state: predict_mean_rk4 or predict_mean_discrete
      • $F, G$
      • $Q_c, Q_d$ $$P^{\prime} = F_d P F_d^T + G_d Q_d G_d^T$$
    • StateHelper::EKFPropagation $$\mathbf{P}_{k+1|k} = \begin{pmatrix} \mathbf{P}_{II_{k+1|k}} & \boldsymbol{\Phi}_k \mathbf{P}_{IC_{k|k}} \\ \mathbf{P}_{IC_{k|k}}^\top \boldsymbol{\Phi}_k^\top & \mathbf{P}_{CC_{k|k}} \end{pmatrix}$$
    • stochastic cloning: StateHelper::augment_clone(state, last_w);

  • get features

    • MSCKF features
    • SLAM features

  • EKF update for MSCKF and SLAM

    • updaterMSCKF->update(state, featsup_MSCKF);
      • UpdaterHelper::get_feature_jacobian_full(state, feat, H_f, H_x, res, Hx_order);
      • UpdaterHelper::nullspace_project_inplace(H_f, H_x, res);
      • Chi2 distance check
      • UpdaterHelper::measurement_compress_inplace(Hx_big, res_big);
      • StateHelper::EKFUpdate()
    • updaterSLAM->update(state, featsup_TEMP);
      • StateHelper::EKFUpdate()
    • updaterSLAM->delayed_init(state, feats_slam_DELAYED);

  • calib cam intrinsics

    • do_calib_camera_intrinsics()

  • retriangulate_active_tracks(message);

  • cleanup

  • updaterSLAM->change_anchors(state);

  • marginalize

    • StateHelper::marginalize_old_clone(state);

Build

• with ROS

  catkin_make -j2

Run

• VIO

  roslaunch ov_msckf pgeneva_ros_eth.launch

  

• VI-Simulator

  roslaunch ov_msckf pgeneva_sim.launch [dosave_pose:=true]

• Evaluation (from https://github.com/uzh-rpg/rpg_trajectory_evaluation)

  rosrun ov_eval plot_trajectories se3 traj_groundtruth.txt traj_estimate.txt