碼上歡樂
相關內容    簡體    繁體

相機位姿估計1_1：OpenCV:solvePnP二次封裝與性能測試

本文轉載自   查看原文   2016-11-28 23:12   10067    機器視覺應用/ 位姿估計

關鍵詞：OpenCV::solvePnP

文章類型：方法封裝、測試

@Author：VShawn(singlex@foxmail.com)

@Date:2016-11-27

@Lab: CvLab202@CSU

前言

今天給大家帶來的是一篇關於程序功能、性能測試的文章，讀過《相機位姿估計1：根據四個特征點估計相機姿態》一文的同學應該會發現，直接使用OpenCV的solvePnP來估計相機位姿，在程序調用上相當麻煩，從一開始的參數設定到最后將計算出的矩陣轉化為相機的位姿參數，需要花費近兩百行代碼。因此為了更方便地調用程序，今天我就給大家帶來一個我自己對solvePnP的封裝類PNPSolver，順便將OpenCV自帶的三種求解方法測試一遍。

類的封裝

封裝的思路我就不寫了，由於博客更新速度趕不上我寫程序的速度，現在發上來的類已經修改過好幾次了，思路也換了幾次。不過大的方向沒變，目的就是只需要輸入參數，輸入坐標點后直接可以得到相機在世界坐標系的坐標。

類的調用順序：

1.初始化PNPSolver類；

2.調用SetCameraMatrix(),SetDistortionCoefficients()設置好相機內參數與鏡頭畸變參數；

3.向Points3D，Points2D中添加一一對應的特征點對；

4.調用Solve()方法運行計算；

5.從屬性Theta_C2W中提取旋轉角，從Position_OcInW中提取出相機在世界坐標系下的坐標。

以下是類體：

PNPSolver.h

#pragma once
#include <opencv2\opencv.hpp>
#include <math.h>
#include <iostream>
#include <fstream>
using namespace std;


// 本類用於快速解決PNP問題，順帶解決空間繞軸旋轉以及圖像系、相機系、世界系三系坐標投影問題
// 默認使用Gao的P3P+重投影法，要求輸入4個特征點
// 調用順序：
// 1.初始化本類
// 2.調用SetCameraMatrix(),SetDistortionCoefficients()設置好相機內參數與鏡頭畸變參數
// 3.向Points3D，Points2D中添加一一對應的特征點對
// 4.調用Solve()方法運行計算
// 5.從RoteM, TransM, W2CTheta等屬性中提出結果
//
// 原理參見：http://www.cnblogs.com/singlex/category/911880.html
// Author：VShawn
// Ver:2016.11.25.0
class PNPSolver
{
public:
	PNPSolver();
	//帶參數初始化
	PNPSolver(double fx, double fy, double u0, double v0, double k_1, double  k_2, double  p_1, double  p_2, double k_3);
	~PNPSolver();

	enum METHOD
	{
		CV_ITERATIVE = CV_ITERATIVE,
		CV_P3P = CV_P3P,
		CV_EPNP = CV_EPNP
	};

	/***********************位姿估計所用特征點**************************/
	vector<cv::Point3f> Points3D;//存儲四個點的世界坐標
	vector<cv::Point2f> Points2D;//存儲四個點的圖像坐標

	/***********************位姿估計結果**************************/
	//最后求出的旋轉矩陣與平移矩陣
	cv::Mat RoteM, TransM;
	//世界系到相機系的三軸旋轉歐拉角，世界系照此旋轉后可以與相機坐標系完全平行。
	//旋轉順序為x、y、z
	cv::Point3f Theta_W2C;
	//相機系到世界系的三軸旋轉歐拉角，相機坐標系照此旋轉后可以與世界坐標系完全平行。
	//旋轉順序為z、y、x
	cv::Point3f Theta_C2W;
	//相機坐標系中，世界坐標系原點Ow的坐標
	cv::Point3f Position_OwInC;
	//世界坐標系中，相機坐標系原點Oc的坐標
	cv::Point3f Position_OcInW;


