最小二乘法

下降策略： 高斯牛顿法、列文伯格-马夸尔特方法

Ceres库 和 g2o 库

如何在 有噪声的数据 中进行准确的 状态估计
——————
6.1 状态估计问题

运动方程和观测方程：

${\mathbf{x}}_k$ ：相机的位姿， 可用SE(3) 描述

观测方程为 针孔相机模型

假设在 ${\mathbf{x}}_k$ 处 对 路标 ${\mathbf{y}}_j$ 进行了一次观测， 对应到图像上的 像素坐标 为 ${\mathbf{z}}_{k,j}$，
则观测方程可表示为：$s{\mathbf{z}}_{k,j}=\mathbf{K}({\mathbf{R}}_k{\mathbf{y}}_j+{\mathbf{t}}_k)$
其中 $\mathbf{K}$ 为相机内参，s为像素点的距离(??)，也是$({\mathbf{R}}_k{\mathbf{y}}_j+{\mathbf{t}}_k)$ 的第三个分量。

数据 受 噪声 影响后的变化

在运动和观测方程中，通常假设两个噪声项 ${\mathbf{w}}_k,{\mathbf{v}}_{k,j}$ 满足零均值的高斯分布：
$${\mathbf{w}}_k\sim \mathcal{N}(0,{\mathbf{R}}_k),{\mathbf{v}}_k\sim \mathcal{N}(0,{\mathbf{Q}}_{k,j})$$

状态估计问题： 通过带噪声的数据 $\mathbf{z}$ 和 $\mathbf{u}$ 推断位姿 $\mathbf{x}$ 和 地图 $\mathbf{y}$ (以及它们的概率分布)。

处理这个状态估计问题的两种方法：
1、增量/渐进(incremental) 【滤波器】

• 持有一个当前时刻的估计状态，然后用新的时刻来更新它。【仅关心当前时刻的状态估计 ${\mathbf{x}}_k$】
• 扩展卡尔曼滤波器

2、批量(batch)方法

• 把数据 “攒” 起来 一并处理。
• 可以在更大的范围达到最优化， 非实时

————————
折中办法
滑动窗口估计法： 固定一些历史轨迹，仅对当前时刻附近的一些轨迹进行优化。

以非线性优化为主的批量优化方法

考虑从 1 到 $N$ 的所有时刻，假设有 $M$ 个路标点。
定义所有时刻的机器人位姿和路标点坐标为：
x = { x 1 , ⋅ ⋅ ⋅ , x N } ， y = { y 1 , ⋅ ⋅ ⋅ , y M } \bm{x} = \{\bm{x}_1,···,\bm{x}_N\}，\bm{y} = \{\bm{y}_1,···,\bm{y}_M\} x={x1​,⋅⋅⋅,xN​}，y={y1​,⋅⋅⋅,yM​}
用不带下标的 $\mathbf{u}$ 表示所有时刻的输入， $\mathbf{z}$ 表示所有时刻的观测数据。
估计 机器人 状态 <==> 已知输入数据 $\mathbf{u}$ 和观测数据 $\mathbf{z}$ 的条件下，求状态 $\mathbf{x},\mathbf{y}$ 的条件概率分布 $P(\mathbf{x},\mathbf{y}|\mathbf{z},\mathbf{u})$
————————
不知道控制输入，只有一张张图像，即只考虑观测方程带来的数据时，相当于估计 $P(\mathbf{x},\mathbf{y}|\mathbf{z})$ 的条件概率分布

• SfM(Structure from Motion)问题 ， 即如何从许多图像中重建三维空间结构。

估计 状态变量 的条件分布
根据 贝叶斯法则：

$$\begin{array}{rl}后验概率P(\mathbf{x},\mathbf{y}|\mathbf{z},\mathbf{u})& =\frac{P(\mathbf{z},\mathbf{u}|\mathbf{x},\mathbf{y})P(\mathbf{x},\mathbf{y})}{P(\mathbf{z},\mathbf{u})}\\ & \propto P(\mathbf{z},\mathbf{u}|\mathbf{x},\mathbf{y})P(\mathbf{x},\mathbf{y})\end{array}$$
似然(Likehood)：$P(\mathbf{z},\mathbf{u}|\mathbf{x},\mathbf{y})$
先验(Prior): $P(\mathbf{x},\mathbf{y})$

