文章目录
最小二乘法
下降策略: 高斯牛顿法、列文伯格-马夸尔特方法
Ceres库 和 g2o 库
如何在 有噪声的数据 中进行准确的 状态估计
——————
6.1 状态估计问题
运动方程和观测方程:
x k \bm{x}_k xk :相机的位姿, 可用SE(3) 描述
观测方程为 针孔相机模型
假设在 x k \bm{x}_k xk 处 对 路标 y j \bm{y}_j yj 进行了一次观测, 对应到图像上的 像素坐标 为 z k , j \bm{z}_{k, j} zk,j,
则观测方程可表示为: s z k , j = K ( R k y j + t k ) s\bm{z}_{k, j} = \bm{K}(\bm{R}_k\bm{y}_j+\bm{t}_k) szk,j=K(Rkyj+tk)
其中 K \bm{K} K 为相机内参,s为像素点的距离(??),也是 ( R k y j + t k ) (\bm{R}_k\bm{y}_j+\bm{t}_k) (Rkyj+tk) 的第三个分量。
数据 受 噪声 影响后的变化
在运动和观测方程中,通常假设两个噪声项 w k , v k , j \bm{w}_k, \bm{v}_{k,j} wk,vk,j 满足零均值的高斯分布:
w k ∼ N ( 0 , R k ) , v k ∼ N ( 0 , Q k , j ) \bm{w}_k \sim \mathcal{N}(\bm{0}, \bm{R}_k),\bm{v}_k \sim \mathcal{N}(\bm{0}, \bm{Q}_{k,j}) wk∼N(0,Rk),vk∼N(0,Qk,j)
状态估计问题: 通过带噪声的数据 z \bm{z} z 和 u \bm{u} u 推断位姿 x \bm{x} x 和 地图 y \bm{y} y (以及它们的概率分布)。
处理这个状态估计问题的两种方法:
1、增量/渐进(incremental)
【滤波器】
- 持有一个当前时刻的估计状态,然后用新的时刻来更新它。【仅关心当前时刻的状态估计 x k \bm{x}_k xk】
- 扩展卡尔曼滤波器
2、
批量(batch)
方法
- 把数据 “攒” 起来 一并处理。
- 可以在更大的范围达到最优化, 非实时
————————
折中办法
滑动窗口估计法
: 固定一些历史轨迹,仅对当前时刻附近的一些轨迹进行优化。
以非线性优化为主的批量优化方法
考虑从 1 到 N N N 的所有时刻,假设有 M M 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}
用不带下标的 u \bm{u} u 表示所有时刻的输入, z \bm{z} z 表示所有时刻的观测数据。
估计 机器人 状态 <==>
已知输入数据 u \bm{u} u 和观测数据 z \bm{z} z 的条件下,求状态 x , y \bm{x, y} x,y 的条件概率分布 P ( x , y ∣ z , u ) P(\bm{x},\bm{y}|\bm{z}, \bm{u}) P(x,y∣z,u)
————————
不知道控制输入,只有一张张图像,即只考虑观测方程带来的数据时,相当于估计 P ( x , y ∣ z ) P(\bm{x},\bm{y}|\bm{z}) P(x,y∣z) 的条件概率分布
SfM(Structure from Motion)问题
, 即如何从许多图像中重建三维空间结构。
估计 状态变量 的条件分布
根据 贝叶斯法则:
后验概率
P
(
x
,
y
∣
z
,
u
)
=
P
(
z
,
u
∣
x
,
y
)
P
(
x
,
y
)
P
(
z
,
u
)
∝
P
(
z
,
u
∣
x
,
y
)
P
(
x
,
y
)
\begin{align*}后验概率 P(\bm{x},\bm{y}|\bm{z},\bm{u}) &= \frac{P(\bm{z},\bm{u}|\bm{x},\bm{y})P(\bm{x},\bm{y})}{P(\bm{z},\bm{u})}\\ &\propto P(\bm{z},\bm{u}|\bm{x},\bm{y})P(\bm{x},\bm{y})\end{align*}
后验概率P(x,y∣z,u)=P(z,u)P(z,u∣x,y)P(x,y)∝P(z,u∣x,y)P(x,y)
似然(Likehood):
P
(
z
,
u
∣
x
,
y
)
P(\bm{z},\bm{u}|\bm{x},\bm{y})
P(z,u∣x,y)
先验(Prior):
P
(
x
,
y
)
P(\bm{x},\bm{y})
P(x,y)
求解 最大 后验概率 等价于 最大化 似然 P ( z , u ∣ x , y ) P(\bm{z},\bm{u}|\bm{x},\bm{y}) P(z,u∣x,y) 和 先验 P ( x , y ) P(\bm{x},\bm{y}) P(x,y) 的乘积。
( x , y ) M A P ∗ = arg max P ( x , y ∣ z , u ) = arg max P ( z , u ∣ x , y ) P ( x , y ) (\bm{x},\bm{y})_\mathrm{MAP}^*=\arg \max P(\bm{x},\bm{y}|\bm{z},\bm{u}) = \arg\max P(\bm{z},\bm{u}|\bm{x},\bm{y})P(\bm{x},\bm{y}) (x,y)MAP∗=argmaxP(x,y∣z,u)=argmaxP(z,u∣x,y)P(x,y)
不知道 机器人位姿 或 路标 大概在什么地方,没有了先验。 即
P
(
x
,
y
)
P(\bm{x},\bm{y})
P(x,y) 不知道。
此时求解最大似然估计(Maximize Likelihood Estimation, MLE)
:
(
x
,
y
)
M
L
E
∗
=
arg
max
P
(
z
,
u
∣
x
,
y
)
(\bm{x},\bm{y})_\mathrm{MLE}^*=\arg \max P(\bm{z},\bm{u}|\bm{x},\bm{y})
(x,y)MLE∗=argmaxP(z,u∣x,y)
似然
P
(
z
,
u
∣
x
,
y
)
P(\bm{z},\bm{u}|\bm{x},\bm{y})
P(z,u∣x,y): 在现在的位姿下,可能产生怎样的观测数据。
最大似然估计
(
x
,
y
)
M
L
E
∗
(\bm{x},\bm{y})_\mathrm{MLE}^*
(x,y)MLE∗: 在什么样的状态下,最可能产生现在观测到的数据。
6.1.2 最小二乘
如何求最大似然估计?
对于某一次观测
z
k
,
j
=
h
(
y
j
,
x
k
)
+
v
k
.
j
\bm{z}_{k, j}=h(\bm{y}_j,\bm{x}_k)+\bm{v}_{k.j}
zk,j=h(yj,xk)+vk.j
设噪声项
v
k
∼
N
(
0
,
Q
k
,
j
)
\bm{v}_k \sim \mathcal{N}(\bm{0}, \bm{Q}_{k,j})
vk∼N(0,Qk,j)
观测数据的条件概率为:
P
(
z
j
,
k
∣
x
k
,
y
j
)
=
N
(
h
(
y
j
,
x
k
)
,
Q
k
,
j
)
P(\bm{z}_{j,k}|\bm{x}_k,\bm{y}_j)=N(h(\bm{y}_j,\bm{x}_k),\bm{Q}_{k,j})
P(zj,k∣xk,yj)=N(h(yj,xk),Qk,j)
稀疏性:运动误差只与
x
k
−
1
,
x
k
\bm{x}_{k-1}, \bm{x}_k
xk−1,xk有关,观测误差只与
x
k
,
y
j
\bm{x}_{k}, \bm{y}_j
xk,yj 有关。【为啥呢?】
用李代数表示增量,是无约束的最小二乘问题
用旋转矩阵/变换矩阵描述位姿,需要引入旋转矩阵自身的约束【须满足
R
R
T
=
I
\bm{R}\bm{R}^T=\bm{I}
RRT=I 且
d
e
t
(
R
det(\bm{R}
det(R) = 1】
如何求解这个最小二乘问题?
6.1.3 例子:批量状态估计
下面的 Q 和 R 似乎反了。
对于一辆沿 x x x 轴前进 或 后退的汽车
设 u k \bm{u}_k uk 为输入, w k \bm{w}_k wk 为噪声,取时间为 k = 1 , 2 , 3 k = 1, 2,3 k=1,2,3
运动方程为: x k = x k − 1 + u k + w k \bm{x}_k=\bm{x}_{k-1}+\bm{u}_k+\bm{w}_k xk=xk−1+uk+wk, w k ∼ N ( 0 , Q k ) \bm{w}_k\sim \mathcal{N}(0, \bm{Q}_k) wk∼N(0,Qk)
设 z k \bm{z}_k zk 为对汽车位置的测量
观测方程为: z k = x k + n k \bm{z}_k=\bm{x}_{k}+\bm{n}_k zk=xk+nk, n k ∼ N ( 0 , R k ) \bm{n}_k\sim \mathcal{N}(0, \bm{R}_k) nk∼N(0,Rk)
根据已有的 v \bm{v} v, y \bm{y} y 进行状态估计
设初始状态 x 0 \bm{x}_0 x0 已知
推导 批量状态 的最大似然估计
令批量状态变量为 x = [ x 0 , x 1 , x 2 , x 3 ] T \bm{x} = [\bm{x}_0,\bm{x}_1,\bm{x}_2,\bm{x}_3]^T x=[x0,x1,x2,x3]T
令批量观测为 z = [ z 1 , z 2 , z 3 ] T \bm{z} = [\bm{z}_1,\bm{z}_2,\bm{z}_3]^T z=[z1,z2,z3]T
令批量输入变量为 u = [ u 1 , u 2 , u 3 ] T \bm{u} = [\bm{u}_1,\bm{u}_2,\bm{u}_3]^T u=[u1,u2,u3]T
最大似然估计为:
x m a p ∗ = arg max P ( x ∣ u , z ) = arg max P ( u , z ∣ x ) = ∏ k = 1 3 P ( u k ∣ x k − 1 , x k ) ∏ k = 1 3 P ( z k ∣ x k ) \begin{align*}\bm{x}_\mathrm{map}^* &= \arg\max P(\bm{x}|\bm{u}, \bm{z})\\ &= \arg\max P(\bm{u}, \bm{z}|\bm{x})\\ &= \prod\limits_{k=1}^3P(\bm{u}_k|\bm{x}_{k-1},\bm{x}_k)\prod\limits_{k=1}^3P(\bm{z}_k|\bm{x}_k)\end{align*} xmap∗=argmaxP(x∣u,z)=argmaxP(u,z∣x)=k=1∏3P(uk∣xk−1,xk)k=1∏3P(zk∣xk)
其中
P ( u k ∣ x k − 1 , x k ) = N ( x k − x k − 1 , Q k ) P(\bm{u}_k|\bm{x}_{k-1},\bm{x}_k)=\mathcal{N}(\bm{x}_k-\bm{x}_{k-1},\bm{Q}_k) P(uk∣xk−1,xk)=N(xk−xk−1,Qk)
P ( z k ∣ x k ) = N ( x k , R k ) P(\bm{z}_k|\bm{x}_k)=\mathcal{N}(\bm{x}_k,\bm{R}_k) P(zk∣xk)=N(xk,Rk)
构建误差变量:
e u , k = x k − x k − 1 − u k e z , k = z k − x k \begin{align*}\bm{e}_{\bm{u},k} &= \bm{x}_k-\bm{x}_{k-1}-\bm{u}_k\\ \bm{e}_{\bm{z},k}&=\bm{z}_k-\bm{x}_k\end{align*} eu,kez,k=xk−xk−1−uk=zk−xk
最小二乘的目标函数为:
min ∑ k = 1 3 e u , k T Q k − 1 e u , k + ∑ k = 1 3 e z , k T R k − 1 e z , k \min\sum\limits_{k=1}^{3}\bm{e}_{\bm{u},k}^T\bm{Q}_k^{-1}\bm{e}_{\bm{u},k} + \sum\limits_{k=1}^{3}\bm{e}_{\bm{z},k}^T\bm{R}_k^{-1}\bm{e}_{\bm{z},k} mink=1∑3eu,kTQk−1eu,k+k=1∑3ez,kTRk−1ez,k
——————
令 y = [ u , z ] T \bm{y}=[\bm{u}, \bm{z}]^T y=[u,z]T
y − H x = e ∼ N ( 0 , Σ ) \bm{y-Hx=e} \sim \mathcal{N}(\bm{0},\bm{\Sigma}) y−Hx=e∼N(0,Σ)
Σ = d i a g ( Q 1 , Q 2 , Q 3 , R 1 , R 2 , R 3 ) \Sigma=\mathrm{diag}(\bm{Q}_1,\bm{Q}_2,\bm{Q}_3,\bm{R}_1,\bm{R}_2,\bm{R}_3) Σ=diag(Q1,Q2,Q3,R1,R2,R3)
x m a p ∗ = arg max e T Σ − 1 e \bm{x}_\mathrm{map}^* = \arg\max \bm{e}^T\bm{\Sigma}^{-1}\bm{e} xmap∗=argmaxeTΣ−1e
有唯一解: x m a p ∗ = ( H T Σ − 1 H ) − 1 H T Σ − 1 y \bm{x}_\mathrm{map}^* = ({\bm{H}^{T}\bm{\Sigma}^{-1}}\bm{H})^{-1}\bm{H}^{T}\bm{\Sigma}^{-1}\bm{y} xmap∗=(HTΣ−1H)−1HTΣ−1y
寻找下降增量 Δ x k \Delta\bm{x}_k Δxk的 4 种方法
6.2 非线性最小二乘
求解一个简单的最小二乘问题:
min
x
F
(
x
)
=
1
2
∣
∣
f
(
x
)
∣
∣
2
2
\min\limits_{\bm{x}}F(\bm{x})=\frac{1}{2}||f(\bm{x})||_2^2
xminF(x)=21∣∣f(x)∣∣22
d
F
d
x
=
0
\frac{dF}{d\bm{x}}=\bm{0}
dxdF=0
得到 导数为0处的极值,可能为极大、极小或鞍点
不足: 需要知道目标函数的全局性质;求解不易
——> 迭代 【求解导数为0 ——> 不断寻找下降增量
Δ
x
k
\Delta\bm{x}_k
Δxk】
————————
如何寻找这个增量
Δ
x
k
\Delta\bm{x}_k
Δxk?
方法
:一阶梯度或二阶梯度法(最速下降法、牛顿法)、高斯牛顿法、列文伯格—马夸尔特方法
6.2.1 一阶和二阶梯度法(最速下降法、牛顿法)
将目标函数在 x k \bm{x}_k xk 附近进行泰勒展开
F ( x k + Δ x k ) ≈ F ( x k ) + J ( x k ) T Δ x k + 1 2 Δ x k T H ( x k ) Δ x k F(\bm{x}_k+\Delta\bm{x}_k) \approx F(\bm{x}_k) + \bm{J}(\bm{x}_k)^T\Delta\bm{x}_k+\frac{1}{2}\Delta\bm{x}_k^T\bm{H}(\bm{x}_k)\Delta\bm{x}_k F(xk+Δxk)≈F(xk)+J(xk)TΔxk+21ΔxkTH(xk)ΔxkJ ( x k ) \bm{J}(\bm{x}_k) J(xk): F ( x k ) F(\bm{x}_k) F(xk)关于 x \bm{x} x 的一阶导数 【梯度、雅可比(Jacobian)矩阵)】
H \bm{H} H: 二阶导数 【海塞(Hessian)矩阵】
一阶梯度或二阶梯度法
:仅保留 泰勒展开 的一阶或二阶项
最速下降法
: 取增量为一阶梯度的反向【
Δ
x
∗
=
−
J
(
x
k
)
\Delta\bm{x}^*=-\bm{J}(\bm{x}_k)
Δx∗=−J(xk)】,再指定一个步长
λ
\lambda
λ
——————
保留二阶梯度信息
Δ
x
∗
=
arg
min
(
F
(
x
)
+
J
(
x
)
T
Δ
x
+
1
2
Δ
x
T
H
Δ
x
)
\Delta\bm{x}^*=\arg\min(F(\bm{x}) + \bm{J}(\bm{x})^T\Delta\bm{x}+\frac{1}{2}\Delta\bm{x}^T\bm{H}\Delta\bm{x})
Δx∗=argmin(F(x)+J(x)TΔx+21ΔxTHΔx)
求 右侧等式 关于 Δ x \Delta\bm{x} Δx 的导数 并令其为 0:
J + H Δ x = 0 \bm{J} + \bm{H}\Delta\bm{x}=\bm{0} J+HΔx=0
H
Δ
x
=
−
J
\bm{H}\Delta\bm{x}=-\bm{J}
HΔx=−J 【牛顿法
】
——————
一阶二阶梯度法
- 直观,把函数在迭代点附近进行泰勒展开,并针对更新量做最小化即可。
- 用一个一次或二次的函数近似原函数,用近似函数的最小值来猜测原函数的极小值。
最速下降法: 过于贪心,容易走出锯齿路线,增加迭代次数
牛顿法
- 需要计算目标函数的 H \bm{H} H 矩阵,规模大时计算困难
————————
对于最小二乘问题,更实用的方法: 高斯牛顿法 和 列文伯格—马夸尔特方法。
6.2.2 高斯牛顿法
思想:将 f ( x ) f(\bm{x}) f(x) 进行一阶的泰勒展开。
牛顿法 展开的是 F ( x ) F(\bm{x}) F(x)
F ( x + Δ x ) ≈ F ( x ) + J ( x ) T Δ x + 1 2 Δ x T H Δ x F(\bm{x}+\Delta\bm{x}) \approx F(\bm{x}) + \bm{J}(\bm{x})^T\Delta\bm{x}+\frac{1}{2}\Delta\bm{x}^T\bm{H}\Delta\bm{x} F(x+Δx)≈F(x)+J(x)TΔx+21ΔxTHΔx
f
(
x
+
Δ
x
)
≈
f
(
x
)
+
J
(
x
)
T
Δ
x
f(\bm{x}+\Delta\bm{x}) \approx f(\bm{x}) + \bm{J}(\bm{x})^T\Delta\bm{x}
f(x+Δx)≈f(x)+J(x)TΔx
其中
J
(
x
)
T
\bm{J}(\bm{x})^T
J(x)T 为
f
(
x
)
f(\bm{x})
f(x) 关于
x
\bm{x}
x 的导数,为
n
×
1
n\times1
n×1 的列向量。
寻找增量
Δ
x
\Delta\bm{x}
Δx ,使得
∣
∣
f
(
x
+
Δ
x
)
∣
∣
2
||f(\bm{x}+\Delta\bm{x})||^2
∣∣f(x+Δx)∣∣2 达到最小。
——> 解一个线性的最小二乘问题:
Δ
x
∗
=
arg
min
Δ
x
1
2
∣
∣
f
(
x
)
+
J
(
x
)
T
Δ
x
∣
∣
2
\Delta\bm{x}^* = \arg\min\limits_{\Delta\bm{x}}\frac{1}{2}||f(\bm{x}) + \bm{J}(\bm{x})^T\Delta\bm{x}||^2
Δx∗=argΔxmin21∣∣f(x)+J(x)TΔx∣∣2
将上述目标函数对 Δ x \Delta\bm{x} Δx 求导,并令导数为零。
1
2
∣
∣
f
(
x
)
+
J
(
x
)
T
Δ
x
∣
∣
2
=
1
2
(
f
(
x
)
+
J
(
x
)
T
Δ
x
)
T
(
f
(
x
)
+
J
(
x
)
T
Δ
x
)
=
1
2
(
f
(
x
)
T
+
Δ
x
T
J
(
x
)
)
(
f
(
x
)
+
J
(
x
)
T
Δ
x
)
=
1
2
(
∣
∣
f
(
x
)
∣
∣
2
+
f
(
x
)
T
J
(
x
)
T
Δ
x
+
Δ
x
T
J
(
x
)
f
(
x
)
+
Δ
x
T
J
(
x
)
J
(
x
)
T
Δ
x
)
\begin{align*}\frac{1}{2}||f(\bm{x}) + \bm{J}(\bm{x})^T\Delta\bm{x}||^2&=\frac{1}{2}(f(\bm{x}) + \bm{J}(\bm{x})^T\Delta\bm{x})^T(f(\bm{x}) + \bm{J}(\bm{x})^T\Delta\bm{x})\\ &=\frac{1}{2}(f(\bm{x})^T + \Delta\bm{x}^T\bm{J}(\bm{x}))(f(\bm{x}) + \bm{J}(\bm{x})^T\Delta\bm{x})\\ &= \frac{1}{2}(||f(\bm{x})||^2+f(\bm{x})^T\bm{J}(\bm{x})^T\Delta\bm{x}+ \Delta\bm{x}^T\bm{J}(\bm{x})f(\bm{x})+\Delta\bm{x}^T\bm{J}(\bm{x})\bm{J}(\bm{x})^T\Delta\bm{x} ) \\ \end{align*}
21∣∣f(x)+J(x)TΔx∣∣2=21(f(x)+J(x)TΔx)T(f(x)+J(x)TΔx)=21(f(x)T+ΔxTJ(x))(f(x)+J(x)TΔx)=21(∣∣f(x)∣∣2+f(x)TJ(x)TΔx+ΔxTJ(x)f(x)+ΔxTJ(x)J(x)TΔx)
求导
一些向量求导公式:
注意当只有一边时,应假设另一边为 I \bm{I} I
( f ( x ) T J ( x ) T ) T + J ( x ) f ( x ) + ( J ( x ) J ( x ) T ) T Δ x + J ( x ) J ( x ) T Δ x = 0 (f(\bm{x})^T\bm{J}(\bm{x})^T)^T+\bm{J}(\bm{x})f(\bm{x}) + (\bm{J}(\bm{x})\bm{J}(\bm{x})^T)^T\Delta\bm{x} + \bm{J}(\bm{x})\bm{J}(\bm{x})^T\Delta\bm{x} =0 (f(x)TJ(x)T)T+J(x)f(x)+(J(x)J(x)T)TΔx+J(x)J(x)TΔx=0
J
(
x
)
f
(
x
)
+
J
(
x
)
J
(
x
)
T
Δ
x
=
0
\bm{J}(\bm{x})f(\bm{x}) + \bm{J}(\bm{x})\bm{J}(\bm{x})^T\Delta\bm{x} =0
J(x)f(x)+J(x)J(x)TΔx=0
J
(
x
)
J
(
x
)
T
Δ
x
=
−
J
(
x
)
f
(
x
)
\bm{J}(\bm{x})\bm{J}(\bm{x})^T\Delta\bm{x} =-\bm{J}(\bm{x})f(\bm{x})
J(x)J(x)TΔx=−J(x)f(x)
增量方程【高斯牛顿方程
】【正规方程(Normal equation)】
H
Δ
x
=
g
\bm{H}\Delta \bm{x}=\bm{g}
HΔx=g
高斯牛顿法 用 J J T \bm{JJ}^T JJT 作为 牛顿法 中 二阶 Hessian 矩阵的近似。
不足:
1、为了求解 增量方程,需要求解 H − 1 \bm{H}^{-1} H−1 ,需要矩阵 H \bm{H} H 可逆,可能出现 J J T \bm{JJ}^T JJT 为奇异矩阵的情况,导致算法不收敛。
2、若是步长 Δ x \Delta\bm{x} Δx 太大,会导致局部近似式 (6.30) 不够准确。
- 即 高斯牛顿法 采用的近似二阶泰勒展开只能在 展开点附近 有较好的 近似效果。
6.2.3 列文伯格-马夸尔特方法 【阻尼牛顿法】【信赖区域法】
修正了 高斯牛顿法的上述不足,但收敛速度比高斯牛顿法慢,被称为阻尼牛顿法(Damped Newton Method)
高斯牛顿法 采用的近似二阶泰勒展开只能在 展开点附近 有较好的 近似效果。
——> 给
Δ
x
\Delta\bm{x}
Δx 添加一个范围 信赖区域
如何确定这个信赖区域的范围?
比较 近似模型 和 实际函数 之间的差异
带 不等式约束 的优化问题,用拉格朗日乘子 把 约束项 放到目标函数中,构成拉格朗日函数:
当参数
λ
\lambda
λ 较小,接近高斯牛顿方法
当参数
λ
\lambda
λ 较大,更接近一阶梯度下降法(最速下降)
问题性质较好——> 高斯牛顿法
问题接近病态——> 列文伯格-马夸尔特方法
——————
非线性优化的通病:变量的初始值对于优化效果影响较大,容易陷入局部极小值。
视觉SLAM: 用ICP、PnP 之类的算法 提供优化初始值。
6.3 实践
6.3.1 手写高斯牛顿法 【Code】
本讲 CMakeLists.txt
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和上一次迭代的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 += inv_sigma * inv_sigma * J * J.transpose();
b += -inv_sigma * inv_sigma * error * J;
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<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; // 输出到cout
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;
}
6.3.3 g2o(General Graphic Optimization) 【Code】
g2o(General Graphic Optimization, G 2 O \mathrm{G^2O} G2O)
结点: 优化变量
边: 误差项
基于 图优化
// 构建图优化,先设定g2o
/*typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType; // 每个误差项优化变量维度为3,误差值维度为1
typedef 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_NEW
CurveFittingEdge(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,误差值维度为1
typedef 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、 证明线性方程 A x = b \bm{Ax = b} Ax=b 当系数矩阵 A \bm{A} A 超定时,最小二乘解为 x = ( A T A ) − 1 A T b \bm{x}=(\bm{A}^T\bm{A})^{-1}\bm{A}^T\bm{b} x=(ATA)−1ATb。
当 方程数 多于 未知数个数时,也就是说对应系数矩阵的行数大于列数【系数矩阵 A \bm{A} A 超定】,这样的方程组称为超定方程组。
方程个数比未知数个数多,不同方程之间会发生"冲突",导致整个方程组没有精确解。但可以设定一个误差标准,找到一个让这个误差标准值最小的近似解。
所以本题可以理解为:针对无精确解的方程组 A x = b \bm{Ax = b} Ax=b ,利用最小二乘误差标准求解其近似值。
求解最小二乘问题:
x ∗ = arg min x 1 2 ∣ ∣ f ( x ) ∣ ∣ 2 \bm{x}^* = \arg\min\limits_{\bm{x}}\frac{1}{2}||f(\bm{x})||^2 x∗=argxmin21∣∣f(x)∣∣2
本题中
1 2 ∣ ∣ f ( x ) ∣ ∣ 2 = 1 2 ( A x − b ) T ( A x − b ) = 1 2 ( x T A T − b T ) ( A x − b ) = 1 2 ( x T A T A x − b T A x − x T A T b + b T b ) \begin{align*}\frac{1}{2}||f(\bm{x})||^2 &=\frac{1}{2}(\bm{Ax - b})^T(\bm{Ax - b})\\ &=\frac{1}{2}(\bm{x}^T\bm{A}^T - \bm{b}^T)(\bm{Ax - b})\\ &=\frac{1}{2}(\bm{x}^T\bm{A}^T\bm{Ax} - \bm{b}^T\bm{Ax}-\bm{x}^T\bm{A}^T \bm{b}+\bm{b}^T\bm{b})\end{align*} 21∣∣f(x)∣∣2=21(Ax−b)T(Ax−b)=21(xTAT−bT)(Ax−b)=21(xTATAx−bTAx−xTATb+bTb)
上式对 x \bm{x} x 求导并令其为0
( A T A ) T x + A T A x − ( b T A ) T − A T b = 0 (\bm{A}^T\bm{A})^T\bm{x}+\bm{A}^T\bm{Ax}-(\bm{b}^T\bm{A})^T-\bm{A}^T \bm{b} =0 (ATA)Tx+ATAx−(bTA)T−ATb=0
A T A x = A T b \bm{A}^T\bm{Ax}=\bm{A}^T \bm{b} ATAx=ATb
x = ( A T A ) − 1 A T b \bm{x}= (\bm{A}^T\bm{A})^{-1}\bm{A}^T\bm{b} x=(ATA)−1ATb
证毕。
题2
2、调研最速下降法、牛顿法、高斯牛顿法 和 列文伯格-马夸尔特方法各有什么优缺点。除了我们举的Ceres 库和g2o 库,还有哪些常用的优化库?你可能会找到一些MATLAB 上的库。
优化库:
GTSAM
(https://gtsam.org/) GTSAM是一个开源的因子图优化库,使用C++实现。它提供了一系列的因子和变量类型,包括了视觉、IMU、GNSS和路标等因子和位姿、速度、IMU偏差和路标等变量。GTSAM还提供了多种优化器,并支持基于纯图的优化和基于混合图的优化。GTSAM有很多应用案例和文档,使用起来相对较易。
题3
3、为什么高斯牛顿法的增量方程系数矩阵可能不正定?不正定有什么几何含义?为什么在这种情况下解就不稳定了?
题4
4、DogLeg 是什么?它与 高斯牛顿法 和 列文伯格-马夸尔特方法 有何异同?
- 整理PPT
Dogleg属于Trust Region优化方法,即用置信域的方法在最速下降法和高斯牛顿法之间进行切换(将二者的搜索步长及方向转化为向量,两个向量进行叠加得到新的方向和置信域内的步长),相当于是一种加权求解。
——————————————————————————————————
LaTex
$\sim$
∼ \sim ∼
$\mathcal{N}$
N \mathcal{N} N
$\iff$
⟺ \iff ⟺
$\propto$
∝ \propto ∝
$\Sigma$
Σ \Sigma Σ