Pywayne Vio Tools

这是一款VIO数据处理工具，核心功能包括SE(3)变换矩阵与[tx,ty,tz,qw,qx,qy,qz]格式位姿的双向互转（支持单条/批量数据），以及带方向箭头的3D位姿轨迹可视化。适用于视觉惯性里程计数据处理、SE(3)位姿分析场景，输入为SE(3)矩阵或标准位姿数组，输出对应转换结果或3D轨迹图。

wangyendt

热度 28.85

最近更新 2026-05-11 17:25:52

分类：数据依赖：代码执行应用场景：数据处理

安装技能

复制下面的话术发给你的 Agent，它会通过 secure-skills 检索并安装该技能。

执行命令安装此 skill： npx secure-skills-cli add tools-2

Metadata

name: pywayne-vio-tools
description: VIO (Visual Inertial Odometry) data processing utilities for SE(3) pose conversion and 3D visualization. Use when working with SE(3) matrices and need pose representation conversion (tx,ty,tz,qw,qx,qy,qz), 3D visualization of pose trajectories with orientation arrows, or Visual Inertial Odometry data transformation and analysis

Pywayne VIO Tools

Overview

Provides utilities for VIO data processing: convert between SE(3) transformation matrices and pose representations, and visualize pose trajectories in 3D with orientation indicators.

Quick Start

from pywayne.vio.tools import SE3_to_pose, pose_to_SE3, visualize_pose
import numpy as np

# Convert SE(3) to pose
SE3 = np.eye(4)  # Single 4x4 SE(3) matrix
pose = SE3_to_pose(SE3)  # Returns [tx, ty, tz, qw, qx, qy, qz]

# Batch conversion
SE3_array = np.random.randn(10, 4, 4)  # 10 SE(3) matrices
poses = SE3_to_pose(SE3_array)

# Convert back to SE(3)
SE3_recon = pose_to_SE3(pose)

# Visualize poses
visualize_pose(poses)  # 3D plot with position markers and orientation arrows

Core Functions

SE3_to_pose

Convert SE(3) transformation matrices to pose representation.

Input: SE3_mat - Single 4x4 SE(3) matrix or array of N SE(3) matrices shape (N, 4, 4)

Output: pose - Array shape (7,) or (N, 7) containing [tx, ty, tz, qw, qx, qy, qz]

Dependencies: qmt.quatFromRotMat() for quaternion extraction

pose_to_SE3

Convert pose representation back to SE(3) transformation matrices.

Input: pose_mat - Single pose shape (7,) or array of N poses shape (N, 7) with [tx, ty, tz, qw, qx, qy, qz]

Output: SE3_mat - Array of SE(3) matrices shape (4, 4) or (N, 4, 4)

Dependencies: qmt.quatToRotMat(), ahrs.Quaternion

visualize_pose

3D visualization of SE(3) poses with position markers and orientation arrows.

Parameters:

poses: Array of poses [tx, ty, tz, qw, qx, qy, qz]
arrow_length_ratio: Scale factor for orientation arrows (default: 0.1)

Visualization:

Black dots: Position (translation)
Red arrow: X-axis orientation
Green arrow: Y-axis orientation
Blue arrow: Z-axis orientation

Dependencies: matplotlib.pyplot, qmt.quatToRotMat()

Data Formats

SE(3) Matrix

[[R00 R01 R02 tx]
 [R10 R11 R12 ty]
 [R20 R21 R22 tz]
 [  0   0   0  1]]

Pose Representation

[tx, ty, tz, qw, qx, qy, qz]

Translation: tx, ty, tz (meters) Quaternion: qw, qx, qy, qz (Hamilton convention)

Dependencies

Required packages:

numpy - Array operations
qmt - Quaternion utilities
ahrs - Quaternion class
matplotlib - 3D visualization

Install with:

pip install numpy qmt ahrs matplotlib