矩阵运算（矩阵运算公式大全）
在slam 和sfm领域，恢复相机位姿和3D点的坐标是其重要的任务，描述一个场景的3D点在不同相机的图像坐标之间的关系被称为对极几何关系。对极几何关系描述的矩阵通常有基本矩阵(fundamental matrix)、本质矩阵（essential matrix）、单应矩阵(homography matrix)。基本矩阵的求解算法有7点法、8点法；基本矩阵的求解算法有5点法、8点法；单应矩阵的求解算法为DLT算法。
在对极几何关系矩阵中，单应矩阵的求解、基本矩阵和本质矩阵的8点法求解算法统称为线性求解，而基本矩阵的7点法和本质矩阵的5点法称为非线性求解，尤其本质矩阵的5点法求解过程非常复杂，大多数教材都是先计算基本矩阵，然后利用内参和基本矩阵求解本质矩阵(如orb2-slam中本质矩阵的求解)，5点法的求解涉及10次多项式的求解，故相关资料较少(包括波恩大学photogrammetry 课程，5点法求解E矩阵的过程也是直接跳过)。
关于其它对极几何(F-7points、F-8points、E-8points、H-DLT)的算法具体介绍，可以关注本人“视觉三维重建的关键技术与实现-colmap代码解析”课程视频，具体课程介绍：https://app0s6nfqrg6303.h5.xiaoeknow.com/v1/course/column/p_609161a1e4b071a81eb781a8?type=3
二、算法介绍
(1) 前提条件
Essential matrix 5个dof,故 最小支撑点为5对匹配点。
(2) 算法流程
首先借鉴求解f矩阵的线性表达式,得到有关本质矩阵元素的表达式如下：
填充元素可得:
step1 : 提取nullspace 的4个特征向量
因为未知数个数为9,E的dof 为5,则以上齐次方程的解存在4个基础解系,那么E矩阵的通解变为基础解系的线性组合,即:
由于E本身尺度不唯一,故为了不失一般性,令c_{w}=1,则未知数元素变为了[c_{x}, c_{y}, c_{z}];
代码如下：
step2 : 加入约束方程
把(1)式带入(2)和(3)中得到9个三元三次方程
step3: 利用Gauss-Jordan消元
由于三元三次求解不好求解，故可以建立等效方程，将其变成Aj=0;其中A矩阵是10*20的大小，j 是未知数c_{x}, c_{y}, c_{z}的20个组合。利用GaussJordan约减A可得到以下矩阵:
将(4)式有关z的多项式全部代入(5),便可得到关于z的一元10次方程.高斯约减的图示如下:
注:将Mx=0,约减成[I B]x=0
代码如下:
step 4: 求解E矩阵
根据step3 得到每个c_{z}都可以求出一组c_{x}, c_{y},带入(1)便可求解出E矩阵:
参考文献:
1.Recent Developments on Direct Relative Orientation[J].H Stewénius.ISPRS 2006
2.An efficient solution to the five-point relative pose problem[J]. D. Nister.EEE-T-PAMI, 26(6), 2004