最近要做一個演算法，用到了位姿估計。位姿估計的使用範圍非常廣泛。主要解決的問題為：在給出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

論文記得用谷歌開啟。

我們就直接上實驗吧。先看原始資料

333.965515 110.721138 45.893448 164.000000  1909.000000
327.870117 110.772079 45.835598 2578.000000 1970.000000
327.908630 115.816376 43.036915 2858.000000 3087.000000
333.731628 115.862755 43.031918 95.000000 3051.000000
330.019196 110.630211 45.871151 1727.000000 1942.000000
330.043457 115.823494 43.027847 1855.000000 3077.000000
331.949371 115.881104 43.020267 943.000000 3072.000000
331.943970 110.598534 45.909332 962.000000 1926.000000

沒錯，用空格分開。前三列依次為x,y,z座標，後兩列為影象的畫素座標。

然後我們看看SovePnp的原始碼都需要些什麼

我們發現除了函式本身定義之外上面還有許多引數解釋。嗯，Opencv果然是專業。

然後我們再看Opencv原始碼呼叫：

bool solvePnP( InputArray _opoints, InputArray _ipoints,
               InputArray _cameraMatrix, InputArray _distCoeffs,
               OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, int flags )
{
    CV_INSTRUMENT_REGION()

    Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
    int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
    CV_Assert( npoints >= 0 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );

    Mat rvec, tvec;
    if( flags != SOLVEPNP_ITERATIVE )
        useExtrinsicGuess = false;

    if( useExtrinsicGuess )
    {
        int rtype = _rvec.type(), ttype = _tvec.type();
        Size rsize = _rvec.size(), tsize = _tvec.size();
        CV_Assert( (rtype == CV_32F || rtype == CV_64F) &&
                   (ttype == CV_32F || ttype == CV_64F) );
        CV_Assert( (rsize == Size(1, 3) || rsize == Size(3, 1)) &&
                   (tsize == Size(1, 3) || tsize == Size(3, 1)) );
    }
    else
    {
        _rvec.create(3, 1, CV_64F);
        _tvec.create(3, 1, CV_64F);
    }
    rvec = _rvec.getMat();
    tvec = _tvec.getMat();

    Mat cameraMatrix0 = _cameraMatrix.getMat();
    Mat distCoeffs0 = _distCoeffs.getMat();
    Mat cameraMatrix = Mat_<double>(cameraMatrix0);
    Mat distCoeffs = Mat_<double>(distCoeffs0);
    bool result = false;

    if (flags == SOLVEPNP_EPNP || flags == SOLVEPNP_DLS || flags == SOLVEPNP_UPNP)
    {
        Mat undistortedPoints;
        undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
        epnp PnP(cameraMatrix, opoints, undistortedPoints);

        Mat R;
        PnP.compute_pose(R, tvec);
        Rodrigues(R, rvec);
        result = true;
    }
    else if (flags == SOLVEPNP_P3P)
    {
        CV_Assert( npoints == 4);
        Mat undistortedPoints;
        undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
        p3p P3Psolver(cameraMatrix);

        Mat R;
        result = P3Psolver.solve(R, tvec, opoints, undistortedPoints);
        if (result)
            Rodrigues(R, rvec);
    }
    else if (flags == SOLVEPNP_ITERATIVE)
    {
        CvMat c_objectPoints = opoints, c_imagePoints = ipoints;
        CvMat c_cameraMatrix = cameraMatrix, c_distCoeffs = distCoeffs;
        CvMat c_rvec = rvec, c_tvec = tvec;
        cvFindExtrinsicCameraParams2(&c_objectPoints, &c_imagePoints, &c_cameraMatrix,
                                     c_distCoeffs.rows*c_distCoeffs.cols ? &c_distCoeffs : 0,
                                     &c_rvec, &c_tvec, useExtrinsicGuess );
        result = true;
    }
    else
        CV_Error(CV_StsBadArg, "The flags argument must be one of SOLVEPNP_ITERATIVE, SOLVEPNP_P3P, SOLVEPNP_EPNP or SOLVEPNP_DLS");
    return result;
}
 

然後P3P演算法要求輸入的控制點個數只能是4對。

好，沒問題了，我們準備好影象，然後開始呼叫。

