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