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