akai-transform 0.0.1__py3-none-any.whl

akai_transform/__init__.py

@@ -0,0 +1,31 @@
+'''
+akai_transform — 空间变换工具库
+----------------------------------------------------------------
+提供 2D/3D 空间变换、四元数运算、SE(3) 李群/李代数、对数四元数等功能。
+
+子模块:
+- tf2d:     2D 空间变换（3×3 齐次变换矩阵）
+- tf3d:     3D 空间变换（4×4 齐次变换矩阵）
+- quat:     四元数运算（[w, x, y, z] 标量在前）
+- se3:      SE(3) 李群/李代数（指数/对数映射）
+- log_quat: 对数四元数（四元数 ↔ R³ 旋转向量）
+- const:    常量和枚举定义
+
+作者: 阿凯爱玩机器人 | 微信: xingshunkai  | QQ: 244561792
+官网: deepsenserobot.com
+B站: https://space.bilibili.com/40344504
+'''
+
+from . import const
+from . import tf2d
+from . import tf3d
+from . import quat
+from . import se3
+from . import log_quat
+
+# 常量便捷导出
+from .const import (
+    PI, DEG2RAD, RAD2DEG, PI_2, PI_D_2,
+    TranslationUnit, AngleUnit,
+    MM, M, DEG, RAD,
+)

akai_transform/const.py

@@ -0,0 +1,51 @@
+'''
+常量与枚举定义
+----------------------------------------------------------------
+提供空间变换库所需的数学常量和单位枚举。
+
+作者: 阿凯爱玩机器人 | 微信: xingshunkai  | QQ: 244561792
+官网: deepsenserobot.com
+B站: https://space.bilibili.com/40344504
+'''
+
+import math
+from enum import IntEnum
+
+# ============================================================
+# 数学常量
+# ============================================================
+
+PI = math.pi
+'''圆周率'''
+
+DEG2RAD = math.pi / 180.0
+'''角度转弧度系数'''
+
+RAD2DEG = 180.0 / math.pi
+'''弧度转角度系数'''
+
+PI_2 = math.pi * 2.0
+'''两倍圆周率 (2π)'''
+
+PI_D_2 = math.pi / 2.0
+'''圆周率的一半 (π/2)'''
+
+# ============================================================
+# 单位枚举
+# ============================================================
+
+class TranslationUnit(IntEnum):
+    '''平移量单位枚举'''
+    MM = 0  # 毫米
+    M = 1   # 米
+
+class AngleUnit(IntEnum):
+    '''角度单位枚举'''
+    DEG = 0  # 度
+    RAD = 1  # 弧度
+
+# 便捷别名
+MM = TranslationUnit.MM
+M = TranslationUnit.M
+DEG = AngleUnit.DEG
+RAD = AngleUnit.RAD

akai_transform/log_quat.py

@@ -0,0 +1,84 @@
+'''
+对数四元数模块
+----------------------------------------------------------------
+提供四元数与对数四元数（R³ 旋转向量）之间的映射，
+用于姿态插值和优化。
+
+理论基础:
+- 对数映射: SO(3) → so(3), q → log(q)
+- 指数映射: so(3) → SO(3), v → exp(v)
+- 应用场景: 姿态插值、轨迹规划、姿态优化等
+
+作者: 阿凯爱玩机器人 | 微信: xingshunkai  | QQ: 244561792
+官网: deepsenserobot.com
+B站: https://space.bilibili.com/40344504
+'''
+
+import numpy as np
+
+
+def quat2log(q):
+    '''单位四元数 → 对数四元数（R³ 旋转向量）
+
+    使用对数映射将 SO(3) 群映射到其李代数 so(3) ≅ R³。
+    支持单个或批量四元数输入。
+
+    :param q: 单位四元数 [w, x, y, z]，形状为 (4,) 或 (..., 4)
+    :return: 对数四元数(旋转向量) [vx, vy, vz]，形状为 (3,) 或 (..., 3)
+    '''
+    q = np.asarray(q, dtype=np.float64)
+    original_shape = q.shape
+
+    if q.ndim == 1:
+        q = q.reshape(1, -1)
+
+    # 归一化
+    q = q / np.linalg.norm(q, axis=-1, keepdims=True)
+
+    w = q[..., 0]
+    vec = q[..., 1:]
+    norm_vec = np.linalg.norm(vec, axis=-1)
+
+    log_q = np.zeros((*q.shape[:-1], 3), dtype=np.float64)
+
+    mask = norm_vec >= 1e-6
+    if np.any(mask):
+        theta = 2.0 * np.arctan2(norm_vec[mask], w[mask])
+        axis = vec[mask] / norm_vec[mask, np.newaxis]
+        log_q[mask] = axis * theta[:, np.newaxis]
+
+    if len(original_shape) == 1:
+        return log_q[0]
+    return log_q
+
+
+def log2quat(v):
+    '''对数四元数（R³ 旋转向量） → 单位四元数
+
+    使用指数映射将李代数 so(3) 映射回 SO(3) 群。
+    支持单个或批量输入。
+
+    :param v: 对数四元数(旋转向量) [vx, vy, vz]，形状为 (3,) 或 (..., 3)
+    :return: 单位四元数 [w, x, y, z]，形状为 (4,) 或 (..., 4)
+    '''
+    v = np.asarray(v, dtype=np.float64)
+    original_shape = v.shape
+
+    if v.ndim == 1:
+        v = v.reshape(1, -1)
+
+    theta = np.linalg.norm(v, axis=-1)
+
+    q = np.zeros((*v.shape[:-1], 4), dtype=np.float64)
+    q[..., 0] = 1.0
+
+    mask = theta >= 1e-6
+    if np.any(mask):
+        axis = v[mask] / theta[mask, np.newaxis]
+        half_theta = 0.5 * theta[mask]
+        q[mask, 0] = np.cos(half_theta)
+        q[mask, 1:] = axis * np.sin(half_theta)[:, np.newaxis]
+
+    if len(original_shape) == 1:
+        return q[0]
+    return q

