三维重建（9）之相机标定【相机矩阵求解】基本概念汇总

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(单应矩阵，3x3，2D->2D)：二视几何下，一个摄像机上的图像点，与另一幅个摄像机的图像点的对应关系。
  • 
    • ,8个自由度，秩为3.
    •  （使用单应矩阵，求基本矩阵；e'为第二副图像关于第一幅图像的极点）
• P(摄像机矩阵，3x4，3D->2D)：本摄像机坐标系下(光心C为原点)的空间点，与对应图像点之间的关系。
  • 
    • 
    • ，11个自由度
• F(基本矩阵，3x3，2D->2D)：二视几何下，一个摄像机上的图像点，与另一个摄像机的图像上对应极线的关系。
  • 重要公式:
  • (图像点 m 在第二幅图像上的对应点 m′，在极线 上（极线方程）)
    • 基本矩阵和相机内外参数的关系： 
    • , 7个自由度；秩是2。
  • 重要性质：
    • 基本矩阵不依赖于摄像机矩阵的选择，等价地说不依赖于世界坐标系的选择；(吴福朝，12.1.1)
• E(本质矩阵)：描述了两幅规范化图像间的极几何，它与基本矩阵一样也是一个秩为 2 的矩阵。
  • 与基本矩阵的不同之处是它仅与摄像机的运动参数有关。因此，从本质矩阵出发可估计出摄像机的欧氏运动参数。
  • 令令
  • 则 
  • 基本矩阵与本质矩阵的关系：

直接线性变换（DLT）

• 在三维视觉中，用于计算Ap=0的矩阵（A:由2D点和3D点坐标组成的矩阵；p: 2D点和3D点的映射关系的3x4的矩阵，称之为相机的内参）；
• 简言之，用于计算相机的内参数，对相机进行标定；
• 要想求解p, 已知的一种方法是对A做SVD(奇异值分解)；
• 

参考：直接线性变换解法（DLT）用于标定相机 - ambitionzz - 博客园

第7章 直接线性变换解法 - 百度文库

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║

参考：线性代数中||A||怎么算_百度知道

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)。我们通常提到的圆锥曲线包括：椭圆、双曲线和抛物线。)

思想：

将三维空间中，两个相互垂直的向量的齐次形式，投影成无穷远处两个消失点(已知)，从而得到一个关于相机内参矩阵(未知)的方程，进而求得相机的内参矩阵。

几何含义：
在平面上，一个3x3的矩阵就是一个二次曲线（或者说是一个椭圆）；
说明：三维空间无穷远处，包了一个平面，在这个平面上有一个椭圆(也叫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） | 机器之心