第六讲 非线性优化"/>
SLAM高翔十四讲（六）第六讲 非线性优化
文章目录
- 一、非线性优化
- 1.1 状态估计问题
- 1.2 最小二乘
- 1.3 非线性最小二乘
- 1.3.1 最速下降法
- 1.3.2 牛顿法
- 1.3.3 高斯牛顿法G-N
- 1.3.4 列文伯格-马夸尔特法L-M
- 二、手写高斯牛顿法
- 2.1 CMakLists.txt
- 2.2 代码实现
- 2.3 结果展示
- 三、Ceres曲线拟合（Ceres最小二乘问题求解库）
- 3.1 CMakLists.txt
- 3.2 代码实现
- 3.3 结果展示
- 四、g2o曲线拟合（g2o图优化库）
- 4.1 CMakLists.txt
- 4.2 代码实现
- 4.3 结果展示
一、非线性优化
目的：在有噪声的数据中进行准确的状态估计，即在带有噪声的数据推断位姿和地图以及它们的概率分布
1.1 状态估计问题
- 运动方程与观测方程：第三四讲、第五讲
- 状态估计问题：在带有噪声的数据推断位姿和地图以及它们的概率分布
- 高斯分布、协方差矩阵
- 处理状态估计的方法？滤波器/增量/渐近法、批量法
- 最大后验估计MAP
- 最大似然估计MLE
状态估计条件分布->最大后验估计->最大似然估计：在什么样的条件下最有可能产生现在观测到的数据。
1.2 最小二乘
- 最大似然估计问题取负对数转换成最小二乘问题
- 马氏距离、信息矩阵
- 误差
1.3 非线性最小二乘
- 问题引出：求解导数为0问题转换成寻求下降增量问题
- 求解方法？（迭代法）
1.3.1 最速下降法
1.3.2 牛顿法
1.3.3 高斯牛顿法G-N
1.3.4 列文伯格-马夸尔特法L-M
具体内容可参考这两篇博客：文章1 ，文章2
二、手写高斯牛顿法
曲线拟合问题：给出曲线y=exp(axx+b*x+c)+w和一组样本点，根据样本点(x,y)求出参数a,b,c。
这里给出3种方法实现：1）手写高斯牛顿 2）Ceres实现高斯牛顿 3）g2o实现高斯牛顿
2.1 CMakLists.txt
cmake_minimum_required(VERSION 3.0)
project(ch6)
set(CMAKE_BUILD_TYPE Release)
set(CMAKE_CXX_STANDARD 14)
list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)# OpenCV
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})# Ceres
find_package(Ceres REQUIRED)
include_directories(${CERES_INCLUDE_DIRS})# g2o
find_package(G2O REQUIRED)
include_directories(${G2O_INCLUDE_DIRS})# Eigen
include_directories("/usr/include/eigen3")add_executable(gaussNewton gaussNewton.cpp)
target_link_libraries(gaussNewton ${OpenCV_LIBS})add_executable(ceresCurveFitting ceresCurveFitting.cpp)
target_link_libraries(ceresCurveFitting ${OpenCV_LIBS} ${CERES_LIBRARIES})add_executable(g2oCurveFitting g2oCurveFitting.cpp)
target_link_libraries(g2oCurveFitting ${OpenCV_LIBS} ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY})
2.2 代码实现
#include <iostream>
#include <chrono>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>using namespace std;
using namespace Eigen;// 曲线y=exp(a*x*x+b*x+c)+w
// 根据(x,y)样本点求出参数a,b,c
// 利用高斯-牛顿法求解int main(int argc, char **argv) {// 1. 设置参数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随机数产生器// 2. 将样本点(x,y)存入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));}// 3. 开始Gauss-Newton迭代int iterations = 100; // 迭代次数100double cost = 0, lastCost = 0; // 本次迭代的cost和上一次迭代的costchrono::steady_clock::time_point t1 = chrono::steady_clock::now();//计时for (int iter = 0; iter < iterations; iter++) {// 4. 构造H * x = bMatrix3d H = Matrix3d::Zero(); // 海塞矩阵Hessian = J^T W^{-1} J in Gauss-NewtonVector3d b = Vector3d::Zero(); // biascost = 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); 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();b += -inv_sigma * inv_sigma * error * J;cost += error * error;}// 5. 求解线性方程 Hx=bVector3d dx = H.ldlt().solve(b);if (isnan(dx[0])) {cout << "result is nan!" << endl;break;}//如果本次误差大于上次误差就退出if (iter > 0 && cost >= lastCost) {cout << "cost: " << cost << ">= last cost: " << lastCost << ", break." << endl;break;}//更新估计参数值:a-dx,b-dy,c-dzae += 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;}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;
}2.3 结果展示
(本次误差)total cost: 3.19575e+06, (更新)update: 0.0455771 0.078164 -0.985329 (更新后的估计参数)estimated params: 2.04558,-0.921836,4.01467
(本次误差)total cost: 376785, (更新)update: 0.065762 0.224972 -0.962521 (更新后的估计参数)estimated params: 2.11134,-0.696864,3.05215
(本次误差)total cost: 35673.6, (更新)update: -0.0670241 0.617616 -0.907497 (更新后的估计参数)estimated params: 2.04432,-0.0792484,2.14465
(本次误差)total cost: 2195.01, (更新)update: -0.522767 1.19192 -0.756452 (更新后的估计参数)estimated params: 1.52155,1.11267,1.3882
(本次误差)total cost: 174.853, (更新)update: -0.537502 0.909933 -0.386395 (更新后的估计参数)estimated params: 0.984045,2.0226,1.00181
(本次误差)total cost: 102.78, (更新)update: -0.0919666 0.147331 -0.0573675 (更新后的估计参数)estimated params: 0.892079,2.16994,0.944438
(本次误差)total cost: 101.937, (更新)update: -0.00117081 0.00196749 -0.00081055 (更新后的估计参数)estimated params: 0.890908,2.1719,0.943628
(本次误差)total cost: 101.937, (更新)update: 3.4312e-06 -4.28555e-06 1.08348e-06 (更新后的估计参数)estimated params: 0.890912,2.1719,0.943629
(本次误差)total cost: 101.937, (更新)update: -2.01204e-08 2.68928e-08 -7.86602e-09 (更新后的估计参数)estimated params: 0.890912,2.1719,0.943629
cost: 101.937>= last cost: 101.937, break.
solve time cost = 0.000112547 seconds.
(估计参数)estimated abc = 0.890912, 2.1719, 0.943629
三、Ceres曲线拟合（Ceres最小二乘问题求解库）
3.1 CMakLists.txt
参见2.1
3.2 代码实现
//
// Created by xiang on 18-11-19.
//#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维，即a、b、cT *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) {// 1. 设置参数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随机数产生器// 2. 将样本点(x,y)存入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));}double abc[3] = {ae, be, ce};// 3. 构建最小二乘问题ceres::Problem problem;for (int i = 0; i < N; i++) {problem.AddResidualBlock( // 4.将参数快和残差块加入到ceres的Problem中// 1）使用自动求导求解误差，<误差类型，输出维度，输入维度> 维数要与前面struct中一致new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(new CURVE_FITTING_COST(x_data[i], y_data[i])//代价函数 曲线拟合成本),nullptr, // 2）核函数，这里不使用，为空abc // 3）待估计参数);}// 5. 配置求解器ceres::Solver::Options options; // options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY; // 增量方程如何求解 (致密_正常_多孔)options.minimizer_progress_to_stdout = true; // 输出到cout// 6. 优化信息ceres::Solver::Summary summary; // 7.开始优化（求解器，最小二乘问题，优化信息）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;// 6. 输出结果cout << summary.BriefReport() << endl;cout << "estimated a,b,c = ";for (auto a:abc) cout << a << " ";cout << endl;return 0;
}
3.3 结果展示
这里迭代了8次，ceres花费时间比手写高斯牛顿长，终止条件是收敛
iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time0 1.597873e+06 0.00e+00 3.52e+06 0.00e+00 0.00e+00 1.00e+04 0 1.81e-05 7.10e-051 1.884440e+05 1.41e+06 4.86e+05 9.88e-01 8.82e-01 1.81e+04 1 4.79e-05 1.55e-042 1.784821e+04 1.71e+05 6.78e+04 9.89e-01 9.06e-01 3.87e+04 1 1.81e-05 1.80e-043 1.099631e+03 1.67e+04 8.58e+03 1.10e+00 9.41e-01 1.16e+05 1 1.69e-05 2.01e-044 8.784938e+01 1.01e+03 6.53e+02 1.51e+00 9.67e-01 3.48e+05 1 1.69e-05 2.22e-045 5.141230e+01 3.64e+01 2.72e+01 1.13e+00 9.90e-01 1.05e+06 1 1.60e-05 2.43e-046 5.096862e+01 4.44e-01 4.27e-01 1.89e-01 9.98e-01 3.14e+06 1 1.60e-05 2.63e-047 5.096851e+01 1.10e-04 9.53e-04 2.84e-03 9.99e-01 9.41e+06 1 1.60e-05 2.85e-04
solve time cost = 0.000306812 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
四、g2o曲线拟合（g2o图优化库）
4.1 CMakLists.txt
参见2.1
4.2 代码实现
#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 {}
};// 误差模型 模板参数：观测值维度，类型，连接顶点类型 这里BaseUnaryEdge表示一元边
class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> {
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWCurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}// 计算曲线模型误差virtual void computeError() override {// 取出顶点，将其转换成Eigen类型const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);const Eigen::Vector3d abc = v->estimate();// 误差计算公式：w=y-exp(a*x*x+b*x+c)_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) {// 1. 设置参数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随机数产生器// 2. 将样本点(x,y)存入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));}// 3. 构建图优化，先设定g2otypedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block; // 每个误差项优化变量维度为3，误差值维度为1// 线性方程求解器std::unique_ptr<Block::LinearSolverType> linearSolver ( new g2o::LinearSolverDense<Block::PoseMatrixType>()); // 矩阵块求解器std::unique_ptr<Block> solver_ptr ( new Block ( std::move(linearSolver)));// 选择优化算法，从GN, LM, DogLeg 中选g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::move(solver_ptr));// g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );// g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );g2o::SparseOptimizer optimizer; // 图模型optimizer.setAlgorithm( solver ); // 设置求解器：线性方程求解器->矩阵块求解器->优化算法->g2ooptimizer.setVerbose( true ); // 打开调试输出// 4. 往图中增加顶点CurveFittingVertex *v = new CurveFittingVertex();v->setEstimate(Eigen::Vector3d(ae, be, ce));v->setId(0);optimizer.addVertex(v);// 5. 往图中增加边for (int i = 0; i < N; i++) {CurveFittingEdge *edge = new CurveFittingEdge(x_data[i]);edge->setId(i);edge->setVertex(0, v); // 1)设置连接的顶点(a,b,c)edge->setMeasurement(y_data[i]); // 2)观测数值(y)edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 1 / (w_sigma * w_sigma)); // 3)信息矩阵：协方差矩阵之逆optimizer.addEdge(edge);}// 6. 执行优化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;// 7. 输出优化值Eigen::Vector3d abc_estimate = v->estimate();cout << "estimated model: " << abc_estimate.transpose() << endl;return 0;
}4.3 结果展示
start optimization
iteration= 0 chi2= 376795.438690 time= 1.3516e-05 cumTime= 1.3516e-05 edges= 100 schur= 0 lambda= 21.496593 levenbergIter= 1
iteration= 1 chi2= 35680.668987 time= 6.08e-06 cumTime= 1.9596e-05 edges= 100 schur= 0 lambda= 10.024449 levenbergIter= 1
iteration= 2 chi2= 2199.597326 time= 5.449e-06 cumTime= 2.5045e-05 edges= 100 schur= 0 lambda= 3.341483 levenbergIter= 1
iteration= 3 chi2= 178.624646 time= 5.223e-06 cumTime= 3.0268e-05 edges= 100 schur= 0 lambda= 1.113828 levenbergIter= 1
iteration= 4 chi2= 103.241076 time= 5.024e-06 cumTime= 3.5292e-05 edges= 100 schur= 0 lambda= 0.371276 levenbergIter= 1
iteration= 5 chi2= 101.938412 time= 5.132e-06 cumTime= 4.0424e-05 edges= 100 schur= 0 lambda= 0.123759 levenbergIter= 1
iteration= 6 chi2= 101.937020 time= 5.179e-06 cumTime= 4.5603e-05 edges= 100 schur= 0 lambda= 0.082506 levenbergIter= 1
iteration= 7 chi2= 101.937020 time= 5.128e-06 cumTime= 5.0731e-05 edges= 100 schur= 0 lambda= 0.055004 levenbergIter= 1
iteration= 8 chi2= 101.937020 time= 5.103e-06 cumTime= 5.5834e-05 edges= 100 schur= 0 lambda= 0.036669 levenbergIter= 1
solve time cost = 0.000259988 seconds.
iteration= 9 chi2= 101.937020 time= 5.117e-06 cumTime= 6.0951e-05 edges= 100 schur= 0 lambda= 0.024446 levenbergIter= 1
estimated model: 0.890912 2.1719 0.943629
更多推荐
SLAM高翔十四讲（六）第六讲 非线性优化
发布评论