5. ビジョンによる形状認識の実装

実験の目的

ROS2 で画像情報を取得し、トピックをパブリッシュできるようになる。2. 物体の形状を認識し、結果を出力できるようになる。

形状認識の原理

形状認識には複数特徴を融合した階層的判定の手法を用い、輪郭の幾何学的特徴を段階的に分析することで、さまざまな形状を正確に分類します。具体的な処理は以下のとおりです。

1. 前処理段階

輪郭の入力：検出された物体の輪郭を受け取ります。
面積によるフィルタリング：まず面積が小さすぎる輪郭を除外します（ノイズ除去）。

2. 円の検出

円形度の計算：式 4πA/P² で輪郭の円形度を計算します。
しきい値判定：円形度が設定したしきい値（例：0.7）を超えた場合は、そのまま円と判定します。
特徴：最も高速かつ直接的な検出経路です。

3. 多角形の分析（円の条件を満たさない場合）

多角形近似：Douglas-Peucker アルゴリズムで輪郭を多角形に近似します。
頂点数のカウント：近似した多角形の頂点数を計算します。

4. 頂点数に基づく分類

頂点 3 個：そのまま三角形と判定します。
頂点 4 個：さらに細かく長方形/正方形を判別します。
縦横比の分析：外接矩形の縦横比が 1:1 に近いかを確認します。
充実度（ソリディティ）の検出：輪郭の面積と凸包の面積の比を計算します。
総合判定：縦横比と充実度を組み合わせて正方形と長方形を区別します。
頂点 5 個：特別に処理し、実際の物体の特性に応じて三角形に補正する場合があります。
頂点 6〜10 個：円形度と組み合わせて再判定し、円または六角形として認識する場合があります。

5. 特殊な補正の仕組み

適応的な処理：角が丸みを帯びた非標準の物体に対して、形状のマッピングを補正します。
実用重視：具体的な応用シーンにおける物体の特性に応じて、認識結果を最適化します。

応用シーン

産業用仕分けロボットのビジョンシステム：自動化された生産ラインで物体の形状を認識し、ロボットアームに正確な対象分類と位置情報を提供します。特に、角が丸みを帯びた非標準形状の物体の処理に適しています。

機能要件

画像領域内の物体の形状を認識し、認識結果を表示します。たとえば、円、三角形、正方形、長方形などの形状の物体を認識できます。

ハードウェアの準備

No.	名称	数量	備考
1	RDK X5	1	組込みメインボード（32G メモリカード付き）。上位機システムを実行します。
2	カードリーダー	1	システムイメージの書き込みに使用します。
3	USB カメラ	1	画像を取得します。
4	電源ケーブル	1	給電用。
5	LAN ケーブル	1	ネットワーク接続用。

回路接続

ハードウェアモジュール

ヒント：ハードウェアモジュールの使用説明

1.1 ハードウェアインターフェース：RDK X5 のインターフェースをすばやく把握できます。

1.1 USB カメラの使い方《1.1 USB カメラの使い方》

実機の配線

ソフトウェア設計

設計プロセス

ビジョンによる形状認識の設計

ソースコードのダウンロード

資料のダウンロード・技術サポートについては、HumansX株式会社までお問い合わせください。

プログラムコード

コードの構成：

camera_node.py：リアルタイムの画像取得とパブリッシュを行い、カメラのトピックをパブリッシュします。

shape_detection.launch.py：複数特徴を融合した形状分類アルゴリズムで形状を認識し、結果をパブリッシュします。

具体的なコード：

shape_detection.launch.py は形状認識の処理プログラムです。プロジェクト全体のコードはソースコード（上記「ソースコードのダウンロード」参照）から入手してください。

"""
--------------------------------------------------------------------------------
功能:形状识别实验
描述:
    打开摄像头后,将图像转换为hsv空间,在图片对应的黄色区域中提取红, 绿, 蓝三种颜色的HSV
    空间,之后根据画出的轮廓判断物体对应的形状，最后将对应形状显示在图片上.
注意事项：
    这里由于本实验使用的物块边角选择的是有弧度的，所以，实际情况测试出来的结果与预期结果
    有一定的出入, 如果更换物块之后, 需要根据实际测试的vertices值, 调整输出对应的形状
日期:2025-09-26
--------------------------------------------------------------------------------
"""

