tftree-manager 0.0.1__tar.gz

tftree_manager-0.0.1/MANIFEST.in

@@ -0,0 +1,20 @@
+include README.md
+prune test
+prune test*
+prune tests*
+prune docs
+prune examples
+prune input*
+prune output*
+prune log*
+prune logs*
+prune tmp*
+prune temp*
+prune .git
+prune .pytest_cache
+prune .mypy_cache
+prune __pycache__
+global-exclude *.log
+global-exclude *.pyc
+global-exclude *.pyo
+global-exclude .DS_Store

tftree_manager-0.0.1/PKG-INFO

@@ -0,0 +1,79 @@
+Metadata-Version: 2.4
+Name: tftree-manager
+Version: 0.0.1
+Summary: TF 树梳理与变换管理：YAML 外参加载/保存、图路径与环检测、变换矩阵计算
+License-Expression: MIT
+Requires-Python: >=3.10
+Description-Content-Type: text/markdown
+Requires-Dist: numpy>=1.20.0
+Requires-Dist: PyYAML>=5.4.0
+Requires-Dist: Flask>=2.0.0
+Provides-Extra: dev
+Requires-Dist: pytest; extra == "dev"
+Requires-Dist: build; extra == "dev"
+Requires-Dist: twine; extra == "dev"
+
+# tftree-manager
+
+TF 树梳理与变换管理：从 YAML 目录加载/保存外参与顺序，构建无向图、路径查找、环检测、变换矩阵计算。
+
+## 安装
+
+```bash
+pip install tftree-manager
+```
+
+## 功能特性
+
+- 从 `yaml_dir/extrinsics/*.yaml` 与 `order_manifest.yaml` 加载/保存外参与顺序
+- 无向图构建、BFS 路径查找、有向环检测
+- 沿路径累积 4x4 变换矩阵、从根节点计算到所有可达节点
+- 更新变换、求逆、重排顺序并写回 YAML
+- 可选 Flask API 服务与命令行入口
+
+## 快速开始
+
+```python
+from tftree_manager import TFTreeManager
+
+manager = TFTreeManager("/path/to/yaml_dir")
+manager.load_from_yaml()
+
+# 所有坐标系
+frames = manager.get_all_frames()
+
+# 两节点间变换矩阵
+T = manager.get_transform("lidar", "camera")
+
+# 从根节点计算到所有节点
+transforms = manager.compute_all_transforms_from_root("world")
+
+# 检测环
+cycles = manager.detect_cycles()
+
+# 保存
+manager.save_to_yaml()
+```
+
+## 命令行
+
+```bash
+# 以 YAML 目录为参数，加载并打印树与环信息
+tftree-manager /path/to/yaml_dir
+
+# 启动 Flask API 服务（默认 0.0.0.0:5000）
+tftree-manager-server
+```
+
+环境变量 `TFTREE_DATA_DIR` 可指定 API 默认数据目录（默认 `./data`）。
+
+## 依赖
+
+- Python >= 3.10
+- numpy >= 1.20.0
+- PyYAML >= 5.4.0
+- Flask >= 2.0.0（API 与 server 入口需要）
+
+## License
+
+MIT

tftree_manager-0.0.1/README.md

@@ -0,0 +1,64 @@
+# tftree-manager
+
+TF 树梳理与变换管理：从 YAML 目录加载/保存外参与顺序，构建无向图、路径查找、环检测、变换矩阵计算。
+
+## 安装
+
+```bash
+pip install tftree-manager
+```
+
+## 功能特性
+
+- 从 `yaml_dir/extrinsics/*.yaml` 与 `order_manifest.yaml` 加载/保存外参与顺序
+- 无向图构建、BFS 路径查找、有向环检测
+- 沿路径累积 4x4 变换矩阵、从根节点计算到所有可达节点
+- 更新变换、求逆、重排顺序并写回 YAML
+- 可选 Flask API 服务与命令行入口
+
+## 快速开始
+
+```python
+from tftree_manager import TFTreeManager
+
+manager = TFTreeManager("/path/to/yaml_dir")
+manager.load_from_yaml()
+
+# 所有坐标系
+frames = manager.get_all_frames()
+
+# 两节点间变换矩阵
+T = manager.get_transform("lidar", "camera")
+
+# 从根节点计算到所有节点
+transforms = manager.compute_all_transforms_from_root("world")
+
+# 检测环
+cycles = manager.detect_cycles()
+
+# 保存
+manager.save_to_yaml()
+```
+
+## 命令行
+
+```bash
+# 以 YAML 目录为参数，加载并打印树与环信息
+tftree-manager /path/to/yaml_dir
+
+# 启动 Flask API 服务（默认 0.0.0.0:5000）
+tftree-manager-server
+```
+
+环境变量 `TFTREE_DATA_DIR` 可指定 API 默认数据目录（默认 `./data`）。
+
+## 依赖
+
+- Python >= 3.10
+- numpy >= 1.20.0
+- PyYAML >= 5.4.0
+- Flask >= 2.0.0（API 与 server 入口需要）
+
+## License
+
+MIT

