视觉SLAM十四讲笔记-6-3

2023-11-19

视觉SLAM十四讲笔记-6-3

文章目录

视觉SLAM十四讲笔记-6-3
- 6.3 实践：曲线拟合问题

6.3 实践：曲线拟合问题

6.3.1 手写高斯牛顿法

接下来用一个简单的例子来说明如何求解最小二乘问题，(最小二乘法（又称最小平方法）是一种数学优化技术。它通过最小化误差的平方和寻找数据的最佳函数匹配。利用最小二乘法可以简便地求得未知的数据，并使得这些求得的数据与实际数据之间误差的平方和为最小．最小二乘问题的主要思想就是求解未知参数，使得预测值与观测值之差（即误差，或者说残差）的平方和达到最小。)
接下来将演示如何手写高斯牛顿法，然后介绍如何使用优化库求解此问题．对于同一个问题，这些实现公式会得到同样的结果，因为核心算法是一样的．
考虑一条满足以下方程的曲线：
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 w w 为高斯噪声，满足 w ∼ ( 0 , σ 2 ) w\sim (0,\sigma ^2) w∼(0,σ2)．在这里故意选择了这样一个非线性模型，使问题不至于太简单．现在，假设有 N N N 个关于 x , y x,y x,y 的观测数据点，想根据这些数据点求出曲线的参数．那么，可以求解下面的最小二乘问题以估计曲线参数：
min ⁡ a , b , c 1 2 ∑ N ∥ y i − exp ⁡ ( a x i 2 + b x i + c ) ∥ 2 \min _{a, b, c} \frac{1}{2} \sum^{N}\left\|y_{i}-\exp \left(a x_{i}^{2}+b x_{i}+c\right)\right\|^{2} a,b,cmin21∑N∥∥yi−exp(axi2+bxi+c)∥∥2
请注意，在这个问题中，待估计的变量为 a , b , c a,b,c a,b,c，而不是 x x x．程序中是先根据生成 x , y x,y x,y 的真值，然后在真值中添加高斯分布的噪声．随后，使用高斯牛顿法从带噪声的数据拟合参数模型．定义误差为：
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)
那么，可以求出每个误差项对于状态变量的导数：
∂ 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 ) \frac{\partial e_i}{\partial a} = -x_i^2exp(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) ∂a∂ei=−xi2exp(axi2+bxi+c)∂b∂ei=−xiexp(axi2+bxi+c)∂c∂ei=−exp(axi2+bxi+c)
于是 J i = [ ∂ e i ∂ a , ， ∂ e i ∂ b ， ∂ e i ∂ c ] T J_i = [\frac{\partial e_i}{\partial a},，\frac{\partial e_i}{\partial b}，\frac{\partial e_i}{\partial c}]^T Ji=[∂a∂ei,，∂b∂ei，∂c∂ei]T
高斯牛顿法的增量方程为：
( ∑ i = 1 100 J i ( σ 2 ) J i T ) Δ x k = ∑ i = 1 100 − J i ( σ 2 ) e i (\sum_{i=1}^{100} J_i(\sigma ^2)J_i^T)\Delta x_k = \sum_{i=1}^{100}-J_i(\sigma ^2)e_i (i=1∑100Ji(σ2)JiT)Δxk=i=1∑100−Ji(σ2)ei
(这里跟书上 P 133 P_{133} P133 写的不一样，书上在左右两边 $ J_i(\sigma ^2)$ 上取了逆，而根据前面高斯牛顿法的推导应该是没有逆的．)
下面通过程序来演示该高斯牛顿求解过程：
首先新建文件夹并在该文件夹下打开 VS Code:

mkdir gaussNewton
cd gaussNewton/
code .

在该文件夹下新建build文件夹及CMakeLists.txt和gaussNewton.cpp
请添加图片描述
新建launch.json文件和tasks.json文件：

//launch.json
{
    // Use IntelliSense to learn about possible attributes.
    // Hover to view descriptions of existing attributes.
    // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
    "version": "0.2.0",
    "configurations": [
        {
            "name": "g++ - 生成和调试活动文件",
            "type": "cppdbg",
            "request":"launch",
            "program":"${workspaceFolder}/build/gaussNewton",
            "args": [],
            "stopAtEntry": false,
            "cwd": "${workspaceFolder}",
            "environment": [],
            "externalConsole": false,
            "MIMode": "gdb",
            "setupCommands": [
                {
                    "description": "为 gdb 启动整齐打印",
                    "text": "-enable-pretty-printing",
                    "ignoreFailures": true
                }
            ],
            "preLaunchTask": "Build",
            "miDebuggerPath": "/usr/bin/gdb"
        }
    ]
}