求解 最大 后验概率 等价于 最大化 似然 $P(\mathbf{z},\mathbf{u}|\mathbf{x},\mathbf{y})$ 和 先验 $P(\mathbf{x},\mathbf{y})$ 的乘积。
$$(\mathbf{x},\mathbf{y}{)}_{\mathrm{MAP}}^{\ast }=\arg \max P(\mathbf{x},\mathbf{y}|\mathbf{z},\mathbf{u})=\arg \max P(\mathbf{z},\mathbf{u}|\mathbf{x},\mathbf{y})P(\mathbf{x},\mathbf{y})$$

不知道 机器人位姿 或 路标 大概在什么地方，没有了先验。 即 $P(\mathbf{x},\mathbf{y})$ 不知道。
此时求解最大似然估计(Maximize Likelihood Estimation, MLE)：
$$(\mathbf{x},\mathbf{y}{)}_{\mathrm{MLE}}^{\ast }=\arg \max P(\mathbf{z},\mathbf{u}|\mathbf{x},\mathbf{y})$$
似然 $P(\mathbf{z},\mathbf{u}|\mathbf{x},\mathbf{y})$： 在现在的位姿下，可能产生怎样的观测数据。
最大似然估计 $(\mathbf{x},\mathbf{y}{)}_{\mathrm{MLE}}^{\ast }$： 在什么样的状态下，最可能产生现在观测到的数据。

6.1.2 最小二乘

如何求最大似然估计？

对于某一次观测 ${\mathbf{z}}_{k,j}=h({\mathbf{y}}_j,{\mathbf{x}}_k)+{\mathbf{v}}_{k.j}$
设噪声项 ${\mathbf{v}}_k\sim \mathcal{N}(0,{\mathbf{Q}}_{k,j})$
观测数据的条件概率为：
$$P({\mathbf{z}}_{j,k}|{\mathbf{x}}_k,{\mathbf{y}}_j)=N(h({\mathbf{y}}_j,{\mathbf{x}}_k),{\mathbf{Q}}_{k,j})$$

稀疏性：运动误差只与 ${\mathbf{x}}_{k-1},{\mathbf{x}}_k$有关，观测误差只与 ${\mathbf{x}}_k,{\mathbf{y}}_j$ 有关。【为啥呢？】

用李代数表示增量，是无约束的最小二乘问题
用旋转矩阵/变换矩阵描述位姿，需要引入旋转矩阵自身的约束【须满足$\mathbf{R}{\mathbf{R}}^T=\mathbf{I}$ 且 $det(\mathbf{R}$) = 1】

如何求解这个最小二乘问题？

6.1.3 例子：批量状态估计
下面的 Q 和 R 似乎反了。

对于一辆沿 $x$ 轴前进 或 后退的汽车
设 ${\mathbf{u}}_k$ 为输入，${\mathbf{w}}_k$ 为噪声，取时间为 $k=1,2,3$
运动方程为：${\mathbf{x}}_k={\mathbf{x}}_{k-1}+{\mathbf{u}}_k+{\mathbf{w}}_k$，${\mathbf{w}}_k\sim \mathcal{N}(0,{\mathbf{Q}}_k)$
设 ${\mathbf{z}}_k$ 为对汽车位置的测量
观测方程为：${\mathbf{z}}_k={\mathbf{x}}_k+{\mathbf{n}}_k$，${\mathbf{n}}_k\sim \mathcal{N}(0,{\mathbf{R}}_k)$

根据已有的 $\mathbf{v}$，$\mathbf{y}$ 进行状态估计

设初始状态 ${\mathbf{x}}_0$ 已知