tftree_manager-0.0.1/pyproject.toml

@@ -0,0 +1,42 @@
+[build-system]
+requires = ["setuptools>=61", "wheel"]
+build-backend = "setuptools.build_meta"
+
+[project]
+name = "tftree-manager"
+version = "0.0.1"
+description = "TF 树梳理与变换管理：YAML 外参加载/保存、图路径与环检测、变换矩阵计算"
+readme = "README.md"
+license = "MIT"
+requires-python = ">=3.10"
+dependencies = [
+    "numpy>=1.20.0",
+    "PyYAML>=5.4.0",
+    "Flask>=2.0.0",
+]
+
+[project.optional-dependencies]
+dev = ["pytest", "build", "twine"]
+
+[project.scripts]
+tftree-manager = "tftree_manager.manager:main"
+tftree-manager-server = "tftree_manager.app:run"
+
+[tool.setuptools.packages.find]
+exclude = [
+    "test*",
+    "tests*",
+    "docs*",
+    "examples*",
+    "input*",
+    "output*",
+    "log*",
+    "logs*",
+    "*.log",
+    "tmp*",
+    "temp*",
+    ".pytest_cache*",
+    ".mypy_cache*",
+    "__pycache__*",
+    "*.pyc",
+]

tftree_manager-0.0.1/setup.cfg

@@ -0,0 +1,4 @@
+[egg_info]
+tag_build = 
+tag_date = 0
+

tftree_manager-0.0.1/tftree_manager/__init__.py

@@ -0,0 +1,20 @@
+"""
+tftree-manager: TF 树梳理与变换管理。
+"""
+from .transform import Transform
+from .tftree_errors import (
+    TFTreeError,
+    TFTreeNotFoundError,
+    TFTreeValidationError,
+    TFTreeKeyError,
+)
+from .manager import TFTreeManager
+
+__all__ = [
+    "Transform",
+    "TFTreeManager",
+    "TFTreeError",
+    "TFTreeNotFoundError",
+    "TFTreeValidationError",
+    "TFTreeKeyError",
+]

tftree_manager-0.0.1/tftree_manager/app.py

