非线性优化

目的： 因为噪声！为了减少噪声带来的影响。

相机模型

现实世界点P，在相机坐标系下坐标P(X,Y,Z),落在投影平面上的坐标为(u,v)，一般把(u,v)写成齐次坐标；
所谓齐次坐标，是为了方便进行矩阵的运算，在向量上面加一个维度，一般把增加的维度默认成1;
焦距fx,fy,相机坐标系和成像平面的平移cx,cy都作为内参写进K（内参矩阵）；

$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$

$\vec{a}\times \vec{b}=a^{\wedge }\vec{b}$
^ 把向量变成其对应的反对称矩阵

优化问题的目标就是找到一组参数，使得目标函数取极值；
比如slam的最小重投影误差就是让目标函数J对T求导，因为旋转矩阵对加法运算不封闭，我们将对李代数求导来间接的对变换矩阵求导了。我们要求解这个最佳的位姿T，一般采用迭代优化，每次迭代都更新一个位姿的增量delta，使得目标函数最小。这个delta就是通过误差函数对T微分得到的。

矩阵的微分$\dot{R}=$ $\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]$ $R(t)$ = ${\phi }^{\wedge }$ $R(t)$
$R=exp({\phi }^{\wedge })$

向量φ=（φ1，φ2，φ3）反应了R的导数性质，故称它在SO(3)上的原点 φ0 附近的正切空间上。这个φ正是李群大SO(3)对应的李代数小so(3)。
即：李群空间的任意一个旋转矩阵R都可以用李代数空间的一个向量的反对称矩阵的指数来近似。

对于任意反对称矩阵，都能找到一个与之对应的向量， a^ = A
则 $A^{\vee }=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)||²₂的泰勒展开，保留一阶，得到解，delta x=- J^T(X)
即：沿着梯度反方向前进可以得到最快的下降；
高斯牛顿法： f(x+Δx) ≈ f(x)+ J(x)Δx,问题转化为求arg min$||f(x)+J(x)\Delta x|{|}_2^2$
对Δx求导，另为0;得到下列增量方程：

J(x)^T J(x) Δx= - J(x)^T f(x)
定义左边的系数为H，右边的系数为g
得 HΔx=g

列文伯格-马夸尔特法：给Δx加上置信区间