OPENMV 返回信息中有Homography矩阵相关信息？若没有该怎么构建Homography矩阵？

an2l

uzbd

详细描述问题：
1、运行的是哪一个程序？
2、运行程序时出现了什么问题？
3、想解决的问题

an2l

@uzbd

-- coding: utf-8 --

import sensor, image, time, math

初始化摄像头

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.VGA)
sensor.skip_frames(time=1000)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)

相机内参设置

f_x = (2.8 / 3.6736) * 640
f_y = (2.8 / 2.7384) * 480
c_x = 640 * 0.5
c_y = 480 * 0.5

系统状态变量

SETTING_BASE = 0
MONITORING = 1
MEASURING = 2
system_state = SETTING_BASE

位置信息

base_position = None # 存储基准位置(中心点)
filtered_position = None
stroke_mm = 0
base_samples = []
current_samples = []
INITIAL_SAMPLES = 20
CURRENT_SAMPLES = 20

运动检测参数

MOTION_THRESHOLD = 20
STABLE_FRAMES = 50
stable_count = 0
last_img = None
roi = (0, 0, 0, 0)
roi1 = (0, 0, 250, 480) # 运动检测的ROI区域

AprilTag参数

K1=14.98
TAG_SIZE_MM = 26
TAG_AREA_MM2 = TAG_SIZE_MM * TAG_SIZE_MM # 标签实际面积
x0, y0, x1, y1 = None, None, None, None

计算Open MV到TAG的距离

def Tag_OpenMV_distance(tx,ty,tz):
Distance1 = K1 * math.sqrt(txtx+tyty+tz*tz)
return Distance1

计算实际距离的函数

def calculate_real_distance(pixel_distance, k_value):
return pixel_distance * k_value

计算二维空间欧氏距离

def euclidean_distance(pos1, pos2):
if pos1 is None or pos2 is None:
return 0.0
dx = pos1[0] - pos2[0]
dy = pos1[1] - pos2[1]
return math.sqrt(dxdx + dydy)

卡尔曼滤波器类

class KalmanFilter:
def init(self, initial_value, process_variance=1, measurement_variance=0.5):
"""
一维卡尔曼滤波器
:param initial_value: 初始估计值
:param process_variance: 过程噪声方差 (Q)
小值 (1e-6 到 1e-4): 状态变化缓慢，滤波器更"保守"
大值 (0.1 到 1.0): 状态变化快速，滤波器响应更快
:param measurement_variance: 测量噪声方差 (R)
小值 (0.01 到 0.1): 测量值可靠，滤波器更信任测量
大值 (1.0 到 10.0): 测量值噪声大，滤波器更信任预测
:estimate_variance: 初始估计方差 (P)
表示初始状态的不确定性
中等不确定性设为1
高不确定性可设10-100
"""
self.state_estimate = initial_value
self.estimate_variance = 20 # 初始估计方差 (P)
self.process_variance = process_variance
self.measurement_variance = measurement_variance

def update(self, measurement):
    prior_estimate = self.state_estimate
    prior_variance = self.estimate_variance + self.process_variance
    kalman_gain = prior_variance / (prior_variance + self.measurement_variance)
    self.state_estimate = prior_estimate + kalman_gain * (measurement - prior_estimate)
    self.estimate_variance = (1 - kalman_gain) * prior_variance
    return self.state_estimate

创建卡尔曼滤波器实例

为四个角点创建滤波器 (每个角点有x,y坐标)

corner_kalman = []
for i in range(4): # 四个角点
corner_kalman.append([
KalmanFilter(initial_value=320.0), # x坐标滤波器
KalmanFilter(initial_value=240.0)]) # y坐标滤波器

中心点卡尔曼滤波器

center_x_kalman = KalmanFilter(initial_value=320.0)
center_y_kalman = KalmanFilter(initial_value=240.0)

边平均长度卡尔曼滤波器

edge_kalman = KalmanFilter(initial_value=50.0)

面积卡尔曼滤波器