@@ -0,0 +1,162 @@
+"""Flask API 服务：TF 树加载、查询、更新、保存。"""
+import os
+from flask import Flask, request, jsonify
+import numpy as np
+
+from .manager import TFTreeManager
+
+app = Flask(__name__)
+
+DATA_DIR = os.environ.get("TFTREE_DATA_DIR", "./data")
+manager = None
+
+
+def get_manager():
+    global manager
+    if manager is None:
+        if not os.path.exists(DATA_DIR):
+            os.makedirs(DATA_DIR, exist_ok=True)
+        manager = TFTreeManager(DATA_DIR)
+        try:
+            manager.load_from_yaml()
+        except Exception:
+            pass
+    return manager
+
+
+@app.route('/api/load', methods=['POST'])
+def load_data():
+    """重新加载数据"""
+    path = request.json.get('path', DATA_DIR)
+    global manager
+    manager = TFTreeManager(path)
+    try:
+        manager.load_from_yaml()
+        return jsonify({"status": "success", "message": f"Loaded from {path}", "frames": list(manager.get_all_frames())})
+    except Exception as e:
+        return jsonify({"status": "error", "message": str(e)}), 400
+
+
+@app.route('/api/frames', methods=['GET'])
+def get_frames():
+    """获取所有坐标系"""
+    m = get_manager()
+    return jsonify({"frames": list(m.get_all_frames())})
+
+
+@app.route('/api/transforms', methods=['GET'])
+def get_all_transforms():
+    """获取所有已定义的变换"""
+    m = get_manager()
+    return jsonify({"transforms": m.order})
+
+
+@app.route('/api/transform', methods=['GET'])
+def get_transform():
+    """计算两个节点间的变换"""
+    source = request.args.get('source')
+    target = request.args.get('target')
+    if not source or not target:
+        return jsonify({"status": "error", "message": "Missing source or target"}), 400
+
+    m = get_manager()
+    T = m.get_transform(source, target)
+    if T is None:
+        return jsonify({"status": "error", "message": f"No path from {source} to {target}"}), 404
+
+    return jsonify({
+        "source": source,
+        "target": target,
+        "matrix": T.tolist(),
+        "translation": T[:3, 3].tolist(),
+        "rotation_matrix": T[:3, :3].tolist()
+    })
+
+
+@app.route('/api/compute_all', methods=['GET'])
+def compute_all():
+    """从根节点计算所有变换"""
+    root = request.args.get('root')
+    if not root:
+        return jsonify({"status": "error", "message": "Missing root"}), 400
+
+    m = get_manager()
+    try:
+        transforms = m.compute_all_transforms_from_root(root)
+        result = {}
+        for frame, T in transforms.items():
+            result[frame] = {
+                "matrix": T.tolist(),
+                "translation": T[:3, 3].tolist()
+            }
+        return jsonify({"root": root, "transforms": result})
+    except Exception as e:
+        return jsonify({"status": "error", "message": str(e)}), 400
+
+
+@app.route('/api/update', methods=['POST'])
+def update_transform():
+    """更新变换参数"""
+    data = request.json
+    source = data.get('source')
+    target = data.get('target')
+    translation = data.get('translation')
+    rotation = data.get('rotation')
+
+    if not source or not target:
+        return jsonify({"status": "error", "message": "Missing source or target"}), 400
+
+    m = get_manager()
+    try:
+        trans_np = np.array(translation) if translation else None
+        rot_np = np.array(rotation) if rotation else None
+        m.update_transform(source, target, trans_np, rot_np)
+        return jsonify({"status": "success"})
+    except Exception as e:
+        return jsonify({"status": "error", "message": str(e)}), 400
+
+
+@app.route('/api/invert', methods=['POST'])
+def invert_transform():
+    """求逆变换"""
+    data = request.json
+    source = data.get('source')
+    target = data.get('target')
+
+    if not source or not target:
+        return jsonify({"status": "error", "message": "Missing source or target"}), 400
+
+    m = get_manager()
+    try:
+        m.invert_transform(source, target)
+        return jsonify({"status": "success"})
+    except Exception as e:
+        return jsonify({"status": "error", "message": str(e)}), 400
+
+
+@app.route('/api/save', methods=['POST'])
+def save_data():
+    """保存到 YAML"""
+    m = get_manager()
+    try:
+        m.save_to_yaml()
+        return jsonify({"status": "success", "message": f"Saved to {m.yaml_dir}"})
+    except Exception as e:
+        return jsonify({"status": "error", "message": str(e)}), 500
+
+
+@app.route('/api/cycles', methods=['GET'])
+def detect_cycles():
+    """检测环"""
+    m = get_manager()
+    cycles = m.detect_cycles()
+    return jsonify({"cycles": cycles})
+
+
+def run(host='0.0.0.0', port=5000, debug=True):
+    """启动 Flask 服务（供命令行入口调用）。"""
+    app.run(host=host, port=port, debug=debug)
+
+
+if __name__ == '__main__':
+    run()

tftree_manager-0.0.1/tftree_manager/graph_ops.py

@@ -0,0 +1,81 @@
+"""
+图操作：无向图构建、BFS 路径、有向环检测。纯函数，无 I/O。
+"""
+from typing import Dict, List, Optional, Set
+
+from .transform import Transform
+
+
+def build_bidirectional_graph(transforms: Dict[str, Transform]) -> Dict[str, Set[str]]:
+    """
+    根据 transforms 构建无向图邻接表（source<->target），用于路径查找与求逆。
+    """
+    graph: Dict[str, Set[str]] = {}
+    for key, t in transforms.items():
+        s, tgt = t.source_frame, t.target_frame
+        if s not in graph:
+            graph[s] = set()
+        if tgt not in graph:
+            graph[tgt] = set()
+        graph[s].add(tgt)
+        graph[tgt].add(s)
+    return graph
+
+
+def find_path_bfs(
+    graph: Dict[str, Set[str]],
+    source: str,
+    target: str,
+) -> Optional[List[str]]:
+    """BFS 从 source 到 target 的路径，不存在返回 None。"""
+    if source not in graph or target not in graph:
+        return None
+    queue = [(source, [source])]
+    visited = {source}
+    while queue:
+        node, path = queue.pop(0)
+        if node == target:
+            return path
+        for neighbor in graph.get(node, []):
+            if neighbor not in visited:
+                visited.add(neighbor)
+                queue.append((neighbor, path + [neighbor]))
+    return None
+
+
+def detect_directed_cycles(transforms: Dict[str, Transform]) -> List[List[str]]:
+    """
+    在有向图（仅 source->target）上检测所有环，返回环列表，每个环为节点 id 列表。
+    """
+    directed: Dict[str, Set[str]] = {}
+    all_nodes: Set[str] = set()
+    for key, t in transforms.items():
+        s, tgt = t.source_frame, t.target_frame
+        all_nodes.add(s)
+        all_nodes.add(tgt)
+        if s not in directed:
+            directed[s] = set()
+        directed[s].add(tgt)
+
+    cycles = []
+    visited = set()
+    rec_stack = set()
+
+    def dfs(node: str, path: List[str]):
+        visited.add(node)
+        rec_stack.add(node)
+        path.append(node)
+        for neighbor in directed.get(node, []):
+            if neighbor not in visited:
+                dfs(neighbor, path.copy())
+            elif neighbor in rec_stack:
+                cycle_start = path.index(neighbor)
+                cycle = path[cycle_start:] + [neighbor]
+                cycles.append(cycle)
+        rec_stack.remove(node)
+
+    for node in sorted(all_nodes):
+        if node not in visited:
+            dfs(node, [])
+
+    return cycles

