/** ****************************************************************************** * @file : 拟合算法及功能函数 * @author : yall * @brief : * @attention : None * @date : 2025/6/18 ****************************************************************************** */ #include "Path_JC.h" #include "studio_geo_c.h" double R_EN = 6371000.0; /** * 交换函数(自用) */ void swap(double *a, double *b) { float temp = *a; *a = *b; *b = temp; } /** * 角转弧 */ double deg2rad(double deg) { return deg * PI / 180.0; } /** * 累计距离 */ void cumdist(studio_line_c *line, float *s, unsigned int size){ for (int i = 1; i < size; i++) { float dx = line->data[i].x - line->data[i+1].x; float dy = line->data[i].y - line->data[i+1].y; s[i] = s[i - 1] + sqrtf(dx * dx + dy * dy); } } /** * 转笛卡尔(弧度简易版) */ void deg2Des(studio_line_c *line, unsigned int size) { studio_point_c lon_lat_0 = studio_line_c_get_point(line,0); for (size_t i = 0; i < size; ++i) { studio_point_c tmp; tmp.x = line->data[i].x - lon_lat_0.x; tmp.y = line->data[i].y - lon_lat_0.y; // 计算平面坐标(米) tmp.x = deg2rad(tmp.x) * R_EN * cos(deg2rad(line->data[i].y)); tmp.y = deg2rad(tmp.y) * R_EN; studio_line_c_set_point(line, i, tmp); } } /** * 中值滤波 */ void median_filter_2d(studio_line_c *input, studio_line_c *output, unsigned int size, int window_size) { int half = window_size / 2; studio_point_c window[window_size]; for (int i = 0; i < size; i++) { int k = 0; for (int j = i - half; j <= i + half; j++) { int idn = j; // 边界处理:复制边界值 if (idn < 0) idn = 0; if (idn >= size) idn = size - 1; window[k++] = input->data[idn]; } //排序(冒泡) for(int i = 0; i < window_size - 1; i++) { for(int j = 0; j < window_size - 1 - i; j++) { if(window[j].x > window[j + 1].x) { swap(&window[j].x, &window[j + 1].x); } if(window[j].y > window[j + 1].y) { swap(&window[j].y, &window[j + 1].y); } } } studio_line_c_add_point(output, window[window_size / 2]); } } /** * 残差滤波--计算量偏大 */ void var_filter(studio_line_c *in_before, studio_line_c *in_after, unsigned int size, float threshold) { // 残差 for (int i = 0; i < size; i++) { in_after->data[i].x = in_before->data[i].x - in_after->data[i].x; in_after->data[i].y = in_before->data[i].y - in_after->data[i].y; } // 方差--可优化存储 float mean_rx = 0, mean_ry = 0; for (int i = 0; i < size; i++) { mean_rx += in_after->data[i].x; mean_ry += in_after->data[i].y; } mean_rx /= size; mean_ry /= size; float std_rx = 0, std_ry = 0; for (int i = 0; i < size; i++) { std_rx += pow(in_after->data[i].x - mean_rx, 2); std_ry += pow(in_after->data[i].y - mean_ry, 2); } std_rx = sqrt(std_rx / size); std_ry = sqrt(std_ry / size); // 阈值判断 bool outliers[size]; for (int i = 0; i < size; i++) { outliers[i] = (fabs(in_after->data[i].x) > threshold * std_rx) || (fabs(in_after->data[i].y) > threshold * std_ry); } int idx = 0; for (int i = 0; i < size; i++) { if (outliers[i]) { studio_line_c_remove_point(in_before, idx); idx++; } } } /** * 样条插样 */ void spline_interpolation(float *s, studio_line_c *line, unsigned int size, studio_line_c *tmp, int set_outs) { // 输入检查 if (size < 2 ) { printf("erro...SPLINE"); return; } // 步长 float step = (s[size - 1] - s[0]) / (set_outs - 1); // 计算插值 int idx = 0; for (int i = 0; i < set_outs; i++) { float tar = s[0] + i * step; // 检索区间 while (idx < size - 1 && s[idx + 1] < tar) { idx++; } // 边界检查 if (tar <= s[0]) { tmp->data[i].x = line->data[0].x; tmp->data[i].y = line->data[0].y; } else if (tar >= s[size - 1]) { tmp->data[i].x = line->data[size - 1].x; tmp->data[i].y = line->data[size - 1].y; } else { // 插值计算 if (fabs(s[idx + 1] - s[idx]) < 1e-10) { tmp->data[i].x = (line->data[idx].x + line->data[idx+1].x) / 2.0; tmp->data[i].y = (line->data[idx].y + line->data[idx+1].y) / 2.0; // 取平均值 } else { tmp->data[i].x = line->data[idx].x + (line->data[idx+1].x - line->data[idx].x) * (tar - s[idx]) / (s[idx+1] + s[idx]); tmp->data[i].y = line->data[idx].y + (line->data[idx+1].y - line->data[idx].y) * (tar - s[idx]) / (s[idx+1] + s[idx]); } } } }