基于opencv3的本质矩阵（essential matrix）估计算例

最新推荐文章于 2024-06-10 21:38:37 发布
最新推荐文章于 2024-06-10 21:38:37 发布
阅读量1.7w 收藏 31
点赞数 9
分类专栏： opencv3 文章标签： opencv3 findEssentialMat essential matrix 本质矩阵 5点算法
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/J10527/article/details/51295099
版权

至今opencv3面世快两年了，五一期间抽空看了下新增的几个关于双目视觉标定算法的使用。


关于双目的标定有一类方法是基于本质矩阵（Essential matrix）的标定方法，本质方程与基础方程都是刻画双目视觉极几何关系的两个方程，只不过它们所使用的坐标系不一样而已，本质方程是在相机光心坐标系下描述的双目成像几何关系，而基础方程是本质方程在相机像素坐标系下的表示。


由于本质矩阵是平移量t叉乘旋转矩阵R的结果。所以通过本质方程估计出本质矩阵，然后再对本质矩阵进行运动分解，即可得到双目摄像头之间的相互位置关系，即双目的标定。值得一提的是，由于本质矩阵的秩为5，分解出来的平移向量t只能得到方向，其模需要使用其它方法来获取。


理论上可以直接使用估计基础矩阵（Fundamental matrix）的方法来估计本质矩阵，如：8点算法，7点算法，及与之结合的最小中值法，RANSAC（随机抽样一致）法等。但是实际中使用这些方法估计出的本质矩阵的结果并不太令人满意，尤其当匹配点对趋于共面时和平移量较小时，这些方法获得的结果完全就不能用。如果用于标定的匹配点对，是在一个平面上的话，这些方法将完全失效，因为平面是一种退化结构或者叫临界曲线（Critical surfaces）。而且在opencv3之前的opencv版本只提供了求取基础矩阵F的API，虽然cv::stereoCalibrate(...)可以得到双目标定结果以及本质矩阵E，但该方法是使用平面标定版辅助的plane2plane（单应性矩阵）估计出各相机的外参数，然后由外参数直接计算出图像相互之间的旋转矩阵R和平移量t，此时获取的刚体运动参数自由度为6。


此外双目标定还有一些方法并不是基于本质矩阵来完成的，如：光束法平差（Bundle adjustment, 摄影测量学叫法）、结构重投影法（机器视觉叫法）等。在此就不进行讨论了。


由于基础矩阵的自由度为7，而本质矩阵的自由度为5。直接使用基础矩阵的估计方法来估计本质矩阵并不适合，而且本质矩阵还有一种叫做5点算法的方法可以估计本质矩阵，该算法比较复杂，涉及到高次多项式方程的求解。但是对于估计本质矩阵，5点算法会得到比7点或8点算法更好的结果，而且不受匹配点集共面的影响。该方法的具体原理可见论文David Nistér. An efficient solution to the five-point relative pose problem. Pattern Analysis and Machine Intelligence, IEEE Transactions on, 26(6):756–770, 2004.和Henrik Stewenius. Calibrated fivepoint solver.


5点算法的大概思路就是把本质方程展开，再加上本质矩阵的充要条件。化简后得到一个高次多项式方程，该多项式实数解中的一个确定出的本质矩阵是系统的真实本质矩阵，对伪解进行剔除。然后对获得的本质矩阵进行使用SVD分解而得到旋转矩阵R和单位平移量t，通常有四对旋转平移组合，进行cheirality check（三维点在相机前方约束），留下一组物理可实现的R,T。即完成双目的相互位置标定。


opencv3中的cv::findEssentialMat(...)即是基于该五点算法实现的本质矩阵的估计，附上使用代码


/**************************************************************
* an example to estimate essential matrix and relative pose of 
* binocular vision sensor using the API function of opencv3.0.0
*  
* by jah10527@126.com
* May, 1st, 2016
* 
***************************************************************/


#include <opencv2/opencv.hpp>
#include <iostream>

using namespace cv;
using namespace std;


