algorithm/pafit_spline/include/Path_JC.c

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