7.9 plus Calibration

Visual measurement-straight/.idea/.gitignore

@@ -0,0 +1,8 @@
+# 默认忽略的文件
+/shelf/
+/workspace.xml
+# 基于编辑器的 HTTP 客户端请求
+/httpRequests/
+# Datasource local storage ignored files
+/dataSources/
+/dataSources.local.xml

Visual measurement-straight/.idea/Visual measurement-straight.iml

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<module type="PYTHON_MODULE" version="4">
+  <component name="NewModuleRootManager">
+    <content url="file://$MODULE_DIR$" />
+    <orderEntry type="inheritedJdk" />
+    <orderEntry type="sourceFolder" forTests="false" />
+  </component>
+</module>

Visual measurement-straight/.idea/inspectionProfiles/Project_Default.xml

@@ -0,0 +1,10 @@
+<component name="InspectionProjectProfileManager">
+  <profile version="1.0">
+    <option name="myName" value="Project Default" />
+    <inspection_tool class="DuplicatedCode" enabled="true" level="WEAK WARNING" enabled_by_default="true">
+      <Languages>
+        <language minSize="95" name="Python" />
+      </Languages>
+    </inspection_tool>
+  </profile>
+</component>

Visual measurement-straight/.idea/inspectionProfiles/profiles_settings.xml

@@ -0,0 +1,6 @@
+<component name="InspectionProjectProfileManager">
+  <settings>
+    <option name="USE_PROJECT_PROFILE" value="false" />
+    <version value="1.0" />
+  </settings>
+</component>

Visual measurement-straight/.idea/misc.xml

@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="Black">
+    <option name="sdkName" value="C:\ProgramData\anaconda3" />
+  </component>
+  <component name="ProjectRootManager" version="2" project-jdk-name="C:\ProgramData\anaconda3" project-jdk-type="Python SDK" />
+</project>

Visual measurement-straight/.idea/modules.xml

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectModuleManager">
+    <modules>
+      <module fileurl="file://$PROJECT_DIR$/.idea/Visual measurement-straight.iml" filepath="$PROJECT_DIR$/.idea/Visual measurement-straight.iml" />
+    </modules>
+  </component>
+</project>

Visual measurement-straight/py/Calibration.py

