algorithm/Path_fitting/Path_JC.c

/**
 ******************************************************************************
 * @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;
}
 */