# #!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
import numpy as np
import sys
import math

class ColorShapeDetector(Node):
    def __init__(self):
        super().__init__('color_shape_detector')

        # 声明 ROI 参数
        self.declare_parameters(
            namespace='',
            parameters=[
                ('roi_x', 155),
                ('roi_y', 120),
                ('roi_width', 385),
                ('roi_height', 250),
                ('min_area', 500),
                ('epsilon_scale', 0.03),  # 多边形逼近精度
                ('circularity_threshold', 0.7),  # 圆度阈值
                ('aspect_ratio_threshold', 0.7)  # 宽高比阈值
            ]
        )

        self.roi_x = self.get_parameter('roi_x').value
        self.roi_y = self.get_parameter('roi_y').value
        self.roi_w = self.get_parameter('roi_width').value
        self.roi_h = self.get_parameter('roi_height').value
        self.min_area = self.get_parameter('min_area').value
        self.epsilon_scale = self.get_parameter('epsilon_scale').value
        self.circularity_threshold = self.get_parameter('circularity_threshold').value
        self.aspect_ratio_threshold = self.get_parameter('aspect_ratio_threshold').value

        self.get_logger().info(f"ROI 设置: x={self.roi_x}, y={self.roi_y}, w={self.roi_w}, h={self.roi_h}")

        # 订阅原始图像
        self.subscription = self.create_subscription(
            Image,
            '/p4/img_raw',
            self.image_callback,
            10
        )
        self.subscription

        # 发布形状检测结果
        self.shape_pub = self.create_publisher(Image, '/p4/shape_detection', 10)

        self.bridge = CvBridge()

        # HSV 颜色范围
        self.color_ranges = {
            'red': [
                (np.array([0, 43, 46]), np.array([10, 255, 255])),
                (np.array([156, 43, 46]), np.array([180, 255, 255]))
            ],
            'green': [
                (np.array([35, 100, 100]), np.array([85, 255, 255]))
            ],
            'blue': [
                (np.array([90, 100, 100]), np.array([130, 255, 255]))
            ]
        }

        self.draw_colors = {
            'red': (0, 0, 255),
            'green': (0, 255, 0),
            'blue': (255, 0, 0)
        }

        self.get_logger().info("改进的形状检测节点已启动")

        # 为每种颜色单独建 mask 发布器
        self.mask_pubs = {}
        for color in self.color_ranges.keys():
            topic = f"/p4/mask_{color}"
            self.mask_pubs[color] = self.create_publisher(Image, topic, 10)
            self.get_logger().info(f"新增mask话题: {topic}")

    def calculate_circularity(self, contour):
        """计算圆度 (4πA/P²)"""
        area = cv2.contourArea(contour)
        perimeter = cv2.arcLength(contour, True)
        if perimeter > 0:
            circularity = 4 * math.pi * area / (perimeter * perimeter)
            return circularity
        return 0

    def calculate_aspect_ratio(self, contour):
        """计算边界框宽高比"""
        _, _, w, h = cv2.boundingRect(contour)
        if h > 0:
            return w / h
        return 0

    def detect_shape(self, contour):
        """改进的形状检测方法"""
        area = cv2.contourArea(contour)
        if area < self.min_area:
            return "unknown"

        # 方法1: 基于圆度检测圆形
        circularity = self.calculate_circularity(contour)
        if circularity > self.circularity_threshold:
            return "circle"

        # 方法2: 多边形逼近
        perimeter = cv2.arcLength(contour, True)
        epsilon = self.epsilon_scale * perimeter
        approx = cv2.approxPolyDP(contour, epsilon, True)
        vertices = len(approx)

        # 方法3: 凸性检测
        hull = cv2.convexHull(approx)
        hull_area = cv2.contourArea(hull)
        solidity = float(area) / hull_area if hull_area > 0 else 0

        # 方法4: 边界框特征
        aspect_ratio = self.calculate_aspect_ratio(contour)

        #综合判断形状。这里由于本实验使用的物块边角选择的是有弧度的，所以，实际情况测试出来的结果与预期结果
        #有一定的出入，如果更换物块之后，需要根据实际测试的vertices值，调整输出对应的形状
        # print("vertices:", vertices)
        #由于本实验选择的物块边角使用的有一定的弧度,所以，三个角的情况很少出现，故这里暂时不考虑该情况
        #如果更换为规则的物体，这里需要根据实际情况更改
        # if vertices == 3:
        #     return "triangle"
        if vertices == 4:
            # 进一步区分正方形和矩形
            if 0.8 <= aspect_ratio <= 1.2 and solidity > 0.9:
                # return "square"
                #这里原本是输出正方形, 但是由于本实验选择的物块边角使用的有一定的弧度，故这里认为是三角形
                #如果更换为规则的物体，这里需要根据实际情况更改
                return "triangle"
            else:
                return "rectangle"
        #这里原本是输出五边形, 由于本实验选择的物块边角使用的有一定的弧度，实际测试发现，
        #本实验使用的三角形会被误认为是五边形, 故这里认为是三角形.如果更换为规则的物体，这里需要根据实际情况更改
        elif vertices == 5:
            # return "pentagon"
            return "triangle"
        elif 6 <= vertices <= 10:
            # 对于更多顶点的多边形，结合圆度判断
            if circularity > 0.6:
                return "circle"  # 可能是近似圆形
            else:
                return "Hexagon"
        else:
            return "unknown"

    def image_callback(self, msg):
        try:
            cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
            original_h, original_w = cv_image.shape[:2]

            # 校验 ROI
            x1 = max(0, self.roi_x)
            y1 = max(0, self.roi_y)
            x2 = min(original_w, self.roi_x + self.roi_w)
            y2 = min(original_h, self.roi_y + self.roi_h)

            if x2 <= x1 or y2 <= y1:
                self.get_logger().warn("ROI 设置无效")
                return

            # 提取 ROI
            roi = cv_image[y1:y2, x1:x2]
            hsv_roi = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV)

            # 创建输出图像
            output = cv_image.copy()
            cv2.rectangle(output, (x1, y1), (x2, y2), (0, 255, 255), 5)  # 黄色 ROI 框

            detected_shapes = []

            for color_name in ['red', 'green', 'blue']:
                mask = None
                for lower, upper in self.color_ranges[color_name]:
                    temp_mask = cv2.inRange(hsv_roi, lower, upper)
                    if mask is None:
                        mask = temp_mask
                    else:
                        mask = cv2.bitwise_or(mask, temp_mask)

                if mask is None:
                    continue

                # 形态学操作
                kernel = np.ones((5, 5), np.uint8)
                mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
                mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)

                # 发布 mask
                mask_msg = self.bridge.cv2_to_imgmsg(mask, encoding="mono8")
                # mask_msg.header = header
                self.mask_pubs[color_name].publish(mask_msg)        

                # 查找轮廓
                contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

                for cnt in contours:
                    area = cv2.contourArea(cnt)
                    if area < self.min_area:
                        continue

                    # 转换为全局坐标
                    cnt_global = cnt + np.array([x1, y1])

                    # 判断形状
                    shape_name = self.detect_shape(cnt_global)
                    if shape_name == "unknown":
                        continue

                    # 计算轮廓特征用于显示
                    M = cv2.moments(cnt_global)
                    if M["m00"] != 0:
                        cx = int(M["m10"] / M["m00"])
                        cy = int(M["m01"] / M["m00"])
                    else:
                        cx, cy = 0, 0

                    # 绘制轮廓和中心点
                    cv2.drawContours(output, [cnt_global], -1, self.draw_colors[color_name], 2)
                    cv2.circle(output, (cx, cy), 5, (255, 255, 255), -1)

                    # 标注形状和颜色
                    label = f"{shape_name}"
                    cv2.putText(output, label, (cx-50, cy-35), cv2.FONT_HERSHEY_SIMPLEX, 
                               0.9, self.draw_colors[color_name], 2)

                    # 记录检测结果
                    detected_shapes.append(f"{color_name} {shape_name}")

            # # 在图像顶部显示检测结果摘要
            # if detected_shapes:
            #     summary = "Detected: " + ", ".join(detected_shapes)
            #     cv2.putText(output, summary, (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 
            #               0.7, (255, 255, 255), 2)

            # 发布形状检测结果
            shape_msg = self.bridge.cv2_to_imgmsg(output, "bgr8")
            shape_msg.header = msg.header
            self.shape_pub.publish(shape_msg)

        except Exception as e:
            self.get_logger().error(f"处理图像时出错: {e}")

    def destroy_node(self):
        cv2.destroyAllWindows()
        super().destroy_node()

