OpenCV的那些事——利用RANSAC消除错误姿态



                      

版权声明：本文为博主原创文章，转载须注明出处。

上一节《关于OpenCV的那些事——跟踪点选取方式和特征点跟踪恢复》讲了两种跟踪和恢复的方法，本来项目就此结束了，想开一个新项目3D重建的，但是老板让发一篇journal，心想着这点东西肯定发不了，于是对项目进行了优化。11月半-1月半，两个月（一个月写，一个月改，老板对格式要求极其严格。。）发了香港的一个图像信号处理的期刊，3月的时候收到了accepted的通知。在这期间，我一边优化着项目，一边思考着我做的到底是什么，下一个项目究竟做什么（3D重建？SLAM？）。当然这个话题在我的下下一篇博客中会提到（视觉slam），这一篇主要讲第一个优化，使用random sample consensus收敛相机姿态。下一篇讲使用最小二乘多项式平滑消除姿态抖动。

我们知道在计算相机姿态的时候，opencv中提供了两种函数：solvePnP, solvePnPRansac。 第二个函数即时利用ransac的思想计算更加精确的姿态。 不过速度真是不敢恭维，完全没法实现实时系统。鉴于之前章节《关于OpenCV的那些事——相机姿态更新》里讲到的相机姿态更新至少使用4组2D/3D点对，我们的思想是对于追踪的n个特征点对，我们先随机生成m个4组点对（m < Cn4(排列组合)），分别计算出m个姿态，然后对于每一个姿态计算重投影误差，小于一定阀值的记录下来，并更新最佳姿态（最小误差），最终返回这个最佳姿态。m也叫迭代次数。当然选择合适的m，既能节省时间，有能找到最佳姿态。重投影误差的阀值也需要做实验找到最合适的。

C++代码如下：

[cpp] view plain copy print ?

1. bool collinear_ornot(Point2f p1, Point2f p2, Point2f p3)   // 三点是否共线  
2. {  
3.     if (abs((p2.x - p1.x)*p3.y - (p2.y - p1.y)*p3.x - p1.y*p2.x + p1.x*p2.y) < 1e-5)  
4.         return true;  
5.     else  
6.         return false;  
7. }  

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

[cpp] view plain copy print ?

1. void random_n_4p(vector<Point2f>& imgP)  
2. {  
3.     srand((unsigned)time(NULL));  
4.     int n = imgP.size();           //n为追踪的特征点的个数  
5.     for (int i = 0; i < ransac_1; i++)    // ransac_1为上文中的m迭代次数，本项目中取20  
6.     {  
7.         do  
8.         {  
9.             n_4[i][0] = rand() % n;  
10.             do  
11.             {  
12.                 n_4[i][1] = rand() % n;  
13.             } while (n_4[i][1] == n_4[i][0]);  
14.             do  
15.             {  
16.                 n_4[i][2] = rand() % n;  
17.             } while (n_4[i][2] == n_4[i][1] || n_4[i][2] == n_4[i][0]);  
18.             do  
19.             {  
20.                 n_4[i][3] = rand() % n;  
21.             } 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]);  
22.         }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]]));  
23.     }       //确保四点中每每三点不共线  
24. }  

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]]));
	}       //确保四点中每每三点不共线
}

[cpp] view plain copy print ?

1. void ransac_cc(vector<Point2f>& imgP, Mat& r, Mat& t)  
2. {  
3.     random_n_4p(imgP);  
4.       
5.     vector<Point2f> temp4imgP;  
6.     vector<Point3f> temp4objP;  
7.     vector<Point2f> temprepP;  
8.     temprepP.resize(imgP.size());  
9.     Mat tempobjPM;  
10.     vector<double> temprv(3), temptv(3), bestr(3),bestt(3);  
11.     Mat temp_r(temprv),temp_t(temptv),best_r(bestr),best_t(bestt);  
12.     float minerror = 1,temperror = 0, errorthreshold = 1;   //重投影误差阀值为1 pixel  
13.     float testerror = 0;  
14.     for (size_t iteration = 0; iteration < ransac_1; iteration++)  
15.     {  
16.         temp4imgP.push_back(imgP[n_4[iteration][0]]);  
17.         temp4imgP.push_back(imgP[n_4[iteration][1]]);  
18.         temp4imgP.push_back(imgP[n_4[iteration][2]]);  
19.         temp4imgP.push_back(imgP[n_4[iteration][3]]);  
20.         temp4objP.push_back(objP[n_4[iteration][0]]);  
21.         temp4objP.push_back(objP[n_4[iteration][1]]);  
22.         temp4objP.push_back(objP[n_4[iteration][2]]);  
23.         temp4objP.push_back(objP[n_4[iteration][3]]);  
24.           
25.         Mat(temp4objP).convertTo(tempobjPM, CV_32F);  
26.         solvePnP(tempobjPM, Mat(temp4imgP), camera_matrix, distortion_coefficients, temp_r, temp_t);  
27.         projectPoints(objPM, temp_r, temp_t, camera_matrix, distortion_coefficients, temprepP);  
28.         testerror = norm(imgP, temprepP, NORM_L2);  
29.         for (size_t i=0; i < imgP.size();i++)  
30.         {  
31.             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));  
32.         }  
33.         if (temperror < errorthreshold)       //重投影误差阀值为1 pixel  
34.         {  
35.             if (temperror < minerror)  
36.             {  
37.                 minerror = temperror;  
38.                 best_r = temp_r;  
39.                 best_t = temp_t;  
40.             }  
41.         }  
42.         temp4imgP.clear();  
43.         temp4objP.clear();  
44.         tempobjPM.setTo(0);  
45.         temperror = 0;  
46.         testerror = 0;  
47.     }  
48.     r = best_r;  
49.     t = best_t;  
50. }  

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

通过找到最小重投影误差，我们找到了最佳姿态，而且速度非常快。下一篇讲使用最小二乘多项式平滑消除姿态抖动。