tftree_manager-0.0.1/tftree_manager/manager.py

@@ -0,0 +1,161 @@
+"""
+TF Tree Manager - 仅调度：持有 transforms/order/graph，对外 API 委托给 transform、yaml_io、graph_ops、transform_math。
+"""
+from pathlib import Path
+from typing import Dict, List, Optional, Set
+
+import numpy as np
+
+from .transform import Transform
+from .yaml_io import load_extrinsics_and_order, save_extrinsics_and_order
+from .graph_ops import build_bidirectional_graph, find_path_bfs, detect_directed_cycles
+from .transform_math import get_transform_along_path, compute_all_from_root
+from .tftree_errors import TFTreeValidationError, TFTreeKeyError
+
+
+class TFTreeManager:
+    """TF 树管理器：调度层，状态 + 委托。"""
+
+    def __init__(self, yaml_dir: str):
+        self.yaml_dir = Path(yaml_dir)
+        self.transforms: Dict[str, Transform] = {}
+        self.order: List[str] = []
+        self.graph: Dict[str, Set[str]] = {}
+
+    def load_from_yaml(self):
+        """从 YAML 目录加载外参与顺序，构建图。"""
+        self.transforms, self.order = load_extrinsics_and_order(self.yaml_dir)
+        self.graph = build_bidirectional_graph(self.transforms)
+
+    def _build_graph(self):
+        """重建无向图（invert 后调用）。"""
+        self.graph = build_bidirectional_graph(self.transforms)
+
+    def detect_cycles(self) -> List[List[str]]:
+        """有向图环检测，委托 graph_ops。"""
+        return detect_directed_cycles(self.transforms)
+
+    def get_transform(
+        self, source: str, target: str, root: Optional[str] = None
+    ) -> Optional[np.ndarray]:
+        """沿路径累积变换矩阵。"""
+        if source == target:
+            return np.eye(4)
+        path = find_path_bfs(self.graph, source, target)
+        if not path:
+            return None
+        return get_transform_along_path(self.transforms, path)
+
+    def compute_all_transforms_from_root(self, root: str) -> Dict[str, np.ndarray]:
+        """从 root 到所有可达节点的变换。"""
+        if root not in self.graph:
+            raise TFTreeValidationError(f"根节点 {root} 不存在于图中", {"root": root})
+        return compute_all_from_root(self.transforms, self.graph, root)
+
+    def update_transform(
+        self,
+        source: str,
+        target: str,
+        translation: Optional[np.ndarray] = None,
+        rotation: Optional[np.ndarray] = None,
+    ):
+        """更新指定边的平移/旋转。"""
+        key = f"{source}->{target}"
+        if key not in self.transforms:
+            raise TFTreeKeyError(f"变换 {key} 不存在", {"key": key})
+        t = self.transforms[key]
+        if translation is not None:
+            t.translation = translation.copy()
+        if rotation is not None:
+            t.rotation = rotation.copy()
+
+    def invert_transform(self, source: str, target: str):
+        """将边 source->target 求逆为 target->source。"""
+        key = f"{source}->{target}"
+        if key not in self.transforms:
+            raise TFTreeKeyError(f"变换 {key} 不存在", {"key": key})
+        t = self.transforms[key]
+        inv = t.inverse()
+        del self.transforms[key]
+        new_key = f"{target}->{source}"
+        self.transforms[new_key] = inv
+        if key in self.order:
+            idx = self.order.index(key)
+            self.order[idx] = new_key
+        self._build_graph()
+
+    def reorder_transforms(self, new_order: List[str]):
+        """重排顺序列表。"""
+        if set(new_order) != set(self.order):
+            raise TFTreeValidationError(
+                "新顺序必须包含所有现有变换", {"order": list(self.order)}
+            )
+        self.order = new_order.copy()
+
+    def save_to_yaml(self):
+        """写回 extrinsics 与 order_manifest。"""
+        save_extrinsics_and_order(self.yaml_dir, self.transforms, self.order)
+
+    def get_all_frames(self) -> Set[str]:
+        """所有节点（frame）集合。"""
+        return set(self.graph.keys())
+
+    def print_tree(self, root: Optional[str] = None):
+        """打印树结构（调试）。"""
+        if not self.graph:
+            print("图为空")
+            return
+        if root is None:
+            all_targets = set()
+            all_sources = set()
+            for key in self.transforms:
+                s, t = key.split("->")
+                all_sources.add(s)
+                all_targets.add(t)
+            roots = all_sources - all_targets
+            root = list(roots)[0] if roots else list(self.graph.keys())[0]
+        print(f"树结构（根节点: {root}）:")
+        visited = set()
+
+        def print_node(node: str, level: int):
+            if node in visited:
+                print("  " * level + f"└─ {node} (已访问)")
+                return
+            visited.add(node)
+            print("  " * level + f"└─ {node}")
+            for neighbor in sorted(self.graph.get(node, [])):
+                if neighbor not in visited:
+                    print_node(neighbor, level + 1)
+
+        print_node(root, 0)
+
+
+def main():
+    """示例用法。"""
+    import sys
+    if len(sys.argv) < 2:
+        print("用法: tftree-manager <yaml_dir>")
+        return
+    yaml_dir = sys.argv[1]
+    manager = TFTreeManager(yaml_dir)
+    manager.load_from_yaml()
+    print(f"加载了 {len(manager.transforms)} 个变换")
+    print(f"坐标系: {manager.get_all_frames()}")
+    cycles = manager.detect_cycles()
+    if cycles:
+        print(f"检测到 {len(cycles)} 个环:")
+        for i, c in enumerate(cycles):
+            print(f"  环 {i+1}: {' -> '.join(c)}")
+    else:
+        print("未检测到环")
+    manager.print_tree()
+    frames = manager.get_all_frames()
+    if frames:
+        root = list(frames)[0]
+        result = manager.compute_all_transforms_from_root(root)
+        print(f"从根节点 {root} 到所有节点的变换: {len(result)} 个")
+    print()
+
+
+if __name__ == "__main__":
+    main()