//tasks.json
{
	"version": "2.0.0",
	"options":{
		"cwd": "${workspaceFolder}/build"   //指明在哪个文件夹下做下面这些指令
	},
	"tasks": [
		{
			"type": "shell",
			"label": "cmake",   //label就是这个task的名字,这个task的名字叫cmake
			"command": "cmake", //command就是要执行什么命令,这个task要执行的任务是cmake
			"args":[
				".."
			]
		},
		{
			"label": "make",  //这个task的名字叫make
			"group": {
				"kind": "build",
				"isDefault": true
			},
			"command": "make",  //这个task要执行的任务是make
			"args": [

			]
		},
		{
			"label": "Build",
			"dependsOrder": "sequence", //按列出的顺序执行任务依赖项
			"dependsOn":[				//这个label依赖于上面两个label
				"cmake",
				"make"
			]
		}
	]
}

#CMakeLists.txt
cmake_minimum_required(VERSION 3.0)

project(GAUSSNEWTON)

#在g++编译时，添加编译参数，比如-Wall可以输出一些警告信息
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=c++11")

#一定要加上这句话，加上这个生成的可执行文件才是可以Debug的，不然不加或者是Release的话生成的可执行文件是无法进行调试的
set(CMAKE_BUILD_TYPE Debug)

# Eigen
include_directories("/usr/include/eigen3")

#此工程要调用opencv库，因此需要添加opancv头文件和链接库
#寻找OpenCV库
find_package(OpenCV REQUIRED)

#添加头文件
include_directories(${OpenCV_INCLUDE_DIRS})

add_executable(gaussNewton gaussNewton.cpp)

#链接OpenCV库
target_link_libraries(gaussNewton ${OpenCV_LIBS})

//gaussNewton.cpp
#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <algorithm>
#include <chrono>

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);
        //rng.gaussian,产生一个均值为0，标准差为σ的随机数(产生一个均匀分布的随机数)
        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

            //J为3行1列， J * J.transpose()为三行三列
            H += inv_sigma * inv_sigma * J * J.transpose();
            //b为三行一列
            b += -inv_sigma * inv_sigma * error * J;

            cost += error * error;
        }
        //求解线性方程Hx = b
        Vector3d dx = H.ldlt().solve(b); //ldlt()表示利用Cholesky分解求dx
        if (isnan(dx[0]))//isnan()函数判断输入是否为非数字，是非数字返回真，nan全称为not a number
        {
            cout << "result is nan!" << endl;
            break;
        }
        if (iter > 0 && cost >= lastcost) //因为iter要大于0，第1次迭代(iter = 0, cost > lastCost)不执行！
        {
            cout << "cost: " << cost << ">= last cost: " << lastcost << ", break." << endl;
            break;
        }
        //更新优化变量ae,be,ce
        ae += dx[0];
        be += dx[1];
        ce += dx[2];

        lastcost = cost;
        cout << "total cost : " << cost << ",\t\tupdate: " << dx.transpose() <<  "\t\testimated 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() << "second. " << endl;
    cout << "estimated abc = " << ae << "," << be << "," << ce << endl;
    return 0;
}

运行结果：
请添加图片描述

6.3.2 使用Ceres进行曲线拟合

本节介绍两个 C + + C++ C++ 的优化库：来自谷歌的 C e r e s Ceres Ceres 库及基于图优化的 g 2 o g2o g2o 库．因为使用 g 2 o g2o g2o 还需要介绍一点图优化的相关知识，所以先来介绍 C e r e s Ceres Ceres，然后介绍一些图优化理论，最后来讲 g 2 o g2o g2o．
优化算法在之后的 “视觉里程计” 和 “后端” 中都会出现，所以请读者务必掌握优化算法的意义，理解程序的内容．

Ceres 简介

