Singleton class for publishing VIO data. More...

#include <VoxlHK.h>

Public Member Functions
	Publisher (const Publisher &)=delete

Publisher &	operator= (const Publisher &)=delete

void	start ()
	Start the publisher.

void	publish (std::shared_ptr< ov_msckf::State > state, const std::map< double, std::vector< std::shared_ptr< ov_core::Feature > > > &used_features_map={})
	Publish VIO data.

void	stop ()
	Stop the publisher.

bool	should_auto_reset (std::shared_ptr< ov_msckf::State > state, int quality, int n_features, double yawrate, double current_velocity, double vel_x, double vel_y)
	Check if auto-reset should be triggered.

double	calcQuality (const std::map< double, std::vector< std::shared_ptr< ov_core::Feature > > > &used_features_map, std::unordered_map< size_t, std::shared_ptr< ov_type::Landmark > > &slam_features, std::shared_ptr< ov_msckf::State > state)
	Calculate Quality of the VIO state (async-aware per-camera freshness ledger)

void	set_first_packet (bool first_packet_)
	Set the first packet flag.

void	publishBlank ()

Static Public Member Functions
static Publisher &	getInstance ()
	Get singleton instance.

static void	ov_vio_control_pipe_cb (int ch, char string, int bytes, void context)
	Control pipe callback function.

Detailed Description

Singleton class for publishing VIO data.

This class manages the publication of VIO data to external systems through pipe interfaces. It implements the singleton pattern to ensure only one instance exists throughout the application lifecycle.

The publisher handles:

VIO state data formatting
Track base information
Coordinate frame transformations
Data packet generation and transmission

Definition at line 130 of file VoxlHK.h.

Member Function Documentation

◆ calcQuality()

double Publisher::calcQuality	(	const std::map< double, std::vector< std::shared_ptr< ov_core::Feature > > > &	used_features_map,
		std::unordered_map< size_t, std::shared_ptr< ov_type::Landmark > > &	slam_features,
		std::shared_ptr< ov_msckf::State >	state
	)

Calculate Quality of the VIO state (async-aware per-camera freshness ledger)

Pools all used-feature entries from the last QUAL_UNION_WINDOW_S (dedup by featid) and scores each camera over the pooled set with the unchanged synced-path grid metric (5x5 grid, SLAM features weighted by covariance largest eigenvalue + quality field, MSCKF by quality field + track length), sum-then-clamp as sync always fused. The union gives each camera its full recent track set – one update's used list is a sparse, noisy sample – so async ticks score like synced batches; a time-constant EMA smooths the published value against single weak updates. Floor is 10 while running (the metric alone must not trip the sustained quality<1 auto-reset timers); resets/FAILED force 0/-1 in publish() itself.

Must only be called from the VIO thread (publish); uses the qual_ema state with no locks by design.

Parameters

used_features_map	Map of used features keyed by update (epoch) timestamp
slam_features	Map of SLAM features from the state
state	Current VIO state for covariance access

Returns: Quality score (10-100 while running, higher is better)

Definition at line 1541 of file VoxlPublisher.cpp.

References vio_manager.

Referenced by publish().

Here is the caller graph for this function:

◆ getInstance()

static Publisher & voxl::Publisher::getInstance ( )

inlinestatic

Get singleton instance.

Returns: Reference to the singleton Publisher instance

Definition at line 137 of file VoxlHK.h.

Referenced by main(), and voxl::register_vio_camera_callback().

Here is the caller graph for this function:

◆ ov_vio_control_pipe_cb()

void Publisher::ov_vio_control_pipe_cb	(	int	ch,
		char *	string,
		int	bytes,
		void *	context
	)

static

Control pipe callback function.

Control-pipe callback for VIO commands.

Callback function to handle control pipe messages

This function is invoked every time a message is received on the VIO control pipe.

Parameters

ch	Channel id supplied by the pipe framework.
string	Pointer to the received buffer.
bytes	Number of valid bytes in `string`.
context	User context pointer supplied during registration.

Note: Matching is performed with the STR_MATCH() macro, which compares the prefix of string against the command literal.

Definition at line 93 of file VoxlPublisher.cpp.

References is_resetting, reset_requested, and soft_reset_requested.

◆ publish()

