Opencv249和Opencv3.0以上的 SolvePnp函数详解（附带程序、算例，应用点对分析）

https://blog.csdn.net/qq_30547073/article/details/78656795

最近要做一个算法，用到了位姿估计。位姿估计的使用范围非常广泛。主要解决的问题为：在给出2D-3D若干点对以及相片的内参信息，如何求得相机中心在世界坐标系下的坐标以及相机的方向（旋转矩阵）。为此笔者做了大量研究，看了许多主流的文章，也是用了许多相关的函数库。主要有OpenMVG、OpenGV、OpenCV这三种。这三个库虽然都集成了EPnp、Upnp、P3P等多种算法，但实际差别还是很大。这一篇博客主要对opencv中的SolvePnp算法做一个总结以及各类实验。

PnP的具体原理我就不过多解释了，这里放几个链接供大家学习：

首先是Opencv的官方文档：https://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html#solvepnp

然后是一篇解释Pnp的很有用的一篇文章：http://blog.csdn.net/cocoaqin/article/details/77841261

然后是Epnp的英文论文：https://pdfs.semanticscholar.org/6ed0/083ff42ac966a6f37710e0b5555b98fd7565.pdf

论文记得用谷歌打开。

然后我们再看Opencv源码调用：

1. bool solvePnP( InputArray _opoints, InputArray _ipoints,
2. InputArray _cameraMatrix, InputArray _distCoeffs,
3. OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, int flags )
4. {
5. CV_INSTRUMENT_REGION()
6. 
   
7. Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
8. int npoints = std::max(opoints.checkVector( 3, CV_32F), opoints.checkVector( 3, CV_64F));
9. CV_Assert( npoints >= 0 && npoints == std::max(ipoints.checkVector( 2, CV_32F), ipoints.checkVector( 2, CV_64F)) );
10. 
    
11. Mat rvec, tvec;
12. if( flags != SOLVEPNP_ITERATIVE )
13. useExtrinsicGuess = false;
14. 
    
15. if( useExtrinsicGuess )
16. {
17. int rtype = _rvec.type(), ttype = _tvec.type();
18. Size rsize = _rvec.size(), tsize = _tvec.size();
19. CV_Assert( (rtype == CV_32F || rtype == CV_64F) &&
20. (ttype == CV_32F || ttype == CV_64F) );
21. CV_Assert( (rsize == Size( 1, 3) || rsize == Size( 3, 1)) &&
22. (tsize == Size( 1, 3) || tsize == Size( 3, 1)) );
23. }
24. else
25. {
26. _rvec.create( 3, 1, CV_64F);
27. _tvec.create( 3, 1, CV_64F);
28. }
29. rvec = _rvec.getMat();
30. tvec = _tvec.getMat();
31. 
    
32. Mat cameraMatrix0 = _cameraMatrix.getMat();
33. Mat distCoeffs0 = _distCoeffs.getMat();
34. Mat cameraMatrix = Mat_< double>(cameraMatrix0);
35. Mat distCoeffs = Mat_< double>(distCoeffs0);
36. bool result = false;
37. 
    
38. if (flags == SOLVEPNP_EPNP || flags == SOLVEPNP_DLS || flags == SOLVEPNP_UPNP)
39. {
40. Mat undistortedPoints;
41. undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
42. epnp PnP(cameraMatrix, opoints, undistortedPoints);
43. 
    
44. Mat R;
45. PnP.compute_pose(R, tvec);
46. Rodrigues(R, rvec);
47. result = true;
48. }
49. else if (flags == SOLVEPNP_P3P)
50. {
51. CV_Assert( npoints == 4);
52. Mat undistortedPoints;
53. undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
54. p3p P3Psolver(cameraMatrix);
55. 
    
56. Mat R;
57. result = P3Psolver.solve(R, tvec, opoints, undistortedPoints);
58. if (result)
59. Rodrigues(R, rvec);
60. }
61. else if (flags == SOLVEPNP_ITERATIVE)
62. {
63. CvMat c_objectPoints = opoints, c_imagePoints = ipoints;
64. CvMat c_cameraMatrix = cameraMatrix, c_distCoeffs = distCoeffs;
65. CvMat c_rvec = rvec, c_tvec = tvec;
66. cvFindExtrinsicCameraParams2(&c_objectPoints, &c_imagePoints, &c_cameraMatrix,
67. c_distCoeffs.rows*c_distCoeffs.cols ? &c_distCoeffs : 0,
68. &c_rvec, &c_tvec, useExtrinsicGuess );
69. result = true;
70. }
71. else
72. CV_Error(CV_StsBadArg, "The flags argument must be one of SOLVEPNP_ITERATIVE, SOLVEPNP_P3P, SOLVEPNP_EPNP or SOLVEPNP_DLS");
73. return result;
74. }

