VIO 工具函数 (tools)

pywayne.vio.tools.SE3_to_pose(SE3_mat: ndarray) → ndarray: 将SE3矩阵转换为pose矩阵 :param SE3_mat: N个SE(3) :return: N个pose, tx, ty, tz, qw, qx, qy, qz

pywayne.vio.tools.pose_to_SE3(pose_mat: ndarray) → ndarray: 将pose矩阵转换为SE3矩阵 :param pose_mat: N个pose, shape: (N, 7), tx, ty, tz, qw, qx, qy, qz :return: N个SE(3)

pywayne.vio.tools.visualize_pose(poses, arrow_length_ratio=0.1)

Initialize the PoseVisualizer with a set of poses and scale factors for arrows.

Parameters:

poses – A list or array of SE(3) poses, each defined as (tx, ty, tz, qw, qx, qy, qz).
arrow_length_ratio – A scalar to determine the length of the arrows relative to the data range.

该模块提供了一些用于视觉惯性里程计 (Visual Inertial Odometry, VIO) 数据处理与分析的工具函数，主要包含以下功能：

将 SE(3) 矩阵转换为 pose 表示
将 pose 表示转换为 SE(3) 矩阵
使用 3D 可视化工具展示 pose 信息

函数说明

pywayne.vio.tools.SE3_to_pose(SE3_mat: ndarray) → ndarray: 将SE3矩阵转换为pose矩阵 :param SE3_mat: N个SE(3) :return: N个pose, tx, ty, tz, qw, qx, qy, qz

pywayne.vio.tools.pose_to_SE3(pose_mat: ndarray) → ndarray: 将pose矩阵转换为SE3矩阵 :param pose_mat: N个pose, shape: (N, 7), tx, ty, tz, qw, qx, qy, qz :return: N个SE(3)

pywayne.vio.tools.visualize_pose(poses, arrow_length_ratio=0.1)

Initialize the PoseVisualizer with a set of poses and scale factors for arrows.

Parameters:

poses – A list or array of SE(3) poses, each defined as (tx, ty, tz, qw, qx, qy, qz).
arrow_length_ratio – A scalar to determine the length of the arrows relative to the data range.

示例:

>>> from pywayne.vio.tools import visualize_pose, SE3_to_pose, pose_to_SE3
>>> import numpy as np
>>>
>>> # SE3 to Pose
>>> SE3 = np.eye(4)
>>> pose = SE3_to_pose(SE3)
>>> print("Pose:", pose)
>>>
>>> # Pose to SE3
>>> SE3_recon = pose_to_SE3(pose)
>>> print("Reconstructed SE3:\n", SE3_recon)
>>>
>>> # Visualize Pose
>>> SE3_array = np.random.randn(5, 4, 4) # Example random data
>>> # Ensure valid rotation matrices for visualization
>>> for i in range(SE3_array.shape[0]):
...     q = np.random.rand(4)
...     q /= np.linalg.norm(q)
...     SE3_array[i, :3, :3] = Quaternion(q).to_DCM()
...     SE3_array[i, :3, 3] = np.random.rand(3) * 5
...     SE3_array[i, 3, :3] = 0
...     SE3_array[i, 3, 3] = 1
>>> poses_to_visualize = SE3_to_pose(SE3_array)
>>> # visualize_pose(poses_to_visualize) # Uncomment to display plot