通过单应矩阵(Homography)求解旋转和平移

本文参考Motion and structure from motion in a piecewise planar environment和ORB-SLAM代码共同完成, 部分证明还需要结合原文, 这里仅仅梳理算法流程.

1. 单应(Homography)矩阵的定义

假定空间中有一个平面, 落在平面上的点满足方程满足:

一个空间点, 在相机1内的坐标为, 在两个相机内的投影分别为

其中, , 分别代表两时刻的旋转和平移关系. 为内参数矩阵, 假定两个相机的内参数矩阵相同. 注意大写为空间点, 小写为图像坐标系的齐次坐标.

考虑到,均为归一化坐标, 因此可以对其乘以


为单应矩阵, 满足. 单应矩阵可以通过4组匹配点, 可以通过线性变换发直接求解其8个变量.

2.通过单应矩阵求解平移和旋转

根据Motion and structure from motion in a piecewise planar environment, 先将单应矩阵转换为矩阵:

对应代码为:

  cv::Mat A = invK*H21*K;

那么, , 三维空间点满足.

对进行SVD分解, , 这里(), 由的特征值的平方根组成(奇异值). , 因此存在三个奇异值.
使用奇异值分解可以将化简为:

新的和旧的符号之间的关系为:

这一组公式是后续重要的计算和的依据.

可以证明, , 见原文. 进而求得

其中.

至此, 代码描述如下:

cv::SVD::compute(A,w,U,Vt,cv::SVD::FULL_UV);
float s = cv::determinant(U)*cv::determinant(Vt);

float d1 = w.at<float>(0);
float d2 = w.at<float>(1);
float d3 = w.at<float>(2);
float aux1 = sqrt((d1*d1-d2*d2)/(d1*d1-d3*d3));
float aux3 = sqrt((d2*d2-d3*d3)/(d1*d1-d3*d3));
float x1[] = {aux1,aux1,-aux1,-aux1};
float x3[] = {aux3,-aux3,aux3,-aux3};

. 那么有4中排列组合, 在考虑的正负, 一共有8中排列组合.

那么下面根据的正负进行分类处理

2.1

那么

其中,

float aux_stheta = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1+d3)*d2);
float ctheta = (d2*d2+d1*d3)/((d1+d3)*d2);
float stheta[] = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta};
for(int i=0; i<4; i++)
{
  cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);
  Rp.at<float>(0,0)=ctheta;
  Rp.at<float>(0,2)=-stheta[i];
  Rp.at<float>(2,0)=stheta[i];
  Rp.at<float>(2,2)=ctheta;

  cv::Mat R = s*U*Rp*Vt;

  cv::Mat tp(3,1,CV_32F);
  tp.at<float>(0)=x1[i];
  tp.at<float>(1)=0;
  tp.at<float>(2)=-x3[i];
  tp*=d1-d3;

  cv::Mat t = U*tp;

, .

2.2

见论文.