@@ -0,0 +1,143 @@
+import cv2
+import numpy as np
+import glob
+from scipy.optimize import minimize
+import model
+import math
+import calc_way
+
+def needs_correction(dist_coeffs, error_threshold=0.5):
+    k1, k2, p1, p2, k3 = dist_coeffs.ravel()
+    if (abs(k1) > 0.1 or abs(k2) > 0.01 or abs(p1) > 0.005 or
+        abs(p2) > 0.005 or abs(k3) > 0.01):
+        return True
+    return False
+
+def calibrate(image_fold, columns, rows, size):
+    # 设置棋盘格参数
+    chessboard_size = (columns, rows)  # 内部角点数量 (columns, rows)
+    square_size = size  # 棋盘格方块实际大小（单位：毫米/厘米/英寸等）
+
+    # 准备对象点 (0,0,0), (1,0,0), (2,0,0) ..., (8,5,0)
+    objp = np.zeros((chessboard_size[0] * chessboard_size[1], 3), np.float32)
+    objp[:, :2] = np.mgrid[0:chessboard_size[0], 0:chessboard_size[1]].T.reshape(-1, 2) * square_size
+
+    # 存储对象点和图像点的数组
+    objpoints = []  # 3D点（真实世界坐标）
+    imgpoints = []  # 2D点（图像坐标）
+
+    # 获取标定图像
+    images = glob.glob(image_fold)
+    # print("找到的图像文件:", images)
+
+    for fname in images:
+        img = cv2.imread(fname)
+        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+
+        # 查找棋盘格角点
+        ret, corners = cv2.findChessboardCorners(gray, chessboard_size, None)
+
+        # 如果找到，添加对象点和图像点
+        if ret:
+            objpoints.append(objp)
+
+            # 亚像素级精确化
+            criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
+            corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
+            imgpoints.append(corners2)
+
+            # 绘制并显示角点
+            cv2.drawChessboardCorners(img, chessboard_size, corners2, ret)
+            cv2.imshow('Corners', img)
+            cv2.waitKey(500)
+
+    cv2.destroyAllWindows()
+
+    # 相机标定
+    ret, camera_matrix, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera(
+            objpoints, imgpoints, gray.shape[::-1], None, None)
+
+    # 输出标定结果
+    print("相机内参矩阵（K矩阵）:\n", camera_matrix)
+    print("\n畸变系数（k1, k2, p1, p2, k3）:\n", dist_coeffs)
+    print("\n重投影误差:", ret)
+
+    if needs_correction(dist_coeffs):
+        print("需要矫正：畸变系数过大")
+    else:
+        print("无需矫正：畸变可忽略")
+    return camera_matrix, dist_coeffs, rvecs, tvecs
+
+def find_corners(image_path, columns, rows):
+    # 读取图像
+    image = cv2.imread(image_path)
+    gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
+
+    # 定义棋盘格尺寸 (内角点数量，非方格数)
+    pattern_size = (columns, rows)  # 例如8x8的棋盘有7x7内角点
+
+    # 查找棋盘格角点
+    ret, corners = cv2.findChessboardCorners(gray, pattern_size, None)
+
+    if ret:
+        # 提高角点检测精度
+        criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
+        corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
+
+        # 绘制检测结果
+        cv2.drawChessboardCorners(image, pattern_size, corners, ret)
+        cv2.imshow('Chessboard Corners', image)
+        cv2.waitKey(0)
+        cv2.destroyAllWindows()
+    else:
+        print("未检测到棋盘格")
+
+    print(corners)
+    # 将形状从 (N,1,2) 转换为 (N,2)
+    corners_reshaped = corners.reshape(-1, 2)
+    # print("所有角点坐标:\n", corners_reshaped)
+    fixed_value = 960  # 固定值
+    column_index = 1  # 操作第2列（索引从0开始）
+
+    # 方法：固定值 - 列值
+    corners_reshaped[:, column_index] = fixed_value - corners_reshaped[:, column_index]
+    # print(corners_reshaped)
+    return corners_reshaped
+
+def fun_test(x,cls,corners):
+    print("标定开始")
+    # print(corners)
+    error = 0
+    model = cls()
+    f = model.f
+    H = model.H - 9
+    gamma = math.atan(1 / (math.tan(x[0]) * math.tan(x[1])))
+    seta = math.atan(1 / math.sqrt(pow(math.tan(x[1]), 2) + 1 / pow(math.tan(x[0]), 2)))
+    column = 0
+    for index, value in enumerate(corners):
+        if index % 11 == 10:
+            column += 1
+            index += 1
+            continue
+        Xw, Yw = calc_way.calc_distance(value[0], value[1], x[0], x[1])
+        Xw1, Yw1 = calc_way.calc_distance(corners[index+1][0], corners[index+1][1], x[0], x[1])
+        d2 = math.sqrt((Xw1 - Xw) ** 2 + (Yw1 - Yw) ** 2)
+        error = error + abs(d2 - 60)
+    return error/80
+
+def get_result_test(cls,corners):
+        params = cls,corners
+        bounds = [(0.1, 1.7), (0.1, 1.7)]
+        result = minimize(
+            fun_test,
+            x0=[0.7, 0.9],
+            args=params,
+            # method='Nelder-Mead',  # 或 'trust-constr'
+            method='L-BFGS-B', bounds=bounds,
+            tol=1e-12,  # 高精度容差
+            options={'gtol': 1e-12, 'maxiter': 1000}
+        )
+        return result
+
+
+

Visual measurement-straight/py/calc_slope_line.py