	/*********************公有方法*****************************/

	//解PNP問題，獲得位姿信息
	//調用后在RoteM, TransM, W2CTheta等屬性中提取計算結果，屬性說明參見注釋
	//輸出參數：CV_ITERATIVE，CV_P3P（默認），CV_EPNP，具體參見Opencv documentation.
	//實測
	//CV_ITERATIVE迭代法似乎只能用4個共面特征點求解，5個點或非共面4點解不出正確的解
	//CV_P3P的Gao的方法可以使用任意四個特征點，特征點數量不能少於4也不能多於4
	//CV_EPNP方法可以實現特征點數>=4的問題求解，不需要4點共面
	//返回值：
	//0正確
	//-1相機內參數或畸變參數未設置
	//-2未提供足夠的特征點，或特征點數目不匹配
	//-3輸入的點數據有誤，詳見printf信息
	int Solve(METHOD method = METHOD::CV_P3P);
	
	//根據計算出的結果將世界坐標重投影到圖像，返回像素坐標點集
	//使用前需要先用Solve()解出相機位姿
	//輸入為世界坐標系的點坐標集合
	//輸出為點投影到圖像上的圖像坐標集合
	vector<cv::Point2f> WordFrame2ImageFrame(vector<cv::Point3f> WorldPoints);



	//根據輸入的參數將圖像坐標轉換到相機坐標中
	//使用前需要先用Solve()解出相機位姿
	//輸入為圖像上的點坐標
	//double F為鏡頭焦距
	//輸出為點在焦距=F時的相機坐標系坐標
	cv::Point3f ImageFrame2CameraFrame(cv::Point2f p, double F);




	//設置相機內參數矩陣
	void SetCameraMatrix(double fx, double fy, double u0, double v0)
	{
		camera_matrix = cv::Mat(3, 3, CV_64FC1, cv::Scalar::all(0));
		camera_matrix.ptr<double>(0)[0] = fx;
		camera_matrix.ptr<double>(0)[2] = u0;
		camera_matrix.ptr<double>(1)[1] = fy;
		camera_matrix.ptr<double>(1)[2] = v0;
		camera_matrix.ptr<double>(2)[2] = 1.0f;
	}
	//設置畸變系數矩陣
	void SetDistortionCoefficients(double k_1, double  k_2, double  p_1, double  p_2, double k_3)
	{
		distortion_coefficients = cv::Mat(5, 1, CV_64FC1, cv::Scalar::all(0));
		distortion_coefficients.ptr<double>(0)[0] = k_1;
		distortion_coefficients.ptr<double>(1)[0] = k_2;
		distortion_coefficients.ptr<double>(2)[0] = p_1;
		distortion_coefficients.ptr<double>(3)[0] = p_2;
		distortion_coefficients.ptr<double>(4)[0] = k_3;
	}












	/********************公有靜態方法*********************/
	//點繞任意向量旋轉，右手系
	static cv::Point3f RotateByVector(double old_x, double old_y, double old_z, double vx, double vy, double vz, double theta)
	{
		double r = theta * CV_PI / 180;
		double c = cos(r);
		double s = sin(r);
		double new_x = (vx*vx*(1 - c) + c) * old_x + (vx*vy*(1 - c) - vz*s) * old_y + (vx*vz*(1 - c) + vy*s) * old_z;
		double new_y = (vy*vx*(1 - c) + vz*s) * old_x + (vy*vy*(1 - c) + c) * old_y + (vy*vz*(1 - c) - vx*s) * old_z;
		double new_z = (vx*vz*(1 - c) - vy*s) * old_x + (vy*vz*(1 - c) + vx*s) * old_y + (vz*vz*(1 - c) + c) * old_z;
		return cv::Point3f(new_x, new_y, new_z);
	}