tftree_manager-0.0.1/tftree_manager/tftree_errors.py

@@ -0,0 +1,35 @@
+"""
+TF Tree 专用异常：便于 D03 独立使用及上层（如 studio）统一捕获并转成 HTTP/JSON。
+"""
+from typing import Any, Optional
+
+
+class TFTreeError(Exception):
+    """树相关异常基类，携带 message 与可选 detail。"""
+
+    def __init__(self, message: str, detail: Optional[dict] = None):
+        self.message = message
+        self.detail = detail or {}
+        super().__init__(message)
+
+    def to_dict(self) -> dict:
+        """便于转成 JSON 响应。"""
+        out = {"message": self.message}
+        if self.detail:
+            out["detail"] = self.detail
+        return out
+
+
+class TFTreeNotFoundError(TFTreeError):
+    """资源不存在：如外参目录、文件缺失。"""
+    pass
+
+
+class TFTreeValidationError(TFTreeError):
+    """校验失败：如根节点不存在、顺序不包含全部变换、参数非法。"""
+    pass
+
+
+class TFTreeKeyError(TFTreeError):
+    """变换 key 不存在（如 update/invert 时指定的 source->target）。"""
+    pass

tftree_manager-0.0.1/tftree_manager/transform.py

@@ -0,0 +1,96 @@
+"""
+Transform 数据类：四元数/旋转矩阵、逆变换、序列化。
+不依赖 manager，供 yaml_io、transform_math、manager 使用。
+"""
+import numpy as np
+from dataclasses import dataclass
+from typing import TYPE_CHECKING
+
+if TYPE_CHECKING:
+    pass
+
+
+@dataclass
+class Transform:
+    """表示一个坐标变换"""
+    source_frame: str
+    target_frame: str
+    translation: np.ndarray  # [x, y, z]
+    rotation: np.ndarray     # [qx, qy, qz, qw] 四元数
+
+    def to_matrix(self) -> np.ndarray:
+        """转换为4x4齐次变换矩阵"""
+        qx, qy, qz, qw = self.rotation
+
+        # 四元数转旋转矩阵
+        R = np.array([
+            [1 - 2*(qy**2 + qz**2), 2*(qx*qy - qz*qw), 2*(qx*qz + qy*qw)],
+            [2*(qx*qy + qz*qw), 1 - 2*(qx**2 + qz**2), 2*(qy*qz - qx*qw)],
+            [2*(qx*qz - qy*qw), 2*(qy*qz + qx*qw), 1 - 2*(qx**2 + qy**2)]
+        ])
+
+        # 构建齐次变换矩阵
+        T = np.eye(4)
+        T[:3, :3] = R
+        T[:3, 3] = self.translation
+        return T
+
+    @staticmethod
+    def from_matrix(matrix: np.ndarray, source: str, target: str) -> "Transform":
+        """从4x4齐次变换矩阵创建Transform"""
+        translation = matrix[:3, 3]
+        R = matrix[:3, :3]
+
+        # 旋转矩阵转四元数
+        trace = np.trace(R)
+        if trace > 0:
+            s = 0.5 / np.sqrt(trace + 1.0)
+            qw = 0.25 / s
+            qx = (R[2, 1] - R[1, 2]) * s
+            qy = (R[0, 2] - R[2, 0]) * s
+            qz = (R[1, 0] - R[0, 1]) * s
+        else:
+            if R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]:
+                s = 2.0 * np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2])
+                qw = (R[2, 1] - R[1, 2]) / s
+                qx = 0.25 * s
+                qy = (R[0, 1] + R[1, 0]) / s
+                qz = (R[0, 2] + R[2, 0]) / s
+            elif R[1, 1] > R[2, 2]:
+                s = 2.0 * np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2])
+                qw = (R[0, 2] - R[2, 0]) / s
+                qx = (R[0, 1] + R[1, 0]) / s
+                qy = 0.25 * s
+                qz = (R[1, 2] + R[2, 1]) / s
+            else:
+                s = 2.0 * np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1])
+                qw = (R[1, 0] - R[0, 1]) / s
+                qx = (R[0, 2] + R[2, 0]) / s
+                qy = (R[1, 2] + R[2, 1]) / s
+                qz = 0.25 * s
+
+        rotation = np.array([qx, qy, qz, qw])
+        return Transform(source, target, translation, rotation)
+
+    def inverse(self) -> "Transform":
+        """返回逆变换"""
+        T_inv = np.linalg.inv(self.to_matrix())
+        return Transform.from_matrix(T_inv, self.target_frame, self.source_frame)
+
+    def to_dict(self) -> dict:
+        """转换为字典格式（用于保存YAML）"""
+        return {
+            'source_frame': self.source_frame,
+            'target_frame': self.target_frame,
+            'translation': {
+                'x': float(self.translation[0]),
+                'y': float(self.translation[1]),
+                'z': float(self.translation[2])
+            },
+            'rotation': {
+                'qx': float(self.rotation[0]),
+                'qy': float(self.rotation[1]),
+                'qz': float(self.rotation[2]),
+                'qw': float(self.rotation[3])
+            }
+        }