我們首先構造好K矩陣，fx、fy都是以畫素為單位的焦距。我們必須事先知道焦距，或者能從Exif中解析出焦距資訊。好，我們直接上核心程式碼。至於貼圖函式

void MappingCloud(VertexPositionColor*&CloudPtr, int PointNum,cv::Mat IMG, cv::Mat K, cv::Mat nin_R, cv::Mat T, bool ifout , std::string OutColorCloudFileName)
{
cout << "Coloring Cloud..." << endl;
std::ofstream CloudOuter;
if (ifout == true)
CloudOuter.open(OutColorCloudFileName);
double reProjection = 0;
for (int i = 0; i < PointNum; i++)
{
//計算3D點雲在影像上的投影位置（畫素）
cv::Mat SingleP(cv::Matx31d(//這裡必須是Matx31d，如果是f則會報錯。opencv還真是嬌氣
CloudPtr[i].x,
CloudPtr[i].y,
CloudPtr[i].z));
cv::Mat change = K*(nin_R*SingleP + T);
change /= change.at<double>(2, 0);
//cout << change.t() << endl;
int xPixel = cvRound(change.at<double>(0, 0));
int yPixel = cvRound(change.at<double>(1, 0));
//取得顏色
uchar* data = IMG.ptr<uchar>(yPixel);
int Blue =data[xPixel * 3+0]; //第row行的第col個畫素點的第一個通道值 Blue
int Green = data[xPixel * 3 + 1]; // Green
int Red = data[xPixel * 3 + 2]; // Red
//顏色賦值
CloudPtr[i].R = Red;
CloudPtr[i].G = Green;
CloudPtr[i].B = Blue;
if (ifout == true)
CloudOuter << CloudPtr[i].x << " " << CloudPtr[i].y << " " << CloudPtr[i].z << " " << CloudPtr[i].R << " " << CloudPtr[i].G << " " << CloudPtr[i].B << endl;
}
if (ifout == true)
CloudOuter.close();
cout << "done!" << endl;
}
std::string intToStdtring(int Num)
{
std::stringstream strStream;
strStream << Num;
std::string s = std::string(strStream.str());
return s;
}