	//將空間點繞Z軸旋轉
	//輸入參數 x y為空間點原始x y坐標
	//thetaz為空間點繞Z軸旋轉多少度，角度制范圍在-180到180
	//outx outy為旋轉后的結果坐標
	static void CodeRotateByZ(double x, double y, double thetaz, double& outx, double& outy)
	{
		double x1 = x;//將變量拷貝一次，保證&x == &outx這種情況下也能計算正確
		double y1 = y;
		double rz = thetaz * CV_PI / 180;
		outx = cos(rz) * x1 - sin(rz) * y1;
		outy = sin(rz) * x1 + cos(rz) * y1;
	}

	//將空間點繞Y軸旋轉
	//輸入參數 x z為空間點原始x z坐標
	//thetay為空間點繞Y軸旋轉多少度，角度制范圍在-180到180
	//outx outz為旋轉后的結果坐標
	static void CodeRotateByY(double x, double z, double thetay, double& outx, double& outz)
	{
		double x1 = x;
		double z1 = z;
		double ry = thetay * CV_PI / 180;
		outx = cos(ry) * x1 + sin(ry) * z1;
		outz = cos(ry) * z1 - sin(ry) * x1;
	}

	//將空間點繞X軸旋轉
	//輸入參數 y z為空間點原始y z坐標
	//thetax為空間點繞X軸旋轉多少度，角度制，范圍在-180到180
	//outy outz為旋轉后的結果坐標
	static void CodeRotateByX(double y, double z, double thetax, double& outy, double& outz)
	{
		double y1 = y;//將變量拷貝一次，保證&y == &y這種情況下也能計算正確
		double z1 = z;
		double rx = thetax * CV_PI / 180;
		outy = cos(rx) * y1 - sin(rx) * z1;
		outz = cos(rx) * z1 + sin(rx) * y1;
	}
private:

	cv::Mat camera_matrix;//內參數矩陣
	cv::Mat distortion_coefficients;//畸變系數

	cv::Mat rvec;//解出來的旋轉向量
	cv::Mat tvec;//解出來的平移向量
};

PNPSolver.cpp

#include "PNPSolver.h"


// 本類用於快速解決PNP問題，順帶解決空間繞軸旋轉以及圖像系、相機系、世界系三系坐標投影問題
// 調用順序：
// 1.初始化本類
// 2.調用SetCameraMatrix(),SetDistortionCoefficients()設置好相機內參數與鏡頭畸變參數
// 3.向Points3D，Points2D中添加一一對應的特征點對
// 4.調用Solve()方法運行計算
// 5.從RoteM, TransM, W2CTheta等屬性中提出結果
//
// 原理參見：http://www.cnblogs.com/singlex/category/911880.html
// Author：VShawn
// Ver:2016.11.26.0
PNPSolver::PNPSolver()
{
	//初始化輸出矩陣
	vector<double> rv(3), tv(3);
	cv::Mat rvec(rv), tvec(tv);
}
PNPSolver::PNPSolver(double fx, double fy, double u0, double v0, double k_1, double  k_2, double  p_1, double  p_2, double k_3)
{
	//初始化輸出矩陣
	vector<double> rv(3), tv(3);
	cv::Mat rvec(rv), tvec(tv);
	SetCameraMatrix(fx, fy, u0, v0);
	SetDistortionCoefficients(k_1, k_2, p_1, p_2, k_3);
}

PNPSolver::~PNPSolver()
{
}

