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

最新推荐文章于 2023-11-30 08:40:57 发布
最新推荐文章于 2023-11-30 08:40:57 发布
阅读量9.9k 收藏 19
点赞数 2
分类专栏： opencv机器视觉

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


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


由于本质矩阵是平移量t叉乘旋转矩阵R的结果。所以通过本质方程估计出本质矩阵，然后再对本质矩阵进行运动分解，即可得到双目摄像头之间的相互位置关系，即双目的标定。值得一提的是，由于本质矩阵的秩为2，分解出来的平移向量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，即完成双目的相互位置标定。


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


[cpp]  view plain  copy
  1. /************************************************************** 
  2. * an example to estimate essential matrix and relative pose of  
  3. * binocular vision sensor using the API function of opencv3.0.0 
  4. *   
  5. * by jah10527@126.com 
  6. * May, 1st, 2016 
  7. *  
  8. ***************************************************************/  
  9.   
  10.   
  11. #include <opencv2/opencv.hpp>  
  12. #include <iostream>  
  13.   
  14. using namespace cv;  
  15. using namespace std;  
  16.   
  17.   
  18. static void calcChessboardCorners(Size boardSize, float squareSize, vector<Point3f>& corners)  
  19. {  
  20.     corners.resize(0);  
  21.   
  22.     for( int i = 0; i < boardSize.height; i++ )  
  23.         for( int j = 0; j < boardSize.width; j++ )  
  24.             corners.push_back(Point3f(float(j*squareSize),  
  25.                                       float(i*squareSize), 0));  
  26. }  
  27.   
  28. int main()  
  29. {  
  30.     int i, point_count = 54;  
  31.     vector<Point2f> rpoints(point_count);  
  32.     vector<Point2f> lpoints(point_count);  
  33.       
  34.     Mat Ml(3,3,CV_64F);  
  35.     Mat Mr(3,3,CV_64F);  
  36.       
  37.     FileStorage fs("camera_l.yml", FileStorage::READ);  
  38.     if( !fs.isOpened() )  
  39.         return -1;  
  40.     fs["camera_matrix"] >> Ml;  
  41.     fs.release();   
  42.       
  43.     fs.open("camera_r.yml", FileStorage::READ);  
  44.     if( !fs.isOpened() )  
  45.         return -1;  
  46.     fs["camera_matrix"] >> Mr;  
  47.     fs.release();   
  48.       
  49.     cout << "Mr:" << endl << Mr << endl;  
  50.     cout << "Ml:" << endl << Ml << endl;  
  51.       
  52.     Mat rim=imread("fight01.jpg", IMREAD_GRAYSCALE);  
  53.     Mat lim=imread("feft01.jpg", IMREAD_GRAYSCALE);  
  54.     findChessboardCorners( rim, Size(6,9), rpoints,  
  55.                     CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_FAST_CHECK | CALIB_CB_NORMALIZE_IMAGE);  
  56.     findChessboardCorners( lim, Size(6,9), lpoints,  
  57.                     CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_FAST_CHECK | CALIB_CB_NORMALIZE_IMAGE);  
  58. /*  cout << "right:" << endl; 
  59.     for (i=0;i<point_count;i++) 
  60.         cout << rpoints[i] << endl; 
  61.     cout << "left:" << endl; 
  62.     for (i=0;i<point_count;i++) 
  63.         cout << lpoints[i] << endl;*/  
  64.     cornerSubPix( rim, rpoints, Size(8,8),  
  65.             Size(-1,-1), TermCriteria( TermCriteria::EPS+TermCriteria::COUNT, 30, 0.1 ));  
  66.     cornerSubPix( lim, lpoints, Size(8,8),  
  67.             Size(-1,-1), TermCriteria( TermCriteria::EPS+TermCriteria::COUNT, 30, 0.1 ));  
  68.               
  69.     for (i=0;i<point_count;i++)  
  70.     {  
  71.         rpoints[i].x = (rpoints[i].x-*((double*)Mr.data+2))/ *((double*)Mr.data);  
  72.         rpoints[i].y = (rpoints[i].y-*((double*)Mr.data+5))/ *((double*)Mr.data+4);  
  73.         lpoints[i].x = (lpoints[i].x-*((double*)Ml.data+2))/ *((double*)Ml.data);  
  74.         lpoints[i].y = (lpoints[i].y-*((double*)Ml.data+5))/ *((double*)Ml.data+4);  
  75. //      cout << "r:" << rpoints[i] << endl;  
  76. //      cout << "l:" << lpoints[i] << endl;  
  77.     }  
  78.       
  79.     Mat E_mat = findEssentialMat(rpoints, lpoints, 1, Point2d(0, 0), RANSAC, 0.999, 1.f);  
  80.     cout << "E:" << endl << E_mat << endl;  
  81.   
  82. /*  Mat rr(3,1,CV_64F); 
  83.     Mat ll(1,3,CV_64F); 
  84.     *((double*)rr.data+2)=1; 
  85.     *((double*)ll.data+2)=1; 
  86.     for (i=0;i<8;i++){ 
  87.         *((double*)rr.data) = rpoints[i].x; 
  88.         *((double*)rr.data+1) = rpoints[i].y; 
  89.         *((double*)ll.data) = lpoints[i].x; 
  90.         *((double*)ll.data+1) = lpoints[i].y; 
  91.         cout << ll*E_mat*rr << endl; 
  92.         cout << rr.t()*E_mat*ll.t() << endl; 
  93.     } 
  94.     Mat R1, R2, t; 
  95.     decomposeEssentialMat(E_mat, R1, R2, t); 
  96.     cout << "R1:" << endl << R1 << endl; 
  97.     cout << "R2:" << endl << R2 << endl; 
  98.     cout << "t:" << endl << t << endl;*/  
  99.       
  100.     Mat R, t;  
  101.     recoverPose(E_mat, rpoints, lpoints, R, t);  
  102.     cout << "t:" << endl << t << endl;  
  103.     cout << "R:" << endl << R << endl;  
  104.     Mat om;  
  105.     Rodrigues(R, om);  
  106.     cout << "om:" << endl << om << endl;  
  107.       
  108.       
  109.     vector<vector<Point3f> > objpt(1);  
  110.     vector<vector<Point2f> > imgpt(1);  
  111.     vector<vector<Point2f> > imgpt_right(1);  
  112.     for (i=0;i<point_count;i++)  
  113.     {  
  114.         imgpt[0].push_back(rpoints[i]);  
  115.         imgpt_right[0].push_back(lpoints[i]);  
  116.     }  
  117.     calcChessboardCorners(Size(6,9), 0.03, objpt[0]);  
  118.   
  119.     Mat RR, TT, E, F;  
  120.     Mat eye3, zeros5;  
  121.     eye3 = Mat::eye(3,3,CV_64F);  
  122.     zeros5 = Mat::zeros(1,5,CV_64F);  
  123.     double err = stereoCalibrate(objpt, imgpt, imgpt_right, eye3, zeros5, eye3, zeros5,  
  124.                                 Size(640,480), RR, TT, E, F, CALIB_FIX_INTRINSIC,  
  125.                                 TermCriteria( TermCriteria::EPS+TermCriteria::COUNT, 30, 0.1 ));  
  126.     cout << "Pair calibration reprojection error = " << err << endl;  
  127.     Rodrigues(RR, om);  
  128.     cout << "TT:" << endl << TT << endl;  
  129.     cout << "RR:" << endl << RR << endl;  
  130.     cout << "om:" << endl << om <<endl;  
  131.     cout << "E:" << endl << E << endl;  
  132.   
  133.     return 0;  
  134. }  



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


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



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