@@ -0,0 +1,74 @@
+import math
+import model
+import calc_way
+from scipy import stats
+
+model = model.Model()
+alpha = model.alpha
+beta = model.beta
+
+"""
+计算地面点的竖直垂线在图像中的斜率
+参数：点在图像中的坐标x、y，相机的外部偏转角参数alpha、beta
+返回值：斜率k
+"""
+def get_k(alpha,beta,x,y):
+    gamma = math.atan(1 / (math.tan(alpha) * math.tan(beta)))
+    seta = math.atan(1 / math.sqrt(pow(math.tan(beta), 2) + 1 / pow(math.tan(alpha), 2)))
+    Xw,Yw = calc_way.calc_distance(x,y,alpha,beta)
+    u = Xw * math.cos(gamma) - Yw * math.sin(gamma)
+    v = Xw * math.sin(gamma) + Yw * math.cos(gamma)
+    tanlamda = -v/u/math.sqrt(1+(math.tan(math.pi/2-seta))**2)
+    lamda = math.atan(tanlamda)
+    k = math.tan(math.pi/2-lamda)
+    return k
+
+def get_k2(alpha,beta,Xw,Yw):
+    gamma = math.atan(1 / (math.tan(alpha) * math.tan(beta)))
+    seta = math.atan(1 / math.sqrt(pow(math.tan(beta), 2) + 1 / pow(math.tan(alpha), 2)))
+    u = Xw * math.cos(gamma) - Yw * math.sin(gamma)
+    v = Xw * math.sin(gamma) + Yw * math.cos(gamma)
+    tanlamda = -v / u / math.sqrt(1 + (math.tan(math.pi / 2 - seta)) ** 2)
+    lamda = math.atan(tanlamda)
+    k = math.tan(math.pi / 2 - lamda)
+    return k
+
+"""
+计算地面点的竖直垂线在图像中的截距
+参数：点在图像中的坐标x、y，斜率k
+返回值：截距b
+"""
+def get_b(x,y,k):
+    b = y - x * k
+    return b
+
+
+"""
+将检测到的路沿上侧数据点拟合为一条直线
+参数：上侧数据点坐标x_top,y_top
+返回值：拟合出直线的斜率slope、截距intercept、r方值
+"""
+def linear_regression(x, y):
+    slope, intercept, r_value, p_value, std_err = stats.linregress(x, y)
+    return slope, intercept, abs(r_value)
+
+"""
+计算图像中两条直线的交点
+参数：   line1: 第一条直线的系数 (A1, B1, C1)
+        line2: 第二条直线的系数 (A2, B2, C2)
+返回值：交点在图像中的二维坐标x、y
+"""
+def find_intersection(line1, line2):
+    A1, B1, C1 = line1
+    A2, B2, C2 = line2
+
+    # 计算行列式
+    denominator = A1 * B2 - A2 * B1
+
+    if denominator == 0:
+        # 直线平行或重合
+        return None
+    else:
+        x = (B1 * C2 - B2 * C1) / denominator
+        y = (A2 * C1 - A1 * C2) / denominator
+        return (x, y)

Visual measurement-straight/py/calc_way.py