推导 批量状态 的最大似然估计
令批量状态变量为 $\mathbf{x}=[{\mathbf{x}}_0,{\mathbf{x}}_1,{\mathbf{x}}_2,{\mathbf{x}}_3{]}^T$
令批量观测为 $\mathbf{z}=[{\mathbf{z}}_1,{\mathbf{z}}_2,{\mathbf{z}}_3{]}^T$
令批量输入变量为 $\mathbf{u}=[{\mathbf{u}}_1,{\mathbf{u}}_2,{\mathbf{u}}_3{]}^T$
最大似然估计为：
$$\begin{array}{rl}{\mathbf{x}}_{\mathrm{map}}^{\ast }& =\arg \max P(\mathbf{x}|\mathbf{u},\mathbf{z})\\ & =\arg \max P(\mathbf{u},\mathbf{z}|\mathbf{x})\\ & =\prod _{k=1}^{3}P({\mathbf{u}}_k|{\mathbf{x}}_{k-1},{\mathbf{x}}_k)\prod _{k=1}^{3}P({\mathbf{z}}_k|{\mathbf{x}}_k)\end{array}$$
其中
$P({\mathbf{u}}_k|{\mathbf{x}}_{k-1},{\mathbf{x}}_k)=\mathcal{N}({\mathbf{x}}_k-{\mathbf{x}}_{k-1},{\mathbf{Q}}_k)$
$P({\mathbf{z}}_k|{\mathbf{x}}_k)=\mathcal{N}({\mathbf{x}}_k,{\mathbf{R}}_k)$
构建误差变量：
$$\begin{array}{rl}{\mathbf{e}}_{\mathbf{u},k}& ={\mathbf{x}}_k-{\mathbf{x}}_{k-1}-{\mathbf{u}}_k\\ {\mathbf{e}}_{\mathbf{z},k}& ={\mathbf{z}}_k-{\mathbf{x}}_k\end{array}$$
最小二乘的目标函数为：
$$\min \sum _{k=1}^3{\mathbf{e}}_{\mathbf{u},k}^T{\mathbf{Q}}_k^{-1}{\mathbf{e}}_{\mathbf{u},k}+\sum _{k=1}^3{\mathbf{e}}_{\mathbf{z},k}^T{\mathbf{R}}_k^{-1}{\mathbf{e}}_{\mathbf{z},k}$$
——————
令 $\mathbf{y}=[\mathbf{u},\mathbf{z}{]}^T$
$$\mathbf{y}-\mathbf{Hx}=\mathbf{e}\sim \mathcal{N}(0,\mathbf{\Sigma })$$

$\Sigma =\mathrm{diag}({\mathbf{Q}}_1,{\mathbf{Q}}_2,{\mathbf{Q}}_3,{\mathbf{R}}_1,{\mathbf{R}}_2,{\mathbf{R}}_3)$
$${\mathbf{x}}_{\mathrm{map}}^{\ast }=\arg \max {\mathbf{e}}^T{\mathbf{\Sigma }}^{-1}\mathbf{e}$$
有唯一解：$${\mathbf{x}}_{\mathrm{map}}^{\ast }=({\mathbf{H}}^T{\mathbf{\Sigma }}^{-1}\mathbf{H}{)}^{-1}{\mathbf{H}}^T{\mathbf{\Sigma }}^{-1}\mathbf{y}$$

寻找下降增量$\Delta {\mathbf{x}}_k$的 4 种方法

6.2 非线性最小二乘

求解一个简单的最小二乘问题：
$$\underset{\mathbf{x}}{\min }F(\mathbf{x})=\frac{1}{2}||f(\mathbf{x})|{|}_2^2$$

$$\frac{dF}{d\mathbf{x}}=0$$
得到 导数为0处的极值，可能为极大、极小或鞍点
不足： 需要知道目标函数的全局性质；求解不易
——> 迭代 【求解导数为0 ——> 不断寻找下降增量$\Delta {\mathbf{x}}_k$】

————————

如何寻找这个增量$\Delta {\mathbf{x}}_k$？
方法：一阶梯度或二阶梯度法(最速下降法、牛顿法)、高斯牛顿法、列文伯格—马夸尔特方法

6.2.1 一阶和二阶梯度法(最速下降法、牛顿法)

将目标函数在 ${\mathbf{x}}_k$ 附近进行泰勒展开
$$F({\mathbf{x}}_k+\Delta {\mathbf{x}}_k)\approx F({\mathbf{x}}_k)+\mathbf{J}({\mathbf{x}}_k{)}^T\Delta {\mathbf{x}}_k+\frac{1}{2}\Delta {\mathbf{x}}_k^T\mathbf{H}({\mathbf{x}}_k)\Delta {\mathbf{x}}_k$$

$\mathbf{J}({\mathbf{x}}_k)$：$F({\mathbf{x}}_k)$关于\bm{x} 的一阶导数 【梯度、雅可比(Jacobian)矩阵)】
$\mathbf{H}$： 二阶导数 【海塞(Hessian)矩阵】

一阶梯度或二阶梯度法：仅保留 泰勒展开 的一阶或二阶项
最速下降法： 取增量为一阶梯度的反向【$\Delta {\mathbf{x}}^{\ast }=-\mathbf{J}({\mathbf{x}}_k)$】，再指定一个步长 $\lambda$
——————
保留二阶梯度信息
$\Delta {\mathbf{x}}^{\ast }=\arg \min (F(\mathbf{x})+\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}+\frac{1}{2}\Delta {\mathbf{x}}^T\mathbf{H}\Delta \mathbf{x})$

