彩色方塊偵測與位置估算

本範例示範如何使用 RGB-D 相機偵測彩色方塊，並估算其在相機座標系中的 3D 位置。

此範例設計為 顏色設定檔建立（Color Profile Creation） 的下一個步驟。前一個工具會建立 HSV 顏色設定檔，並儲存至 color_profiles.json。本偵測器會載入這些 HSV 設定檔，在 RGB 影像中尋找符合條件的顏色區域，讀取每個偵測方塊中心周圍的對齊深度資料，並將 2D 影像位置轉換為 3D 位置。

提示

這是一個以教學為導向的範例。目前的實作為了相容既有的下游工作流程，會使用 AprilTagDetectionArray 訊息格式發布結果。實際偵測的物件是彩色方塊，並不是真正的 AprilTag。

概述

偵測器會依序執行以下步驟：

RGB Image + Aligned Depth Image + CameraInfo
        |
        v
Load HSV profiles from color_profiles.json
        |
        v
Convert BGR image to HSV
        |
        v
Create color mask from HSV ranges
        |
        v
Apply ROI and contour filtering
        |
        v
Find block center in image coordinates
        |
        v
Read median depth around the center point
        |
        v
Convert pixel + depth to 3D camera-frame position
        |
        v
Estimate approximate 2D yaw from the contour
        |
        v
Publish debug image and detection result

您將學到什麼

閱讀並執行本範例後，您將了解如何：

從 JSON 檔案載入 HSV 顏色設定檔。
使用 OpenCV 顏色閾值處理偵測指定顏色的方塊。
使用 cv2.inRange() 建立二值化遮罩。
使用 cv2.findContours() 尋找候選方塊區域。
使用 ROI、輪廓面積與深度範圍過濾偵測結果。
使用對齊後的深度資料估算物件距離。
使用 CameraInfo 提供的相機內參，將影像座標轉換為 3D 座標。
從旋轉矩形估算近似的 yaw 角度。
發布除錯影像與偵測結果，供後續機器手臂規劃使用。

與顏色設定檔建立的關係

本範例會使用前一個工具所產生的輸出：

Color Profile Creation
        |
        v
color_profiles.json
        |
        v
Color Block Detection & Position Estimation

預期的 JSON 格式如下：

{
  "profiles": {
    "blue": {
      "ranges": [
        {
          "lower": [95, 80, 60],
          "upper": [125, 255, 255]
        }
      ],
      "created_from": {
        "rgb_topic": "/camera/color/image_raw",
        "samples": 8
      }
    }
  }
}

每個設定檔都包含一組或多組 HSV 範圍。對於紅色這類顏色，多組範圍特別有用，因為其 Hue 值可能跨越 OpenCV Hue 範圍的 0 / 180 邊界。

當 color_profiles.json 的檔案修改時間發生變化時，偵測器會自動重新載入該檔案。這表示您可以更新設定檔後直接繼續測試，不需要重新啟動節點。

為什麼在這裡使用 HSV？

偵測器不會直接比較原始 RGB 數值，而是將相機影像轉換為 HSV，再套用 color_profiles.json 中定義的 HSV 閾值。

HSV 的優點在於能將顏色資訊與亮度資訊分離：

Channel	意義	用途
H	Hue / 顏色類型	有助於辨識主要顏色，例如紅色、綠色、藍色或黃色。
S	Saturation / 顏色純度	有助於排除灰色、白色或低彩度區域。
V	Value / 亮度	有助於處理同一物件在明亮或陰暗區域的差異。

OpenCV 使用以下 HSV 數值範圍：

H: 0 to 180
S: 0 to 255
V: 0 to 255

這與許多線上 HSV 工具不同，這些工具通常會將 Hue 顯示為 0 to 360 度。

主要處理流程

1. 訂閱 RGB、Depth 與相機資訊

節點會訂閱三個輸入 topic：

輸入	預設 topic	用途
RGB 影像	`/camera/color/image_raw`	用於顏色偵測。
對齊後的深度影像	`/camera/aligned_depth_to_color/image_raw`	用於估算距離。
CameraInfo	`/camera/color/camera_info`	用於取得相機內參。

深度影像應與 RGB 影像完成對齊。也就是說，相同的 RGB 像素座標與深度像素座標應對應到場景中的同一個位置。

2. 載入目標顏色設定檔

節點會從以下檔案載入 HSV 範圍：

./color_profiles.json

預設目標設定檔為：

blue

可透過 ROS 2 參數變更目標顏色：

