Eigen库：手写高斯-牛顿法
问题描述
对一条曲线 y = e x p ( a x 2 + b x + c ) + w y=exp(ax^2+bx+c)+w y=exp(ax2+bx+c)+w，其中 a 、 b 、 c a、b、c a、b、c为曲线参数，噪声项 w ∼ N ( 0 , σ 2 ) w\sim \mathcal{N}(0,\sigma^2) w∼N(0,σ2)。
假设现有 N N N个关于 x ， y x，y x，y的观测数据点，希望能够求取曲线参数 a 、 b 、 c a、b、c a、b、c，则可构建非线性最小二乘问题用于估计参数：
min a , b , c 1 2 ∑ i = 1 N ∥ y i − e x p ( a x i 2 + b x i + c ) ∥ 2 \min_{a,b,c}\frac{1}{2}\sum_{i=1}^N\begin{Vmatrix}y_i-exp(ax_i^2+bx_i+c)\end{Vmatrix}^2 a,b,cmin21i=1∑N∥∥yi−exp(axi2+bxi+c)∥∥2
定义误差为： e i = y i − e x p ( a x i 2 + b x i + c ) e_i=y_i-exp(ax_i^2+bx_i+c) ei=yi−exp(axi2+bxi+c)，则可求的雅克比矩阵：
J i = [ ∂ e i ∂ a ∂ e i ∂ b ∂ e i ∂ c ] J_i=\begin{bmatrix}\frac{\partial e_i}{\partial a}\\\frac{\partial e_i}{\partial b}\\\frac{\partial e_i}{\partial c}\end{bmatrix} Ji=⎣⎡∂a∂ei∂b∂ei∂c∂ei⎦⎤
其中，偏导数表达式如下：
∂ e i ∂ a = − x i 2 e x p ( a x i 2 + b x i + c ) ∂ e i ∂ b = − x i e x p ( a x i 2 + b x i + c ) ∂ e i ∂ c = − e x p ( a x i 2 + b x i + c ) \begin{aligned} \frac{\partial e_i}{\partial a}&=-x_i^2\:exp(ax_i^2+bx_i+c)\\ \frac{\partial e_i}{\partial b}&=-x_i\:exp(ax_i^2+bx_i+c)\\ \frac{\partial e_i}{\partial c}&=-\:exp(ax_i^2+bx_i+c)\\ \end{aligned} ∂a∂ei∂b∂ei∂c∂ei=−xi2exp(axi2+bxi+c)=−xiexp(axi2+bxi+c)=−exp(axi2+bxi+c)
则可构建高斯-牛顿法中增量方程如下：
( ∑ i = 1 100 J i ( σ 2 ) − 1 J i T ) Δ x k = ∑ i = 1 100 − J i ( σ 2 ) − 1 e i \Bigl(\sum_{i=1}^{100}J_i(\sigma^2)^{-1}\:J_i^T\Bigr)\:\Delta x_k=\sum_{i=1}^{100}-J_i(\sigma^2)^{-1}\:e_i (i=1∑100Ji(σ2)−1JiT)Δxk=i=1∑100−Ji(σ2)−1ei
工程构建
采用Ubuntu20.04+CLion进行开发，主要使用Eigen库进行矩阵计算。
工程结构
├── CMakeLists.txt
├── include
│ └── main.h
└── src
├── CMakeLists.txt
└── main.cpp
CMake配置
主要使用了OpenCV库生成数据点，Eigen库用于矩阵计算。根目录下CMakeLists.txt文件内容如下：
# cmake version
cmake_minimum_required(VERSION 3.21)
# project name
project(Study)
# cpp version
set(CMAKE_CXX_STANDARD 14)
# eigen
include_directories("/usr/include/eigen3")
# opencv
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
# incldue
include_directories(include)
# src
add_subdirectory(src)
src下CMakeLists.txt文件内容如下：
# exec
add_executable(Study main.cpp)
# link opencv
target_link_libraries(Study ${OpenCV_LIBRARIES})
头文件配置
include目录下头文件main.h内容如下：
//
// Created by jasonli on 2022/2/12.
//
#ifndef STUDY_MAIN_H
#define STUDY_MAIN_H
#include 源文件配置
src下源文件main.cpp导入头文件：
#include "main.h"
int main()
{
return 0;
}
代码
整体代码以及注释如下：
#include "main.h"
int main()
{
/*-------- 初始参数配置 --------*/
// 实际曲线参数
double ar = 1.0, br = 1.0, cr = 1.0;
// 估计曲线参数初始值
double ae = 2.0, be = 1.0, ce = 5.0;
// 采样观测数据点个数
int N = 100;
// 噪声标准差及其倒数
double w_sigma = 1.0;
double inv_sigma = 1.0 / w_sigma;
// 随机数生成器
cv::RNG rng;
/*-------- 观测数据生成 --------*/
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 --------*/
// 迭代次数
int iterations = 100;
// 迭代cost以及上一次迭代cost
double cost, lastCost = 0;
// 开始求解时间t1
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
for(int iter=0; iter < iterations; iter++){
// 增量方程左侧值初始化：Hessian = J^T W^{-1} J in Gauss-Newton
Matrix3d H = Matrix3d::Zero();
// 增量方程右侧值初始化：g = - W^{-1} ei J
Vector3d g = Vector3d::Zero();
// 估计同真实的差距值，归零化
cost = 0;
for(int i=0; i < N; i++){
// 获取第i个数据点
double xi = x_data[i], yi = y_data[i];
// 误差
double ei = yi - exp(ae * xi * xi + be * xi + ce);
// 雅克比矩阵
Vector3d J;
J[0] = - xi * xi * exp(ae * xi * xi + be * xi + ce);
J[1] = - xi * exp(ae * xi * xi + be * xi + ce);
J[2] = - exp(ae * xi * xi + be * xi + ce);
// 增量方程系数
H += inv_sigma * inv_sigma * J * J.transpose();
g += -inv_sigma * inv_sigma * ei * J;
// 差距
cost += ei * ei;
}
// 求解增量方程Hdx=g
Vector3d dx = H.ldlt().solve(g);
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\t update:" << dx.transpose() << "\t\t estimated params:" << ae << "," << be << "," << ce << endl;
}
// 结束求解时间t2
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
// 求解花费时间t2 - t1
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;
}
输出
输出如下：
total cost:9.26353e+07, update:-0.00563613 -0.006202 -0.981441 estimated params:1.99436,0.993798,4.01856
total cost:1.23782e+07, update:-0.0154356 -0.0158777 -0.950534 estimated params:1.97893,0.97792,3.06802
total cost:1.61827e+06, update:-0.0425927 -0.0364985 -0.87242 estimated params:1.93634,0.941422,2.19561
total cost:199423, update: -0.11697 -0.0600174 -0.697204 estimated params:1.81937,0.881404,1.4984
total cost:20992.7, update: -0.287499 -0.00580094 -0.403671 estimated params:1.53187,0.875603,1.09473
total cost:1519.46, update:-0.437282 0.186207 -0.135058 estimated params:1.09458,1.06181,0.959671
total cost:132.709, update: -0.205976 0.142068 -0.0272031 estimated params:0.888608,1.20388,0.932468
total cost:102.041, update: -0.0110366 0.00856986 -0.00121983 estimated params:0.877572,1.21245,0.931248
total cost:102.004, update: 7.74339e-05 -9.85241e-05 2.39198e-05 estimated params:0.877649,1.21235,0.931272
total cost:102.004, update:-3.05581e-07 3.32264e-07 -5.91727e-08 estimated params:0.877649,1.21235,0.931272
total cost:102.004, update: 1.77986e-09 -2.04038e-09 4.27718e-10 estimated params:0.877649,1.21235,0.931272
cost:102.004>= last cost :102.004, break.
solve time cost = 0.00442306seconds.
estimated abc = 0.877649 , 1.21235 , 0.931272