本文参考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 
见论文.