int PNPSolver::Solve(METHOD method)
{
	//數據校驗
	if (camera_matrix.cols == 0 || distortion_coefficients.cols == 0)
	{
		printf("ErrCode:-1,相機內參數或畸變參數未設置！\r\n");
		return -1;
	}

	if (Points3D.size() != Points2D.size())
	{
		printf("ErrCode:-2，3D點數量與2D點數量不一致！\r\n");
		return -2;
	}
	if (method == METHOD::CV_P3P || method == METHOD::CV_ITERATIVE)
	{
		if (Points3D.size() != 4)
		{
			printf("ErrCode:-2,使用CV_ITERATIVE或CV_P3P方法時輸入的特征點數量應為4！\r\n");
			return -2;
		}
	}
	else
	{
		if (Points3D.size() < 4)
		{
			printf("ErrCode:-2,輸入的特征點數量應大於4！\r\n");
			return -2;
		}
	}

	////TODO::檢驗是否是共面的四點
	//if ((method == METHOD::CV_ITERATIVE || method == METHOD::CV_EPNP) && Points2D.size() == 4)
	//{
	//	//通過向量兩兩叉乘獲得法向量，看法向量是否平行
	//}






	/*******************解決PNP問題*********************/
	//有三種方法求解
	solvePnP(Points3D, Points2D, camera_matrix, distortion_coefficients, rvec, tvec, false, method);	//實測迭代法似乎只能用共面特征點求位置
	//solvePnP(Points3D, Points2D, camera_matrix, distortion_coefficients, rvec, tvec, false, CV_ITERATIVE);	//實測迭代法似乎只能用共面特征點求位置
	//solvePnP(Points3D, Points2D, camera_matrix, distortion_coefficients, rvec, tvec, false, CV_P3P);		//Gao的方法可以使用任意四個特征點
	//solvePnP(Points3D, Points2D, camera_matrix, distortion_coefficients, rvec, tvec, false, CV_EPNP);


	/*******************提取旋轉矩陣*********************/
	double rm[9];
	RoteM = cv::Mat(3, 3, CV_64FC1, rm);
	Rodrigues(rvec, RoteM);
	double r11 = RoteM.ptr<double>(0)[0];
	double r12 = RoteM.ptr<double>(0)[1];
	double r13 = RoteM.ptr<double>(0)[2];
	double r21 = RoteM.ptr<double>(1)[0];
	double r22 = RoteM.ptr<double>(1)[1];
	double r23 = RoteM.ptr<double>(1)[2];
	double r31 = RoteM.ptr<double>(2)[0];
	double r32 = RoteM.ptr<double>(2)[1];
	double r33 = RoteM.ptr<double>(2)[2];
	TransM = tvec;

	//計算出相機坐標系的三軸旋轉歐拉角，旋轉后可以轉出世界坐標系。
	//旋轉順序為z、y、x
	double thetaz = atan2(r21, r11) / CV_PI * 180;
	double thetay = atan2(-1 * r31, sqrt(r32*r32 + r33*r33)) / CV_PI * 180;
	double thetax = atan2(r32, r33) / CV_PI * 180;

	//相機系到世界系的三軸旋轉歐拉角，相機坐標系照此旋轉后可以與世界坐標系完全平行。
	//旋轉順序為z、y、x
	Theta_C2W.z = thetaz;
	Theta_C2W.y = thetay;
	Theta_C2W.x = thetax;

	//計算出世界系到相機系的三軸旋轉歐拉角，世界系照此旋轉后可以轉出相機坐標系。
	//旋轉順序為x、y、z
	Theta_W2C.x = -1 * thetax;
	Theta_W2C.y = -1 * thetay;
	Theta_W2C.z = -1 * thetaz;
	

	/*************************************此處計算出相機坐標系原點Oc在世界坐標系中的位置**********************************************/

	/***********************************************************************************/
	/* 當原始坐標系經過旋轉z、y、x三次旋轉后，與世界坐標系平行，向量OcOw會跟着旋轉 */
	/* 而我們想知道的是兩個坐標系完全平行時，OcOw的值 */
	/* 因此，原始坐標系每次旋轉完成后，對向量OcOw進行一次反相旋轉，最終可以得到兩個坐標系完全平行時的OcOw */
	/* 該向量乘以-1就是世界坐標系下相機的坐標 */
	/***********************************************************************************/

	//提出平移矩陣，表示從相機坐標系原點，跟着向量(x,y,z)走，就到了世界坐標系原點
	double tx = tvec.ptr<double>(0)[0];
	double ty = tvec.ptr<double>(0)[1];
	double tz = tvec.ptr<double>(0)[2];

	//x y z 為唯一向量在相機原始坐標系下的向量值
	//也就是向量OcOw在相機坐標系下的值
	double x = tx, y = ty, z = tz;
	Position_OwInC.x = x;
	Position_OwInC.y = y;
	Position_OwInC.z = z;
	//進行三次反向旋轉
	CodeRotateByZ(x, y, -1 * thetaz, x, y);
	CodeRotateByY(x, z, -1 * thetay, x, z);
	CodeRotateByX(y, z, -1 * thetax, y, z);


	//獲得相機在世界坐標系下的位置坐標
	//即向量OcOw在世界坐標系下的值
	Position_OcInW.x = x*-1;
	Position_OcInW.y = y*-1;
	Position_OcInW.z = z*-1;

	return 0;
}


