最小二乘法曲线拟合（代码注释）

最小二乘使所有点到曲线的方差最小.利用最小二乘对扫描线上的所有数据点进行拟合,得到一条样条曲线,然后逐点计算每一个点Pi到样条曲线的欧拉距离ei（即点到曲线的最短距离）,ε是距离的阈值，事先给定，如果ei≥ε，则将该点判断为噪点.

该方法最重要的事先拟合样条曲线。

确定曲线类型的方法：根据已知数据点类型初步确定曲线类型，经验观察初步尝试拟合函数类型.

曲线类型选择：直线，二次曲线，三次曲线，对数函数拟合，幂函数拟合，直至方差最小。

直线：f(X1) = aX1 + b;

二次曲线：f(X1) = aX12 + bX1 + c;

对数函数：f(X1) = a + b log(X1);

幂函数： f(X1) = aX1b

#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>using namespace std;
using namespace Eigen;int main(int argc, char **argv)
{double ar = 1.0, br = 2.0, cr = 1.0;         // 真实参数值double ae = 2.0, be = -1.0, ce = 5.0;        // 估计参数值int N = 100;                                 // 数据点double w_sigma = 1;                        // 噪声Sigma值double inv_sigma = 1.0 / w_sigma;  //计算sigma的倒数，之后用于误差归一化cv::RNG rng;                                 // OpenCV随机数产生器//产生100组数据，xi,yi(带噪声)vector<double> x_data, y_data;for (int i = 0; i < N; i++){double x = i / 100.0; //相当于x范围是0-1x_data.push_back(x);//rng.gaussian(val)，表示生成一个服从均值为0，标准差为val的高斯分布的随机数y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));}// 开始Gauss-Newton迭代int iterations = 100;    // 迭代次数double cost = 0, lastCost = 0;  // 本次迭代的cost代价和上一次迭代的cost代价for (int iter = 0; iter < iterations; iter++){//将矩阵H初始化为3*3零矩阵，表示海塞矩阵，H = J * (sigma * sigma).transpose() * J.transpose()Matrix3d H = Matrix3d::Zero();             // Hessian = J^T W^{-1} J in Gauss-Newton//将b初始化为3*1零向量，b = -J * (sigma * sigma).transpose() * error，error表示测量方程的残差Vector3d b = Vector3d::Zero();             // biascost = 0;//遍历所有数据点计算H，b和costfor (int i = 0; i < N; i++){double xi = x_data[i], yi = y_data[i];  // 第i个数据点double error = yi - exp(ae * xi * xi + be * xi + ce);  //观测值-模型计算值Vector3d J; // 雅可比矩阵J[0] = -xi * xi * exp(ae * xi * xi + be * xi + ce);  // de/daJ[1] = -xi * exp(ae * xi * xi + be * xi + ce);  // de/dbJ[2] = -exp(ae * xi * xi + be * xi + ce);  // de/dcH += inv_sigma * inv_sigma * J * J.transpose();  //这里除以sigma是归一化b += -inv_sigma * inv_sigma * error * J;cost += error * error; //error表示测量方程的残差//cout << "N: " << i <<endl;}// 求解线性方程 Hx=bVector3d dx = H.ldlt().solve(b);  //ldlt()表示利用Cholesky分解求dxif (isnan(dx[0]))  //isnan()函数判断输入是否为非数字，是非数字返回真，nan全称为not a number{cout << "result is nan!" << endl;break;}if (iter > 0 && cost >= lastCost)  //因为iter要大于0，第1次迭代(iter = 0, cost > lastCost)不执行！{cout << "cost: " << cost << ">= last cost: " << lastCost << ", break." << endl;break;}//更新优化变量ae，be和ce！ae += dx[0];be += dx[1];ce += dx[2];lastCost = cost;//cout << "iter total: " << iter << endl;cout << "total cost: " << cost << ", \t\tupdate: " << dx.transpose() <<"\t\testimated params: " << ae << "," << be << "," << ce << endl;}cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;return 0;
}

最小二乘法曲线拟合（代码注释）相关推荐

视觉slam十四讲ch6曲线拟合代码注释（笔记版）
1 #include <opencv2/core/core.hpp> 2 #include <ceres/ceres.h> 3 #include <chrono> ...
matlab polyfit c语言,算法——纯C语言最小二乘法曲线拟合
算法--纯C语言最小二乘法曲线拟合 [复制链接] 写完,还没来得及写注释,已通过Matlab的polyfit验证(阶数高或者数据量太大会有double数据溢出的危险,低阶的都吻合),时间有点紧,程序注 ...
c语言平曲线,算法——纯C语言最小二乘法曲线拟合
算法--纯C语言最小二乘法曲线拟合写完,还没来得及写注释,已通过Matlab的polyfit验证(阶数高或者数据量太大会有double数据溢出的危险,低阶的都吻合),时间有点紧,程序注释,数学推导等 ...
最小二乘法曲线拟合 java_最小二乘法拟合java实现源程序（转）
因为我所在的项目要用到最小二乘法拟合,所有我抽时间将C++实现的程序改为JAVA实现,现在贴出来,供大家参考使用. /** * 函数功能:最小二乘法曲线拟合 * @param x 实型一维数组,长度为 ...
归并排序(代码注释超详细)
归并排序: (复制粘贴百度百科没什么意思),简单来说,就是对数组进行分组,然后分组进行排序,排序完最后再整合起来排序! 我看了很多博客,都是写的8个数据呀什么的(2^4,分组方便),我就想着,要是10 ...
代码注释//_您应该停止编写//的五个代码注释，并且//应该开始的一个注释
代码注释// 提供来自您最喜欢和最受欢迎的开源项目的示例-React,Angular,PHP,Pandas等! (With examples from your favorite and most p ...
tensorflow笔记：流程，概念和简单代码注释
tensorflow是google在2015年开源的深度学习框架,可以很方便的检验算法效果.这两天看了看官方的tutorial,极客学院的文档,以及综合tensorflow的源码,把自己的心得整理了一 ...
yolov3网络结构图_目标检测——YOLO V3简介及代码注释（附github代码——已跑通）...
GitHub: liuyuemaicha/PyTorch-YOLOv3github.com 注:该代码fork自eriklindernoren/PyTorch-YOLOv3,该代码相比master分 ...
Kotlin------函数和代码注释
定义函数 Kotlin定义一个函数的风格大致如下访问控制符 fun 方法名(参数,参数,参数) : 返回值类型{...... } 访问控制符:与Java有点差异,Kotlin的访问范围从大到小分别是 ...
php代码注释处理类库,php代码注释
代码注释在多人开发的时候非常重要,现象一下,一段代码没有任何主要你去结合运行的效果去看实现的逻辑,那是非常费劲的事. 如果让别人看懂你写的代码,代码注释启动非常重要的作用.一个不会写代码注释的不是一个 ...

最小二乘法曲线拟合（代码注释）

最小二乘法曲线拟合（代码注释）相关推荐

最新文章

热门文章