非线性优化 (曲线拟合) 问题：高斯牛顿、g2o 方法总结 |
您所在的位置：网站首页 › 函数拟合方法有哪些 › 非线性优化 (曲线拟合) 问题：高斯牛顿、g2o 方法总结 |
非线性优化 (曲线拟合) 问题：高斯牛顿、g2o 方法总结
|
其实还有一个Ceres库可以进行优化，但是之前的博客已经具体分析了，所以这里就对其余两个进行了介绍，相关的内容是SLAM14讲里面的知识
一、理论部分
我们先用一个简单的例子来说明如何求解最小二乘问题，然后展示如何手写高斯牛顿法和优化库求解此问题 高斯牛顿法
在曲线拟合问题中，整个问题只有一个顶点：曲线模型的参数
a, b, c
；而每个带噪声的数据点，构成了一个个误差项，也就是图优化的边。但这里的边与我们平时想的边不太一样，它们是一元边
（
Unary Edge
），即
只连接一个顶点
——因为我们整个图只有一个顶点。 所以在图 6-3
中，我们就只能把它画成自己连到自己的样子了。事实上，图优化中一条边可以连接一个、两个或多个顶点，这主要反映在每个误差与多少个优化变量有关。在稍微有些玄妙的说法中，我们把它叫做超边
（
Hyper Edge
），整个图叫做
超图
（
Hyper Graph
） ①。弄清了这个图模型之后，接下来就是在 g2o
中建立该模型，进行优化了。作为
g2o
的 用户，我们要做的事主要有以下几个步骤：
1. 定义顶点和边的类型；
2. 构建图；
3. 选择优化算法；
4. 调用 g2o 进行优化，返回结果。
二、实践部分
2.1 手写高斯牛顿法
#include
#include
#include
#include
#include
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随机数产生器
vector 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;
}
// 求解线性方程 Hx=b
Vector3d dx = H.ldlt().solve(b);
if (isnan(dx[0])) {
cout = lastCost) {
cout |
【本文地址】