$\mathbf{J}+\mathbf{H}\Delta \mathbf{x}=0$

$\mathbf{H}\Delta \mathbf{x}=-\mathbf{J}$ 【牛顿法】

——————

一阶二阶梯度法

• 直观，把函数在迭代点附近进行泰勒展开，并针对更新量做最小化即可。
• 用一个一次或二次的函数近似原函数，用近似函数的最小值来猜测原函数的极小值。

最速下降法： 过于贪心，容易走出锯齿路线，增加迭代次数
牛顿法

• 需要计算目标函数的 $\mathbf{H}$ 矩阵，规模大时计算困难

————————
对于最小二乘问题，更实用的方法： 高斯牛顿法 和 列文伯格—马夸尔特方法。

6.2.2 高斯牛顿法

思想：将 $f(\mathbf{x})$ 进行一阶的泰勒展开。

牛顿法 展开的是 $F(\mathbf{x})$
$F(\mathbf{x}+\Delta \mathbf{x})\approx F(\mathbf{x})+\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}+\frac{1}{2}\Delta {\mathbf{x}}^T\mathbf{H}\Delta \mathbf{x}$

$$f(\mathbf{x}+\Delta \mathbf{x})\approx f(\mathbf{x})+\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}$$
其中$\mathbf{J}(\mathbf{x}{)}^T$ 为 $f(\mathbf{x})$ 关于 $\mathbf{x}$ 的导数，为 $n\times 1$ 的列向量。

寻找增量 $\Delta \mathbf{x}$ ，使得 $||f(\mathbf{x}+\Delta \mathbf{x})|{|}^2$ 达到最小。
——> 解一个线性的最小二乘问题：
$$\Delta {\mathbf{x}}^{\ast }=\arg \underset{\Delta \mathbf{x}}{\min }\frac{1}{2}||f(\mathbf{x})+\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}|{|}^2$$

将上述目标函数对 $\Delta \mathbf{x}$ 求导，并令导数为零。

$$\begin{array}{rl}\frac{1}{2}||f(\mathbf{x})+\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}|{|}^2& =\frac{1}{2}(f(\mathbf{x})+\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}{)}^T(f(\mathbf{x})+\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x})\\ & =\frac{1}{2}(f(\mathbf{x}{)}^T+\Delta {\mathbf{x}}^T\mathbf{J}(\mathbf{x}))(f(\mathbf{x})+\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x})\\ & =\frac{1}{2}(||f(\mathbf{x})|{|}^2+f(\mathbf{x}{)}^T\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}+\Delta {\mathbf{x}}^T\mathbf{J}(\mathbf{x})f(\mathbf{x})+\Delta {\mathbf{x}}^T\mathbf{J}(\mathbf{x})\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x})\end{array}$$
求导

一些向量求导公式：
注意当只有一边时，应假设另一边为 $\mathbf{I}$

$$(f(\mathbf{x}{)}^T\mathbf{J}(\mathbf{x}{)}^T{)}^T+\mathbf{J}(\mathbf{x})f(\mathbf{x})+(\mathbf{J}(\mathbf{x})\mathbf{J}(\mathbf{x}{)}^T{)}^T\Delta \mathbf{x}+\mathbf{J}(\mathbf{x})\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}=0$$

$$\mathbf{J}(\mathbf{x})f(\mathbf{x})+\mathbf{J}(\mathbf{x})\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}=0$$
$$\mathbf{J}(\mathbf{x})\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}=-\mathbf{J}(\mathbf{x})f(\mathbf{x})$$

增量方程【高斯牛顿方程】【正规方程(Normal equation)】 $\mathbf{H}\Delta \mathbf{x}=\mathbf{g}$

高斯牛顿法 用 ${\mathbf{JJ}}^T$ 作为 牛顿法 中 二阶 Hessian 矩阵的近似。

不足：
1、为了求解 增量方程，需要求解 ${\mathbf{H}}^{-1}$ ，需要矩阵 $\mathbf{H}$ 可逆，可能出现 ${\mathbf{JJ}}^T$ 为奇异矩阵的情况，导致算法不收敛。
2、若是步长 $\Delta \mathbf{x}$ 太大，会导致局部近似式 (6.30) 不够准确。

