SolvePnPRansac位姿估計算法

SolvePnPRansac是PnP位姿估計魯棒算法的一種，下面是Opencv 接口函數的描述

/*  max 注釋
*   函數功能：用ransac的方式求解PnP問題
*
*   參數：
*   [in]    _opoints                參考點在世界坐标系下的點集；float or double
*   [in]    _ipoints                參考點在相機像平面的坐标；float or double
*   [in]    _cameraMatrix           相機内參
*   [in]    _distCoeffs             相機畸變系數
*   [out]   _rvec                   旋轉矩陣
*   [out]   _tvec                   平移向量
*   [in]    useExtrinsicGuess       若果求解PnP使用疊代算法，初始值可以使用猜測的初始值（true），也可以使用解析求解的結果作為初始值（false）。
*   [in]    iterationsCount         Ransac算法的疊代次數，這隻是初始值，根據估計外點的機率，可以進一步縮小疊代次數；（此值函數内部是會不斷改變的）,是以一開始可以賦一個大的值。
*   [in]    reprojectionErrr        Ransac篩選内點和外點的距離門檻值，這個根據估計内點的機率和每個點的均方差（假設誤差按照高斯分布）可以計算出此門檻值。
*   [in]    confidence              此值與計算采樣（疊代）次數有關。此值代表從n個樣本中取s個點，N次采樣可以使s個點全為内點的機率。
*   [out]   _inliers                傳回内點的序列。為矩陣形式
*   [in]    flags                   最小子集的計算模型；
*                                                 SOLVEPNP_ITERATIVE(此方案，最小模型用的EPNP，内點選出之後用了一個疊代)；
*                                                 SOLVE_P3P(P3P隻用在最小模型上，内點選出之後用了一個EPNP)      
*                                                 SOLVE_AP3P(AP3P隻用在最小模型上，内點選出之後用了一個EPNP)
*                                                 SOLVE_EPnP(最小模型上&内點選出之後都采用了EPNP)
*    傳回值：
*         成功傳回true，失敗傳回false；
*/
bool solvePnPRansac(InputArray _opoints, InputArray _ipoints,
	InputArray _cameraMatrix, InputArray _distCoeffs,
	OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess,
	int iterationsCount, float reprojectionError, double confidence,
	OutputArray _inliers, int flags)
           

下面是使用例程：

// 自己測試 p3p測位姿算法
#if 1
#include "test_precomp.hpp"

void generate3DPointCloud(std::vector<cv::Point3f>& points, cv::Point3f pmin = cv::Point3f(-1, -1, 5), cv::Point3f pmax = cv::Point3f(1, 1, 10))
{
	cv::RNG rng = cv::theRNG(); // fix the seed to use "fixed" input 3D points

	for (size_t i = 0; i < points.size(); i++)
	{
		float _x = rng.uniform(pmin.x, pmax.x);
		float _y = rng.uniform(pmin.y, pmax.y);
		float _z = rng.uniform(pmin.z, pmax.z);
		points[i] = cv::Point3f(_x, _y, _z);
	}
}

void generateCameraMatrix(cv::Mat& cameraMatrix, cv::RNG& rng)
{
	const double fcMinVal = 1e-3;
	const double fcMaxVal = 100;
	cameraMatrix.create(3, 3, CV_64FC1);
	cameraMatrix.setTo(cv::Scalar(0));
	cameraMatrix.at<double>(0, 0) = rng.uniform(fcMinVal, fcMaxVal);
	cameraMatrix.at<double>(1, 1) = rng.uniform(fcMinVal, fcMaxVal);
	cameraMatrix.at<double>(0, 2) = rng.uniform(fcMinVal, fcMaxVal);
	cameraMatrix.at<double>(1, 2) = rng.uniform(fcMinVal, fcMaxVal);
	cameraMatrix.at<double>(2, 2) = 1;
}

void generateDistCoeffs(cv::Mat& distCoeffs, cv::RNG& rng)
{
	distCoeffs = cv::Mat::zeros(4, 1, CV_64FC1);
	for (int i = 0; i < 3; i++)
		distCoeffs.at<double>(i, 0) = rng.uniform(0.0, 1.0e-6);
}

void generatePose(cv::Mat& rvec, cv::Mat& tvec, cv::RNG& rng)
{
	const double minVal = 1.0e-3;
	const double maxVal = 1.0;
	rvec.create(3, 1, CV_64FC1);
	tvec.create(3, 1, CV_64FC1);
	for (int i = 0; i < 3; i++)
	{
		rvec.at<double>(i, 0) = rng.uniform(minVal, maxVal);
		tvec.at<double>(i, 0) = rng.uniform(minVal, maxVal / 10);
	}
}