static void calcChessboardCorners(Size boardSize, float squareSize, vector<Point3f>& corners)
{
    corners.resize(0);

    for( int i = 0; i < boardSize.height; i++ )
        for( int j = 0; j < boardSize.width; j++ )
            corners.push_back(Point3f(float(j*squareSize),
                                      float(i*squareSize), 0));
}

static bool readStringList( const string& filename, vector<string>& l )
{
    l.resize(0);
    FileStorage fs(filename, FileStorage::READ);
    if( !fs.isOpened() )
        return false;
    FileNode n = fs.getFirstTopLevelNode();
    if( n.type() != FileNode::SEQ )
        return false;
    FileNodeIterator it = n.begin(), it_end = n.end();
    for( ; it != it_end; ++it )
        l.push_back((string)*it);
    return true;
}

void calc3Dpts(vector<Point2f> &pr, vector<Point2f> &pl,Mat &MR,Mat &ML, vector<Point3f> &pts)
{
	
	Mat cof(4,4,CV_64F), z;
	double u1,v1,u2,v2;
	int len, i, j;
	len = pr.size();
	pts.reserve(len);
	for (i=0; i < len; i++)
	{
		u1=pr[i].x;
		v1=pr[i].y;
		u2=pl[i].x;
		v2=pl[i].y;
		
		*((double*)cof.data)=*((double*)MR.data+8)*u1-*((double*)MR.data);
		*((double*)cof.data+1)=*((double*)MR.data+9)*u1-*((double*)MR.data+1);
		*((double*)cof.data+2)=*((double*)MR.data+10)*u1-*((double*)MR.data+2);
		*((double*)cof.data+3)=*((double*)MR.data+11)*u1-*((double*)MR.data+3);
		*((double*)cof.data+4)=*((double*)MR.data+8)*v1-*((double*)MR.data+4);
		*((double*)cof.data+5)=*((double*)MR.data+9)*v1-*((double*)MR.data+5);
		*((double*)cof.data+6)=*((double*)MR.data+10)*v1-*((double*)MR.data+6);
		*((double*)cof.data+7)=*((double*)MR.data+11)*v1-*((double*)MR.data+7);
		*((double*)cof.data+8)=*((double*)ML.data+8)*u2-*((double*)ML.data);
		*((double*)cof.data+9)=*((double*)ML.data+9)*u2-*((double*)ML.data+1);
		*((double*)cof.data+10)=*((double*)ML.data+10)*u2-*((double*)ML.data+2);
		*((double*)cof.data+11)=*((double*)ML.data+11)*u2-*((double*)ML.data+3);
		*((double*)cof.data+12)=*((double*)ML.data+8)*v2-*((double*)ML.data+4);
		*((double*)cof.data+13)=*((double*)ML.data+9)*v2-*((double*)ML.data+5);
		*((double*)cof.data+14)=*((double*)ML.data+10)*v2-*((double*)ML.data+6);
		*((double*)cof.data+15)=*((double*)ML.data+11)*v2-*((double*)ML.data+7);
		
	//	cout << "cof:" << endl << cof << endl;
		SVD::solveZ(cof, z);
		for (j=0;j<3;j++) *((double*)z.data+j)/=*((double*)z.data+3);
		pts.push_back(Point3f(float(*((double*)z.data)), float(*((double*)z.data+1)), float(*((double*)z.data+2))));
	}
}