void OpencvPnPTest()//測試opencv的Pnp演算法
{
	std::ifstream reader(".\\TextureMappingData\\楊芳8點座標.txt");
	if (!reader)
	{
		std::cout << "開啟錯誤！";
		system("pause");
		exit(0);
	}
	string imgName = std::string(".\\TextureMappingData\\IMG_0828.JPG");
	cv::Mat IMG = cv::imread(".\\TextureMappingData\\IMG_0828.JPG");
	double IMGW = IMG.cols;//楊芳8點座標.txt
	double IMGH = IMG.rows;
	double RealWidth = 35.9;
	double CCDWidth = RealWidth / (IMGW >= IMGH ? IMGW : IMGH);//一定要取較長的那條邊
	double f = 100.0;
	double fpixel = f / CCDWidth;
	cv::Mat K_intrinsic(cv::Matx33d(
		fpixel, 0, IMGH / 2.0,
		0, fpixel, IMGW / 2.0,
		0, 0, 1));
	int UsePointCont = 5;
	vector<cv::Point3f> ConP3DVec;
	vector<cv::Point2f> ConP2DVec;
	cv::Point3f ConP3D;
	cv::Point2f ConP2D;
	cout << "AllPoints: " << endl;
	while (reader >> ConP3D.x)
	{
		reader >> ConP3D.y;
		reader >> ConP3D.z;
		reader >> ConP2D.x;
		reader >> ConP2D.y;
		ConP3DVec.push_back(ConP3D);
		ConP2DVec.push_back(ConP2D);
		cout << setprecision(10)
			<< ConP3D.x << " " << ConP3D.y << " " << ConP3D.z << " "
			<< ConP2D.x << " " << ConP2D.y << endl;
		if (ConP3DVec.size() == UsePointCont)
			break;
	}
	reader.close();
	cout << "BaseInformation: " << endl;
	cout << "imgName: " << imgName<< endl;
	cout << "width: " << IMGW << endl;
	cout << "height: " << IMGH << endl;
	cout << "UsePointCont: " << UsePointCont << endl;
	cout << "RealWidth: " << RealWidth << endl;
	cout << "camerapixel: " << CCDWidth << endl;
	cout << "f: " << f << endl;
	cout << "fpixel: " << fpixel << endl;
	cout << "KMatrix: " << K_intrinsic << endl;

	//對於後母戊鼎opencv原始幾種解法報告：
	//對於8個點的情況：EPnP表現良好，DLS表現良好，EPnp與Upnp呼叫的函式介面相同
	//對於4個點的情況：P3P表現良好，EPnp表現良好，然而P3P實際輸入點數為4個，那麼最後一個點用於檢核。
	//Epnp在4點的時候表現時好時壞，和控制點的選取狀況相關。倘若增加為5個點，則精度有著明顯提升
	//EPnP在4點的情況下貼圖完全錯誤，比後方交會的效果還要更加扭曲
	//4點DLS的方法取得了和EPnp同樣的貼圖結果,從結果來看非常扭曲
	//4點的ITERATOR方法取得了很好的貼圖效果，甚至比P3P還要好，因為P3P只用三個點參與計算
	//也就是說，在控制點對數量較少的情況下（4點）,只有P3P可以給出正確結果
	//增加一對控制點(變為5對)之後，Epnp的魯棒性迅速提升，可以得到正確結果,DLS也同樣取得了正確結果
	//增加到5對控制點後P3P失效，因為只讓用4個點，不多不少.
	//同樣的，5點的ITERATOR方法結果正確。
	//----------結論：使用多於4點的EPnP最為穩妥.所求得的矩陣為計算機視覺矩陣
	//int flag = cv::SOLVEPNP_EPNP;std::string OutCloudPath = std::string(".\\Output\\EPNP_") + intToStdtring(UsePointCont) + std::string("Point_HMwuding.txt");
	//int flag = cv::SOLVEPNP_DLS;std::string OutCloudPath = std::string(".\\Output\\DLS_") + intToStdtring(UsePointCont) + std::string("Point_HMwuding.txt");  
	//int flag = cv::SOLVEPNP_P3P;std::string OutCloudPath = std::string(".\\Output\\P3P_") + intToStdtring(UsePointCont) + std::string("Point_HMwuding.txt");
	int flag = cv::SOLVEPNP_ITERATIVE; std::string OutCloudPath = std::string(".\\Output\\ITERATIVE_") + intToStdtring(UsePointCont) + std::string("Point_HMwuding.txt");
	cout << endl << "Soving Pnp Using Method" << flag<<"..."<< endl;
	cv::Mat Rod_r ,TransMatrix ,RotationR;
	bool success = cv::solvePnP(ConP3DVec, ConP2DVec, K_intrinsic, cv::noArray(), Rod_r, TransMatrix,false, flag);
	Rodrigues(Rod_r, RotationR);//將旋轉向量轉換為羅德里格旋轉矩陣
	cout << "r:" << endl << Rod_r << endl;
	cout << "R:" << endl << RotationR << endl;
	cout << "T:" << endl << TransMatrix << endl;
	cout << "C(Camera center:):" << endl << -RotationR.inv()*TransMatrix << endl;//這個C果然是相機中心，十分準確
	Comput_Reprojection(ConP3DVec, ConP2DVec, K_intrinsic, RotationR, TransMatrix);

	//=====下面讀取點雲並進行紋理對映
	cout << "Reading Cloud..." << endl;
	std::string CloudPath = std::string(".\\PointCloud\\HMwuding.txt");
	ScarletCloudIO CloudReader(CloudPath);
	CloudReader.Read();
	CloudReader.PrintStationStatus();
	VertexPositionColor *CloudPtr = CloudReader.PtCloud().PointCloud;
	int CloudNum = CloudReader.PtCloud().PointNum;
	MappingCloud(CloudPtr,CloudNum, IMG, K_intrinsic,//點雲、數量、內參
		RotationR, TransMatrix,true, OutCloudPath);//旋轉、平移(並非是相機中心)、點雲最終的放置路徑

	//下面可以繼續研究旋轉平移以及比例縮放的影響
}

首先是原始

------------------DLS_4Point_HMwuding:(錯誤)

------------------DLS_5Point_HMwuding:(正確)

-----------------------EPNP_4Point_HMwuding.txt(錯誤)

-----------------------EPNP_5Point_HMwuding.txt(正確)

---------------------ITERATIVE_4Point_HMwuding.txt（正確）

-----------------P3P_4Point_HMwuding.txt（正確）

基本上就這麼多了