Ceres 是一个广泛使用的最小二乘问题求解库．在 Ceres 中，作为用户只需要按照一定步骤定义待解的优化问题，然后交给求解器去计算． Ceres 求解的最小二乘问题最一般的形式如下(带边界的核函数最小二乘)：
min ⁡ x 1 2 ∑ i ρ i ( ∥ f i ( x i 1 , ⋯ x i n ) ∥ 2 ) s.t. l j ⩽ x j ⩽ u j . \begin{array}{ll} \min _{x} & \frac{1}{2} \sum_{i} \rho_{i}\left(\left\|f_{i}\left(x_{i_{1}}, \cdots x_{i_{n}}\right)\right\|^{2}\right) \\ \text { s.t. } & l_{j} \leqslant x_{j} \leqslant u_{j} . \end{array} minx s.t. 21∑iρi(∥fi(xi1,⋯xin)∥2)lj⩽xj⩽uj.
在这个问题中， x 1 , x 2 , . . . , x n x_1,x_2,...,x_n x1,x2,...,xn 为优化变量，又称为参数块， f i f_i fi 称为代价函数(Cost function)，也称为残差块(Residual blocks)，在SLAM中也可理解为误差项． l j l_j lj和 u j u_j uj为第 j j j个优化变量的上限和下限．在简单的情况下，取 l j = − ∞ l_j= -\infty lj=−∞， u j = + ∞ u_j = +\infty uj=+∞(不限制优化变量的边界)．此时，目标函数由许多平方项经过一个核函数 ρ ( . ) \rho(.) ρ(.) 之后求和组成．同时，可以取 ρ ( . ) \rho(.) ρ(.) 为恒等函数，那么目标函数即为许多项的平方和．这样就得到了无约束的最小二乘问题．
为了让 Ceres 求解这个问题，需要做以下几件事：
1.定义每个参数块．参数块通常为平凡的向量，但是在SLAM里也可以定义为四元数，李代数这种特殊的结构．如果是向量，需要为每个参数块分配一个 double 数组来存储变量的值．
2.定义残差块的计算方式．残差块通常关联若干个参数块，对它们进行一些自定义的计算，然后返回残差值．Ceres 对它们求平方和之后，作为目标函数的值．
3.残差块往往也需要定义雅克比的计算方式．在 Ceres 中，可以使用它提供的"自动求导" 功能，也可以手动指定雅克比的计算过程．如果要使用自动求导，那么残差块需要按照特定的写法手写：残差的计算过程应该是一个带模板的括号运算符．
4.将所有的参数块和残差块加入 Ceres 定义的 Problem 对象中，调用 Solve 函数求解即可．求解之前，可以传入一些配置信息，例如迭代次数，终止条件等，也可以使用默认的配置．

安装Ceres

首先安装依赖项：

sudo apt-get install liblapack-dev libsuitesparse-dev libcxsparse3.1.2 libgflags-dev libgoogle-glog-dev libgtest-dev

结果发现会报：
请添加图片描述
解决办法：link 和 link
然后安装 Ceres：
首先，下载 Ceres，链接: link
然后进入文件夹下，使用下述命令安装：

mkdir build
cd build
cmake ..
make -j4
sudo make install

发现报错，报错如下：

