Skip to main content

Documentation Index

Fetch the complete documentation index at: https://mintlify.com/rpng/open_vins/llms.txt

Use this file to discover all available pages before exploring further.

OpenVINS is designed as a modular research platform. The core filter exposed through VioManager is intentionally decoupled from any particular ROS layer, making it straightforward to build external systems on top of it. The RPNG group maintains four official extension repositories that demonstrate how OpenVINS can be extended for plane-aided odometry, groundtruth generation, large-scale mapping, and loop closure.

Official Extension Projects

ov_plane

Plane-aided visual-inertial odometryA real-time monocular VIO system that detects and tracks environmental planes without requiring a stereo camera, depth sensor, or neural network. Detected planes are either maintained in the state vector as long-lived landmarks or marginalized for efficiency. Planar regularities are applied to both in-state SLAM features and out-of-state MSCKF point features, enabling long-term point-to-plane loop-closures over large spatial volumes.Released April 2023 alongside the Indoor AR Table dataset.

vicon2gt

Groundtruth trajectory generation from motion captureGenerates high-accuracy groundtruth IMU state trajectories using a motion capture system (Vicon or OptiTrack). Computes the full 15-DOF inertial state at camera frame rate and produces groundtruth files in the same format as the EuRoC MAV dataset. Fuses inertial and motion capture data and estimates all unknown spatial-temporal calibrations between the sensors.Released November 2020.

ov_maplab

maplab interface for batch offline optimizationExports visual-inertial runs from OpenVINS into the ViMap structure used by maplab. State estimates and raw images are appended to the ViMap as OpenVINS processes a dataset. After the run, features are re-extracted and triangulated using maplab’s feature system. Supports multi-session map merging and batch trajectory optimization.Released August 2019.

ov_secondary

Loosely-coupled loop closure secondary threadA secondary pose graph thread that detects loop closures and corrects accumulated drift, adapted from the VINS-Fusion loop closure module. Operates in a loosely-coupled manner — no correction information flows back into the core OpenVINS filter. Improvements over the upstream code include exposed loop closure parameters, camera intrinsics subscription, simplified topic-only configuration, and improved detection frequency.Released May 2020.

Extension Details

ov_plane — Plane-Aided VIO

ov_plane extends OpenVINS to exploit structural regularities in man-made environments. The plane detection algorithm runs in real time on monocular images. Detected planes are tracked across frames and can be:
  • Maintained in state as long-lived plane landmarks participating in EKF updates
  • Marginalized when their contribution to efficiency is outweighed by compute cost
Point features are regularized to detected planes, providing an implicit form of long-term loop closure even without an explicit pose graph. This is particularly effective in structured indoor environments such as offices and warehouses. Use case: indoor mobile robotics, AR, or any scenario where planar surfaces dominate the scene.

vicon2gt — Groundtruth Generation

Evaluating VIO systems requires high-quality groundtruth trajectories. vicon2gt bridges the gap between raw motion capture marker data and the format expected by standard evaluation tools. The utility:
  1. Fuses Vicon/OptiTrack 6-DOF pose streams with IMU readings
  2. Estimates unknown time offsets and spatial calibration between the motion capture and IMU frames
  3. Outputs a full 15-DOF trajectory (position, orientation, velocity, biases) at camera frame rate
The output format matches EuRoC MAV groundtruth CSVs, allowing direct use with ov_eval. Use case: researchers setting up their own capture environments who need to generate dataset groundtruth comparable to publicly available benchmarks.

ov_maplab — Offline Batch Optimization

While OpenVINS produces real-time state estimates, ov_maplab enables post-hoc refinement using maplab’s batch optimizer. The workflow is:
  1. Run OpenVINS online through a dataset; ov_maplab records state estimates and images into a ViMap
  2. After the run completes, maplab re-extracts and triangulates features using its own feature pipeline
  3. Batch optimization refines the trajectory and map jointly
This is useful for generating high-quality maps from multiple sessions or for producing a reference trajectory against which real-time results can be compared. Use case: multi-session mapping, place recognition research, or any application requiring a globally consistent offline map.

ov_secondary — Loop Closure Thread

