目的: 因为噪声!为了减少噪声带来的影响。
相机模型
现实世界点P,在相机坐标系下坐标P(X,Y,Z),落在投影平面上的坐标为(u,v),一般把(u,v)写成齐次坐标;
所谓齐次坐标,是为了方便进行矩阵的运算,在向量上面加一个维度,一般把增加的维度默认成1;
焦距fx,fy,相机坐标系和成像平面的平移cx,cy都作为内参写进K(内参矩阵);
Z ( u , v , 1 ) = K P − − − − − − − Z (u,v,1) = K P------- Z(u,v,1)=KP−−−−−−− *
把P换成世界坐标系下的Pw表示:P = R Pw + t = T Pw
Z Puv = K T Pw
因为P=(X,Y,Z), *式两边同除以Z ,得Puv = K T Pw ‘
其中,T Pw‘=(X,Y,Z)/Z=(X/Z,Y/Z,1),这个叫做归一化坐标;相机前Z=1的平面称为归一化平面。
李群,李代数
群: 集合 + 运算
例如:
特殊正交群:SO(3): 旋转矩阵 + 矩阵乘法
特殊欧氏群:SE(3):变换矩阵 + 矩阵乘法
李群是连续的群,能够在空间内连续运动;
李代数:每个李群都有对应的李代数
两个向量a,b的叉积,可以写成a的反称矩阵乘以向量b
旋转矩阵的微分是一个反对称(也叫斜对称)矩阵左乘它本身
反对称矩阵的定义就是 A T = − A A^T = -A AT=−A
a
⃗
×
b
⃗
=
a
∧
b
⃗
\vec{a} \times \vec{b}= a^{\wedge} \, \vec{b}
a×b=a∧b
^ 把向量变成其对应的反对称矩阵
优化问题的目标就是找到一组参数,使得目标函数取极值;
比如slam的最小重投影误差就是让目标函数J对T求导,因为旋转矩阵对加法运算不封闭,我们将对李代数求导来间接的对变换矩阵求导了。我们要求解这个最佳的位姿T,一般采用迭代优化,每次迭代都更新一个位姿的增量delta,使得目标函数最小。这个delta就是通过误差函数对T微分得到的。
矩阵的微分
R
˙
=
\dot{R} =
R˙=
[
0
−
ϕ
3
ϕ
2
ϕ
3
0
−
ϕ
1
−
ϕ
2
ϕ
1
0
]
\begin{bmatrix} 0 & -\phi_3 & \phi_2 \\ \phi_3 & 0 & -\phi_1 \\ -\phi_2 & \phi_1 & 0 \end{bmatrix}
⎣⎡0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10⎦⎤
R
(
t
)
R(t)
R(t) =
φ
∧
φ^{\wedge}
φ∧
R
(
t
)
R(t)
R(t)
R
=
e
x
p
(
φ
∧
)
R = exp(φ^{\wedge})
R=exp(φ∧)
向量φ=(φ1,φ2,φ3)反应了R的导数性质,故称它在SO(3)上的原点 φ0 附近的正切空间上。这个φ正是李群大SO(3)对应的李代数小so(3)。
即:李群空间的任意一个旋转矩阵R都可以用李代数空间的一个向量的反对称矩阵的指数来近似。
对于任意反对称矩阵,都能找到一个与之对应的向量, a^ = A
则 A ∨ = a A^\vee=a A∨=a
(倒^) ∨ \vee ∨把矩阵变成向量;
exp(ξ^)是指 指数映射,即矩阵的指数;可以用泰勒展开;
状态估计
讨论观测方程:在Xk处对路标Yj进行了一次观测,对应到图像上的像素Zk,j;
通过带噪声数据推断位姿和地图,构成了一个状态估计问题。即求P(x|z);
P(x|z)叫后验概率,一般是我们求解的目标;P(z|x) 是条件概率,又叫似然,一般是通过历史数据统计得到。一般不把它叫做先验概率,但从定义上也符合先验定义。
利用贝叶斯公式,P(x|z)=P(z|x) P(x)/P(z)
因为直接求后验概率是困难的,假设不知道位姿是什么,也就没有了先验概率P(x),
那问题就转化为找到一个x,使P(z|x)最大。
即“在什么样的状态下,最有可能产生现在观测到的数据”
P(z|x)~N(h,Q)
满足高维高斯分布
化简后,得到一个误差的和函数,即最小二乘问题(min(||f(x)^2||2))
问题转变成了求极值的问题:
参考:http://blog.sina.com.cn/s/blog_7445c2940102x3x4.html
我们将原问题找f(x)的最小值(此处x为n维向量,不是一维的),变为找到f(x+Δx)的最小值,再依此迭代多次,逼近f(x)的最小值。此时,x变为已知,而未知是Δx的大小(当然也可以理解为方向,因为n维向量表示了一个方向)。也就是说,我们希望找到一个Δx,使得函数f(x+Δx)在点x处取得最小值。
最速下降法:利用||f(x)||22的泰勒展开,保留一阶,得到解,delta x=- JT(X)
即:沿着梯度反方向前进可以得到最快的下降;
高斯牛顿法: f(x+Δx) ≈ f(x)+ J(x)Δx,问题转化为求arg min
∣
∣
f
(
x
)
+
J
(
x
)
Δ
x
∣
∣
2
2
||f(x)+J(x)Δx||^2_ 2
∣∣f(x)+J(x)Δx∣∣22
对Δx求导,另为0;得到下列增量方程:
J(x)T J(x) Δx= - J(x)T f(x)
定义左边的系数为H,右边的系数为g
得 HΔx=g
列文伯格-马夸尔特法:给Δx加上置信区间