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

转载请说明出处

http://blog.csdn.net/zhubaohua_bupt/article/details/74011005


视觉SLAM的核心是求取相机的位姿并建图。


在优化位姿时,其思想是构造一个关于位姿变化的误差函数,当这个误差函数最小时,认为此时估计的位姿最优。

无论是直接法还是特征点法,位姿的迭代优化都是是求解一个最小二乘问题,


在优化位姿时

<1>直接法   最小二乘的误差函数是,前后帧所有参与求解位姿像素块的灰度差。通常这一步又称之位图像对齐。

<2>特征点法   最小二乘的误差函数,重投影误差(PnP,地图点到当前图像的投影点与匹配点的距离差和)



本篇博客是介绍的是,SLAM优化位姿时,误差函数位姿雅可比矩阵的推导。

误差函数对于位姿扰动的雅可比矩阵,决定着下一步最优迭代估计时,位姿增量的方向。因此十分重要。

下面我们先以直接法雅可比矩阵开始推导,特征点法类似。

图像最后的雅可比矩阵可以由,所有参与直接法像素点雅可比矩阵的叠加形成。

1 直接法:

SLAM优化位姿时,误差函数的雅可比矩阵的推导。_第1张图片

SLAM优化位姿时,误差函数的雅可比矩阵的推导。_第2张图片SLAM优化位姿时,误差函数的雅可比矩阵的推导。_第3张图片SLAM优化位姿时,误差函数的雅可比矩阵的推导。_第4张图片SLAM优化位姿时,误差函数的雅可比矩阵的推导。_第5张图片SLAM优化位姿时,误差函数的雅可比矩阵的推导。_第6张图片SLAM优化位姿时,误差函数的雅可比矩阵的推导。_第7张图片SLAM优化位姿时,误差函数的雅可比矩阵的推导。_第8张图片

至此,我们推导出了直接法中,误差函数对于李代数位姿的雅克比矩阵。


2 特征点法

 


在特征点法的SLAM中,在PnP优化位姿时,误差函数是重投影误差


PnP特征点法与直接法的雅克比矩阵有什么不同呢?还得从投影过程说起。PnP是什么?


这个过程可用图像表示如下

SLAM优化位姿时,误差函数的雅可比矩阵的推导。_第9张图片

 

 

PnP的过程可描述如下,一堆世界坐标系下的三维地图点P(X,Y,Z),


<1> 首先经过我们要优化的位姿转换至当前帧的坐标系下P’(X’,Y’,Z’)


 

 

<2> 然后透过相机内参投影至当前帧图像坐标系下p’(x’,y’)


 

 

<3> 最后缩小地图点在当前图像帧上投影点p’匹配点pmatch(xmatch,ymatch)距离误差,来优化位姿。


上述的过程就是PnP投影过程,实际的优化有若干次这样的迭代


SLAM优化位姿时,误差函数的雅可比矩阵的推导。_第10张图片

 


 

SLAM优化位姿时,误差函数的雅可比矩阵的推导。_第11张图片



 

转载于:https://www.cnblogs.com/zhubaohua-bupt/p/7182785.html

你可能感兴趣的:(SLAM优化位姿时,误差函数的雅可比矩阵的推导。)