三维重建笔记_相机标定(相机矩阵求解)基本概念汇总
目录1. 数学知识1.1. 单应性矩阵1.2. SVD(奇异值分解)1.3.矩阵范数 ||A||2. 相机知识2.1.相机模型2.2. 相机矩阵2.3 相机标定算法--同时标定内参外参3. 对极几何3.1多视几何基本矩阵 F（Fundamental matrix）本质矩阵 E (Essential matrix)单位矩阵 I (Ident...
目录
1.2 对比: H(单应矩阵)、P(摄像机矩阵)、F(基本矩阵)、E(本质矩阵)
2.3 相机标定算法--同时标定内参外参 -- Resnecting
3.1.1 基本矩阵 F（Fundamental matrix）
3.1.2 本质矩阵 E (Essential matrix)
3.1.3 单位矩阵 I (Identity matrix)
5.1.2 Image of Absolute Conic(绝对二次曲线的映射)
5.1.3 通过Vanishing Points(消失点)求相机内参
5.2 用Circular Points求相机内参(张氏标定)
5.2.2 The Absolute Conic in 3D Space(三维空间中的绝对二次曲线)
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 的矩阵。
- 与基本矩阵的不同之处是它仅与摄像机的运动参数有关。因此，从本质矩阵出发可估计出摄像机的欧氏运动参数。
- 令
令
- 则
- 基本矩阵与本质矩阵的关系：
本质矩阵E，基础矩阵F，单应矩阵H 之间的区别：
直接线性变换（DLT）
- 在三维视觉中，用于计算Ap=0的矩阵（A:由2D点和3D点坐标组成的矩阵；p: 2D点和3D点的映射关系的3x4的矩阵，称之为相机的内参）；
- 简言之，用于计算相机的内参数，对相机进行标定；
- 要想求解p, 已知的一种方法是对A做SVD(奇异值分解)；
参考：直接线性变换解法（DLT）用于标定相机 - ambitionzz - 博客园
1.3 SVD(奇异值分解)
SVD: 通过分解矩阵，来解方程中未知数使用的一种数学方法，比如求本质矩阵E。
从图片的压缩结果可以看出来，奇异值可以被看作成一个矩阵的代表值，或者说，奇异值能够代表这个矩阵的信息。当奇异值越大时，它代表的信息越多。因此，我们取前面若干个最大的奇异值，就可以基本上还原出数据本身。
参考1：SVD（奇异值分解）小结 - EndlessCoding - 博客园
参考2：计算机视觉中的数学方法 8.3.2 ，吴福朝 中科院自动化所
1.4 矩阵范数 ||A||
常用的三种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的定义如下：
3.1.1 基本矩阵 F（Fundamental matrix）
[Oliver Faugeras 1992]
特别注意：这里的p和q表示的是像素点的齐次坐标。
------------------------------------------------------------------------------
------------------------------------------------------------------------------
对比Essential Matrix:
-------------------------------------------------------------------------------
-------------------------------------------------------------------------------
- 两视图的对极几何约束关系，可以通过3X3的基本矩阵F，来描述。
- 即9个未知数，因为是 up to scale（比例放缩）的，所以减少一个自由度，
- 同时 rank(F)=2,即对极几何约束条件（Fe1=0）,所以又减少一个自由度(degree of freedom)，所以有7个DOF.
- 即至少需要7对点来估计F
- 7点算法-求F(Fundamental Matrix)： 三维重建笔记_多视几何_求基本矩阵F_六/七/八点法_误差评估_惊鸿一博-CSDN博客_三维重建重投影误差
- 图像1中p像素点，其对应图像2中像素点q，在极线Fp上。
------------------------------------------------------------------------------
------------------------------------------------------------------------------
- 两个对应点p,q的极线约束方程： q^T F p = 0
- 可以使用基本矩阵F，求出epipoles(极点)
- 因为 Fe = 0
- 推导如下图
------------------------------------------------------------------------------
------------------------------------------------------------------------------
- Fundamental matrix 和 epipolar line有什么用？
- 帮我们缩小了搜索范围（比如在搜索feature点时）；
- 将二维搜索变成一维搜索。
基本矩阵 F的计算步骤
- 1. 求一个初始的F值
- 2.基本矩阵约束条件，调整初始F值（反应到图像）
------------------------------------------------------------------------------
------------------------------------------------------------------------------
------------------------------------------------------------------------------
为什么需要加 rank(F) =2 的约束？
------------------------------------------------------------------------------
为什么需要 data normalization?
优化八点算法，提高F的稳定性？，效果会很好！
data normalization是什么？比如图像，将其归一化到[‐1,1]x[‐1,1]范围内。
3.1.2 本质矩阵 E (Essential matrix)
由来：
特别注意：下面3幅图中p和q，表示的是向量（相机中心连接像素点坐标的方向向量，normalized 过的）
-----------------------------------------------------------------------------------------
-----------------------------------------------------------------------------------------
------------------------------------------------------------------------------------------
-------------------------------------------------------------------------------------------
定义：
特别注意：p和q(下面的一幅图，头上加波浪线)表示的是向量（相机中心连接像素点坐标的方向向量）
------------------------------------------------------------------
------------------------------------------------------------------
对比Fundamental Matrix:
------------------------------------------------------------------
------------------------------------------------------------------
- 1. 基本矩阵F可通过8组对应点线性求解（8点法）、本质矩阵E可通过5组对应点求解（5点法，因为E=R[t]x ,R旋转矩阵也有3个自由度，t有2个自由度，所以5个自由度至少需要5个点）；
- 2. 通过SVD分解，可以从本质矩阵E中，分解相机R和t；
- 3. 已知相机K、 R、 t，可通过三角化求解三维点X；
- 4. 至此，得到两视图重建的初始值，之后通过BA（Bundle Adjustment）对相机内参、相机位姿和三维结构K、 R、 t、 X进行非线性优化；
- 三维重建笔记_相机标定_求本质矩阵E：三维重建笔记_相机标定_求本质矩阵E_惊鸿一博-CSDN博客
3.1.3 单位矩阵 I (Identity matrix)
众所周知，在矩阵的乘法中，有一种矩阵起着特殊的作用，如同数的乘法中的1，这种矩阵被称为单位矩阵。它是个方阵，从左上角到右下角的对角线（称为主对角线）上的元素均为1。除此以外全都为0。
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 用Circular Points求相机内参(张氏标定)
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） | 机器之心
开放原子开发者工作坊旨在鼓励更多人参与开源活动，与志同道合的开发者们相互交流开发经验、分享开发心得、获取前沿技术趋势。工作坊有多种形式的开发者活动，如meetup、训练营等，主打技术交流，干货满满，真诚地邀请各位开发者共同参与！
更多推荐
32
0- 0
已为社区贡献2条内容
所有评论(0)