最近看到一篇 paper, 讲的是对齐之后的一种新的位姿估计的算法, 文章题目是 An Efficient Solution to the Homography-Based Relative Pose Problem With a Common Reference Direction
文中引用了一篇文章:"Homography Based Egomotion Estimation with a Common Direction", 感觉还挺有意思
文中的背景是, 现在很多相机都能够获取 imu 的信息, 根据 imu 来获取重力的方向, 根据重力的方向, 可以将相机进行对齐, 所谓对齐的意思是, 将某一个轴对齐到同一方向, 然后再利用一种更加方便的方法求取 homography, 进而恢复出 r 和 t
对齐大致如下图
在对齐之后, 作者一共讨论三种情况, 来求取 homography 以及根据求解得到的 homography 来恢复 r 和 t
假设已经沿着 z 轴进行对齐, 实际上旋转矩阵 R 只剩一个自由度进行估计, R 可以表示为如下形式
这个时候 homography 和 r 与 t 的关系也变得比较简单
根据如上公式, 作者一共讨论了四种情况, 关于这四种情况, 我们逐一讨论,
第一种情况是, 已知地面上的两个点, 即可求取 homography, 也能够得到 r 和 t
第二种情况是, 对应点在垂直于地面上的竖直平面上, 这样的点需要两个, 并且要求已知竖直平面的法向量
第三种情况和第二种情况类似, 已知竖直平面上的 2.5 个点, 并且不要求这个数值平面的法向量已知
第四种情况是, 任意平面, 发向量已经知道
论文中讨论的非常细致, 并且我复现了 case1 和 case3, 代码 https://github.com/XGBoost/homography2rt
针对文中的坑我还是有必要说一下, 在复现 case3 的时候, 因为求解的多项是四次的, 而且化简起来非常的复杂, 所以, 我采取了 matlab 符号求解的方式,
哎妈是真的慢, 有多慢呢, 估计速度差 1000 倍吧.
detail 如下,
我有一个一元四次方程, 并且这个方程的系数非常复杂, 以至于很难化简, 但是在 matlab 中你可以用符号来定义未知变量, 从而让程序自己化简,
大概如下
- https://github.com/XGBoost/homography2rt/blob/master/pami3findHomography.m#L50
- syms lambda;
- h = temp11+lambda*temp22;
- f = h(1)^2*h(6)^2-2*h(1)*h(2)*h(5)*h(6)+h(2)^2*h(5)^2+h(3)^2*h(6)^2-2*h(3)*h(4)*h(5)*h(6)+h(4)^2*h(5)^2-h(5)^2-h(6)^2;
- root2 = double(solve(f))
但是, 如果能够不怕麻烦, 写出四次方程的系数, 用 roots 函数实际上很快.
来源: http://www.bubuko.com/infodetail-3383762.html