我们发现Epnp、Upnp以及DLS调用的是同一个接口。喂喂，这也太敷衍了吧。。。按理说应该是不一样才对。也许以后会改吧。

然后P3P算法要求输入的控制点个数只能是4对。

好，没问题了，我们准备好图像，然后开始调用。

我们首先构造好K矩阵，fx、fy都是以像素为单位的焦距。我们必须事先知道焦距，或者能从Exif中解析出焦距信息。好，我们直接上核心代码。至于贴图函数

1. void MappingCloud(VertexPositionColor*&CloudPtr, int PointNum,cv::Mat IMG, cv::Mat K, cv::Mat nin_R, cv::Mat T, bool ifout , std::string OutColorCloudFileName)
2. {
3. cout << "Coloring Cloud..." << endl;
4. std::ofstream CloudOuter;
5. if (ifout == true)
6. CloudOuter.open(OutColorCloudFileName);
7. double reProjection = 0;
8. for ( int i = 0; i < PointNum; i++)
9. {
10. //计算3D点云在影像上的投影位置（像素）
11. cv:: Mat SingleP(cv::Matx31d(//这里必须是Matx31d，如果是f则会报错。opencv还真是娇气
12. CloudPtr[i].x,
13. CloudPtr[i].y,
14. CloudPtr[i].z));
15. cv::Mat change = K*(nin_R*SingleP + T);
16. change /= change.at< double>( 2, 0);
17. //cout << change.t() << endl;
18. int xPixel = cvRound(change.at< double>( 0, 0));
19. int yPixel = cvRound(change.at< double>( 1, 0));
20. //取得颜色
21. uchar* data = IMG.ptr<uchar>(yPixel);
22. int Blue =data[xPixel * 3+ 0]; //第row行的第col个像素点的第一个通道值 Blue
23. int Green = data[xPixel * 3 + 1]; // Green
24. int Red = data[xPixel * 3 + 2]; // Red
25. //颜色赋值
26. CloudPtr[i].R = Red;
27. CloudPtr[i].G = Green;
28. CloudPtr[i].B = Blue;
29. if (ifout == true)
30. CloudOuter << CloudPtr[i].x << " " << CloudPtr[i].y << " " << CloudPtr[i].z << " " << CloudPtr[i].R << " " << CloudPtr[i].G << " " << CloudPtr[i].B << endl;
31. }
32. if (ifout == true)
33. CloudOuter.close();
34. cout << "done!" << endl;
35. }
36. std:: string intToStdtring(int Num)
37. {
38. std:: stringstream strStream;
39. strStream << Num;
40. std:: string s = std:: string(strStream.str());
41. return s;
42. }

[cpp] view plain copy