def main(args=None):
    rclpy.init(args=args)
    node = ColorShapeDetector()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        print(f"节点运行出错: {str(e)}")
    finally:
        try:
            rclpy.shutdown()
        except RuntimeError as e:
            error_msg = str(e)
            if "already called" in error_msg or "already shutdown" in error_msg:
                pass
            else:
                print(f"Unexpected error during shutdown: {e}", file=sys.stderr)

if __name__ == '__main__':
    main()

# #!/usr/bin/env python3

# import rclpy
# from rclpy.node import Node
# from sensor_msgs.msg import Image
# from cv_bridge import CvBridge
# import cv2
# import numpy as np
# import sys

# class ColorShapeDetector(Node):
#     def __init__(self):
#         super().__init__('color_shape_detector')

#         # 声明 ROI 参数
#         self.declare_parameters(
#             namespace='',
#             parameters=[
#                 ('roi_x', 155),
#                 ('roi_y', 120),
#                 ('roi_width', 385),
#                 ('roi_height', 250),
#                 ('min_area', 500)
#             ]
#         )

#         self.roi_x = self.get_parameter('roi_x').value
#         self.roi_y = self.get_parameter('roi_y').value
#         self.roi_w = self.get_parameter('roi_width').value
#         self.roi_h = self.get_parameter('roi_height').value
#         self.min_area = self.get_parameter('min_area').value

