非线性优化
1. 状态估计问题
1.1 最大后验与最大似然
1.2 最小二乘的引出
2. 非线性最小二乘
对于不方便直接求解的最小二乘问题,我们可以用迭代的方式,从一个初始值出发,不断地更新当前的优化变量,使目标函数下降。具体步骤可列写如下:
2.1 一阶和二阶梯度法
二阶的就是牛顿法
一阶和二阶梯度法都十分直观,只要把函数在迭代点附近进行泰勒展开,并针对更新量作最小化即可。由于泰勒展开之后函数变成了多项式,所以求解增量时只需解线性方程即可,避免了直接求导函数为零这样的非线性方程的困难。不过,这两种方法也存在它们自身的问题。最速下降法过于贪心,容易走出锯齿路线,反而增加了迭代次数。而牛顿法则需要计算目标函数的
H
\bm{H}
H 矩阵,这在问题规模较大时非常困难,我们通常倾向于避免
H
\bm{H}
H 的计算。
2.2 Gauss-Newton
求解增量方程是整个优化问题的核心所在。如果我们能够顺利解出该方程,那么Gauss-Newton 的算法步骤可以写成:
原则上,它要求我们所用
的近似H 矩阵是可逆的(而且是正定的),但实际数据中计算得到的
J
T
J
\bm{J^TJ}
JTJ 却只有半正定性。
2.3 Levenberg-Marquadt
该给
Δ
x
Δx
Δx 添加一个信赖区域(Trust Region),不能让它太大而使得近似不准确,这类方法也被称之为信赖区域方法(Trust Region Method)。
们构建一个改良版的非线性优化框架,该框架会比Gauss Newton 有更好的效果:
3 实践Ceres
Ceres库安装参考链接:
Ubuntu20 安装Ceres库
拟合曲线
相应代码,相比如高翔github上的有改动
main.cpp
#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>
#include <chrono>
using namespace std;
// 代价函数的计算模型
struct CURVE_FITTING_COST
{
CURVE_FITTING_COST ( double x, double y ) : _x ( x ), _y ( y ) {}
// 残差的计算
template <typename T>
bool operator() (
const T* const abc, // 模型参数,有3维
T* residual ) const // 残差
{
residual[0] = T ( _y ) - ceres::exp ( abc[0]*T ( _x ) *T ( _x ) + abc[1]*T ( _x ) + abc[2] ); // y-exp(ax^2+bx+c)
return true;
}
const double _x, _y; // x,y数据
};
int main ( int argc, char** argv )
{
double a=1.0, b=2.0, c=1.0; // 真实参数值
int N=100; // 数据点
double w_sigma=1.0; // 噪声Sigma值
cv::RNG rng; // OpenCV随机数产生器
double abc[3] = {0,0,0}; // abc参数的估计值
vector<double> x_data, y_data; // 数据
cout<<"generating data: "<<endl;
for ( int i=0; i<N; i++ )
{
double x = i/100.0;
x_data.push_back ( x );
y_data.push_back (
exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
);
cout<<x_data[i]<<" "<<y_data[i]<<endl;
}
// 构建最小二乘问题
ceres::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.linear_solver_type = ceres::DENSE_QR; // 增量方程如何求解
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 ); // 开始优化
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<<" ";
cout<<endl;
return 0;
}
CMakeLists.txt
cmake_minimum_required( VERSION 2.8 )
project( ceres_curve_fitting )
set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_STANDARD 14 )
# 添加cmake模块以使用ceres库
list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )
# 寻找Ceres库并添加它的头文件
find_package( Ceres REQUIRED )
include_directories( ${CERES_INCLUDE_DIRS} )
# OpenCV
find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_DIRS} )
add_executable( curve_fitting main.cpp )
# 与Ceres和OpenCV链接
target_link_libraries( curve_fitting ${CERES_LIBRARIES} ${OpenCV_LIBS} )
输出结果
generating data:
0 2.71828
0.01 2.93161
0.02 2.12942
......
iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
0 1.824887e+04 0.00e+00 1.38e+03 0.00e+00 0.00e+00 1.00e+04 0 9.05e-05 1.86e-04
1 2.748700e+39 -2.75e+39 0.00e+00 7.67e+01 -1.52e+35 5.00e+03 1 1.08e-04 3.62e-04
2 2.429783e+39 -2.43e+39 0.00e+00 7.62e+01 -1.35e+35 1.25e+03 1 7.42e-05 4.74e-04
3 1.213227e+39 -1.21e+39 0.00e+00 7.30e+01 -6.73e+34 1.56e+02 1 4.55e-05 5.52e-04
4 1.852387e+37 -1.85e+37 0.00e+00 5.56e+01 -1.03e+33 9.77e+00 1 6.32e-05 6.39e-04
5 6.714689e+31 -6.71e+31 0.00e+00 2.96e+01 -3.85e+27 3.05e-01 1 6.28e-05 7.33e-04
6 9.500531e+12 -9.50e+12 0.00e+00 9.50e+00 -8.39e+08 4.77e-03 1 4.89e-05 8.39e-04
7 1.776982e+04 4.79e+02 1.83e+03 2.58e-01 1.18e+00 1.43e-02 1 8.89e-05 9.51e-04
8 1.599969e+04 1.77e+03 3.45e+03 5.53e-01 1.46e+00 4.29e-02 1 8.45e-05 1.06e-03
9 1.060557e+04 5.39e+03 7.62e+03 7.33e-01 1.68e+00 1.29e-01 1 1.40e-04 1.22e-03
10 3.669783e+03 6.94e+03 9.60e+03 5.25e-01 1.39e+00 3.86e-01 1 1.31e-04 1.38e-03
11 5.397541e+02 3.13e+03 5.00e+03 2.66e-01 1.12e+00 1.16e+00 1 8.45e-05 1.49e-03
12 1.484444e+02 3.91e+02 1.22e+03 8.46e-02 1.02e+00 3.48e+00 1 8.40e-05 1.60e-03
13 1.216815e+02 2.68e+01 3.76e+02 4.17e-02 1.01e+00 1.04e+01 1 1.30e-04 1.75e-03
14 9.290109e+01 2.88e+01 2.42e+02 9.10e-02 1.01e+00 3.13e+01 1 8.40e-05 1.86e-03
15 6.674330e+01 2.62e+01 1.09e+02 1.33e-01 1.00e+00 9.39e+01 1 8.36e-05 1.97e-03
16 5.936574e+01 7.38e+00 2.14e+01 1.08e-01 9.94e-01 2.82e+02 1 8.39e-05 2.08e-03
17 5.653118e+01 2.83e+00 1.36e+01 1.57e-01 9.98e-01 8.45e+02 1 8.41e-05 2.18e-03
18 5.310764e+01 3.42e+00 8.50e+00 2.81e-01 9.89e-01 2.53e+03 1 1.30e-04 2.34e-03
19 5.125939e+01 1.85e+00 2.84e+00 2.98e-01 9.90e-01 7.60e+03 1 1.30e-04 2.49e-03
20 5.097693e+01 2.82e-01 4.34e-01 1.48e-01 9.95e-01 2.28e+04 1 8.42e-05 2.60e-03
21 5.096854e+01 8.39e-03 3.24e-02 2.87e-02 9.96e-01 6.84e+04 1 8.36e-05 2.71e-03
solve time cost = 0.00283303 seconds.
Ceres Solver Report: Iterations: 22, Initial cost: 1.824887e+04, Final cost: 5.096854e+01, Termination: CONVERGENCE
estimated a,b,c = 0.891943 2.17039 0.944142
4 实践:g2o
4.1 图优化理论简介
图优化,是把优化问题表现成图(Graph)的一种方式。这里的图是图论意义上的图。一个图由若干个顶点(Vertex),以及连接着这些节点的边(Edge)组成。进而,用顶点表示优化变量,用边表示误差项。
我们用三角形表示相机位姿节点,用圆形表示路标点,它们构成了图优化的顶点;同时,蓝色线表示相机的运动模型,红色虚线表示观测模型,它们构成了图优化的边。此时,虽然整个问题的数学形式仍是式(6.12)那样,但现在我们可以直观地看到问题的结构了。如果我们希望,也可以做去掉孤立顶点或**优先优化边数较多(或按图论的术语,度数较大)**的顶点这样的改进。但是最基本的图优化,是用图模型来表达一个非线性最小二乘的优化问题。而我们可以利用图模型的某些性质,做更好的优化。
4.2 使用 g2o 拟合曲线
为了使用 g2o,首先要做的是将曲线拟合问题抽象成图优化。这个过程中,只要记住节点为优化变量,边为误差项即可。因此,曲线拟合的图优化问题可以画成图 6-3 的形式。
为 g2o 的用户,我们要做的事主要有以下几个步骤:
- 定义顶点和边的类型;
- 构建图;
- 选择优化算法;
- 调用 g2o 进行优化,返回结果。
g2o库安装参考链接:
Ubuntu20 安装g2o库
相应代码
main.cpp
#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> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// 重置
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 {}
};
// 误差模型 模板参数:观测值维度,类型,连接顶点类型
class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}
// 计算曲线模型误差
virtual void computeError() 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 void linearizeOplus() override {
const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
const Eigen::Vector3d abc = v->estimate();
double y = exp(abc[0] * _x * _x + abc[1] * _x + abc[2]);
_jacobianOplusXi[0] = -_x * _x * y;
_jacobianOplusXi[1] = -_x * y;
_jacobianOplusXi[2] = -y;
}
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; // 数据
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)); // 信息矩阵:协方差矩阵之逆
optimizer.addEdge(edge);
}
// 执行优化
cout << "start optimization" << endl;
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
optimizer.initializeOptimization();
optimizer.optimize(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;
}
CmakeLists.txt
cmake_minimum_required( VERSION 2.8 )
project( g2o_curve_fitting )
set( CMAKE_BUILD_TYPE "Release" )
# 添加cmake模块以使用ceres库
list( APPEND CMAKE_MODULE_PATH /home/ql/slamebook/lib/g2o/cmake_modules)
# set(G2O_ROOT /usr/local/include/g2o)
# 寻找G2O
find_package( G2O REQUIRED )
include_directories(
${G2O_INCLUDE_DIRS}
"/usr/include/eigen3"
)
# OpenCV
find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_DIRS} )
add_executable( curve_fitting main.cpp )
# 与G2O和OpenCV链接
target_link_libraries( curve_fitting
${OpenCV_LIBS}
g2o_core g2o_stuff
)
输出成功效果