• 即 高斯牛顿法 采用的近似二阶泰勒展开只能在 展开点附近 有较好的 近似效果。

6.2.3 列文伯格-马夸尔特方法 【阻尼牛顿法】【信赖区域法】

修正了 高斯牛顿法的上述不足，但收敛速度比高斯牛顿法慢，被称为阻尼牛顿法(Damped Newton Method)

高斯牛顿法 采用的近似二阶泰勒展开只能在 展开点附近 有较好的 近似效果。
——> 给 $\Delta \mathbf{x}$ 添加一个范围 信赖区域

如何确定这个信赖区域的范围？
比较 近似模型 和 实际函数 之间的差异



带 不等式约束 的优化问题，用拉格朗日乘子 把 约束项 放到目标函数中，构成拉格朗日函数：

当参数 $\lambda$ 较小，接近高斯牛顿方法
当参数 $\lambda$ 较大，更接近一阶梯度下降法(最速下降)

问题性质较好——> 高斯牛顿法
问题接近病态——> 列文伯格-马夸尔特方法

——————
非线性优化的通病：变量的初始值对于优化效果影响较大，容易陷入局部极小值。
视觉SLAM： 用ICP、PnP 之类的算法 提供优化初始值。

6.3 实践

6.3.1 手写高斯牛顿法 【Code】

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)project(ch6)set(CMAKE_CXX_STANDARD 17)  ## 新版 g2o库 需要
set(CMAKE_BUILD_TYPE Release)
set(CMAKE_CXX_FLAGS "-std=c++14 -O3")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)  # g2o部分  若是 新版g2o库   需要修改 源代码  
target_link_libraries(g2oCurveFitting ${OpenCV_LIBS} ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY})

gaussNewton.cpp

#include <iostream>
#include <chrono>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>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 * 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();             // 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);  // de/daJ[1] = -xi * exp(ae * xi * xi + be * xi + ce);  // de/dbJ[2] = -exp(ae * xi * xi + be * xi + ce);  // de/dcH += 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 << ", break." << endl;break;}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() << " seconds. " << endl;cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;return 0;
}

6.3.2 谷歌的优化库 Ceres 【最小二乘问题求解库】【Code】

ceresCurveFitting.cpp

//
// 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维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 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));}double abc[3] = {ae, be, ce};// 构建最小二乘问题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_NORMAL_CHOLESKY;  // 增量方程如何求解options.minimizer_progress_to_stdout = true;   // 输出到coutceres::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;
}

6.3.3 g2o(General Graphic Optimization) 【Code】

g2o(General Graphic Optimization, ${\mathrm{G}}^2\mathrm{O}$)

结点： 优化变量
边： 误差项

基于 图优化

代码修改参考链接

// 构建图优化，先设定g2o/*typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;  // 每个误差项优化变量维度为3，误差值维度为1typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型*/std::unique_ptr<g2o::BlockSolverX::LinearSolverType> linearSolver (new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>());// 梯度下降方法，可以从GN, LM, DogLeg 中选/*auto solver = new g2o::OptimizationAlgorithmGaussNewton(g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));*/std::unique_ptr<g2o::BlockSolverX> solver_ptr (new g2o::BlockSolverX(std::move(linearSolver)));g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(std::move(solver_ptr));g2o::SparseOptimizer optimizer;     // 图模型optimizer.setAlgorithm(solver);   // 设置求解器optimizer.setVerbose(true);       // 打开调试输出

/usr/local/include/g2o/stuff/tuple_tools.h:41:46: error: ‘tuple_size_v’ is not a member of ‘std’; did you mean ‘tuple_size’?41 |       f, t, i, std::make_index_sequence<std::tuple_size_v<std::decay_t<T>>>());

解决办法：
在 CMakeLists.txt 中添加 set(CMAKE_CXX_STANDARD 17)
——————————————
g2oCurveFitting.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_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));}// 构建图优化，先设定g2o   typedef  别名替换/*typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;  // 每个误差项优化变量维度为3，误差值维度为1typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型*/std::unique_ptr<g2o::BlockSolverX::LinearSolverType> linearSolver (new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>());// 梯度下降方法，可以从GN, LM, DogLeg 中选/*auto solver = new g2o::OptimizationAlgorithmGaussNewton(g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));*/std::unique_ptr<g2o::BlockSolverX> solver_ptr (new g2o::BlockSolverX(std::move(linearSolver)));g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(std::move(solver_ptr));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;
}

