SLAM优化位姿时，偏差函数的雅可比矩阵的推导。

转载请说明出处：

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