ov_secondary subscribes to the marginalized feature tracks, 3D feature positions, and camera calibration topics that OpenVINS publishes (added in PR#66). These are fed into a DBoW2-based loop closure detector. When a loop is detected, a pose graph optimization corrects the estimated trajectory.
Because ov_secondary is loosely coupled, the corrected trajectory is produced as a separate output. The core OpenVINS filter continues to run independently and is not updated with the loop closure correction. This keeps the filter consistent but means the online odometry output does not benefit from loop closures.

Building Your Own Extension

OpenVINS is structured so that external packages can depend on ov_msckf as a library without modifying the core code.

VioManager API

The primary integration point is ov_msckf::VioManager. Your extension node creates a VioManager instance, feeds it sensor data, and reads state estimates back out.

Constructor

#include "core/VioManager.h"
#include "core/VioManagerOptions.h"

// Load parameters (from YAML or ROS param server)
ov_msckf::VioManagerOptions params;
// ... populate params ...

auto vio = std::make_shared<ov_msckf::VioManager>(params);

Feeding Measurements

// IMU measurement
ov_core::ImuData imu_msg;
imu_msg.timestamp = t;
imu_msg.wm = gyro;   // Eigen::Vector3d angular velocity
imu_msg.am = accel;  // Eigen::Vector3d linear acceleration
vio->feed_measurement_imu(imu_msg);

// Camera measurement (monocular or stereo)
ov_core::CameraData cam_msg;
cam_msg.timestamp = t;
cam_msg.sensor_ids = {0};     // camera IDs
cam_msg.images = {cv_image};  // std::vector<cv::Mat>
vio->feed_measurement_camera(cam_msg);

Reading State and Features

if (vio->initialized()) {
  // Current filter state
  auto state = vio->get_state();

  // 3D SLAM features in global frame
  auto slam_feats = vio->get_features_SLAM();

  // Features used in the last MSCKF update
  auto msckf_feats = vio->get_good_features_MSCKF();

  // Active tracked features (for downstream use, e.g. loop closure)
  double active_time;
  std::unordered_map<size_t, Eigen::Vector3d> feat_posinG, feat_uvd;
  vio->get_active_tracks(active_time, feat_posinG, feat_uvd);

  // Visualization image with drawn tracks
  cv::Mat viz = vio->get_historical_viz_image();
}

Full Public API Summary

MethodDescription
feed_measurement_imu(ImuData)Ingest an IMU sample
feed_measurement_camera(CameraData)Ingest camera frame(s) and trigger update
feed_measurement_simulation(...)Ingest simulated feature observations
initialize_with_gt(imustate)Bypass initializer with known groundtruth state
initialized()Returns true once the filter is running
initialized_time()Timestamp of filter initialization
get_params()Returns a copy of the current parameter struct
get_state()shared_ptr to the full filter State
get_propagator()shared_ptr to the Propagator
get_features_SLAM()3D SLAM landmark positions (global frame)
get_features_ARUCO()3D ARUCO tag positions (global frame)
get_good_features_MSCKF()Features used in the most recent update
get_active_tracks(...)All currently tracked features with positions and image coordinates
get_active_image(...)The image frame used for active track projection
get_historical_viz_image()Visualization image with overlaid feature tracks

CMake Integration

Add ov_msckf as a dependency in your extension’s CMakeLists.txt:
find_package(ov_msckf REQUIRED)

add_executable(my_extension src/my_extension.cpp)
ament_target_dependencies(my_extension ov_msckf rclcpp sensor_msgs)

Topics Published by OpenVINS (for Loosely-Coupled Extensions)

OpenVINS publishes the following topics that external packages like ov_secondary subscribe to:
TopicTypeDescription
/ov_msckf/odomimunav_msgs/OdometryCurrent IMU state estimate
/ov_msckf/points_slamsensor_msgs/PointCloud2Active SLAM feature positions
/ov_msckf/points_msckfsensor_msgs/PointCloud2MSCKF features from last update
/ov_msckf/points_activesensor_msgs/PointCloud2All currently tracked features (retriangulated)
/ov_msckf/loop_featssensor_msgs/PointCloud2Marginalized feature tracks for loop closure
/ov_msckf/cam0/track_imagesensor_msgs/ImageVisualization of tracked features
For new extensions, prefer subscribing to /ov_msckf/points_active for the most complete and up-to-date pointcloud. This is populated by the retriangulate_active_tracks call that runs every frame.

Build docs developers (and LLMs) love