基于模板匹配的位姿优化

原创已于 2024-01-08 21:48:04 修改 · 1.5k 阅读

1 ·

CC 4.0 BY-SA版权

文章标签：

#计算机视觉

于 2019-07-28 22:32:11 首次发布

SLAM 专栏收录该内容

15 篇文章

订阅专栏

本文探讨了在SmallBlurryImage::IteratePosRelToTarget方法中如何优化两帧图像之间的相对位姿，包括2D平面的旋转和平移，以及如何粗略估计3D相对位姿。通过SE3fromSE2方法，使用雅克比矩阵计算光度误差，以减弱光照强度变化的影响。最终，通过优化SE3位姿，实现更精确的3D位置估计。

部署运行你感兴趣的模型镜像

转载请注明出处：https://blog.youkuaiyun.com/q_z_r_s/article/details/97620308

机器感知
一个专注于SLAM、三维重建、机器视觉等相关技术文章分享的公众号
公众号：机器感知

根据程序 SmallBlurryImage::IteratePosRelToTarget 记单个对应像素光度误差为：
在这里插入图片描述
其中，dMeanOffset也为待优化变量，即两个模板间的平均光度补偿，这样就可以在一定程度上减弱光照强度不一样的影响。

其中，待优化变量为 $[tx,ty,θ,dMeanOffset][t_x, t_y, \theta, dMeanOffset]$ ，根据式(3)可求得雅克比矩阵如下：
在这里插入图片描述
故总的雅克比矩阵如下：

通过执行SmallBlurryImage::IteratePosRelToTarget 方法后，此时就可以获得两帧图像间在2D平面上的相对旋转和平移了，接下来就是使用这个得到的2D相对旋转和平移来粗略估计3D相对位姿，使用SmallBlurryImage::SE3fromSE2 ，该方法首先通过在参考帧模板上设置两个点，然后根据刚获得的SE2相对位姿投影到当前帧模板像素坐标系下，为了优化两帧模板间的SE3相对位姿，同时将参考帧上刚刚的两点转换到归一化平面上，然后使用待优化的SE3位姿都应到当前帧像素坐标系下，由此可构成这两个投影的残差，从而优化SE3位姿，投影残差定义如下：
在这里插入图片描述
其中，使用SE3进行投影的公式如下：

这里需要注意的是，该方法认为这两帧模板是没有平移的！使用SE2进行投影的公式如下：在这里插入图片描述
由此可计算式关于SE3的雅克比矩阵：

其中，第一个因式由ATANCamera::GetProjectionDerivs给出，后两个因式在本函数中计算得到，因为通过SE3在当前帧相机坐标系下的投影坐标 $x,y,z]^T$ 会随着不断优化的SE3而变化。