1. <code class="language-cpp">void OpencvPnPTest()//测试opencv的Pnp算法  
2. {  
3.     std::ifstream reader(".\\TextureMappingData\\杨芳8点坐标.txt");  
4.     if (!reader)  
5.     {  
6.         std::cout << "打开错误！";  
7.         system("pause");  
8.         exit(0);  
9.     }  
10.     string imgName = std::string(".\\TextureMappingData\\IMG_0828.JPG");  
11.     cv::Mat IMG = cv::imread(".\\TextureMappingData\\IMG_0828.JPG");  
12.     double IMGW = IMG.cols;//杨芳8点坐标.txt  
13.     double IMGH = IMG.rows;  
14.     double RealWidth = 35.9;  
15.     double CCDWidth = RealWidth / (IMGW >= IMGH ? IMGW : IMGH);//一定要取较长的那条边  
16.     double f = 100.0;  
17.     double fpixel = f / CCDWidth;  
18.     cv::Mat K_intrinsic(cv::Matx33d(  
19.         fpixel, 0, IMGH / 2.0,  
20.         0, fpixel, IMGW / 2.0,  
21.         0, 0, 1));  
22.     int UsePointCont = 5;  
23.     vector<cv::Point3f> ConP3DVec;  
24.     vector<cv::Point2f> ConP2DVec;  
25.     cv::Point3f ConP3D;  
26.     cv::Point2f ConP2D;  
27.     cout << "AllPoints: " << endl;  
28.     while (reader >> ConP3D.x)  
29.     {  
30.         reader >> ConP3D.y;  
31.         reader >> ConP3D.z;  
32.         reader >> ConP2D.x;  
33.         reader >> ConP2D.y;  
34.         ConP3DVec.push_back(ConP3D);  
35.         ConP2DVec.push_back(ConP2D);  
36.         cout << setprecision(10)  
37.             << ConP3D.x << " " << ConP3D.y << " " << ConP3D.z << " "  
38.             << ConP2D.x << " " << ConP2D.y << endl;  
39.         if (ConP3DVec.size() == UsePointCont)  
40.             break;  
41.     }  
42.     reader.close();  
43.     cout << "BaseInformation: " << endl;  
44.     cout << "imgName: " << imgName<< endl;  
45.     cout << "width: " << IMGW << endl;  
46.     cout << "height: " << IMGH << endl;  
47.     cout << "UsePointCont: " << UsePointCont << endl;  
48.     cout << "RealWidth: " << RealWidth << endl;  
49.     cout << "camerapixel: " << CCDWidth << endl;  
50.     cout << "f: " << f << endl;  
51.     cout << "fpixel: " << fpixel << endl;  
52.     cout << "KMatrix: " << K_intrinsic << endl;  
53.   
54.     //对于后母戊鼎opencv原始几种解法报告：  
55.     //对于8个点的情况：EPnP表现良好，DLS表现良好，EPnp与Upnp调用的函数接口相同  
56.     //对于4个点的情况：P3P表现良好，EPnp表现良好，然而P3P实际输入点数为4个，那么最后一个点用于检核。  
57.     //Epnp在4点的时候表现时好时坏，和控制点的选取状况相关。倘若增加为5个点，则精度有着明显提升  
58.     //EPnP在4点的情况下贴图完全错误，比后方交会的效果还要更加扭曲  
59.     //4点DLS的方法取得了和EPnp同样的贴图结果,从结果来看非常扭曲  
60.     //4点的ITERATOR方法取得了很好的贴图效果，甚至比P3P还要好，因为P3P只用三个点参与计算  
61.     //也就是说，在控制点对数量较少的情况下（4点）,只有P3P可以给出正确结果  
62.     //增加一对控制点(变为5对)之后，Epnp的鲁棒性迅速提升，可以得到正确结果,DLS也同样取得了正确结果  
63.     //增加到5对控制点后P3P失效，因为只让用4个点，不多不少.  
64.     //同样的，5点的ITERATOR方法结果正确。  
65.     //----------结论：使用多于4点的EPnP最为稳妥.所求得的矩阵为计算机视觉矩阵  
66.     //int flag = cv::SOLVEPNP_EPNP;std::string OutCloudPath = std::string(".\\Output\\EPNP_") + intToStdtring(UsePointCont) + std::string("Point_HMwuding.txt");  
67.     //int flag = cv::SOLVEPNP_DLS;std::string OutCloudPath = std::string(".\\Output\\DLS_") + intToStdtring(UsePointCont) + std::string("Point_HMwuding.txt");    
68.     //int flag = cv::SOLVEPNP_P3P;std::string OutCloudPath = std::string(".\\Output\\P3P_") + intToStdtring(UsePointCont) + std::string("Point_HMwuding.txt");  
69.     int flag = cv::SOLVEPNP_ITERATIVE; std::string OutCloudPath = std::string(".\\Output\\ITERATIVE_") + intToStdtring(UsePointCont) + std::string("Point_HMwuding.txt");  
70.     cout << endl << "Soving Pnp Using Method" << flag<<"..."<< endl;  
71.     cv::Mat Rod_r ,TransMatrix ,RotationR;  
72.     bool success = cv::solvePnP(ConP3DVec, ConP2DVec, K_intrinsic, cv::noArray(), Rod_r, TransMatrix,false, flag);  
73.     Rodrigues(Rod_r, RotationR);//将旋转向量转换为罗德里格旋转矩阵  
74.     cout << "r:" << endl << Rod_r << endl;  
75.     cout << "R:" << endl << RotationR << endl;  
76.     cout << "T:" << endl << TransMatrix << endl;  
77.     cout << "C(Camera center:):" << endl << -RotationR.inv()*TransMatrix << endl;//这个C果然是相机中心，十分准确  
78.     Comput_Reprojection(ConP3DVec, ConP2DVec, K_intrinsic, RotationR, TransMatrix);  
79.   
80.     //=====下面读取点云并进行纹理映射  
81.     cout << "Reading Cloud..." << endl;  
82.     std::string CloudPath = std::string(".\\PointCloud\\HMwuding.txt");  
83.     ScarletCloudIO CloudReader(CloudPath);  
84.     CloudReader.Read();  
85.     CloudReader.PrintStationStatus();  
86.     VertexPositionColor *CloudPtr = CloudReader.PtCloud().PointCloud;  
87.     int CloudNum = CloudReader.PtCloud().PointNum;  
88.     MappingCloud(CloudPtr,CloudNum, IMG, K_intrinsic,//点云、数量、内参  
89.         RotationR, TransMatrix,true, OutCloudPath);//旋转、平移(并非是相机中心)、点云最终的放置路径  
90.   
91.     //下面可以继续研究旋转平移以及比例缩放的影响  
92. }</code>  

