2025电赛C题分析总结

前言

本文实现了对2025年电赛C题的题目分析与总结。本文中的程序均运行于旭日X3派，若使用树莓派或香橙派等设备，只需稍作修改可以一样运行。同时，本文没有对重叠的正方形与印有数字的正方形进行处理，可以自己调用yolov5等检测器进行检测后处理，测距算法相同。

题目要求分析

在2025年电赛C题中，题目要求的主要目标是测量目标物到摄像头的距离以及目标物图形的尺寸。根据目标物的大小以及复杂程度不同，将题目分为了基础题与提高题，同时提高题还要求在目标物面与轴线呈一定角度的情况下进行测量。

思路分析

要实现目标物尺寸的测量，首先要在画面出识别出目标物的轮廓。因此在总的算法设计上由两部分组成，即目标物识别算法+测距算法。

目标物识别算法

题目中总共有一下几种目标物：边长不等的正方形（基础题大，提高题尺寸更小），边长不等的三角形，半径不等的圆形，分离的正方形组合图形，部分重叠的正方形组合图形，正方形中印有数字的组合图形。对于这些目标物，我们可以分为两类进行处理：正方形、三角形、圆形以及分离的正方形组合图形为一类，这些图形属于基础图形，轮廓明确，可以使用传统视觉算法Opencv的轮廓识别算法进行处理。而对于部分重叠的正方形组合图形以及印有数字的正方形组合图形，传统的视觉算法难以实现，需要使用深度学习算法进行处理。

测距算法

测距算法是这道题的核心算法也是最难的部分。查阅资料可以发现，利用摄像头测距有单目与双目两种方式，一般双目的方式更精确但需要的算力也更大。使用单目摄像头由于丢失了深度信息，需要采用参考物的方式来确定距离以及目标物的尺寸。单目测距算法主要有以下几种实现形式：

• 基于地面假设的单目测距。该方法不需要参考物，但需要假设地面平坦且与摄像头平行，同时要求目标物的下底边必须与地面接触。该方法的原理是利用透视关系，假设相机前方的地面是一个平坦的二维平面，相机安装在已知高度h处，并已知相机光轴与地面之间的夹角θ，利用射线与地面平面的角点计算目标物在三维空间中的位置。详细可以参考github开源工程基于地面假设的单目测距，与csdn上关于原理推导的解读
  单目测距原理推导。
• 基于参考物的单目测距。该方法相关的资料较多，也是较简单常用的方法。该方法简单来说就是已知一个参考物的实际尺寸，比如一个边长为10cm的正方形放置在距离镜头1米的位置时，摄像头拍摄到的图像中该正方形的像素为100，那么当距离未知时，只要根据画面中该参考物的像素就可以推算出目标物的距离。而对于目标物尺寸，也只要根据像素大小
  进行换算即可。
• PnP算法。PnP算法是一个经典的计算机视觉问题，主要用于从2D图像点和3D空间点之间的对应关系中估计相机的姿态和位置。PnP算法可以通过已知的3D空间点和对应的2D图像点来计算相机的外参（旋转和平移矩阵），从而实现目标物的测距和定位。PnP算法通常需要至少4个对应点来进行求解，且需要已知相机的内参（焦距、主点等）。该方法的优点是可以在已知相机内参的情况下，直接通过图像点和空间点的对应关系来计算目标物的距离和位置，适用于多种场景。
  

在本题中，由于不知道目标物是否直接与参考平面接触，且摄像头高度可能需要调节，而不是固定的，因此基于地面假设的单目测距方法不适用。当测量的目标物悬空于参考平面时，测量存在较大误差。而利用相似三角形的测距方法原理较简单，但由于本题要求的精度较高，且由于摄像头可能存在的畸变等原因，精度可能达不到要求，因此也不采用。利用PnP解算的方法通过相机标定测量出相机内参，消除了畸变的影响，同时利用已经较成熟的求解PnP问题的算法，不仅可以求解出距离与目标物尺寸，还可以求解出目标物的姿态信息（如偏转角、翻滚角、偏航角等），因此本题采用PnP算法进行测距。

代码实现

目标物识别算法

在本题中，我将目标物的识别封装成一个类shapeDetection，方便在距离检测算法中直接调用。

    def __init__(self, image):
        self.image = image
        self.gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
        self.gray_blur = cv2.medianBlur(self.gray, 5)
        self.blur = cv2.GaussianBlur(self.gray_blur, (5, 5), 0)
        _, self.thresh = cv2.threshold(self.gray_blur, 100, 255, cv2.THRESH_BINARY_INV)
        self.edges = cv2.Canny(self.blur, 50, 150, apertureSize=3)
        self.contours, self.hierarchy = cv2.findContours(self.thresh, 1, 2)

