|
@@ -0,0 +1,286 @@
|
|
|
+/**
|
|
|
+ ******************************************************************************
|
|
|
+ * @file : 拟合算法及功能函数
|
|
|
+ * @author : yall
|
|
|
+ * @brief :
|
|
|
+ * @attention : None
|
|
|
+ * @date : 2025/6/18
|
|
|
+ ******************************************************************************
|
|
|
+ */
|
|
|
+#include "Path_JC.h"
|
|
|
+#include "studio_geo_c.h"
|
|
|
+
|
|
|
+/**
|
|
|
+ * 交换函数(自用)
|
|
|
+ */
|
|
|
+void swap(float *a, float *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 p_0 = studio_line_c_get_point(line,0);
|
|
|
+ for (int i = 0; i < size; i++)
|
|
|
+ {
|
|
|
+ studio_point_c tmp;
|
|
|
+ studio_point_c p_i = studio_line_c_get_point(line,i);
|
|
|
+ tmp.x = p_i.x - p_0.x;
|
|
|
+ tmp.y = p_i.y - p_0.y;
|
|
|
+ studio_line_c_set_point(line, i, tmp);
|
|
|
+ }
|
|
|
+ // 转化成m
|
|
|
+ for (int i = 0; i < size; i++)
|
|
|
+ {
|
|
|
+ studio_point_c tmp;
|
|
|
+ studio_point_c p_i = studio_line_c_get_point(line,i);
|
|
|
+ tmp.x = p_i.x * (PI / 180.0) * R_EN * cos(deg2rad(tmp.y));
|
|
|
+ tmp.y = p_i.y * (PI / 180.0) * 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]);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+/**
|
|
|
+拟合过程
|
|
|
+
|
|
|
+struct ArrayWrapper Path_fit(double data[MAX_POINTS][2], int window_size)
|
|
|
+ {
|
|
|
+ // 实际数据点数
|
|
|
+ int n = MAX_POINTS;
|
|
|
+
|
|
|
+ // 提取经纬度数据
|
|
|
+ double longitude[MAX_POINTS], latitude[MAX_POINTS];
|
|
|
+ for (int i = 0; i < n; i++) {
|
|
|
+ longitude[i] = data[i][0];
|
|
|
+ latitude[i] = data[i][1];
|
|
|
+ }
|
|
|
+
|
|
|
+ // 计算与第一个点的经纬度差值
|
|
|
+ double delta_lon[MAX_POINTS], delta_lat[MAX_POINTS];
|
|
|
+ for (int i = 0; i < n; i++) {
|
|
|
+ delta_lon[i] = longitude[i] - longitude[0];
|
|
|
+ delta_lat[i] = latitude[i] - latitude[0];
|
|
|
+ }
|
|
|
+
|
|
|
+ // 转换为米单位坐标
|
|
|
+ double delta_x[MAX_POINTS], delta_y[MAX_POINTS];
|
|
|
+ for (int i = 0; i < n; i++) {
|
|
|
+ delta_x[i] = delta_lon[i] * (PI / 180.0) * R_EN * cos(deg2rad(latitude[i]));
|
|
|
+ delta_y[i] = delta_lat[i] * (PI / 180.0) * R_EN;
|
|
|
+ }
|
|
|
+
|
|
|
+ // 中值滤波
|
|
|
+ double x_filtered[MAX_POINTS], y_filtered[MAX_POINTS];
|
|
|
+ medfilt1(delta_x, x_filtered, n, window_size);
|
|
|
+ medfilt1(delta_y, y_filtered, n, window_size);
|
|
|
+
|
|
|
+ // 删除离群值
|
|
|
+ double residual_x[MAX_POINTS], residual_y[MAX_POINTS];
|
|
|
+ for (int i = 0; i < n; i++) {
|
|
|
+ residual_x[i] = delta_x[i] - x_filtered[i];
|
|
|
+ residual_y[i] = delta_y[i] - y_filtered[i];
|
|
|
+ }
|
|
|
+ double threshold_x = 0.1, threshold_y = 0.1; // 阈值
|
|
|
+ int outliers[MAX_POINTS] = {0};
|
|
|
+ for (int i = 0; i < n; i++) {
|
|
|
+ if (fabs(residual_x[i]) > threshold_x || fabs(residual_y[i]) > threshold_y) {
|
|
|
+ outliers[i] = 1;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ double x_cleaned[MAX_POINTS], y_cleaned[MAX_POINTS];
|
|
|
+ int cleaned_count = 0;
|
|
|
+ for (int i = 0; i < n; i++) {
|
|
|
+ if (!outliers[i]) {
|
|
|
+ x_cleaned[cleaned_count] = x_filtered[i];
|
|
|
+ y_cleaned[cleaned_count] = y_filtered[i];
|
|
|
+ cleaned_count++;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ // 参数化数据点:计算累积距离
|
|
|
+ double dist[MAX_POINTS] = {0};
|
|
|
+ for (int i = 1; i < cleaned_count; i++) {
|
|
|
+ dist[i] = sqrt(pow(x_cleaned[i] - x_cleaned[i - 1], 2) + pow(y_cleaned[i] - y_cleaned[i - 1], 2));
|
|
|
+ }
|
|
|
+ double cumdist[MAX_POINTS] = {0};
|
|
|
+ for (int i = 1; i < cleaned_count; i++) {
|
|
|
+ cumdist[i] = cumdist[i - 1] + dist[i];
|
|
|
+ }
|
|
|
+
|
|
|
+ // 生成均匀分布的参数值
|
|
|
+ int num_points = 10000;
|
|
|
+ double t_uniform[num_points];
|
|
|
+ for (int i = 0; i < num_points; i++) {
|
|
|
+ t_uniform[i] = (cumdist[cleaned_count - 1] * i) / (num_points - 1);
|
|
|
+ }
|
|
|
+
|
|
|
+ // 样条插值拟合
|
|
|
+ double x_fit[num_points], y_fit[num_points];
|
|
|
+ spline_interpolation(cumdist, x_cleaned, cleaned_count, t_uniform, x_fit, num_points);
|
|
|
+ spline_interpolation(cumdist, y_cleaned, cleaned_count, t_uniform, y_fit, num_points);
|
|
|
+
|
|
|
+ // 均匀采样30个点
|
|
|
+ int num_uniform = 30;
|
|
|
+ double t_uniform_samples[num_uniform];
|
|
|
+ for (int i = 0; i < num_uniform; i++) {
|
|
|
+ t_uniform_samples[i] = (cumdist[cleaned_count - 1] * i) / (num_uniform - 1);
|
|
|
+ }
|
|
|
+ double x_uniform[num_uniform], y_uniform[num_uniform];
|
|
|
+ spline_interpolation(cumdist, x_cleaned, cleaned_count, t_uniform_samples, x_uniform, num_uniform);
|
|
|
+ spline_interpolation(cumdist, y_cleaned, cleaned_count, t_uniform_samples, y_uniform, num_uniform);
|
|
|
+
|
|
|
+ struct ArrayWrapper xy;
|
|
|
+ // 输出均匀采样的点
|
|
|
+ for (int i = 0; i < num_uniform; i++) {
|
|
|
+ xy.data_xy[i][0] = x_uniform[i];
|
|
|
+ xy.data_xy[i][1] = y_uniform[i];
|
|
|
+ }
|
|
|
+
|
|
|
+ return xy;
|
|
|
+}
|
|
|
+ */
|