int main_p3p()
{
	std::vector<cv::Point3f> points;
	points.resize(500);
	generate3DPointCloud(points);


	std::vector<cv::Mat> rvecs, tvecs;
	cv::Mat trueRvec, trueTvec;
	cv::Mat intrinsics, distCoeffs;

	generateCameraMatrix(intrinsics, cv::RNG());	
	generateDistCoeffs(distCoeffs, cv::RNG());

	generatePose(trueRvec, trueTvec, cv::RNG());

	std::vector<cv::Point3f> opoints;
	opoints = std::vector<cv::Point3f>(points.begin(), points.begin() + 3);

	std::vector<cv::Point2f> projectedPoints;
	projectedPoints.resize(opoints.size());
	projectPoints(cv::Mat(opoints), trueRvec, trueTvec, intrinsics, distCoeffs, projectedPoints);

	std::cout << "intrinsics: " << intrinsics << std::endl;
	std::cout << "distcoeffs: " << distCoeffs << std::endl;
	std::cout << "trueRvec: " << trueRvec << std::endl;
	std::cout << "trueTvec: " << trueTvec << std::endl;

	std::cout << "oPoint: " << opoints << std::endl;
	std::cout << "projectedPoints: " << projectedPoints << std::endl;



	std::cout<<"result numbers A: :"<<solveP3P(opoints, projectedPoints, intrinsics, distCoeffs, rvecs, tvecs, cv::SOLVEPNP_AP3P)<<std::endl;
	//std::cout << "result numbers: :" << solveP3P(opoints, projectedPoints, intrinsics, distCoeffs, rvecs, tvecs, cv::SOLVEPNP_P3P) << std::endl;

	bool isTestSuccess = false;
	double error = DBL_MAX;
	for (unsigned int i = 0; i < rvecs.size() /*&& !isTestSuccess*/; ++i)
	{
		double rvecDiff = cvtest::norm(rvecs[i], trueRvec, cv::NORM_L2);
		double tvecDiff = cvtest::norm(tvecs[i], trueTvec, cv::NORM_L2);
		isTestSuccess = rvecDiff < 1.0e-4 && tvecDiff < 1.0e-4;
		error = std::min(error, std::max(rvecDiff, tvecDiff));
		std::cout << "i: " << i << std::endl;
		std::cout << "error: " << error << std::endl;
		std::cout << "rvec: " << rvecs[i] << std::endl;

	}

	system("pause");
	return 0;
}
int main()
{
	std::vector<cv::Point3f> points;
	points.resize(500);
	generate3DPointCloud(points);


	std::vector<cv::Mat> rvecs, tvecs;
	cv::Mat trueRvec, trueTvec;
	cv::Mat intrinsics, distCoeffs;

	generateCameraMatrix(intrinsics, cv::RNG());
	generateDistCoeffs(distCoeffs, cv::RNG());

	generatePose(trueRvec, trueTvec, cv::RNG());

	std::vector<cv::Point3f> opoints;
	//opoints = std::vector<cv::Point3f>(points.begin(), points.begin() + 4);
	opoints = std::vector<cv::Point3f>(points.begin(), points.end());

	std::vector<cv::Point2f> projectedPoints;
	projectedPoints.resize(opoints.size());
	projectPoints(cv::Mat(opoints), trueRvec, trueTvec, intrinsics, distCoeffs, projectedPoints);

	cv::RNG rng = cv::RNG();
	for (size_t i = 0; i < projectedPoints.size(); i++)
	{
		if (i % 100 == 0)
		{
			projectedPoints[i] = projectedPoints[rng.uniform(0, (int)points.size() - 1)];
		}
	}

	std::cout << "intrinsics: " << intrinsics << std::endl;
	std::cout << "distcoeffs: " << distCoeffs << std::endl;
	std::cout << "trueRvec: " << trueRvec << std::endl;
	std::cout << "trueTvec: " << trueTvec << std::endl;

	//std::cout << "oPoint: " << opoints << std::endl;
	//std::cout << "projectedPoints: " << projectedPoints << std::endl;



	//solveP3P(opoints, projectedPoints, intrinsics, distCoeffs, rvecs, tvecs, cv::SOLVEPNP_AP3P);
	cv::Mat rvec, tvec;
	solvePnP(opoints, projectedPoints, intrinsics, distCoeffs, rvec, tvec, false,cv::SOLVEPNP_EPNP);
	

	std::cout << rvec <<"---" <<norm(rvec-trueRvec)<<std::endl;
	std::cout << tvec << std::endl;


	std::cout << "---------------Ransac--------------------"<< std::endl;
	solvePnPRansac(opoints, projectedPoints, intrinsics, distCoeffs, rvec, tvec,false,100,2);
	std::cout << rvec << "---" << norm(rvec - trueRvec) << std::endl;
	std::cout << tvec << std::endl;
	system("pause");
}



#endif