/home/yjq/Downloads/ceres-solver-master/include/ceres/jet.h:204:13: error: specialization of ‘template<class ... _Tp> struct std::common_type’ in different namespace [-fpermissive]
 struct std::common_type<ceres::Jet<T, N>, U> {
             ^
In file included from /usr/include/c++/5/bits/move.h:57:0,
                 from /usr/include/c++/5/bits/stl_pair.h:59,
                 from /usr/include/c++/5/bits/stl_algobase.h:64,
                 from /usr/include/c++/5/memory:62,
                 from /home/yjq/Downloads/ceres-solver-master/include/ceres/autodiff_cost_function.h:128,
                 from /home/yjq/Downloads/ceres-solver-master/include/ceres/ceres.h:37,
                 from /home/yjq/Downloads/ceres-solver-master/examples/rosenbrock.cc:31:
/usr/include/c++/5/type_traits:2123:12: error:   from definition of ‘template<class ... _Tp> struct std::common_type’ [-fpermissive]
     struct common_type;
            ^
In file included from /home/yjq/Downloads/ceres-solver-master/include/ceres/internal/autodiff.h:153:0,
                 from /home/yjq/Downloads/ceres-solver-master/include/ceres/autodiff_cost_function.h:130,
                 from /home/yjq/Downloads/ceres-solver-master/include/ceres/ceres.h:37,
                 from /home/yjq/Downloads/ceres-solver-master/examples/rosenbrock.cc:31:
/home/yjq/Downloads/ceres-solver-master/include/ceres/jet.h:209:13: error: specialization of ‘template<class ... _Tp> struct std::common_type’ in different namespace [-fpermissive]
 struct std::common_type<ceres::Jet<T, N>, ceres::Jet<U, N>> {
             ^
In file included from /usr/include/c++/5/bits/move.h:57:0,
                 from /usr/include/c++/5/bits/stl_pair.h:59,
                 from /usr/include/c++/5/bits/stl_algobase.h:64,
                 from /usr/include/c++/5/memory:62,
                 from /home/yjq/Downloads/ceres-solver-master/include/ceres/autodiff_cost_function.h:128,
                 from /home/yjq/Downloads/ceres-solver-master/include/ceres/ceres.h:37,

这时参考链接：link，升级 gcc和 g++
利用下面代码查看编译器版本：

gcc -v
g++ -v

将编译器默认版本改为7即可，在终端输入下面代码：

sudo add-apt-repository ppa:ubuntu-toolchain-r/test
sudo apt-get update 
sudo apt-get install gcc-7
sudo apt-get install g++-7

配置:将gcc7，g++7作为默认选项：

sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-7 100
sudo update-alternatives --config gcc
 
sudo update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-7 100
sudo update-alternatives --config g++

然后重新编译，发现还是报错：
这时参考链接：link，由于 link 的版本较低，得下载高版本的 Ceres 版本．
请添加图片描述
按照如下步骤进行安装：

cd ceres-solver
mkdir build
cd build
cmake ..
make 
sudo make install

安装成功．

请添加图片描述

使用Ceres拟合曲线

//launch.json
{
    // Use IntelliSense to learn about possible attributes.
    // Hover to view descriptions of existing attributes.
    // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
    "version": "0.2.0",
    "configurations": [
        {
            "name": "g++ - 生成和调试活动文件",
            "type": "cppdbg",
            "request":"launch",
            "program":"${workspaceFolder}/build/ceresCurveFitting",
            "args": [],
            "stopAtEntry": false,
            "cwd": "${workspaceFolder}",
            "environment": [],
            "externalConsole": false,
            "MIMode": "gdb",
            "setupCommands": [
                {
                    "description": "为 gdb 启动整齐打印",
                    "text": "-enable-pretty-printing",
                    "ignoreFailures": true
                }
            ],
            "preLaunchTask": "Build",
            "miDebuggerPath": "/usr/bin/gdb"
        }
    ]
}

//tasks.json
{
	"version": "2.0.0",
	"options":{
		"cwd": "${workspaceFolder}/build"   //指明在哪个文件夹下做下面这些指令
	},
	"tasks": [
		{
			"type": "shell",
			"label": "cmake",   //label就是这个task的名字,这个task的名字叫cmake
			"command": "cmake", //command就是要执行什么命令,这个task要执行的任务是cmake
			"args":[
				".."
			]
		},
		{
			"label": "make",  //这个task的名字叫make
			"group": {
				"kind": "build",
				"isDefault": true
			},
			"command": "make",  //这个task要执行的任务是make
			"args": [

			]
		},
		{
			"label": "Build",
			"dependsOrder": "sequence", //按列出的顺序执行任务依赖项
			"dependsOn":[				//这个label依赖于上面两个label
				"cmake",
				"make"
			]
		}
	]
}

#CMakeLists.txt
cmake_minimum_required(VERSION 3.0)

project(CERESCURVECERES)

#在g++编译时，添加编译参数，比如-Wall可以输出一些警告信息
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=c++11")

#一定要加上这句话，加上这个生成的可执行文件才是可以Debug的，不然不加或者是Release的话生成的可执行文件是无法进行调试的
set(CMAKE_BUILD_TYPE Debug)

# Eigen
include_directories("/usr/include/eigen3")

#此工程要调用opencv库，因此需要添加opancv头文件和链接库
#寻找OpenCV库
find_package(OpenCV REQUIRED)
#添加头文件
include_directories(${OpenCV_INCLUDE_DIRS})

#find Ceres
find_package(Ceres REQUIRED)
include_directories(${CERES_INCLUDE_DIRS})

add_executable(ceresCurveCeres ceresCurveCeres.cpp)

#链接OpenCV库
target_link_libraries(ceresCurveFitting ${OpenCV_LIBS} ${CERES_LIBRARIES})

//ceresCurveCeres.cpp
#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>
#include <algorithm>
#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 {
        //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;
         //返回bool类型，计算结果已经存入函数外的residual变量中
    }
    const double _x, _y; // x,y数据 结构体CURVE_FITTING_COST中的成员变量
};


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的协方差值，初始化为1
    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));
    }

    double abc[3] = {ae, be, ce}; //定义优化变量

    //构建最小二乘问题
    ceres::Problem 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
    options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY; //增量方程如何求解
    //增量方程求解方式，本质上是稠密矩阵求逆的加速方法选择
    options.minimizer_progress_to_stdout = true; //options.minimizer_progress_to_stdout表示是否向终端输出优化过程信息

    ceres::Solver::Summary summary; //优化信息，利用ceres执行优化
    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;
}

