本節內容已在筆記本進行推導分為2D-2D、3D-2D、3D-3D。

三位場景中的同一個三維點在不同視角下的像點存在著一種約束關係：對極約束，基礎矩陣E是這種約束關係的代數表示，並且這種約束關係獨立與場景的結構，只依賴與相機的內參K和外參R t（相對位姿）。

1、可以通過通過匹配的像點對計算出兩幅影象的基礎矩陣E，

2、然後分解基礎矩陣得到相機的相對位姿R t。

本節內容

對極約束

通過匹配點估算基礎矩陣F

OpenCV介面程式碼實現

總結

一、對極約束

1、對極幾何描述的是兩檢視之間的記憶體射影關係，同一個三維點在兩個不同的視角下的像點存在著約束關係，如下圖三維點XX在兩幅影象的像點分分別為x,x′（2D-2D）

已知：x和x'畫素座標，相機內參K

求解：X空間座標（XYZ）、外參R、t

2、單應矩陣H幾何意義：

2D單應H指的是將射影平面上的點集xixi對映到另一個射影平面的點集x′ixi′上的射影變換

3、基礎矩陣F幾何意義：

點到直線的對映      F=e^ H

4、核心公式推導：

二、通過匹配點對估算基礎矩陣F

基礎矩陣F 表示的是影象中的像點p1到另一幅影象對極線l2的對映，有如下公式： l2=F p1.

對極約束：畫素點p2必定在對極線l2上           

這樣僅通過匹配的點對p1 p2，就可以計算出兩檢視的基礎矩陣F。

1、8點法：基礎矩陣F是一個3×3的矩陣，有9個未知元素。然而，上面的公式中使用的齊次座標，齊次座標在相差一個常數因子下是相等，F也就只有8個未知元素，也就是說，只需要8對匹配的點對就可以求解出兩檢視的基礎矩陣F。

2、最小二乘法：

8點法中，只使用8對匹配的點對估計基礎矩陣，但通常兩幅影象的匹配的點對遠遠多於8對，可以利用更多匹配的點對來求解上面的方程。

基礎矩陣F的秩為2

三、OpenCV 介面

一、整體框架

#include <iostream>
#include <opencv2/opencv.hpp>
using namespace std;
using namespace cv;

//宣告函式
//兩張影象得到關鍵點（keypoints）和描述子之間匹配關係（matches）
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);
//從特徵點(keypoints)和匹配關係（matches）求解位姿(R,t)
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)
{
	//1. 讀取影象
	Mat img_1 = imread("1.png");
	Mat img_2 = imread("2.png");
	//2. 得到關鍵點（keypoints）和描述子之間匹配關係（matches）
	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;
	//3. 估計兩張影象間運動R t
	Mat R, t;
	pose_estimation_2d2d(keypoints_1, keypoints_2, matches, R, t);
	//4. 驗證E=t^R*scale,t^是t的反對稱矩陣
	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;
	//5. 驗證對極約束
	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;
}
 

•  通過對極約束對求得的R t進行驗證  
• x2 和 x1是歸一化座標（畫素座標系p到相機座標系x）   

• 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)
  		);
  }

• 使用齊次座標，3*1 ，最後一個是1。
• keypoints使用vector<KeyPoint> 型別； matches使用vector<DMatch> ; Mat& R; Mat& t;

二、兩張影象得到關鍵點（keypoints）和描述子之間匹配關係（matches）

上一節知識點。

三、從特徵點(keypoints)和匹配關係（matches）求解位姿(R,t)

void pose_estimation_2d2d(std::vector<KeyPoint> keypoints_1,
	std::vector<KeyPoint> keypoints_2,
	std::vector< DMatch > matches,
	Mat& R, Mat& t)
{
	// 相機內參K
	Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);

	//-- 對齊匹配的點對，並用.pt轉化為畫素座標。把匹配點轉換為簡單的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);//queryIdx第一個影象索引
		points2.push_back(keypoints_2[matches[i].trainIdx].pt);//trainIdx第二個影象索引
	}

	//-- 計算基礎矩陣F
	Mat fundamental_matrix;
	fundamental_matrix = findFundamentalMat(points1, points2, CV_FM_8POINT);
	cout << "fundamental_matrix is " << endl << fundamental_matrix << endl;

	//-- 計算本質矩陣E
	Point2d principal_point(325.1, 249.7);	//相機光心
	double focal_length = 521;			//相機焦距

	Mat essential_matrix;
	essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);
	cout << "essential_matrix is " << endl << essential_matrix << endl;

	//-- 計算單應矩陣H
	Mat homography_matrix;
	homography_matrix = findHomography(points1, points2, RANSAC, 3);
	cout << "homography_matrix is " << endl << homography_matrix << endl;

	//-- 從本質矩陣E中恢復旋轉和平移資訊.
	recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);
	cout << "R is " << endl << R << endl;
	cout << "t is " << endl << t << endl;

}

1、findFundamentalMat函式（計算基礎矩陣F）

Mat cv::findFundamentalMat(points1,points2,method = FM_RANSAC,
double ransacReprojThreshold = 3.,double confidence = 0.99, OutputArray mask = noArray() )

points1,points2 是匹配點對的畫素座標，並且能夠一一對應

method 使用那種方法，預設的是FM_RANSAC也就是RANSAC的方法估算基礎矩陣。CV_FM_8POINT是8點法。

ransacReprojThreshold表示RANSAC迭代過程中，判斷點是內點還是外點的閾值（到對極線的畫素距離）；

confidence表示內點佔的比例，以此來判斷估計出的基礎矩陣是否正確。

2、findEssentialMat函式（計算本質矩陣E）

Mat cv::findEssentialMat(points1,points2,cameraMatrix,method = RANSAC,
double prob = 0.999,double threshold = 1.0,OutputArray mask = noArray() )	

 cameraMatrix        

Mat K=(Mat_<double> (3,3)<<520.9,0,325.1,0,521.0,249.7,0,0,1);

prob 本質矩陣E正確的概率

threshold  RANSAC閾值

3、findHomography函式（計算單應矩陣H）

Mat cv::findHomography	(Points1,Points2,method = RANSAC,
double 	ransacReprojThreshold = 3,OutputArray mask = noArray(),
const int 	maxIters = 2000, const double 	confidence = 0.995 )	

 maxIter 最大迭代次數

4、computeCorrespondEpilines函式（計算對極線）

void cv::computeCorrespondEpilines(points,int whichImage,F,OutputArray lines )

5、recoverPose函式（從 E 中恢復R和t）

int cv::recoverPose	(E,points1,points2,cameraMatrix,R,t,mask = noArray() )	

int cv::recoverPose	(E,points1,points2,R,t,double focal = 1.0,Point2d pp = Point2d(0, 0),
mask = noArray() )	

focal： 相機焦距

pp ：相機光心

四、總結

1、主要用E分解運動，H需要假設特徵點在平面上。

2、E本身具有尺度等價性，通常對t進行歸一化，令它的長度為1。

• 尺度不確定性

對t的歸一化 相當於固定了尺度，導致單目視覺的尺度不確定性，釐米還是米

把特徵點深度歸一化可以控制場景規模大小，樹枝上更穩定些。

 純旋轉導致t 為0，E也是0 ，無法求解R。單目初始化不能只有純旋轉，必須要有一定平移。

• 初始化的純旋轉
• 多餘8點的情況

傾向於用RANSAC