-p target_color:=green

節點也支援：

-p target_color:=all

當 target_color 設定為 all 時，偵測器會載入 JSON 檔案中的所有設定檔，並嘗試偵測所有已設定的顏色。

3. 將 BGR 影像轉換為 HSV

OpenCV 會以 BGR 格式處理影像。偵測器會在套用顏色設定檔之前，先將影像轉換為 HSV：

hsv = cv2.cvtColor(bgr_for_detection, cv2.COLOR_BGR2HSV)

接著使用以下方式套用每組 HSV 範圍：

mask = cv2.inRange(hsv, lower, upper)

若設定檔包含多組範圍，遮罩會使用 bitwise OR 合併。

4. 套用 ROI 過濾

偵測器會使用正規化影像座標套用可設定的感興趣區域（ROI）：

roi_x_min, roi_x_max
roi_y_min, roi_y_max

預設 ROI：

x = 影像寬度的 20% 到 80%
y = 影像高度的 48% 到 100%

只有位於 ROI 內的像素會參與偵測。對於機器手臂展示而言，如果方塊固定放置在桌面上的已知工作區域中，這項功能相當實用。

除錯影像也會繪製演算法實際使用的 ROI，讓使用者可以清楚看到偵測有效範圍。

5. 尋找顏色區域

建立遮罩並套用 ROI 後，節點會利用輪廓尋找相連的顏色區域：

contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

較小的區域會透過以下參數忽略：

min_contour_area

這有助於移除雜訊與過小的誤偵測區域。

在 target_color:=all 模式下，偵測器也會記錄已使用的遮罩區域，以減少不同顏色設定檔之間的重複偵測。

6. 估算方塊中心

對每個有效輪廓，偵測器會計算影像矩，並將輪廓中心作為方塊中心：

u = 影像座標中的中心 x
v = 影像座標中的中心 y

中心點會繪製在除錯影像上。

7. 讀取中心點周圍的深度資訊

節點不只使用單一深度像素，而是會讀取中心點周圍的一小塊區域：

depth_patch_radius = 2

因此預設區域大小為：

5 x 5 pixels

節點會先過濾無效深度值，再取有效資料的中位數深度。

相比只使用單一深度像素，這種方式更穩定，因為深度影像可能包含雜訊或缺失值。

可接受的深度範圍可透過以下參數設定：

depth_min_m = 0.30
depth_max_m = 0.60

如果偵測到的方塊深度超出此範圍，該偵測結果會被忽略。

8. 將像素與深度轉換為 3D 位置

偵測器會使用 CameraInfo 中的相機內參：

fx, fy, cx, cy

接著將中心像素與深度轉換為相機座標系中的 3D 座標：

X = (u - cx) * Z / fx
Y = (v - cy) * Z / fy
Z = depth

其中：

符號	意義
`u`	像素 x 座標
`v`	像素 y 座標
`Z`	以公尺為單位的深度
`fx`, `fy`	相機焦距
`cx`, `cy`	相機光學中心

輸出位置位於相機座標系中。

發布的 Pose 也會加上一個可設定的 Z 軸偏移量：

pose_z_offset_m = 0.03

當目標 Pose 需要略高於量測到的方塊表面時，此設定會相當實用。

9. 估算近似 2D Yaw

節點會使用下列方式，從偵測輪廓估算近似的 2D yaw 角度：

rect = cv2.minAreaRect(contour)

此函式會建立包圍輪廓的旋轉矩形，接著從該矩形的長邊方向估算 yaw 角度。

這適合用於教學中說明方向估測的概念，但不能取代完整的 6D 物件姿態估測器。

yaw 角度會轉換為只包含 yaw 的 quaternion：

qx = 0
qy = 0
qz = sin(yaw / 2)
qw = cos(yaw / 2)

10. 發布除錯影像與偵測結果

節點會發布：

輸出	預設 topic	用途
除錯影像	`/color_block/debug_image`	顯示 ROI、輪廓、中心點、旋轉矩形、yaw 箭頭與估算位置文字。
偵測結果	`/tag_detections`	使用 `AprilTagDetectionArray` 發布偵測到的方塊姿態。

輸出參數名稱為：

detection_topic

為了相容既有工作流程，預設 topic 仍為 /tag_detections。

執行方式

1. 準備顏色設定檔

先執行 Color Profile Creation 工具並儲存設定檔：

color_profiles.json

請確認設定檔名稱與本偵測器使用的目標顏色一致，例如：