运行结果：
请添加图片描述
程序中演示的 Ceres 用法有如下几项：
1.定义残差块的类．方法是书写一个类(或者结构体)，并在类中定义模板参数的()运算符，这样该类就成为了一个拟函数．这种定义方式使得 Ceres 可以像调用函数一样，对该类的某个对象(比如a)调用a < double > () 的方法.事实上， Ceres 会把雅克比矩阵作为参数类型传入此函数，从而实现自动求导的功能．
2.程序中 double abc[3] 即为参数块，而对于残差块，对于每一个数据构造 CURVE_FITTING_COST 对象，然后调用 AddResidualBlock 将误差项添加到目标函数中．
由于优化需要梯度，这里有若干个选择：(1)使用 Ceres 的自动求导(Auto Diff)；(2)使用数值求导(Numeric Diff)；(3)自行推导解析的导数形式，提供给 Ceres．因为自动求导在编码上是最方便的，于是使用自动求导．
3.自动求导需要指定误差项和优化变量的维度．这里的误差是标量，维度为1；优化变量为a,b,c三个量，维度为3．于是在自动求导类 AutoDiffCostFunction 的模板参数中设定变量维度为1,3．
4.设定好问题后，调用 Solve 函数进行求解．首先，可以在 options 中配置(非常详细的)优化选项．例如，可以选择使用 Line Search 还是 Trust Region，迭代次数，步长等．可以查看 Options 的定义，看看有哪些优化方法可以选择，当然默认的配置已经可以用于很广泛的问题了．
虽然这个例子中使用 Ceres 比手写高斯牛顿法速度慢．但是， Ceres 的优点是提供了自动求导工具，使得不必去计算很麻烦的雅克比矩阵．Ceres 的自动求导是通过模板元来实现的，在编译时期就可以完成自动求导工作，不过仍然是数值求导．

6.3.3 使用g2o进行曲线拟合

本节介绍另外一个在 SLAM 领域广为使用的优化库：g2o(General Graphic Optimization, G2O)．它是一个基于图优化的库．图优化是一种将非线性优化与图论结合起来的理论，因此在使用之前，先介绍图优化理论．

图优化理论介绍

非线性最小二乘：由很多个误差项之和组成．然而，目标函数仅描述了优化变量和许多个误差项，但是尚不清楚它们之间的关联．例如，某个优化变量 x j x_j xj 存在于多少个误差项中尼?能保证对它的优化是有意义的吗?进一步，希望能够直观地看到该优化问题长什么样．
图优化，是把优化问题表现为图的一种方式．这里的图是图论意义上的图．一个图由若干个顶点(Vertex)和连接这些顶点的边(Edge)组成．进而，用顶点表示优化变量，用边表示误差项．于是，对于任意一个上述形式的非线性最小二乘问题，可以构建与之对应到一个图．(这里可以简单的称为图，也可以用概率图中的定义，称之为贝叶斯图或者因子图).
下图是一个简单的图优化例子,图片参考链接：link
在这里插入图片描述
图中用三角形表示相机位姿节点，用圆形表示路标点．它们构成了图优化的顶点；同时，实现表示相机的运动模型，虚线表示观测模型，它们构成图优化的边．此时，虽然整个问题的数学形式仍然是最小二乘的形式：
m i n J ( x , y ) = ∑ k e u , k T R k − 1 e u , k + ∑ k ∑ j e z , k , j T Q k , j − 1 e z , k , j min \;J(\boldsymbol{x},\boldsymbol{y})=\sum_{k} e_{u, k}^{T} \boldsymbol{R}_{k}^{-1} e_{u, k}+\sum_{k} \sum_{j} e_{z, k, j}^{T} Q_{k, j}^{-1} e_{z, k, j} minJ(x,y)=k∑eu,kTRk−1eu,k+k∑j∑ez,k,jTQk,j−1ez,k,j
但现在可以直观地看到问题的结构了．如果希望，也可以做去掉孤立顶点或优先优化边数较多(或按照图论的术语，度数较大)的顶点这样进行改进．但是最基本的图优化是用图模型来表达一个非线性最小二乘的优化问题，但是可以利用图模型的某些性质做更好的优化．
g2o是一个通用的图优化库．＂通用＂意味着可以在 g2o 里求解任何能够表示为图优化的最小二乘问题．

g2o的编译与安装

首先安装依赖项，用下面命令安装时会显示无法定位软件包

sudo apt-get install qt5−qmake qt5−default libqglviewer−dev−qt5 libsuitesparse−dev libcxsparse3 libcholmod3

请添加图片描述
下面一个一个安装

sudo apt-get install qt5-qmake
sudo apt-get install qt5-default
sudo apt-get install　libsuitesparse−dev

这三个安装没有问题．
对于libqglviewer−dev−qt5,解决方案参考链接：link
去官网下载，官网链接：link，然后进入文件夹，通过下述命令进行安装．

cd QGLViewer
qmake
make
sudo make install

对于libcxsparse3 和 libcholmod3，安装时通过 tab 键查看具体的版本，安装具体的版本．
参考链接：link
至此依赖安装完成．
然后安装 g2o，链接：link