area_kalman = KalmanFilter(initial_value=1000.0)

加权平均位置计算

def weighted_average_position(positions):
if not positions:
return None
#n = len(positions)
total_weight = 0
weighted_x = 0
weighted_y = 0
for i, pos in enumerate(positions):
weight = i + 1
total_weight += weight
weighted_x += pos[0] * weight
weighted_y += pos[1] * weight
return (weighted_x / total_weight, weighted_y / total_weight)

运动检测函数

def detect_motion(current_img):
global last_img, stable_count
if last_img is None:
last_img = current_img.copy()
return True
diff = current_img.copy(roi=roi).difference(last_img.copy(roi=roi))
stats = diff.get_statistics()
last_img = current_img.copy()
avg_change = (stats.l_max() - stats.l_min()) / 2
if avg_change > MOTION_THRESHOLD:
stable_count = 0
return True
else:
stable_count += 1
return False

================ 三重尺寸计算方法 ================

def calculate_edge_lengths(corners):
"""
计算四条边的长度
"""
edges = []
n = len(corners)
for i in range(n):
j = (i + 1) % n
dx = corners[j][0] - corners[i][0]
dy = corners[j][1] - corners[i][1]
edges.append(math.sqrt(dxdx + dydy))
return edges

def calculate_area(corners):
"""
使用鞋带公式计算多边形面积
"""
n = len(corners)
area = 0.0
for i in range(n):
j = (i + 1) % n
area += corners[i][0] * corners[j][1]
area -= corners[j][0] * corners[i][1]
return abs(area) / 2.0

def filter_corners(corners):
"""
对四个角点应用卡尔曼滤波
"""
filtered_corners = []
for i in range(4):
x = corner_kalman[i][0].update(corners[i][0])
y = corner_kalman[i][1].update(corners[i][1])
filtered_corners.append((x, y))
return filtered_corners

def calculate_center(corners):
"""
计算四个角点的中心
"""
x_sum = sum(corner[0] for corner in corners)
y_sum = sum(corner[1] for corner in corners)
return (x_sum / 4, y_sum / 4)

def calculate_combined_size(filtered_corners):
"""
三重尺寸计算方法：
1. 计算四条边的平均长度
2. 使用鞋带公式计算面积
3. 从面积推导等效边长
4. 加权组合 (30% 边平均 + 20% 面积等效 + 50%TAG返回的像素宽度与高度的平均值)
"""
# 计算四条边长度
edges = calculate_edge_lengths(filtered_corners)
avg_edge = sum(edges) / len(edges)

# 计算面积
area = calculate_area(filtered_corners)

# 计算等效边长 (假设为正方形)
equiv_edge = math.sqrt(area)

# 加权组合
combined_size = 0.3 * avg_edge + 0.2 * equiv_edge + 0.5 * tag.h-1#((tag.h+tag.w)/2)

return combined_size, avg_edge, equiv_edge, area

================ 三重尺寸计算方法结束 ================

clock = time.clock()

主循环

while(True):
clock.tick()
img = sensor.snapshot().lens_corr(1.6)

