一、自写高斯-牛顿法
该程序是要进行一个非线性优化，对非线性函数的系数进行优化
y=exp(ax2+bx+c)
给定初始的系数 ae,be,ce(估计的) ar,br,cr(真实的)
源码如下：
//
// Created by wenbo on 2020/10/28.
//
//该程序是手写高斯牛顿法，对一个简单的非线性函数进行优化
//y=exp(ax2+bx+c)
#include<iostream>
#include<opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <chrono>
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;//求个倒数，？？
//利用opencv随机数产生器
cv::RNG rng;
vector<double> x_data,y_data;//定义容器分别来接受x ,y的数据
//得到真实值
for(int i=0;i<N;i++)
{
double x=i/100.0;//得到数据点 0，，，，，，，
x_data.push_back(x);//再把数据点（自变量）放回容器x_data
y_data.push_back(exp(ar*x*x+br*x+cr)+rng.gaussian(w_sigma*w_sigma));//w_sigma*w_sigma是噪声方差
//这里把每个自变量所对应的真实值（y_data）算了出来，用作观测值
}
//高斯牛顿迭代
int iters=100;//迭代次数
double cost=0,lastcost=0;//本次迭代和上次迭代，最终的目标函数，要将误差进行平方
chrono::steady_clock::time_point t1=chrono::steady_clock::now();//
//理一下思路
//要算出J 矩阵 利用J 矩阵 算出H矩阵
//之后解线性方程组 Hx=b
//判断最后的cost满足调节没有，满足就不用迭代
//要定义误差函数 yi=y_data[i] error=yi-exp(ae*x*x+be*x+ce)//用真实值减去估计值得到误差函数
//J矩阵是误差函数对各个进行优化的系数的导数所组成的向量
//H=(协方差矩阵转置)*J*J.transopose()//协方差转置来源于随机噪声
//b=-(协方差矩阵转置)*error*J
//cost=error*error
for(int iter=0;iter<iters;iter++)
{
Matrix3d H=Matrix3d::Zero();
Vector3d b=Vector3d::Zero();
cost=0;
for(int i=0;i<N;i++)
{
double xi=x_data[i], yi=y_data[i];//拿到对应的数据点
double error =yi-exp(ae*xi*xi+be*xi+ce);//得到误差含糊是
Eigen::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;
}
//求解线性方程组 Hx=b
Vector3d dx=H.ldlt().solve(b);//利用ldlt分解
//也可用QR分解
//Vector3d dx=H.colPivHouseholderQr().solve(b);//利用Qr分解
//isnan() 判断是不是NAN值（not a number非法数字）
// 标准库中定义了一个宏：NAN来表示非法数字。
// 比如负数开方、负数求对数、0.0/0.0、0.0* INFINITY(无穷大)、INFINITY/INFINITY、INFINITY-INFINITY
// 以上操作都会得到NAN。
// 注意：如果是整数0/0会产生操作异常
//isinf()测试某个浮点数是否是无限大，其中INF表示无穷大
if(isnan(dx[0]))
{
cout<<"result is nan"<<endl;
break;
}
//如果优化结束，就应该退出迭代
if(iter>0&&cost>=lastcost)//判断迭代结束
{
cout<<"cost="<<cost<<">="<<"lastcost="<<lastcost<<",break!"<<endl;
break;
}
//否则继续优化系数
ae+=dx[0];
be+=dx[1];
ce+=dx[2];
lastcost=cost;
cout<<"cost:"<<cost<<",\t\t update: "<<dx.transpose()<<
"\t\t estimated params: "<<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<<"cost_time="<<time_used.count()<<endl;
cout<<"estimated abc ="<<ae<<","<<be<<","<<ce<<endl;
return 0;
}
运行结果如下：
cost:3.19575e+06, update: 0.0455771 0.078164 -0.985329 estimated params: 2.04558,-0.921836,4.01467
cost:376785, update: 0.065762 0.224972 -0.962521 estimated params: 2.11134,-0.696864,3.05215
cost:35673.6, update: -0.0670241 0.617616 -0.907497 estimated params: 2.04432,-0.0792484,2.14465
cost:2195.01, update: -0.522767 1.19192 -0.756452 estimated params: 1.52155,1.11267,1.3882
cost:174.853, update: -0.537502 0.909933 -0.386395 estimated params: 0.984045,2.0226,1.00181
cost:102.78, update: -0.0919666 0.147331 -0.0573675 estimated params: 0.892079,2.16994,0.944438
cost:101.937, update: -0.00117081 0.00196749 -0.00081055 estimated params: 0.890908,2.1719,0.943628
cost:101.937, update: 3.4312e-06 -4.28555e-06 1.08348e-06 estimated params: 0.890912,2.1719,0.943629
cost:101.937, update: -2.01204e-08 2.68928e-08 -7.86602e-09 estimated params: 0.890912,2.1719,0.943629
cost=101.937>=lastcost=101.937,break!
cost_time=0.000228831
estimated abc =0.890912,2.1719,0.943629
2、利用g2o进行非线性优化
该程序是要进行一个非线性优化，对非线性函数的系数进行优化
y=exp(ax2+bx+c)
给定初始的系数 ae,be,ce(估计的) ar,br,cr(真实的)
源码如下：
//
//
//g2o
#include <iostream>
#include <g2o/core/g2o_core_api.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>
using namespace std;
class CurveFittingVertex: public g2o::BaseVertex<3,Eigen::Vector3d>//定义顶点类。继承g2o的父类（基础顶点类）
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW//这个必须写，我也不知道为什么
//重置，估计系数（待优化的系数）重写虚函数
// override保留字表示当前函数重写了基类的虚函数
virtual void setToOriginImpl() override
{
_estimate<<0,0,0;//设置估计的系数
}
//更新估计系数（待优化的系数）重写虚函数
virtual void oplusImpl(const double *update) override
{
_estimate += Eigen::Vector3d(update);
}
// 存盘和读盘：留空
virtual bool read(istream &in) {
}
//
virtual bool write(ostream &out) const {
}
};
//误差模板（边） 模板参数：观测值（真实值 y）维度，类型。链接顶点类型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW//这个必须写，我也不知道为什么
//有参构造
CurveFittingEdge(double x): BaseUnaryEdge(), _x(x) {
}//这里的构造函数里面还调用了BaseUnaryEdge的构造函数
//计算曲线模型误差 重写虚函数
virtual void computeError() override
{
const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
const Eigen::Vector3d abc = v->estimate();//abc待优化的系数
//误差函数 =y-exp(ax2+bx+c) 这里面的abc都是待优化的系数
_error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0));
}
// 计算雅可比矩阵 重写虚函数
virtual void linearizeOplus() override
{
//定义顶点
const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
//拿出顶点模板里面的待优化系数
const Eigen::Vector3d abc = v->estimate();
//计算误差
_error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0));
}
// 存盘和读盘：留空
virtual bool read(istream &in) {
}
virtual bool write(ostream &out) const {
}
//属性
public:
double _x; // x 值， y 值为 _measurement
};
//至此顶点和边的模板就写完了
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随机数产生器
vector<double> x_data, y_data; // 数据
//真实值，
// 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));
}
// 构建图优化，先设定g2o
//定义维度类型
typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType; // 每个误差项优化变量维度为3，误差值维度为1
//线性求解器类型
typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;
// 梯度下降方法，可以从GN, LM, DogLeg 中选
//把维度信息，线性求解器类型，算法放进去求解器
auto solver = new g2o::OptimizationAlgorithmGaussNewton(
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm(solver); // 设置求解器
optimizer.setVerbose(true); // 打开调试输出
//上面把图优化构建好了
//传入数据
// 往图中增加顶点
CurveFittingVertex *v = new CurveFittingVertex();//创建顶点
v->setEstimate(Eigen::Vector3d(ae, be, ce));//设置估计值
v->setId(0);//编号
optimizer.addVertex(v);//加入顶点
// 往图中增加边
for (int i = 0; i < N; i++) {
CurveFittingEdge *edge = new CurveFittingEdge(x_data[i]);// 创建边
edge->setId(i); //编号
edge->setVertex(0, v); // 设置连接的顶点。顶点和对应编号的边连接
edge->setMeasurement(y_data[i]); // 观测数值 （真实值）
edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 1 / (w_sigma * w_sigma)); // 信息矩阵： 1 / (w_sigma * w_sigma)协方差矩阵之逆
optimizer.addEdge(edge); //加入边
}
// 执行优化
cout << "start optimization" << endl;
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
optimizer.initializeOptimization(); //开始优化
optimizer.optimize(10); //迭代次数10
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;
// 输出优化值
Eigen::Vector3d abc_estimate = v->estimate();
cout << "estimated model: " << abc_estimate.transpose() << endl;
return 0;
}
运行结果如下：
start optimization
solve time cost = 0.00582179 seconds.
estimated model: 2 -1 5
iteration= 0 chi2= 3195746.523015 time= 2.9264e-05 cumTime= 2.9264e-05 edges= 100 schur= 0
iteration= 1 chi2= 3195746.523015 time= 2.7616e-05 cumTime= 5.688e-05 edges= 100 schur= 0
iteration= 2 chi2= 3195746.523015 time= 2.8487e-05 cumTime= 8.5367e-05 edges= 100 schur= 0
iteration= 3 chi2= 3195746.523015 time= 5.7382e-05 cumTime= 0.000142749 edges= 100 schur= 0
iteration= 4 chi2= 3195746.523015 time= 3.061e-05 cumTime= 0.000173359 edges= 100 schur= 0
iteration= 5 chi2= 3195746.523015 time= 2.9421e-05 cumTime= 0.00020278 edges= 100 schur= 0
iteration= 6 chi2= 3195746.523015 time= 2.0262e-05 cumTime= 0.000223042 edges= 100 schur= 0
iteration= 7 chi2= 3195746.523015 time= 2.1139e-05 cumTime= 0.000244181 edges= 100 schur= 0
iteration= 8 chi2= 3195746.523015 time= 2.026e-05 cumTime= 0.000264441 edges= 100 schur= 0
iteration= 9 chi2= 3195746.523015 time= 2.5783e-05 cumTime= 0.000290224 edges= 100 schur= 0
3、利用ceres进行非线性优化
源码如下：
//
//
#include<iostream>
#include<opencv2/core/core.hpp>
#include <ceres/ceres.h>
#include <chrono>
using namespace std;
//利用ceres进行非线性优化（曲线拟合）
//步骤：
//1、定义内个参数块，位每个参数分配double数组来存储变量的值
//2、定义残差块的计算方式
//3、残差块往往也需要定义雅克比的计算方式
//4、把左右的参数块和残差块加入Ceres定义的Problem的对象中，调用solve函数求解即可
//代价函数的计算模型，这里面计算的是误差（残差）函数 Ceres应该会对这个误差函数进行平方，得到目标函数
struct CURVE_FITTING_COST
{
CURVE_FITTING_COST(double x,double y):_x(x),_y(y) {
}//初始化数据 这里面的_x _y 是真实的数据，_y也就是观测值
//残差计算 y-exp(ax2+bx+c) ,其中这个式子里面的a b c值咱们的估计值，也就是希望优化的系数
//重载()运算符，()运算符能够利用其传入的参数abc，residual，算出残差，返回true
//我个人理解是，在构建Problem的时候，这个重载被集成在Problem里了
template<typename T>
bool operator ()(const T *const abc, T *residual)
const{
// y-exp(ax^2+bx+c),其中这个式子里面的a b c值咱们的估计值，也就是希望优化的系数
residual[0] = T(_y) - ceres::exp(abc[0] * T(_x) * T(_x) + abc[1] * T(_x) + abc[2]);
return true;
}
const double _x,_y;
};
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;//估计参数值，对这组数进行优化
double abc[3]={
ae,be,ce};
int N=100;//数据的个数
double w_sigma=1.0;//噪声sigma的值
double inv_sigma=1.0/w_sigma; //求个倒数，？？
//利用opencv随机数产生器
cv::RNG rng;
vector<double> x_data,y_data;//定义容器分别来接受x ,y的数据
//得到真实值
for(int i=0;i<N;i++)
{
double x=i/100.0;//得到数据点 0，，，，，，，
x_data.push_back(x);//再把数据点（自变量）放回容器x_data
y_data.push_back(exp(ar*x*x+br*x+cr)+rng.gaussian(w_sigma*w_sigma));//w_sigma*w_sigma是噪声方差
//这里把每个自变量所对应的真实值（y_data）算了出来，用作观测值
}
//构建最小二乘问题，也就是非线性优化问题
ceres::Problem problem;
//把每个数据点都放进problem中
for(int i=0;i<N;i++)
{
problem.AddResidualBlock(
// 使用自动求导，模板参数：误差类型，输出维度，输入维度，维数要与前面struct中一致
new ceres::AutoDiffCostFunction<CURVE_FITTING_COST,1,3>(
//传入真实数据
new CURVE_FITTING_COST(x_data[i],y_data[i])
),
nullptr,//核函数，这里不使用，空就完事了
abc//待优化的系数，也就是咱们的估计值
);
}
//配置求解器
ceres::Solver::Options options;//options用来配置
options.linear_solver_type=ceres::DENSE_NORMAL_CHOLESKY;//利用DENSE_NORMAL_CHOLESKY求解增量方差 Hx=b
options.minimizer_progress_to_stdout=true;//允许 输出到cout
ceres::Solver::Summary summary;//优化信息
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
ceres::Solve(options, &problem, &summary); // 开始优化 将配置信息和要求解的问题放进去，把优化的信息放在summary里
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<<summary.BriefReport()<<endl;
cout<<"estimated a,b,c ";
for(auto a:abc) cout<<a<<" ";//遍历abc
cout<<endl;
return 0;
}
运行结果如下：
iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
0 1.597873e+06 0.00e+00 3.52e+06 0.00e+00 0.00e+00 1.00e+04 0 6.29e-05 1.43e-04
1 1.884440e+05 1.41e+06 4.86e+05 9.88e-01 8.82e-01 1.81e+04 1 1.48e-04 1.81e-03
2 1.784821e+04 1.71e+05 6.78e+04 9.89e-01 9.06e-01 3.87e+04 1 6.89e-05 2.02e-03
3 1.099631e+03 1.67e+04 8.58e+03 1.10e+00 9.41e-01 1.16e+05 1 8.80e-05 2.25e-03
4 8.784938e+01 1.01e+03 6.53e+02 1.51e+00 9.67e-01 3.48e+05 1 4.10e-05 2.42e-03
5 5.141230e+01 3.64e+01 2.72e+01 1.13e+00 9.90e-01 1.05e+06 1 7.10e-05 2.61e-03
6 5.096862e+01 4.44e-01 4.27e-01 1.89e-01 9.98e-01 3.14e+06 1 6.60e-05 2.82e-03
7 5.096851e+01 1.10e-04 9.53e-04 2.84e-03 9.99e-01 9.41e+06 1 6.51e-05 3.03e-03
solve time cost = 0.00320557 seconds.
Ceres Solver Report: Iterations: 8, Initial cost: 1.597873e+06, Final cost: 5.096851e+01, Termination: CONVERGENCE
estimated a,b,c 0.890908 2.1719 0.943628