1 数学知识
1.1 单应矩阵
Homography: 单应矩阵H，描述物体在世界坐标系和像素坐标系之间的位置映射关系，包含放缩因子、相机内参、相机外参。
1.1.1 参考1
- 单应性矩阵的理解及求解3： 单应性矩阵的理解及求解3_lyhbkz的博客-CSDN博客_单应性矩阵
1.1.2 参考2
- 计算机视觉中的数学方法 3.2.3 ，吴福朝 中科院自动化所
1.1.3 参考3
- 计算机视觉中的数学方法 4.2，吴福朝 中科院自动化所
单应矩阵能干什么？
（slam 十四讲）在根据本质矩阵E求解相机运动时，通常会采用八点法，但是当求解的输入参数图像点对在同一平面时，八点法会退化，无法求解相机的运动。采用单应矩阵此时仍然可以求解，所以用到了单应矩阵。求解H方法同样是解方程根据 一对点对 p2 = Hp1，转换成向量相乘形式，根据8个点（每对点可以提供2个方程）列出8个方程，变成矩阵形式，求解方程Ax = b。 然后从H中分解出运动R,t 等变量。
1.2 对比: H(单应矩阵)、P(摄像机矩阵)、F(基本矩阵)、E(本质矩阵)
- H(单应矩阵，3×3，2D->2D)：二视几何下，一个摄像机上的图像点，与另一幅个摄像机的图像点的对应关系。
,8个自由度，秩为3.
（使用单应矩阵，求基本矩阵；e’为第二副图像关于第一幅图像的极点）
- P(摄像机矩阵，3×4，3D->2D)：本摄像机坐标系下(光心C为原点)的空间点，与对应图像点之间的关系。
，11个自由度
- F(基本矩阵，3×3，2D->2D)：二视几何下，一个摄像机上的图像点，与另一个摄像机的图像上对应极线的关系。
- 重要公式:
(图像点 m 在第二幅图像上的对应点 m′，在极线
上（极线方程
）)
- 基本矩阵和相机内外参数的关系：
, 7个自由度；秩是2。
- 基本矩阵和相机内外参数的关系：
- 重要性质：
- 基本矩阵不依赖于摄像机矩阵的选择，等价地说不依赖于世界坐标系的选择；(吴福朝，12.1.1)
- E(本质矩阵)：描述了两幅规范化图像间的极几何，它与基本矩阵一样也是一个秩为 2 的矩阵。
- 与基本矩阵的不同之处是它仅与摄像机的运动参数有关。因此，从本质矩阵出发可估计出摄像机的欧氏运动参数。
- 令
令
- 则
- 基本矩阵与本质矩阵的关系：
直接线性变换（DLT）
- 在三维视觉中，用于计算Ap=0的矩阵（A:由2D点和3D点坐标组成的矩阵；p: 2D点和3D点的映射关系的3×4的矩阵，称之为相机的内参）；
- 简言之，用于计算相机的内参数，对相机进行标定；
- 要想求解p, 已知的一种方法是对A做SVD(奇异值分解)；
参考：直接线性变换解法（DLT）用于标定相机 – ambitionzz – 博客园
1.3奇异值分解
SVD: 通过分解矩阵，来解方程中未知数使用的一种数学方法，比如求本质矩阵E。
从图片的压缩结果可以看出来，奇异值可以被看作成一个矩阵的代表值，或者说，奇异值能够代表这个矩阵的信息。当奇异值越大时，它代表的信息越多。因此，我们取前面若干个最大的奇异值，就可以基本上还原出数据本身。
参考1：SVD（奇异值分解）小结 – EndlessCoding – 博客园
参考2：计算机视觉中的数学方法 8.3.2 ，吴福朝 中科院自动化所
1.4 矩阵范数
常用的三种p-范数，诱导出的矩阵范数是：
1-范数：║A║1 = max{ ∑|ai1|, ∑|ai2| ,…… ,∑|ain| } (列和范数，A每一列元素绝对值之和的最大值) (其中∑|ai1|表示第一列元素绝对值的和，∑|ai1|=|a11|+|a21|+…+|an1|，其余类似)；
2-范数：║A║2 = A的最大奇异值 = ( max{ λi(A^H*A) } ) ^{1/2} (欧几里德范数，又称谱范数，即A^H*A特征值λi中最大者λ1的平方根，其中A^H为A的转置共轭矩阵)；
二范数又称谱范数：即，求解矩阵A与自身转置乘积所得矩阵的模最大特征值，记这个特征值的模叫做矩阵的谱半径，也就是此矩阵的谱范数，注意这里做的乘积是必要的，就是方阵化，因为我们一般的矩阵不一定是方阵并不一定有特征值。
∞-范数：║A║∞ = max{ ∑|a1j|, ∑|a2j| ,…, ∑|amj| } (行和范数，A每一行元素绝对值之和的最大值) (其中为∑|a1j| 第一行元素绝对值的和，其余类似)
范式的相容性：║XY║≤║X║║Y║
2 相机知识
2.1 相机模型
————————————————————————-
————————————————————————-
- 移动相机到拍摄位置，镜头对准某个方向(视图变换,view transform)
- 将拍摄对象，移到场景中的某个位置(模型变换,model transform)
- 设置相机焦距，或调整缩放比例(投影变换,projection transform)
- 对结果图像拉伸或者压缩，变换为需要的图片大小(视口变换,viewpoint transform)；视口变换对应于选择被冲洗相片的大小这个阶段。我们希望照片像钱包一样大还是像海报一样大？在计算机图形中，视口是一个矩形的窗口区域，图像就是在这个区域中绘制的。
透视投影(perspective projection) : 棱台模型， 透视投影属于中心投影。透视投影图简称为透视图或透视，它是从某个投射中心将物体投射到单一投影面上所得到的图形。
正交投影(orthographic projection)：长方体模型，投影线垂直于投影面的投影属于正交投影 ，也称为平行投影。
参考：三维重建笔记_投影变换_平行投影 透视投影 基本概念 图示 矩阵 公式_惊鸿一博-CSDN博客_平行投影矩阵
2.2 相机矩阵
对于一个空间点P：世界坐标系–相机外参(R,t)–>相机坐标系–相机内参K–>像素(图像)坐标系
相机矩阵分解为两个矩阵的乘积：内参矩阵K和外参矩阵[R|−RC]
内参矩阵 K: 是将空间点在相机坐标系下（光心是原点）的3D坐标（归一化的3D坐标，比如（X/Z,Y/Z,1）），变换到2D齐次图像坐标（成像平面像素坐标系）。
相机内参的计算，是将内参矩阵分解为切变(shear,类似于将长方形压成平行四边形的变形方式)、缩放、平移变换，分别对应轴倾斜s、焦距f、主点偏移(x,y) :
外参矩阵(R, t)：描述的是世界坐标中相机的位置，及其指向方向。有两个成分：旋转矩阵R和平移向量t。它们并非恰好对应相机的旋转和平移；描述的就是如何将世界坐标系中的点变换到相机坐标系中。
外参矩阵以刚体变换矩阵的形式可以记为：左边一个3∗3旋转矩阵，右边一个3∗1的平移列向量 。
参考：相机矩阵(Camera Matrix) 相机矩阵(Camera Matrix) – 简书
视口变换 3.4 3.4 视口变换 – 51CTO.COM
2.3 相机标定算法
–同时标定内参外参 – Resnecting
Resection: 把相机内参外参放在一起进行标定的方法。
一个黄金标准算法 – 最小化重投影误差平方和
在知道6个点对应关系（2D图像坐标点 <-对应-> 3D世界坐标点）的条件下，先使用 Normalization+DLT的方法，计算出一个初始的相机矩阵，再用上述相机矩阵，最小化几何误差，进一步优化（比如梯度下降等非线性优化方法）相机矩阵，最后反Normalization，得到真实的相机矩阵（此处指相机外参）。
用到的相关基本概念：
————————————– Normalization —————————————-
做法：对所有点做平移+放缩的变换，让这些点均匀分布在一个圆上/球上
目的：提高数值计算的稳定性
—————————————– DLT ————————————————–
———————————- 几何误差 与 代数误差 ———————————-
几何误差：图像实际点 与 计算投影点之间的误差；通过几何误差求解的方法，也称之为光束法平差（Bundle Adjustment）。
代数误差：使用DLT方法时，用于度量的一个误差。(在下张图中，将分母2乘都左侧后，再相减)
两者区别：相差了一个倍数关系。
由上图可以看出，转换矩阵P，含有12个自由度（m00 ~ m23），在进行尺度放缩（比如同时除以m23），
可以减少一个自由度，这样转换矩阵的确定，需要11个自由度，即需要6个对应点对求解2D<->3D转换矩阵P。
3 对极几何
提到对极几何，一定是对两幅图像而言，对极几何实际上是“两幅图像之间的对极几何”，它是图像平面与以基线为轴的平面束的交的几何（这里的基线是指连接摄像机中心的直线），以下图为例：对极几何描述的是左右两幅图像(点x和x’对应的图像)与以CC’为轴的平面束的交的几何约束！
基本概念： 基线，对极平面（epipolar plane），极线（epopolar line），5点共面约束(c, x, X, x’ c’共面)
参考：对极几何基本概念 对极几何基本概念_tina的博客-CSDN博客_对极几何
3.1 多视几何
一个物体在多个成像表面形成的图像（可以是同一个相机或者多个相机）：
同一个点（如Point1）在不同图像之间的对应关系：
在两视图下，基础矩阵F和本质矩阵E的定义如下：
4. 使用PnP方法，进行相机外参的标定
涉及坐标系：世界坐标系(3D点坐标已知) –(R,T)待求解–> 相机坐标系(3D点坐标) –> 图像坐标系(2D点坐标已知)
PnP问题：属于相机标定的子问题；
原理：在相机内参固定（已知）的情况下，根据世界坐标系的3D点坐标，和对应的图像坐标系的2D点坐标，求相机外参（R,t）(自然是相对于世界坐标系的旋转和平移)。如视觉SLAM中，小车在运动过程中，相机的内参通常不会发生改变。
描述：已知图像2D坐标点，和对应的世界坐标系下的3D坐标点（含三维坐标点到相机中心的距离），求相机坐标系下的三维点坐标，然后，用来求解世界坐标系和相机坐标系的坐标变换了。
可参考论文：Xiao Shan Gao, Xiao Rong Hou, Jianliang Tang, and Hang Fei Cheng. 2003. Complete solution classification for the perspective-three-point problem. IEEE Trans. Pattern Anal. Mach. Intell. 25, 8 (2003), 930–943.
4.1 经典的P3P算法
即最小解，当有3个点的时候，由世界坐标系点+对应的图像坐标系点，求相机坐标系下的三维点坐标。
下面的角度θ，是通过相机的内参计算确定的。
评论： 利用的正是三角形的余弦定理，上面ppt截图中di dj是未知的（相机光心到世界坐标系下的3D点的距离）， dij是已知的（世界坐标系下的两个3D点的距离）。
4.1.1 角度θ的计算
注意： K^(-1) 表示相机坐标系下的光线（相机中心连接像素点）的方向，其中K是相机的内参。
4.2 线性PnP算法
选定多组（>3）对应点，构造多个方程，求解线性方程组的方法，求相机坐标系下的三维点坐标。
但是该方法，算法复杂度O(n^5)。
4.3 EPnP Algorithm
针对上述复杂度过高问题的一个改进，算法复杂度O(n)，IJCV2008。原理与P3P问题一致，使用比较广泛的算法。
5. 相机内参的标定
5.1 用Vanishing Points求相机内参
5.1.1 Vanishing Points(消失点)
5.1.2 Image of Absolute Conic(绝对二次曲线的映射)
Absolute Conic：绝对二次曲线，又翻译为，绝对圆锥曲线。
(圆锥曲线几何定义：用一个平面去截一个二次锥面，得到的交线就称为圆锥曲线(conic sections)。我们通常提到的圆锥曲线包括：椭圆、双曲线和抛物线。)
思想：
将三维空间中，两个相互垂直的向量的齐次形式，投影成无穷远处两个消失点(已知)，从而得到一个关于相机内参矩阵(未知)的方程，进而求得相机的内参矩阵。
几何含义：
在平面上，一个3×3的矩阵就是一个二次曲线（或者说是一个椭圆）；
说明：三维空间无穷远处，包了一个平面，在这个平面上有一个椭圆(也叫Absolute Conic)，是我们人为想象出来的，这个椭圆投影在图像上，依然是一个椭圆，这个图像上的椭圆方程，就是上述方程 “0=…”（及关于内参K的一个方程）。
参考：The Image of the Absolute Conic: http://www.cs.unc.edu/~marc/tutorial/node87.html
5.1.3 通过
Vanishing Points(消失点
)求相机内参
通过一张图（消失点可知），标定相机内参。
5.2 求相机内参(张氏标定)
5.2.1 Circular Points(虚圆点)
在平面摄影几何中，任何一个两维点都有一个homogeneous coordinate(齐次坐标)，
如果第三项为0，则该点为无穷远点；如果这个点含有虚数项(-i 和 i)，那么这两个点叫做circular points(虚圆点)。
虚圆点如何产生？
一个(任意的)圆和无穷远直线的交点。
为什么叫虚圆点？
因为所有的圆都经过这两个点(这两个点在无穷远直线上)。
5.2.2 The Absolute Conic in 3D Space(三维空间中的绝对二次曲线)
将三维空间切成很多平面，各种不同朝向的平面。
如：将一个大地平面绕着一个轴(如东西方向的轴)转一圈，得到三维空间;
每个大地平面上都有这样一条 the line at infinity(无穷远直线)，每一条这样的无穷远直线上都有2个circular points(虚圆点);
所有的这些平面上的所有circular points(虚圆点)，最终会形成什么呢？答，形成一个椭圆，即一个absolute conic。
5.2.3 Absolute Conic有什么用？
想象有一个无穷远的平面，比如天空，包住了这个三维世界，“天空”中有个想象的椭圆（absolute conic），
对着天空拍了一张照片，它投成到图像平面上还是一个椭圆，这个图像上的椭圆KK,的重要性就是，它直接
代表了相机的内参。你知道了absolute conic在图像中位置，你就能把内参求出来。即想求相机内参，就是
去找absolute conic在哪里。
5.2.4 张氏标定求相机内参？
思想：
circular points -homography-> Image of Absolute Conic -decomposition-> Intrinsic Metrix
步骤：
- 假设图形面有3张标定板的图像，在每一块标定板上，我们都可以建立一个，从标定板平面到图像平面的一个Homography的一个映射。因为标定板上有正方形，正方形上有4个顶点，可以根据4个顶点就可以求出H(homography)。
- 每个标定板平面都有2个circular points，可以利用H求出这2个circular points，在图像平面的坐标。
- 这里有3块板，一共6个circular points。因为absolute conic（绝对二次曲线）是由circular points（虚圆点）组成的conic（二次曲线），而fit一个conic只需要5个点，所以这种方法可以 fit 一个 conic，即 Image of Absolute Conic，即 K^(-T).K^(-1) ，即相机的内参。
6. 实践：调用opencv中的函数求解E/F/H
参考 视觉SLAM十四讲中7.4节； 代码来源：pose_estimation_2d2d.cpp
- 求本质矩阵E cv::findEssentialMat
- 从本质矩阵E 中恢复相机的外参（运动/旋转R和平移t）cv::recoverPose
- 求基础矩阵F cv::findFundamentalMat
- 求单应矩阵H cv::findHomography
-
#include <iostream>
-
#include <opencv2/core/core.hpp>
-
#include <opencv2/features2d/features2d.hpp>
-
#include <opencv2/highgui/highgui.hpp>
-
#include <opencv2/calib3d/calib3d.hpp>
-
// #include "extra.h" // use this if in OpenCV2
-
using
namespace std;
-
using
namespace cv;
-
-
/****************************************************
-
* 本程序演示了如何使用2D-2D的特征匹配估计相机运动
-
* **************************************************/
-
-
void find_feature_matches (
-
const Mat& img_1, const Mat& img_2,
-
std::vector<KeyPoint>& keypoints_1,
-
std::vector<KeyPoint>& keypoints_2,
-
std::vector< DMatch >& matches );
-
-
void pose_estimation_2d2d (
-
std::vector<KeyPoint> keypoints_1,
-
std::vector<KeyPoint> keypoints_2,
-
std::vector< DMatch > matches,
-
Mat& R, Mat& t );
-
-
// 像素坐标转相机归一化坐标
-
Point2d pixel2cam ( const Point2d& p, const Mat& K );
-
-
int main ( int argc, char** argv )
-
{
-
if ( argc !=
3 )
-
{
-
cout<<
"usage: pose_estimation_2d2d img1 img2"<<endl;
-
return
1;
-
}
-
//-- 读取图像
-
Mat img_1 =
imread ( argv[
1], CV_LOAD_IMAGE_COLOR );
-
Mat img_2 =
imread ( argv[
2], CV_LOAD_IMAGE_COLOR );
-
-
vector<KeyPoint> keypoints_1, keypoints_2;
-
vector<DMatch> matches;
-
find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );
-
cout<<
"一共找到了"<<matches.
size() <<
"组匹配点"<<endl;
-
-
//-- 估计两张图像间运动
-
Mat R,t;
-
pose_estimation_2d2d ( keypoints_1, keypoints_2, matches, R, t );
-
-
//-- 验证E=t^R*scale
-
Mat t_x = (
Mat_<
double> (
3,
3 ) <<
-
0, -t.
at<
double> (
2,
0 ), t.
at<
double> (
1,
0 ),
-
t.
at<
double> (
2,
0 ),
0, -t.
at<
double> (
0,
0 ),
-
-t.
at<
double> (
1,
0 ), t.
at<
double> (
0,
0 ),
0 );
-
-
cout<<
"t^R="<<endl<<t_x*R<<endl;
-
-
//-- 验证对极约束
-
Mat K = (
Mat_<
double> (
3,
3 ) <<
520.9,
0,
325.1,
0,
521.0,
249.7,
0,
0,
1 );
-
for ( DMatch m: matches )
-
{
-
Point2d pt1 =
pixel2cam ( keypoints_1[ m.queryIdx ].pt, K );
-
Mat y1 = (
Mat_<
double> (
3,
1 ) << pt1.x, pt1.y,
1 );
-
Point2d pt2 =
pixel2cam ( keypoints_2[ m.trainIdx ].pt, K );
-
Mat y2 = (
Mat_<
double> (
3,
1 ) << pt2.x, pt2.y,
1 );
-
Mat d = y2.
t() * t_x * R * y1;
-
cout <<
"epipolar constraint = " << d << endl;
-
}
-
return
0;
-
}
-
-
void find_feature_matches ( const Mat& img_1, const Mat& img_2,
-
std::vector<KeyPoint>& keypoints_1,
-
std::vector<KeyPoint>& keypoints_2,
-
std::vector< DMatch >& matches )
-
{
-
//-- 初始化
-
Mat descriptors_1, descriptors_2;
-
// used in OpenCV3
-
Ptr<FeatureDetector> detector = ORB::
create();
-
Ptr<DescriptorExtractor> descriptor = ORB::
create();
-
// use this if you are in OpenCV2
-
// Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
-
// Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
-
Ptr<DescriptorMatcher> matcher = DescriptorMatcher::
create (
"BruteForce-Hamming" );
-
//-- 第一步:检测 Oriented FAST 角点位置
-
detector->
detect ( img_1,keypoints_1 );
-
detector->
detect ( img_2,keypoints_2 );
-
-
//-- 第二步:根据角点位置计算 BRIEF 描述子
-
descriptor->
compute ( img_1, keypoints_1, descriptors_1 );
-
descriptor->
compute ( img_2, keypoints_2, descriptors_2 );
-
-
//-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
-
vector<DMatch> match;
-
//BFMatcher matcher ( NORM_HAMMING );
-
matcher->
match ( descriptors_1, descriptors_2, match );
-
-
//-- 第四步:匹配点对筛选
-
double min_dist=
10000, max_dist=
0;
-
-
//找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
-
for (
int i =
0; i < descriptors_1.rows; i++ )
-
{
-
double dist = match[i].distance;
-
if ( dist < min_dist ) min_dist = dist;
-
if ( dist > max_dist ) max_dist = dist;
-
}
-
-
printf (
"-- Max dist : %f \n", max_dist );
-
printf (
"-- Min dist : %f \n", min_dist );
-
-
//当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
-
for (
int i =
0; i < descriptors_1.rows; i++ )
-
{
-
if ( match[i].distance <=
max (
2*min_dist,
30.0 ) )
-
{
-
matches.
push_back ( match[i] );
-
}
-
}
-
}
-
-
-
Point2d pixel2cam ( const Point2d& p, const Mat& K )
-
{
-
return
Point2d
-
(
-
( p.x - K.
at<
double> (
0,
2 ) ) / K.
at<
double> (
0,
0 ),
-
( p.y - K.
at<
double> (
1,
2 ) ) / K.
at<
double> (
1,
1 )
-
);
-
}
-
-
-
void pose_estimation_2d2d ( std::vector<KeyPoint> keypoints_1,
-
std::vector<KeyPoint> keypoints_2,
-
std::vector< DMatch > matches,
-
Mat& R, Mat& t )
-
{
-
// 相机内参,TUM Freiburg2
-
Mat K = (
Mat_<
double> (
3,
3 ) <<
520.9,
0,
325.1,
0,
521.0,
249.7,
0,
0,
1 );
-
-
//-- 把匹配点转换为vector<Point2f>的形式
-
vector<Point2f> points1;
-
vector<Point2f> points2;
-
-
for (
int i =
0; i < (
int ) matches.
size(); i++ )
-
{
-
points1.
push_back ( keypoints_1[matches[i].queryIdx].pt );
-
points2.
push_back ( keypoints_2[matches[i].trainIdx].pt );
-
}
-
-
//-- 计算基础矩阵
-
Mat fundamental_matrix;
-
fundamental_matrix =
findFundamentalMat ( points1, points2, CV_FM_8POINT );
-
cout<<
"fundamental_matrix is "<<endl<< fundamental_matrix<<endl;
-
-
//-- 计算本质矩阵
-
Point2d principal_point ( 325.1, 249.7 );
//相机光心, TUM dataset标定值
-
double focal_length =
521;
//相机焦距, TUM dataset标定值
-
Mat essential_matrix;
-
essential_matrix =
findEssentialMat ( points1, points2, focal_length, principal_point );
-
cout<<
"essential_matrix is "<<endl<< essential_matrix<<endl;
-
-
//-- 计算单应矩阵
-
Mat homography_matrix;
-
homography_matrix =
findHomography ( points1, points2, RANSAC,
3 );
-
cout<<
"homography_matrix is "<<endl<<homography_matrix<<endl;
-
-
//-- 从本质矩阵中恢复旋转和平移信息.
-
recoverPose ( essential_matrix, points1, points2, R, t, focal_length, principal_point );
-
cout<<
"R is "<<endl<<R<<endl;
-
cout<<
"t is "<<endl<<t<<endl;
-
}
参考：SFU 浙江大学 计算机视觉课程 谭平教授 从相机标定到SLAM，极简三维视觉六小时课程视频（附PPT） | 机器之心