int main()
{
	int i, point_count = 54;
	
	Mat Kl, Dl, imagePointsL, rtL;
	Mat Kr, Dr, imagePointsR, rtR;
	
  FileStorage fs("out_camera_left.yml", FileStorage::READ);
  if( !fs.isOpened() )
    return -1;
  fs["camera_matrix"] >> Kl;
  fs["distortion_coefficients"] >> Dl;
  fs["image_points"] >> imagePointsL;
  fs["extrinsic_parameters"] >> rtL;
	fs.release(); 
	
	fs.open("out_camera_right.yml", FileStorage::READ);
    if( !fs.isOpened() )
      return -1;
  fs["camera_matrix"] >> Kr;
  fs["distortion_coefficients"] >> Dr;
  fs["image_points"] >> imagePointsR;
  fs["extrinsic_parameters"] >> rtR;
	fs.release(); 
	
	cout << "Kr:" << endl << Kr << endl;
	cout << "Kl:" << endl << Kl << endl;
	cout << "Dr:" << endl << Dr << endl;
	cout << "Dl:" << endl << Dl << endl;
	cout << "imagePointsL channels:" << endl << imagePointsL.channels() << endl;
	cout << "imagePointsR channels:" << endl << imagePointsR.channels() << endl;
	
	Mat rvecL(3,1,CV_64F,rtL.data);
	Mat tvecL(3,1,CV_64F,rtL.data+3*sizeof(double));
	Mat rvecR(3,1,CV_64F,rtR.data);
	Mat tvecR(3,1,CV_64F,rtR.data+3*sizeof(double));
	Mat rotL, rotR;
	Rodrigues(rvecL, rotL);
	Rodrigues(rvecR, rotR);
	
	vector<vector<Point2f>> ptsL, ptsR, ptsLu, ptsRu;
	ptsL.resize(1);
	ptsR.resize(1);
	ptsLu.resize(1);
	ptsRu.resize(1);

	for (int j=0;j<1;j++)
	{
		for (i=0;i<imagePointsL.cols;i++)
		{
//		  cout << imagePointsL.at<Point2f>(j,i) << endl;
			ptsL[j].push_back(imagePointsL.at<Point2f>(j,i));
			ptsR[j].push_back(imagePointsR.at<Point2f>(j,i));
		}
		undistortPoints(ptsL[j], ptsLu[j], Kl, Dl);
	  undistortPoints(ptsR[j], ptsRu[j], Kr, Dr);
	}
/*  FILE *fp;
  fp = fopen("data.txt","w");
	fprintf(fp,"L:\n");
	for (i=0;i<imagePointsL.cols;i++)
	{
    fprintf(fp,"%f,%f\n",ptsLu[0][i].x,ptsLu[0][i].y);
	  cout << ptsLu[0][i] << endl;
	}
  cout << "distorted---undistorted:R" << endl;
  fprintf(fp,"L:\n");
  for (i=0;i<imagePointsL.cols;i++)
	{
    fprintf(fp,"%f,%f\n",ptsRu[0][i].x,ptsRu[0][i].y);
	  cout << ptsRu[0][i] << endl;
	}
	fclose(fp);*/
	cout << "out" << endl;
	Mat E_mat;
	E_mat = findEssentialMat(ptsLu[0], ptsRu[0], 1, Point2d(0, 0), RANSAC, 0.999, 1.f);
	cout << "E:" << endl << E_mat/E_mat.at<double>(2,2) << endl;

	for (i=0;i<ptsLu[0].size();i++)
	{
	  Mat ptL, ptR, ptL64, ptR64;
	  ptL = Mat(ptsLu[0][i]);
	  ptL.push_back(1.0f);
	  ptR = Mat(ptsRu[0][i]);
	  ptR.push_back(1.0f);
	  ptL.convertTo(ptL64,CV_64F);
	  ptR.convertTo(ptR64,CV_64F);
	  CV_Assert(ptL64.depth() == E_mat.depth());
		cout << ptR64.t()*E_mat*ptL64 << endl;
//		cout << ptL64.t()*E_mat*ptR64 << endl;
	}
  /*
	Mat R1, R2, t_;
	decomposeEssentialMat(E_mat, R1, R2, t_);
	cout << "R1:" << endl << R1 << endl;
	cout << "R2:" << endl << R2 << endl;
	cout << "t_:" << endl << t_ << endl;*/
	
	Mat R, t;
	recoverPose(E_mat, ptsLu[0], ptsRu[0], R, t);
	cout << "t:" << endl << t << endl;
	cout << "R:" << endl << R << endl;

	vector<vector<Point3f>> objpt(ptsL.size());
	for (i=0;i<ptsL.size();i++)
		calcChessboardCorners(Size(6,9), 0.03, objpt[i]);

	Mat RR, TT, E, F;
	double err = stereoCalibrate(objpt, ptsL, ptsR, Kl, Dl, Kr, Dr,
								Size(640,480), RR, TT, E, F, CALIB_FIX_INTRINSIC,
								TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 100, 1e-6) );
	cout << "Pair calibration reprojection error = " << err << endl;
	cout << "TT:" << endl << TT << endl;
	cout << "RR:" << endl << RR << endl;
	cout << "E:" << endl << E/E.at<double>(2,2) << endl;
  cout << "R:" << rotR*rotL.t() << endl;
  cout << "t:" << tvecR-rotR*rotL.t()*tvecL << endl;
	
	Mat ML(3,4,CV_64F);
	Mat MR(3,4,CV_64F);
	Kl.copyTo(ML.colRange(0,3));
  ML.col(3)=Mat::zeros(3,1,CV_64F);
	MR.colRange(0,3)=Kr*R;
	MR.col(3)=Kr*t;
	cout << "ML:" << endl << ML << endl;
	cout << "MR:" << endl << MR << endl;
	
	
	vector<Point3f> pts;
	calc3Dpts(ptsLu[0], ptsRu[0], MR, ML, pts);
	cout << pts.size() << endl;
	for (i=0;i<pts.size();i++) cout << pts[i] << endl;

	return 0;
}