安装 g2o 时报 cmake 版本太低，参考link，进行新版本安装，安装时不要卸载旧版本，卸载旧版本会导致以前基于 cmake 的程序出问题．
升级完 cmake 后，发现上一个基于 ceres 的拟合曲线程序报错，
请添加图片描述
参考链接：link，把 CMakeLists.txt中set(CMAKE_CXX_FLAGS “-O0 -std=c++11”)
改为
set(CMAKE_CXX_FLAGS “-O0 -std=c++14”)解决问题．

弄好上面配置后，还是在原先下载的 g2o 安装包link里直接按照下述命令进行安装：

mkdir build
cd build
cmake ..
make
sudo make install

请添加图片描述
安装成功后，g2o的头文件将位于/usr/local/g2o下，库文件位于 /usr/local/lib/ 下．

使用 g2o 拟合曲线

为了使用 g2o，首先要将曲线拟合问题抽象成图优化．在这个过程中，只要记住节点为优化变量，边为误差项即可．曲线拟合对应的图优化模型可以用下图表示．
图片参考链接：link
在这里插入图片描述
在曲线拟合问题中，整个问题只有一个顶点：曲线模型的参数为a,b,c；而各个带噪声的数据点，构成了一个个误差项，也就是图优化的边．这里的边和平时说的边不一样，这里的边是一元边，即只连接一个顶点．
弄清这个图后，接下来就是在 g2o中建立该模型进行优化．作为 g2o 的用户，需要做的事情主要包含以下步骤：
1.定义顶点和边的类型；
2.构建图;
3.选择优化算法;
4.调用 g2o 进行优化，返回结果．

//launch.json
{
    // Use IntelliSense to learn about possible attributes.
    // Hover to view descriptions of existing attributes.
    // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
    "version": "0.2.0",
    "configurations": [
        {
            "name": "g++ - 生成和调试活动文件",
            "type": "cppdbg",
            "request":"launch",
            "program":"${workspaceFolder}/build/g2oCurveFitting",
            "args": [],
            "stopAtEntry": false,
            "cwd": "${workspaceFolder}",
            "environment": [],
            "externalConsole": false,
            "MIMode": "gdb",
            "setupCommands": [
                {
                    "description": "为 gdb 启动整齐打印",
                    "text": "-enable-pretty-printing",
                    "ignoreFailures": true
                }
            ],
            "preLaunchTask": "Build",
            "miDebuggerPath": "/usr/bin/gdb"
        }
    ]
}

//tasks.json
{
	"version": "2.0.0",
	"options":{
		"cwd": "${workspaceFolder}/build"   //指明在哪个文件夹下做下面这些指令
	},
	"tasks": [
		{
			"type": "shell",
			"label": "cmake",   //label就是这个task的名字,这个task的名字叫cmake
			"command": "cmake", //command就是要执行什么命令,这个task要执行的任务是cmake
			"args":[
				".."
			]
		},
		{
			"label": "make",  //这个task的名字叫make
			"group": {
				"kind": "build",
				"isDefault": true
			},
			"command": "make",  //这个task要执行的任务是make
			"args": [

			]
		},
		{
			"label": "Build",
			"dependsOrder": "sequence", //按列出的顺序执行任务依赖项
			"dependsOn":[				//这个label依赖于上面两个label
				"cmake",
				"make"
			]
		}
	]
}

#CMakeLists.txt
cmake_minimum_required(VERSION 3.0)

project(G2OCURVEG2O)

#在g++编译时，添加编译参数，比如-Wall可以输出一些警告信息
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=c++14")

#一定要加上这句话，加上这个生成的可执行文件才是可以Debug的，不然不加或者是Release的话生成的可执行文件是无法进行调试的
set(CMAKE_BUILD_TYPE Debug)

# Eigen
include_directories("/usr/include/eigen3")

#此工程要调用opencv库，因此需要添加opancv头文件和链接库
#寻找OpenCV库
find_package(OpenCV REQUIRED)
#添加头文件
include_directories(${OpenCV_INCLUDE_DIRS})

# g2o
find_package(G2O REQUIRED)
include_directories(${G2O_INCLUDE_DIRS})

add_executable(g2oCurveg2o g2oCurveg2o.cpp)

#链接OpenCV库,Ceres库，
target_link_libraries(g2oCurveg2o ${OpenCV_LIBS} ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY})