@@ -0,0 +1,198 @@
+import math
+
+import numpy as np
+
+import model
+
+model = model.Model()
+u0 = model.u0
+v0 = model.v0
+f = model.f
+H = model.H
+dx = model.dx
+dy = model.dy
+alpha = model.alpha
+beta = model.beta
+"""
+计算图像中一点到车辆坐标的距离
+参数：点在图像中的坐标x、y，相机的外部偏转角参数alpha、beta
+返回值：点在车辆坐标系下的二维坐标
+"""
+def calc_distance(x, y, alpha, beta):
+    Xp = (x - u0) * dx
+    Yp = -(y - v0) * dy
+    sq = math.sqrt(f * f + Yp * Yp)
+    eta = math.atan(Yp / f)
+    gamma = math.atan(1 / (math.tan(alpha) * math.tan(beta)))
+    seta = math.atan(1/math.sqrt(pow(math.tan(beta), 2) + 1 / pow(math.tan(alpha), 2)))
+    MP = H * math.fabs(Xp) / (math.sqrt(f ** 2 + Yp ** 2) * math.sin(seta + eta))
+    OM = H / math.tan(seta + eta)
+    epsilon = math.atan((H * Xp / (math.sqrt(f ** 2 + Yp ** 2) * math.sin(seta + eta))) / OM)
+    d = math.sqrt(MP ** 2 + OM ** 2)
+    Xw = d * math.cos(gamma + epsilon)
+    Yw = -d * math.sin(gamma + epsilon)
+    return Xw, Yw
+
+"""
+计算车辆坐标系下某点在图像中的位置
+参数：点在车辆坐标系下的坐标Xw、Yw，相机的外部偏转角参数alpha、beta
+返回值：点在图像中的二维坐标
+"""
+def calc_distance2(Xw, Yw, alpha, beta):
+    d = math.sqrt(Xw ** 2 + Yw ** 2)
+    seta = math.atan(1 / math.sqrt(pow(math.tan(beta), 2) + 1 / pow(math.tan(alpha), 2)))
+    gamma = math.atan(1 / (math.tan(alpha) * math.tan(beta)))
+    # 计算组合角度
+    angle = math.atan2(-Yw, Xw)  # 使用atan2处理所有象限
+    # 解出epsilon
+    epsilon = angle - gamma
+    # 将结果调整到[-pi, pi]范围内
+    epsilon = (epsilon + math.pi) % (2 * math.pi) - math.pi
+    OM = math.fabs(d * math.cos(epsilon))
+    MP = math.fabs(d * math.sin(epsilon))
+    eta = math.atan(H / OM) - seta
+    Yp = math.tan(eta) * f
+    Xp = MP * math.sqrt(f ** 2 + Yp ** 2) * math.sin(seta + eta)/H
+    if epsilon > 0 :
+        Xp = Xp
+    else:
+        Xp = -Xp
+    x = Xp / dx +u0
+    y = -Yp / dy +v0
+    return int(x), int(y)
+
+
+
+"""
+计算图像中一点距离地面的高度
+参数：地面端点在图像中的坐标x_bot、y_bot，垂线上端点在图像中的坐标x_top、y_top，相机的外部偏转角参数alpha、beta
+返回值：点距离地面的高度
+"""
+def calc_height(x_bot,y_bot,x_top,y_top,alpha,beta):
+    Xp = (x_bot - u0) * dx
+    Yp = -(y_bot - v0) * dy
+    sq = math.sqrt(f * f + Yp * Yp)
+    eta = math.atan(Yp / f)
+    gamma = math.atan(1 / (math.tan(alpha) * math.tan(beta)))
+    seta = math.atan(1/math.sqrt(pow(math.tan(beta), 2) + 1 / pow(math.tan(alpha), 2)))
+    MP = H * math.fabs(Xp) / (math.sqrt(f ** 2 + Yp ** 2) * math.sin(seta + eta))
+    OM = H/math.tan(seta + eta)
+    Yp1 = -(y_top - v0) * dy
+    sq1 = math.sqrt(f * f + Yp1 * Yp1)
+    eta1 = math.atan(Yp1 / f)
+    # print(f"Yp1: {Yp1},eta1: {eta1}")
+    Zw = H - OM*math.tan(seta + eta1)
+    return Zw
+
+"""
+计算图像中车辆坐标系Xw、Yw所对应的轴线
+参数：
+返回值：轴线上点在图像中的二维坐标x、y
+"""
+def calc_zeros_yto0(val):
+    # 定义搜索范围和步长
+    x_range = np.linspace(0, 1280, 1000)
+    y_range = np.linspace(0, 960, 1000)
+
+    # 存储解
+    solutions = []
+    tolerance = 1e-3  # 容忍误差
+
+    # 网格搜索
+    for x in x_range:
+        for y in y_range:
+            error = equations_yto0(x, y, val)
+            if error < tolerance:
+                solutions.append((x, y))
+
+    # 去除近似重复的解
+    unique_solutions = []
+    for sol in solutions:
+        # 检查是否与已找到的解近似
+        is_unique = True
+        for usol in unique_solutions:
+            if np.allclose(sol, usol, atol=1):
+                is_unique = False
+                break
+        if is_unique:
+            unique_solutions.append(sol)
+    x = []
+    y = []
+    # print("找到的解:")
+    for sol in unique_solutions:
+        # print(f"x = {sol[0]:.4f}, y = {sol[1]:.4f}")
+        x.append(sol[0])
+        y.append(sol[1])
+    return x, y
+
+def equations_yto0(x, y, val):
+    Xp = (x - u0) * dx
+    Yp = -(y - v0) * dy
+    sq = math.sqrt(f * f + Yp * Yp)
+    eta = math.atan(Yp / f)
+    gamma = math.atan(1 / (math.tan(alpha) * math.tan(beta)))
+    seta = math.atan(1/math.sqrt(pow(math.tan(beta), 2) + 1 / pow(math.tan(alpha), 2)))
+    MP = H * math.fabs(Xp) / (math.sqrt(f ** 2 + Yp ** 2) * math.sin(seta + eta))
+    OM = H / math.tan(seta + eta)
+    epsilon = math.atan((H * Xp / (math.sqrt(f ** 2 + Yp ** 2) * math.sin(seta + eta))) / OM)
+    d = math.sqrt(MP ** 2 + OM ** 2)
+    Xw = d * math.cos(gamma + epsilon)
+    Yw = -d * math.sin(gamma + epsilon)
+    return (Yw - val)**2   # 误差平方和
+
+"""
+计算图像中车辆坐标系Yw=某一固定值所对应的轴线
+参数：
+返回值：轴线上点在图像中的二维坐标x、y
+"""
+def calc_zeros_xto0(val):
+    # 定义搜索范围和步长
+    x_range = np.linspace(0, 1280, 1000)
+    y_range = np.linspace(0, 960, 1000)
+
+    # 存储解
+    solutions = []
+    tolerance = 1e-3  # 容忍误差
+
+    # 网格搜索
+    for x in x_range:
+        for y in y_range:
+            error = equations_xto0(x, y, val)
+            if error < tolerance:
+                solutions.append((x, y))
+
+    # 去除近似重复的解
+    unique_solutions = []
+    for sol in solutions:
+        # 检查是否与已找到的解近似
+        is_unique = True
+        for usol in unique_solutions:
+            if np.allclose(sol, usol, atol=1):
+                is_unique = False
+                break
+        if is_unique:
+            unique_solutions.append(sol)
+    x = []
+    y = []
+    # print("找到的解:")
+    for sol in unique_solutions:
+        # print(f"x = {sol[0]:.4f}, y = {sol[1]:.4f}")
+        x.append(sol[0])
+        y.append(sol[1])
+    return x, y
+
+def equations_xto0(x, y, val):
+    Xp = (x - u0) * dx
+    Yp = -(y - v0) * dy
+    sq = math.sqrt(f * f + Yp * Yp)
+    eta = math.atan(Yp / f)
+    gamma = math.atan(1 / (math.tan(alpha) * math.tan(beta)))
+    seta = math.atan(1/math.sqrt(pow(math.tan(beta), 2) + 1 / pow(math.tan(alpha), 2)))
+    MP = H * math.fabs(Xp) / (math.sqrt(f ** 2 + Yp ** 2) * math.sin(seta + eta))
+    OM = H / math.tan(seta + eta)
+    epsilon = math.atan((H * Xp / (math.sqrt(f ** 2 + Yp ** 2) * math.sin(seta + eta))) / OM)
+    d = math.sqrt(MP ** 2 + OM ** 2)
+    Xw = d * math.cos(gamma + epsilon)
+    Yw = -d * math.sin(gamma + epsilon)
+    return (Xw-val)**2   # 误差平方和
+

