SLAM 基于高斯牛顿法的曲线拟合

这个程序实现了高斯牛顿法拟合一个较为复杂的非线性函数。真实的函数模型是代码里给的真实值a1,b2,c1,把abc代入函数模型得到具体的模型f(x)加入随机噪声后生成了散点图.下面的图片同时绘制了真实曲线和散点图。程序会根据散点图的数据使用高斯牛顿法拟合出a,b,c.第2次迭代最后的拟合结果优化的终止条件是cost变化非常小这是每次迭代的cost,可以看到第11次迭代后的cost和第10次虽然显示上没有区别但是底层的浮点数是不相同的cost改变量已经非常小了 所以就结束优化了。下面是优化结果的列表对比代码#include iostream #include chrono #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.0; // 噪声Sigma值 double inv_sigma 1.0 / w_sigma; cv::RNG rng; // OpenCV随机数产生器 vectordouble x_data, y_data; // 数据 for (int i 0; i N; i) { double x i / 100.0; x_data.push_back(x); 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 chrono::steady_clock::time_point t1 chrono::steady_clock::now(); for (int iter 0; iter iterations; iter) { Matrix3d H Matrix3d::Zero(); // Hessian J^T W^{-1} J in Gauss-Newton Vector3d b Vector3d::Zero(); // bias cost 0; for (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/da J[1] -xi * exp(ae * xi * xi be * xi ce); // de/db J[2] -exp(ae * xi * xi be * xi ce); // de/dc H inv_sigma * inv_sigma * J * J.transpose(); b -inv_sigma * inv_sigma * error * J; cost error * error; } // 求解线性方程 Hxb Vector3d dx H.ldlt().solve(b); if (isnan(dx[0])) { cout result is nan! endl; break; } if (iter 0 cost lastCost) { cout cost: cost last cost: lastCost , break. endl; break; } ae dx[0]; be dx[1]; ce dx[2]; lastCost cost; cout total cost: cost , \t\tupdate: dx.transpose() \t\testimated params: ae , be , ce endl; } chrono::steady_clock::time_point t2 chrono::steady_clock::now(); chrono::durationdouble time_used chrono::duration_castchrono::durationdouble(t2 - t1); cout solve time cost time_used.count() seconds. endl; cout estimated abc ae , be , ce endl; return 0; }参考高翔《视觉SLAM十四讲》