转载请说明出处：
http://blog.csdn.net/zhubaohua_bupt/article/details/74011005
视觉SLAM的核心是求取相机的位姿并建图。
在优化位姿时，其思想是构造一个关于位姿变化的误差函数，当这个误差函数最小时，认为此时估计的位姿最优。
无论是直接法还是特征点法，位姿的迭代优化都是是求解一个最小二乘问题，
在优化位姿时
<1>直接法 最小二乘的误差函数是，前后帧所有参与求解位姿像素块的灰度差。通常这一步又称之位图像对齐。
<2>特征点法 最小二乘的误差函数是，重投影误差(PnP,地图点到当前图像的投影点与匹配点的距离差和)
本篇博客是介绍的是，SLAM优化位姿时，误差函数对位姿雅可比矩阵的推导。
误差函数对于位姿扰动的雅可比矩阵，决定着下一步最优迭代估计时，位姿增量的方向。因此十分重要。
下面我们先以直接法雅可比矩阵开始推导，特征点法类似。
图像最后的雅可比矩阵可以由，所有参与直接法像素点雅可比矩阵的叠加形成。
1 直接法：
至此，我们推导出了直接法中，误差函数对于李代数位姿的雅克比矩阵。
2 特征点法
在特征点法的SLAM中，在PnP优化位姿时，误差函数是重投影误差。
PnP特征点法与直接法的雅克比矩阵有什么不同呢？还得从投影过程说起。PnP是什么？
这个过程可用图像表示如下
PnP的过程可描述如下，一堆世界坐标系下的三维地图点P(X,Y,Z)，
<1> 首先经过我们要优化的位姿转换至当前帧的坐标系下P’(X’,Y’,Z’)，
<2> 然后透过相机内参投影至当前帧图像坐标系下p’(x’,y’)，
<3> 最后缩小地图点在当前图像帧上投影点p’与匹配点pmatch(xmatch,ymatch)距离误差，来优化位姿。
上述的过程就是PnP投影过程，实际的优化有若干次这样的迭代。