import numpy as np from sklearn.linear_model import RANSACRegressor, LinearRegression import matplotlib.pyplot as plt import pandas as pd import calc_way 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 def filter_middle_percent(data_x, data_y, cameraModel): """ 保留数组中间80%的数据（删除首尾各10%）。 参数: data (np.ndarray): 输入数组（可以是一维或多维，但会先展平）。 返回: np.ndarray: 中间80%的数据。 """ # 展平数组（确保处理的是所有数据点） # Xw_bot = [] # for i in range(len(data_x)): # Xw, Yw = calc_way.calc_distance(cameraModel, data_x[i], data_y[i]) # Xw_bot.append(Xw) # Xw_bot = np.array(Xw_bot) # flattened_data = Xw_bot.flatten() flattened_data = data_x.flatten() # # 计算10%和90%分位数 # lower_bound = np.percentile(flattened_data, 15) # upper_bound = np.percentile(flattened_data, 75) # # # 筛选中间80%的数据 # mask = (data >= lower_bound) & (data <= upper_bound) # 计算最大值、最小值和总范围 data_min = np.min(flattened_data) data_max = np.max(flattened_data) data_range = data_max - data_min # print(f'data_range: {data_range}, data_min: {data_min}, data_max: {data_max}') # 计算中间80%的上下界 lower_bound = data_min + cameraModel.filt_percent * data_range upper_bound = data_max - cameraModel.filt_percent * data_range # 筛选数据 mask = (flattened_data >= lower_bound) & (flattened_data <= upper_bound) return mask def load_data(cameraModel, txt_name): """从用户输入的文件路径加载二维XY数据""" while True: filepath = txt_name.strip() if filepath.lower() == 'q': return None, None try: data = np.loadtxt(filepath) if cameraModel.grid_downsample: int_data = data.astype(int) grid_sampled = grid_downsample(int_data, cell_size=cameraModel.cell_size) data = grid_sampled if data.shape[1] != 2: print("错误：文件必须包含两列数据（X和Y）") continue x = data[:, 0].reshape(-1, 1) y = data[:, 1].reshape(-1, 1) y = cameraModel.camera_height - y if cameraModel.filter: mask = filter_middle_percent(x, y, cameraModel) x_clean = x[mask] y_clean = y[mask] else: x_clean = x y_clean = y return x_clean.reshape(-1, 1), y_clean.reshape(-1, 1) except Exception as e: print(f"加载文件出错: {e}") def ransac_fit(x, y, residual_threshold=2.5): """执行RANSAC拟合并返回模型和内点/外点""" ransac = RANSACRegressor( LinearRegression(), residual_threshold=residual_threshold, random_state=42 ) ransac.fit(x, y) inlier_mask = ransac.inlier_mask_ outlier_mask = ~inlier_mask return ransac.estimator_, inlier_mask, outlier_mask def get_data(cameraModel, txt_name): # 加载数据 x, y = load_data(cameraModel, txt_name) # print(f"文件数据点个数为：{len(x)}\n") if x is None: return 0, None, None, None, None, None, None, None, None # 第一次RANSAC拟合 model1, inlier_mask1, outlier_mask1 = ransac_fit(x, y, residual_threshold=cameraModel.ransac_residual_threshold) x_inliers1 = x[inlier_mask1] y_inliers1 = y[inlier_mask1] # 获取第一次拟合的外点 x_outliers1 = x[outlier_mask1] y_outliers1 = y[outlier_mask1] # 第二次RANSAC拟合（在外点上） model2, inlier_mask2, outlier_mask2 = None, None, None # print(f"第一次拟合内点数量：{len(x_inliers1)}\n外点数量：{len(x_outliers1)}\n") if len(x_outliers1) > cameraModel.outlier_num: # 确保有足够的外点进行第二次拟合 model2, inlier_mask2, outlier_mask2 = ransac_fit(x_outliers1, y_outliers1, residual_threshold=cameraModel.ransac_residual_threshold) else: print(f"外点数量不足\n第一次拟合内点数量：{len(x_inliers1)}\n外点数量：{len(x_outliers1)}\n") return 0, None, None, None, None, None, None, None, None x_inliers2 = x_outliers1[inlier_mask2] y_inliers2 = y_outliers1[inlier_mask2] # 获取第二次拟合的外点 x_outliers2 = x_outliers1[outlier_mask2] y_outliers2 = y_outliers1[outlier_mask2] print(f"第二次拟合内点数量：{len(x_inliers2)}\n外点数量：{len(x_outliers2)}\n") m1 = model1.predict(np.array([600]).reshape(-1, 1)) m2 = model2.predict(np.array([600]).reshape(-1, 1)) # 判断上下沿 if m1 > m2: model_top, model_bot = model1, model2 x_top, x_bot = x_inliers1, x_inliers2 y_top, y_bot = y_inliers1, y_inliers2 else: model_top, model_bot = model2, model1 x_top, x_bot = x_inliers2, x_inliers1 y_top, y_bot = y_inliers2, y_inliers1 # 统一提取斜率和截距 slope_top = model_top.coef_[0][0] intercept_top = model_top.intercept_[0] slope_bot = model_bot.coef_[0][0] intercept_bot = model_bot.intercept_[0] # plt.figure(figsize=(14, 7)) # # # 绘制原始内点 # plt.scatter(x_inliers1, y_inliers1, # color='limegreen', marker='o', s=30, alpha=0.7, # label='first_inliers') # # # 绘制第一次拟合的直线 # line_x = np.array([x.min(), x.max()]).reshape(-1, 1) # line_y_1 = model1.predict(line_x) # plt.plot(line_x, line_y_1, color='darkgreen', linewidth=3, # label='first RANSAC') # # plt.scatter(x_inliers2, y_inliers2, # color='yellow', marker='o', s=100, edgecolor='black', # label='second_inliers') # # # 绘制第二次拟合的直线 # line_x = np.array([x.min(), x.max()]).reshape(-1, 1) # line_y_1 = model2.predict(line_x) # plt.plot(line_x, line_y_1, color='darkgreen', linewidth=3, # label='second RANSAC') # # plt.scatter(x_outliers2, y_outliers2, # color='gray', marker='*', s=100, edgecolor='black', # label='second outliner') # # plt.xlabel('X', fontsize=12) # plt.ylabel('Y', fontsize=12) # plt.title(f'{txt_name}', fontsize=14) # plt.legend(fontsize=10, loc='best') # plt.grid(True, alpha=0.3) # plt.tight_layout() # plt.show() return 1, x_bot, y_bot, slope_bot, intercept_bot, x_top, y_top, slope_top, intercept_top