路径_样条(简易)

Path_fitting/main.c

@@ -1,37 +0,0 @@
-#include <stdio.h>
-#include <stdlib.h>
-#include <math.h>
-
-#include "Path_JC.h"
-#include "studio_geo_c.h"
-
-
-int  main()
-{
-    // 初始化
-    studio_line_c line = studio_line_c_init();
-    studio_line_c tmp_line = studio_line_c_init();
-
-
-    
-    // 转笛卡尔(简)
-    deg2Des(&line, line.size);
-
-    // 中值滤波
-    median_filter_2d(&line, &tmp_line, line.size, 3);
-
-    // 残差滤波(可不用)
-    var_filter(&line, &tmp_line, line.size, 0.5); 
-
-    // 累计距离
-    float *sum_dis_tmp = (float*)malloc(line.size * sizeof(float));
-    cumdist(line.data, sum_dis_tmp, line.size);
-
-    // 样条插样(一阶)
-    spline_interpolation(sum_dis_tmp, line, line.size, &tmp_line, 30);
-
-    // 释放内存
-    studio_line_c_destroy(&line);
-    studio_line_c* out_line = (studio_line_c*)realloc(&tmp_line, 30 * sizeof(studio_line_c));
-
-}

Path_fitting/.vscode/launch.json → pafit_spline/.vscode/launch.json

@@ -9,7 +9,7 @@
             "type": "cppdbg",
             "request": "launch",
             //"program": "输入程序名称，例如 ${workspaceFolder}/a.exe",
-            "program": "${fileDirname}\\main.exe",
+            "program": "${fileDirname}\\build\\main.exe",
             "args": [],
             "stopAtEntry": false,
             "cwd": "${fileDirname}",

Path_fitting/.vscode/tasks.json → pafit_spline/.vscode/tasks.json

@@ -9,9 +9,10 @@
                 "-g",
                 //"${file}",
                 "*.c",//当前文件夹所有的.c文件
+                "include/*.c", // 包含include文件夹下的.c文件
                 "-o",
                 //"${fileDirname}\\${fileBasenameNoExtension}.exe"