tftree_manager-0.0.1/tftree_manager/transform_math.py

@@ -0,0 +1,57 @@
+"""
+变换计算：沿路径累积矩阵、从 root 计算到所有节点。不负责 I/O 和图构建。
+"""
+import numpy as np
+from typing import Dict, List, Optional
+
+from .transform import Transform
+
+
+def get_transform_along_path(
+    transforms: Dict[str, Transform],
+    path: List[str],
+) -> Optional[np.ndarray]:
+    """
+    沿 path 累积变换矩阵，path 为节点序列 [n0, n1, ..., nk]。
+    若某条边不存在（正向与逆向都不存在）则返回 None。
+    """
+    if len(path) < 2:
+        return np.eye(4)
+    T = np.eye(4)
+    for i in range(len(path) - 1):
+        from_frame = path[i]
+        to_frame = path[i + 1]
+        key = f"{from_frame}->{to_frame}"
+        if key in transforms:
+            T = T @ transforms[key].to_matrix()
+        else:
+            key_inv = f"{to_frame}->{from_frame}"
+            if key_inv in transforms:
+                T = T @ transforms[key_inv].inverse().to_matrix()
+            else:
+                return None
+    return T
+
+
+def compute_all_from_root(
+    transforms: Dict[str, Transform],
+    graph: Dict[str, set],  # Set[str] per node
+    root: str,
+) -> Dict[str, np.ndarray]:
+    """
+    从 root 到图中所有可达节点的变换矩阵。
+    调用方需保证 root 在 graph 中；否则由调用方抛异常。
+    """
+    from .graph_ops import find_path_bfs
+
+    result = {root: np.eye(4)}
+    for node in graph:
+        if node == root:
+            continue
+        path = find_path_bfs(graph, root, node)
+        if path is None:
+            continue
+        T = get_transform_along_path(transforms, path)
+        if T is not None:
+            result[node] = T
+    return result