if system_state == SETTING_BASE:
    tags = img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y,
            families=image.TAG36H11, tag_size=TAG_SIZE_MM)
            
    img.draw_rectangle(roi1, color=(255, 0, 0))
    
    if tags:
        tag = tags[0]
        
        # 检查标签质心是否在ROI1区域内
        tag_in_roi = (roi1[0] <= tag.cxf <= roi1[0] + roi1[2] and 
                        roi1[1] <= tag.cyf <= roi1[1] + roi1[3])
        
        if not tag_in_roi:
        # 标签不在ROI1区域内，显示提示信息
            img.draw_string(0, 10, "Move tag to ROI1 area !!!", color=(255, 255, 0))
            continue  # 跳过后续处理
        
        tx,ty,tz=tag.x_translation,tag.y_translation,tag.z_translation
        x0 , y0 = tag.cxf , tag.cyf
        
        # 获取原始角点
        raw_corners = tag.corners
        
        # 对角点进行卡尔曼滤波
        filtered_corners = filter_corners(raw_corners)
        
        # 计算滤波后的中心点
        center_x, center_y = calculate_center(filtered_corners)
        
        # 对TAG质心进行卡尔曼滤波
        #filtered_center_x = center_x_kalman.update(tag.cxf)
        #filtered_center_y = center_y_kalman.update(tag.cyf)
        
        # 对中心点进行卡尔曼滤波
        filtered_center_x = center_x_kalman.update(center_x)
        filtered_center_y = center_y_kalman.update(center_y)
        
        # 使用双重尺寸计算方法
        #combined_size, avg_edge, equiv_edge, area = calculate_combined_size(filtered_corners)
        
        # 对平均边和面积进行卡尔曼滤波
        #filtered_avg_edge = edge_kalman.update(avg_edge)
        #filtered_area = area_kalman.update(area)
        
        # 重新计算组合尺寸（使用滤波后的值）
        #combined_size = 0.7 * filtered_avg_edge + 0.3 * math.sqrt(filtered_area)#+0.4 *((tag.h+tag.w)/2)
        
        # 绘制标签轮廓和角点
        for i in range(4):
            # 绘制滤波后的角点
            img.draw_circle(int(filtered_corners[i][0]), int(filtered_corners[i][1]), 5, color=(0, 255, 255))
            # 绘制原始角点
            img.draw_circle(int(raw_corners[i][0]), int(raw_corners[i][1]), 3, color=(255, 0, 255))
            # 绘制边
            next_i = (i + 1) % 4
            img.draw_line(
                int(filtered_corners[i][0]), int(filtered_corners[i][1]),
                int(filtered_corners[next_i][0]), int(filtered_corners[next_i][1]),
                color=(0, 255, 0)
            )
        
        # 绘制滤波后的中心点
        img.draw_cross(int(filtered_center_x), int(filtered_center_y), color=(0, 255, 0), size=10)
        
        # 显示尺寸信息
        img.draw_string(0, 400, "ID:%d" % tag.id, color=(255, 0, 0))
        #img.draw_string(0, 370, "AvgEdge:%.1f" % filtered_avg_edge, color=(255, 0, 0))
        #img.draw_string(0, 390, "EquivEdge:%.1f" % math.sqrt(filtered_area), color=(0, 255, 0))
        #img.draw_string(0, 380, "Combined:%.1f" % combined_size, color=(0, 255, 255))
        #img.draw_string(0, 360, "Area:%.1f" % filtered_area, color=(255, 255, 0))
        img.draw_string(0, 350, "H:%d" % tag.rect[3], color=(255, 255, 0))
        img.draw_string(0, 340, "W:%d" % tag.rect[2], color=(255, 255, 0))
        # 记录滤波后的中心位置
        current_position = (filtered_center_x, filtered_center_y)
        base_samples.append(current_position)
        
        progress = len(base_samples) / INITIAL_SAMPLES * 100
        img.draw_string(0, 10, "Setting base:%d%%" % progress, color=(255, 255, 0))

        if len(base_samples) >= INITIAL_SAMPLES:
            # 计算加权平均位置
            base_position = weighted_average_position(base_samples)
            print("Base position set: (%.1f, %.1f)" % base_position)
            img.draw_string(0, 420, "The base position is set!", color=(255, 0, 0))
            system_state = MONITORING
            roi = (0, tag.rect[1]-20, 640, tag.rect[2]+40)  # 运动检测的ROI区域
            base_samples = []
            current_samples = []
    else:
        img.draw_string(0, 10, "Place piston at base position", color=(255, 0, 0))
        