blue

2. 啟動 RGB-D 相機

啟動相機驅動程式，並確認以下 topic 已存在：

ros2 topic list

預期 topic：

/camera/color/image_raw
/camera/aligned_depth_to_color/image_raw
/camera/color/camera_info

3. 執行偵測器

範例：

python3 ros2_color_block_detector.py \
  --ros-args \
  -p rgb_topic:=/camera/color/image_raw \
  -p depth_topic:=/camera/aligned_depth_to_color/image_raw \
  -p camera_info_topic:=/camera/color/camera_info \
  -p profile_file:=./color_profiles.json \
  -p target_color:=blue

偵測所有已設定的顏色設定檔：

python3 ros2_color_block_detector.py \
  --ros-args \
  -p profile_file:=./color_profiles.json \
  -p target_color:=all

調整偵測 ROI：

python3 ros2_color_block_detector.py \
  --ros-args \
  -p profile_file:=./color_profiles.json \
  -p target_color:=blue \
  -p roi_x_min:=0.20 \
  -p roi_x_max:=0.80 \
  -p roi_y_min:=0.48 \
  -p roi_y_max:=1.00

調整有效深度範圍：

python3 ros2_color_block_detector.py \
  --ros-args \
  -p profile_file:=./color_profiles.json \
  -p target_color:=blue \
  -p depth_min_m:=0.30 \
  -p depth_max_m:=0.60

ROS 2 參數

參數	預設值	說明
`rgb_topic`	`/camera/color/image_raw`	RGB 影像輸入 topic。
`depth_topic`	`/camera/aligned_depth_to_color/image_raw`	對齊後的深度影像輸入 topic。
`camera_info_topic`	`/camera/color/camera_info`	相機內參輸入 topic。
`debug_image_topic`	`/color_block/debug_image`	除錯影像輸出 topic。
`detection_topic`	`/tag_detections`	偵測結果輸出 topic。
`target_color`	`blue`	目標顏色設定檔名稱。使用 `all` 可偵測所有設定檔。
`profile_file`	`./color_profiles.json`	HSV 設定檔 JSON 路徑。
`depth_unit`	`mm`	深度影像單位。`mm` 表示毫米，`m` 表示公尺。
`depth_min_m`	`0.30`	最小有效深度，單位為公尺。
`depth_max_m`	`0.60`	最大有效深度，單位為公尺。
`depth_patch_radius`	`2`	中心點周圍深度取樣區域半徑。`2` 表示 `5 x 5` 區域。
`min_contour_area`	`1000.0`	用於過濾小型雜訊的最小輪廓面積。
`roi_x_min`	`0.20`	ROI 左邊界，以影像寬度正規化。
`roi_x_max`	`0.80`	ROI 右邊界，以影像寬度正規化。
`roi_y_min`	`0.48`	ROI 上邊界，以影像高度正規化。
`roi_y_max`	`1.00`	ROI 下邊界，以影像高度正規化。
`output_frame_id`	`camera_link`	發布偵測結果 Header 所使用的 Frame ID。
`pose_z_offset_m`	`0.03`	加到發布 Pose 位置上的 Z 軸偏移量。
`enable_preprocess`	`false`	在較困難的影像條件下啟用選用的影像前處理。
`process_period_s`	`0.05`	處理計時器週期，單位為秒。

結果視覺化

您可以使用 rqt_image_view 查看除錯影像：

rqt_image_view /color_block/debug_image

除錯影像會顯示：

偵測 ROI。
顏色輪廓。
偵測到的顏色名稱。
方塊中心點。
包圍輪廓的旋轉矩形。
Yaw 方向箭頭。
估算的 3D 位置。

輸出結果概念

偵測器會將訊息發布到設定的 detection_topic。

概念上，每個偵測結果包含：

color name
image center point
2D rotated box corners
3D position: X, Y, Z
orientation: yaw-only quaternion

目前的實作會將顏色設定檔名稱儲存在：

det.family

例如：

family: blue
pose.position.x: X
pose.position.y: Y
pose.position.z: Z + pose_z_offset_m
pose.orientation.z: qz
pose.orientation.w: qw

疑難排解

無法偵測到方塊

請確認：

target_color 是否與 color_profiles.json 中的設定檔名稱一致。
設定檔路徑是否正確。
方塊是否位於 ROI 範圍內。
輪廓面積是否大於 min_contour_area。
方塊是否位於有效深度範圍內。
目前光線條件是否與建立設定檔時相近。