如梦如幻2015
关注
  • 2
    点赞
  • 踩
  • 19
    收藏
    觉得还不错? 一键收藏
  • 5
    评论
专栏目录
OpenCV——本质矩阵相关函数
m0_55019159的博客
05-13 1692
在计算相机位姿时，通常需要使用本质矩阵来描述两幅图像之间的关系。在OpenCV中，可以使用findEssentialMat函数来计算两幅图像之间的本质矩阵。具体来说，findEssentialMat函数接收三个输入参数：匹配点对、相机内参矩阵和可选的参数。其中，匹配点对是指两幅图像中对应的特征点，相机内参矩阵是相机的内部参数，包括相机的焦距、主点、畸变参数等。可选的参数包括方法类型、RANSAC阈值、RANSAC重复次数等。findEssentialMat函数返回一个输出参数：本质矩阵。
ASIFT-Python-ICP
baidu_40840693的博客
01-07 542
pipeline_python_asift.py # Python 2/3 compatibility from __future__ import print_function import sys import numpy as np import cv2 as cv # local modules from common import Timer from descriptor i...
1_2 图像与图像间恢复pose
fireflychh的博客
05-29 167
采用opencv的findEssentialMat(points1, points2, focal_length, principle_point)函数可以根据图像匹配点计算出本质矩阵，进而通过opencv的recoverPose(essential_matrix, points1,points2,R,t, focal_length, principle_point)计算出旋转R和平移t。由于2d图像计算出的旋转平移没有尺度信息，所以计算过程中采用了归一化信息，即t的模长为1。
[学习笔记-SLAM篇]基础函数findEssentialMat
warningm_dm的博客
12-23 6521
findEssentialMat 求取本征矩阵，使用RANSAC方法，进一步排除失配点。 Mat findEssentialMat( InputArray points1, InputArray points2, InputArray cameraMatrix, int method = RANSAC, double prob = 0.999, double threshold = 1.0,
findEssentialMat
05-02
基于opencv3的本质矩阵估计算例
基于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）或八点算法等...
opencv常用函数表
11-30 1363
函数名 功能说明 cv2.imread() 读取图像文件 cv2.imshow() 显示图像窗口 cv2.imwrite() 保存图像文件 cv2.cvtColor() 颜色空间转换 cv2.resize() 图像缩放 cv2.flip() 图像翻转 cv2.rectangle() 绘制矩形 cv2.circle() 绘制圆形 cv2.line() 绘制直线 cv2.putText() 绘制文本 c
视觉SLAM ch7代码总结（二）
qq_41451702的博客
03-23 5369
目录 一、求解对极约束求解相机运动 二、三角测量 一、求解对极约束求解相机运动 1.特征提取与匹配函数 //特征提取和匹配 void find_feature_matches( const Mat &img_1,const Mat &img_2, vector<KeyPoint> &keypoints1, vector<KeyPoint> &keypoints2, ve
使用cv::findFundamentalMat要注意的几点
wangerwei86的专栏
06-21 1832
在新版的OpenCV中，很多C代码都被重新封装成了C++代码，相应的其调用接口也发生了改变，而文档中的叙述也越来越不清楚，往往导致使用过程中出现各种问题。     在处理立体图像对的时候经常会用到对极几何的知识，计算基础矩阵也是很常见的事。OpenCV实现了基本矩阵的算法。对于老版本的C代码，计算基本矩阵的RANSAC方法中有一个迭代次数不收敛的bug，可能导致每次计算的采样次数都达到最大限
python opencv实现2D-2D对极几何求解位姿以及三角测量
做减求空
03-20 5221
使用jupyter (参考视觉SLAM十四讲第二版第七章源码) # 查看当前kernel下已安装的包 #!pip install opencv-python --user # opencv-python 4.5.1.48 #!pip install -i https://pypi.tuna.tsinghua.edu.cn/simple opencv-python==3.4.8.29 #!pip list --format=columns 1.ORB特征点提取和配准 import cv2 as
OpenCV双目视觉测距
laocui1的博客
04-04 2225
4.计算基础矩阵：使用这些特征点计算基础矩阵，该矩阵可以用于估计两个相机之间的几何关系。2.捕获图像：使用两个相机捕获两个不同视角的图像。5.计算本质矩阵：使用相机内部参数和基础矩阵计算本质矩阵。请注意，这只是OpenCV测量两个相机之间距离的一种方法，还有其他方法可以使用，具体取决于您的具体应用场景和需求。6.计算位移矩阵：使用cv2.stereoRectify()函数来计算视差校正和投影矩阵，并从中提取位移矩阵。7.计算距离：最后，使用位移矩阵和三角剖分算法来计算两个摄像头之间的距离。
6.8-6.11
richlaji的专栏
06-08 399
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 7014
本节内容已在笔记本进行推导分为2D-2D、3D-2D、3D-3D。 三位场景中的同一个三维点在不同视角下的像点存在着一种约束关系：对极约束，基础矩阵E是这种约束关系的代数表示，并且这种约束关系独立与场景的结构，只依赖与相机的内参K和外参R t（相对位姿）。 1、可以通过通过匹配的像点对计算出两幅图像的基础矩阵E， 2、然后分解基础矩阵得到相机的相对位姿R t。 本节内容 对极约束 通过...
根据特征匹配关系，用OpenCV计算基础矩阵F, 本质矩阵E,旋转矩阵R,平移向量t
qq_41253960的博客
04-01 5982
计算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); ..
python用sfm算法获取相机内参矩阵
05-04
SFM (Structure-from-Motion)算法可以用于从一系列2D图像中恢复出3D场景和相机的内外参矩阵。在这个过程中，相机的内参矩阵是一个非常重要的参数，因为它描述了相机的畸变、焦距、主点等信息。 在Python中，可以使用OpenCV库来实现SFM算法，并获取相机的内参矩阵。具体步骤如下： 1. 读取图像序列，并进行特征点提取和匹配。 ```python import cv2 # 读取图像序列 img1 = cv2.imread('image1.jpg') img2 = cv2.imread('image2.jpg') # 特征点提取和匹配 sift = cv2.xfeatures2d.SIFT_create() kp1, des1 = sift.detectAndCompute(img1, None) kp2, des2 = sift.detectAndCompute(img2, None) bf = cv2.BFMatcher() matches = bf.knnMatch(des1, des2, k=2) ``` 2. 通过Essential Matrix计算相机的内外参矩阵。 ```python # 通过Essential Matrix计算相机的内外参矩阵 pts1 = [] pts2 = [] for m, n in matches: if m.distance < 0.75 * n.distance: pts1.append(kp1[m.queryIdx].pt) pts2.append(kp2[m.trainIdx].pt) pts1 = np.int32(pts1) pts2 = np.int32(pts2) F, mask = cv2.findFundamentalMat(pts1, pts2, cv2.FM_RANSAC) E, _ = cv2.findEssentialMat(pts1, pts2, focal=1.0, pp=(0, 0)) _, R, t, mask = cv2.recoverPose(E, pts1, pts2) K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]]) ``` 其中，`fx`和`fy`是相机的焦距，`cx`和`cy`是主点的坐标。 3. 获取相机的内参矩阵。 ```python # 获取相机的内参矩阵 fx = K[0, 0] fy = K[1, 1] cx = K[0, 2] cy = K[1, 2] ``` 这样，就可以使用SFM算法获取相机的内参矩阵了。需要注意的是，这里的相机内参矩阵是基于单应性(Homography)的恢复方法，可能存在误差。如果需要更高精度的相机内参矩阵，可以使用标定板等方法进行相机标定。
评论 5
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明：

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

余额充值