elif system_state == MONITORING:
    motion_detected = detect_motion(img)
    status = "In motion" if motion_detected else "In stability (%d/%d)" % (stable_count, STABLE_FRAMES)
    img.draw_rectangle(roi, color=(0, 255, 0))
    img.draw_string(0, 10, "State: %s" % status, color=(255, 255, 0))
        
    if stable_count >= STABLE_FRAMES:
        print("活塞已停止，开始最终定位")
        img.draw_string(0, 20, "The piston has stopped and begins final positioning!", color=(255, 0, 0))
        system_state = MEASURING

elif system_state == MEASURING:
    tags = img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y,
                            families=image.TAG36H11, tag_size=TAG_SIZE_MM)
        
    if tags:
        tag = tags[0]
        
        x1 , y1 = tag.cxf , tag.cyf
        
        # 获取原始角点
        raw_corners = tag.corners
        
        # 对角点进行卡尔曼滤波
        filtered_corners = filter_corners(raw_corners)
        
        # 计算滤波后的中心点
        center_x, center_y = calculate_center(filtered_corners)
        
        # 对中心点进行卡尔曼滤波
        #filtered_center_x = center_x_kalman.update(tag.cxf)
        #filtered_center_y = center_y_kalman.update(tag.cyf)
        
        filtered_center_x = center_x_kalman.update(center_x)
        filtered_center_y = center_y_kalman.update(center_y)
        
        # 使用双重尺寸计算方法
        combined_size, avg_edge, equiv_edge, area = calculate_combined_size(filtered_corners)
        
        # 对平均边和面积进行卡尔曼滤波
        filtered_avg_edge = edge_kalman.update(avg_edge)
        filtered_area = area_kalman.update(area)
        
        # 重新计算组合尺寸（使用滤波后的值）
        combined_size = 0.3 * filtered_avg_edge + 0.2 * math.sqrt(filtered_area)+0.5 *  tag.h-1#((tag.h+tag.w)/2)
        
        # 计算比例系数
        k_value = TAG_SIZE_MM / combined_size
        
        # 绘制标签轮廓和角点
        for i in range(4):
            # 绘制滤波后的角点
            img.draw_circle(int(filtered_corners[i][0]), int(filtered_corners[i][1]), 5, color=(0, 255, 255))
            # 绘制原始角点
            img.draw_circle(int(raw_corners[i][0]), int(raw_corners[i][1]), 3, color=(255, 0, 255))
            # 绘制边
            next_i = (i + 1) % 4
            img.draw_line(
                int(filtered_corners[i][0]), int(filtered_corners[i][1]),
                int(filtered_corners[next_i][0]), int(filtered_corners[next_i][1]),
                color=(0, 255, 0)
            )
        
        # 绘制滤波后的中心点
        img.draw_cross(int(filtered_center_x), int(filtered_center_y), color=(0, 255, 0), size=10)
                  
        # 显示尺寸信息
        img.draw_string(0, 400, "ID:%d" % tag.id, color=(255, 0, 0))
        img.draw_string(0, 370, "AvgEdge:%.1f" % filtered_avg_edge, color=(255, 0, 0))
        img.draw_string(0, 390, "EquivEdge:%.1f" % math.sqrt(filtered_area), color=(0, 255, 0))
        img.draw_string(0, 380, "Combined:%.1f" % combined_size, color=(0, 255, 255))
        img.draw_string(0, 360, "Area:%.1f" % filtered_area, color=(255, 255, 0))
        img.draw_string(0, 350, "H:%d" % tag.rect[3], color=(255, 255, 0))
        img.draw_string(0, 340, "W:%d" % tag.rect[2], color=(255, 255, 0))
        # 记录滤波后的中心位置
        current_position = (filtered_center_x, filtered_center_y)
        current_samples.append(current_position)
        
        # 保持采样列表长度
        if len(current_samples) > CURRENT_SAMPLES:
             current_samples.pop(0)

        # 计算加权平均位置
        if current_samples:
            avg_x, avg_y = weighted_average_position(current_samples)
            
            # 计算像素距离
            pixel_distance = euclidean_distance((avg_x, avg_y), base_position)
            
            # 计算实际距离
            stroke_mm = pixel_distance * k_value

            # 显示行程信息
            text = "Distance:%.1fmm" % stroke_mm
            img.draw_string(0, 440, text, color=(0, 255, 0))
            pos_text = "Pos:(%.1f,%.1f)" % (avg_x, avg_y)
            img.draw_string(0, 450, pos_text, color=(255, 255, 0))
            k_text = "K:%.4f Size:%.1f" % (k_value, combined_size)
            img.draw_string(0, 470, k_text, color=(255, 255, 0))
            
            # 显示旋转角度信息
            Xrot_text = "XRot:%.1f°" % (math.degrees(tag.x_rotation)-180)
            img.draw_string(0, 230, Xrot_text, color=(150, 150, 255))
            Yrot_text = "YRot:%.1f°" % math.degrees(tag.y_rotation)
            img.draw_string(0, 240, Yrot_text, color=(150, 150, 255))
            Zrot_text = "ZRot:%.1f°" % math.degrees(tag.z_rotation)
            img.draw_string(0, 250, Zrot_text, color=(150, 150, 255))
            
            print("%s | Combined:%.1fpx | K:%.4f" % (text, combined_size, k_value))
    else:
        img.draw_string(0, 420, "Tag lost!", color=(255, 0, 0))
        text = "Distance:%.1fmm" % stroke_mm
        img.draw_string(0, 440, text, color=(0, 255, 0))
        