除錯影像顯示 `WAITING FOR DEPTH`

這表示已收到 RGB 影像，但深度影像尚未準備完成。

請檢查：

ros2 topic echo /camera/aligned_depth_to_color/image_raw --once

並確認深度 topic 已與 RGB 影像對齊。

除錯影像顯示 `WAITING FOR CAMERA INFO`

這表示節點尚未收到相機內參。

請檢查：

ros2 topic echo /camera/color/camera_info --once

除錯影像顯示 `NO VALID COLOR PROFILE`

這通常表示設定檔無法載入，或指定的目標設定檔不存在。

請確認：

profile_file 路徑。
JSON 格式。
target_color 名稱。
JSON 最上層欄位名稱是否為 profiles。

位置看起來不正確

請確認：

深度單位是否正確。
CameraInfo topic 是否對應到 RGB 相機。
深度影像是否已對齊彩色影像。
方塊中心點是否落在無效或缺失的深度像素上。
目標方塊是否位於 depth_min_m 與 depth_max_m 範圍內。

誤偵測過多

可嘗試：

在目前的光線條件下重新建立 HSV 顏色設定檔。
增加 min_contour_area。
使用較乾淨的背景。
縮小顏色設定檔中的 HSV margin。
將偵測範圍限制在更小的 ROI。
如果光線條件較複雜，可嘗試設定 enable_preprocess:=true。

教學說明

本範例適合用於說明機器手臂工作流程中的感知部分：

Color Profile
    -> HSV Color Detection
    -> Contour Filtering
    -> Center Point Estimation
    -> Depth Lookup
    -> 3D Position Estimation
    -> Approximate Orientation Estimation
    -> Robot Arm Planning

此範例刻意保持簡單並易於修改。若要用於更進階的應用，可進一步加入：

更完整的 RGB 與深度影像同步機制。
用於清理遮罩的 Morphological Operations。
更穩健的輪廓過濾方式。
使用自訂偵測訊息取代 AprilTagDetectionArray。
完整的 6D 姿態估測。
從相機座標系轉換至機器人 Base Frame 的 TF Transform。

下一步

估算出的 3D 位置與 yaw 方向可以傳送到 MoveIt 等運動規劃模組。

在機器手臂展示中，系統即可依序：

偵測指定顏色的方塊。
估算方塊姿態。
將目標 Pose 傳送給 Planner。
控制機器手臂執行抓取或堆疊動作。

概述​

您將學到什麼​

與顏色設定檔建立的關係​

為什麼在這裡使用 HSV？​

主要處理流程​

1. 訂閱 RGB、Depth 與相機資訊​

2. 載入目標顏色設定檔​

3. 將 BGR 影像轉換為 HSV​

4. 套用 ROI 過濾​

5. 尋找顏色區域​

6. 估算方塊中心​

7. 讀取中心點周圍的深度資訊​

8. 將像素與深度轉換為 3D 位置​

9. 估算近似 2D Yaw​

10. 發布除錯影像與偵測結果​

執行方式​

1. 準備顏色設定檔​

2. 啟動 RGB-D 相機​

3. 執行偵測器​

ROS 2 參數​

結果視覺化​

輸出結果概念​

疑難排解​

無法偵測到方塊​

除錯影像顯示 WAITING FOR DEPTH​

除錯影像顯示 WAITING FOR CAMERA INFO​

除錯影像顯示 NO VALID COLOR PROFILE​

位置看起來不正確​

誤偵測過多​

教學說明​

下一步​

概述

您將學到什麼

與顏色設定檔建立的關係

為什麼在這裡使用 HSV？

主要處理流程

1. 訂閱 RGB、Depth 與相機資訊

2. 載入目標顏色設定檔

3. 將 BGR 影像轉換為 HSV

4. 套用 ROI 過濾

5. 尋找顏色區域

6. 估算方塊中心

7. 讀取中心點周圍的深度資訊

8. 將像素與深度轉換為 3D 位置

9. 估算近似 2D Yaw

10. 發布除錯影像與偵測結果

執行方式

1. 準備顏色設定檔

2. 啟動 RGB-D 相機

3. 執行偵測器

ROS 2 參數

結果視覺化

輸出結果概念

疑難排解

無法偵測到方塊

除錯影像顯示 `WAITING FOR DEPTH`

除錯影像顯示 `WAITING FOR CAMERA INFO`

除錯影像顯示 `NO VALID COLOR PROFILE`

位置看起來不正確

誤偵測過多

教學說明

下一步