在进行目标物检测前，首先对图像进行预处理，包括转灰度图，高斯模糊，二值化等操作，然后进行Canny边缘检测，目标是消除噪声，提取出比较清晰的轮廓。

    def detect_rectangles(self):
        rectangles = []
        min_dist_thresh=0
        for cnt in self.contours:
            approx = cv2.approxPolyDP(cnt, 0.015 * cv2.arcLength(cnt, True), True)
            if len(approx) == 4:
                #ordered_pts = self.order_points(pts)
                # 计算每条边的角度
                pts = [tuple(pt[0]) for pt in approx]
                # 计算四点两两之间的距离
                dists = []
                for i in range(4):
                    for j in range(i + 1, 4):
                        d = np.linalg.norm(np.array(pts[i]) - np.array(pts[j]))
                        dists.append(d)
                if min(dists) < min_dist_thresh:
                    continue  # 有两点太近，跳过
                x, y, w, h = cv2.boundingRect(cnt)
                ratio = float(w) / h
                if w > 30 and h > 30 and not (0.9 <= ratio <= 1.1):
                    pts = [tuple(pt[0]) for pt in approx]
                    ordered_pts = self.order_points(pts)
                    rectangles.append(ordered_pts)
        return rectangles

矩形检测算法，若拟合出来的多边形是四边形，且面积大于一定值（避免一些小的噪声矩形的干扰）的同时边长不相等，则认为是矩形

    def detect_squares(self):
        squares = []
        for cnt in self.contours:
            approx = cv2.approxPolyDP(cnt, 0.02 * cv2.arcLength(cnt, True), True)
            if len(approx) == 4:
                x, y, w, h = cv2.boundingRect(cnt)
                ratio = float(w) / h
                if w > 30 and h > 30 and 0.9 <= ratio <= 1.1:
                    # 4条长度均值作为边长
                    pts = [tuple(pt[0]) for pt in approx]
                    side_lengths = [np.linalg.norm(np.array(pts[i]) - np.array(pts[(i+1)%4])) for i in range(4)]
                    avg_side = np.mean(side_lengths)
                    squares.append(avg_side)
        return squares

在矩形检测的基础上，若边长相近，则认为是正方形

    def detect_triangles(self):
        triangles = []
        for cnt in self.contours:
            approx = cv2.approxPolyDP(cnt, 0.01 * cv2.arcLength(cnt, True), True)
            if len(approx) == 3 and cv2.contourArea(cnt) > 200:
                pts = [tuple(pt[0]) for pt in approx]
                a = np.linalg.norm(np.array(pts[0]) - np.array(pts[1]))
                b = np.linalg.norm(np.array(pts[1]) - np.array(pts[2]))
                c = np.linalg.norm(np.array(pts[2]) - np.array(pts[0]))
                triangles.append([a, b, c])
        return triangles

三角形检测算法，返回三角形的三边长度

    def detect_circles(self):
        circles = []
        detected = cv2.HoughCircles(self.edges, cv2.HOUGH_GRADIENT, dp=1, minDist=40, param1=170, param2=50, minRadius=20, maxRadius=600)
        if detected is not None:
            detected = np.uint16(np.around(detected))
            for i in detected[0, :]:
                diameter = i[2] * 2
                circles.append(diameter)
        return circles

圆形检测算法，利用霍夫圆，具体霍夫圆参数需要根据实际情况进行调节

    @staticmethod
    def order_points(pts):
        pts = sorted(pts, key=lambda x: x[1])
        top = sorted(pts[:2], key=lambda x: x[0])
        bottom = sorted(pts[2:], key=lambda x: x[0])
        return [top[0], top[1], bottom[1], bottom[0]]

在实现图形的检测算法后，为方便后续测距算法的利用，需要对检测到的矩形四点坐标进行排序，排序规则为左上，右上，右下，左下。

测距算法

K = np.array(   [[1.6919e+03 ,0.00000000e+00 ,2.519001e+02],
 [0.00000000e+00 ,1.6926e+03 ,3.372762e+02],
 [0.00000000e+00 ,0.00000000e+00 ,1.00000000e+00]] ,dtype=np.float32)
dist = np.array(   [[-0.4375,-3.1847 ,0 , 0,0]],dtype=np.float32) # 如果你有畸变参数，替换这里
#dist=np.zeros(5, dtype=np.float32)  # 假设无畸变
# 黑框A4纸的内边界实际尺寸（单位：cm）
black_width_cm = 17  # A4纸宽度约为17cm
black_height_cm = 25.7

在测距算法中，我选取A4黑框内边缘作为参考物，由于黑框宽度为2cm，结合A4纸实际尺寸，可以得到黑框内轮廓的长度与宽度。同时，我们也需要相机的内参矩阵和畸变参数，这些参数的获得可以通过相机标定获得，具体的标定方法可以用OpenCV或MATLAB的相机标定工具。参考资料如下：