#         self.get_logger().info(f"ROI 设置: x={self.roi_x}, y={self.roi_y}, w={self.roi_w}, h={self.roi_h}")

#         # 订阅原始图像
#         self.subscription = self.create_subscription(Image,'/p4/img_raw',self.image_callback,10)
#         self.subscription

#         # 发布形状检测结果
#         self.shape_pub = self.create_publisher(Image, '/p4/shape_detection', 10)

#         self.bridge = CvBridge()

#         # HSV 颜色范围
#         self.color_ranges = {
#             'red':   [ (np.array([0, 43, 46]), np.array([10, 255, 255])), 
#                        (np.array([156, 43, 46]), np.array([180, 255, 255]))],
#             'green': [ (np.array([35, 100, 100]), np.array([85, 255, 255])) ],
#             'blue':  [ (np.array([90, 100, 100]), np.array([130, 255, 255])) ]
#         }

#         self.draw_colors = {
#             'red': (0, 0, 255),
#             'green': (0, 255, 0),
#             'blue': (255, 0, 0)
#         }
#         self.center_color = (0, 165, 255)  # Orange

#         self.get_logger().info("改进版颜色+形状检测节点已启动")

#         # 为每种颜色单独建 mask 发布器
#         self.mask_pubs = {}
#         for color in self.color_ranges.keys():
#             topic = f"/p4/mask_{color}"
#             self.mask_pubs[color] = self.create_publisher(Image, topic, 10)
#             self.get_logger().info(f"新增mask话题: {topic}")

#     def detect_shape(self, contour):
#         """更鲁棒的形状识别"""
#         area = cv2.contourArea(contour)
#         if area < 100:  # 额外小面积过滤
#             return "unknown"

#         # 计算周长
#         peri = cv2.arcLength(contour, True)
#         if peri == 0:
#             return "unknown"

#         # 圆度: 4πA / P²，完美圆=1.0
#         circularity = 4 * np.pi * area / (peri * peri)

#         # 如果圆度很高，直接判为圆
#         if circularity > 0.85:
#             return "circle"

#         # 多边形近似（使用更小的 epsilon 提高精度）
#         epsilon = 0.02 * peri  # 原来是 0.04，现在更精细
#         approx = cv2.approxPolyDP(contour, epsilon, True)
#         num_vertices = len(approx)

#         # 凸性检查：排除凹多边形（如星形）
#         hull = cv2.convexHull(contour)
#         hull_area = cv2.contourArea(hull)
#         if hull_area == 0:
#             return "unknown"
#         solidity = float(area) / hull_area  # 实心度
#         if solidity < 0.8:  # 太凹的形状跳过
#             return "unknown"

