非线性优化问题基本形式概述

非线性优化问题以及在视觉SLAM中的应用

1.0 最小二乘基础概念

  • 定义

\quad 找到一个 n 维的变量 x ∗ ∈ R n \mathbf{x}^{*} \in \mathbb{R}^{n} xRn , 使得损失函数 F ( x ) F(\mathbf{x}) F(x) 取局部最小值:

F ( x ) = 1 2 ∑ i = 1 m ( f i ( x ) ) 2 F(\mathbf{x})=\frac{1}{2} \sum_{i=1}^{m}\left(f_{i}(\mathbf{x})\right)^{2} F(x)=21i=1m(fi(x))2
\quad 其中 f i f_{i} fi 是残差函数, 比如测量值和预测值之间的差, 且有 m ≥ n m \geq n mn 。 部最小值指对任意 ∥ x − x ∗ ∥ < δ \left\|\mathbf{x}-\mathbf{x}^{*}\right\|<\delta xx<δ F ( x ∗ ) ≤ F ( x ) F\left(\mathbf{x}^{*}\right) \leq F(\mathbf{x}) F(x)F(x)
\quad 损失函数泰勒展开,假设损失函数 F ( x ) F(\mathbf{x}) F(x) 是可导并且平滑的, 因此, 二阶泰勒展开:
F ( x + Δ x ) = F ( x ) + J Δ x + 1 2 Δ x ⊤ H Δ x + O ( ∥ Δ x ∥ 3 ) F(\mathbf{x}+\Delta \mathbf{x})=F(\mathbf{x})+\mathbf{J} \Delta \mathbf{x}+\frac{1}{2} \Delta \mathbf{x}^{\top} \mathbf{H} \Delta \mathbf{x}+O\left(\|\Delta \mathbf{x}\|^{3}\right) F(x+Δx)=F(x)+JΔx+21ΔxHΔx+O(∥Δx3)

这里要着重注意一下,这里的 J \mathbf{J} J H \mathbf{H} H 都是每一个残差项的雅可比堆叠(计算)而来,实际上对于初学者来说并不直观,后面我们会以一个曲线拟合和 B A BA BA 问题来详细分析一下如何通过连加来推算到 J \mathbf{J} J H \mathbf{H} H

\quad 其中 J \mathbf{J} J H \mathbf{H} H 分别为损失函数 F F F 对变量 x x x 的一阶导和二阶导矩阵,也就是我们通常所说的雅可比矩阵和海塞矩阵。(这里的 x \mathbf{x} x 包含了所有待优化的变量,在视觉SLAM问题中,一般是相机的 Pose 和已经三角化的点的坐标或者逆深度,且由于相机一般能观测到的3D点的个数是有限的,因此其雅可比矩阵也就是稀疏的,只有两个地方的雅可比求导是不为0的,参考14讲P247,那么 J i , j T J i , j J_{i,j}^TJ_{i,j} Ji,jTJi,j,则只有四个地方是不为0的)。

  • 损失函数泰勒展开的性质