• MATLAB相机参数标定
• OpenCV相机标定

detector = shapeDetection.ShapeDetector(img)
            rects = detector.detect_rectangles()
            if rects and rects[0] is not None:
                areas=[]
                valid_areas=[]
                valid_indices=[]
                for pts in rects:
                    pts_np = np.array(pts, dtype=np.float32)
                    area = cv2.contourArea(pts_np)
                    areas.append(area)
                    #print(areas)
                # 取面积最小的那个
                for i in range(len(areas)):
                    if areas[i] > 25000:
                        #print(areas[i])
                        valid_areas.append(areas[i])
                        valid_indices.append(i)
                if valid_areas:
                    min_idx_in_valid = np.argmin(valid_areas)
                    min_idx=valid_indices[min_idx_in_valid]
                    inner_rect = rects[min_idx]
                    image_points = np.array(detector.detect_rectangles()[min_idx], dtype=np.float32)

所有长度的测量都依赖黑色内边框，因此我们首先要准确地确定黑色内边框的四个顶点坐标。这里，我们调用之前写的目标物检测算法中的矩形检测函数。由于背景的干扰，我们可能会检测到一些干扰矩形，因此可以对面积进行一定的限定（这里可以自己调）。同时，矩形检测函数会把内边框与外边框同时检测出来，因此我们需要对面积进行比较，选择面积较小的那个。

# 对应的真实世界坐标（单位：cm）
                object_points = np.array([
                    [0, 0, 0],
                    [black_width_cm, 0, 0],
                    [black_width_cm, black_height_cm, 0],
                    [0, black_height_cm, 0]
                ], dtype=np.float32)

                # =============================
                # 使用 PnP 求解相机位姿（测距）
                # =============================
                retval, rvec, tvec = cv2.solvePnP(object_points, image_points, K, dist)
                if not retval:
                    print("PnP求解失败")
                    exit()
                R, _ = cv2.Rodrigues(rvec)  # 3x3旋转矩阵

                # 提取欧拉角（以ZYX顺序为例，单位为弧度）
                sy = np.sqrt(R[0, 0] ** 2 + R[1, 0] ** 2)
                singular = sy < 1e-6

                if not singular:
                    x = np.arctan2(R[2, 1], R[2, 2])  # pitch
                    y = np.arctan2(-R[2, 0], sy)  # yaw
                    z = np.arctan2(R[1, 0], R[0, 0])  # roll
                else:
                    x = np.arctan2(-R[1, 2], R[1, 1])
                    y = np.arctan2(-R[2, 0], sy)
                    z = 0

                # 转为角度
                pitch = np.degrees(x)
                yaw = np.degrees(y)
                roll = np.degrees(z)
                print(f"pitch={pitch:.2f}°, yaw={yaw:.2f}°, roll={roll:.2f}°")
                ser.write(("A:%.2f,%.2f,%.2f\n" % (yaw,pitch,roll)).encode('utf-8'))

                # Z轴方向距离 D
                distance_cm = tvec[2][0]
                if 100 < distance_cm < 200:
                    distance_list.append(distance_cm)
                if len(distance_list)>5:
                    sorted_list = sorted(distance_list)
                    trimmed = sorted_list[1:-1]
                    avg_distance = sum(trimmed) / len(trimmed)
                    print(f"去极值平均距离: {avg_distance:.2f} cm")
                    ser.write(("D:%.2f\n" % avg_distance).encode('utf-8'))
                    distance_list = []  # 清空，准备下一轮

由于OpenCV已经内置了PnP解算的算法，我们直接调用即可。通过PnP解算，我们不仅可以获得目标物的距离，还可以获得目标物的三维姿态信息，可作为“其他”部分进行扩充。求距离时，为减小误差，我将所有检测到的距离存到列表中，去掉一个最大最小值后取平均值作为最终的距离。

squares= detector.detect_squares()
                circles= detector.detect_circles()
                triangles= detector.detect_triangles()
                rects = detector.detect_rectangles()
                #print(rects)
                if len(squares)!=0:
                   # w_px= squares[0]  # 假设检测到的正方形边长
                   w_px=min(squares)   #最小正方形的边长
                elif len(circles)!=0:
                    w_px = circles[0]
                elif len(triangles)!=0:
                    for sides in triangles:
                        a,b,c=sides
                        w_px = (a+b+c)/3

检测正方形、三角形、圆形都是一样的原理，直接调用我们之前写的目标检测算法，得出像素点的边长。

                if len(rects)!=0:
                    """
                    for pts in rects:
                        pts_np = np.array(pts, dtype=np.float32)
                        area = cv2.contourArea(pts_np)
                        print(area)
                        if area<10000:
                            left_height = np.linalg.norm(pts_np[0] - pts_np[3])
                            right_height = np.linalg.norm(pts_np[1] - pts_np[2])
                            # 取平均值作为竖直方向长度
                            w_px = (left_height + right_height) / 2
                            print(w_px)
                            break
                    """
                    for pts in rects:
                        pts_np = np.array(pts, dtype=np.float32)
                        area = cv2.contourArea(pts_np)
                        #print(area)
                        if area < 10000:  # 确认是小正方形
                            # 确保点顺序正确
                            ordered_pts = order_points(pts_np)

                            # 已经通过PnP求得A4纸的位姿(rvec和tvec)，现在将小正方形的四个角点反投影到3D空间
                            # 首先将像素坐标归一化
                            image_points_normalized = []
                            for pt in ordered_pts:
                                # 像素坐标转归一化坐标
                                x = (pt[0] - K[0, 2]) / K[0, 0]
                                y = (pt[1] - K[1, 2]) / K[1, 1]
                                image_points_normalized.append([x, y])

                            # 构建投影矩阵
                            R, _ = cv2.Rodrigues(rvec)
                            RT = np.column_stack((R, tvec))
                            P = np.dot(K, RT)

                            # 求出四个角点在3D空间中的坐标
                            world_points = []
                            for pt in image_points_normalized:
                                # 假设所有点都在z=0平面上(与A4纸相同平面)
                                x, y = pt
                                # 使用RT矩阵从归一化坐标反投影到世界坐标
                                # 构建方程: λ*[x,y,1] = R*[X,Y,0] + t
                                A = np.array([
                                    [R[0, 0] - x * R[2, 0], R[0, 1] - x * R[2, 1]],
                                    [R[1, 0] - y * R[2, 0], R[1, 1] - y * R[2, 1]]
                                ])
                                b = np.array([
                                    [tvec[0, 0] - x * tvec[2, 0]],
                                    [tvec[1, 0] - y * tvec[2, 0]]
                                ])
                                XY = np.linalg.solve(A, -b)
                                world_points.append([XY[0, 0], XY[1, 0], 0])

                            # 计算世界坐标系中正方形的边长
                            world_points = np.array(world_points, dtype=np.float32)
                            side_lengths = [
                                np.linalg.norm(world_points[1] - world_points[0]),  # 上边
                                np.linalg.norm(world_points[2] - world_points[1]),  # 右边
                                np.linalg.norm(world_points[3] - world_points[2]),  # 下边
                                np.linalg.norm(world_points[0] - world_points[3])  # 左边
                            ]

                            # 计算真实边长(取平均值)
                            real_size_cm = np.mean(side_lengths)
                            print(f"正方形实际边长: {real_size_cm:.2f} cm")

                            # 显示在图像上
                            cv2.putText(img_show, f"Size: {real_size_cm:.2f} cm",
                                        (30, 120), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 2)

                            # 添加到列表中进行平滑处理
                            if real_size_cm < 17:  # 确保边长小于17cm
                                x_list.append(real_size_cm)
                            if len(x_list) > 5:
                                sorted_list = sorted(x_list)
                                trimmed = sorted_list[1:-1]
                                avg_x = sum(trimmed) / len(trimmed)+0.5
                                print(f"去极值平均边长: {avg_x:.2f} cm")
                                ser.write(("X:%.2f\n" % avg_x).encode('utf-8'))
                                x_list = []
                            break

在提高题中，有一题目标物绕轴线旋转了一定角度，这里我们需要进行特殊处理。由于目标物绕轴线旋转了一定角度，画面中看到的正方形不再是正方形，而是一个矩形。在这里，我们需要逆向调用PnP解算算法。由于在前面我们已经得出了目标物的距离，这里我们可以利用目标物的距离反算出旋转后的小正方形的尺寸。

fx = K[0][0]
                if w_px!=0:
                    x_cm = (w_px * distance_cm) / fx
                    if x_cm<17:
                        x_list.append(x_cm)
                    if len(x_list)>5:
                        sorted_list = sorted(x_list)
                        trimmed = sorted_list[1:-1]
                        avg_x = sum(trimmed) / len(trimmed)+0.5
                        print(f"去极值平均边长: {avg_x:.2f} cm")
                        ser.write(("X:%.2f\n" % avg_x).encode('utf-8'))
                        cv2.putText(img_show, f"x = {avg_x:.2f}m", (30, 80), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0),
                                    2)
                        x_list = []

目标物未旋转的情况下，直接根据像素点的边长与相机内参计算出目标物的实际边长。

参考资料

• 结合yolov4检测的测距算法
• 树莓派上运行的单目视觉测距算法
• 实时距离检测
• 单目摄像头测距测速算法

完整代码

完整代码可参考2025电赛C题完整代码