-                "${fileDirname}\\main.exe"//生成的可执行程序
+                "${fileDirname}\\build\\main.exe"//生成的可执行程序
             ],
             "options": {
                 "cwd": "${fileDirname}"

Path_fitting/Path_JC.c → pafit_spline/include/Path_JC.c

@@ -10,10 +10,12 @@
 #include "Path_JC.h"
 #include "studio_geo_c.h"
 
+double R_EN = 6371000.0;
+
 /**
  * 交换函数(自用)
  */
-void swap(float *a, float *b) {
+void swap(double *a, double *b) {
     float temp = *a;
     *a = *b;
     *b = temp;
@@ -184,103 +186,3 @@ void spline_interpolation(float *s, studio_line_c *line, unsigned int size, stud
         }
     }
 }
-
-/**
-拟合过程
-
-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;
-}
- */

Path_fitting/Path_JC.h → pafit_spline/include/Path_JC.h

@@ -4,15 +4,17 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <math.h>
+#include "studio_geo_c.h"
 
 // 数据初始化
 #define MAX_POINTS 500
 #define max_points 30
 #define PI 3.14159265358979323846
-const double R_EN = 6371000.0;
+extern double R_EN; 
+
 
 // 交换函数
-void swap(float *a, float *b);
+void swap(double *a, double *b);
 // 角转弧
 double deg2rad(double deg);
 // 转笛卡尔

pafit_spline/main.c

@@ -0,0 +1,185 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+
+#include "include/Path_JC.h"
+#include "include/studio_geo_c.h"
+
+int  main()
+{
+    // 初始化
+    studio_line_c line = studio_line_c_init();
+    studio_line_c tmp_line = studio_line_c_init();
+    studio_line_c_add_point(&line, studio_point_init( 120.0798731, 36.2145540));
+    studio_line_c_add_point(&line, studio_point_init( 120.0798738, 36.2145750));
+    studio_line_c_add_point(&line, studio_point_init( 120.0798675, 36.2146510));
+    studio_line_c_add_point(&line, studio_point_init( 120.0798691, 36.2147368));
+    studio_line_c_add_point(&line, studio_point_init( 120.0798703, 36.2148213));
+    studio_line_c_add_point(&line, studio_point_init( 120.0798723, 36.2149135));
+    studio_line_c_add_point(&line, studio_point_init( 120.0798666, 36.2149940));
+    studio_line_c_add_point(&line, studio_point_init( 120.0798696, 36.2150526));
+    studio_line_c_add_point(&line, studio_point_init( 120.0798878, 36.2151376));
+    studio_line_c_add_point(&line, studio_point_init( 120.0799663, 36.2151995));
+    studio_line_c_add_point(&line, studio_point_init( 120.0800706, 36.2152123));
+    studio_line_c_add_point(&line, studio_point_init( 120.0801838, 36.2152091));
+    studio_line_c_add_point(&line, studio_point_init( 120.0803001, 36.2152075));
+    studio_line_c_add_point(&line, studio_point_init( 120.0804170, 36.2152073));
+    studio_line_c_add_point(&line, studio_point_init( 120.0805423, 36.2152265));
+    studio_line_c_add_point(&line, studio_point_init( 120.0806736, 36.2152321));
+    studio_line_c_add_point(&line, studio_point_init( 120.0808106, 36.2152131));
+    studio_line_c_add_point(&line, studio_point_init( 120.0809561, 36.2152573));
+    studio_line_c_add_point(&line, studio_point_init( 120.0811123, 36.2152301));
+    studio_line_c_add_point(&line, studio_point_init( 120.0812173, 36.2152231));
+    studio_line_c_add_point(&line, studio_point_init( 120.0812853, 36.2152213));
+    studio_line_c_add_point(&line, studio_point_init( 120.0813195, 36.2152221));
+    studio_line_c_add_point(&line, studio_point_init( 120.0813926, 36.2152216));
+    studio_line_c_add_point(&line, studio_point_init( 120.0815376, 36.2152218));
+    studio_line_c_add_point(&line, studio_point_init( 120.0817028, 36.2152380));
+    studio_line_c_add_point(&line, studio_point_init( 120.0818716, 36.2152200));
+    studio_line_c_add_point(&line, studio_point_init( 120.0820525, 36.2152473));
+    studio_line_c_add_point(&line, studio_point_init( 120.0822268, 36.2152360));
+    studio_line_c_add_point(&line, studio_point_init( 120.0824110, 36.2152340));
+    studio_line_c_add_point(&line, studio_point_init( 120.0826048, 36.2152435));
+    studio_line_c_add_point(&line, studio_point_init( 120.0827026, 36.2152465));
+    studio_line_c_add_point(&line, studio_point_init( 120.0828165, 36.2151315));
+    studio_line_c_add_point(&line, studio_point_init( 120.0828205, 36.2149925));
+    studio_line_c_add_point(&line, studio_point_init( 120.0828221, 36.2148178));
+    studio_line_c_add_point(&line, studio_point_init( 120.0828221, 36.2146781));
+    studio_line_c_add_point(&line, studio_point_init( 120.0827581, 36.2145310));
+    studio_line_c_add_point(&line, studio_point_init( 120.0826711, 36.2144438));
+    studio_line_c_add_point(&line, studio_point_init( 120.0825228, 36.2144608));
+    studio_line_c_add_point(&line, studio_point_init( 120.0824073, 36.2144546));
+    studio_line_c_add_point(&line, studio_point_init( 120.0822536, 36.2143848));
+    studio_line_c_add_point(&line, studio_point_init( 120.0822430, 36.2142630));
+    studio_line_c_add_point(&line, studio_point_init( 120.0822438, 36.2140925));
+    studio_line_c_add_point(&line, studio_point_init( 120.0822320, 36.2139576));
+    studio_line_c_add_point(&line, studio_point_init( 120.0821196, 36.2138603));
+    studio_line_c_add_point(&line, studio_point_init( 120.0819660, 36.2138743));
+    studio_line_c_add_point(&line, studio_point_init( 120.0817780, 36.2139215));
+    studio_line_c_add_point(&line, studio_point_init( 120.0816141, 36.2138886));
+    studio_line_c_add_point(&line, studio_point_init( 120.0814238, 36.2138956));
+    studio_line_c_add_point(&line, studio_point_init( 120.0813170, 36.2139134));
+    studio_line_c_add_point(&line, studio_point_init( 120.0811468, 36.2138908));
+    studio_line_c_add_point(&line, studio_point_init( 120.0810021, 36.2138916));
+    studio_line_c_add_point(&line, studio_point_init( 120.0808083, 36.2138886));
+    studio_line_c_add_point(&line, studio_point_init( 120.0806713, 36.2138871));
+    studio_line_c_add_point(&line, studio_point_init( 120.0804978, 36.2138565));
+    studio_line_c_add_point(&line, studio_point_init( 120.0804513, 36.2137123));
+    studio_line_c_add_point(&line, studio_point_init( 120.0804206, 36.2135151));
+    studio_line_c_add_point(&line, studio_point_init( 120.0804290, 36.2134026));
+    studio_line_c_add_point(&line, studio_point_init( 120.0804241, 36.2132476));
+    studio_line_c_add_point(&line, studio_point_init( 120.0804116, 36.2131533));
+    studio_line_c_add_point(&line, studio_point_init( 120.0802560, 36.2131256));
+    studio_line_c_add_point(&line, studio_point_init( 120.0800926, 36.2131391));
+    studio_line_c_add_point(&line, studio_point_init( 120.0799135, 36.2130891));
+    studio_line_c_add_point(&line, studio_point_init( 120.0798561, 36.2129658));
+    studio_line_c_add_point(&line, studio_point_init( 120.0798958, 36.2127938));
+    studio_line_c_add_point(&line, studio_point_init( 120.0798420, 36.2126813));
+    studio_line_c_add_point(&line, studio_point_init( 120.0799233, 36.2125713));
+    studio_line_c_add_point(&line, studio_point_init( 120.0800858, 36.2125850));
+    studio_line_c_add_point(&line, studio_point_init( 120.0803211, 36.2125908));
+    studio_line_c_add_point(&line, studio_point_init( 120.0804851, 36.2125933));
+    studio_line_c_add_point(&line, studio_point_init( 120.0806090, 36.2125443));
+    studio_line_c_add_point(&line, studio_point_init( 120.0805893, 36.2124040));
+    studio_line_c_add_point(&line, studio_point_init( 120.0805901, 36.2122559));
+    studio_line_c_add_point(&line, studio_point_init( 120.0805881, 36.2121241));
+    studio_line_c_add_point(&line, studio_point_init( 120.0805778, 36.2119698));
+    studio_line_c_add_point(&line, studio_point_init( 120.0805850, 36.2118591));
+    studio_line_c_add_point(&line, studio_point_init( 120.0806126, 36.2116988));
+    studio_line_c_add_point(&line, studio_point_init( 120.0806191, 36.2115958));
+    studio_line_c_add_point(&line, studio_point_init( 120.0805838, 36.2114628));
+    studio_line_c_add_point(&line, studio_point_init( 120.0806978, 36.2114123));
+    studio_line_c_add_point(&line, studio_point_init( 120.0808786, 36.2114136));
+    studio_line_c_add_point(&line, studio_point_init( 120.0810340, 36.2114101));
+    studio_line_c_add_point(&line, studio_point_init( 120.0811905, 36.2114678));
+    studio_line_c_add_point(&line, studio_point_init( 120.0811971, 36.2116091));
+    studio_line_c_add_point(&line, studio_point_init( 120.0812023, 36.2117358));
+    studio_line_c_add_point(&line, studio_point_init( 120.0812043, 36.2118245));
+    studio_line_c_add_point(&line, studio_point_init( 120.0812191, 36.2119326));
+    studio_line_c_add_point(&line, studio_point_init( 120.0812308, 36.2120568));
+    studio_line_c_add_point(&line, studio_point_init( 120.0812268, 36.2121850));
+    studio_line_c_add_point(&line, studio_point_init( 120.0812186, 36.2123110));
+    studio_line_c_add_point(&line, studio_point_init( 120.0812106, 36.2124338));
+    studio_line_c_add_point(&line, studio_point_init( 120.0812160, 36.2125433));
+    studio_line_c_add_point(&line, studio_point_init( 120.0813505, 36.2125793));
+    studio_line_c_add_point(&line, studio_point_init( 120.0815203, 36.2125826));
+    studio_line_c_add_point(&line, studio_point_init( 120.0816993, 36.2125836));
+    studio_line_c_add_point(&line, studio_point_init( 120.0818490, 36.2126136));
+    studio_line_c_add_point(&line, studio_point_init( 120.0818573, 36.2127058));
+    studio_line_c_add_point(&line, studio_point_init( 120.0818611, 36.2128105));
+    studio_line_c_add_point(&line, studio_point_init( 120.0818571, 36.2129800));
+    studio_line_c_add_point(&line, studio_point_init( 120.0817858, 36.2130876));
+    studio_line_c_add_point(&line, studio_point_init( 120.0816405, 36.2131116));
+    studio_line_c_add_point(&line, studio_point_init( 120.0814890, 36.2131188));
+    studio_line_c_add_point(&line, studio_point_init( 120.0813440, 36.2131221));
+    studio_line_c_add_point(&line, studio_point_init( 120.0811786, 36.2131273));
+    studio_line_c_add_point(&line, studio_point_init( 120.0809845, 36.2131298));
+    studio_line_c_add_point(&line, studio_point_init( 120.0808101, 36.2131210));
+    studio_line_c_add_point(&line, studio_point_init( 120.0806503, 36.2131143));
+    studio_line_c_add_point(&line, studio_point_init( 120.0804963, 36.2131231));
+    studio_line_c_add_point(&line, studio_point_init( 120.0804051, 36.2131858));
+    studio_line_c_add_point(&line, studio_point_init( 120.0804320, 36.2133146));
+    studio_line_c_add_point(&line, studio_point_init( 120.0804310, 36.2134671));
+    studio_line_c_add_point(&line, studio_point_init( 120.0804475, 36.2135978));
+    studio_line_c_add_point(&line, studio_point_init( 120.0804466, 36.2136908));
+    studio_line_c_add_point(&line, studio_point_init( 120.0804383, 36.2137131));
+    studio_line_c_add_point(&line, studio_point_init( 120.0804383, 36.2137353));
+    studio_line_c_add_point(&line, studio_point_init( 120.0804388, 36.2138191));
+    studio_line_c_add_point(&line, studio_point_init( 120.0804340, 36.2139444));
+    studio_line_c_add_point(&line, studio_point_init( 120.0804275, 36.2140595));
+    studio_line_c_add_point(&line, studio_point_init( 120.0804311, 36.2141696));
+    studio_line_c_add_point(&line, studio_point_init( 120.0804268, 36.2142596));
+    studio_line_c_add_point(&line, studio_point_init( 120.0804221, 36.2143248));
+    studio_line_c_add_point(&line, studio_point_init( 120.0804638, 36.2144208));
+    studio_line_c_add_point(&line, studio_point_init( 120.0806425, 36.2144220));
+    studio_line_c_add_point(&line, studio_point_init( 120.0807880, 36.2144381));
+    studio_line_c_add_point(&line, studio_point_init( 120.0809288, 36.2144316));
+    studio_line_c_add_point(&line, studio_point_init( 120.0810285, 36.2144035));
+    studio_line_c_add_point(&line, studio_point_init( 120.0811473, 36.2143823));
+    studio_line_c_add_point(&line, studio_point_init( 120.0812603, 36.2143681));
+    studio_line_c_add_point(&line, studio_point_init( 120.0813386, 36.2143738));
+    studio_line_c_add_point(&line, studio_point_init( 120.0814586, 36.2143900));
+    studio_line_c_add_point(&line, studio_point_init( 120.0816028, 36.2144006));
+    studio_line_c_add_point(&line, studio_point_init( 120.0817420, 36.2144165));
+    studio_line_c_add_point(&line, studio_point_init( 120.0818983, 36.2144256));
+    studio_line_c_add_point(&line, studio_point_init( 120.0820240, 36.2144223));
+    studio_line_c_add_point(&line, studio_point_init( 120.0821661, 36.2144248));
+    studio_line_c_add_point(&line, studio_point_init( 120.0822980, 36.2144378));
+    studio_line_c_add_point(&line, studio_point_init( 120.0824326, 36.2144521));
+    studio_line_c_add_point(&line, studio_point_init( 120.0825441, 36.2144588));
+    studio_line_c_add_point(&line, studio_point_init( 120.0826215, 36.2144620));
+    studio_line_c_add_point(&line, studio_point_init( 120.0827366, 36.2144941));
+    studio_line_c_add_point(&line, studio_point_init( 120.0827791, 36.2145916));
+    studio_line_c_add_point(&line, studio_point_init( 120.0828050, 36.2146823));
+    studio_line_c_add_point(&line, studio_point_init( 120.0828073, 36.2148071));
+    studio_line_c_add_point(&line, studio_point_init( 120.0828108, 36.2149333));
+    studio_line_c_add_point(&line, studio_point_init( 120.0827991, 36.2150419));
+    studio_line_c_add_point(&line, studio_point_init( 120.0827600, 36.2151475));
+    studio_line_c_add_point(&line, studio_point_init( 120.0826771, 36.2152325));
+    studio_line_c_add_point(&line, studio_point_init( 120.0825873, 36.2152428));
+    studio_line_c_add_point(&line, studio_point_init( 120.0824720, 36.2152433));
+    studio_line_c_add_point(&line, studio_point_init( 120.0823341, 36.2152420));
+    studio_line_c_add_point(&line, studio_point_init( 120.0822156, 36.2152483));
+    studio_line_c_add_point(&line, studio_point_init( 120.0822723, 36.2155193));
+
+    
+    // 转笛卡尔(简)
+    deg2Des(&line, line.size);
+
+    // 中值滤波
+    median_filter_2d(&line, &tmp_line, line.size, 3);
+
+    // 残差滤波(可不用)
+    var_filter(&line, &tmp_line, line.size, 0.5); 
+
+    // 累计距离
+    float *sum_dis_tmp = (float*)malloc(line.size * sizeof(float));
+    cumdist(&line, sum_dis_tmp, line.size);
+
+    // 样条插样(一阶)
+    spline_interpolation(sum_dis_tmp, &line, line.size, &tmp_line, 30);
+
+    // 释放内存
+    studio_line_c_destroy(&line);
+    studio_line_c_destroy(&tmp_line);
+}