所有文章

Transform 的方向語義:為什麼 Calibration 結果放進 TF 有時要 Invert,有時不用

·16 min read
ROS2
Robotics
Coordinate Systems
TF
Computer Vision
Calibration
ICP
OpenCV

第一段: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_icp

lookup_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 1Step 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 確認結構

核心要點

必須記住的三件事

  1. Active = 點在動;Passive = 座標系在換,數學形式相同,語義完全不同
  2. ROS2 TF 永遠是 passive:lookup_transform(A, B) 回傳的 T 滿足 p_A = T × p_B
  3. 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_inv

D. 不確定 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
數值極大或 NaNT 本身有問題(奇異矩陣、未初始化)回去查 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=basebase_link → camera_optical否T 方向已符合 TF 要求
ICP source=base, target=camerabase_link → camera_optical是source/target 對調,T 方向反了
OpenCV solvePnPbase_link → camera_link是active convention
lookup_transform 結果再發布同原本的 target → source否本來就是 TF 格式
hand-eye calibration,eye-in-handend_effector → camera_link查工具文件文件說「T_cam_to_ee」→ 是;「T_ee_to_cam」→ 否;不確定 → 用 [D]
hand-eye calibration,eye-to-handbase_link → camera_link查工具文件文件說「T_cam_to_base」→ 是;「T_base_to_cam」→ 否;不確定 → 用 [D]

參考資料