C++手写高斯牛顿法
因为毕设要用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和上一次迭代的cost
double 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/da
J[1] = -x_data[j] * exp(ae * x_data[j] * x_data[j] + be * x_data[j] + ce); //de/db
J[2] = -exp(ae * x_data[j] * x_data[j] + be * x_data[j] + ce); //de/dc
err = 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.943629
Process 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,增量，变量
更新lastcost
lastcost=cost;
}
只用eigen库来实现Gauss-Newton优化总结到这，下一章再写下通过Ceres、G2o来实现优化