第一段:Active 和 Passive 是什麼
最直覺的區別
給一個旋轉矩陣 R,有兩種完全不同的使用方式:
Active transform(主動變換)
點在動,座標系不動。
p' = R × p
物理意義:把點 p 旋轉到新位置 p',兩者在同一個座標系裡描述。
Passive transform(被動變換)
點不動,座標系在換。
p_B = R × p_A
物理意義:同一個物理點,在座標系 A 裡是 p_A,在座標系 B 裡是 p_B。點沒有移動,只是換了一個角度描述它。
關鍵關係
Active 和 passive 使用的是互為逆矩陣的旋轉:
R_passive = R_active⁻¹ = R_activeᵀ (旋轉矩陣的逆等於轉置)
為什麼矩陣長得一模一樣卻語義不同
Active: p' = R × p (p 被旋轉到新位置)
Passive: p_B = R × p_A (同一個點,不同 frame 的表達)
兩個式子的形式完全相同。如果沒有人告訴你這個 R 是哪種 convention,光看式子無法判斷——程式跑得起來,數值也在合理範圍,但語義是錯的。
第二段:ROS2 TF 是 Passive Convention
lookup_transform(target_frame, source_frame) 回傳的 transform T 滿足:
p_target = T × p_source
同一個物理點,在 source_frame 裡是 p_source,在 target_frame 裡是 p_target。TF 從來不「移動點」,只是幫你換一個座標系描述它。
當你發布一個靜態 TF:
tf_msg.header.frame_id = 'base_link' # parent
tf_msg.child_frame_id = 'camera_link' # child
tf_msg.transform = T這個 T 的語義是:
「把
camera_link裡的點,轉換到base_link所需要的 transform。」
第三段:兩步判斷框架
把任何 calibration 結果發布到 TF 之前,先走完這兩步。兩步是獨立的,各自可能出錯,症狀也不同。
Step 1:決定 TF tree 的層級
誰是 parent?誰是 child?
由機器人的物理結構決定,和數學無關:
base_link(機器人底座)
└─ camera_link(相機) ← 相機是底座的子節點
Step 2:確認 T 的方向
你手上的 T,是否滿足
p_parent = T × p_child?
- 是 → 直接發布,不需要 invert
- 否 → 先
T = inv(T),再發布
第四段:案例一 — ICP
ICP 輸出的語義
ICP calibration 設定:
- Source:相機點雲,在
camera_depth_optical_frame座標系 - Target:URDF + FK 重建的理論點雲,在
base_link座標系
ICP 求解 T,使得:
T × p_camera ≈ p_base
對應兩步框架:
- Step 1: parent =
base_link,child =camera_depth_optical_frame✅ - Step 2: T 滿足
p_base = T × p_camera✅
→ T 可以直接發布,不需要 invert。
為什麼 ICP 輸出剛好不需要 invert? 不是因為 ICP 是「passive 演算法」,而是因為我們的 problem setup:source 在 camera frame、target 在 base frame,ICP 求出的 T 自然滿足 TF 的要求。如果哪天 source/target 對調,輸出就會反過來。
如果 Step 1 填反了
# 錯誤:parent/child 對調
static_tf.header.frame_id = 'camera_link' # ❌
static_tf.child_frame_id = 'base_link' # ❌
static_tf.transform = T_icplookup_transform('base_link', 'camera_link') 回傳的是 inv(T_icp) 而不是 T_icp。
數學上發生了什麼:
正確:p_base = T_icp × p_camera
填反:p_base = inv(T_icp) × p_camera
inv(T_icp) 是「把 base 的點轉到 camera 空間」的 transform,拿它來轉換 camera 點雲,等於把點雲送往錯誤的方向。
症狀:
假設相機在機器人前方 0.5m、右側 0.2m:
正確 T_icp 的效果:camera 的點 → 出現在 base_link 前方 0.5m
inv(T_icp) 的效果:camera 的點 → 出現在 base_link 後方 0.5m(鏡像位置)
點雲整體呈現鏡像翻轉,且翻轉的軸和相機相對機器人的安裝位置有關,不一定是單純的左右鏡像。
如果 Step 2 出錯(ICP source/target 對調)
如果誰不小心把 ICP 的 source 和 target 對調:
# 錯誤:source 和 target 反了
result = icp(source=theoretical_cloud, target=actual_camera_cloud)ICP 輸出的 T 滿足:
T × p_base ≈ p_camera
也就是 inv(T_icp)。此時 Step 2 出錯,需要 invert 才能用,但症狀和 Step 1 填反完全一樣——點雲鏡像翻轉。
這兩種錯的症狀相同,但根本原因不同,要用不同方式修。
診斷方式:用附錄 D 的原點測試確認 T 的方向,再判斷是 Step 1 還是 Step 2 出問題。
如果 Step 1 和 Step 2 同時出錯(最危險的情況)
# Step 2 出錯:T 是 inv(T_icp)
# Step 1 出錯:parent/child 又填反了
static_tf.header.frame_id = 'camera_link' # ❌
static_tf.child_frame_id = 'base_link' # ❌
static_tf.transform = inv(T_icp) # ❌TF 看到 parent/child 反了,查詢時會再取一次 inv:
lookup_transform('base_link', 'camera_link')
= inv(inv(T_icp))
= T_icp ← 正確的值!
兩個錯誤互相抵消,點雲顯示結果完全正確。
這是最難察覺的情況。它只會在其他情境下爆發——例如當你把這個 TF 鏈接到其他節點,或者有其他程式直接查詢 lookup_transform('camera_link', 'base_link') 時,就會拿到錯誤的結果。
工程上的啟示: 「結果看起來正確」不代表實作是對的。TF 的 parent/child 語義影響的是整個 tree,不只是單一的查詢方向。
第五段:案例二 — OpenCV solvePnP
幾乎所有做 camera-robot calibration 或 AR marker 定位的人都踩過這個坑。
solvePnP 的輸出語義
cv2.solvePnP 輸出 rvec 和 tvec,語義是:
p_camera = R × p_world + t
把 world(或 robot base)的點轉到 camera 空間——這是 active convention。
對應兩步框架:
- Step 1: parent =
base_link,child =camera_link✅ - Step 2: T 滿足
p_camera = T × p_world,方向反了 ❌
→ 需要 invert T,再發布。
如果 Step 2 沒有 invert(直接發布)
# 錯誤:直接用 solvePnP 的結果
R_mat, _ = cv2.Rodrigues(rvec)
tvec = tvec.reshape(3, 1)
static_tf.header.frame_id = 'base_link'
static_tf.child_frame_id = 'camera_link'
static_tf.transform.rotation = quaternion_from_matrix(R_mat) # ❌
static_tf.transform.translation = tvec # ❌數學上發生了什麼:
solvePnP 的 tvec 代表「world 原點在 camera 座標系裡的位置」,不是「camera 原點在 world 座標系裡的位置」。
具體例子:
實際情況:相機在機器人前方 0.5m、高度 0.3m
solvePnP 的 tvec:約等於 [-0.5, -0.3, ...] (machine origin in camera space)
直接發布後,TF 認為 camera_link 在 base_link 的位置是 [-0.5, -0.3, ...]
實際上 camera_link 應該在 [0.5, 0, 0.3]
症狀:
相機的 frame 出現在近似「預期位置的負方向」——如果相機應該在機器人前上方,它反而出現在機器人後下方。旋轉方向也是反的。
若是用轉換後的點雲去對照理論模型:點雲出現的位置和理論模型關於某個軸近似對稱,但不是簡單的鏡像。
正確做法(含 SE(3) inverse 說明)
R_mat, _ = cv2.Rodrigues(rvec)
tvec = tvec.reshape(3, 1) # OpenCV 有時回傳 (3,1),有時 (3,),先統一
# solvePnP 給的是 p_camera = R × p_world + t(active)
# 需要 invert,公式為 SE(3) inverse:
#
# T = [R | t] → T⁻¹ = [Rᵀ | -Rᵀt]
# [0 | 1] [0 | 1 ]
#
# 注意:平移部分不是直接取負號,必須先乘上 Rᵀ
R_inv = R_mat.T
t_inv = -R_inv @ tvec
static_tf.header.frame_id = 'base_link'
static_tf.child_frame_id = 'camera_link'
static_tf.transform.rotation = quaternion_from_matrix(R_inv)
static_tf.transform.translation = t_inv.flatten()如果 Step 1 也填反了
# 兩步都錯
static_tf.header.frame_id = 'camera_link' # ❌
static_tf.child_frame_id = 'base_link' # ❌
static_tf.transform = T_pnp # 未 invert ❌和 ICP 的情況一樣,兩個錯誤互相抵消,lookup_transform('base_link', 'camera_link') 的結果可能意外正確,但 TF tree 語義是錯的,在其他查詢方向或其他節點使用時會爆。
四種組合的症狀對比
| Step 1 | Step 2 | 症狀 |
|---|---|---|
| ✅ 正確 | ✅ 正確 | 點雲對齊,顯示正確 |
| ❌ 填反 | ✅ 正確 | 點雲鏡像翻轉 |
| ✅ 正確 | ❌ 未 invert | 點雲出現在預期位置的「負方向」 |
| ❌ 填反 | ❌ 未 invert | 表面上看起來正確,但語義錯誤,換個查詢方向就爆 |
第六段:現場快速驗證
方法一:原點測試(發布前)
# 把 child frame 的原點乘上 T,應該得到 child 在 parent 的物理位置
origin = np.array([0, 0, 0, 1])
result = T @ origin
print("child frame 原點在 parent frame 的位置:", result[:3])
# 對照:相機實際裝在機器人哪裡?方法二:RViz 或前端視覺化(發布後)
| 看到的現象 | 最可能的原因 | 對應修正 |
|---|---|---|
| 點雲對齊 | 正確 | — |
| 點雲鏡像翻轉 | Step 1 填反,或 Step 2 source/target 對調 | 原點測試確認後修正 |
| 點雲出現在預期位置的負方向 | Step 2 未 invert(solvePnP 常見) | invert T |
| 點雲看起來對,但換個查詢方向就錯 | Step 1 和 Step 2 同時錯(抵消) | 重新確認 parent/child 和 T 方向 |
| Frame 完全不出現 | TF tree 斷掉 | ros2 run tf2_tools view_frames 確認結構 |
核心要點
必須記住的三件事
- Active = 點在動;Passive = 座標系在換,數學形式相同,語義完全不同
- ROS2 TF 永遠是 passive:
lookup_transform(A, B)回傳的 T 滿足p_A = T × p_B - Step 1 和 Step 2 各自獨立出錯,症狀不同;兩個同時錯反而可能表面上正確,是最危險的情況
附錄:Step-by-Step 對照查詢表
從哪裡開始:
- 不知道從哪裡起步 → 從 [A] 開始,依序走
- 知道 T 的來源,但不確定方向 → 直接跳 [D] 做原點測試
- 已經發布 TF,但顯示異常 → 直接跳 [E] 對症查詢
A. 先確認你手上的 T 是從哪裡來的
| 來源 | T 的語義 | 進入下一步 |
|---|---|---|
| ICP(source=camera, target=robot) | p_robot = T × p_camera | → [B] |
OpenCV solvePnP (rvec, tvec) | p_camera = T × p_world | → [C] |
lookup_transform(A, B) 的回傳值 | p_A = T × p_B | → [B] |
| 自己寫的 FK / URDF | 看你怎麼計算 | → [D] |
| 其他不確定的來源 | 不確定 | → [D] |
B. T 方向正確(p_parent = T × p_child)→ 直接發布
static_tf.header.frame_id = 'parent_frame'
static_tf.child_frame_id = 'child_frame'
static_tf.transform = T確認 parent/child 的層級是否和機器人物理結構一致。
C. T 方向反了(p_child = T × p_parent)→ 先 invert,再發布
# 通用做法
T_inv = np.linalg.inv(T)
# SE(3) 解析公式(數值更穩定):
R = T[:3, :3]
t = T[:3, 3:]
R_inv = R.T
t_inv = -R_inv @ t # 平移不是直接取負號,要先乘 R_inv
static_tf.header.frame_id = 'parent_frame'
static_tf.child_frame_id = 'child_frame'
static_tf.transform = T_invD. 不確定 T 的方向 → 用原點測試判斷
origin = np.array([0, 0, 0, 1])
result = T @ origin
print("child frame 原點在 parent frame 的位置:", result[:3])| 結果 | 代表 | 下一步 |
|---|---|---|
| 和 child 的物理位置吻合 | T 方向正確 | → [B] |
| 是物理位置的鏡像或負值 | T 方向反了 | → [C] |
| 接近 (0, 0, 0) | parent/child 可能填反,重查 Step 1 | 交換 frame_id |
| 數值極大或 NaN | T 本身有問題(奇異矩陣、未初始化) | 回去查 calibration 流程 |
E. 發布後顯示異常 → 對症修正
症狀一:點雲鏡像翻轉
→ Step 1 填反 或 Step 2 source/target 對調
→ 先用 [D] 確認 T 方向,再決定是改 parent/child 還是 invert T
症狀二:點雲出現在預期位置的負方向(如前→後、上→下)
→ Step 2 未 invert(solvePnP 最常見)
→ 回到 [C],invert T
症狀三:點雲看起來對,但換個查詢方向就錯
→ Step 1 和 Step 2 同時錯互相抵消
→ 重新從 [A] 走一遍,不要依賴表面結果
症狀四:frame 完全不出現
→ TF tree 斷掉
→ ros2 run tf2_tools view_frames 確認結構
症狀五:frame 位置正確,但旋轉方向怪
→ optical frame 問題(camera_depth_frame vs camera_depth_optical_frame)
→ 參考:ROS2 Frame Tree 那篇文章
F. 常見情境快速對照
| 情境 | Step 1(parent → child) | 需要 invert? | 備注 |
|---|---|---|---|
| ICP source=camera, target=base | base_link → camera_optical | 否 | T 方向已符合 TF 要求 |
| ICP source=base, target=camera | base_link → camera_optical | 是 | source/target 對調,T 方向反了 |
| OpenCV solvePnP | base_link → camera_link | 是 | active convention |
| lookup_transform 結果再發布 | 同原本的 target → source | 否 | 本來就是 TF 格式 |
| hand-eye calibration,eye-in-hand | end_effector → camera_link | 查工具文件 | 文件說「T_cam_to_ee」→ 是;「T_ee_to_cam」→ 否;不確定 → 用 [D] |
| hand-eye calibration,eye-to-hand | base_link → camera_link | 查工具文件 | 文件說「T_cam_to_base」→ 是;「T_base_to_cam」→ 否;不確定 → 用 [D] |