可以看出两种标定方法获取的结果不太一样。


重构出的标定版的三维坐标如下图所示，红色为本质矩阵方法获得的标定版空间情况，蓝色为stereoCalibrate(...)的运动参数三维重构标定版角点坐标的结果。



源码及数据下载点击打开链接


在实际使用中，建议尽量避免匹配点对来自于同一个平面，且必须有平移量，这样估计出的本质矩阵会好很多！如果只有旋转的存在，无平移量，可以参考http://blog.csdn.net/j10527/article/details/79077739

J10527
关注
  • 9
    点赞
  • 踩
  • 31
    收藏
    觉得还不错? 一键收藏
  • 10
    评论
专栏目录
findEssentialMat
05-02
基于opencv3的本质矩阵估计算例
opencv--笔记——本征矩阵&基础矩阵
Jhonny_yang的博客
09-06 1605
本征矩阵E 本征矩阵E ：描述相机之间的位姿关系； 定义：矩阵E包含在物理空间中两个摄像机相关的旋转和平移信息； opencv 求本征矩阵函数原型如下： Mat findEssentialMat( InputArray points1, InputArray points2, InputArr...
什么是正交矩阵？
最新发布
...
06-10 1087
一个简单的2x2正交矩阵例子是旋转矩阵`，它代表了二维平面上的一个特定角度的旋转操作，且保持向量的长度和方向间的直角关系不变。因此，上述矩阵满足正交矩阵的定义。
[学习笔记-SLAM篇]基础函数findEssentialMat
warningm_dm的博客
12-23 6487
findEssentialMat 求取本征矩阵，使用RANSAC方法，进一步排除失配点。 Mat findEssentialMat( InputArray points1, InputArray points2, InputArray cameraMatrix, int method = RANSAC, double prob = 0.999, double threshold = 1.0,
OpenCV——本质矩阵相关函数
m0_55019159的博客
05-13 1646
在计算相机位姿时，通常需要使用本质矩阵来描述两幅图像之间的关系。在OpenCV中，可以使用findEssentialMat函数来计算两幅图像之间的本质矩阵。具体来说，findEssentialMat函数接收三个输入参数：匹配点对、相机内参矩阵和可选的参数。其中，匹配点对是指两幅图像中对应的特征点，相机内参矩阵是相机的内部参数，包括相机的焦距、主点、畸变参数等。可选的参数包括方法类型、RANSAC阈值、RANSAC重复次数等。findEssentialMat函数返回一个输出参数：本质矩阵。
findEssentialMat参数
热门推荐
Pandas_xxx的博客
06-13 1万+
求取本征矩阵，使用RANSAC方法，进一步排除失配点函数原型：Mat findEssentialMat( InputArray points1, InputArray points2, InputArray cameraMatrix, int method = RANSAC, ...
计算基本矩阵和本质矩阵，以及旋转角度和平移量
qq_43432872的博客
05-11 1866
计算本质矩阵 第一步 用手机拍摄不同角度的两张照片，有平移有旋转，保存至文件夹 第二步 检测两张图片的特征点，并保存特征点检测图片，代码如下 import numpy as np import cv2 from matplotlib import pyplot as plt #根据不同的照片格式，选择不同的方法 img11 = cv2.imread('1.jpg') img12 = cv2.imread('2.jpg')#导入第一问的照片 img11 = cv2.cvtColor(img11, cv2.
基于Matlab和OpenCV的双目测距1
08-08
外参包括旋转矩阵（Rotation Matrix）、平移矩阵（Translation Matrix）、基础矩阵（Fundamental Matrix）和特征矩阵（Essential Matrix）。旋转矩阵描述了相机2相对于相机1的旋转，平移矩阵表示相机2在相机1坐标系...
视觉里程计-基于OpenCV实现的视觉里程计-附项目源码-优质项目实战.zip
05-23
4. **姿态估计**：通过匹配的特征点对，运用RANSAC算法剔除异常值，然后使用Epipolar Geometry（极几何）理论，如 Essential Matrix 或 Fundamental Matrix 来计算两帧之间的相对旋转和平移。最终得到的运动模型可以...
基础矩阵-Python代码-计算机视觉
05-23
在计算机视觉领域，基础矩阵（Fundamental Matrix）是一种重要的数学工具，用于描述两幅图像之间的几何关系，特别是在处理由同一场景但不同视角拍摄的图像对时。基础矩阵是由两摄像机的内参和相对姿态决定的，包含了...
基于opencv的三维重建代码
04-10
4. **三角测量**：找到匹配的关键点后，可以使用Epipolar Geometry（极几何）理论，如Essential Matrix（基础矩阵）或Fundamental Matrix（基础矩阵）来估计相机间的关系。接着，通过RANSAC（随机抽样一致）等方法...
FundamentalMatrix：使用已校准的摄像机进行基本矩阵估计
02-17
3. **基本矩阵的估计**：基本矩阵F满足由Epipolar Geometry理论得出的几个几何约束，例如Essential Matrix E可以通过外参和内参推导得出，而E可以进一步分解得到F。此外，还可以使用直接线性变换（DLT）或八点算法等...
python 立体匹配_真实场景的双目立体匹配（Stereo Matching）获取深度图详解
weixin_39729115的博客
12-04 2319
双目立体匹配一直是双目视觉的研究热点，双目相机拍摄同一场景的左、右两幅视点图像，运用立体匹配匹配算法获取视差图，进而获取深度图。而深度图的应用范围非常广泛，由于其能够记录场景中物体距离摄像机的距离，可以用以测量、三维重建、以及虚拟视点的合成等。之前有两篇博客简要讲过OpenCV3.4中的两种立体匹配算法效果比较：http://www.cnblogs.com/riddick/p/8318997.ht...
根据特征匹配关系，用OpenCV计算基础矩阵F, 本质矩阵E,旋转矩阵R,平移向量t
qq_41253960的博客
04-01 5966
计算F cv::Mat F = cv::findFundamentalMat(vector<cv::Point2f>& points1, vector<cv::Point2f>& points2, int method, ... ); 计算E cv::Mat E = cv::findEssentialMet(points1, points2, K); ..
【多视几何】对极几何（Epipolar Geometry）基础及OpenCV实现：对极约束、基础矩阵、本质矩阵和单应矩阵
CCC的专栏
04-16 9173
对极几何（Epipolar Geometry）是计算机视觉理论的基础，它描述了同一场景中两幅图像（2D-2D）之间的视觉几何关系，在图像匹配、三维重建等领域应用广泛。本文中将涉及到以下知识点： 对极约束（Epipolar constraint） 基础矩阵（Fundamental Matrix） 本质矩阵（Essential Matrix） 单应矩阵（Homography Matrix） 1 对极约束（Epipolar constraint） 1.1 基本术语 首先看一个理想情况（无噪声）。如图所示为两
3月12日 对极几何，本征矩阵，基础矩阵，F/E矩阵计算，恢复旋转与平移，三角化视图重建
Hali_Botebie的博客
03-12 3287
本征矩阵——相机坐标系的关系&基础矩阵——图像像素坐标系的关系（加入内参） 链接：https://blog.csdn.net/ssw_1990/article/details/53355572 本征矩阵$$E包含物理空间中两个摄像机相关的旋转和平移信息，本征矩阵E将左摄像机观测到的点的物理坐标与右摄像机观测到的相同点的位置关联起来。 基本矩阵FFF除了包含EEE的信息外，还包含两个摄像...
6.8-6.11
richlaji的专栏
06-08 398
6.8 特征点的计算和描述子的计算可以用OpenCV中实现好的类，比如ORB类实例化一个对象，然后调用对象的detect方法计算关键点位置，compute方法计算描述子 DMatch中distance是描述子的距离，而不是两个匹配点之间坐标的距离，所以是越小越好。然后距离是根据匹配的方法不同而不同，比如如果用NORM_HAMMING就是汉明距离，整数，如果是L2则是浮点数 如何输出描述子可以查看ht
《视觉SLAM十四讲精品总结》6.1：VO—— 2D-2D对极约束求位姿R、t
try_again_later的博客
08-13 7013
本节内容已在笔记本进行推导分为2D-2D、3D-2D、3D-3D。 三位场景中的同一个三维点在不同视角下的像点存在着一种约束关系：对极约束，基础矩阵E是这种约束关系的代数表示，并且这种约束关系独立与场景的结构，只依赖与相机的内参K和外参R t（相对位姿）。 1、可以通过通过匹配的像点对计算出两幅图像的基础矩阵E， 2、然后分解基础矩阵得到相机的相对位姿R t。 本节内容 对极约束 通过...
cv2.findEssentialMat
09-19
`cv2.findEssentialMat` 是 OpenCV 中的一个函数，用于从两张图像的特征点中计算本质矩阵。本质矩阵是相机的内参矩阵和外参矩阵的组合，它可以描述两个视角之间的相对运动关系。 该函数的语法如下： ``` retval, mask = cv2.findEssentialMat(points1, points2, cameraMatrix[, method[, prob[, threshold[, mask]]]]) ``` 其中，参数含义如下： - `points1`：第一张图像中的特征点坐标，可以是一个 `N` 行 2 列的浮点型数组，也可以是一个 `1` 行 `N` 列的单通道矩阵。 - `points2`：第二张图像中对应的特征点坐标，和 `points1` 的形状相同。 - `cameraMatrix`：相机内参矩阵，是一个 `3` 行 `3` 列的浮点型数组。 - `method`：计算本质矩阵的方法，有两种选择：`cv2.RANSAC` 和 `cv2.LMEDS`，默认为 `cv2.RANSAC`。 - `prob`：RANSAC 方法中选择随机样本的概率，默认为 `0.999`。 - `threshold`：RANSAC 方法中选择内点的阈值，默认为 `1.0`。 - `mask`：用于标记内点和外点的掩码，如果为 `None`，则所有点都被视为内点。输出的 `mask` 是一个 `1` 行 `N` 列的单通道矩阵，内点对应的值为 `1`，外点对应的值为 `0`。 该函数的返回值包括两个： - `retval`：计算出来的本质矩阵，是一个 `3` 行 `3` 列的浮点型数组。 - `mask`：用于标记内点和外点的掩码，与输入参数 `mask` 的形状相同。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 10
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值