实验的报告以及调整的参数都已经在注释中写清楚了。下面我给出这几个贴图效果。我们采用的命名规则为：方法_控制点个数

首先是原始

5.solvepnp算法比较与分析

CV_P3P  可以使用任意4个特征点求解，不要共面，特征点数量不为4时报错


 迭代单个牌子-好

CV_ITERATIVE

该方法基于Levenberg-Marquardtoptimization迭代求解PNP问题，实质是迭代求出重投影误差最小的解，这个解显然不一定是正解。 实测该方法只有用4个共面的特征点时才能求出正确的解，使用5个特征点或4点非共面的特征点都得不到正确的位姿。

只能用4个共面的特征点来解位姿

P3P单个牌子-好

CV_P3P

这个方法使用非常经典的Gao方法解P3P问题，求出4组可能的解，再通过对第四个点的重投影，返回重投影误差最小的点。 论文《CompleteSolution Classification for the Perspective-Three-Point Problem》

可以使用任意4个特征点求解，不要共面，特征点数量不为4时报错

EpNP

CV_EPNP

该方法使用EfficientPNP方法求解问题，具体怎么做的当时网速不好我没下载到论文，后面又懒得去看了。 论文《EPnP:Efficient Perspective-n-Point Camera Pose Estimation》

对于N个特征点，只要N>3就能够求出正解。

DLS和UPnP 效果一样