akai_transform/quat.py

@@ -0,0 +1,447 @@
+'''
+四元数运算模块
+----------------------------------------------------------------
+提供四元数的基础运算、向量旋转、插值和统计功能。
+四元数格式统一为 [w, x, y, z]（标量在前）。
+
+作者: 阿凯爱玩机器人 | 微信: xingshunkai  | QQ: 244561792
+官网: deepsenserobot.com
+B站: https://space.bilibili.com/40344504
+'''
+
+import numpy as np
+
+
+# ============================================================
+# 基础操作
+# ============================================================
+
+def identity():
+    '''返回单位四元数 [1, 0, 0, 0]
+
+    :return: 单位四元数 (4,)
+    '''
+    return np.array([1.0, 0.0, 0.0, 0.0])
+
+
+def norm(q):
+    '''计算四元数的模长
+
+    :param q: 四元数 [w, x, y, z]
+    :return: 模长标量
+    '''
+    return np.linalg.norm(q)
+
+
+def is_unit(q, tol=1e-6):
+    '''判断是否为单位四元数
+
+    :param q: 四元数 [w, x, y, z]
+    :param tol: 误差容限
+    :return: 是否为单位四元数
+    '''
+    return abs(norm(q) - 1.0) < tol
+
+
+def normalize(q):
+    '''归一化为单位四元数
+
+    :param q: 四元数 [w, x, y, z]
+    :return: 单位四元数 (4,)
+    '''
+    q = np.asarray(q, dtype=float)
+    return q / np.linalg.norm(q)
+
+
+def conjugate(q):
+    '''四元数共轭
+
+    :param q: 四元数 [w, x, y, z]
+    :return: 共轭四元数 [w, -x, -y, -z]
+    '''
+    q = np.asarray(q, dtype=float)
+    return np.array([q[0], -q[1], -q[2], -q[3]])
+
+
+def reverse(q):
+    '''四元数取反
+
+    :param q: 四元数 [w, x, y, z]
+    :return: 取反后的四元数 [-w, -x, -y, -z]
+    '''
+    return -np.asarray(q, dtype=float)
+
+
+def inv(q):
+    '''四元数求逆
+
+    :param q: 四元数 [w, x, y, z]
+    :return: 逆四元数 (4,)
+    '''
+    q = np.asarray(q, dtype=float)
+    q_conj = conjugate(q)
+    n2 = np.dot(q, q)
+    return q_conj / n2
+
+
+# ============================================================
+# 运算
+# ============================================================
+
+def dot(q1, q2):
+    '''四元数点乘
+
+    :param q1: 四元数 [w, x, y, z]
+    :param q2: 四元数 [w, x, y, z]
+    :return: 点乘标量
+    '''
+    return float(np.dot(q1, q2))
+
+
+def multiply(q1, q2):
+    '''四元数 Grabmann 乘积（Hamilton 积）
+
+    :param q1: 四元数 [w, x, y, z]
+    :param q2: 四元数 [w, x, y, z]
+    :return: 乘积四元数 (4,)
+    '''
+    a, b, c, d = q1[0], q1[1], q1[2], q1[3]
+    mat_left = np.array([
+        [ a, -b, -c, -d],
+        [ b,  a, -d,  c],
+        [ c,  d,  a, -b],
+        [ d, -c,  b,  a]
+    ])
+    return mat_left @ np.asarray(q2, dtype=float)
+
+
+# ============================================================
+# 向量旋转
+# ============================================================
+
+def rotate_vec(q, v):
+    '''用四元数旋转向量
+
+    :param q: 单位四元数 [w, x, y, z]
+    :param v: 3D 向量 (3,)
+    :return: 旋转后的向量 (3,)
+    '''
+    q_v = np.array([0.0, v[0], v[1], v[2]])
+    q_conj = conjugate(q)
+    result = multiply(multiply(q, q_v), q_conj)
+    return result[1:]
+
+
+def rotate_vec_nq(q_list, v):
+    '''批量旋转：多个四元数旋转同一个向量
+
+    :param q_list: 四元数列表，每个 [w, x, y, z]
+    :param v: 3D 向量 (3,)
+    :return: 旋转后的向量列表
+    '''
+    return [rotate_vec(q, v) for q in q_list]
+
+
+def rotate_vec_nv(q, v_list):
+    '''批量旋转：同一个四元数旋转多个向量
+
+    :param q: 单位四元数 [w, x, y, z]
+    :param v_list: 向量列表，每个 (3,)
+    :return: 旋转后的向量列表
+    '''
+    return [rotate_vec(q, v) for v in v_list]
+
+
+# ============================================================
+# 旋转度量
+# ============================================================
+
+def angle_diff(q1, q2, select_min=True):
+    '''计算两个四元数在超球面上的角度差
+
+    :param q1: 单位四元数 [w, x, y, z]
+    :param q2: 单位四元数 [w, x, y, z]
+    :param select_min: True 选最小角度 [0, π/2]，False 选 [π/2, π]
+    :return: (角度差值, 是否需要将 q2 取反)
+    '''
+    q1_u = normalize(q1)
+    q2_u = normalize(q2)
+
+    # 判断相等或相反
+    if np.allclose(q1_u, q2_u, atol=1e-8):
+        return 0.0, False
+    if np.allclose(q1_u, -q2_u, atol=1e-8):
+        return 0.0, True
+
+    cos_theta = float(np.dot(q1_u, q2_u))
+    is_q_reverse = False
+
+    if (select_min and cos_theta < 0) or (not select_min and cos_theta > 0):
+        cos_theta = -cos_theta
+        is_q_reverse = True
+
+    cos_theta = np.clip(cos_theta, -1.0, 1.0)
+    theta = np.arccos(cos_theta)
+    return float(theta), is_q_reverse
+
+
+def relative_rotation(q1, q2):
+    '''求解两个四元数的相对旋转 q_delta
+
+    满足 q_delta * q1 = q2，即 q_delta = q2 * q1^(-1)
+
+    :param q1: 起始四元数 [w, x, y, z]
+    :param q2: 目标四元数 [w, x, y, z]
+    :return: 相对旋转四元数 (4,)
+    '''
+    q1_conj = conjugate(q1)
+    return multiply(q2, q1_conj)
+
+
+# ============================================================
+# 插值
+# ============================================================
+
+def lerp(q1, q2, t):
+    '''四元数线性插值（结果非单位四元数）
+
+    :param q1: 起始四元数 [w, x, y, z]
+    :param q2: 终止四元数 [w, x, y, z]
+    :param t: 插值参数 [0, 1]
+    :return: 插值四元数 (4,)
+    '''
+    q1 = np.asarray(q1, dtype=float)
+    q2 = np.asarray(q2, dtype=float)
+    return (1.0 - t) * q1 + t * q2
+
+
+def nlerp(q1, q2, t):
+    '''四元数正规化线性插值（结果为单位四元数）
+
+    :param q1: 起始四元数 [w, x, y, z]
+    :param q2: 终止四元数 [w, x, y, z]
+    :param t: 插值参数 [0, 1]
+    :return: 单位四元数 (4,)
+    '''
+    return normalize(lerp(q1, q2, t))
+
+
+def slerp(q1, q2, t):
+    '''四元数球面线性插值
+
+    :param q1: 起始四元数 [w, x, y, z]
+    :param q2: 终止四元数 [w, x, y, z]
+    :param t: 插值参数 [0, 1]
+    :return: 单位四元数 (4,)
+    '''
+    q1 = np.asarray(q1, dtype=float)
+    q2 = np.asarray(q2, dtype=float)
+    cos_theta = np.clip(float(np.dot(q1, q2)), -1.0, 1.0)
+    # 过于接近时退化为 NLerp
+    if abs(cos_theta) > 0.9995:
+        return nlerp(q1, q2, t)
+    theta = np.arccos(cos_theta)
+    sin_theta = np.sin(theta)
+    q_interp = (q1 * np.sin((1.0 - t) * theta) + q2 * np.sin(t * theta)) / sin_theta
+    return normalize(q_interp)
+
+
+def interp_p2p(q1, q2, t_list, short_path=True):
+    '''两点间四元数批量插值（默认使用 Slerp）
+
+    :param q1: 起始四元数 [w, x, y, z]
+    :param q2: 终止四元数 [w, x, y, z]
+    :param t_list: 插值参数列表
+    :param short_path: 是否选择最短路径
+    :return: 四元数列表
+    '''
+    q1_u = normalize(q1)
+    q2_u = normalize(q2)
+
+    # 判断相等或相反
+    if np.allclose(q1_u, q2_u, atol=1e-3) or np.allclose(q1_u, -q2_u, atol=1e-3):
+        return [q1_u.copy() for _ in t_list]
+
+    cos_theta = float(np.dot(q1_u, q2_u))
+    cos_theta = np.clip(cos_theta, -1.0, 1.0)
+
+    # 选择最短/最长路径
+    if (short_path and cos_theta < 0) or (not short_path and cos_theta > 0):
+        cos_theta = -cos_theta
+        q2_u = -q2_u
+
+    is_close = abs(cos_theta) > 0.9995
+    theta = np.arccos(cos_theta)
+
+    result = []
+    for t in t_list:
+        if is_close:
+            result.append(nlerp(q1_u, q2_u, t))
+        else:
+            sin_theta = np.sin(theta)
+            q_interp = (q1_u * np.sin((1.0 - t) * theta) + q2_u * np.sin(t * theta)) / sin_theta
+            result.append(normalize(q_interp))
+    return result
+
+
+def squad(q0, q1, s0, s1, t):
+    '''四元数 Squad 插值（Spherical Quadrangle）
+
+    Squad 是 Slerp 的高阶扩展，用于多途径点的平滑插值。
+    类似于位置空间的圆弧过渡，Squad 在姿态空间提供 C1 连续性。
+
+    :param q0: 起始四元数 [w, x, y, z]
+    :param q1: 终止四元数 [w, x, y, z]
+    :param s0: 起始控制四元数 [w, x, y, z]
+    :param s1: 终止控制四元数 [w, x, y, z]
+    :param t: 插值参数 [0, 1]
+    :return: 插值四元数 (4,)
+    '''
+    # Squad(q0, q1, s0, s1, t) = Slerp(Slerp(q0, q1, t), Slerp(s0, s1, t), 2t(1-t))
+    a = slerp(q0, q1, t)
+    b = slerp(s0, s1, t)
+    return slerp(a, b, 2.0 * t * (1.0 - t))
+
+
+def compute_control_quat(q_prev, q_curr, q_next):
+    '''计算 Squad 插值的控制四元数
+
+    控制四元数用于保证多途径点插值的平滑性（C1 连续）。
+    计算公式基于对数映射：
+    s_i = q_i * exp(-0.25 * (log(q_i^-1 * q_{i-1}) + log(q_i^-1 * q_{i+1})))
+
+    :param q_prev: 前一个四元数 [w, x, y, z]
+    :param q_curr: 当前四元数 [w, x, y, z]
+    :param q_next: 后一个四元数 [w, x, y, z]
+    :return: 当前点的控制四元数 (4,)
+    '''
+    q_prev = normalize(q_prev)
+    q_curr = normalize(q_curr)
+    q_next = normalize(q_next)
+
+    # 计算相对旋转
+    q_curr_inv = inv(q_curr)
+    q_rel_prev = multiply(q_curr_inv, q_prev)
+    q_rel_next = multiply(q_curr_inv, q_next)
+
+    # 转换为对数空间（旋转向量）
+    def quat_log(q):
+        '''四元数对数映射：q -> log(q)'''
+        q = normalize(q)
+        w = q[0]
+        v = q[1:]
+        v_norm = np.linalg.norm(v)
+        if v_norm < 1e-8:
+            return np.array([0.0, 0.0, 0.0])
+        theta = 2.0 * np.arctan2(v_norm, w)
+        return (theta / v_norm) * v
+
+    def quat_exp(omega):
+        '''四元数指数映射：log(q) -> q'''
+        theta = np.linalg.norm(omega)
+        if theta < 1e-8:
+            return np.array([1.0, 0.0, 0.0, 0.0])
+        half_theta = theta / 2.0
+        s = np.sin(half_theta) / theta
+        return np.array([np.cos(half_theta), s * omega[0], s * omega[1], s * omega[2]])
+
+    # 对数空间平均
+    log_prev = quat_log(q_rel_prev)
+    log_next = quat_log(q_rel_next)
+    log_avg = -0.25 * (log_prev + log_next)
+
+    # 转回四元数空间
+    q_delta = quat_exp(log_avg)
+    s = multiply(q_curr, q_delta)
+    return normalize(s)
+
+
+def interp_multi_waypoints(q_list, t_list, method='squad'):
+    '''多途径点四元数插值
+
+    :param q_list: 四元数途径点列表，每个 [w, x, y, z]，至少 2 个点
+    :param t_list: 插值参数列表，范围 [0, n-1]，n 为途径点数量
+    :param method: 插值方法，'slerp' 或 'squad'
+    :return: 插值四元数列表
+    '''
+    q_list = [normalize(q) for q in q_list]
+    n = len(q_list)
+
+    if n < 2:
+        raise ValueError("至少需要 2 个途径点")
+
+    if method == 'slerp':
+        # Slerp 分段插值（折线效果）
+        result = []
+        for t in t_list:
+            t = np.clip(t, 0.0, n - 1.0)
+            idx = int(np.floor(t))
+            if idx >= n - 1:
+                result.append(q_list[-1].copy())
+            else:
+                local_t = t - idx
+                result.append(slerp(q_list[idx], q_list[idx + 1], local_t))
+        return result
+
+    elif method == 'squad':
+        # Squad 平滑插值
+        if n < 3:
+            # 少于 3 个点时退化为 Slerp
+            return interp_multi_waypoints(q_list, t_list, method='slerp')
+
+        # 计算所有控制四元数
+        s_list = []
+        for i in range(n):
+            if i == 0:
+                # 首点：使用镜像策略
+                q_prev = multiply(q_list[0], inv(multiply(inv(q_list[0]), q_list[1])))
+                s = compute_control_quat(q_prev, q_list[0], q_list[1])
+            elif i == n - 1:
+                # 末点：使用镜像策略
+                q_next = multiply(q_list[-1], inv(multiply(inv(q_list[-1]), q_list[-2])))
+                s = compute_control_quat(q_list[-2], q_list[-1], q_next)
+            else:
+                # 中间点：正常计算
+                s = compute_control_quat(q_list[i - 1], q_list[i], q_list[i + 1])
+            s_list.append(s)
+
+        # 执行 Squad 插值
+        result = []
+        for t in t_list:
+            t = np.clip(t, 0.0, n - 1.0)
+            idx = int(np.floor(t))
+            if idx >= n - 1:
+                result.append(q_list[-1].copy())
+            else:
+                local_t = t - idx
+                q_interp = squad(q_list[idx], q_list[idx + 1], s_list[idx], s_list[idx + 1], local_t)
+                result.append(q_interp)
+        return result
+
+    else:
+        raise ValueError(f"未知的插值方法: {method}")
+
+
+# ============================================================
+# 统计
+# ============================================================
+
+def mean(q_list):
+    '''四元数均值（最大特征值法）
+
+    :param q_list: 四元数列表，每个 [w, x, y, z]
+    :return: 均值四元数 (4,)
+    '''
+    q_arr = np.array(q_list, dtype=float)
+    n = q_arr.shape[0]
+    # 外积累加
+    A = np.zeros((4, 4))
+    for i in range(n):
+        q = q_arr[i]
+        A += np.outer(q, q)
+    A /= n
+    # 特征值分解，取最大特征值对应的特征向量
+    eigenvalues, eigenvectors = np.linalg.eigh(A)
+    # eigh 返回升序排列，最大特征值在最后
+    q_mean = eigenvectors[:, -1]
+    return normalize(q_mean)