//根據計算出的結果將世界坐標重投影到圖像，返回像素坐標點集
//輸入為世界坐標系的點坐標集合
//輸出為點投影到圖像上的圖像坐標集合
vector<cv::Point2f> PNPSolver::WordFrame2ImageFrame(vector<cv::Point3f> WorldPoints)
{
	vector<cv::Point2f> projectedPoints;
	cv::projectPoints(WorldPoints, rvec, tvec, camera_matrix, distortion_coefficients, projectedPoints);
	return projectedPoints;
}



//根據輸入的參數將圖像坐標轉換到相機坐標中
//使用前需要先用Solve()解出相機位姿
//輸入為圖像上的點坐標
//double F為鏡頭焦距
//輸出為點在焦距=F時的相機坐標系坐標
cv::Point3f PNPSolver::ImageFrame2CameraFrame(cv::Point2f p, double F)
{
	double fx;
	double fy; 
	double u0;
	double v0;

	fx = camera_matrix.ptr<double>(0)[0];
	u0 = camera_matrix.ptr<double>(0)[2];
	fy = camera_matrix.ptr<double>(1)[1];
	v0 = camera_matrix.ptr<double>(1)[2];
	double zc = F;
	double xc = (p.x - u0)*F / fx;
	double yc = (p.y - v0)*F / fy;
	return cv::Point3f(xc, yc, zc);
}

　　

一個典型的調用示例

	//初始化PNPSolver類
	PNPSolver p4psolver;
	//初始化相機參數
	p4psolver.SetCameraMatrix(fx, fy, u0, v0);
	//設置畸變參數
	p4psolver.SetDistortionCoefficients(k1, k2, p1, p2, k3);
　　　　 //設置特征點的世界坐標
	p4psolver.Points3D.push_back(cv::Point3f(0, 0, 0));		//P1三維坐標的單位是毫米
	p4psolver.Points3D.push_back(cv::Point3f(0, 200, 0));	//P2
	p4psolver.Points3D.push_back(cv::Point3f(150, 0, 0));	//P3
	//p4psolver.Points3D.push_back(cv::Point3f(150, 200, 0));	//P4
	p4psolver.Points3D.push_back(cv::Point3f(0, 100, 105));	//P5

	cout << "test2:特征點世界坐標 = " << endl << p4psolver.Points3D << endl;
