视觉Slam14讲笔记第6讲非线性优化
一、g2o方法使用过程中的问题
由于自身编译器是c++14标准，但是g2o的版本是17标准。导致引用g2o库时有相关报错信息，比如：
g2o/stuff/tuple_tools.h:41:71: error: type/value mismatch at argument 1 in template parameter list for ‘template<long unsigned int _Num> using make_index_sequence = std::make_integer_sequence<long unsigned int, _Num>’41 | f, t, i, std::make_index_sequence<std::tuple_size<std::decay<T>>>());
等等。
可做如下几个操作，
1.在编译g2o库时强制使用c++14标准，并且指定相应的安装路径【如果环境只有一个g2o版本，可以不用这个操作】
cmake .. -DCMAKE_CXX_STANDARD=14 -DCMAKE_CXX_STANDARD_REQUIRED=ON -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=【安装目录】/slambook2-master/3rdparty/g2o/install -DG2O_USE_CXX11_ABI=ON -DG2O_BUILD_EXAMPLES=OFF -DG2O_BUILD_APPS=OFF2.在编译使用g2o非线性优化的代码时，指定目录去寻找g2o以及相应的库
project(g2oCurveFitting)cmake_minimum_required(VERSION 3.10)find_package(Eigen3 REQUIRED)
find_package(OpenCV REQUIRED)set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD_REQUIRED ON)set(G2O_INCLUDE_DIRS "--------/slam14/slambook2-master/3rdparty/g2o/install/include")
set(G2O_LIBRARIES ---------/slam14/slambook2-master/3rdparty/g2o/install/lib/libg2o_core.so---------/slam14/slambook2-master/3rdparty/g2o/install/lib/libg2o_stuff.so
)include_directories(${EIGEN3_INCLUDE_DIR} ${OpenCV_INCLUDE_DIRS} ${G2O_INCLUDE_DIRS})add_executable(g2oCurveFitting g2oCurveFitting.cpp)
target_link_libraries(g2oCurveFitting ${OpenCV_LIBS} ${G2O_LIBRARIES})
3.相应的修改代码
最终执行代码如下：
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/g2o_core_api.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>#include <Eigen/Core>
#include <chrono>
#include <cmath>
#include <iostream>
#include <opencv2/core/core.hpp>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_NEWCurveFittingEdge(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));}// 构建图优化，先设定g2otypedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>>BlockSolverType; // 每个误差项优化变量维度为3，误差值维度为1typedef g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType>LinearSolverType; // 线性求解器类型// 梯度下降方法，可以从GN, LM, DogLeg 中选auto solver = new g2o::OptimizationAlgorithmGaussNewton(std::make_unique<BlockSolverType>(std::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;
}
执行结果为：
start optimization
iteration= 0 chi2= 376785.128234 time= 0.00107027 cumTime= 0.00107027 edges= 100 schur= 0
iteration= 1 chi2= 35673.566018 time= 0.000846484 cumTime= 0.00191675 edges= 100 schur= 0
iteration= 2 chi2= 2195.012304 time= 0.000880363 cumTime= 0.00279711 edges= 100 schur= 0
iteration= 3 chi2= 174.853126 time= 0.000863566 cumTime= 0.00366068 edges= 100 schur= 0
iteration= 4 chi2= 102.779695 time= 0.000838804 cumTime= 0.00449948 edges= 100 schur= 0
iteration= 5 chi2= 101.937194 time= 0.000842615 cumTime= 0.0053421 edges= 100 schur= 0
iteration= 6 chi2= 101.937020 time= 0.000968364 cumTime= 0.00631046 edges= 100 schur= 0
iteration= 7 chi2= 101.937020 time= 0.000840048 cumTime= 0.00715051 edges= 100 schur= 0
iteration= 8 chi2= 101.937020 time= 0.000848594 cumTime= 0.00799911 edges= 100 schur= 0
iteration= 9 chi2= 101.937020 time= 0.000839935 cumTime= 0.00883904 edges= 100 schur= 0
solve time cost = 0.0102802 seconds.
estimated model: 0.890912 2.1719 0.943629
二、手写高斯牛顿代码以及执行结果
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <iostream>
#include <opencv2/opencv.hpp>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<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));}// 开始Gauss-Newton迭代// 迭代次数int iterations = 100;double cost = 0, lastCost = 0; // 本次迭代的cost和上次迭代的costchrono::steady_clock::time_point t1 = chrono::steady_clock::now();for (int iter = 0; iter < iterations; iter++) {Matrix3d H = Matrix3d::Zero(); // HessianVector3d b = Vector3d::Zero(); // biascost = 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);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;}// 求解线性方程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 << endl;break;}ae += dx[0];be += dx[1];ce += dx[2];lastCost = cost;cout << "total cost: " << cost << ", \t\tupdate: " << dx.transpose()<< "\t\testimated parmas: " << 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 << "time used: " << time_used.count() << " seconds. " << endl;cout << "estimated params: " << ae << ", " << be << ", " << ce << endl;return 0;
}
结果如下：
total cost: 3.19575e+06, update: 0.0455771 0.078164 -0.985329 estimated parmas: 2.04558,-0.921836,4.01467
total cost: 376785, update: 0.065762 0.224972 -0.962521 estimated parmas: 2.11134,-0.696864,3.05215
total cost: 35673.6, update: -0.0670241 0.617616 -0.907497 estimated parmas: 2.04432,-0.0792484,2.14465
total cost: 2195.01, update: -0.522767 1.19192 -0.756452 estimated parmas: 1.52155,1.11267,1.3882
total cost: 174.853, update: -0.537502 0.909933 -0.386395 estimated parmas: 0.984045,2.0226,1.00181
total cost: 102.78, update: -0.0919666 0.147331 -0.0573675 estimated parmas: 0.892079,2.16994,0.944438
total cost: 101.937, update: -0.00117081 0.00196749 -0.00081055 estimated parmas: 0.890908,2.1719,0.943628
total cost: 101.937, update: 3.4312e-06 -4.28555e-06 1.08348e-06 estimated parmas: 0.890912,2.1719,0.943629
total cost: 101.937, update: -2.01204e-08 2.68928e-08 -7.86602e-09 estimated parmas: 0.890912,2.1719,0.943629
cost: 101.937 >= last cost: 101.937
time used: 0.00635663 seconds.
estimated params: 0.890912, 2.1719, 0.943629
三、用ceres库实现非线性优化
代码
#include <ceres/ceres.h>#include <chrono>
#include <iostream>
#include <opencv2/core/core.hpp>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, T* residual) const {// y-exp(ax^2+bx+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; // 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; // 估计参数值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));}double abc[3] = {ae, be, ce};// 构建最小二乘问题ceres::Problem problem;for (int i = 0; i < N; i++) {// 使用自动推导，模板参数：误差类型、输出维度、输入维度，维数要与前面struct一致problem.AddResidualBlock(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_NORMAL_CHOLESKY;options.minimizer_progress_to_stdout = true;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;
}
执行结果：
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 4.80e-04 1.00e-031 1.884440e+05 1.41e+06 4.86e+05 9.88e-01 8.82e-01 1.81e+04 1 8.84e-04 1.91e-032 1.784821e+04 1.71e+05 6.78e+04 9.89e-01 9.06e-01 3.87e+04 1 4.86e-04 2.40e-033 1.099631e+03 1.67e+04 8.58e+03 1.10e+00 9.41e-01 1.16e+05 1 5.10e-04 2.92e-034 8.784938e+01 1.01e+03 6.53e+02 1.51e+00 9.67e-01 3.48e+05 1 4.62e-04 3.38e-035 5.141230e+01 3.64e+01 2.72e+01 1.13e+00 9.90e-01 1.05e+06 1 4.67e-04 3.85e-036 5.096862e+01 4.44e-01 4.27e-01 1.89e-01 9.98e-01 3.14e+06 1 4.63e-04 4.32e-037 5.096851e+01 1.10e-04 9.53e-04 2.84e-03 9.99e-01 9.41e+06 1 4.62e-04 4.78e-03
solve time cost = 0.00486601 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