\quad 忽略泰勒展开的高阶项,损失函数变成了二次函数,可以轻易得到如下性质:

  1. 如果在点 x s x_s xs 处有导数为 0 0 0 ,则称这个点为稳定点。
  2. 在点 x s x_s xs 处对应的 Hessian 为 H \mathbf{H} H
    • 如果是正定矩阵,即它的特征值都大于 0 0 0,则在 x s x_s xs 处有 F ( x ) F (x) F(x) 为局部最小值。
    • 如果是复定矩阵,即它的特征值都小于 0 0 0,则在 x s x_s xs 处有 F ( x ) F (x) F(x) 为局部最大值。
    • 如果是不定矩阵,即它的特征值大于 0 0 0 也有小于 0 0 0 的,则 x s x_s xs 处为鞍点。
  • 求解方法主要有以下两种:
    • 直接求解:线性最小二乘(这里不再赘述,为线性代数的内容,超定方程的通解为 x = ( A T A ) − 1 A   b x=(A^TA)^{-1}A\ b x=(ATA)1A b
    • 迭代下降法:适用于线性和非线性最小二乘

2.0 迭代下降求解方式

  • 迭代法初衷

    找到一个下降方向使得损失函数随着 x x x 的迭代逐渐减少直到 x ∗ x^* x
    F ( x k + 1 ) < F ( x k ) F(x_{k+1})F(xk+1)<F(xk)
    分两个步骤;第一,找到下降方向单位向量 d d d ,第二,确定下降的步长 a a a

    假设 a a a 足够的小,又因为 d d d 为单位向量,因此可以将 a d ad ad 看作是一个微小的变化量 △ x \triangle{x} x,我们可以对损失函数进行一阶泰勒展开:
    F ( x + a   d ) ≈ F ( x ) + a J d F(\mathbf{x}+a \ \mathbf{d}) \approx F(\mathbf{x}) + a\mathbf{J}\mathbf{d} F(x+a d)F(x)+aJd
    只需要寻找下降方向,满足:
    J d < 0 \mathbf{Jd}<0 Jd<0
    通过 line search 的方法找到下降的步长: a ∗ = a r g m i n a > 0 [ F ( x + a d ) ] a^*=argmin_{a>0} [F(x+ad)] a=argmina>0[F(x+ad)]

2.1 最速下降法: 适用于迭代的开始阶段

适用于迭代的开始阶段

\quad 从下降方向的条件(单位向量)可以知道: J d = ∣ ∣ J ∣ ∣ c o s θ \mathbf{Jd=||J||}cos\theta Jd=∣∣J∣∣cosθ ,其中 θ \theta θ 表示的是下降方向和梯度方向的夹角. 当 θ = π \theta = \pi θ=π 有:

这里为什么能写成向量的内积运算,笔者在刚开始看起来还认为是两个矩阵相乘法,却直接写成了内积运算,仔细思索发现 d d d 其实上是一个和 x x x 相同维度的单位向量,其纬度为 n × 1 n\times 1 n×1 ,而雅可比矩阵

d = − J T ∣ ∣ J ∣ ∣ \mathbf{d=\frac{-J^T}{||J||}} d=∣∣J∣∣JT

\quad 梯度的负方向为最速下降方向。缺点:最优值附近震荡,收敛慢。

2.2 牛顿法: 适用于最优值附近

在局部最优点 x ∗ x^∗ x 附近,如果 x + ∆ x x + ∆x x+x 是最优解,则损失函数对 ∆ x ∆x x 的导数等于 0 0 0,对公式 (2) 取一阶导有:
∂ ∂ Δ x ( F ( x ) + J Δ x + 1 2 Δ x ⊤ H Δ x ) = J T + H Δ x = 0 \begin{align*} \frac{\partial}{\partial \Delta x}\left (F(\mathbf{x})+\mathbf{J} \Delta \mathbf{x}+\frac{1}{2} \Delta \mathbf{x}^{\top} \mathbf{H} \Delta \mathbf{x} \right) &=\mathbf{J^T} + \mathbf{H}\Delta x =0 \end{align*} Δx(F(x)+JΔx+21ΔxHΔx)=JT+HΔx=0
得到: ∆ x = − H − 1 J T ∆x = -\mathbf{H^{-1}J^T} x=H1JT 。缺点:二阶导矩阵计算复杂。

这里我们其实既可以看作是多个残差的分量相加后组成的 H H H,也可以看作是每个残差单独的 H H H

2.3 阻尼法:防止 Δ x \Delta x Δx 的模过大

将损失函数的二阶泰勒展开记作:
F ( x + Δ x ) ≈ L ( Δ x ) = F ( x ) + J Δ x + 1 2 Δ x ⊤ H Δ x F(\mathbf{x}+\Delta x)\approx L(\Delta x) = F(\mathbf{x})+\mathbf{J} \Delta \mathbf{x}+\frac{1}{2} \Delta \mathbf{x}^{\top} \mathbf{H} \Delta \mathbf{x} F(x+Δx)L(Δx)=F(x)+JΔx+21ΔxHΔx
求以下函数的最小化:
Δ x = a r g   m i n Δ x { L ( Δ x ) + 1 2 μ Δ x T Δ x } \Delta x = arg \ \underset{\Delta x}{min} \left \{ L \left (\Delta x\right ) + \frac{1}{2}\mu \Delta x^T\Delta x \right \} Δx=arg Δxmin{L(Δx)+21μΔxTΔx}
其中, μ ≥ 0 μ ≥ 0 μ0 为阻尼因子, $ \frac{1}{2}\mu \Delta x^T\Delta x $是惩罚项。对新的损失函数求一阶导,并令其等于 0 0 0 有:
L ‘ ( Δ x ) + μ Δ x = 0 ( H + μ I ) Δ x = − J T \mathbf{L^`(\Delta x)} + \mu \mathbf{\Delta x} = 0 \\ (\mathbf{H}+\mu\mathbf{I})\Delta x = -\mathbf{J^T} L(Δx)+μΔx=0(H+μI)Δx=JT

2.4 Gauss-Newton 和 LM

残差函数 f ( x ) f(x) f(x) 为非线性函数,对其进行一阶泰勒近似有:
f ( x + Δ x ) ≈ ℓ ( Δ x ) ≡ f ( x ) + J Δ x f(x+\Delta x)\approx \ell (\Delta x) \equiv f(x)+J\Delta x f(x+Δx)(Δx)f(x)+JΔx
带入损失函数:
F ( x + Δ x ) ≈ L ( Δ x ) ≡ 1 2 ℓ ( Δ x ) ⊤ ℓ ( Δ x ) = 1 2 f ⊤ f + Δ x ⊤ J ⊤ f + 1 2 Δ x ⊤ J ⊤ J Δ x = F ( x ) + Δ x ⊤ J ⊤ f + 1 2 Δ x ⊤ J ⊤ J Δ x \begin{aligned} F(\mathbf{x}+\Delta \mathbf{x}) \approx L(\Delta \mathbf{x}) & \equiv \frac{1}{2} \ell(\Delta \mathbf{x})^{\top} \ell(\Delta \mathbf{x}) \\ & =\frac{1}{2} \mathbf{f}^{\top} \mathbf{f}+\Delta \mathbf{x}^{\top} \mathbf{J}^{\top} \mathbf{f}+\frac{1}{2} \Delta \mathbf{x}^{\top} \mathbf{J}^{\top} \mathbf{J} \Delta \mathbf{x} \\ & =F(\mathbf{x})+\Delta \mathbf{x}^{\top} \mathbf{J}^{\top} \mathbf{f}+\frac{1}{2} \Delta \mathbf{x}^{\top} \mathbf{J}^{\top} \mathbf{J} \Delta \mathbf{x} \end{aligned} F(x+Δx)L(Δx)21(Δx)(Δx)=21ff+ΔxJf+21ΔxJJΔx=F(x)+ΔxJf+21ΔxJJΔx
这里我们假设暂时只关注其中的一项(其实也可以是所有损失项的叠加,最终形式是一样的)。在 x x x 处进行的泰勒展开,则认为 x x x 是已知的,现在的损失函数是一个关于 Δ x \Delta x Δx 的函数,其最小值,则令关于 Δ x \Delta x Δx 的导数为 0 0 0 即可。可以得到:
( J T J ) Δ x g n = − J T f \mathbf{(J^T J)}\Delta x_{gn}=-\mathbf{J^T f} (JTJ)Δxgn=JTf
上面这个形式就是我们在论文或者各种SLAM问题中经常见到的形式了, H Δ x = b \mathbf{H \Delta x =b} HΔx=b,也叫做 normal equation

曲线拟合理解

现在我们通过非线性最小二乘来进行线性拟合实验,将理论应用于实际中去。假设曲线方程为:
y = exp ⁡ ( a x 2 + b x + c ) y=\exp (ax^2+bx+c) y=exp(ax2+bx+c)
其中设 a = 1 , b = 2 , c = 3 a=1,b=2,c=3 a=1,b=2,c=3

现在加入噪声项生成带有高斯分布的噪声数据,当然不是高斯分布的数据也是可以的,但是在自己实验的时候尽量不要出现外点数据,因为我们并没有处理外点数据的策略。则生成数据的方程为:
y = exp ⁡ ( a x 2 + b x + c ) + w y=\exp (ax^2+bx+c) + w y=exp(ax2+bx+c)+w
其中 w w w 为符合高斯分布的噪声数据。

通过如下程序生成观测数据:

double ar = 1.0, br = 2.0, cr = 1.0;         // 真实参数值
int N = 100;                                 					// 数据点
double w_sigma = 1.0;                      	 	 // 噪声Sigma值
vector 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));
}

接下来我们关心雅可比如何计算,误差项 f i ( a , b , c ) f_i(a,b,c) fi(a,b,c) 可以写成如下形式:
f i ( a , b , c ) = y i − e x p ( a e x i 2 + b e x i + c e ) f_i(a,b,c)=y_i-exp(a_ex_i^2+b_ex_i+c_e) fi(a,b,c)=yiexp(aexi2+bexi+ce)
我们知道这两项相减是绝对不可能相等的,因为在生成数据的时候加入了高斯噪声。我们这里有 N N N 个观测,即 i ∈ ( 1 − 100 ) i\in (1-100) i(1100),我们将其写成连加的形式
F ( X ) = ∑ i = 1 N ( y i − e x p ( a e x i 2 + b e x i + c e ) ) 2 F(X)=\sum_{i=1}^{N}\left (y_i-exp(a_ex_i^2+b_ex_i+c_e) \right)^2 F(X)=i=1N(yiexp(aexi2+bexi+ce))2
该式中右边就是残差项的具体形式,我们对其进行平方,防止负的残差和正的残差抵消的情况,前面我们已经说过可以将残差项通过一阶泰勒展开进行近似,然后进行平方得到每一个残差项的具体形式:

f ( x + Δ x ) ≈ f ( x ) + J ( x ) Δ x f(x+\Delta x)\approx f(x)+J(x)\Delta x f(x+Δx)f(x)+J(x)Δx

1 2 ∥ f ( x ) + J ( x ) Δ x ∥ 2 = 1 2 ( f ( x ) + J ( x ) Δ x ) T ( f ( x ) + J ( x ) Δ x ) = 1 2 ( ∥ f ( x ) ∥ 2 2 + 2 f ( x ) T J ( x ) Δ x + Δ x T J ( x ) T J ( x ) Δ x ) = Ω ( Δ x ) \begin{aligned} \frac{1}{2}\|f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x}\|^{2} & =\frac{1}{2}(f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x})^{T}(f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x}) \\ & =\frac{1}{2}\left(\|f(\boldsymbol{x})\|_{2}^{2}+2 f(\boldsymbol{x})^{T} \boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x}+\Delta \boldsymbol{x}^{T} \boldsymbol{J}(\boldsymbol{x})^{T} \boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x}\right) \\ &=\Omega(\Delta x) \end{aligned} 21f(x)+J(x)Δx2=21(f(x)+J(x)Δx)T(f(x)+J(x)Δx)=21(f(x)22+2f(x)TJ(x)Δx+ΔxTJ(x)TJ(x)Δx)=Ω(Δx)

此时由于某时刻的观测已知,因此误差项是一个关于 Δ x \Delta x Δx 的二次函数,求该项的最小值只要让关于 Δ x \Delta x Δx 的导数为 0 0 0 即可。求导后可得:
2 J ( x ) T f ( x ) + 2 J ( x ) T J ( x ) Δ x = 0 J ( x ) T J ( x ) Δ x = − J ( x ) T f ( x ) \begin{array}{l} 2 \boldsymbol{J}(\boldsymbol{x})^{T} f(\boldsymbol{x})+2 \boldsymbol{J}(\boldsymbol{x})^{T} \boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x}=\mathbf{0} \\ \boldsymbol{J}(\boldsymbol{x})^{T} \boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x}=-\boldsymbol{J}(\boldsymbol{x})^{T} f(\boldsymbol{x}) \end{array} 2J(x)Tf(x)+2J(x)TJ(x)Δx=0J(x)TJ(x)Δx=J(x)Tf(x)

这里我们简单的记:
J ( x ) T f ( x ) = η J ( x ) T J ( x ) Δ x = H Δ x \boldsymbol{J}(\boldsymbol{x})^{T} f(\boldsymbol{x}) = \mathbf{\eta}\\ \boldsymbol{J}(\boldsymbol{x})^{T} \boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x}=\mathbf{H\Delta x} J(x)Tf(x)=ηJ(x)TJ(x)Δx=HΔx

即我们常见的形式:

读者要注意到这里的 b b b 其实就是上面的 − η -\eta η

H Δ x = b \mathbf{H\Delta x=b} HΔx=b
这里我们假设残差项记为 e i \mathbf{e_i} ei 一共有 N N N 个观测,则有 N N N 个残差项。
F ( X ) = e 1 2 + e 2 2 + e 3 2 + e 4 2 + e 5 2 + ⋯ + e N 2 F(X)=\mathbf{e_1^2 + e_2^2+ e_3^2+ e_4^2+ e_5^2+ \dots + e_N^2} F(X)=e12+e22+e32+e42+e52++eN2
整个 F ( X ) F(X) F(X) 此时是关于待优化变量的函数,每一项分别用各自的一阶泰勒展开近似,注意这里的每一项由于观测的不同,每一项都是一个不同的函数表达式,但是优化变量都是一样的。得到如下结果:

1 2 ∥ f ( x ) + J ( x ) Δ x ∥ 2 = Ω ( Δ x ) \begin{aligned} \frac{1}{2}\|f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x}) \Delta \boldsymbol{x}\|^{2} &=\Omega(\Delta x) \end{aligned} 21f(x)+J(x)Δx2=Ω(Δx)

F ( X ) ≈ Ω ( Δ x ) 1 + Ω ( Δ x ) 2 + Ω ( Δ x ) 3 + Ω ( Δ x ) 4 ⋯ + Ω ( Δ x ) N F(X)\approx \Omega(\Delta x)_1 + \Omega(\Delta x)_2+ \Omega(\Delta x)_3+ \Omega(\Delta x)_4 \dots + \Omega(\Delta x)_N F(X)Ω(Δx)1+Ω(Δx)2+Ω(Δx)3+Ω(Δx)4+Ω(Δx)N

这里的 Δ x \Delta x Δx 是我们在使用基于迭代下降的方法中所选中的步长和方向,如果 F ( X ) F(X) F(X) Δ x \Delta x Δx 为某个值时取得极小值,则 Δ x \Delta x Δx无论是在任何一个方向加或者减函数值都会上升,此时这个点则为极小值点,这里的叙述不太数学化,但是大家联想一下极小值的定义,应该是可以理解的,当达到该条件后,那么该点关于 Δ x \Delta x Δx 的导数一定为 0 0 0 。所以对此时的 F ( X ) F(X) F(X)求导并让其等于 0 0 0 得到:
H 1 Δ x + η 1 + H 2 Δ x + η 2 + H 3 Δ x + η 3 ⋯ + H N Δ x + η N = 0 \mathbf{H_1\Delta x } + \mathbf{\eta_1} + \mathbf{H_2\Delta x } + \mathbf{\eta_2} + \mathbf{H_3\Delta x } + \mathbf{\eta_3} \dots + \mathbf{H_N\Delta x } + \mathbf{\eta_N} = \mathbf{0} H1Δx+η1+H2Δx+η2+H3Δx+η3+HNΔx+ηN=0
再将该式子变形,将关于 Δ x \Delta x Δx 的项都移动到左边,没有关于 Δ x \Delta x Δx 的移动到右边:
H 1 Δ x + H 2 Δ x + H 3 Δ x + ⋯ + H N Δ x = − η 1 − η 2 − η 3 ⋯ − η N \mathbf{H_1\Delta x } + \mathbf{H_2\Delta x } + \mathbf{H_3\Delta x } + \dots + \mathbf{H_N\Delta x }= - \mathbf{\eta_1} - \mathbf{\eta_2} - \mathbf{\eta_3} \dots - \mathbf{\eta_N} H1Δx+H2Δx+H3Δx++HNΔx=η1η2η3ηN
其实也就是:
H 1 Δ x + H 2 Δ x + H 3 Δ x + ⋯ + H N Δ x = b 1 + b 2 + b 3 ⋯ + b N \mathbf{H_1\Delta x } + \mathbf{H_2\Delta x } + \mathbf{H_3\Delta x } + \dots + \mathbf{H_N\Delta x }= \mathbf{b_1} + \mathbf{b_2} + \mathbf{b_3} \dots + \mathbf{b_N} H1Δx+H2Δx+H3Δx++HNΔx=b1+b2+b3+bN
写成连加的形式:
Δ x ∑ i = 1 N H = ∑ i = 1 N b \Delta x\sum_{i=1}^{N}H = \sum_{i=1}^{N}b Δxi=1NH=i=1Nb
这里我们就通过每一项的一个具体形式来推倒出最后的 H 和 b 是怎么来的了。也就是我们经常在程序中见到的 += 操作的原理:

H +=  J * J.transpose();
b += -J * error;

我们再次回到曲线拟合的题目中去,待优化的变量就三个 a , b , c a,b,c a,b,c 则每一个残差项都含有这三个参数,我们称其雅可比为稠密的(虽然只有三个参数,视觉BA问题中由于相机观测的特殊性,其雅可比矩阵是稀疏的),对每一个残差向分别求雅可比,然后求和得到最终的 H H H b b b ,然后求解一次 Δ x \Delta x Δx ,Step 一次,根据判断条件选择是否继续进行迭代。每一个残差项对于 Δ x \Delta x Δx 的雅可比为
J [ 0 ] a = − x i 2 exp ⁡ ( a e x i 2 − b e x i − c ) J [ 1 ] b = − x i exp ⁡ ( a e x i 2 − b e x i − c ) J [ 2 ] c = − exp ⁡ ( a e x i 2 − b e x i − c ) J[0]_a = -x_i^2 \exp(a_ex_i^2-b_ex_i-c) \\ J[1]_b = -x_i \exp(a_ex_i^2-b_ex_i-c) \\ J[2]_c = -\exp(a_ex_i^2-b_ex_i-c) \\ J[0]a=xi2exp(aexi2bexic)J[1]b=xiexp(aexi2bexic)J[2]c=exp(aexi2bexic)
得到了雅可比,那么剩下的就是迭代求解即可,完整代码如下,来自14讲配套代码:

#include 
#include 
#include 
#include 
#include 

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 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和上一次迭代的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

      H +=  J * J.transpose();
      b += -J * error;

      cost += error * error;
    }

    // 求解线性方程 Hx=b
    Vector3d 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 time_used = chrono::duration_cast>(t2 - t1);
  cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

  cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
  return 0;
}

  • 运行结果如下:
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.00440302 seconds. 
estimated abc = 0.890912, 2.1719, 0.943629
  • 和真实结果对比,这里的准确度取决于我们噪声方差的大小
a a a b b b c c c
Estimate 0.890912 0.890912 0.890912 2.1719 2.1719 2.1719 0.943629 0.943629 0.943629
Real 1 1 1 2 2 2 1 1 1

下一节我们来讨论一下视觉SLAM中的非线性优化问题的具体形式,以及其 H H H b b b 的由来和构建方法。

你可能感兴趣的:(VIO,非线性优化,VIO,视觉SLAM)