#include <iostream>
#include <g2o/core/g2o_core_api.h>
#include <g2o/core/base_vertex.h>//g2o顶点（Vertex）头文件 视觉slam十四讲p141用顶点表示优化变量，用边表示误差项
#include <g2o/core/base_unary_edge.h>//g2o边（edge）头文件
#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>//dogleg算法头文件
#include <g2o/solvers/dense/linear_solver_dense.h>//稠密矩阵求解
#include <Eigen/Core>//Eigen核心模块
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>
 
using namespace std;
 
// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> //:表示继承，public表示公有继承；CurveFittingVertex是派生类，BaseVertex<3, Eigen::Vector3d>是基类
{
public://以下定义的成员变量和成员函数都是公有的
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW//解决Eigen库数据结构内存对齐问题
 
  // 重置
  virtual void setToOriginImpl() override //virtual表示该函数为虚函数，override保留字表示当前函数重写了基类的虚函数
  {
    _estimate << 0, 0, 0;
  }
 
  // 更新
  virtual void oplusImpl(const double *update) override 
  {
    _estimate += Eigen::Vector3d(update);//更新量累加
  }
 
  // 存盘和读盘：留空
  virtual bool read(istream &in) {} //istream类是c++标准输入流的一个基类
  //可参照C++ Primer Plus第六版的6.8节
  virtual bool write(ostream &out) const {} //ostream类是c++标准输出流的一个基类
  //可参照C++ Primer Plus第六版的6.8节
};
 
// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW//解决Eigen库数据结构内存对齐问题
 
  CurveFittingEdge(double x) : BaseUnaryEdge<1, double, CurveFittingVertex>(), _x(x) {}//使用列表赋初值
 
  // 计算曲线模型误差
  virtual void computeError() override //virtual表示虚函数，保留字override表示当前函数重写了基类的虚函数
  {
    const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);//创建指针v
    const Eigen::Vector3d abc = v->estimate();//将estimate()值赋给abc
    _error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0));//视觉slam十四讲p133式6.39
  }
 
  // 计算雅可比矩阵
  virtual void linearizeOplus() override //virtual表示虚函数，保留字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]);//视觉slam十四讲p133式6.38上面的式子
    _jacobianOplusXi[0] = -_x * _x * y;//视觉slam十四讲p133式6.40第一个
    _jacobianOplusXi[1] = -_x * y;//视觉slam十四讲p133式6.40第二个
    _jacobianOplusXi[2] = -y;//视觉slam十四讲p133式6.40第三个
  }
 
  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随机数产生器  RNG为OpenCV中生成随机数的类，全称是Random Number Generator
 
  vector<double> x_data, y_data;      // double型数据
  for (int i = 0; i < N; i++) {
    double x = i / 100.0;//相当于x范围是0-1
    x_data.push_back(x);//x_data存储的数值 所给的100个观测点数据
    y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
    //rng.gaussian(w_sigma * w_sigma)为opencv随机数产生高斯噪声
    //rng.gaussian(val)表示生成一个服从均值为0，标准差为val的高斯分布的随机数  视觉slam十四讲p133式6.38上面的表达式
  }
 
  // 构建图优化，先设定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>()));
    //c++中的make_unique表示智能指针类型，而g2o中的make_unique表示
  g2o::SparseOptimizer optimizer;     // 图模型
  optimizer.setAlgorithm(solver);   // 设置求解器
  optimizer.setVerbose(true);       // 打开调试输出
 
  // 往图中增加顶点
  CurveFittingVertex *v = new CurveFittingVertex();//指针v
  v->setEstimate(Eigen::Vector3d(ae, be, ce));  //ae、be和ce表示优化变量
  v->setId(0);//对顶点进行编号，里面的0你可以写成任意的正整数，但是后面设置edge连接顶点时，必须要和这个一致
  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->setVertex(0, v); 其中的0为该边连接的第一个顶点，即前面编号为0的顶点v，因为是一元边，因此只需这一句。
    //根据函数公式：y=exp(ax^2+bx+c)+w可知，y为观测值，故有edge->setMeasurement(y_data[i])。图构建出之后，即可进行迭代求解。
    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;//输出start optimization
  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;
}

运行时发现报错，找不到
请添加图片描述
参考链接：link，要将下载的 g2o 中cmake_modules 路径添加到 CMakeLists.txt 中去，因此添加完的 CMakeLists.txt 如下：

#CMakeLists.txt
cmake_minimum_required(VERSION 3.0)

project(G2OCURVEG2O)

#在g++编译时，添加编译参数，比如-Wall可以输出一些警告信息
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=c++14")

#一定要加上这句话，加上这个生成的可执行文件才是可以Debug的，不然不加或者是Release的话生成的可执行文件是无法进行调试的
set(CMAKE_BUILD_TYPE Debug)