tftree_manager-0.0.1/tftree_manager/yaml_io.py

@@ -0,0 +1,100 @@
+"""
+YAML 读写：order_manifest + extrinsics 目录。
+输入 yaml_dir，返回/接收 (transforms_dict, order_list)，不持有图。
+"""
+import yaml
+import numpy as np
+from pathlib import Path
+from typing import Dict, List, Tuple
+
+from .transform import Transform
+from .tftree_errors import TFTreeNotFoundError
+
+
+def load_extrinsics_and_order(yaml_dir: Path) -> Tuple[Dict[str, "Transform"], List[str]]:
+    """
+    从 yaml_dir 读取 order_manifest 与 extrinsics/*.yaml，返回 (transforms, order)。
+    外参目录不存在时抛出 TFTreeNotFoundError。
+    """
+    extrinsics_dir = yaml_dir / "extrinsics"
+    if not extrinsics_dir.exists():
+        raise TFTreeNotFoundError(f"外参目录不存在: {extrinsics_dir}", {"path": str(extrinsics_dir)})
+
+    order: List[str] = []
+    order_file = yaml_dir / "order_manifest.yaml"
+    if order_file.exists():
+        with open(order_file, "r", encoding="utf-8") as f:
+            order_data = yaml.safe_load(f)
+            if order_data:
+                if "extrinsics_order" in order_data:
+                    order = list(order_data["extrinsics_order"])
+                elif "extrinsics" in order_data:
+                    for item in order_data["extrinsics"]:
+                        rel_file = item.get("file", "")
+                        path = yaml_dir / rel_file
+                        if path.exists():
+                            with open(path, "r", encoding="utf-8") as fp:
+                                data = yaml.safe_load(fp) or {}
+                            src = data.get("source_frame", "")
+                            tgt = data.get("target_frame", "")
+                            if src or tgt:
+                                order.append(f"{src}->{tgt}")
+
+    transforms: Dict[str, Transform] = {}
+    for yaml_file in sorted(extrinsics_dir.glob("*.yaml")):
+        with open(yaml_file, "r", encoding="utf-8") as f:
+            data = yaml.safe_load(f)
+            if data:
+                source = data["source_frame"]
+                target = data["target_frame"]
+                translation = np.array([
+                    data["translation"]["x"],
+                    data["translation"]["y"],
+                    data["translation"]["z"],
+                ])
+                rotation = np.array([
+                    data["rotation"]["qx"],
+                    data["rotation"]["qy"],
+                    data["rotation"]["qz"],
+                    data["rotation"]["qw"],
+                ])
+                t = Transform(source, target, translation, rotation)
+                key = f"{source}->{target}"
+                transforms[key] = t
+                if key not in order:
+                    order.append(key)
+
+    return transforms, order
+
+
+def save_extrinsics_and_order(
+    yaml_dir: Path,
+    transforms: Dict[str, "Transform"],
+    order: List[str],
+) -> None:
+    """
+    将 transforms 与 order 写回 yaml_dir：extrinsics/000.yaml 等 + order_manifest.yaml。
+    """
+    extrinsics_dir = yaml_dir / "extrinsics"
+    extrinsics_dir.mkdir(exist_ok=True)
+
+    for yaml_file in extrinsics_dir.glob("*.yaml"):
+        yaml_file.unlink()
+
+    for i, key in enumerate(order):
+        if key not in transforms:
+            continue
+        transform = transforms[key]
+        filepath = extrinsics_dir / f"{i:03d}.yaml"
+        with open(filepath, "w", encoding="utf-8") as f:
+            yaml.dump(transform.to_dict(), f, default_flow_style=False, allow_unicode=True)
+
+    order_file = yaml_dir / "order_manifest.yaml"
+    order_data = {}
+    if order_file.exists():
+        with open(order_file, "r", encoding="utf-8") as f:
+            order_data = yaml.safe_load(f) or {}
+    order_data["extrinsics"] = [{"index": i, "file": f"extrinsics/{i:03d}.yaml"} for i in range(len(order))]
+    order_data["extrinsics_order"] = order
+    with open(order_file, "w", encoding="utf-8") as f:
+        yaml.dump(order_data, f, default_flow_style=False, allow_unicode=True, sort_keys=False)