if x0 is not None and y0 is not None:
    # 在基准点上方显示行程文本
    text = "Distance:%.1fmm" % stroke_mm
    img.draw_string(int(x0)+3, int(y0)-10, text, color=(255, 255, 0))
       
    # 在基准点上方画一条短竖线
    img.draw_line(int(x0), int(y0+20), int(x0), int(y0-20), color=(255, 0, 0), thickness=1)
       
    # 如果当前测量位置存在，绘制线段和标记
    if x1 is not None and y1 is not None:
        # 绘制从基准点到当前点的线段
        img.draw_line(int(x0), int(y0), int(x1), int(y1), color=(255, 0, 0), thickness=1)
        # 在当前点上方画一条短竖线
        img.draw_line(int(x1), int(y1+20), int(x1), int(y1-20), color=(255, 0, 0), thickness=1)
        img.draw_line(int(x1-20), int(y1-20), int(x1), int(y1), color=(255, 0, 0), thickness=1)
        img.draw_line(int(x1-20), int(y1+20), int(x1), int(y1), color=(255, 0, 0), thickness=1)

# 显示系统状态
states = ["SETTING_BASE","MONITORING", "MEASURING"]
img.draw_string(0, 430, "State:%s" % states[system_state], color=(255, 255, 255))
img.draw_string(0, 410, "FPS:%.1f" % clock.fps(), color=(255, 255, 255))


# 显示基准位置
if system_state == MEASURING and base_position is not None:
    base_text = "Base:(%.1f,%.1f)" % base_position
    img.draw_string(0, 460, base_text, color=(255, 255, 0))

利用此段程序计算标签移动距离，标签运动时会产生俯仰角和偏航角导致像素宽度或高度出现偏移，利用矩阵进行校正透视投影方法减小误差

OPENMV 返回信息中有​​Homography矩阵​​相关信息？若没有该怎么构建​​Homography矩阵？

-- coding: utf-8 --

初始化摄像头

相机内参设置

系统状态变量

位置信息

运动检测参数

AprilTag参数

计算Open MV到TAG的距离

计算实际距离的函数

计算二维空间欧氏距离

卡尔曼滤波器类

创建卡尔曼滤波器实例

为四个角点创建滤波器 (每个角点有x,y坐标)

中心点卡尔曼滤波器

边平均长度卡尔曼滤波器

面积卡尔曼滤波器

加权平均位置计算

运动检测函数

================ 三重尺寸计算方法 ================

================ 三重尺寸计算方法结束 ================

主循环

OPENMV 返回信息中有Homography矩阵相关信息？若没有该怎么构建Homography矩阵？