每次的运行时间都不太一样
直觉上:

——————
6.4 小结
SLAM中经常遇到的一种非线性优化问题：由许多个误差项平方和组成的最小二乘问题。
实践部分： 手写高斯牛顿法、Ceres 和 g2o 库 求解同一个曲线拟合问题。

g2o 提供了大量现成的顶点和边，非常便于相机位姿估计问题。

习题

√ 题1

1、 证明线性方程 $\mathbf{Ax}=\mathbf{b}$ 当系数矩阵 $\mathbf{A}$ 超定时，最小二乘解为 $\mathbf{x}=({\mathbf{A}}^T\mathbf{A}{)}^{-1}{\mathbf{A}}^T\mathbf{b}$。

当 方程数 多于 未知数个数时，也就是说对应系数矩阵的行数大于列数【系数矩阵 $\mathbf{A}$ 超定】，这样的方程组称为超定方程组。
方程个数比未知数个数多，不同方程之间会发生"冲突"，导致整个方程组没有精确解。但可以设定一个误差标准，找到一个让这个误差标准值最小的近似解。

所以本题可以理解为：针对无精确解的方程组 $\mathbf{Ax}=\mathbf{b}$ ，利用最小二乘误差标准求解其近似值。

求解最小二乘问题：
$${\mathbf{x}}^{\ast }=\arg \underset{\mathbf{x}}{\min }\frac{1}{2}||f(\mathbf{x})|{|}^2$$
本题中
$$\begin{array}{rl}\frac{1}{2}||f(\mathbf{x})|{|}^2& =\frac{1}{2}(\mathbf{Ax}-\mathbf{b}{)}^T(\mathbf{Ax}-\mathbf{b})\\ & =\frac{1}{2}({\mathbf{x}}^T{\mathbf{A}}^T-{\mathbf{b}}^T)(\mathbf{Ax}-\mathbf{b})\\ & =\frac{1}{2}({\mathbf{x}}^T{\mathbf{A}}^T\mathbf{Ax}-{\mathbf{b}}^T\mathbf{Ax}-{\mathbf{x}}^T{\mathbf{A}}^T\mathbf{b}+{\mathbf{b}}^T\mathbf{b})\end{array}$$
上式对 $\mathbf{x}$ 求导并令其为0
$$({\mathbf{A}}^T\mathbf{A}{)}^T\mathbf{x}+{\mathbf{A}}^T\mathbf{Ax}-({\mathbf{b}}^T\mathbf{A}{)}^T-{\mathbf{A}}^T\mathbf{b}=0$$
$${\mathbf{A}}^T\mathbf{Ax}={\mathbf{A}}^T\mathbf{b}$$
$$\mathbf{x}=({\mathbf{A}}^T\mathbf{A}{)}^{-1}{\mathbf{A}}^T\mathbf{b}$$
证毕。

其它解法链接

题2

2、调研最速下降法、牛顿法、高斯牛顿法 和 列文伯格-马夸尔特方法各有什么优缺点。除了我们举的Ceres 库和g2o 库，还有哪些常用的优化库？你可能会找到一些MATLAB 上的库。

优化库：
GTSAM（https://gtsam.org/） GTSAM是一个开源的因子图优化库，使用C++实现。它提供了一系列的因子和变量类型，包括了视觉、IMU、GNSS和路标等因子和位姿、速度、IMU偏差和路标等变量。GTSAM还提供了多种优化器，并支持基于纯图的优化和基于混合图的优化。GTSAM有很多应用案例和文档，使用起来相对较易。

题3

3、为什么高斯牛顿法的增量方程系数矩阵可能不正定？不正定有什么几何含义？为什么在这种情况下解就不稳定了？

题4

4、DogLeg 是什么？它与 高斯牛顿法 和 列文伯格-马夸尔特方法 有何异同？

• 整理PPT

参考链接-P18

Dogleg属于Trust Region优化方法，即用置信域的方法在最速下降法和高斯牛顿法之间进行切换（将二者的搜索步长及方向转化为向量，两个向量进行叠加得到新的方向和置信域内的步长），相当于是一种加权求解。

——————————————————————————————————

LaTex

$\sim$

$\sim$

 $\mathcal{N}$

$\mathcal{N}$

$\iff$

$⟺$

$\propto$

$\propto$

$\Sigma$

$\Sigma$