tftree_manager-0.0.1/tftree_manager.egg-info/PKG-INFO

@@ -0,0 +1,79 @@
+Metadata-Version: 2.4
+Name: tftree-manager
+Version: 0.0.1
+Summary: TF 树梳理与变换管理：YAML 外参加载/保存、图路径与环检测、变换矩阵计算
+License-Expression: MIT
+Requires-Python: >=3.10
+Description-Content-Type: text/markdown
+Requires-Dist: numpy>=1.20.0
+Requires-Dist: PyYAML>=5.4.0
+Requires-Dist: Flask>=2.0.0
+Provides-Extra: dev
+Requires-Dist: pytest; extra == "dev"
+Requires-Dist: build; extra == "dev"
+Requires-Dist: twine; extra == "dev"
+
+# tftree-manager
+
+TF 树梳理与变换管理：从 YAML 目录加载/保存外参与顺序，构建无向图、路径查找、环检测、变换矩阵计算。
+
+## 安装
+
+```bash
+pip install tftree-manager
+```
+
+## 功能特性
+
+- 从 `yaml_dir/extrinsics/*.yaml` 与 `order_manifest.yaml` 加载/保存外参与顺序
+- 无向图构建、BFS 路径查找、有向环检测
+- 沿路径累积 4x4 变换矩阵、从根节点计算到所有可达节点
+- 更新变换、求逆、重排顺序并写回 YAML
+- 可选 Flask API 服务与命令行入口
+
+## 快速开始
+
+```python
+from tftree_manager import TFTreeManager
+
+manager = TFTreeManager("/path/to/yaml_dir")
+manager.load_from_yaml()
+
+# 所有坐标系
+frames = manager.get_all_frames()
+
+# 两节点间变换矩阵
+T = manager.get_transform("lidar", "camera")
+
+# 从根节点计算到所有节点
+transforms = manager.compute_all_transforms_from_root("world")
+
+# 检测环
+cycles = manager.detect_cycles()
+
+# 保存
+manager.save_to_yaml()
+```
+
+## 命令行
+
+```bash
+# 以 YAML 目录为参数，加载并打印树与环信息
+tftree-manager /path/to/yaml_dir
+
+# 启动 Flask API 服务（默认 0.0.0.0:5000）
+tftree-manager-server
+```
+
+环境变量 `TFTREE_DATA_DIR` 可指定 API 默认数据目录（默认 `./data`）。
+
+## 依赖
+
+- Python >= 3.10
+- numpy >= 1.20.0
+- PyYAML >= 5.4.0
+- Flask >= 2.0.0（API 与 server 入口需要）
+
+## License
+
+MIT

tftree_manager-0.0.1/tftree_manager.egg-info/SOURCES.txt

@@ -0,0 +1,17 @@
+MANIFEST.in
+README.md
+pyproject.toml
+tftree_manager/__init__.py
+tftree_manager/app.py
+tftree_manager/graph_ops.py
+tftree_manager/manager.py
+tftree_manager/tftree_errors.py
+tftree_manager/transform.py
+tftree_manager/transform_math.py
+tftree_manager/yaml_io.py
+tftree_manager.egg-info/PKG-INFO
+tftree_manager.egg-info/SOURCES.txt
+tftree_manager.egg-info/dependency_links.txt
+tftree_manager.egg-info/entry_points.txt
+tftree_manager.egg-info/requires.txt
+tftree_manager.egg-info/top_level.txt

tftree_manager-0.0.1/tftree_manager.egg-info/dependency_links.txt

@@ -0,0 +1 @@
+

tftree_manager-0.0.1/tftree_manager.egg-info/entry_points.txt

@@ -0,0 +1,3 @@
+[console_scripts]
+tftree-manager = tftree_manager.manager:main
+tftree-manager-server = tftree_manager.app:run

tftree_manager-0.0.1/tftree_manager.egg-info/requires.txt

@@ -0,0 +1,8 @@
+numpy>=1.20.0
+PyYAML>=5.4.0
+Flask>=2.0.0
+
+[dev]
+pytest
+build
+twine

tftree_manager-0.0.1/tftree_manager.egg-info/top_level.txt

@@ -0,0 +1,2 @@
+dist
+tftree_manager