转载请说明出处：

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投影过程，实际的优化有若干次这样的迭代。