void Publisher::publish	(	std::shared_ptr< ov_msckf::State >	state,
		const std::map< double, std::vector< std::shared_ptr< ov_core::Feature > > > &	used_features_map = `{}`
	)

Publish VIO data.

Publish VIO data to external systems.

Publishes the current VIO state and tracking information to external systems through the configured pipe interfaces.

Parameters

state	Current VIO state
used_features_map	Used-features map, by reference (no copy). The caller runs on the VIO thread synchronously after the update, so the reference stays valid for the whole call; publish must never re-enter feed/drain.

This method formats and publishes the current VIO state and tracking information to external systems through the configured pipe interfaces.

The function performs the following operations:

Formats VIO data packet with current state information
Performs coordinate frame transformations (OpenVINS to FRD)
Calculates angular velocity from quaternion differences
Extracts and formats covariance matrices
Handles camera-to-IMU extrinsic parameters
Publishes both simple and extended VIO data packets

The coordinate frame transformation involves:

Converting from OpenVINS coordinate frame to Front-Right-Down (FRD)
Handling initialization state with NED rotation zeroing
Applying proper quaternion and rotation matrix transformations

Parameters

state Current VIO state containing pose, velocity, and covariance

Definition at line 151 of file VoxlPublisher.cpp.

References alt_z, auto_reset_max_v_cov_timeout_s, auto_reset_max_velocity, auto_reset_min_feature_timeout_s, auto_reset_min_features, calcQuality(), voxl::dirtyOmega(), en_debug, EXTENDED_CH, fast_yaw_thresh, fast_yaw_timeout_s, frame_transform, has_acc_jerk, is_resetting, non_static, ok_state_grace_timeout_s, quality_bad_to_good_count, quality_good_to_bad_count, quality_high_thresh, quality_initial_to_bad_count, quality_initial_to_good_count, quality_low_thresh_good, quality_low_thresh_initial, voxl::R_OV_FRD(), reset_num_counter, reset_requested, should_auto_reset(), SIMPLE_CH, vio_error_codes, vio_manager, and vio_state.

Referenced by voxl::register_vio_camera_callback().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ publishBlank()

void Publisher::publishBlank ( )

Definition at line 1654 of file VoxlPublisher.cpp.

◆ set_first_packet()

void voxl::Publisher::set_first_packet ( bool first_packet_ )

inline

Set the first packet flag.

This function sets the first packet flag to the provided value.

Parameters

first_packet The value to set the first packet flag to

Definition at line 237 of file VoxlHK.h.

◆ should_auto_reset()

bool Publisher::should_auto_reset	(	std::shared_ptr< ov_msckf::State >	state,
		int	quality,
		int	n_features,
		double	yawrate,
		double	current_velocity,
		double	vel_x,
		double	vel_y
	)

Check if auto-reset should be triggered.

Evaluates current VIO state and error conditions to determine if an automatic reset should be triggered.

Parameters

state	Current VIO state
quality	Current quality value
n_features	Number of tracked features
yawrate	Calculated yaw rate from angular velocity
current_velocity	Current velocity magnitude
vel_x	X-component of velocity
vel_y	Y-component of velocity

Returns: true if auto-reset should be triggered, false otherwise

This function evaluates the current VIO state and various error conditions to determine if an automatic reset should be triggered. It implements the same logic as the legacy code but in a more modular way.

Parameters

state	Current VIO state
quality	Current quality value
n_features	Number of tracked features

Returns: true if auto-reset should be triggered, false otherwise

Definition at line 1147 of file VoxlPublisher.cpp.

References auto_reset_max_v_cov_timeout_s, auto_reset_max_velocity, auto_reset_min_feature_timeout_s, auto_reset_min_features, en_auto_reset, en_debug, fast_yaw_thresh, fast_yaw_timeout_s, and reset_num_counter.

Referenced by publish().

Here is the caller graph for this function:

◆ start()

void Publisher::start ( )

Start the publisher.

Initializes the publisher and prepares it for data transmission.

Initializes the publisher and prepares it for data transmission. Sets the first_packet flag to true to handle the initial angular velocity calculation.

Definition at line 53 of file VoxlPublisher.cpp.

References voxl::HealthCheck::getInstance(), voxl::HealthCheck::start(), and vio_state.

Referenced by main().

Here is the call graph for this function: