modules/visual_measure.py

import cv2
import cv2.aruco as aruco
import numpy as np
import config
import math

class VisualMeasurer:
def init(self):
# 定义 ArUco 字典
self.aruco_dict = aruco.getPredefinedDictionary(getattr(aruco, config.ARUCO_DICT_TYPE))
self.parameters = aruco.DetectorParameters()

    # 简单的相机内参 (如果没有标定，使用估算值)

    # fx, fy 约为 frame_width, cx, cy 为中心点

    w, h = config.FRAME_WIDTH, config.FRAME_HEIGHT

    self.camera_matrix = np.array([[w, 0, w/2], [0, w, h/2], [0, 0, 1]], dtype=float)

    self.dist_coeffs = np.zeros((5, 1)) # 假设无畸变



    # 用于计算速度的缓存

    self.last_yaw = 0

    self.last_time = 0



def process(self, frame, current_time):

    """

    处理图像，返回姿态数据

    """

    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

    corners, ids, rejected = aruco.detectMarkers(gray, self.aruco_dict, parameters=self.parameters)



    data = {

        "pitch": 0.0,

        "roll": 0.0,

        "yaw": 0.0,

        "speed": 0.0,

        "detected": False

    }



    if ids is not None:

        # 姿态估计

        rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(corners, config.MARKER_SIZE, self.camera_matrix, self.dist_coeffs)



        # 绘制坐标轴 (取第一个检测到的码)

        cv2.drawFrameAxes(frame, self.camera_matrix, self.dist_coeffs, rvecs[0], tvecs[0], 0.03)



        # 旋转向量转旋转矩阵

        rmat, _ = cv2.Rodrigues(rvecs[0])



        # 计算欧拉角 (简单转换，根据实际坐标系可能需要调整)

        # 这里使用简单的转换公式

        sy = math.sqrt(rmat[0,0] * rmat[0,0] +  rmat[1,0] * rmat[1,0])

        singular = sy < 1e-6



        if not singular:

            x = math.atan2(rmat[2,1] , rmat[2,2])

            y = math.atan2(-rmat[2,0], sy)

            z = math.atan2(rmat[1,0], rmat[0,0])

        else:

            x = math.atan2(-rmat[1,2], rmat[1,1])

            y = math.atan2(-rmat[2,0], sy)

            z = 0



        # 弧度转角度

        pitch = math.degrees(x)

        roll = math.degrees(y)

        yaw = math.degrees(z) # Z轴旋转通常对应水平转动



        # 计算速度 (rad/s)

        speed = 0.0

        if self.last_time != 0:

            dt = current_time - self.last_time

            if dt > 0:

                delta_yaw = abs(math.radians(yaw) - math.radians(self.last_yaw))

                # 处理 180度/-180度 跳变

                if delta_yaw > math.pi:

                    delta_yaw = 2*math.pi - delta_yaw

                speed = delta_yaw / dt



        # 更新缓存

        self.last_yaw = yaw

        self.last_time = current_time



        data = {

            "pitch": round(pitch, 1),

            "roll": round(roll, 1),

            "yaw": round(yaw, 1),

            "speed": round(speed, 2),

            "detected": True

        }



    return frame, data

这部分代码是整个项目的数学核心，也是最难写的部分。如果你要自己写出来，不能上来就写代码，而是要先在脑子里建立一个“从图像到数据”的流水线。

要把这段代码写出来，你的思考过程应该是这样的：四个步骤。

🧠 思考步骤一：准备工具（初始化

__init__

）

你的想法：
“我要去测量一个二维码（ArUco码）。首先，我得告诉电脑我要找哪种二维码？还有，我的摄像头‘视力’怎么样（焦距、中心点），这样才能算出距离。”

代码实现：

1. 选字典：ArUco 有很多种规格（4×4, 5×5, 6×6）。
   • self.aruco_dict = ...
     : 告诉程序，我们只找
     DICT_4X4_50
     这一种，别的不管。
2. 设参数：
   • self.camera_matrix
     : 这是重点。在计算机视觉里，要从 2D 图片算出 3D 角度，必须知道摄像头的内参矩阵。
   • 小白怎么写？ 真正的做法是先做“相机标定”（拍棋盘格）。但在比赛或毕设里，为了省事，我们可以估算：
     • 焦距 (fx, fy) ≈ 图片宽度 (比如 640)。
     • 光心 (cx, cy) ≈ 图片中心 (320, 240)。
     • 这就有了代码里那个
       np.array([[w, 0, w/2]...])
       。这是一个通用的“万金油”矩阵。

🔍 思考步骤二：寻找目标（检测

detectMarkers

）

你的想法：
“每一帧图片传进来，我第一件事是把彩照变黑白（因为黑白对比度高，好找）。然后扫描全图，看看有没有那个二维码的黑框框。”

代码实现：

1. 变黑白：
   gray = cv2.cvtColor(...)
   。
2. 找码：
   aruco.detectMarkers(...)
   。
   • 这个函数是 OpenCV 帮我们写好的。
   • 它返回三个东西：
     corners
     （四个角的坐标），
     ids
     （二维码的编号），
     rejected
     （看着像但不是的垃圾）。

📐 思考步骤三：计算姿态（核心难点）

你的想法：
“好，现在找到二维码了。但我怎么知道它是平放的，还是斜着的？我需要把它的 2D 像素坐标，转换成 3D 空间的旋转角度。”

代码实现：

1. 姿态估计：
   aruco.estimatePoseSingleMarkers(...)
   。
   • 你需要告诉它：二维码在真实世界里有多大（比如
     0.05
     米）。
   • 它会返回两个关键向量：
     • rvecs
       (Rotation Vector)：旋转向量（它怎么转的）。
     • tvecs
       (Translation Vector)：平移向量（它离我多远，在哪个位置）。
2. 画坐标轴：
   cv2.drawFrameAxes(...)
   。
   • 就是你在屏幕上看到的那个红绿蓝三根线（X, Y, Z轴），让你直观看到算得对不对。

🧮 思考步骤四：数学翻译（从向量到欧拉角）

你的想法（最痛苦的一步）：
“电脑给我的是

代码实现（这段通常是抄公式，不用死记）：

1. 罗德里格斯变换：
   cv2.Rodrigues(rvecs[0])
   。
   • 把“旋转向量”变成“3×3 旋转矩阵”。
2. 矩阵转欧拉角：
   • 代码里那一大段
     math.atan2(...)
     就是在做三角函数运算。
   • 逻辑：通过矩阵里不同位置的数值比值，反推出 X、Y、Z 三个轴分别转了多少度。
   • 小白提示：这段代码是通用的数学公式。你在网上搜“Rotation Matrix to Euler Angles python”，出来的就是这段。你不需要自己推导，只需要知道
     pitch
     ,
     roll
     ,
     yaw
     分别代表什么。
     • Pitch (俯仰)：点头。
     • Roll (翻滚)：歪头。
     • Yaw (偏航)：摇头（转头）。对于桥梁旋转，我们通常看 Yaw。

⏱️ 思考步骤五：计算速度（物理题）

你的想法：
“我现在知道桥转到 30 度了。但我还要知道它转得有多快。初中物理公式：速度 = 路程 / 时间。”

代码实现：

1. 获取时间差：
   dt = current_time - self.last_time
   。
2. 获取角度差：
   delta_yaw = abs(yaw - self.last_yaw)
   。
3. 处理“跳变”（这是个坑）：
   • 如果桥从 179 度转到了 -179 度（转了一圈），数学上差值是 358 度，但实际只转了 2 度。
   • 所以代码里有一句
     if delta_yaw > math.pi:
     来处理这种过界的情况。
4. 算速度：
   speed = delta_yaw / dt
   。
5. 更新缓存：把现在的角度和时间存起来，留给下一帧用。

📝 总结：如果让你自己写，怎么写？

不要试图背代码，按这个填空题的思路来写：

class MyMeasurer:

    def __init__(self):

        # 1. 准备字典 (aruco.getPredefinedDictionary)

        # 2. 假装知道相机参数 (np.array 估算)

        pass



    def process(self, img, time):

        # 1. 找码 (detectMarkers)

        if 找到了:

            # 2. 算 3D 姿态 (estimatePoseSingleMarkers) -> 得到 rvec

            # 3. 把 rvec 变成 矩阵 (Rodrigues)

            # 4. 把 矩阵 变成 角度 (atan2 公式，直接复制粘贴)

            # 5. 算速度 ( (新角度-旧角度) / 时间 )

            return 数据

        return 空

最核心的建议：
除了第四步（矩阵转欧拉角）那个复杂的数学公式建议直接复制我的代码外，其他的步骤（找码、算姿态、算速度）逻辑都很直白，你可以尝试自己理解并写出来。