　　　　 //設置特征點的圖像坐標
	p4psolver.Points2D.push_back(cv::Point2f(2985, 1688));	//P1
	p4psolver.Points2D.push_back(cv::Point2f(5081, 1690));	//P2
	p4psolver.Points2D.push_back(cv::Point2f(2997, 2797));	//P3
	//p4psolver.Points2D.push_back(cv::Point2f(5544, 2757));	//P4
	p4psolver.Points2D.push_back(cv::Point2f(4148, 673));	//P5

	cout << "test2:圖中特征點坐標 = " << endl << p4psolver.Points2D << endl;

	if (p4psolver.Solve(PNPSolver::METHOD::CV_P3P) == 0)
		cout << "test2:CV_P3P方法:	相機位姿→" << "Oc坐標=" << p4psolver.Position_OcInW << "	相機旋轉=" << p4psolver.Theta_W2C << endl;
	if (p4psolver.Solve(PNPSolver::METHOD::CV_ITERATIVE) == 0)
		cout << "test2:CV_ITERATIVE方法:	相機位姿→" << "Oc坐標=" << p4psolver.Position_OcInW << "	相機旋轉=" << p4psolver.Theta_W2C << endl;
	if (p4psolver.Solve(PNPSolver::METHOD::CV_EPNP) == 0)
		cout << "test2:CV_EPNP方法:	相機位姿→" << "Oc坐標=" << p4psolver.Position_OcInW << "	相機旋轉=" << p4psolver.Theta_W2C << endl;

方法測試

OpenCV提供了三種方法進行PNP計算，三種方法具體怎么計算的就請各位自己查詢opencv documentation以及相關的論文了，我看了個大概然后結合自己實際的測試情況給出一個結論，不一定正確，僅供參考：

方法名

說明

測試結論

CV_P3P

這個方法使用非常經典的Gao方法解P3P問題，求出4組可能的解，再通過對第四個點的重投影，返回重投影誤差最小的點。

論文《Complete Solution Classification for the Perspective-Three-Point Problem》

可以使用任意4個特征點求解，不要共面，特征點數量不為4時報錯

CV_ITERATIVE

該方法基於Levenberg-Marquardt optimization迭代求解PNP問題，實質是迭代求出重投影誤差最小的解，這個解顯然不一定是正解。

實測該方法只有用4個共面的特征點時才能求出正確的解，使用5個特征點或4點非共面的特征點都得不到正確的位姿。

 

只能用4個共面的特征點來解位姿

CV_EPNP

該方法使用EfficientPNP方法求解問題，具體怎么做的當時網速不好我沒下載到論文，后面又懶得去看了。

論文《EPnP: Efficient Perspective-n-Point Camera Pose Estimation》

對於N個特征點，只要N>3就能夠求出正解。

測試截圖：

1.使用四個共面的特征點，顯然三種方法都能得到正解，但相互之間略有誤差。

2使用四個非共面的特征點，CV_ITERATIVE方法解錯了。

3.使用5個特征點求解，只有CV_EPNP能夠用

性能測試

最后對三種方法的性能進行測試，通過對test1重復執行1000次獲得算法的運行時間，從結果可以看出迭代法顯然是最慢的，Gao的P3P+重投影法用時最少，EPNP法其次。

總結

綜合以上的測試，推薦使用CV_P3P來解決實際問題，該方法對於有四個特征點的情況限制少、運算速度快。當特征點數大於4時，可以取多組4特征點計算出結果再求平均值，或者為了簡單點就直接使用CV_EPNP法。

不推薦使用CV_ITERATIVE方法。

 

 

 

程序下載地址：Github


免責聲明！

本站轉載的文章為個人學習借鑒使用，本站對版權不負任何法律責任。如果侵犯了您的隱私權益，請聯系本站郵箱yoyou2525@163.com刪除。



猜您在找 python+opencv2相機位姿估計 相機位姿估計1：根據四個特征點估計相機姿態 相機位姿估計2：[應用]實時位姿估計與三維重建相機姿態 相機位姿估計3：根據兩幅圖像的位姿估計結果求某點的世界坐標 axios 二次封裝 二次封裝 volley二次封裝 layui二次封裝 python斷言二次封裝 elementUI table二次封裝
 
粵ICP備18138465號   © 2018-2025 CODEPRJ.COM