AgentSkillsCN

pywayne-vio-tools

VIO（视觉惯性里程计）数据处理实用工具，可用于 SE(3) 位姿转换与 3D 可视化。当您需要处理 SE(3) 矩阵，并进行位姿表示转换（tx、ty、tz、qw、qx、qy、qz）、以方向箭头可视化位姿轨迹，或对视觉惯性里程计数据进行转换与分析时，此工具包将助您一臂之力。

SKILL.md
--- frontmatter
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

python
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

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

Pose Representation

code
[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:

bash
pip install numpy qmt ahrs matplotlib