pose_graph op• 数据种类:pose → table
• 调用: import fullseye as fs; fs.ledger.optimize_pose_graph(poses_init, edges, fix_first=True, max_iter=200)(要直接调用实现,import pose_graph; pose_graph.optimize_pose_graph(poses_init, edges, fix_first=True, max_iter=200);从台账取用则 ops3d.get("optimize_pose_graph"))
由相对位姿约束 + 回环闭合优化全局位姿。→ dict{poses, rmse, cost}。
对 poses_init (N,6)=[rvec|t] 的初始估计(含漂移)进行优化,使其满足 edges 的相对位姿约束。fix_first=True 时固定第一个节点以消除 gauge。
• edges: (i, j, rvec_ij, t_ij[, w_rot, w_trans]) 的列表。(rvec_ij, t_ij) 是“从 i 看到的 j”= T_i⁻¹ ∘ T_j 的测量值(与 relative_pose 约定相同)。里程计 (i, i+1) 与回环(相距较远的 i, j)可以用同一形式混合。权重会把残差乘以 sqrt(w)(省略时为 1)。
• 各边的残差是把 measured⁻¹ ∘ predicted 转为旋转向量 + 平移的 6 维量, 用 scipy.optimize.least_squares(method="lm") 最小化(max_nfev = max_iter × 参数个数)。
• 返回值:poses (N,6)(若 fix_first 则第一个保持初始值)、rmse = mean_edge_error(poses, edges)、cost = 最终代价(残差 2 次方和的 1/2)。
fail-closed(失败即拒绝):节点数 < 2、边为 0 条、边的下标超出 [0, N)(负下标的静默回绕也拒绝)时抛出 ValueError。若设 fix_first=False,gauge 会残留,解不唯一(LM 返回初始值附近的 1 个解)。旋转为 rad,平移为坐标单位,由于两者排在同一个残差向量中,单位相差很大时用权重来对齐。既不使用 3D 点也不使用观测像素(包含它们进行优化的是 bundle_adjust)。
• 示例数据目录(下载 URL / 许可证) —— 2-D 用 skimage.data(BSD/公有领域)加合成图,3-D 给出真实数据源(Stanford/PDS 等)的下载 URL。
• 算子来历与参考文献 —— 该算子族所依据的研究/方法出处。
• pose_graph_slam — py -3.11 examples_3d/pose_graph_slam.py
table 作为输入)fuse_to_voxel · mesh_select_lod
pose_graph)relative_pose · mean_edge_error
*Provenance: pose_graph.py — 3D 算子登记表。本条目由 tools/opdocs.py md 自动生成(请勿手工编辑)。*
© 2026 Kazufumi Furuse — Fullseye operator documentation. Licensed under Apache-2.0.