Visual measurement-straight/py/get_data.py

@@ -0,0 +1,150 @@
+import numpy as np
+import pandas as pd
+import calc_way
+from scipy import stats
+import calc_slope_line
+import matplotlib.pyplot as plt
+import model
+import os
+# 数据截断线
+model = model.Model()
+limit_slope = model.limit_slope
+limit_intercept = model.limit_intercept
+def grid_downsample(points, cell_size=15):
+    """网格化降采样，保持空间结构"""
+    df = pd.DataFrame(points, columns=['x', 'y'])
+    df['x_grid'] = (df['x'] // cell_size) * cell_size
+    df['y_grid'] = (df['y'] // cell_size) * cell_size
+    sampled = df.groupby(['x_grid', 'y_grid']).first().reset_index()
+    return sampled[['x', 'y']].values
+
+"""
+读取yolo网络识别路沿的坐标数据,筛选出目标区域的数据点，并将路沿上下侧数据分离
+参数：保存数据的txt文件路径
+返回值：在目标区域内的下侧数据点坐标x_bot、y_bot，上侧数据点坐标x_top,y_top
+"""
+def get_data(txt_name):
+    # 加载数据
+    data = np.loadtxt(txt_name)
+    int_data = data.astype(int)
+
+    # 网格化降采样
+    grid_sampled = grid_downsample(int_data, cell_size=20)
+
+    # 数据截断
+    x = []
+    y = []
+    for i in range(grid_sampled.shape[0]):
+        grid_sampled[i][1] = 960 - int(grid_sampled[i][1])
+        if limit_slope * int(grid_sampled[i][0]) + limit_intercept - int(grid_sampled[i][1]) < 0:
+            continue
+        x.append(int(grid_sampled[i][0]))
+        y.append(int(grid_sampled[i][1]))
+    x = np.array(x)
+    y = np.array(y)
+
+    # 原始数据粗分类
+    slope, intercept, r_2 = calc_slope_line.linear_regression(x, y)
+    y_pred = slope * x + intercept
+    x_bot = []
+    y_bot = []
+    x_top = []
+    y_top = []
+    for i in range(len(x)):
+        if x[i] * slope + intercept - y[i] > 0:
+            x_bot.append(x[i])
+            y_bot.append(y[i])
+        else:
+            x_top.append(x[i])
+            y_top.append(y[i])
+    x_bot = np.array(x_bot)
+    y_bot = np.array(y_bot)
+    x_top = np.array(x_top)
+    y_top = np.array(y_top)
+    slope_bot, intercept_bot, r2_bot = calc_slope_line.linear_regression(x_bot, y_bot)
+    slope_top, intercept_top, r2_top = calc_slope_line.linear_regression(x_top, y_top)
+    print(f"未清洗数据拟合上下沿：r2_bot = {r2_bot},r2_top = {r2_top}")
+
+    # 第一次数据清洗，消除误识别点
+    # 计算残差
+    residuals = y - y_pred
+    # 计算残差的标准差 (MSE的平方根)
+    residual_std = np.sqrt(np.sum(residuals ** 2) / (len(x) - 2))
+    standardized_residuals = residuals / residual_std
+    # 设置阈值 (常用 2.5-3.0 个标准差)
+    threshold = 2.0
+    # 标记异常点
+    outlier_mask = np.abs(standardized_residuals) > threshold
+    outliers_x = x[outlier_mask]
+    outliers_y = y[outlier_mask]
+    print(f"第一次数据清洗发现 {np.sum(outlier_mask)} 个异常点:")
+    for i, (x_val, y_val) in enumerate(zip(outliers_x, outliers_y)):
+        print(f"点 {i + 1}: x={x_val}, y={y_val}, 残差={residuals[outlier_mask][i]:.2f}")
+    # 剔除异常点
+    clean_x = x[~outlier_mask]
+    clean_y = y[~outlier_mask]
+    clean_slope, clean_intercept, clean_r_2 = calc_slope_line.linear_regression(clean_x, clean_y)
+    print(f"清洗数据后整体拟合参数r_2 = {r_2}")
+
+    # 第一次数据清洗后的数据再分类
+    x_bot_clean = []
+    y_bot_clean = []
+    x_top_clean = []
+    y_top_clean = []
+    for i in range(len(clean_x)):
+        if clean_x[i] * clean_slope + clean_intercept - clean_y[i] > 0:
+            x_bot_clean.append(clean_x[i])
+            y_bot_clean.append(clean_y[i])
+        else:
+            x_top_clean.append(clean_x[i])
+            y_top_clean.append(clean_y[i])
+    x_bot_clean = np.array(x_bot_clean)
+    y_bot_clean = np.array(y_bot_clean)
+    x_top_clean = np.array(x_top_clean)
+    y_top_clean = np.array(y_top_clean)
+
+    # 第二次数据清洗，消除误分类点
+    clean_slope_bot, clean_intercept_bot, clean_r2_bot = calc_slope_line.linear_regression(x_bot_clean, y_bot_clean)
+    clean_slope_top, clean_intercept_top, clean_r2_top = calc_slope_line.linear_regression(x_top_clean, y_top_clean)
+    print(f"清洗数据后上下沿拟合参数clean_r2_bot = {clean_r2_bot},clean_r2_top = {clean_r2_top}")
+    # 绘制拟合线
+    y_bot_pred = clean_slope_bot * x_bot_clean + clean_intercept_bot
+    y_top_pred = clean_slope_top * x_top_clean + clean_intercept_top
+    # 计算残差
+    residuals_bot = y_bot_clean - y_bot_pred
+    residuals_top = y_top_clean - y_top_pred
+    # 计算残差的标准差 (MSE的平方根)
+    residual_std_bot = np.sqrt(np.sum(residuals_bot ** 2) / (len(x_bot_clean) - 2))
+    residual_std_top = np.sqrt(np.sum(residuals_top ** 2) / (len(x_top_clean) - 2))
+    # 计算标准化残差 (Z-score)
+    standardized_residuals_bot = residuals_bot / residual_std_bot
+    standardized_residuals_top = residuals_top / residual_std_top
+    # 设置阈值 (常用 2.5-3.0 个标准差)
+    threshold = 1.0
+    # 标记异常点
+    outlier_mask_bot = np.abs(standardized_residuals_bot) > threshold
+    outlier_mask_top = np.abs(standardized_residuals_top) > threshold
+    outliers_x_bot = x_bot_clean[outlier_mask_bot]
+    outliers_y_bot = y_bot_clean[outlier_mask_bot]
+    outliers_x_top = x_top_clean[outlier_mask_top]
+    outliers_y_top = y_top_clean[outlier_mask_top]
+    print(f"第二次数据清洗下沿发现 {np.sum(outlier_mask_bot)} 个异常点:")
+    # for i, (x_val, y_val) in enumerate(zip(outliers_x_bot, outliers_y_bot)):
+    #     print(f"点 {i + 1}: x={x_val}, y={y_val}, 残差={residuals_bot[outlier_mask_bot][i]:.2f}")
+    print(f"第二次数据清洗上沿发现 {np.sum(outlier_mask_top)} 个异常点:")
+    # for i, (x_val, y_val) in enumerate(zip(outliers_x_top, outliers_y_top)):
+    #     print(f"点 {i + 1}: x={x_val}, y={y_val}, 残差={residuals_top[outlier_mask_top][i]:.2f}")
+    # 剔除异常点
+    x_bot_clean = x_bot_clean[~outlier_mask_bot]
+    y_bot_clean = y_bot_clean[~outlier_mask_bot]
+    x_top_clean = x_top_clean[~outlier_mask_top]
+    y_top_clean = y_top_clean[~outlier_mask_top]
+
+    # 判断数据的有效性
+    clean_slope_bot, clean_intercept_bot, clean_r2_bot = calc_slope_line.linear_regression(x_bot_clean, y_bot_clean)
+    clean_slope_top, clean_intercept_top, clean_r2_top = calc_slope_line.linear_regression(x_top_clean, y_top_clean)
+    print(f"清洗数据后上下沿拟合参数clean_r2_bot = {clean_r2_bot},clean_r2_top = {clean_r2_top}")
+    if ((1-clean_r2_bot) > 1e-3) or ((1-clean_r2_top) > 1e-3):
+        print("无效数据")
+        return 0, None, None, None, None
+    return 1, x_bot_clean, y_bot_clean, x_top_clean, y_top_clean

Visual measurement-straight/py/measure_lib.py

@@ -0,0 +1,45 @@
+import math
+import numpy as np
+import matplotlib.pyplot as plt
+import calc_way
+import get_data
+import calc_slope_line
+import cv2
+import model
+import os
+import time
+
+model = model.Model()
+alpha = model.alpha
+beta = model.beta
+
+
+def vs_measurement(txt_name, position=900):
+    # 加载数据
+    state, x_bot, y_bot, x_top, y_top = get_data.get_data(txt_name)
+    if state == 0:
+        return 0, None, None
+    # 拟合上下沿
+    slope_bot, intercept_bot, r2_bot = calc_slope_line.linear_regression(x_bot, y_bot)
+    slope_top, intercept_top, r2_top = calc_slope_line.linear_regression(x_top, y_top)
+    Xw_bot = []
+    Yw_bot = []
+    for i in range(len(x_bot)):
+        Xw, Yw = calc_way.calc_distance(x_bot[i], y_bot[i], alpha, beta)
+        Xw_bot.append(Xw)
+        Yw_bot.append(Yw)
+    slope_Xw, intercept_Xw, r2_Xw = calc_slope_line.linear_regression(Xw_bot, Yw_bot)
+    # 计算路沿与车身夹角
+    angle = math.atan(slope_Xw)
+    # 位置修正
+    position = position + model.x
+    # 确认目标点位置并计算高度
+    Yw_pos = slope_Xw * position + intercept_Xw
+    x_pos, y_pos = calc_way.calc_distance2(position, Yw_pos, alpha, beta)
+    k_pos = calc_slope_line.get_k(alpha, beta, x_pos, y_pos)
+    b_pos = calc_slope_line.get_b(x_pos, y_pos, k_pos)
+    x_pos_top, y_pos_top = calc_slope_line.find_intersection((k_pos, -1, b_pos), (slope_top, -1, intercept_top))
+    Zw_pos = calc_way.calc_height(x_pos, y_pos, x_pos_top, y_pos_top, alpha, beta)
+    distance_pos = -(Yw_pos + model.y) * math.cos(angle)
+    return 1, Zw_pos, distance_pos
+

Visual measurement-straight/py/model.py

@@ -0,0 +1,25 @@
+import numpy as np
+from scipy.optimize import minimize
+
+#保存相机内参、外参等参数
+class Model:
+    def __init__(self):
+        # 内参
+        self.K = np.array([[801.8319, 0, 647.8920],
+                           [0, 801.7619, 532],
+                           [0, 0, 1]])
+        self.f = 3.6
+        self.H = 1019.0000170167332
+        self.dx = self.f / self.K[0, 0]
+        self.dy = self.f / self.K[1, 1]
+        self.u0 = self.K[0, 2]
+        self.v0 = self.K[1, 2]
+        # 外参
+        self.alpha = 0.7072338025822084
+        self.beta = 0.9077237961986776
+        # 位置修正
+        self.y = -70
+        self.x = 22
+        # 数据截断线
+        self.limit_slope = 0.3259949467095897
+        self.limit_intercept = 452.86565535382374