因为毕设要用Bundle Adjustment 进行数据优化，涉及到优化方法的实现，所以想训练一下编程实现的能力。这篇文章也是参考SLAM14讲，通过高斯牛顿法来完成曲线拟合，从而熟悉高斯牛顿法流程和编程

一、只用eigen库实现

1.1 头文件

#include <iostream>
#include <chrono> //计时模块，非核心代码
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>

1.2 高斯牛顿法原理

设曲线为
y = e x p ( a x 2 + b x + c ) y=exp(ax^2+bx+c) y=exp(ax2+bx+c)
加噪声后为
y = e x p ( a x 2 + b x + c ) + W , W 为噪声 y=exp(ax^2+bx+c)+W ,W为噪声 y=exp(ax2+bx+c)+W,W为噪声
获得多个曲线数据点 ( x i , y i ) (x_i,y_i) (xi,yi)。
目标函数为
F = a r g ( a , b , c ) m i n ∑ i = 1 N e i 2 = ( y i − e x p ( a x i 2 + b x i + c ) F=_{arg(a,b,c)}min\sum_{i=1}^Ne_i^2=(y_i-exp(ax_i^2+bx_i+c) F=arg(a,b,c)mini=1∑Nei2=(yi−exp(axi2+bxi+c)
那么对每项误差 e i e_i ei求雅可比得到 J i J_i Ji，得到增量方程
( ∑ i = 1 N J i J i T ) d x = ∑ i = 1 N ( − J i e i ) (\sum_{i=1}^NJ_iJ_i^T)dx=\sum_{i=1}^N(-J_ie_i) (i=1∑NJiJiT)dx=i=1∑N(−Jiei)
求解线性方程，得到增量，更新变量，检查cost是否下降，是的话继续迭代，直到收敛为止

1.3 代码

曲线数据生成

  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随机数产生器vector<double> 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迭代chrono::steady_clock::time_point t1 = chrono::steady_clock::now(); //计时器int iterations = 100;    // 迭代次数double cost = 0, lastCost = 0;  // 本次迭代的cost和上一次迭代的costdouble err;  //每个误差项误差bool switch1=true;
//    bool switch1= false;if(switch1) {//      iterations=10;for (int i = 0; i < iterations; ++i) {//****每次迭代前，把这几个变量置0很重要，之前就是因为没有置0导致数值累计，数值越来越大，优化结果错误了。Vector3d J; //误差函数雅克比Matrix<double, 3, 3> H = Matrix3d::Zero();   //高斯牛顿矩阵Vector3d g = Vector3d::Zero();     //增量方程右边向量cost=0;//加载高斯牛顿矩阵、增量方程右边向量、误差平方和for (int j = 0; j < N; j++) {J[0] = -x_data[j] * x_data[j] * exp(ae * x_data[j] * x_data[j] + be * x_data[j] + ce);  //de/daJ[1] = -x_data[j] * exp(ae * x_data[j] * x_data[j] + be * x_data[j] + ce);    //de/dbJ[2] = -exp(ae * x_data[j] * x_data[j] + be * x_data[j] + ce);      //de/dcerr = y_data[j] - exp(ae * x_data[j] * x_data[j] + be * x_data[j] + ce);  //每一项的误差H += J * J.transpose();g -= J * err;      //这里是负号，一定要注意，之前忘了，debug了很久cost += err * err;  //误差和}cout<<"第"<<i<<"迭代时J100为\t"<<J.transpose()<<endl;  //debug用的，用来检查Jacob计算是否正确//求解方程Hdx=g Vector3d dx = H.ldlt().solve(b);Vector3d dx;dx = H.ldlt().solve(g);  //似乎是用cholesky分解解线性方程组，没有研究if (isnan(dx[0])) {cout << "result is nan!" << endl;break;}//更新变量ae += dx[0];be += dx[1];ce += dx[2];//跳出循环条件if (i > 0 && cost >= lastCost) {      //i>0很重要，否则第一次迭代就跳出循环cout << "当前误差:" << cost << ">=上次误差:" << lastCost << "迭代结束" << endl;break;}lastCost = cost;cout << "迭代次数N=" << i << "\t" << "误差平方和为：" << cost << "\t" << "变量增量为：" << dx.transpose() << "\t估计的参数a,b,c:"<< ae << "," << be << "," << ce << endl;}}

输出结果

  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);cout << "solve time cost = " << time_used.count() << " seconds. " << endl;cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;return 0;
}

最终输出结果

第0迭代时J100为   -383.791 -387.668 -391.584
迭代次数N=0    误差平方和为：3.19575e+06  变量增量为：0.0455771  0.078164 -0.985329  估计的参数a,b,c:2.04558,-0.921836,4.01467
第1迭代时J100为  -161.875  -163.51 -165.161
迭代次数N=1    误差平方和为：376785    变量增量为： 0.065762  0.224972 -0.962521  估计的参数a,b,c:2.11134,-0.696864,3.05215
第2迭代时J100为  -82.3911 -83.2233  -84.064
迭代次数N=2    误差平方和为：35673.6   变量增量为：-0.0670241   0.617616  -0.907497   估计的参数a,b,c:2.04432,-0.0792484,2.14465
第3迭代时J100为   -57.382 -57.9616 -58.5471
迭代次数N=3    误差平方和为：2195.01   变量增量为：-0.522767   1.19192 -0.756452  估计的参数a,b,c:1.52155,1.11267,1.3882
第4迭代时J100为  -52.5053 -53.0356 -53.5714
迭代次数N=4    误差平方和为：174.853   变量增量为：-0.537502  0.909933 -0.386395  估计的参数a,b,c:0.984045,2.0226,1.00181
第5迭代时J100为  -51.8599 -52.3838 -52.9129
迭代次数N=5    误差平方和为：102.78    变量增量为：-0.0919666   0.147331 -0.0573675   估计的参数a,b,c:0.892079,2.16994,0.944438
第6迭代时J100为  -51.7746 -52.2976 -52.8259
迭代次数N=6    误差平方和为：101.937   变量增量为：-0.00117081  0.00196749 -0.00081055    估计的参数a,b,c:0.890908,2.1719,0.943628
第7迭代时J100为  -51.7741 -52.2971 -52.8253
迭代次数N=7    误差平方和为：101.937   变量增量为：  3.4312e-06 -4.28555e-06  1.08348e-06 估计的参数a,b,c:0.890912,2.1719,0.943629
第8迭代时J100为  -51.7741 -52.2971 -52.8254
迭代次数N=8    误差平方和为：101.937   变量增量为：-2.01204e-08  2.68928e-08 -7.86602e-09 估计的参数a,b,c:0.890912,2.1719,0.943629
第9迭代时J100为  -51.7741 -52.2971 -52.8254
迭代次数N=9    误差平方和为：101.937   变量增量为：1.07059e-10 -1.4233e-10 4.12042e-11    估计的参数a,b,c:0.890912,2.1719,0.943629
solve time cost = 0.000600477 seconds.
estimated abc = 0.890912, 2.1719, 0.943629Process finished with exit code 0

1.4 结果可视化

由于对曲线进行了加噪，所以曲线参数a,b,c没有真值，想到通过计算的a,b,c来绘制曲线，与真实曲线对比，可以大概看出迭代过程收敛趋势
下图绘制了真实带噪声曲线，以后迭代过程中1，2，3，4及收敛次曲线

二、手写高斯牛顿法代码框架总结

用伪代码表示一下核心流程

for(迭代次数)
{//三个参数置零，否则会累加H=0;g=0;cost=0;for(遍历所有误差项ei){计算Ji、每项的误差err;H+=ji*ji^t;g-=Ji*ei;cost+=err^2;}跳出循环判断if(cost>=lastcost) //本例中使用cost不下降进行判断，也可以用增量的模是否小于某个阈值进行判断break;解增量方程  H*dx=g（参考高翔代码，使用的cholesky分解）dx=H.ldlt().solve(g);     //需要去深入理解更新变量X=X+dx;输出当前cost,增量，变量更新lastcostlastcost=cost;
}

只用eigen库来实现Gauss-Newton优化总结到这，下一章再写下通过Ceres、G2o来实现优化

C++手写高斯牛顿法相关推荐

Ceres 库：基础使用，以手写高斯-牛顿法为例
Ceres 库简介 Ceres库为Google开发的开源C++非线性优化库,被广泛使用于求解最小二乘问题. Ceres库的Github主页如下: 安装首先,下载Cere的源码: git clone ...
slam十四讲-ch6-非线性优化（包含手写高斯牛顿、使用g2o库、使用ceres库三种方法的源码详细注释）
一.自写高斯-牛顿法该程序是要进行一个非线性优化,对非线性函数的系数进行优化 y=exp(ax2+bx+c) 给定初始的系数 ae,be,ce(估计的) ar,br,cr(真实的) 源码如下: // ...
《视觉SLAM十四讲》手写高斯牛顿—笔记记录
<视觉SLAM十四讲>手写高斯牛顿-笔记记录我们的最终目的:使用高斯牛顿法,拟合参数abc 我们的实际小目标:求解增量方程得到ΔX(有了Δx就可以不停的迭代Eabc使得拟合Rabc啦) ...
lio-sam框架：点云匹配之手写高斯牛顿下降优化求状态量更新
lio-sam框架:点云匹配之手写高斯牛顿优化求状态量更新前言代码分前言 LIO-SAM的全称是:Tightly-coupled Lidar Inertial Odometry via Smoo ...
【slam十四讲第二版】【课本例题代码向】【第七讲~视觉里程计Ⅱ】【使用LK光流（cv）】【高斯牛顿法实现单层光流和多层光流】【实现单层直接法和多层直接法】
[slam十四讲第二版][课本例题代码向][第七讲~视觉里程计Ⅱ][使用LK光流(cv)][高斯牛顿法实现单层光流和多层光流][实现单层直接法和多层直接法] 0 前言 1 使用LK光流(cv) 1.1 ...
手写系列之手写LM(Levenberg–Marquardt)算法(基于eigen)
紧接上次的手写高斯牛顿,今天顺便将LM算法也进行了手写实现,并且自己基于eigen的高斯牛顿进行了对比,可以很明显的看到,LM的算法收敛更快,精度也略胜一筹,这次高博的书不够用了,参考网上伪代码进行实 ...
高斯朴素贝叶斯分类的原理解释和手写代码实现
来源:DeepHub IMBA 本文约3500字,建议阅读10+分钟本文与你介绍高斯分布的基本概念及代码实现. Gaussian Naive Bayes (GNB) 是一种基于概率方法和高斯分布的机 ...
Opencv——图像添加椒盐噪声、高斯滤波去除噪声原理及手写Python代码实现
一.噪声我们将常会听到平滑(去噪),锐化(和平滑是相反的),那我们就会有疑惑?什么是噪声呢?图像噪声是指存在于图像数据中不必要的或多余的干扰信息,噪声的存在严重影响了图像的质量.噪声在理论上是&qu ...
曲线拟合（高斯牛顿法，Ceres,g2o)
实践:曲线拟合问题我们先通过高斯牛顿法来求解最小二乘问题,然后介绍使用优化库(Ceres/g2o)来求解此问题. 例题: 考虑一条满足如下方程的曲线: y = exp(ax2x^2x2+ bx + ...

C++手写高斯牛顿法