最近看到一篇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函数实际上很快。