list( APPEND CMAKE_MODULE_PATH /home/lss/Downloads/g2o/cmake_modules )
set(G2O_ROOT /usr/local/include/g2o)

# Eigen
include_directories("/usr/include/eigen3")

#此工程要调用opencv库，因此需要添加opancv头文件和链接库
#寻找OpenCV库
find_package(OpenCV REQUIRED)
#添加头文件
include_directories(${OpenCV_INCLUDE_DIRS})

# g2o
find_package(G2O REQUIRED)
include_directories(${G2O_INCLUDE_DIRS})

add_executable(g2oCurveg2o g2oCurveg2o.cpp)

#链接OpenCV库,Ceres库，
target_link_libraries(g2oCurveg2o ${OpenCV_LIBS} ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY})

此时再运行，发现 Release 模型下可以正常运行：
请添加图片描述
而 Debug 模式下又报错误：

此时，参考链接：link，安装 gflags 和 glog 两个库:(一定按照如下步骤进行安装，按照正常的 c m a k e . . cmake .. cmake..,make,make install 安装，在 glog 安装时会报错。)

git clone https://github.com/gflags/gflags.git
cd gflags
mkdir build && cd build
cmake -DCMAKE_INSTALL_PREFIX=/usr/local -DBUILD_SHARED_LIBS=ON -DGFLAGS_NAMESPACE=gflags ../ 
make -j4
sudo make install

git clone https://github.com/google/glog
cd glog/cmake/
cmake ..
sudo make install

而安装 glog 时需要 cmake 的版本不能低于 3.16,因此刚升级为 3.15 不能用，还需再升级.
直接采用下面命令进行升级即可。

wget https://cmake.org/files/v3.22/cmake-3.22.1.tar.gz
cd cmake-3.22.1
./configure
make
sudo make install
cmake --version //3.22.1

升级完 cmake 继续编译安装 glog即可，然后在 CMakeLists.txt 中要加入 glog 相关的语句，链接 glog 相关的库：

#CMakeLists.txt
cmake_minimum_required(VERSION 3.0)

project(G2OCURVEG2O)

#在g++编译时，添加编译参数，比如-Wall可以输出一些警告信息
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=c++14")

#一定要加上这句话，加上这个生成的可执行文件才是可以Debug的，不然不加或者是Release的话生成的可执行文件是无法进行调试的
set(CMAKE_BUILD_TYPE Debug)

list( APPEND CMAKE_MODULE_PATH /home/lss/Downloads/g2o/cmake_modules )
set(G2O_ROOT /usr/local/include/g2o)

# Eigen
include_directories("/usr/include/eigen3")

#此工程要调用opencv库，因此需要添加opancv头文件和链接库
#寻找OpenCV库
find_package(OpenCV REQUIRED)
#添加头文件
include_directories(${OpenCV_INCLUDE_DIRS})

find_package (glog 0.6.0 REQUIRED)
# g2o
find_package(G2O REQUIRED)
include_directories(${G2O_INCLUDE_DIRS})

add_executable(g2oCurveg2o g2oCurveg2o.cpp)

#链接OpenCV库,Ceres库，
target_link_libraries(g2oCurveg2o ${OpenCV_LIBS} ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY} glog::glog)

再在 Debug 模式下运行得到结果：
请添加图片描述
在这个程序中，从 g2o 中派生出了用于曲线拟合的图优化顶点和边: CurveFittingVertex 和 CurveFitting, 这实质上扩展了 g2o 的使用方式。这两个类分别派生自 BaseVertex 和 BaseUnaryEdge.在派生类中，重写了重要的虚函数：
1.顶点的更新函数：oplusImpl。优化过程中最重要的是增量 Δ x \Delta x Δx 的计算。该函数处理的是 x k + 1 = x k + Δ x x_{k+1} = x_k + \Delta x xk+1=xk+Δx，
在曲线拟合过程中，由于优化变量本身位于向量空间中，这个更新计算确实就是简单的加法。但是对于相机的位姿等不一定有加法运算。这时，就要重新定义增量如何加到现有的估计上的行为的。
2.顶点的重置函数：setToOriginImpl()，这里把估计值置零即可。
3.边的误差计算函数：computeError。该函数需要取出边所连接的顶点的当前估计值，根据曲线模型，与它的观测值进行比较。这和最小二乘问题的误差模型是一致的。
4.边的雅克比计算函数：linearizeOplus。这个函数里计算里计算量每条边相对于顶点的雅克比。
5.存盘和读盘函数：read,write。由于并不想进行读/写操作，所以留空。
定义了顶点和边之后，在 main 函数里声明了一个图模型，然后按照生成的噪声数据，往往图模型中添加顶点和边，最后调用优化函数进行优化。

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)