關于OpenCV的那些事——利用RANSAC消除錯誤姿态

上一節《關于OpenCV的那些事——跟蹤點選取方式和特征點跟蹤恢複》講了兩種跟蹤和恢複的方法，這一篇主要講第一個優化，使用random sample consensus收斂相機姿态。下一篇講使用最小二乘多項式平滑消除姿态抖動。

我們知道在計算相機姿态的時候，opencv中提供了兩種函數：solvePnP, solvePnPRansac。 第二個函數即是利用ransac的思想計算更加精确的姿态。 鑒于之前章節《關于OpenCV的那些事——相機姿态更新》裡講到的相機姿态更新至少使用4組2D/3D點對，我們自己試着實作一下RANSAC。 思想是對于追蹤的n個特征點對，我們先随機生成m個4組點對（m < Cn4(排列組合)），分别計算出m個姿态，然後對于每一個姿态計算重投影誤差，小于一定閥值的記錄下來，并更新最佳姿态（最小誤差），最終傳回這個最佳姿态。m也叫疊代次數。當然選擇合适的m，既能節省時間，有能找到最佳姿态。重投影誤差的閥值也需要做實驗找到最合适的。

C++代碼如下：

bool collinear_ornot(Point2f p1, Point2f p2, Point2f p3)   // 三點是否共線
{
	if (abs((p2.x - p1.x)*p3.y - (p2.y - p1.y)*p3.x - p1.y*p2.x + p1.x*p2.y) < 1e-5)
		return true;
	else
		return false;
}
           

void random_n_4p(vector<Point2f>& imgP)
{
	srand((unsigned)time(NULL));
	int n = imgP.size();           //n為追蹤的特征點的個數
	for (int i = 0; i < ransac_1; i++)    // ransac_1為上文中的m疊代次數，本項目中取20
	{
		do
		{
			n_4[i][0] = rand() % n;
			do
			{
				n_4[i][1] = rand() % n;
			} while (n_4[i][1] == n_4[i][0]);
			do
			{
				n_4[i][2] = rand() % n;
			} while (n_4[i][2] == n_4[i][1] || n_4[i][2] == n_4[i][0]);
			do
			{
				n_4[i][3] = rand() % n;
			} while (n_4[i][3] == n_4[i][2] || n_4[i][3] == n_4[i][1] || n_4[i][3] == n_4[i][0]);
		}while (collinear_ornot(imgP[n_4[i][0]], imgP[n_4[i][1]], imgP[n_4[i][2]]) || collinear_ornot(imgP[n_4[i][0]], imgP[n_4[i][1]], imgP[n_4[i][3]]) || collinear_ornot(imgP[n_4[i][1]], imgP[n_4[i][2]], imgP[n_4[i][3]]));
	}       //確定四點中每每三點不共線
}
           

void ransac_cc(vector<Point2f>& imgP, Mat& r, Mat& t)
{
	random_n_4p(imgP);
	
	vector<Point2f> temp4imgP;
	vector<Point3f> temp4objP;
	vector<Point2f> temprepP;
	temprepP.resize(imgP.size());
	Mat tempobjPM;
	vector<double> temprv(3), temptv(3), bestr(3),bestt(3);
	Mat temp_r(temprv),temp_t(temptv),best_r(bestr),best_t(bestt);
	float minerror = 1,temperror = 0, errorthreshold = 1;   //重投影誤差閥值為1 pixel
	float testerror = 0;
	for (size_t iteration = 0; iteration < ransac_1; iteration++)
	{
		temp4imgP.push_back(imgP[n_4[iteration][0]]);
		temp4imgP.push_back(imgP[n_4[iteration][1]]);
		temp4imgP.push_back(imgP[n_4[iteration][2]]);
		temp4imgP.push_back(imgP[n_4[iteration][3]]);
		temp4objP.push_back(objP[n_4[iteration][0]]);
		temp4objP.push_back(objP[n_4[iteration][1]]);
		temp4objP.push_back(objP[n_4[iteration][2]]);
		temp4objP.push_back(objP[n_4[iteration][3]]);
		
		Mat(temp4objP).convertTo(tempobjPM, CV_32F);
		solvePnP(tempobjPM, Mat(temp4imgP), camera_matrix, distortion_coefficients, temp_r, temp_t);
		projectPoints(objPM, temp_r, temp_t, camera_matrix, distortion_coefficients, temprepP);
		testerror = norm(imgP, temprepP, NORM_L2);
		for (size_t i=0; i < imgP.size();i++)
		{
			temperror += sqrt((imgP[i].x - temprepP[i].x)*(imgP[i].x - temprepP[i].x) + (imgP[i].y - temprepP[i].y)*(imgP[i].y - temprepP[i].y));
		}
		if (temperror < errorthreshold)       //重投影誤差閥值為1 pixel
		{
			if (temperror < minerror)
			{
				minerror = temperror;
				best_r = temp_r;
				best_t = temp_t;
			}
		}
		temp4imgP.clear();
		temp4objP.clear();
		tempobjPM.setTo(0);
		temperror = 0;
		testerror = 0;
	}
	r = best_r;
	t = best_t;
}
           

通過找到最小重投影誤差，我們找到了最佳姿态，而且速度還行。下一篇講使用最小二乘多項式平滑消除姿态抖動。