#         # 根据顶点数判断
#         if num_vertices == 3:
#             return "triangle"
#         elif num_vertices == 4:
#             # 可选：检查是否为矩形（纵横比接近1）
#             x, y, w, h = cv2.boundingRect(contour)
#             aspect_ratio = float(w) / h
#             if 0.85 <= aspect_ratio <= 1.15:
#                 return "square"
#             else:
#                 return "rectangle"
#         elif num_vertices == 5:
#             return "pentagon"
#         elif num_vertices == 6:
#             return "hexagon"
#         else:
#             # 顶点太多但圆度不够 → 可能是复杂形状，视为 unknown
#             return "unknown"

#     def image_callback(self, msg):
#         try:
#             cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
#             original_h, original_w = cv_image.shape[:2]

#             # 校验 ROI
#             x1 = max(0, self.roi_x)
#             y1 = max(0, self.roi_y)
#             x2 = min(original_w, self.roi_x + self.roi_w)
#             y2 = min(original_h, self.roi_y + self.roi_h)

#             if x2 <= x1 or y2 <= y1:
#                 self.get_logger().warn("ROI 设置无效")
#                 return

#             roi = cv_image[y1:y2, x1:x2]
#             hsv_roi = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV)

#             output = cv_image.copy()
#             cv2.rectangle(output, (x1, y1), (x2, y2), (0, 255, 255), 2)  # 黄色 ROI 框

#             for color_name in ['red', 'green', 'blue']:
#                 mask = None
#                 for lower, upper in self.color_ranges[color_name]:
#                     temp_mask = cv2.inRange(hsv_roi, lower, upper)
#                     if mask is None:
#                         mask = temp_mask
#                     else:
#                         mask = cv2.bitwise_or(mask, temp_mask)

#                 if mask is None:
#                     continue

#                 # 形态学操作（更强去噪）
#                 kernel = np.ones((5, 5), np.uint8)
#                 mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
#                 mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)

#                 # 发布 mask
#                 mask_msg = self.bridge.cv2_to_imgmsg(mask, encoding="mono8")
#                 # mask_msg.header = header
#                 self.mask_pubs[color_name].publish(mask_msg)

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

#                 for cnt in contours:
#                     area = cv2.contourArea(cnt)
#                     if area < self.min_area:
#                         continue

#                     cnt_global = cnt + np.array([x1, y1])
#                     shape_name = self.detect_shape(cnt_global)

#                     if shape_name == "unknown":
#                         continue

#                     # 绘制边界框
#                     x, y, w, h = cv2.boundingRect(cnt_global)
#                     cv2.rectangle(output, (x, y), (x + w, y + h), self.draw_colors[color_name], 2)

#                     # 只显示形状名称（按你要求）
#                     cv2.putText(output, shape_name, (x, y - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.6, self.draw_colors[color_name], 2)

#             # 发布结果
#             shape_msg = self.bridge.cv2_to_imgmsg(output, "bgr8")
#             shape_msg.header = msg.header
#             self.shape_pub.publish(shape_msg)

#         except Exception as e:
#             self.get_logger().error(f"处理图像时出错: {e}")

#     def destroy_node(self):
#         cv2.destroyAllWindows()
#         super().destroy_node()

# def main(args=None):
#     rclpy.init(args=args)
#     node = ColorShapeDetector()
#     try:
#         rclpy.spin(node)
#     except KeyboardInterrupt:
#         pass
#     except Exception as e:
#         print(f"节点运行出错: {str(e)}")
#     finally:
#         try:
#             rclpy.shutdown()
#         except RuntimeError as e:
#             error_msg = str(e)
#             if "already called" in error_msg or "already shutdown" in error_msg:
#                 pass
#             else:
#                 print(f"Unexpected error during shutdown: {e}", file=sys.stderr)

# if __name__ == '__main__':
#     main()

操作手順

実験結果

さまざまな形状の物体をカメラの下に置くと、各物体の形状を認識できます。下図のように、Circle（円）と triangle（三角形）を認識しています。

長方形や正方形の物体も置いて、認識結果を確認してみてください。

サポート窓口

資料のダウンロード・技術サポートについては、HumansX株式会社までお問い合わせください。